高精度捷聯(lián)式慣性導(dǎo)航系統(tǒng)算法研究

1、word文檔整理分享參考資料高精度捷聯(lián)式慣性導(dǎo)航系統(tǒng)算法研究1. 引言隨著計(jì)算機(jī)技術(shù)的發(fā)展，捷聯(lián)式慣性導(dǎo)航系統(tǒng)(strapdow n In ertial Navigatio nSystem, SINS )的概念被提出，它取消了平臺(tái)式慣性導(dǎo)航系統(tǒng)中復(fù)雜的機(jī)械平臺(tái)裝置，而將慣性傳感器直接固聯(lián)在載體上。SINS具有制造和維護(hù)成本低、體積小、重量輕以及可靠性高等優(yōu)點(diǎn)，目前在高、中、低精度領(lǐng)域都得到了廣泛使用 捷聯(lián)算法的基本框圖如圖1所示。陀螺儀加速度計(jì)慣性傳感器誤差補(bǔ)償導(dǎo)航計(jì)算機(jī)地速與位置的 計(jì)算顯示器姿態(tài)角計(jì)算圖1捷聯(lián)算法的基本框圖在捷聯(lián)慣性導(dǎo)航系統(tǒng)中，慣性傳感器直接固聯(lián)在載體上，因此對(duì)慣性傳感器的

2、性能提出了更高的要求。SINS中使用的陀螺所承受的動(dòng)態(tài)范圍較大，一般能夠達(dá)到100 /s，與此同時(shí)，SINS中的陀螺和加速度計(jì)與載體一起進(jìn)行角運(yùn)動(dòng)和線運(yùn)動(dòng)，這增加了導(dǎo)航計(jì)算機(jī)輸出數(shù)據(jù)的難度和復(fù)雜性。姿態(tài)實(shí)時(shí)計(jì)算是捷聯(lián)慣導(dǎo)的關(guān)鍵技術(shù)，也是影響捷聯(lián)慣 導(dǎo)系統(tǒng)導(dǎo)航精度的重要因素。載體的姿態(tài)和航向是載體坐標(biāo)系和地理坐標(biāo)系之間的方位關(guān)系，兩坐標(biāo)系之間的方位關(guān)系等效于力學(xué)中的剛體定點(diǎn)轉(zhuǎn)動(dòng)問題。在剛體定點(diǎn)轉(zhuǎn)動(dòng)理論中，描述動(dòng)坐標(biāo)系相對(duì) 參考坐標(biāo)系方位關(guān)系的方法有歐拉角法、四元數(shù)法、方向余弦法以及等效旋轉(zhuǎn)矢量法。 本報(bào)告對(duì)這四種姿態(tài)算法進(jìn)行簡(jiǎn)單介紹，并結(jié)合研究對(duì)象對(duì)等效旋轉(zhuǎn)矢量算法進(jìn)行重點(diǎn) 研究。針對(duì)角速率輸入

3、陀螺構(gòu)成的捷聯(lián)式慣性導(dǎo)航系統(tǒng)，本報(bào)告給出了一種改進(jìn)的姿態(tài) 算法，并在圓錐運(yùn)動(dòng)環(huán)境下對(duì)該算法進(jìn)行數(shù)學(xué)仿真，驗(yàn)證了該方法的可能性。2. 姿態(tài)算法介紹2.1歐拉角法一個(gè)動(dòng)坐標(biāo)系相對(duì)參考坐標(biāo)系的方位可以完全由動(dòng)坐標(biāo)系依次繞三個(gè)不同軸轉(zhuǎn)動(dòng) 三個(gè)角度進(jìn)行確定。把載體坐標(biāo)系OXbybZb作為動(dòng)坐標(biāo)系，導(dǎo)航坐標(biāo)系OXnynZn(即地理坐H、俯仰角P、橫搖角R可得到載體坐從而進(jìn)一步可以得到捷聯(lián)姿態(tài)矩陣。標(biāo)系)作為參考坐標(biāo)系，導(dǎo)航系依次轉(zhuǎn)過航向角 標(biāo)系，通過求解歐拉角微分方程得到三個(gè)歐拉角,歐拉角微分方程如下所示:>1R11cosPsin Psin R-sin R0cosP0sin R cos P | 汽n

4、bx-cos P sin PcosRb豹n byb豹n bz(1)式(1)即為歐拉角微分方程，求解方程可以得到三個(gè)歐拉角，也就是航向角、俯仰角以及橫搖角，根據(jù)三個(gè)姿態(tài)角和姿態(tài)矩陣元素之間的關(guān)系即可以得到姿態(tài)矩陣Cb。2.2方向余弦法常用方向余弦姿態(tài)矩陣微分方程的形式為(1)cn = nb c n式中為載體坐標(biāo)系相對(duì)地理坐標(biāo)系的轉(zhuǎn)動(dòng)角速度在載體坐標(biāo)軸向的分量的反對(duì)稱矩陣形式，具體表達(dá)式如式(2) obk'nb-0I唸bI Tnbyb-nbz0b''nbxb''nbybYJ nbx0用畢卡逼近法求解矩陣微分方程，其解為bCn(t：t)二 C(t) I+詈述+

5、=(礙畀式中2.3四元數(shù)法'03bnbz心日bbnby if%；kdt =迥bz0一日 bbnbxLn-氐日；byA0bbnbx0-Ux2nby2r 鳥word文檔整理分享四元數(shù)微分方程的形式為Q (t)=； Q (t)Q n；(4)2其中，qt)是姿態(tài)四元數(shù)，nb-lirx 'y 'z ，為b系相對(duì)n系的角速度。求解四元數(shù)微分方程一般用計(jì)算機(jī)實(shí)現(xiàn)，常用方法有畢卡逼近增量法和數(shù)值積分算 法，本報(bào)告關(guān)于四元數(shù)求解姿態(tài)角算法采用的都是數(shù)值積分算法，因此此處僅對(duì)四元數(shù) 四階-龍格庫(kù)塔算法進(jìn)行簡(jiǎn)單介紹。對(duì)四元數(shù)微分方程Q = 2 Q ' ；b式中:雹=o ;xib

6、9;bbjbkb，為載體坐標(biāo)系相對(duì)于導(dǎo)航坐標(biāo)系的角速度矢量 的四元數(shù)表達(dá)形式。用四階龍格庫(kù)塔法求解微分方程的過程如下：Q(t h)二Q(t) - k1 2k2 2k3 k4(6)6其中：ki W 'Q(。毗，k2'iQ(t) +2-2世 2 I 2丿J1 k"lik3Q(t)- nb t, k4Q(t) k3h 1 ' ' nb(t h)f2 IL 222其中h為姿態(tài)更新周期。這種方法的實(shí)質(zhì)就是在(t, t+h)時(shí)間間隔內(nèi)求取多個(gè)斜率值，加權(quán)求平均得到更精 確的平均斜率。當(dāng)采用四元數(shù)Q來表示轉(zhuǎn)動(dòng)時(shí)，若起始時(shí)刻四元數(shù)Q取為單位四元數(shù)，則依據(jù)轉(zhuǎn)動(dòng) 的四元數(shù)

7、變換定理可知，Q應(yīng)恒為單位四元數(shù)，即應(yīng)滿足下面的約束方程：qo q： q； qf =1但由于計(jì)算誤差的存在，破壞了上面的約束條件，因此必須對(duì)Q進(jìn)行歸一化處理。采用下式實(shí)現(xiàn)四元數(shù)的最佳歸一化：Q=吋酬+?2jb +壑旋+& +帛+q?2.4等效旋轉(zhuǎn)矢量算法(8)參考資料word文檔整理分享由于剛體運(yùn)動(dòng)的不可交換性誤差，圓錐誤差補(bǔ)償算法是提高運(yùn)載體姿態(tài)更新精度的 一種有效途徑。1971年Bortz和Jordon提出了等效旋轉(zhuǎn)矢量概念，將載體的姿態(tài)四元 數(shù)更新轉(zhuǎn)換為姿態(tài)變化四元數(shù)的更新，為姿態(tài)更新的多子樣算法提供了理論依據(jù)。1983年Miller探討了錐運(yùn)動(dòng)條件下等效旋轉(zhuǎn)矢量的三子樣優(yōu)化算法

8、，優(yōu)化指標(biāo)是圓錐誤差 影響達(dá)到最小。在此基礎(chǔ)上，Lee和Yoon研究了四子樣算法，Jia ng研究了利用本更新 周期內(nèi)的三子樣及前更新周期內(nèi)的角增量計(jì)算旋轉(zhuǎn)矢量的優(yōu)化算法。旋轉(zhuǎn)矢量的微分方程是：b(t) - (t)-(t)(t)(t)(t)(t)(9)2 12式中，(t)為旋轉(zhuǎn)矢量，(t)為陀螺輸出角速率。由式(9)可以推導(dǎo)求得旋轉(zhuǎn)矢量 的表達(dá)式，如二子樣、三子樣等都是工程中常用的算法。二子樣算法表達(dá)式：'(t). "：(1 =二(t) 4(12(12(12a)b)c)2彷+1 厶二23(10)算法漂移率：虬=丄320(詼)4960(11)陀螺輸出角速率時(shí)的二子樣算法表達(dá)式：

9、當(dāng)陀螺輸出為角速率時(shí)，旋轉(zhuǎn)矢量法中的角增量可由以下公式提取算法漂移率：廠a2】15 2(13)2280三子樣算法表達(dá)式：927二.心+衛(wèi)厶弓.門彳+乂厶飛(14)20401 6算法漂移率：e 丄3 -h(15)204120當(dāng)陀螺輸出為角速率時(shí)，旋轉(zhuǎn)矢量法中的角增量可由以下公式提取,i t T 一，t 2T T 36I 3丿I 3丿/(165 t 23T 36(16dt+Tl+8 Jt+2!+5,I 3丿2T(t T) T 6(16A0 = ” (t) + 3 tJ+3 ,：t +(16a)b)c)d)(17)算法漂移率:式中， 9i, i=1, 2, 3為姿態(tài)更新周期內(nèi)第i次陀螺采樣值；a為圓

10、錐運(yùn)動(dòng)半錐 角；Q為圓錐運(yùn)動(dòng)角速率；h為姿態(tài)更新周期。3. 角速率輸入的捷聯(lián)慣導(dǎo)姿態(tài)算法研究與實(shí)現(xiàn)1971年，Bortz提出了旋轉(zhuǎn)矢量微分方程，為一種全新的姿態(tài)算法提供了理論基礎(chǔ)， 有效地解決了算法求解過程中存在的不可交換性誤差；在此基礎(chǔ)上，Miller提出了二子樣、三子樣誤差補(bǔ)償算法，而 Lee在此基礎(chǔ)上又提出了四子樣算法；Jia ng提出了利用 當(dāng)前周期與前一周期陀螺儀采樣信號(hào)進(jìn)行誤差補(bǔ)償新算法，能夠提高算法精度；Savage對(duì)前人工作進(jìn)行了總結(jié)，并就姿態(tài)更新算法的實(shí)現(xiàn)提出了一系列技巧，給出了完整的捷 聯(lián)慣導(dǎo)姿態(tài)算法編排和離散更新方法。 這些算法的實(shí)現(xiàn)都是基于陀螺儀的角增量信息展 開的，當(dāng)

11、陀螺儀輸出為角速率信號(hào)時(shí)，利用數(shù)值積分方法將角速率信息轉(zhuǎn)換為角增量信 息，然后直接應(yīng)用傳統(tǒng)圓錐誤差補(bǔ)償算法，算法精度會(huì)降低2個(gè)數(shù)量級(jí)，不能滿足載體高動(dòng)態(tài)環(huán)境下對(duì)姿態(tài)解算精度的要求。因此有必要研究一種能夠直接利用陀螺儀輸出角 速率信息進(jìn)行姿態(tài)解算算法，從而進(jìn)一步提高捷聯(lián)慣導(dǎo)復(fù)雜工作環(huán)境下的姿態(tài)解算精 度。旋轉(zhuǎn)矢量的微分方程近似為:(18)對(duì)于傳統(tǒng)的二子樣算法來說，在一個(gè)計(jì)算周期TT+h內(nèi)需要三次陀螺采樣值，采樣時(shí)間h內(nèi)，載體的角速度用拋物線擬合為：(T )二 a 2b 3c 2，0<t < h(19)記角增量為. = (.)=°，(T )d.(20)則由式(19)和式(20

12、)得灼(T) =co(T十壬)甘=a也(T)蟲(J")心=2b(21(21a)b)創(chuàng)(T)=出(T+訓(xùn)詳=6c(21c)/(T) = J)仃")好=0,i =3,4,5 (21 d)日(0)3 |詳=0(22a)或0)=也十=(T + 1)=a(22 b)制(0)=胡創(chuàng)詳仃+刊詳=2 b(22 c)胡 1(0)=劇創(chuàng)詳=(T+t)|=6 c(22 d)A3)(0)=辭詳=8(j (T+可詳二 0 ,i =4,5,6(22e)又由于姿態(tài)更新周期h 一般為毫秒級(jí)的量，：也可視為小量，這樣有式(23)成立：.：()：.“(.)(23)這樣式(18)可寫成v-( J=仃.)一.：(

13、.)(T )(24)2對(duì)式(24)求各階導(dǎo)數(shù)，并考慮到式(21d)和式(22e)：；()二 |(T ) 33"(.)二" ) (T .廠一厶創(chuàng)()(T ) "() *(T )(25C) 宀() (T ) 22 宀() i(T )(25a)2 2(25 b)関()=!仃.)G(5)( .)=2 4( )(T ) 3 左() 曲T )(25d) 門(.) (T )宀()！ (T ):"6)(.)=5.VI(.) ：d(T)(25e) 宀()宀(T )2 2譏0) = a1J； (0)=2葉a a=2b21(0) =6c2b a+a 2b=6c a b2133

14、(0)= 6cxa + 2bx2b+ ax6c=6axe(26)2 2 2(0)=2 6c 2b+3 2b 6c=12b c："6)(0) =5 6c 6c=0：(i)(0) = 0,i =7,8,9,將(h)用泰勒級(jí)數(shù)展開，得：h?h? h4評(píng)h二0 h0000 +-0(27)將式(26)代入式(27)得:23131415(h) =ahbhchabhachbch(28)6410設(shè)陀螺在t=T, t=T+h/2 , t=T+h的角速率分別為：“、上、匕，可用陀螺角速率估計(jì)a、b、c的大小如下：a =咎(29)1b3 1 ' 4；r 2 - ，32h 1232C = 37(叫加

15、2皿3)將式(29)代入式(28)并考慮陀螺的角增量輸出，則旋轉(zhuǎn)矢量可用下式估計(jì)：2 2=日+Xh國(guó)1疋國(guó)3 +Yh2匯(3 _國(guó)1 )(30)式中 X=1/60 , 丫=7/90。對(duì)捷聯(lián)式慣性導(dǎo)航系統(tǒng)姿態(tài)更新來說，錐運(yùn)動(dòng)是最惡劣的工作環(huán)境條件，它會(huì)誘發(fā) 數(shù)學(xué)平臺(tái)的嚴(yán)重漂移，所以對(duì)旋轉(zhuǎn)矢量算法作優(yōu)化處理時(shí)常以錐運(yùn)動(dòng)作為環(huán)境條件。根 據(jù)錐運(yùn)動(dòng)得到的準(zhǔn)確四元數(shù) q( h)與旋轉(zhuǎn)矢量叮(h)確定的計(jì)算四元數(shù)q(h)可以求得誤差 四元數(shù)q(h)，再把誤差表達(dá)式進(jìn)行泰勒級(jí)數(shù)展開，選擇適當(dāng)?shù)腦和Y使得誤差表達(dá)式各項(xiàng)系數(shù)為零，從而得出：X=1/180，丫=7/90。對(duì)于光纖陀螺輸出為角速率時(shí)，式(30)中第

16、一項(xiàng)的角增量需要通過數(shù)值積分算法估 計(jì)其值大小，通常采用Simpson公式求解：1180.272h -i - -3h290(31)(32)算法漂移率:4. 算法仿真驗(yàn)證4.1圓錐運(yùn)動(dòng)介紹圓錐運(yùn)動(dòng)是剛體運(yùn)動(dòng)的一種幾何現(xiàn)象，剛體受到環(huán)境振動(dòng)影響或本身具有的角運(yùn) 動(dòng)，如果剛體在兩個(gè)正交軸方向存在頻率相同的角振動(dòng)速率時(shí)，第三個(gè)正交軸在空間將 繞其平衡位置作錐面或近似錐面的運(yùn)動(dòng)，稱為剛體的圓錐運(yùn)動(dòng)。圖2所示為經(jīng)典圓錐運(yùn)動(dòng)示意圖。Xx參考資料圖2經(jīng)典圓錐運(yùn)動(dòng)示意圖下面對(duì)經(jīng)典圓錐運(yùn)動(dòng)作簡(jiǎn)要介紹。假設(shè)載體坐標(biāo)系為 o-xyz，參考坐標(biāo)系為dXYZ在某一時(shí)刻t有公共原點(diǎn)0。參考坐標(biāo)系和載體坐標(biāo)系的關(guān)系可通過繞

17、YZ平面內(nèi)的oL軸旋轉(zhuǎn)a角而得到，并且oL在YoZ平面內(nèi)以門角速度旋轉(zhuǎn)，則o-xyz坐標(biāo)系偏離開o-XYZ坐標(biāo)系，且ox軸位于頂點(diǎn)為o半錐角為a的圓錐面上，y軸和z軸將分別繞丫軸和Z軸作振蕩運(yùn)動(dòng)。在這種條件下，載體坐標(biāo)系和參考坐標(biāo)系之間的變換四元數(shù)為:Q (t)u R(t)sin2(33)式中，uR(t)為等效軸方向的單位矢量，a為動(dòng)坐標(biāo)系繞uR(t)轉(zhuǎn)動(dòng)的角度，由圖2 得:uR(t) _ 10cos(t) sin(i】t)T(34)可得載體坐標(biāo)系和參考坐標(biāo)系之間的變化四元數(shù)為:Q(t) = ico- I 0 sin" icos(0t)sin"a Isin(Ct J:12丿

18、12丿12丿(35)由四元數(shù)微分方程可以計(jì)算得經(jīng)典圓錐運(yùn)動(dòng)的角速度矢量為:-c sin(a)sinf.1 sin(a)cos(36)4.2圓錐誤差分析假設(shè)載體俯仰軸和橫滾軸作同頻不同相的角振動(dòng)，即:P(t) =PmCOS("t)R(t) = RmS in (沁)H (t) =0(37)根據(jù)歐拉角微分方程可知，b =知«z f與H、P、R的關(guān)系為:3z00 - sincosR P I RR P(38)對(duì)于微幅振動(dòng)，有:(39)在方位軸上會(huì)整x = _Pm° sin (Ct)9 y = Rm。COS(0t)0z = 1 RmPmQcos(20t) +1 】 L 2式(

19、39)說明，當(dāng)載體沿俯仰軸和橫滾軸作同頻不同相的角振動(dòng)時(shí), 流出直流分量，且當(dāng)相位相差 90°時(shí)，常值誘導(dǎo)漂移率最大:(40)此誘導(dǎo)漂移角速率稱為圓錐誤差。4.3算法漂移率仿真為了驗(yàn)證改進(jìn)的圓錐誤差補(bǔ)償算法有效性，對(duì)傳統(tǒng)等效旋轉(zhuǎn)矢量算法及旋轉(zhuǎn)矢量改 進(jìn)算法在標(biāo)準(zhǔn)圓錐運(yùn)動(dòng)條件下進(jìn)行仿真。word文檔整理分享I. 仿真環(huán)境設(shè)置圓錐仿真環(huán)境下，旋轉(zhuǎn)矢量方程為：-0 1二 acosgt)asi n(0t)(1) 取半錐角a=1.0。，陀螺采樣周期為0.005s，考察旋轉(zhuǎn)矢量算法漂移率與圓錐 運(yùn)動(dòng)角頻率的關(guān)系，仿真結(jié)果如圖3所示。(2) 取半錐角a=1.0。，圓錐運(yùn)動(dòng)的角速率為 Q =2.0

20、n,考察旋轉(zhuǎn)矢量算法漂移率與算法迭代頻率的關(guān)系，仿真結(jié)果如圖4所示。II. 仿真結(jié)果圖3算法漂移率與圓錐運(yùn)動(dòng)角頻率的關(guān)系率移漂法算gl率移漂法算圖4算法漂移率與姿態(tài)更新頻率的關(guān)系圖3可以看出，傳統(tǒng)二子樣算法與改進(jìn)二子樣算法的漂移率都隨圓錐運(yùn)動(dòng)角頻率的增大而增大，但在相同的角頻率下，改進(jìn)算法誤差漂移率比同子樣傳統(tǒng)算法要小35個(gè)參考資料word文檔整理分享數(shù)量級(jí)；圖4可以看出，改進(jìn)算法與傳統(tǒng)算法相比，算法精度都隨著姿態(tài)更新頻率的增 大而提高，在相同姿態(tài)更新頻率下，改進(jìn)算法的精度顯然高于傳統(tǒng)算法。4.4典型圓錐運(yùn)動(dòng)環(huán)境姿態(tài)解算仿真分析仿真流程如圖5所示。開始圖5算法仿真流程圖結(jié)束I.仿真環(huán)境設(shè)置圓錐

21、運(yùn)動(dòng)的參數(shù)設(shè)置為：半錐角a=1.0。，錐運(yùn)動(dòng)角頻率 f=1.OHz，采樣周期為0.005s，仿真時(shí)間為30s。(1) 傳統(tǒng)二子樣旋轉(zhuǎn)矢量算法在角增量輸入情況下的姿態(tài)誤差曲線如圖6所示；當(dāng)只有角速率輸入時(shí)，通過Simpson積分法轉(zhuǎn)化后得到的姿態(tài)誤差曲線如圖7所示。(2) 改進(jìn)二子樣旋轉(zhuǎn)矢量算法在角速率輸入時(shí)的姿態(tài)誤差曲線如圖8所示。II.仿真結(jié)果參考資料-6x 10t/(s)圖6圓錐環(huán)境角增量輸入下的二子樣旋轉(zhuǎn)矢量算法的姿態(tài)誤差曲線-3x 10nt/(s)圖7圓錐環(huán)境角速率輸入下的二子樣旋轉(zhuǎn)矢量算法的姿態(tài)誤差曲線)P-4t/(s)圖8圓錐環(huán)境角速率輸入下的二子樣改進(jìn)旋轉(zhuǎn)矢量算法姿態(tài)誤差曲線標(biāo)準(zhǔn)圓錐運(yùn)動(dòng)環(huán)境下圓錐誤差主要體現(xiàn)在俯仰角上，且與時(shí)間呈線性