慣性導(dǎo)航原理-文檔資料

1、123iiiizyOx4eeezyOxeweOziOzeeyOx5tttzyOx6nnnzyOxnztznxtxnyty 7pppzyOx8bbbzyOx910四元數(shù)：描述剛體角運(yùn)動(dòng)的數(shù)學(xué)工具四元數(shù)：描述剛體角運(yùn)動(dòng)的數(shù)學(xué)工具 ( (quaternions)針對(duì)捷聯(lián)慣導(dǎo)系統(tǒng)，可彌補(bǔ)歐拉參數(shù)在描述和解算方面的不足。針對(duì)捷聯(lián)慣導(dǎo)系統(tǒng)，可彌補(bǔ)歐拉參數(shù)在描述和解算方面的不足。 四元數(shù)的表示四元數(shù)的表示由一個(gè)實(shí)單位和三個(gè)虛數(shù)單位由一個(gè)實(shí)單位和三個(gè)虛數(shù)單位 i, j, k 組成的數(shù)組成的數(shù) kPjPiPq3211或者省略或者省略 1，寫成，寫成kPjPiPq321i, j, k 服從如下運(yùn)算公式：服從如下運(yùn)

2、算公式： 11i, j, k 服從如下運(yùn)算公式服從如下運(yùn)算公式 1kkjjiikijjiijkkjjkiikkPjPiPq321 稱作標(biāo)量部分，稱作標(biāo)量部分， kPjPiP321稱作矢量部分稱作矢量部分 四元數(shù)的另一種表示法四元數(shù)的另一種表示法 Pq,P 泛指矢量部分泛指矢量部分提示：四元數(shù)與剛體轉(zhuǎn)動(dòng)的關(guān)系提示：四元數(shù)與剛體轉(zhuǎn)動(dòng)的關(guān)系12kPjPiPq321kjivM3211四元數(shù)加減法四元數(shù)加減法 MqkPjPiPv)()()()(332211或簡(jiǎn)單表示為或簡(jiǎn)單表示為 PvMq,132四元數(shù)乘法四元數(shù)乘法 )(321321kjivkPjPiPMq)(332211PPPviPPvP)(2332

3、11jPPvP)(311322kPPvP)(122133或簡(jiǎn)單表示為或簡(jiǎn)單表示為 PvPPvMq 關(guān)于相乘符號(hào)關(guān)于相乘符號(hào) 關(guān)于交換律和結(jié)合律關(guān)于交換律和結(jié)合律143共軛四元數(shù)共軛四元數(shù) 僅向量部分符號(hào)相反的兩個(gè)四元數(shù)僅向量部分符號(hào)相反的兩個(gè)四元數(shù) ),(Pq和和 ),(*Pq互為共軛互為共軛 可證明：可證明： *)*(qhqh4四元數(shù)的范數(shù)四元數(shù)的范數(shù) q定義定義 2322212*PPPqqq1q則稱為規(guī)范化四元數(shù)則稱為規(guī)范化四元數(shù) 155逆四元數(shù)逆四元數(shù) qq11qq*當(dāng)當(dāng) 1q時(shí)時(shí) *1qq6四元數(shù)的除法四元數(shù)的除法 若若 Mqh 則則 1 Mhq若若 Mhq 則則 Mhq1不能表示為不

4、能表示為 hMq （含義不確切含義不確切 ）16一個(gè)坐標(biāo)系或矢量相對(duì)參考坐標(biāo)系旋轉(zhuǎn)，一個(gè)坐標(biāo)系或矢量相對(duì)參考坐標(biāo)系旋轉(zhuǎn)， 轉(zhuǎn)角為轉(zhuǎn)角為， 轉(zhuǎn)軸轉(zhuǎn)軸 n n 與參考系各軸間的方向余弦值為與參考系各軸間的方向余弦值為cos、cos、cos。 則表示該旋轉(zhuǎn)的四元數(shù)可以寫為則表示該旋轉(zhuǎn)的四元數(shù)可以寫為 nkjiq2sin2coscos2sincos2sincos2sin2cos為特征四元數(shù)為特征四元數(shù) （范數(shù)為范數(shù)為 1 ）四元數(shù)既表示了轉(zhuǎn)軸方向，又表示了轉(zhuǎn)角大小（轉(zhuǎn)動(dòng)四元數(shù)）四元數(shù)既表示了轉(zhuǎn)軸方向，又表示了轉(zhuǎn)角大小（轉(zhuǎn)動(dòng)四元數(shù)）17如果矢量如果矢量 R 相對(duì)固定坐標(biāo)系旋轉(zhuǎn)相對(duì)固定坐標(biāo)系旋轉(zhuǎn)，旋轉(zhuǎn)四元

5、數(shù)為，旋轉(zhuǎn)四元數(shù)為 q，轉(zhuǎn)動(dòng)后，轉(zhuǎn)動(dòng)后的矢量為的矢量為 R， 則這種轉(zhuǎn)動(dòng)關(guān)系可通過(guò)四元數(shù)旋轉(zhuǎn)運(yùn)算來(lái)實(shí)現(xiàn)則這種轉(zhuǎn)動(dòng)關(guān)系可通過(guò)四元數(shù)旋轉(zhuǎn)運(yùn)算來(lái)實(shí)現(xiàn)1 qRqR含義：矢量含義：矢量 R 相對(duì)固定坐標(biāo)系產(chǎn)生旋轉(zhuǎn)，相對(duì)固定坐標(biāo)系產(chǎn)生旋轉(zhuǎn)，轉(zhuǎn)角和轉(zhuǎn)軸由轉(zhuǎn)角和轉(zhuǎn)軸由 q 決定決定18如果如果坐標(biāo)系坐標(biāo)系 OXYZ 發(fā)生發(fā)生 q 旋轉(zhuǎn)，得到新坐標(biāo)系旋轉(zhuǎn)，得到新坐標(biāo)系 OXYZ 一個(gè)相對(duì)原始坐標(biāo)系一個(gè)相對(duì)原始坐標(biāo)系 OXYZ 不發(fā)生旋轉(zhuǎn)變換的矢量不發(fā)生旋轉(zhuǎn)變換的矢量 V zkyjxiV矢量矢量 V 在新坐標(biāo)系上在新坐標(biāo)系上 OXYZ 的投影為的投影為 kzjyixV則不變矢量則不變矢量 V 在兩個(gè)坐標(biāo)系上的投影

6、之間存在如下關(guān)系：在兩個(gè)坐標(biāo)系上的投影之間存在如下關(guān)系： qVqVee1式中式中 zkyjxiVekzjyixVe分別稱為矢量分別稱為矢量 V 在坐標(biāo)系在坐標(biāo)系 OXYZ 和和 OXYZ 上的映像上的映像19zkyjxiV kzjyixVkzjyixVezkyjxiVe20qVqVee1將該投影變換式展開，也就是把將該投影變換式展開，也就是把kzjyixVezkyjxiVekPjPiPq321kPjPiPq3211代入上述投影變換式代入上述投影變換式kzjyix)(321kPjPiP)(zkyjxi)(321kPjPiP進(jìn)行四元數(shù)乘法運(yùn)算，整理運(yùn)算結(jié)果可得進(jìn)行四元數(shù)乘法運(yùn)算，整理運(yùn)算結(jié)果可得2

7、1zyxCzyx其中方向余弦矩陣其中方向余弦矩陣 C222123213223113223212223212313212322212)( 2)( 2)( 2)( 2)( 2)( 2PPPPPPPPPPPPPPPPPPPPPPPPPPP22多次旋轉(zhuǎn)多次旋轉(zhuǎn)的合成的合成對(duì)于一個(gè)坐標(biāo)系經(jīng)過(guò)多次旋轉(zhuǎn)后，新坐標(biāo)系和原始坐標(biāo)系之間對(duì)于一個(gè)坐標(biāo)系經(jīng)過(guò)多次旋轉(zhuǎn)后，新坐標(biāo)系和原始坐標(biāo)系之間的關(guān)系等效于一個(gè)一次轉(zhuǎn)動(dòng)的效果，的關(guān)系等效于一個(gè)一次轉(zhuǎn)動(dòng)的效果， 相應(yīng)地有合成轉(zhuǎn)動(dòng)四元數(shù)相應(yīng)地有合成轉(zhuǎn)動(dòng)四元數(shù) 假定假定 q1、q2 分別是第一次轉(zhuǎn)動(dòng)、第二次轉(zhuǎn)動(dòng)的四元數(shù)分別是第一次轉(zhuǎn)動(dòng)、第二次轉(zhuǎn)動(dòng)的四元數(shù) q 是合成轉(zhuǎn)動(dòng)的四元

8、數(shù)，是合成轉(zhuǎn)動(dòng)的四元數(shù)，那么有如下關(guān)系成立：那么有如下關(guān)系成立： 21qqq上式中上式中 q1 和和 q2 的轉(zhuǎn)軸方向必須以映象的形式給出。的轉(zhuǎn)軸方向必須以映象的形式給出。 如果如果 q1 和和 q2 的轉(zhuǎn)軸方向都以原始坐標(biāo)系的分量表示，則有的轉(zhuǎn)軸方向都以原始坐標(biāo)系的分量表示，則有 12qqq23用四元數(shù)旋轉(zhuǎn)變換的方法求取兩個(gè)坐標(biāo)系之間的方向余弦表。用四元數(shù)旋轉(zhuǎn)變換的方法求取兩個(gè)坐標(biāo)系之間的方向余弦表。 坐標(biāo)系坐標(biāo)系 OXYZ 相對(duì)相對(duì)OXYZ 三次旋轉(zhuǎn)，以三次旋轉(zhuǎn)，以歐拉角歐拉角 、的的形式給出。形式給出。 第一轉(zhuǎn)，繞第一轉(zhuǎn)，繞 Z 軸轉(zhuǎn)軸轉(zhuǎn)角，瞬時(shí)轉(zhuǎn)軸角，瞬時(shí)轉(zhuǎn)軸 n 和和 k 軸重合，則

9、轉(zhuǎn)動(dòng)四元軸重合，則轉(zhuǎn)動(dòng)四元數(shù)為數(shù)為 kq2sin2cos124第二轉(zhuǎn)，繞第二轉(zhuǎn)，繞 OX1 軸轉(zhuǎn)軸轉(zhuǎn)角，角，瞬時(shí)轉(zhuǎn)軸瞬時(shí)轉(zhuǎn)軸 n 的方向表示式為的方向表示式為 )sin(cosji其轉(zhuǎn)動(dòng)四元數(shù)為其轉(zhuǎn)動(dòng)四元數(shù)為 nq2sin2cos2)sin(cos2sin2cosji25由于由于 q1 和和 q2 的瞬時(shí)轉(zhuǎn)軸的瞬時(shí)轉(zhuǎn)軸都是以同一個(gè)坐標(biāo)系的方向余弦來(lái)都是以同一個(gè)坐標(biāo)系的方向余弦來(lái)表示，則合成轉(zhuǎn)動(dòng)四元數(shù)表示，則合成轉(zhuǎn)動(dòng)四元數(shù) q 的計(jì)算采用：的計(jì)算采用：12qqqkji2sin2cos)sin(cos2sin2coskji2sin2cos2sin2sin2cos2sin2cos2cos26以瞬時(shí)轉(zhuǎn)

10、軸以瞬時(shí)轉(zhuǎn)軸映象映象形式給出形式給出轉(zhuǎn)動(dòng)四元數(shù)的表達(dá)式并求轉(zhuǎn)動(dòng)四元數(shù)的表達(dá)式并求出合成轉(zhuǎn)動(dòng)四元數(shù)出合成轉(zhuǎn)動(dòng)四元數(shù) 第一次轉(zhuǎn)時(shí)，映象形式的第一次轉(zhuǎn)時(shí)，映象形式的 q1 和非映象形式的和非映象形式的 q1 是是一致的：一致的： kq2sin2cos127第二轉(zhuǎn)繞第二轉(zhuǎn)繞 OX1 軸轉(zhuǎn)軸轉(zhuǎn) 角角瞬時(shí)轉(zhuǎn)軸瞬時(shí)轉(zhuǎn)軸 n 是由是由 OX 經(jīng)過(guò)經(jīng)過(guò)第一轉(zhuǎn)轉(zhuǎn)換來(lái)的第一轉(zhuǎn)轉(zhuǎn)換來(lái)的OX 軸對(duì)應(yīng)單位矢量軸對(duì)應(yīng)單位矢量 i，所，所以定義以定義 n 的映象為的映象為 i則則 q2 的映象表示式為的映象表示式為 iq2sin2cos228第三轉(zhuǎn)，繞第三轉(zhuǎn)，繞 OZ 軸轉(zhuǎn)動(dòng)軸轉(zhuǎn)動(dòng) 角角瞬時(shí)轉(zhuǎn)軸瞬時(shí)轉(zhuǎn)軸 n 是由是由 OZ

11、經(jīng)過(guò)經(jīng)過(guò)第一轉(zhuǎn)和第二轉(zhuǎn)轉(zhuǎn)換來(lái)的第一轉(zhuǎn)和第二轉(zhuǎn)轉(zhuǎn)換來(lái)的OZ 軸對(duì)應(yīng)單位矢量軸對(duì)應(yīng)單位矢量 k，所以定義所以定義 n 的映象為的映象為 k則則 q3 的映象表示式為的映象表示式為kq2sin2cos329由于由于 q1 、q2 和和 q3 都是映象形式都是映象形式 ，所以三次轉(zhuǎn)動(dòng)的合成轉(zhuǎn)動(dòng)所以三次轉(zhuǎn)動(dòng)的合成轉(zhuǎn)動(dòng)四元數(shù)四元數(shù) q 為為321qqqqkik2sin2cos2sin2cos2sin2cosji2sin2sin2cos2sin2cos2cosk2sin2cos據(jù)此可算出對(duì)應(yīng)的方向余弦表?yè)?jù)此可算出對(duì)應(yīng)的方向余弦表 30坐標(biāo)系旋轉(zhuǎn)時(shí)，不變矢量坐標(biāo)系旋轉(zhuǎn)時(shí)，不變矢量 V 在兩個(gè)坐標(biāo)系上的投影之間

12、存在在兩個(gè)坐標(biāo)系上的投影之間存在如下關(guān)系：如下關(guān)系： qVqVee1在一些資料中，四元數(shù)的轉(zhuǎn)動(dòng)公式也經(jīng)常寫成如下的形式在一些資料中，四元數(shù)的轉(zhuǎn)動(dòng)公式也經(jīng)常寫成如下的形式 1qqVVEE這個(gè)公式的意義是說(shuō)，在一個(gè)超復(fù)數(shù)空間中，或者在一個(gè)固這個(gè)公式的意義是說(shuō)，在一個(gè)超復(fù)數(shù)空間中，或者在一個(gè)固定坐標(biāo)系中，矢量定坐標(biāo)系中，矢量 VE 按著四元數(shù)按著四元數(shù) q 所表示的方向和大小轉(zhuǎn)所表示的方向和大小轉(zhuǎn)動(dòng)了一個(gè)角度，得到一個(gè)新的矢量動(dòng)了一個(gè)角度，得到一個(gè)新的矢量 VE31四元數(shù)法能得到迅速發(fā)展，是由于飛行器控制與導(dǎo)航的發(fā)展，要四元數(shù)法能得到迅速發(fā)展，是由于飛行器控制與導(dǎo)航的發(fā)展，要求更合理地描述剛體空間運(yùn)

13、動(dòng)，以及便于計(jì)算機(jī)的應(yīng)用。求更合理地描述剛體空間運(yùn)動(dòng)，以及便于計(jì)算機(jī)的應(yīng)用。采用方向余弦矩陣描述飛行器運(yùn)動(dòng)時(shí)，要積分矩陣微分方程式：采用方向余弦矩陣描述飛行器運(yùn)動(dòng)時(shí)，要積分矩陣微分方程式： CC式中式中C為動(dòng)坐標(biāo)系轉(zhuǎn)置到定坐標(biāo)系的方向余弦矩陣，為動(dòng)坐標(biāo)系轉(zhuǎn)置到定坐標(biāo)系的方向余弦矩陣，為動(dòng)坐為動(dòng)坐標(biāo)系相對(duì)定坐標(biāo)系旋轉(zhuǎn)角速度標(biāo)系相對(duì)定坐標(biāo)系旋轉(zhuǎn)角速度的反對(duì)稱矩陣：的反對(duì)稱矩陣： 000 xyxzyz包含包含 9 個(gè)一階微個(gè)一階微分方程式，計(jì)算分方程式，計(jì)算量比較大量比較大 32如果采用四元數(shù)法，則是要求解四元數(shù)方程式如果采用四元數(shù)法，則是要求解四元數(shù)方程式 qq21q 為動(dòng)坐標(biāo)系的轉(zhuǎn)動(dòng)四元數(shù)，為動(dòng)

14、坐標(biāo)系的轉(zhuǎn)動(dòng)四元數(shù)， 為動(dòng)坐標(biāo)系相對(duì)定坐標(biāo)系為動(dòng)坐標(biāo)系相對(duì)定坐標(biāo)系的旋轉(zhuǎn)角速度，也表示為四元數(shù)的旋轉(zhuǎn)角速度，也表示為四元數(shù) kjizyx 0按四元數(shù)乘積展開按四元數(shù)乘積展開 xyzyzyyzxzyxPPPPPPPPPPPP2131323213212222只要解四個(gè)一階微分只要解四個(gè)一階微分方程式組方程式組即可即可3334RrMaMgMaRMgTMaMTRamaraRa 根據(jù)質(zhì)心運(yùn)動(dòng)定理和相對(duì)運(yùn)動(dòng)學(xué)原理，根據(jù)質(zhì)心運(yùn)動(dòng)定理和相對(duì)運(yùn)動(dòng)學(xué)原理， 飛行體質(zhì)心運(yùn)動(dòng)的微分方程（在慣性坐標(biāo)系下）為：飛行體質(zhì)心運(yùn)動(dòng)的微分方程（在慣性坐標(biāo)系下）為： 式中，式中，-飛行體的質(zhì)量；飛行體的質(zhì)量；-推力；推力；-空氣阻

15、力；空氣阻力；-慣性空間飛行時(shí)，導(dǎo)彈質(zhì)心加速度；慣性空間飛行時(shí)，導(dǎo)彈質(zhì)心加速度；，-由推力產(chǎn)生的加速度；由推力產(chǎn)生的加速度；-由阻力引起的阻力加速度。由阻力引起的阻力加速度。35RTagaa)( mamgmar)(gaarra)(RTraaa由上式可得出由上式可得出 飛行體質(zhì)心運(yùn)動(dòng)的微分方程（在彈體坐標(biāo)系下）為：飛行體質(zhì)心運(yùn)動(dòng)的微分方程（在彈體坐標(biāo)系下）為： 或或 ， 式中式中是動(dòng)點(diǎn)的相對(duì)加速度，將（是動(dòng)點(diǎn)的相對(duì)加速度，將（* *）代入上式）代入上式 得得 36)(gaasTaRasaa)()(agmmamgf由上式可知，測(cè)得的由上式可知，測(cè)得的是推力加速度是推力加速度和阻力加速度和阻力加速度

16、的矢量和，稱為視加速度，在實(shí)際的測(cè)試中由加速度傳感器得到的值是的矢量和，稱為視加速度，在實(shí)際的測(cè)試中由加速度傳感器得到的值是在敏感軸上的分量，實(shí)際的慣性坐標(biāo)系下的加速度在敏感軸上的分量，實(shí)際的慣性坐標(biāo)系下的加速度可通過(guò)上式變換得到，在彈體坐標(biāo)系上動(dòng)點(diǎn)的力為可通過(guò)上式變換得到，在彈體坐標(biāo)系上動(dòng)點(diǎn)的力為稱為比力，加速度計(jì)實(shí)際是通過(guò)比力來(lái)測(cè)量加速度的。稱為比力，加速度計(jì)實(shí)際是通過(guò)比力來(lái)測(cè)量加速度的。373839捷聯(lián)慣導(dǎo)系統(tǒng)算法概述捷聯(lián)慣導(dǎo)系統(tǒng)算法概述算法：從慣性儀表輸出到導(dǎo)航與控制信息算法：從慣性儀表輸出到導(dǎo)航與控制信息捷聯(lián)慣導(dǎo)算法的基本內(nèi)容：捷聯(lián)慣導(dǎo)算法的基本內(nèi)容：一、系統(tǒng)初始化一、系統(tǒng)初始化(I

17、nitialization)：1、給定飛行器初始位置、速度等、給定飛行器初始位置、速度等2、數(shù)學(xué)平臺(tái)的初始對(duì)準(zhǔn)、數(shù)學(xué)平臺(tái)的初始對(duì)準(zhǔn)3、慣性儀表的校準(zhǔn)、慣性儀表的校準(zhǔn)二、慣性儀表誤差補(bǔ)償二、慣性儀表誤差補(bǔ)償(Compensation)三、姿態(tài)矩陣的計(jì)算三、姿態(tài)矩陣的計(jì)算四、導(dǎo)航計(jì)算四、導(dǎo)航計(jì)算五、導(dǎo)航控制信息的提取五、導(dǎo)航控制信息的提取40姿態(tài)矩陣的計(jì)算姿態(tài)矩陣的計(jì)算假設(shè)數(shù)學(xué)坐標(biāo)系模擬地理坐標(biāo)系假設(shè)數(shù)學(xué)坐標(biāo)系模擬地理坐標(biāo)系飛行器姿態(tài)的描述：飛行器姿態(tài)的描述： 航向角航向角、俯仰角、俯仰角、滾動(dòng)角、滾動(dòng)角一、歐拉微分方程一、歐拉微分方程從地理坐標(biāo)系到載體坐標(biāo)系從地理坐標(biāo)系到載體坐標(biāo)系的旋轉(zhuǎn)順序：的

18、旋轉(zhuǎn)順序： 方向余弦矩陣：方向余弦矩陣：CCCCbE41coscossincoscossinsinsinsincossincossincoscoscossinsinsincossinsinsincossincossincoscos飛行器相對(duì)地理坐標(biāo)系的角速度：飛行器相對(duì)地理坐標(biāo)系的角速度：TbEbzbEbybEbxbEb000000CCC42000000CCCcoscossin0cossincos0sin01求解歐拉角速率得求解歐拉角速率得bEbzbEbybEbx1coscossin0cossincos0sin01bEbzbEbybEbxcossin0cossincoscos0sincossi

19、nsincoscos1注意事項(xiàng)：當(dāng)注意事項(xiàng)：當(dāng) = 90 度時(shí)，方程出現(xiàn)奇點(diǎn)度時(shí)，方程出現(xiàn)奇點(diǎn)43二、方向余弦矩陣微分方程及其解二、方向余弦矩陣微分方程及其解 CCbEbEbEbCC其中其中000 xyxzyzbEb由于陀螺儀直接測(cè)得的是載體由于陀螺儀直接測(cè)得的是載體相對(duì)慣性空間的角速度，所以：相對(duì)慣性空間的角速度，所以：biEbibbEb導(dǎo)航計(jì)算可以得到導(dǎo)航計(jì)算可以得到EIE有有EbEiEbEbiECC 因此因此EbEiEbEbibbEbCC 得得EbEiEbibEbEbCCC44bEbEbEbCC的精確解（畢卡逼近）：的精確解（畢卡逼近）：)()(tCttCEbEb220000)(cos1

20、sinbEbbEbI1nnttbEbbEbdt其中其中000bEbXbEbYbEbXbEbZbEbYbEbZbEb22220)()()(bEbZbEbYbEbXbEb方向不變時(shí)的精確解方向不變時(shí)的精確解九個(gè)微分方程求解，計(jì)算量大九個(gè)微分方程求解，計(jì)算量大45三、四元數(shù)微分方程式及其解三、四元數(shù)微分方程式及其解由第一章，四元數(shù)微分方程式：由第一章，四元數(shù)微分方程式：qqb對(duì)對(duì) 的處理類似上一節(jié)的處理類似上一節(jié)b32132102/2/2/2/02/2/2/2/02/2/2/2/0PPPPPPxyzxzyyzxzyx精確解：精確解：)0(2sin2cos)(000qItq其中：其中：21ttbdt4

21、6)0(2sin2cos)(000qItq其中：其中：21ttbdt0000 xyzxzyyzxzyx22220ZYX47四、姿態(tài)和航向角的計(jì)算四、姿態(tài)和航向角的計(jì)算根據(jù)載體和地理坐標(biāo)系之間的方向余弦矩陣可確定姿態(tài)、航向角根據(jù)載體和地理坐標(biāo)系之間的方向余弦矩陣可確定姿態(tài)、航向角coscossincoscossinsinsinsincossincossincoscoscossinsinsincossinsinsincossincossincoscosbEC333231232221131211TTTTTTTTTCbE)(sin131T33231TTtg11121TTtg姿態(tài)、航向角姿態(tài)、航向角真值

22、的判斷真值的判斷48)270,180(180)180,90(180)90,0()90,0(2/02/000000000002333主主主主真象限TT)270,180(180)180,90(180)0 ,90(360)90,0(2/302/0000000000001211主主主主真象限TT如利用四元數(shù)微分方程求解，如利用四元數(shù)微分方程求解，則先利用四元數(shù)求解結(jié)果計(jì)算則先利用四元數(shù)求解結(jié)果計(jì)算方向余弦矩陣的元素方向余弦矩陣的元素(1-58)：232221211PPPT)(232112PPPT)(213223PPPT222123233PPPT)(223113PPPT49姿態(tài)矩陣的實(shí)時(shí)計(jì)算姿態(tài)矩陣的實(shí)

23、時(shí)計(jì)算因假定因假定“數(shù)學(xué)平臺(tái)數(shù)學(xué)平臺(tái)”跟蹤地理坐標(biāo)系，因跟蹤地理坐標(biāo)系，因此此biEbibbEb所以可得相應(yīng)的姿態(tài)矩陣微分方程（所以可得相應(yīng)的姿態(tài)矩陣微分方程（6-12）：）：EbEiEbibEbEbCCC或四元數(shù)微分方程：或四元數(shù)微分方程：)()()(tqtqbiEbib注意事項(xiàng)：注意事項(xiàng)：1、上述兩個(gè)方程中的角速度表達(dá)式不一樣、上述兩個(gè)方程中的角速度表達(dá)式不一樣2、方程第二項(xiàng)較小，計(jì)算時(shí)速度可以低一些、方程第二項(xiàng)較小，計(jì)算時(shí)速度可以低一些50一、角增量算法一、角增量算法(Angular Increment Algorithm)角增量：陀螺儀數(shù)字脈沖輸出，每個(gè)脈沖代表一個(gè)角增量角增量：陀螺儀

24、數(shù)字脈沖輸出，每個(gè)脈沖代表一個(gè)角增量一個(gè)采樣周期內(nèi)，陀螺輸出脈沖數(shù)對(duì)應(yīng)的角增量為：一個(gè)采樣周期內(nèi)，陀螺輸出脈沖數(shù)對(duì)應(yīng)的角增量為：tttibdt1、矩陣微分方程、矩陣微分方程(Matrix Differential Equation)計(jì)算計(jì)算根據(jù)矩陣微分方程的精確解（根據(jù)矩陣微分方程的精確解（6-20），有：），有：220000)(cos1sin)()(bibbibEbEbItCttCEbEiEbibEbEbCCC（解（解的第一項(xiàng)）的第一項(xiàng)）51220000)(cos1sin)()(bibbibEbEbItCttC展開合并上式，得展開合并上式，得)()(tCttCEbEbCSCSCSCCSCSC

25、SCCYXXZYYXZXZYXZZYXYXZZYXZY)(1)(1)(1222222其中其中200cos1C00sinS52將前式簡(jiǎn)寫為：將前式簡(jiǎn)寫為：CtCttCEbEb)()(或離散形式：或離散形式：CnCnCEbEb)() 1(C按按 Cn、Sn 取不同的近似值，形成相應(yīng)的一階取不同的近似值，形成相應(yīng)的一階 四階算法四階算法一階算法：一階算法：bibEbEbInCnC)() 1(111)(XYXZYZEbnC333231232221131211TTTTTTTTTCbE令令可將上述算法解寫成可將上述算法解寫成矩陣元素的形式：矩陣元素的形式：53XYZXYZXYZXYZXYZXYZnTnTn

26、TnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnT)()()() 1()()()() 1()()()() 1()()()() 1()()()() 1()()()() 1()()()() 1()()()() 1()()()() 1(231333331333232333231313221232321232222232221212211131311131212131211111 一階增量算法一階增量算法54當(dāng)當(dāng) Cn、Sn 取取 n = 2, 3, 4 時(shí)：時(shí)：二階增量算法：二階增量算法：2)(21)() 1(bib

27、bibEbEbInCnC三階增量算法：三階增量算法：220)(21)61 ()() 1(bibbibEbEbInCnC四階增量算法：四階增量算法：22020)(2421()61 ()() 1(bibbibEbEbInCnC552、四元數(shù)微分方程的計(jì)算：、四元數(shù)微分方程的計(jì)算：)0(2sin2cos)(0000qItq其中，其中，I 為單位四元數(shù)，為單位四元數(shù)， 如如 （6-24）所示：）所示：21ttbdt0000 xyzxzyyzxzyx寫成迭代形式：寫成迭代形式：)(2sin2cos) 1(0000nqInq56設(shè)設(shè)2cos0C002sinS一階算法：一階算法：)(21) 1(nqInq5

28、7)(21) 1(nqInq)(1212121211212121211212121211nqXYZXZYYZXZYX或展開為元素形式：或展開為元素形式：)(21)(21)(21)() 1(321nPnPnPnnZYX)(21)(21)(21)() 1(3211nPnPnnPnPYZX)(21)(21)(21)() 1(3122nPnPnnPnPXZY)(21)(21)(21)() 1(2133nPnPnnPnPXYZ58同理，可得二階算法：同理，可得二階算法：)(21)81 () 1(20nqInq三階算法：三階算法：)()4821()81 () 1(2020nqInq四階算法：四階算法：)(

29、)4821()38481 () 1(204020nqInq59用一階用一階 四階龍格四階龍格-庫(kù)塔積分矩陣和四元數(shù)微分方程庫(kù)塔積分矩陣和四元數(shù)微分方程1、一階龍格、一階龍格-庫(kù)塔法庫(kù)塔法(Runge-Kutta)一個(gè)矩陣微分方程一個(gè)矩陣微分方程)(),()(ttXftX當(dāng)初始條件已知，其一階龍格當(dāng)初始條件已知，其一階龍格-庫(kù)塔的解為庫(kù)塔的解為:)(),()()(ttXTftXTtX方程的解為初始值加上以初方程的解為初始值加上以初始點(diǎn)斜率為斜率的一個(gè)增量始點(diǎn)斜率為斜率的一個(gè)增量斜率斜率K的準(zhǔn)確度不同，解的的準(zhǔn)確度不同，解的精確度也不同精確度也不同60（1）姿態(tài)矩陣微分方程）姿態(tài)矩陣微分方程EbE

30、iEbibEbEbCCC簡(jiǎn)化為簡(jiǎn)化為)()()(ttCtC其一階龍格其一階龍格-庫(kù)塔解：庫(kù)塔解：)()()()(ttTCtCTtC展開為展開為元素形元素形式：式：)()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()(231333331333232333231313221232321232222232221212211131311132212131211111ttTttTTtTTtTttTttTTtTTtTttTttTTtTTtTttT

31、ttTTtTTtTttTttTTtTTtTttTttTTtTTtTttTttTTtTTtTttTttTTtTTtTttTttTTtTTtTXYZXYZXYZXYZXYZXYZ與一階與一階增量算增量算法一致法一致61（2）四元數(shù)微分方程）四元數(shù)微分方程qqb32132102/2/2/2/02/2/2/2/02/2/2/2/0PPPPPPxyzxzyyzxzyx或或一階龍格一階龍格-庫(kù)塔解庫(kù)塔解)()()()(tqtTtqTtqb)()()()()()(2)()(321tPttPttPtTtTtZYX)()()()()()(2)()(3211tPttPtttTtPTtPYZX)()()()()()

32、(2)()(3122tPttPtttTtPTtPXZY)()()()()()(2)()(2133tPttPtttTtPTtPXYZ622、二階龍格、二階龍格-庫(kù)塔法庫(kù)塔法對(duì)一階算法適當(dāng)改進(jìn)，使平均斜率更準(zhǔn)確一些對(duì)一階算法適當(dāng)改進(jìn)，使平均斜率更準(zhǔn)確一些)(),(1ttXfK1)(TKtXY)(,2TtYfK二階龍格二階龍格-庫(kù)塔算法的解：庫(kù)塔算法的解：2)()(21KKTtXTtX（1）矩陣微分方程）矩陣微分方程)()()(ttCtC)()(1ttCK1)(TKtCY)(2TtYK63二階龍格二階龍格-庫(kù)塔解：庫(kù)塔解：2)()(21KKTtCTtC設(shè)設(shè)333231232221131211YYYY

33、YYYYYY)()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()(231333331333232333231313221232321232222232221212211131311132212131211111ttTttTTtTTtYttTttTTtTTtYttTttTTtTTtYttTttTTtTTtYttTttTTtTTtYttTttTTtTTtYttTttTTtTTtYttTttTTtTTtYttTttTTtTTtYXYZXYZ

34、XYZXYZXYZXYZ則則64)()()()()()(2)()(312131211111TtYTtYttTttTTtTTtTYZYZ)()()()()()(2)()(113111312121TtYTtYttTttTTtTTtTZXZX)()()()()()(2)()(211121113131TtYTtYttTttTTtTTtTXYXY)()()()()()(2)()(322232221212TtYTtYttTttTTtTTtTYZYZ)()()()()()(2)()(123212322222TtYTtYttTttTTtTTtTZXZX)()()()()()(2)()(221222123232

35、TtYTtYttTttTTtTTtTXYXY)()()()()()(2)()(332333231313TtYTtYttTttTTtTTtTYZYZ)()()()()()(2)()(133313332323TtYTtYttTttTTtTTtTZXzX)()()()()()(2)()(231323133333TtYTtYttTttTTtTTtTXYXY65（2）四元數(shù)微分方程）四元數(shù)微分方程)()(1tqtKb1)(TKtqYYTtKb)(2)(2)()(21KKTtqTtq)()()()()()(21)()()()()()(21)()()()()()(21)()()()()()(21211331

36、12321132110tPttPtttKtPttPtttKtPttPtttKtPttPttPtKXYZXZYYZXZYX133312221111100)()()()(TKtPYTKtPYTKtPYTKtY66)()()(21)()()(21)()()(21)()()(2121023310223202132120YTtYTtYTtKYTtYTtYTtKYTtYTtYTtKYTtYTtYTtKXYZXZYYZXZYX)(2)()()(2)()()(2)()()(2)()(2313332212222111112010KKTtPTtPKKTtPTtPKKTtPTtPKKTtTt673、四階龍格、四階龍

37、格-庫(kù)塔法庫(kù)塔法)(),()(ttXftX)(),(1ttXfK)2(,2)(12TtTKtXfK)2(,2)(23TtTKtXfK)2(,)(34TtTKtXfK則解則解226)()(4321KKKKTtXTtX（1）矩陣微分方程）矩陣微分方程)()()(ttCtC)()(1ttCK)2(2)(12TtTKtCK)2(2)(23TtTKtCK)()(34TtTKtCK則解則解226)()(4321KKKKTtCTtC68（2）四元數(shù)微分方程）四元數(shù)微分方程qq)()(1tqtKb2)()2(12TKtqTtKb2)()2(23TKtqTtKb)()(34TKtqTtKb則解則解226)()(4321KKKKTtqTtq)()()()()()(21)()()()()()(21)()()()()()(21)()()()()()(2121133112321132110tPttPtttKtPttPtttKtPttPtttKtPttPttPtKXY