捷聯(lián)慣性導航系統(tǒng)的解算方法

1、,慣性導航系統(tǒng)原理,3捷聯(lián)式慣導系統(tǒng) 程向紅 2010.03.19,1,10/21/2020,2010-03-19,2,3捷聯(lián)式慣導系統(tǒng),3.1 捷聯(lián)式慣導算法概述 3.2 姿態(tài)矩陣的計算 3.3 姿態(tài)矩陣計算機執(zhí)行算法,2,10/21/2020,2010-03-19,3,3.1 捷聯(lián)式慣導算法概述,捷 聯(lián) 式 慣 導 算 法, b,ib,f b,ib,P, R, H , L,VE ,VN,捷聯(lián)式慣導航系統(tǒng)是一個信息處理系統(tǒng)，就是將載體上安裝的慣性 儀表所測量的載體運動信息，經(jīng)過計算處理成所需要的導航信息。,b,姿態(tài)矩 陣 計算,加速度計組,導 航 計算機,VE,初始條件,b,SF,VN,n,

2、Cb,n,SF,b,in,t,H P R,3,10/21/2020,捷聯(lián)式慣性導航系統(tǒng)=信息處理系統(tǒng),根據(jù)捷聯(lián)式慣導的應用和功能要求不同，計算的內(nèi)容和要 求，有很大的差別。常有 SINSStrapdown Inertial Navigation Systems SVRUStrapdown Vertical Reference Uint SAHRSStrapdown Attitude and Heading Reference Systems IMUInertial measurement Unit,捷 聯(lián) 式 慣 導 算 法,b,ib fib,b,P, R, H,EN, L,V ,V,2010

3、-03-19,4,10/21/2020,接聯(lián)式慣導的算法的基本內(nèi)容 （1）系統(tǒng)的啟動和自檢測 （2）系統(tǒng)初始化 （3）慣性儀表的誤差補償 （4）姿態(tài)矩陣的計算 （5）導航計算 （6）制導和控制信息的提取,2010-03-19,5,10/21/2020,（1）系統(tǒng)的啟動和自檢測,系統(tǒng)啟動后，各個部分的工作是否正常，要 通過自檢測程序加以檢測，其中包括電源、慣 性儀表、計算機以及計算機軟件。 通過自檢測，發(fā)現(xiàn)有不正常，則發(fā)出告警信息(或 故障碼)。系統(tǒng)的自檢測是保證系統(tǒng)進入導航狀態(tài) 后能正常工作、提高系統(tǒng)可靠性的措施。,2010-03-19,6,10/21/2020,（2）系統(tǒng)初始化,為何要初始化

4、？ 給定載體（艦船、飛行器、車輛等）的初始位置 （經(jīng)度和緯度）和初始速度等初始信息。,導航平臺的初始對準,慣性儀表的校準 Calibration,平臺式,姿態(tài)矩陣的初始值,用物理的方法來實現(xiàn),標度系數(shù) 加速度計,捷聯(lián)式,陀螺儀,進行測定,漂移 偏置,2010-03-19,7,10/21/2020,（3）慣性儀表的誤差補償,對捷聯(lián)式慣導系統(tǒng)來說，由于慣性儀表直接安裝 在載體上，因此，載體的線運動和角運動都引起 較大的誤差。 為了保證系統(tǒng)的精度，必須對慣性儀表的誤差進行 補償，最好的補償方法是計算機補償。 在計算機中通過專用的軟件來實現(xiàn)誤差補償。,2010-03-19,8,10/21/2020,（

5、4）姿態(tài)矩陣的計算,姿態(tài)矩陣的計算是捷聯(lián)式慣導算法中最重要的一 部分，也是捷聯(lián)式系統(tǒng)所特有的。 不管捷聯(lián)式慣導應用和功能要求如何，姿態(tài)矩陣 的計算卻是不可少的。姿態(tài)矩陣算法是本章重點 討論的內(nèi)容。,2010-03-19,9,10/21/2020,（5）導航計算,導航計算就是把加速度計的輸出信息變換到導航坐 標系，然后，計算載體速度、位置等導航信息。,2010-03-19,10,10/21/2020,（6）制導和控制信息的提取,制導和控制信息的提取，載體的姿態(tài)既可用來 顯示也是控制系統(tǒng)最基本的控制信息。 此外，載體的角速度和線速度信息也都是控制 載體所需要的信息。 這些信息可以從姿態(tài)矩陣的元素和

6、陀螺加速度 計的輸出中提取出來。,2010-03-19,11,10/21/2020,捷聯(lián)式慣導系統(tǒng)算法流程圖,啟動 自 檢 測 初 始 化 姿態(tài)陣計算,迭 代 次 數(shù),控 制 信 息 提 取 返回9,2010-03-19,12,YES 導 航 計算,NO,10/21/2020,2010-03-19,13,3.2 姿態(tài)矩陣的計算,捷聯(lián)式慣導中，載體地理位置就是地理坐標系相對 地球坐標系的方位。而載體的姿態(tài)和航向則是載體 坐標系相對于地理坐標系的方位關(guān)系。確定兩個坐 標系的方位關(guān)系問題，是力學中的剛體定點轉(zhuǎn)到理 論。在剛體定點轉(zhuǎn)動理論中，描述動坐標系相對參 考坐標系方位關(guān)系的方法有多種。,四參數(shù)法

7、 1843年發(fā)明的，首先在數(shù)學中引入四元數(shù)，以 后用在剛體定位問題。凱里.克萊茵（Cayley- Klein）參數(shù)法，是在1897年提出的。 九參數(shù)法基于方向余弦的概念，也稱 方向余弦法。,三參數(shù)法,歐拉角法 ，是歐拉在1776年提出的。 四元數(shù)法。威廉.哈密頓(William Hamilton)在,等效轉(zhuǎn)動矢量法,13,10/21/2020,3.2 姿態(tài)矩陣的計算,3.2.1 歐拉角法 3.2.2 方向余弦法 3.2.3 四元數(shù)法 3.2.4 等效轉(zhuǎn)動矢量法,2010-03-19,14,10/21/2020,3.2.1 歐拉角法,Xb,ENU作為參考坐標系，則航向 角H，縱搖角（俯仰角）P和

8、橫 搖角（橫滾角、傾斜角）R。就 是一組歐拉角。 歐拉角沒有嚴格的定義，根 據(jù)需要，可以選用不同的歐拉 角組。第一次轉(zhuǎn)動，可以繞三 個軸中的任一個轉(zhuǎn)動，故有3種 可能，第二次有2種可能，第三 次也有2種可能?？偣灿?2種可 能。,E,Xb,O,U,N,H.,Zb,Yb,Xb, Y,Yb,Zb,b,Zb,P.,R.,H,P,R,一個動坐標系相對參考坐標系的方位，完全可以由動坐 標系依次繞3個不同的軸轉(zhuǎn)動的3個轉(zhuǎn)角來確定。 如把OXbYbZb作為動坐標系，,2010-03-19,15,10/21/2020,2,010-03-1916,用歐拉角表示的姿態(tài)矩陣,001U ,0N ,0 E , Y si

9、n H, cos H, ,b , Xb ,sin H cos H,Z, v- _ CH, b,0 sin Pcos P Z ,00 cos Psin P Yb , ,b , X , v- _, 0, ,1, Z ,b , X , Yb,b,CP,b,cos R Z ,Yb , ,b ,0 sin R X 01, v- _,sin R0, Yb ,cos R,b , Z, X,0,b,CR,b,cos P cos R ,cos P sin R ,sin R cos H sin P cos R sin H, ,cos R cos H sin P sin R sin H,cos R sin H si

10、n P sin R cos H cos P cos H sin R sin H sin P cos R cos H,cos P sin H,sin P,b n,C,E,X ,b,O,U Zb,b N,H.,Y ,Xb,X ,b,Y Yb,b,Z ,b,b,Z,P.,R.,H,P,R,HPR,16,10/21/2020,歐拉角微分方程,表示載體坐標,系相對地理坐標系的角 速度矢量在載體坐標系 軸向的分量構(gòu)成的列矩 陣。,E,Xb,O,U Zb,b N,H.,Y ,Xb,X ,b,Y Yb,b,Z ,b,b,Z,P.,R.,H,P,R,b,nb, 0 , 0 R. , 0 , 0 ,P. ,R ,

11、 0 C,H. , 0 ,RP , CC,nby ,nbz , b, b,b,nbx,HPR,2010-03-19,17,10/21/2020,歐拉角微分方程,cos P cos R H. , R. ,0 sin R cos P P. 01 0,sin R,nby ,nbz ,cos R,nbx , b, b,sin P,b,nby ,nbz , b,cos P cos R , ,sin R, R. ,cos R,0 sin R cos P 01sin P 0,H. , P. ,1,b,b nbx,cos R sin Pnby ,nbz , b,sin R cos P b, R. cos P

12、sin P sin R,1 ,cos P cos R0 cos P 0,sin R,H. , P. ,b,nbx,cos R,b,Cn,求解微分方程,3個歐拉角,航向角 (H),姿態(tài)角(P,R),2010-03-19,18,10/21/2020,2010-03-19,19,歐拉角法應用中的問題,求解方程可以直接得到航向和姿態(tài)信息，歐 拉角法得到的姿態(tài)陣永遠是正交陣，用這個矩 陣將比力fbfn信息的坐標變換時，變換后的信 息中不存在非正交誤差。因此，用歐拉角法得 到的姿態(tài)矩陣無需進行正交化處理。 歐拉角微分方程中包含三角函數(shù)的運算， 給實時計算帶來困難，當P=90。時，方程式 出現(xiàn)“奇點”，使計

13、算溢出。,cos P cos R0 cos P 0,sin R cos P, R 1 sin P sin R,cos R sin P,cos P ,sin R,cos R,b nbx,b,nby , b, nbz , P. ,H. ,.,返回3.2,垂,直,發(fā),射,困,難,！,19,10/21/2020,3.2.2 方向余弦法,方向余弦表示的姿態(tài)矩陣 方向余弦法用矢量的方向余弦來表示姿態(tài)矩陣的方法。 用in, jn, kn表示沿地理坐標系軸向的單位矢量。 ib, jb, kb沿載體坐標系軸向的單位矢量。ib在地理坐 標系內(nèi)的方位完全可以由ib的三個方向余弦來確定，其 表達式為 ib (ib i

14、n )in (ib jn ) jn (ib kn )kn cos(ib in ) jb ( jb in )in ( jb jn ) jn ( jb kn )kn kb (kb in )in (kb jn ) jn (kb kn )kn,2010-03-19,20,10/21/2020,方向余弦法,kb kn kn ,n ,j jj k j,ib kn in ,n ,kb in, j j i, ib in,kb jn,ib jn,kb ,b , ib ,b,bn,bn,kb ,b ,b j, ib ,kn ,n j n, in ,b Cbn,n,kb kn ,bn ,j jj k,ib kn ,

15、kb in, j i, ib in,kb jn,ib jn,bn,bn,Cb,n,寫成矩陣形式為：,2010-03-19,21,10/21/2020,矢量的坐標變換,旋轉(zhuǎn)矢量的坐標變換,固定矢量的坐標變換,固定矢量的坐標變換是一個在空間大小和方向都不 變的矢量在兩個不同方位的坐標系軸向分量之間的變 換關(guān)系，也即同一個矢量在兩個不同的坐標系軸向投 影之間的變換關(guān)系。,是指一個矢量大小不變，但在方向上轉(zhuǎn)動了一個位 置，這個矢量轉(zhuǎn)動前和轉(zhuǎn)動后在同一個坐標系軸向 分量之間的變換關(guān)系。,2010-03-19,22,10/21/2020,固定矢量的坐標變換,Zk r bT b,r X bib Yb jb,

16、bb,b:載體坐標系 n:地理坐標系,一個矢量r，寫成載體坐標系軸向分量形式：,Zk r nT n,r X nin Yn jn,nn,同一個矢量r，如果寫成地理坐標系軸向分量形式：,r bT b r nT n, Zb ,b , b j , Xb ,r b Y,kb ,b , ib , Zn ,n , X n , Y,r n,n jn kn , in ,b Cb n,n,bTbTbnT,n,rb rC n rn,rbT Cb r nT,n,由于r是同一個矢量，故,由于正交陣，故,n b,bTb1,n,(Cn ) (C), C,兩邊求轉(zhuǎn)置,nTT,bTbTT,(Cn )(r), (r), Cn r

17、 b,b,r n, Cb r n,n,r b,2010-03-19,23,10/21/2020,旋轉(zhuǎn)矢量的坐標變換,由于動坐標系隨同矢量轉(zhuǎn)動，故rbT=rnT 互逆,r轉(zhuǎn)動前的矢量,r 轉(zhuǎn)動后的矢量 假定有一個動坐標系和矢量固連，在矢量轉(zhuǎn)動 前，取動坐標系b和參考坐標系n重合，則： r=rnTn,b Cb nr r nT Cb n,nn,r =rbTb,如果用r n表示轉(zhuǎn)動后的矢量在參考坐標系軸向的 分量構(gòu)成的矩陣，則,r rnT n,rnT r nT Cb,n, C n r n,b,rn, Cb r n,n,r b,由于坐標系不動而是矢量轉(zhuǎn)動，它 相應于矢量固定時坐標系方向轉(zhuǎn)動,nTb,n,

18、 rCn,2010-03-19,24,10/21/2020,2010-03-19,25,方向余弦矩陣微分方程,由矢量相對導數(shù)和絕對導數(shù)的關(guān)系式, r,dr dr dtdt,nb,nb,假定地理坐標系為參考坐標系，作為參考 坐標系認為它在空間是不動的，即, 0,n,dt,dr, nb r,dr dt,b, b r bk r nbbnbb,b,r., nbx , ,nb nbz,0, nbznby 0, ,0,nbynbx,bk,b,nb,載體坐標系相對地理坐標系的轉(zhuǎn)動角速度在b系軸 向分量的反對稱矩陣（Skew symmetric matrix）,25,10/21/2020,2010-03-19

19、,26,方向余弦矩陣微分方程,另外，從固定矢量的坐標變換關(guān)系式有, C. br n Cbr. n nn,r. b, Cb r n,n,r b,兩邊求導, 0 r.b r.b,r. n, C. br n C. bCnrb,nnb,考慮,bn, Cn Cb,bk,nb,.,b,兩邊同右乘Cn,bkb, nb Cn,b n,C,., nb rb nb rb,bbk,nbk, Cb nb,n,C.b,bk, nb, ( bk )T nb,bk1,(nb ),返回3.2,26,10/21/2020,方向余弦矩陣微分方程的幾種表示形式,bkb, nb Cn,b n n b,C,.,nbk,bnb, C,C

20、,.,式中的角速度都是用載體坐標系內(nèi)的分量 表示的，如果角速度在地理坐標系軸向的 分量表示時，則可用角速度反對稱矩陣的 相似變換來得到。,bnkn,nnbb,bk nb, C C,nbkb,bnbn,nk nb, CC,左式可以用展開的方式推導,bnk,nnb,b n, C ,C,.,nkn,n bnbb, C,C,.,在捷聯(lián)慣導系統(tǒng)中，由于陀螺是固聯(lián)于載體上的， 所以直接測量的角速度是載體坐標系軸向的分量。 那么計算時哪個公式最方便？,常用的姿態(tài)矩陣微分方程的4種形式。,2010-03-19,27,10/21/2020,方向余弦矩陣微分方程,陀螺儀測量的是載體相對于慣性空間的角速度 b,nn

21、bk bbnb, C,C,.,ib,而式中需要的則是,bk nb,兩者的關(guān)系為：,bk in,bk ib,bk nb, , ,bnkn,nibb,bk ib,C C, ,bk ),n (bk ib,in,b,n b, C,C, ,.,nkn inb,nbk bib,C, C, ,包括載體的 姿態(tài)和航向 的變換角速 度，數(shù)值較 大（如飛機 可達400。/s）,則是地球角速 度和載體的位 移運動相對地 心形成的角速 度，這個角速 度比較小，一 般為每小時幾 十度。,在實時計算上式時，第一 項需要用較高的速度計算 ，用迭代算法時，迭代頻 率要高，而第二項則可用 較低迭代頻率計算。可以 看作是對第一項

22、的修正。,2010-03-19,28,10/21/2020,2010-03-19,29,3.2.2.4 矩陣微分方程的解 下面是解方程的推導過程。,C (t) C(0) C(t)dt,nbk,bnb,b,0,C (t) C(0) C(0) C (t)dtdt,t,nbkbk,nbnb,t,b,n,b,00,C (t)dtdt, C(0) C(0)dt ,ttt,bk,nb,bk,nb,n,b,bk,nb, ,00,0,把等式右邊的表達式逐次代入積分號內(nèi), , ,dtdt ,C (t) C(0) C(0)dt C(0),ttt,bk,nb,bk,nb,bk,nb,n,b,tt,bk,nb,bk,

23、nb,t,bk,nb,n,b,C (t)dtdtdt,0 0 0,0 0,0,dtdt,C(t) C(0)I dt ,ttt,bk,nb,bk,nb,bk,nb,n,b, ,00,0, ,C(t)dtdtdt .,ttt,bk,nb,bk,nb,bk,nb,n,b,000,第2次代入得,這樣不斷的進行代入，便得到,bk 2,0,1 2,0,00,0 0,(dt),dtdt dtddt , ,t,nb,t,bk,nb,tt,bk,nb,tt,bk,nb,bk,nb,bk 3,0,1 6,0 0 0,(dt),C (t)dtdtdt , ,t,nb,ttt,bk,nb,bk,nb,bk,nb,n,

24、b,C. n Cn bk bbnb 變系數(shù)的齊次微分方程,t,n,可用畢卡(Peano-Baker)逼近法求解，積分上式則有,第1次代入得,C (t) C(0)I dt dtdt3 .,0,6,1,bk2,00,2,1,t,bk,nb,t,nb,t,bk,bnb,故 n,Cb (t) C(0)e,t,bk,nb,dt,n,0,29,10/21/2020,2010-03-19,30,矩陣微分方程的解,Cb (t) C(0)e,t,bk,nb,dt,n,0,C(t t) C(t)e,tn1,tn,bk,nb dt,n,b,bk,nb dt nb,tn1,t n,bk,bk,C n (t t) C(

25、t)enb,b, , b b, , 0, b b,0,0 b,nbynbx,b nbx,nbz,nby,nbz,bk,nb,bk2,12nb 3,e K I K K(),nb,bk,bk,nb,I單位陣;K1, K2, K3系數(shù)。,t=tn+1- tn,下面來求三個系數(shù)。由矩陣的特征方程,如果知道了K1, K2, K3三個系數(shù)，則矩陣指數(shù)函數(shù)就可以表示成 一個矩陣二次方程。 bk,nb,來求它的特征值。, b b,det(I bk ) b b, b b,nbynbx,nbx,nbnbz,nbznby, , ,2 0,2,2,3b,b,nbz,b,nby,nbx, , ,2 0,2,2,2, ,

26、b,nbz,b,nby,b,nbx,3 2 0,01, =0 ,2,3,=士j,0,令,將矩陣的特征值代入方程,=0， K1=1, K I K bk K( bk )2 12nb3nb,enb,bk,30,10/21/2020,用201四0-03參-19數(shù)法。,31,矩陣微分方程的解,=j0 =-j0,bk2,nb3nb,2 0,1,e K I K K(),bk, bk,nb, j0,e e,0,K2 ,bk )2, 2,00, sin 0 bk 1 cos 0 (,nb,nb,e I, bk,nb,=0， K1=1 2,e K1 K 2 j0 K3 ( j0 ) e,j,2, K1 K2 j0

27、 K3 ( j0 ), j 0,2, 2K1 2K3 (0 ) 2K2 0, j 0,j 0,e e,( )2,0, 1 cos 0,3,K,j0,sin 0,Cn (t t) C(t)I sin 0 bk 1 cos 0 ( bk )2 , 2,0,0,nb,bnb,矩陣微分方程的精確解,這個精確解的前提條件是,nbk bbnb,. n,C C,+,bk,nb dt nb,tn1,t n,bk,這個式子只有在t=tn+1- tn內(nèi)角速度矢量nb方向不變的條件下才有意義，由于轉(zhuǎn)動 的不可交換性，當nb方向隨時間變化時，角速度的積分是無意義的。 用方向余弦法求解姿態(tài)矩陣避免了歐拉角法方程退化的現(xiàn)

28、象，可以全姿態(tài)工作，但 是，由于方向余弦矩陣具有九個元素，所有，解算矩陣微分方程時，實際上是結(jié)算 九個聯(lián)立微分方程，一般說來，計算工作量比較大，為了減小計算工作量，可以采,31,10/21/2020,3.2.3 四元數(shù)法,四元數(shù)理論是數(shù)學中的一個古老的分支，1943年由威廉. 哈密頓(William Hamilton)首先提出，目點是研究空間 幾何，一種類似平面問題中使用復數(shù)那樣的方法。但是 ，這個理論建立以后，長期沒有得到實際應用，直到空 間技術(shù)出現(xiàn)以后，特別是捷聯(lián)式制導技術(shù)出現(xiàn)以后，這 一古老的數(shù)學分支，又重新受到人們的重視，得到了實 際的應用。,四元數(shù)的基本概念,四元數(shù)是由1個實數(shù)單位1

29、和3個虛數(shù)單位i,j,k組成的含 有4個元的數(shù)，其形式為 Q (q0 , q1 , q2 , q3 ) q0 q1i q2 j q3k q0 q 標量矢量,2010-03-19,32,10/21/2020,3.2.3 四元數(shù)法 3.2.3.1 四元數(shù)的基本概念 3.2.3.2 四元數(shù)理論 3.2.3.3 矢量坐標變換的四元數(shù)描述 3.2.3.4 四元數(shù)和方向余弦矩陣的關(guān)系 3.2.3.5 四元數(shù)微分方程,2010-03-19,33,10/21/2020,Z Re 實軸,Im 虛軸,O, z cos j z sin ,Z z1 jz2 z e j,j 1,u uxi u y j uz k,u 1

30、,Z sin k,iuZ sin ju,ux Z sin ,Z Z cos ,q3,z,q2,y,q1,q0, v- _, v- _, v- _,_, v-,2010-03-19,34,10/21/2020,四元數(shù)的基本概念, Zcos ux sini uy sinj uz sink ,(Quaternions) Q Z,Z cos q0,Z ux sin q1,Z uy sin q2,Z uz sin q3, Q cos u sin ,Q q0 q1i q2 j q3 k Q eu,由于它具有和復數(shù)類似的形式，可看作是復數(shù)的 推廣，因此，也有“超復數(shù)”之稱。,四元數(shù)的3種表示形式,2010-

31、03-19,35,10/21/2020,坐標系的等效轉(zhuǎn)動,E,Xb,O,U Zb,N,H.,Yb,Xb,Xb, Y,Yb,Zb,b,Zb,P.,R.,H,P,R,Xb,X r,u,Yb,Yr,b,Z,Zr,2010-03-19,36,10/21/2020,四元數(shù)的基本概念,如果用u表示歐拉軸向的單位矢量，則動坐標系的方 位，完全可由u和 兩個參數(shù)來確定。用u和 兩個參 數(shù)，可以構(gòu)造一個四元數(shù)， 1,如果把u寫成分量的形式則： Q cos usin i usin j usin k 2x2y2z 2 q0 q1i q2 j q3k, 2,q0 cos, 2,q1 ux sin, 2,q2 u y

32、sin, 12,q3 uz sin,Q cos u sin ,22,四元數(shù)是張量為1的四元數(shù)，即,Q (q 2 q 2 q 2 q 2 ) 2 1 0123,這樣的四元數(shù)稱作“規(guī)范化”的四元數(shù)，而用來描 述剛體定點轉(zhuǎn)動的四元數(shù)就稱作變換四元數(shù)。,u e2,2010-03-19,37,10/21/2020,3.2.3.2 四元數(shù)理論,四元數(shù)相等 如果兩個四元數(shù)對應的元素相等，則兩個四元數(shù)相等。 四元數(shù)相加 0 1i 2 j 3 k m0 m1i m2 j m3k, 0 m0 (1 m1 )i (2 m2 ) j (3 m3 )k 對應元素相加,則,四元數(shù)相加，服從一般加法的交換律和結(jié)合律，即,

33、( ) ( ),（交換律） （結(jié)合律）,2010-03-19,38,10/21/2020,四元數(shù)理論,四元數(shù)與標量相乘 a a0 a1i a2 j a3k,式中a標量,各個元素分別乘以標量,(ab) (ba) a a (a b) a b a( ) a a,（分配律）,（交換律）,（結(jié)合律）,2010-03-19,39,10/21/2020,四元數(shù)理論,四元數(shù)與四元數(shù)相乘 0 1i 2 j 3k m0 m1i m2 j m3 k 。 (0 1i 2 j 3 k ) 。 (m0 m1i m2 j m3 k ) 0 m0 1m1 2 m2 3m3 0 (m1i m2 j m3k),乘積的矢量形式,2

34、010-03-19,40,10/21/2020,2010-03-19, 3 41,四元數(shù)理論, 。 0m0 1m1 2m2 3m3 i(0m1 1m0 2m3 3m2 ) j(0m2 2m0 3m1 1m3 ) k(0m3 3m0 1m2 2m1) 乘積的四元數(shù)形式,n1 0m1 1m0 2m3 3m2 n2 0m2 2m0 3m1 1m3 n3 0m3 3m0 1m2 2m1,n0 0m0 1m1 2m2 3m3 ,nnn T,Q(n) n,3,2,1,0, T,Q() ,3,2,1,0,mmm T,Q(m) m,123,0,00 1 2 3,0,1 ,1,0,1 ,2,2,3,0,1,3,

35、2,2,3 3,21,0 3 ,n , m ,n ,m ,n , m ,n ,m ,Q(n) M ()Q(m),n0 ,m0,m1 m2 m3 0 ,n1 m1,m0m3m2 1 m3m0 m2m1,n2 m2,m1 2 m0,n3 m3, , ,-矩陣四元數(shù),Q(n) M (m)Q(),矩陣的“核”,41,10/21/2020,2010-03-19,42,四元數(shù)理論,M()和M*(m)除元素不同外，其核互為轉(zhuǎn)置。 這種四元數(shù)乘積的矩陣形式，也可推廣到三個 以上的四元數(shù)乘積。如：,Q( 。 。 ) M()Q( 。 ) M ()M (P)Q(m) M (P)Q( 。 ) M (P)M()Q(m)

36、,M()M (P) M (P)M (),說明M*和M具有可交換性。而一般的矩陣相乘，則是不可交換的. Q(。 。 ) M ()Q(。 ) M ()M (m)Q(P) M(。 )Q(P),M ( 。 ) M ()M (m),Q( 。 。 ) M (P)Q( 。 ) M (P)M (m)Q() M ( 。 )Q(),M (。 ) M (P)M (m),順序相乘,逆序相乘,類似正交陣的乘積的轉(zhuǎn)置或方陣乘積的求逆，也是逆序,42,10/21/2020,四元數(shù)理論 。 。 ,( 。 ) 。 。 ( 。 ) 。 ( ) 。 。 四元數(shù)的共軛 如果一個四元數(shù)為八=+ 則定義其共軛四元數(shù)為*=,結(jié)合律 分配律

37、,推理1 (八+M)*= 八*+M* 四元數(shù)之和的共軛等于共軛之和 推理2 (八M)*= M*八*兩個四元數(shù)之積的共軛等于共軛 四元數(shù)等于兩個四元數(shù)共軛之積取相反的順序。 四元數(shù)的范數(shù),四元數(shù)的范數(shù)定義為,2222,3,2,1,0, ,2010-03-19,43,10/21/2020,四元數(shù)理論,=八八* =八*八 八=1的四元數(shù)稱為規(guī)范化的四元數(shù)。 八M =八 M=M 八,四元數(shù)的逆,則八-1八=八八-1,1,22,0123,2,2, ,2010-03-19,44,10/21/2020,3.2.3.3 矢量坐標變換的四元數(shù)描述,一個矢量r在參考坐標系（這里用地理坐標系作參 考系）軸向的分量形

38、式為 r=xnin+yn jn+ znkn 式中xn, yn, zn為r在地理坐標系軸向的分量。 in, jn, kn為地理坐標系軸向的單位矢量。 用xn, yn, zn把r寫成四元數(shù)形式即： Rn=0+xni+yn j+ znk =0+r Rn就叫做矢量r在地理坐標系上的四元數(shù)影像。 i, j, k是四元數(shù)的虛數(shù)單位，而r則是四元數(shù)的矢量 部分。 顯然，如果認為i, j, k和in, jn, kn重合，則四元 數(shù)的矢量部分就是三維空間的矢量r本身。,2010-03-19,45,10/21/2020,旋轉(zhuǎn)矢量的坐標變換,定義假設(shè)矢量r繞通過定點“O”的某一軸轉(zhuǎn)動了一個 角度，則和矢量固聯(lián)的動坐

39、標系和參考坐標系之間的 變換四元數(shù)為：,22,Q cos usin,式中u為轉(zhuǎn)軸方向的單位矢量。這個四元數(shù)的范數(shù)為,Q q2 q2 q2 q2 1 0123,轉(zhuǎn)動前的矢量用r表示，轉(zhuǎn)動后的矢量用r表示,則r和r的關(guān)系可由四元數(shù)來描述，即,稱作“規(guī)范化”的四元數(shù).,r Q。 r 。Q*,Q* cos usin四元數(shù)的共軛四元數(shù) 2 2,黃式兩邊同時左乘Q*右乘Q得 因為,Q*。r。Q Q*。Q 。 r 。 Q*。Q Q*。Q Q 。 Q* 1,2 2 (cos usin) 。 (cos usin) cos sin 1,22,22,2,2,r Q* 。 r。Q,2010-03-19,46,10/2

40、1/2020,2010-03-19,47,證明,A O 當矢量r繞OO旋轉(zhuǎn)時，矢端A在空間的軌跡是一個圓，這個 圓平面和轉(zhuǎn)軸垂直，圓心為O在旋轉(zhuǎn)軸上。在圓上取一點B， 使AOB=90。，則按矢量關(guān)系有下列關(guān)系式：,O,r,r,A,B,u,A,O,O,B,OO,u,O u,r,A,OO (r u)u,OO OA r,因為ab=|a|b|cos,OA =r OO= r(ru)u OB= uOA= u( r(ru)u),= ur (ru)uu= ur,uu=0,OB= ur,47,10/21/2020,2010-03-19,48,證明, A,B,OAcos A,O,OBsin,O,O,r,r,A,B

41、,u,A,如果Q=q0+q，R=r0+r,則利用式可以寫成矢量形式為：,QR= q0 r0+ q0r+qr0qr+qr 利用上式將QrQ*展開,v-_v-_ q0q,Q cos usin,22,r=R=r0+rr0=0,q0 OA = r(ru)u OB= ur,q, sin (ur) cos r (ur)sin v-2 _ 2 v- _2,八= 0 m0+0 m+ m0 m+ m OA = OA cos+ OBsin = cos ( r(ru)u)+ ur sin r=OO+OA =(ru)u+cosr cos (ru)u+sin (ur),=(1 cos)(ru)u +cos r+sin

42、(ur),Q。r cosr usinr (ur)sin,222,48,10/21/2020,2010-03-19,49,推導,q 0 q0q 0 q*qq0 (cos r (u r)sin ) (usin ) (cos r (u r)sin ) (usin ) 2 v- 2 _2 2 v- 2 _2 qq*qq*, sin cos (u r) sin2 (u r)u cos (cos r (u r)sin ) 2 v- 2 _ 2v- _ 2 2 v- _2, Q 。 r 。 Q* (sin(u r) cosr (u r)sin) 。 (cos usin),2,222,2q, = sincos

43、(u r) sin2(u r)u cos2r sincos(u r) sincos(u r),2222,Q 。 R q0r0 q0r qr0 q r q r,sin2 (u r) u sin cos (r u) sin2 (u r) u 222 2 第一項與第五項相消，第六項為零第四項與第七項合并 sin2 (u r)u cos2 r sin (u r) sin2 (u r) u,222,22,22,q0,q0，q,49,10/21/2020,證明 QrQ* sin2 (u r)u cos2 r sin(u r) sin2 (u r) u,222,利用 QrQ*, sin2 (u r)u co

44、s2 r sin (ur) sin2 (uu)r (u r)u 22 2, cos2 r sin2 r sin (ur) 2sin2 (u r)u,222,r Q。r 。 Q*, cos r sin (ur) (1 cos )(u r)u r=OO+OA =(ru)u+cosr cos (ru)u+sin (ur) =(1 cos)(ru)u +cosr+sin (ur),(ur)u=(uu)r(ur)u,2010-03-19,50,10/21/2020,固定矢量的坐標變換,如果矢量固定不動，而動坐標系相對參考坐標系轉(zhuǎn)動了一個角度， 則以四元數(shù)描述的矢量在兩個坐標系上的分量的變換關(guān)系為,r Q

45、。r 。 Q*,如果將四元數(shù)Q,R,M的四個元寫成列矢量，即表示成,3.2.3.4 四元數(shù)和方向余弦矩陣的關(guān)系,Q(q) q1 q2 ,q0 ,q3 ,Q(r) 1 Q(m) r2 ,r0 m0 r m ,r3 m3 ,m2 ,1 ,將固定矢量的坐標變換式，Rb =Q*RrQ 寫成矩陣形式，并以地理坐標系為參考坐標系，則有,Q(Rb ) M(Q*)M(Q)Q(Rn ),Rb =QRrQ,*,Rr =QRbQ*Rb =Q*RrQ,2010-03-19,51,10/21/2020,3.2.3.4四元數(shù)和方向余弦矩陣的關(guān)系,Q(Rb ) M(Q*)M(Q)Q(Rn ),Q(R ) 0 xyz ,T,

46、bb,bb,Q(R ) 0,xyz ,T,n q0 q1 q2 q3 qq,n,n,n,2 ,qq q,M (Q*) q qq q2, q, q3, q0q1,q1q0,1,0,3,2,3,10,q2q3,q,2 ,q q ,q2,M *(Q) ,q3,0 q3q0q1 q2 q1q0 q0q1q2q3 qqq,3,1,yn ,n , qx,zn , q1 q2 q3 0 qq,2 ,q2 q3,2 ,qq,q0,03 q3q0q1 q2 q1q0, q2,b , q3,03 q3q0q1 q2 q1q0,yb ,x,zb , 0 ,1,1,2010-03-19,52,10/21/2020,四

47、元數(shù)與姿態(tài)矩陣的關(guān)系, q2(q2q3 q0q1)yn ,2(q1q3 q0q2 )xn ,n , 0 ,0123 , , ,q2 q2 q2 q2,q q q q,2(q q q q ),2(q q q q ),q q,q q )q,0 2(q1q2 q0q3 ), q2 q2 23,q0 q1,z,2(q q,0 0 0,0,0,2222,2301,02,13,2,2 23,22,1,0,03,12,22,0123,2(q2q3 q0q1 )yn ,n ,2(q1q3 q0q2 )xn , ,2(q1q3 q0q2 ),yb 2(q1q2 q0q3 ),q0 q1,q0 q1 q q,q2

48、 q2 q2 q2 0123 2(q2q3 q0q1),2(q1q2 q0q3 ), q2 q3,zb ,xb ,z,2222,23,2,222,q2 q2 q2 q2 ,2(q2q3 q0q1),3 ,2(q1q3 q0q2 ),2(q q q q),Cn 2(q1q2 q0q3 ),q0 q1,2(q q q q ), q2 q2 23,q0 q1,2(q1q2 q0q3 ), q2 q3,012,2301,1302,22,2222,b,2010-03-19,53,10/21/2020,由四元數(shù)計算姿態(tài)矩陣,由 b Cbn 可知上式可寫為,n,zb T31T33 zn ,23 n ,y,T13 xn ,y T,T11T12 22 T32,b ,xb ,TT,21,T23 2(q2q3 q0q1 ) T31 2(q1q3 q0