慣性導(dǎo)航原理

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

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

3、PvP)(122133或簡單表示為或簡單表示為 PvPPvMq 關(guān)于相乘符號關(guān)于相乘符號 關(guān)于交換律和結(jié)合律關(guān)于交換律和結(jié)合律3共軛四元數(shù)共軛四元數(shù) 僅向量部分符號相反的兩個四元數(shù)僅向量部分符號相反的兩個四元數(shù) ),(Pq和和 ),(*Pq互為共軛互為共軛 可證明：可證明： *)*(qhqh4四元數(shù)的范數(shù)四元數(shù)的范數(shù) q定義定義 2322212*PPPqqq1q則稱為規(guī)范化四元數(shù)則稱為規(guī)范化四元數(shù) 5逆四元數(shù)逆四元數(shù) qq11qq*當當 1q時時 *1qq6四元數(shù)的除法四元數(shù)的除法 若若 Mqh 則則 1 Mhq若若 Mhq 則則 Mhq1不能表示為不能表示為 hMq （含義不確切含義不確切

4、）一個坐標系或矢量相對參考坐標系旋轉(zhuǎn)，一個坐標系或矢量相對參考坐標系旋轉(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)動四元數(shù)）四元數(shù)既表示了轉(zhuǎn)軸方向，又表示了轉(zhuǎn)角大?。ㄞD(zhuǎn)動四元數(shù)）如果矢量如果矢量 R 相對固定坐標系旋轉(zhuǎn)相對固定坐標系旋轉(zhuǎn)，旋轉(zhuǎn)四元數(shù)為，旋轉(zhuǎn)四元數(shù)為 q，轉(zhuǎn)動后，轉(zhuǎn)動后的矢量為的矢

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

6、： qVqVee1式中式中 zkyjxiVekzjyixVe分別稱為矢量分別稱為矢量 V 在坐標系在坐標系 OXYZ 和和 OXYZ 上的映像上的映像zkyjxiV kzjyixVkzjyixVezkyjxiVeqVqVee1將該投影變換式展開，也就是把將該投影變換式展開，也就是把kzjyixVezkyjxiVekPjPiPq321kPjPiPq3211代入上述投影變換式代入上述投影變換式kzjyix)(321kPjPiP)(zkyjxi)(321kPjPiP進行四元數(shù)乘法運算，整理運算結(jié)果可得進行四元數(shù)乘法運算，整理運算結(jié)果可得zyxCzyx其中方向余弦矩陣其中方向余弦矩陣 C2221232

7、13223113223212223212313212322212)( 2)( 2)( 2)( 2)( 2)( 2PPPPPPPPPPPPPPPPPPPPPPPPPPP多次旋轉(zhuǎn)多次旋轉(zhuǎn)的合成的合成對于一個坐標系經(jīng)過多次旋轉(zhuǎn)后，新坐標系和原始坐標系之間對于一個坐標系經(jīng)過多次旋轉(zhuǎn)后，新坐標系和原始坐標系之間的關(guān)系等效于一個一次轉(zhuǎn)動的效果，的關(guān)系等效于一個一次轉(zhuǎn)動的效果， 相應(yīng)地有合成轉(zhuǎn)動四元數(shù)相應(yīng)地有合成轉(zhuǎn)動四元數(shù) 假定假定 q1、q2 分別是第一次轉(zhuǎn)動、第二次轉(zhuǎn)動的四元數(shù)分別是第一次轉(zhuǎn)動、第二次轉(zhuǎn)動的四元數(shù) q 是合成轉(zhuǎn)動的四元數(shù)，是合成轉(zhuǎn)動的四元數(shù)，那么有如下關(guān)系成立：那么有如下關(guān)系成立： 21

8、qqq上式中上式中 q1 和和 q2 的轉(zhuǎn)軸方向必須以映象的形式給出。的轉(zhuǎn)軸方向必須以映象的形式給出。 如果如果 q1 和和 q2 的轉(zhuǎn)軸方向都以原始坐標系的分量表示，則有的轉(zhuǎn)軸方向都以原始坐標系的分量表示，則有 12qqq用四元數(shù)旋轉(zhuǎn)變換的方法求取兩個坐標系之間的方向余弦表。用四元數(shù)旋轉(zhuǎn)變換的方法求取兩個坐標系之間的方向余弦表。 坐標系坐標系 OXYZ 相對相對OXYZ 三次旋轉(zhuǎn)，以三次旋轉(zhuǎn)，以歐拉角歐拉角 、的的形式給出。形式給出。 第一轉(zhuǎn)，繞第一轉(zhuǎn)，繞 Z 軸轉(zhuǎn)軸轉(zhuǎn)角，瞬時轉(zhuǎn)軸角，瞬時轉(zhuǎn)軸 n 和和 k 軸重合，則轉(zhuǎn)動四元軸重合，則轉(zhuǎn)動四元數(shù)為數(shù)為 kq2sin2cos1第二轉(zhuǎn)，繞第二轉(zhuǎn)

9、，繞 OX1 軸轉(zhuǎn)軸轉(zhuǎn)角，角，瞬時轉(zhuǎn)軸瞬時轉(zhuǎn)軸 n 的方向表示式為的方向表示式為 )sin(cosji其轉(zhuǎn)動四元數(shù)為其轉(zhuǎn)動四元數(shù)為 nq2sin2cos2)sin(cos2sin2cosji由于由于 q1 和和 q2 的瞬時轉(zhuǎn)軸的瞬時轉(zhuǎn)軸都是以同一個坐標系的方向余弦來都是以同一個坐標系的方向余弦來表示，則合成轉(zhuǎn)動四元數(shù)表示，則合成轉(zhuǎn)動四元數(shù) q 的計算采用：的計算采用：12qqqkji2sin2cos)sin(cos2sin2coskji2sin2cos2sin2sin2cos2sin2cos2cos以瞬時轉(zhuǎn)軸以瞬時轉(zhuǎn)軸映象映象形式給出形式給出轉(zhuǎn)動四元數(shù)的表達式并求轉(zhuǎn)動四元數(shù)的表達式并求出合成

10、轉(zhuǎn)動四元數(shù)出合成轉(zhuǎn)動四元數(shù) 第一次轉(zhuǎn)時，映象形式的第一次轉(zhuǎn)時，映象形式的 q1 和非映象形式的和非映象形式的 q1 是是一致的：一致的： kq2sin2cos1第二轉(zhuǎn)繞第二轉(zhuǎn)繞 OX1 軸轉(zhuǎn)軸轉(zhuǎn) 角角瞬時轉(zhuǎn)軸瞬時轉(zhuǎn)軸 n 是由是由 OX 經(jīng)過經(jīng)過第一轉(zhuǎn)轉(zhuǎn)換來的第一轉(zhuǎn)轉(zhuǎn)換來的OX 軸對應(yīng)單位矢量軸對應(yīng)單位矢量 i，所，所以定義以定義 n 的映象為的映象為 i則則 q2 的映象表示式為的映象表示式為 iq2sin2cos2第三轉(zhuǎn)，繞第三轉(zhuǎn)，繞 OZ 軸轉(zhuǎn)動軸轉(zhuǎn)動 角角瞬時轉(zhuǎn)軸瞬時轉(zhuǎn)軸 n 是由是由 OZ 經(jīng)過經(jīng)過第一轉(zhuǎn)和第二轉(zhuǎn)轉(zhuǎn)換來的第一轉(zhuǎn)和第二轉(zhuǎn)轉(zhuǎn)換來的OZ 軸對應(yīng)單位矢量軸對應(yīng)單位矢量 k，所

11、以定義所以定義 n 的映象為的映象為 k則則 q3 的映象表示式為的映象表示式為kq2sin2cos3由于由于 q1 、q2 和和 q3 都是映象形式都是映象形式 ，所以三次轉(zhuǎn)動的合成轉(zhuǎn)動所以三次轉(zhuǎn)動的合成轉(zhuǎn)動四元數(shù)四元數(shù) q 為為321qqqqkik2sin2cos2sin2cos2sin2cosji2sin2sin2cos2sin2cos2cosk2sin2cos據(jù)此可算出對應(yīng)的方向余弦表據(jù)此可算出對應(yīng)的方向余弦表 坐標系旋轉(zhuǎn)時，不變矢量坐標系旋轉(zhuǎn)時，不變矢量 V 在兩個坐標系上的投影之間存在在兩個坐標系上的投影之間存在如下關(guān)系：如下關(guān)系： qVqVee1在一些資料中，四元數(shù)的轉(zhuǎn)動公式也經(jīng)

12、常寫成如下的形式在一些資料中，四元數(shù)的轉(zhuǎn)動公式也經(jīng)常寫成如下的形式 1qqVVEE這個公式的意義是說，在一個超復(fù)數(shù)空間中，或者在一個固這個公式的意義是說，在一個超復(fù)數(shù)空間中，或者在一個固定坐標系中，矢量定坐標系中，矢量 VE 按著四元數(shù)按著四元數(shù) q 所表示的方向和大小轉(zhuǎn)所表示的方向和大小轉(zhuǎn)動了一個角度，得到一個新的矢量動了一個角度，得到一個新的矢量 VE四元數(shù)法能得到迅速發(fā)展，是由于飛行器控制與導(dǎo)航的發(fā)展，要四元數(shù)法能得到迅速發(fā)展，是由于飛行器控制與導(dǎo)航的發(fā)展，要求更合理地描述剛體空間運動，以及便于計算機的應(yīng)用。求更合理地描述剛體空間運動，以及便于計算機的應(yīng)用。采用方向余弦矩陣描述飛行器運動

13、時，要積分矩陣微分方程式：采用方向余弦矩陣描述飛行器運動時，要積分矩陣微分方程式： CC式中式中C為動坐標系轉(zhuǎn)置到定坐標系的方向余弦矩陣，為動坐標系轉(zhuǎn)置到定坐標系的方向余弦矩陣，為動坐為動坐標系相對定坐標系旋轉(zhuǎn)角速度標系相對定坐標系旋轉(zhuǎn)角速度的反對稱矩陣：的反對稱矩陣： 000 xyxzyz包含包含 9 個一階微個一階微分方程式，計算分方程式，計算量比較大量比較大 如果采用四元數(shù)法，則是要求解四元數(shù)方程式如果采用四元數(shù)法，則是要求解四元數(shù)方程式 qq21q 為動坐標系的轉(zhuǎn)動四元數(shù)，為動坐標系的轉(zhuǎn)動四元數(shù)， 為動坐標系相對定坐標系為動坐標系相對定坐標系的旋轉(zhuǎn)角速度，也表示為四元數(shù)的旋轉(zhuǎn)角速度，也

14、表示為四元數(shù) kjizyx 0按四元數(shù)乘積展開按四元數(shù)乘積展開 xyzyzyyzxzyxPPPPPPPPPPPP2131323213212222只要解四個一階微分只要解四個一階微分方程式組方程式組即可即可RrMaMgMaRMgTMaMTRamaraRa 根據(jù)質(zhì)心運動定理和相對運動學(xué)原理，根據(jù)質(zhì)心運動定理和相對運動學(xué)原理， 飛行體質(zhì)心運動的微分方程（在慣性坐標系下）為：飛行體質(zhì)心運動的微分方程（在慣性坐標系下）為： 式中，式中，-飛行體的質(zhì)量；飛行體的質(zhì)量；-推力；推力；-空氣阻力；空氣阻力；-慣性空間飛行時，導(dǎo)彈質(zhì)心加速度；慣性空間飛行時，導(dǎo)彈質(zhì)心加速度；，-由推力產(chǎn)生的加速度；由推力產(chǎn)生的加

15、速度；-由阻力引起的阻力加速度。由阻力引起的阻力加速度。RTagaa)( mamgmar)(gaarra)(RTraaa由上式可得出由上式可得出 飛行體質(zhì)心運動的微分方程（在彈體坐標系下）為：飛行體質(zhì)心運動的微分方程（在彈體坐標系下）為： 或或 ， 式中式中是動點的相對加速度，將（是動點的相對加速度，將（* *）代入上式）代入上式 得得 )(gaasTaRasaa)()(agmmamgf由上式可知，測得的由上式可知，測得的是推力加速度是推力加速度和阻力加速度和阻力加速度的矢量和，稱為視加速度，在實際的測試中由加速度傳感器得到的值是的矢量和，稱為視加速度，在實際的測試中由加速度傳感器得到的值是在

16、敏感軸上的分量，實際的慣性坐標系下的加速度在敏感軸上的分量，實際的慣性坐標系下的加速度可通過上式變換得到，在彈體坐標系上動點的力為可通過上式變換得到，在彈體坐標系上動點的力為稱為比力，加速度計實際是通過比力來測量加速度的。稱為比力，加速度計實際是通過比力來測量加速度的。捷聯(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)初始化(Initialization)：1、給定飛行器初始位置、速度等、給定飛行器初始位置、速度等2、數(shù)學(xué)平臺的初始對準、數(shù)學(xué)平臺的初始對準3、慣性

17、儀表的校準、慣性儀表的校準二、慣性儀表誤差補償二、慣性儀表誤差補償(Compensation)三、姿態(tài)矩陣的計算三、姿態(tài)矩陣的計算四、導(dǎo)航計算四、導(dǎo)航計算五、導(dǎo)航控制信息的提取五、導(dǎo)航控制信息的提取姿態(tài)矩陣的計算姿態(tài)矩陣的計算假設(shè)數(shù)學(xué)坐標系模擬地理坐標系假設(shè)數(shù)學(xué)坐標系模擬地理坐標系飛行器姿態(tài)的描述：飛行器姿態(tài)的描述： 航向角航向角、俯仰角、俯仰角、滾動角、滾動角一、歐拉微分方程一、歐拉微分方程從地理坐標系到載體坐標系從地理坐標系到載體坐標系的旋轉(zhuǎn)順序：的旋轉(zhuǎn)順序： 方向余弦矩陣：方向余弦矩陣：CCCCbEcoscossincoscossinsinsinsincossincossincoscos

18、cossinsinsincossinsinsincossincossincoscos飛行器相對地理坐標系的角速度：飛行器相對地理坐標系的角速度：TbEbzbEbybEbxbEb000000CCC000000CCCcoscossin0cossincos0sin01求解歐拉角速率得求解歐拉角速率得bEbzbEbybEbx1coscossin0cossincos0sin01bEbzbEbybEbxcossin0cossincoscos0sincossinsincoscos1注意事項：當注意事項：當 = 90 度時，方程出現(xiàn)奇點度時，方程出現(xiàn)奇點二、方向余弦矩陣微分方程及其解二、方向余弦矩陣微分方程及

19、其解 CCbEbEbEbCC其中其中000 xyxzyzbEb由于陀螺儀直接測得的是載體由于陀螺儀直接測得的是載體相對慣性空間的角速度，所以：相對慣性空間的角速度，所以：biEbibbEb導(dǎo)航計算可以得到導(dǎo)航計算可以得到EIE有有EbEiEbEbiECC 因此因此EbEiEbEbibbEbCC 得得EbEiEbibEbEbCCCbEbEbEbCC的精確解（畢卡逼近）：的精確解（畢卡逼近）：)()(tCttCEbEb220000)(cos1sinbEbbEbI1nnttbEbbEbdt其中其中000bEbXbEbYbEbXbEbZbEbYbEbZbEb22220)()()(bEbZbEbYbEb

20、XbEb方向不變時的精確解方向不變時的精確解九個微分方程求解，計算量大九個微分方程求解，計算量大三、四元數(shù)微分方程式及其解三、四元數(shù)微分方程式及其解由第一章，四元數(shù)微分方程式：由第一章，四元數(shù)微分方程式：qqb對對 的處理類似上一節(jié)的處理類似上一節(jié)b32132102/2/2/2/02/2/2/2/02/2/2/2/0PPPPPPxyzxzyyzxzyx精確解：精確解：)0(2sin2cos)(000qItq其中：其中：21ttbdt)0(2sin2cos)(000qItq其中：其中：21ttbdt0000 xyzxzyyzxzyx22220ZYX四、姿態(tài)和航向角的計算四、姿態(tài)和航向角的計算根據(jù)

21、載體和地理坐標系之間的方向余弦矩陣可確定姿態(tài)、航向角根據(jù)載體和地理坐標系之間的方向余弦矩陣可確定姿態(tài)、航向角coscossincoscossinsinsinsincossincossincoscoscossinsinsincossinsinsincossincossincoscosbEC333231232221131211TTTTTTTTTCbE)(sin131T33231TTtg11121TTtg姿態(tài)、航向角姿態(tài)、航向角真值的判斷真值的判斷)270,180(180)180,90(180)90,0()90,0(2/02/000000000002333主主主主真象限TT)270,180(180)

22、180,90(180)0 ,90(360)90,0(2/302/0000000000001211主主主主真象限TT如利用四元數(shù)微分方程求解，如利用四元數(shù)微分方程求解，則先利用四元數(shù)求解結(jié)果計算則先利用四元數(shù)求解結(jié)果計算方向余弦矩陣的元素方向余弦矩陣的元素(1-58)：232221211PPPT)(232112PPPT)(213223PPPT222123233PPPT)(223113PPPT姿態(tài)矩陣的實時計算姿態(tài)矩陣的實時計算因假定因假定“數(shù)學(xué)平臺數(shù)學(xué)平臺”跟蹤地理坐標系，因跟蹤地理坐標系，因此此biEbibbEb所以可得相應(yīng)的姿態(tài)矩陣微分方程（所以可得相應(yīng)的姿態(tài)矩陣微分方程（6-12）：）：E

23、bEiEbibEbEbCCC或四元數(shù)微分方程：或四元數(shù)微分方程：)()()(tqtqbiEbib注意事項：注意事項：1、上述兩個方程中的角速度表達式不一樣、上述兩個方程中的角速度表達式不一樣2、方程第二項較小，計算時速度可以低一些、方程第二項較小，計算時速度可以低一些一、角增量算法一、角增量算法(Angular Increment Algorithm)角增量：陀螺儀數(shù)字脈沖輸出，每個脈沖代表一個角增量角增量：陀螺儀數(shù)字脈沖輸出，每個脈沖代表一個角增量一個采樣周期內(nèi)，陀螺輸出脈沖數(shù)對應(yīng)的角增量為：一個采樣周期內(nèi)，陀螺輸出脈沖數(shù)對應(yīng)的角增量為：tttibdt1、矩陣微分方程、矩陣微分方程(Matr

24、ix Differential Equation)計算計算根據(jù)矩陣微分方程的精確解（根據(jù)矩陣微分方程的精確解（6-20），有：），有：220000)(cos1sin)()(bibbibEbEbItCttCEbEiEbibEbEbCCC（解（解的第一項）的第一項）220000)(cos1sin)()(bibbibEbEbItCttC展開合并上式，得展開合并上式，得)()(tCttCEbEbCSCSCSCCSCSCSCCYXXZYYXZXZYXZZYXYXZZYXZY)(1)(1)(1222222其中其中200cos1C00sinS將前式簡寫為：將前式簡寫為：CtCttCEbEb)()(或離散形式

25、：或離散形式：CnCnCEbEb)() 1(C按按 Cn、Sn 取不同的近似值，形成相應(yīng)的一階取不同的近似值，形成相應(yīng)的一階 四階算法四階算法一階算法：一階算法：bibEbEbInCnC)() 1(111)(XYXZYZEbnC333231232221131211TTTTTTTTTCbE令令可將上述算法解寫成可將上述算法解寫成矩陣元素的形式：矩陣元素的形式：XYZXYZXYZXYZXYZXYZnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnT)()()() 1()()()() 1()()()() 1(

26、)()()() 1()()()() 1()()()() 1()()()() 1()()()() 1()()()() 1(231333331333232333231313221232321232222232221212211131311131212131211111 一階增量算法一階增量算法當當 Cn、Sn 取取 n = 2, 3, 4 時：時：二階增量算法：二階增量算法：2)(21)() 1(bibbibEbEbInCnC三階增量算法：三階增量算法：220)(21)61 ()() 1(bibbibEbEbInCnC四階增量算法：四階增量算法：22020)(2421()61 ()() 1(bib

27、bibEbEbInCnC2、四元數(shù)微分方程的計算：、四元數(shù)微分方程的計算：)0(2sin2cos)(0000qItq其中，其中，I 為單位四元數(shù)，為單位四元數(shù)， 如如 （6-24）所示：）所示：21ttbdt0000 xyzxzyyzxzyx寫成迭代形式：寫成迭代形式：)(2sin2cos) 1(0000nqInq設(shè)設(shè)2cos0C002sinS一階算法：一階算法：)(21) 1(nqInq)(21) 1(nqInq)(1212121211212121211212121211nqXYZXZYYZXZYX或展開為元素形式：或展開為元素形式：)(21)(21)(21)() 1(321nPnPnPnn

28、ZYX)(21)(21)(21)() 1(3211nPnPnnPnPYZX)(21)(21)(21)() 1(3122nPnPnnPnPXZY)(21)(21)(21)() 1(2133nPnPnnPnPXYZ同理，可得二階算法：同理，可得二階算法：)(21)81 () 1(20nqInq三階算法：三階算法：)()4821()81 () 1(2020nqInq四階算法：四階算法：)()4821()38481 () 1(204020nqInq用一階用一階 四階龍格四階龍格-庫塔積分矩陣和四元數(shù)微分方程庫塔積分矩陣和四元數(shù)微分方程1、一階龍格、一階龍格-庫塔法庫塔法(Runge-Kutta)一個矩

29、陣微分方程一個矩陣微分方程)(),()(ttXftX當初始條件已知，其一階龍格當初始條件已知，其一階龍格-庫塔的解為庫塔的解為:)(),()()(ttXTftXTtX方程的解為初始值加上以初方程的解為初始值加上以初始點斜率為斜率的一個增量始點斜率為斜率的一個增量斜率斜率K的準確度不同，解的的準確度不同，解的精確度也不同精確度也不同（1）姿態(tài)矩陣微分方程）姿態(tài)矩陣微分方程EbEiEbibEbEbCCC簡化為簡化為)()()(ttCtC其一階龍格其一階龍格-庫塔解：庫塔解：)()()()(ttTCtCTtC展開為展開為元素形元素形式：式：)()()()()()()()()()()()()()()(

30、)()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()(231333331333232333231313221232321232222232221212211131311132212131211111ttTttTTtTTtTttTttTTtTTtTttTttTTtTTtTttTttTTtTTtTttTttTTtTTtTttTttTTtTTtTttTttTTtTTtTttTttTTtTTtTttTttTTtTTtTXYZXYZXYZXYZXYZXYZ與一階與一階增量算增量算法一致法一致（2）四元數(shù)

31、微分方程）四元數(shù)微分方程qqb32132102/2/2/2/02/2/2/2/02/2/2/2/0PPPPPPxyzxzyyzxzyx或或一階龍格一階龍格-庫塔解庫塔解)()()()(tqtTtqTtqb)()()()()()(2)()(321tPttPttPtTtTtZYX)()()()()()(2)()(3211tPttPtttTtPTtPYZX)()()()()()(2)()(3122tPttPtttTtPTtPXZY)()()()()()(2)()(2133tPttPtttTtPTtPXYZ2、二階龍格、二階龍格-庫塔法庫塔法對一階算法適當改進，使平均斜率更準確一些對一階算法適當改進，

32、使平均斜率更準確一些)(),(1ttXfK1)(TKtXY)(,2TtYfK二階龍格二階龍格-庫塔算法的解：庫塔算法的解：2)()(21KKTtXTtX（1）矩陣微分方程）矩陣微分方程)()()(ttCtC)()(1ttCK1)(TKtCY)(2TtYK二階龍格二階龍格-庫塔解：庫塔解：2)()(21KKTtCTtC設(shè)設(shè)333231232221131211YYYYYYYYYY)()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()(231

33、333331333232333231313221232321232222232221212211131311132212131211111ttTttTTtTTtYttTttTTtTTtYttTttTTtTTtYttTttTTtTTtYttTttTTtTTtYttTttTTtTTtYttTttTTtTTtYttTttTTtTTtYttTttTTtTTtYXYZXYZXYZXYZXYZXYZ則則)()()()()()(2)()(312131211111TtYTtYttTttTTtTTtTYZYZ)()()()()()(2)()(113111312121TtYTtYttTttTTtTTtTZXZX)

34、()()()()()(2)()(211121113131TtYTtYttTttTTtTTtTXYXY)()()()()()(2)()(322232221212TtYTtYttTttTTtTTtTYZYZ)()()()()()(2)()(123212322222TtYTtYttTttTTtTTtTZXZX)()()()()()(2)()(221222123232TtYTtYttTttTTtTTtTXYXY)()()()()()(2)()(332333231313TtYTtYttTttTTtTTtTYZYZ)()()()()()(2)()(133313332323TtYTtYttTttTTtTTt

35、TZXzX)()()()()()(2)()(231323133333TtYTtYttTttTTtTTtTXYXY（2）四元數(shù)微分方程）四元數(shù)微分方程)()(1tqtKb1)(TKtqYYTtKb)(2)(2)()(21KKTtqTtq)()()()()()(21)()()()()()(21)()()()()()(21)()()()()()(2121133112321132110tPttPtttKtPttPtttKtPttPtttKtPttPttPtKXYZXZYYZXZYX133312221111100)()()()(TKtPYTKtPYTKtPYTKtY)()()(21)()()(21)()

36、()(21)()()(2121023310223202132120YTtYTtYTtKYTtYTtYTtKYTtYTtYTtKYTtYTtYTtKXYZXZYYZXZYX)(2)()()(2)()()(2)()()(2)()(2313332212222111112010KKTtPTtPKKTtPTtPKKTtPTtPKKTtTt3、四階龍格、四階龍格-庫塔法庫塔法)(),()(ttXftX)(),(1ttXfK)2(,2)(12TtTKtXfK)2(,2)(23TtTKtXfK)2(,)(34TtTKtXfK則解則解226)()(4321KKKKTtXTtX（1）矩陣微分方程）矩陣微分方程)(

37、)()(ttCtC)()(1ttCK)2(2)(12TtTKtCK)2(2)(23TtTKtCK)()(34TtTKtCK則解則解226)()(4321KKKKTtCTtC（2）四元數(shù)微分方程）四元數(shù)微分方程qq)()(1tqtKb2)()2(12TKtqTtKb2)()2(23TKtqTtKb)()(34TKtqTtKb則解則解226)()(4321KKKKTtqTtq)()()()()()(21)()()()()()(21)()()()()()(21)()()()()()(2121133112321132110tPttPtttKtPttPtttKtPttPtttKtPttPttPtKXYZXZY