第二章剛體空間變換

1、剛體空間變換作者：追風少年 時間:2016/12/29本文為作者原創(chuàng)文章，未經(jīng)作者允許不得轉(zhuǎn)載，謝謝合作！前言在定位導航領域，最基本知識當屬空間幾何、剛體變換等，尤其是對于機器人領域，開發(fā) 人員涉及到大量的空間轉(zhuǎn)換和剛體運動方面細節(jié)。稍有不慎，就有可能發(fā)生錯誤和混淆，有時 一個小的錯誤，就會導致算法無法正常運行，令人十分困惱.本文梳理了一些基礎的剛體變換 知識，更側(cè)豆于從應用角度來分析剛體空間變換.一笛卡爾坐標以及右手法則Note:英國法律規(guī)定馬路行車靠左行駛，我國法律規(guī)定行車靠右行駛.空間XYZ三維坐標中，當在某個平面內(nèi)確定XY軸的正方向時，Z軸的選擇有兩種可能 性：垂直平面的兩個方向。為了

2、規(guī)范坐標系建立的一致性，選擇右手定則進行確定，我們稱為 笛卡爾坐標.右手定則：伸出右手，將拇描指向X軸的正方向，食指指向Y軸正方向，則中指所指示的 方向即為Z軸的正方向.規(guī)定旋轉(zhuǎn)方向：在右手坐標系中，物體旋轉(zhuǎn)的正方向是右手螺旋法則，即從該軸正半軸向 原點看，逆時針方向為其正方向。圖a）右手規(guī)則Orientation o< Axm ofandPolarity of Rotation圖b）規(guī)定旋轉(zhuǎn)方向二坐標系室內(nèi)定位過程中，需要建立不同坐標系的觀測，如我體坐標系（BodyCoordinate；觀測的加速度、角速度等轉(zhuǎn)換到世界坐標系(Earth Coordinate)進行運算。這里選最常用的兩

3、種坐標系解釋。裁體坐標軸(B)：一般地，在航天飛行領域和計算機圖形學中，選取載體前進方向為x軸正方向，右側(cè)冀方向為y軸正方向，向下為z軸正方向.世界坐標系(E)：Tips:在實際項目開發(fā)中，我們往往會使用一些開源庫進行開發(fā)，需要強調(diào)的是，在使 用其提供的API Z前，必須要搞清楚開源庫函數(shù)坐標系的選取和基準是否與目前系統(tǒng)參照有所 沖突，有時方向不一致十分影響程序的運行，尤其是科研研發(fā)當中！三空間變換3.1什么是空間變換簡言Z，空間變換是把戟體坐標系i動)下觀測的變量(向量丿換算成世界坐標系(定丿下的對應 值，獲得有用的數(shù)據(jù)，即將載體Body坐標系B轉(zhuǎn)換到世界坐標系E設空間八尺下的一組肌位正交基

4、為下的一組單位正交基為億,厶,&,則B坐標系卜的基坐標在坐標系A卜的表示為Xfi =+ r2l Jl + 41*2 = A< Vb = rill + r21Jl + rill = Jl= rl5i2 + ruj2 + r3ik2 = k,由空間變換的定位可知，使用不同基坐標觀測一個矢量，會在不同的度量空間下得到不同的表示.而空間變換的意義就在于能夠建立兩個空間的連接，即旋轉(zhuǎn)矩陣顯然，由向竜空間旋轉(zhuǎn)知，ARfl = Rotation.表示為作用于基坐標A到基坐標B的旋轉(zhuǎn);ARJ乍用于B下的坐標則得到由4基坐標構(gòu)成的空間，稱為坐標變換.上式助記符，八只的含義為B坐標系下的單位正交基在

5、坐標系4下的表示。則如果需 要將但坐標系上的矢量表示為坐標系,則叭乘以這組正交aAxfl,A>«,Azj即可： ax=aRhhx其中，AR&表示剛體3相對于坐標系A的姿態(tài)的旋轉(zhuǎn)矩陣.Tips:空間由一組正交基表示.則空間變換的核心當然就是這組正交基之間的關系.步驟 為使用農(nóng)示S尋找仏）到（4、之間變換 使用切表示h3.2向量旋轉(zhuǎn)與坐標系姿態(tài)向帚旋轉(zhuǎn)是指在同一坐標系下，經(jīng)過旋轉(zhuǎn)后原向疑與當前向疑之間的變換關系，注噸空間 位置關系.坐標系姿態(tài)則是指同一觀測量任不同坐標系（基元）下農(nóng)示之間的變換關系，-般地需要將 其中一個基坐標旋轉(zhuǎn)至另一個基坐標對齊.從兩者的定義來說，向雖旋

6、轉(zhuǎn)描述的是旋轉(zhuǎn)矩陣作用于空間向屋而坐標系込態(tài)則是旋轉(zhuǎn)矩 陣作用于一組基坐標.不難發(fā)現(xiàn)，在數(shù)學表達形式上，旋轉(zhuǎn)矩陣表達形式一致，其含義卻大不相同.不可混淆.3.3基本旋轉(zhuǎn)將繞X. Y. Z軸的旋轉(zhuǎn)變換稱為基本變換.RlXQ =1 0 00 cos& - sin 0 0 sm& cqsO R(YZ) =cos& 0 sin 00 1 0 -sin3 0 cos。 /?(乙 0)=cqsO - Sill 00sm& cos。 00 0 1 任何的旋轉(zhuǎn)都可以由如上的基本變換組合而成.但旋轉(zhuǎn)合成過程中需要區(qū)分兩種情況：旋 轉(zhuǎn)軸固定和旋轉(zhuǎn)軸不固定.若旋轉(zhuǎn)經(jīng)過的姿態(tài)為ABC,

7、旋轉(zhuǎn)合成應該分情況討論：載體每次的旋轉(zhuǎn)過程都是繞著同一皐坐標系庫坐標變換，如地球坐標系為參照)進行，則旋轉(zhuǎn)合成可以表示為T=TJJ載體毎次的過程都足繞著當前坐標系(聯(lián)體坐標突換.如中問姿態(tài)的坐標系為參照丿進行，則旋轉(zhuǎn)合成可以表示為T=T6J、以匕規(guī)律.通常被稱之為右乘聯(lián)體左乘基.該口訣會在之后的歐拉角以及各種旋轉(zhuǎn)表達方 式中表現(xiàn)出來.可以看出合成運動與旋轉(zhuǎn)次序有著必然聯(lián)系.四空間姿態(tài)空間姿態(tài)的農(nóng)示方式有方向余弦(Direction Cosine八 歐拉角(Euler Angle八 四元數(shù)法 (Quaternion).其中方向余(DirectionCosine八歐拉角(EulerAngle)表示

8、方法直觀便于進行空 間理解和圖形展示但運算性質(zhì)不佳，而卩q元數(shù)法在數(shù)學性質(zhì)上有著很好的表達能力，所以在實 際應用中常常需要相互轉(zhuǎn)換.獲取全面的姿態(tài)表示，其優(yōu)劣將在下而進行討論.4.1 方向余弦法（Direction Cosine）已知在坐標系1的向量V1農(nóng)示為可以表達為:rX1/J''嚴也+ y叢+ zK =>1 =町=A VvJAT/v廠Tr-il-11 r 1 孔 X。VArK：>v = Ol2 + v2 => %= y。+ v2T最后有空間變換公式:Il 12 'Jl a 12=2 +Il J2 Ji J2V2十Ii -K2 'J1K2K

9、, -I2 K1J2 Ki - K2 (】2叼十121/2十K2乙2)cos（7i）COS（72）cos（73）cos（cii） cos（Bi）COS（Q2） COS（32）COS（°3）COS（03）4.2 歐拉角(Euler Angle)歐拉角農(nóng)示方法由著名數(shù)學家歐拉首次提出，主要用于飛行器、制導與導航、小型飛行器 等設備調(diào)節(jié)姿態(tài)，以便到達指定姿態(tài)和位置，這種方式的空間變換便于人的理解和操作.歐拉 角表示方法認為，任意姿態(tài)變換可以分解為分別繞X、 Z軸的旋轉(zhuǎn)變換合成來表示.旋轉(zhuǎn)次序：依次繞著三個坐標軸的旋轉(zhuǎn)可以描述任意一個旋轉(zhuǎn).事實上這樣會有27種可能 的旋線次序，但是僅僅有12

10、種満足連續(xù)2次旋轉(zhuǎn)不為同一旋轉(zhuǎn)軸，如下所示:At)G (1.2.1),(1- 2.3), (1,3.1),(1.3.2),(2.1.2) , (2,1,3), (2,3,1),(2,3,2),(3.1.2) , (3.1.3), (3.2.1),(3. 2.旳.圖中黑色加粗旋轉(zhuǎn)序列是實際應用中最常見的選擇.序列3,】，3(ZXZ丿，被稱為Leonhard Euler,是以世紀瑞士數(shù)學家和物理學家Leonhard 命名，主要應用于陀螺運動中剛體自旋的姿態(tài)表示.序列1, 2, 3(XYZ丿，被稱為6rdan Euler,是以文藝復興時期意大利數(shù)學家Gerolamo Cardano命名，又叫Tait

11、-Bryan Angles.以19世紀蘇格蘭數(shù)學物理學家Peter Guthne Tait命名, 主要應用于航空工程和計算機圖形學領域.本文主要介紹旋轉(zhuǎn)次序為XYZ的歐拉角表示，按如下旋轉(zhuǎn)順序XYZ：在YZ平面內(nèi)，繞x軸旋轉(zhuǎn)0角度，稱為翻滾角rolling,在XZ平面內(nèi)，繞y軸旋轉(zhuǎn)0角度，稱為俯仰角pitch,在XY平面內(nèi)，繞z軸旋轉(zhuǎn)角度，稱為航向角heading ,為方便理解.特附匕歐拉旋粋槻頻満示地址:hg:/wHki(zmAhz/M_XN:k()TIMTI=hmd歐拉角的表示方法：考世從載休坐標系旋轉(zhuǎn)到世界坐標系時.旋轉(zhuǎn)矩陣可以由墓木矩陣構(gòu)成，按照ZYX旋轉(zhuǎn)順序，我們很容易獲得其屁轉(zhuǎn)矩陣

12、為cBc屮=> £v= sQsBc屮一 cQs屮C0S0C 屮+ S0S 屮C&S屮S0S0S 屮+ c(frcy/cQsBs屮一 st/fcy/-sO鳥=/?(X"廣忌丫0嚴乙屮)E Xri00 'cO0s0c屮-si/of Xy=0C070010SI/Cl/0y0$0C(f>-s60ce001z_歐拉角速度與角速度0:Tips:靜止狀態(tài)下，為了初始化載體姿態(tài),這時如何求解謹態(tài)矩陣（ZYX旋轉(zhuǎn)次序丿呢？s av=>ay=azC0C屮S0S0C 屮一gs 屮C0S0C 屮+ S0S 屮C0S屮S0S0S屮七 ccy/cQsOs屮一 s如屮a

13、x = gsO ay = -gcOs(t> =>az = -gc(/K'0(fi= atan2-ayy-az)0 = otan.2(心,Jay' +血，) 0 «0 0=R（y、O）*Ri Z、屮）0+ R(乙 )e+03. 00申c0cy/c8s 屮s6 s屮（屮0 e001 _y_4.3 四元數(shù)法(Quaternion)四元數(shù)法是由19世紀愛爾蘭數(shù)學家William Roxn Hamilton提出.什么是四元數(shù)？四元 數(shù)是由一個實數(shù)和一個三維復數(shù)相加組成基本性質(zhì)四元數(shù)共輒:ft四元數(shù)模長:四元數(shù)逆:iwii乘法:q®p =p) = Q(q)

14、p=Q(p)qQ® =£q、-如-_侑-纟Q® = Q3s s 亦)=Q(q)TA*JS昇:-q, q.The inverse mappings, frcm a unit quaternion to rhe ccr responciing axis and angle of rotation, are aq : H K and n7 : H > S2, (lelined by(q) = 2；i(“s .n7(q):=qi：3Ml;3<11:3(198)(199)四元數(shù)姿態(tài)表示四元數(shù)表示法中姿態(tài)變換是載體坐標系以空間向雖/為軸旋轉(zhuǎn)角度0得到.如右圖所示，

15、 Frame B是由Frame A以向疑"廠為軸旋轉(zhuǎn)角度&而得.四元數(shù)；q = cos(0/2) Ar siii /2»描述了剛體妁定點轉(zhuǎn)動，可認為B系是由A系經(jīng)過無中間過程的一次性等效旋轉(zhuǎn) 形成的，：q包含了這種等效旋轉(zhuǎn)的全部信息：竹為旋轉(zhuǎn)瞬軸 和旋轉(zhuǎn)方向，0為轉(zhuǎn)過的角度.向量旋轉(zhuǎn)：令；q=h Qi ch 表示由載體坐標系向地理坐標系乙旋轉(zhuǎn)表示的四元數(shù)，使用 四元數(shù)對向量進行旋轉(zhuǎn)，得：2v=討兩討T= :(1)fv= fiv®(2)其中，式 表示的幾何含義為向最繞q：進行旋轉(zhuǎn)后與'vfi合，式表示不同坐標觀 測Z間的變換并無兒何意義.四元數(shù)姿態(tài)更

16、新因為對角速率進行枳分，可以計算載體旋轉(zhuǎn)的角度.一般地，選取s=q©.0:;q® 制 ® ；q進行姿態(tài)更新（區(qū)別與£燈角選取s亠作為姿態(tài)），因為每次進行姿態(tài)更新時，有冋 一基準.在導航的應用過程中，常使用：勺將載體的姿態(tài)通過陀螺儀的旋轉(zhuǎn)速度變換到新姿態(tài)上俗 稱姿態(tài)更新.四元數(shù)微分方程：W=o a）v 冬四元數(shù)更新：Ecit+i = R +由姿態(tài)更新等式，四元數(shù)所表示的旋轉(zhuǎn)矩陣為2礦一1+ 2q： 2（弘俗+幺侑）2（綣侑一紐偽）；R = 2（仏偽一 q、qj 2qj_ 1 + 2q；2（g« + q、qj2（仏仍 + qg 2（弘s - q、q

17、j 2q； 一 1 + 2qj 從實際應用來看，四元數(shù)即代表裁體狀態(tài)，又代表姿態(tài)更新行為從獲取載體的姿態(tài)來看， 四元數(shù)可以農(nóng)示為對應的歐拉角；從姿態(tài)更新的角度上來看，陀螺儀的旋轉(zhuǎn)引起的姿態(tài)變換可 表示為當前卩q元數(shù)與o/waZ、當前卩q元數(shù)的逆三者之間叉乘運算的結(jié)果.四元數(shù)均值在UKF卡爾曼濾波過程中，需要對四元數(shù)姿態(tài)求均值，本小節(jié)從這里出發(fā)探索四元數(shù)一些 有趣的性質(zhì).使用四元數(shù)作為狀態(tài)最時，由于卩q元數(shù)構(gòu)成的空間并不是歐幾米德空間，即阿元 數(shù)運算并不滿足算子的封閉性和結(jié)合性.在對四元數(shù)進行UT變換時，不能使用常規(guī)的求均值 方式.四元數(shù)與噪聲在卩q元數(shù)狀態(tài)方程中，噪聲叫增加了物體姿態(tài)不確定度（

18、Deg/Tune Interval）.叫是一個三維噪聲向呈，它不能被簡單地加到單位四元數(shù)上，需要轉(zhuǎn)化為單位四元數(shù)。若隨機變量叫服從均值為0,方差為0卻的髙斯噪聲.可以將叫看成是繞著xyz旋轉(zhuǎn)的向屋，則可等價于u = sin(a / 2)II 可IIq表示旋轉(zhuǎn)角度"表示旋轉(zhuǎn)軸，號位卩q元數(shù)可以表示為:q = cos(a / 2).sm(a! 2)基于迭代的四元數(shù)均值對于四元數(shù)來說，律立一種新的度呈方式描述兩個四兀數(shù)Z間的距離,成為最為關鍵的部分.由四元數(shù)的傳遞性可知：角七代角曠角角7=> 2i =Z =（h® qj'q,與</,之間的距離可由表示，則這兩個

19、四元數(shù)之間的夾角為0 = 2arccos（A70）使用&可表示四元數(shù)之間的距離設N個四元數(shù)的均值為4,則該均值與所有四元數(shù)之間 的距離和為© ng ;e表示估計的均值四元數(shù)和真實的均值卩q元數(shù)之間的誤差.e是一個描向真實均值四元數(shù) 的旋皴向量，故存H會不斷逼近均值向屋，直到誤差相至無幾.一般地，初始值選擇Sigma中心點時，迭代一次悶可完成均值獲取.為其他點時，迭代2次可以獲取均值.故基于迭代的方式效率很髙基于代價函數(shù)的最優(yōu)化四元數(shù)均值為此，早在60年代Wahbci就提出了構(gòu)造函數(shù)代價求解四元數(shù)均值問題，基于這種恩想文獻 學者們提出了各種不同的算法.其中文獻3戌接以姿態(tài)矩陣為

20、對象來求解加權均值四元數(shù)的方 法因為不涉及矩陣的奇異值分解使得算法相對簡單而得到較為的廣泛應用，該算法具體如下：構(gòu)造姿態(tài)矩陣代價函數(shù)如下：-Nq = aignun工 n; | A（q） - A（q ） |匸igi 冋 >=1其中 弘足由濾波卩q元數(shù)構(gòu)成的正交姿態(tài)矩陣，尸為Fiubtuiu范數(shù)為更新四元數(shù)J&ua 點.由Frobenius范數(shù)定義及姿態(tài)矩陣的正交性3,四元數(shù)均值可轉(zhuǎn)化為求如下代價函數(shù)值最大 時的向量解：q = aig max cf KqikKK = 4M L，M=±wqq：從而均值卩q元數(shù)q即為矩陣K授大特征值對應的特征向帚：.五總結(jié):不同的姿態(tài)變換，實質(zhì)

21、上只是表達形式上的不一致其本質(zhì)的幾何關系卻并沒有什么區(qū)別， 四元數(shù)姿態(tài)農(nóng)示方法雖然計算便利，但不便于直觀調(diào)試和應用，故往往需要將四元數(shù)轉(zhuǎn)換為更 加形象的歐拉角，其轉(zhuǎn)換公式為>roll-atan2(2q3q4 +2q©2，q； W)歐拉角0=pitch=-asin(2q2q4 -2qtq3)headingatan2(2q2q,)(XVZ)在將四元數(shù)轉(zhuǎn)換為角度Angle a程中，需要使用反三角函數(shù)Jtan2,其表達式為atan(j/jr) if .r > 0atan(i/a;) - tt if x <0 A 1/ < 0atan(i/j：) + tt if t < 0 A y > 0各種坐標系之間的相互轉(zhuǎn)換容易混亂，這里必須要注總的是:歐拉角的變換主要為b系（載體丿T參考系（R系丿四元數(shù)的變換主要為參考系迅系）T b系（載體）參考文獻 秦永元慣性導航M科學出版社,201