1、第二部分高動(dòng)態(tài)環(huán)境下捷聯(lián)慣導(dǎo)系統(tǒng)姿態(tài)算法研究1.引言在常規(guī)的捷聯(lián)慣性導(dǎo)航 系統(tǒng)的姿態(tài)算法中，總是認(rèn)為 載體坐標(biāo)系和參考坐標(biāo)系間 的轉(zhuǎn)換是通過一系列的轉(zhuǎn)動(dòng) 來實(shí)現(xiàn)的，而這些轉(zhuǎn)動(dòng)間的次 序并不重要，這是基于無限小 轉(zhuǎn)動(dòng)是矢量的原理來得到的。(1) 歐拉角由參考系OXKZ至載體 系OXbYbZb的變換可以通過 依次繞不同坐標(biāo)軸的三次連3-1-2歐拉角轉(zhuǎn)動(dòng)K續(xù)轉(zhuǎn)動(dòng)來定義。姿態(tài)歐拉角的運(yùn)動(dòng)學(xué)方程:(cox COS 0 + CDz sin 0)COS 0 cos 0 + (cox sin 6 - coz cos 0)sin 0 coz cos 0 -cdx sin 6方程中存在三角函數(shù)，給實(shí)時(shí)計(jì)算帶來困難

2、；且當(dāng)0 = 90°時(shí)，方 程中出現(xiàn)奇點(diǎn)，使0廠的解變得不確定，因而使其使用受到限制，不 能用于全姿態(tài)飛行器上。(2)方向余弦陣由參考系OXYZ至載體系OXbYbZb的姿態(tài)矩陣為C("0,&)=Cy(&)q(0)C&)第1次轉(zhuǎn)動(dòng):cos/-sin/sin/cos/0 0 11 0第 2 次轉(zhuǎn)動(dòng)：C*(0)= 0 COS00 sin0cos。0第3次轉(zhuǎn)動(dòng)：Cy(e)= o 1sin。0方向余弦矩陣的微分方程為：0sin0COS0-sin 60COS&C = CQ式中°z£1 = coz 0方程的解為:(3)元數(shù)四元數(shù)是具有四

3、個(gè)元素的超復(fù)數(shù)，它可以描述一個(gè)坐標(biāo)系或一個(gè)矢 量相對(duì)于某一坐標(biāo)系的旋轉(zhuǎn)Q0q =實(shí)數(shù)為四元數(shù)的標(biāo)量部分，03為四元數(shù)的矢量部分。s、<71= cos03 =Q2= esmQ3式中，0是表示旋轉(zhuǎn)軸方向的單位向量，0是旋轉(zhuǎn)角。 四個(gè)元素滿足正交約束方程亦 +qf +涇 + 二 1第一次轉(zhuǎn)動(dòng):第二次轉(zhuǎn)動(dòng):q"=C°S2 ° °sin?2cos? sin?2 2第三次轉(zhuǎn)動(dòng):A八 cosi0.esm 02由上述3-1-2歐拉角轉(zhuǎn)動(dòng)得到合成四元數(shù) q=qQq" 把上式表示為三次坐標(biāo)轉(zhuǎn)換矩陣的乘積，有A（q） = A（q，® q"

4、® qw）=禺（q°人（q） （q） 由四元數(shù)表征的運(yùn)動(dòng)學(xué)微分方程為 0 = 1%式中0coz方程的解為我們?cè)趯?duì)運(yùn)動(dòng)學(xué)方程求解時(shí)，都用了角速度矢量的積分，即A& = J :+人方 codt o而上述積分有意義的前提條件是角速度矢量方向不變。在力學(xué)中，剛體的有限轉(zhuǎn)動(dòng)是不可交換的。這個(gè)轉(zhuǎn)動(dòng)的不可交換性 決定了轉(zhuǎn)動(dòng)不是矢量，也就是兩次以上的轉(zhuǎn)動(dòng)不能相加。根據(jù)歐拉旋轉(zhuǎn)定理，飛行器的轉(zhuǎn)動(dòng)從任一給定方位到任一其它方位 可通過連續(xù)繞瞬時(shí)軸轉(zhuǎn)動(dòng)獲得，而瞬時(shí)角速度方向在空間不斷地改變， 對(duì)一個(gè)在空間方向隨時(shí)間變換的角速度矢量進(jìn)行積分是無意義的。當(dāng)不是定軸轉(zhuǎn)動(dòng)時(shí)，即血C）的方向在空間變

5、化時(shí)，式是不成立的，即角改變量不是矢量。因此，在采用角速度矢量積分時(shí)， 使得計(jì)算產(chǎn)生了誤差，稱為轉(zhuǎn)動(dòng)不可交換性誤差。對(duì)于捷聯(lián)慣導(dǎo)姿態(tài)更新來說，錐運(yùn)動(dòng)是最惡劣的工作環(huán)境條件，此 運(yùn)動(dòng)造成的不可交換性誤差影響最大，會(huì)誘發(fā)數(shù)學(xué)平臺(tái)的嚴(yán)重漂移。因 此，錐運(yùn)動(dòng)通常被作為檢驗(yàn)姿態(tài)算法優(yōu)劣的條件，也就說如果能夠確保 錐運(yùn)動(dòng)環(huán)境條件下的算法漂移最小，就一定能確保在其余環(huán)境條件下的算法漂移最小。2.錐運(yùn)動(dòng)與錐誤差圓錐效應(yī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)

6、動(dòng)或圓錐效應(yīng)。Y軸：QCOS（0t）Z軸：Qsin（0）圓錐運(yùn)動(dòng)對(duì)應(yīng)的角速度矢量在載體坐標(biāo)系上的分量為co? - -2 sin2 bcoy = 一屮 sm q sm 0bcoz =i/sma cos y/t該圓錐運(yùn)動(dòng)會(huì)在載體坐標(biāo)系毛軸上產(chǎn)生常值角速度，該角速度具有 與陀螺常值漂移相同的性質(zhì)，該角速度必為屯軸陀螺所敏感，從而產(chǎn)生 視在的測(cè)量誤差，即圓錐誤差。以上所討論的兩個(gè)角振動(dòng)的相位差為90。，當(dāng)相位差為°時(shí)，同樣 可以導(dǎo)出毛軸的常值角速度為b c 2 G cox - 一2肖 sm 一 sm ©由于角振動(dòng)速度的幅值、頻率和相位一般是隨機(jī)變量，由上式給出 的圓錐誤差表達(dá)式不能

7、用于實(shí)時(shí)的修正計(jì)算，只能用來說明圓錐誤差的 存在和對(duì)誤差量級(jí)的估計(jì)。圓錐誤差與剛體有限轉(zhuǎn)動(dòng)產(chǎn)生的不可交換誤差具有相同性質(zhì)。換言 之，圓錐誤差是在三維角振動(dòng)環(huán)境下剛體有限轉(zhuǎn)動(dòng)產(chǎn)生的不可交換誤 差。因此，可以在姿態(tài)算法中用一切解決不可交換性誤差的方法來減少 圓錐誤差的影響。3.旋轉(zhuǎn)矢量算法3.1旋轉(zhuǎn)矢量的定義由剛體定點(diǎn)轉(zhuǎn)動(dòng)的歐拉定 理，參考坐標(biāo)系可通過繞歐拉 軸旋轉(zhuǎn)特定的角度與固結(jié)于剛 體的動(dòng)坐標(biāo)系重合。設(shè)歐拉軸 上的單位矢量為力，旋轉(zhuǎn)角度為 0，則旋轉(zhuǎn)矢量定義為=0龍二血yF '當(dāng)剛體的定軸轉(zhuǎn)動(dòng)用旋轉(zhuǎn)矢量表示為如下時(shí)，剛體的運(yùn)動(dòng)可稱為圓 錐運(yùn)動(dòng)： 0 =Q COS（0）a sin（0）_

8、其中Q表示圓錐運(yùn)動(dòng)的錐半角，肖表示圓錐運(yùn)動(dòng)的角頻率。剛體在三維空間作任意轉(zhuǎn)動(dòng)時(shí)，其等效旋轉(zhuǎn)矢量w在i軸的分量 與剛體運(yùn)動(dòng)角速度在i軸上的分量之間有如下關(guān)系式： 2二 J： a）idt + At式中，外為由于測(cè)量的不可交換性而造成的不可測(cè)量項(xiàng)。對(duì)圓錐誤差的 補(bǔ)償，實(shí)際上就是求外的大小，旋轉(zhuǎn)矢量法能夠?qū)A錐誤差進(jìn)行有效的 補(bǔ)償。3. 2旋轉(zhuǎn)矢量微分方程旋轉(zhuǎn)矢量微分方程為計(jì)算捷聯(lián)慣性系統(tǒng)的姿態(tài)矩陣建立了全面的 理論基礎(chǔ)。根據(jù)Euler理論,旋轉(zhuǎn)矢量是姿態(tài)矩陣大小為+1的特征值所對(duì)應(yīng)的 特征向量，即(C /)=0且有廠 r sin 0不(1 cos0)不 IC = / + - x + 式中，二yzF表

9、示旋轉(zhuǎn)矢量，C為機(jī)體坐標(biāo)系與參考坐 標(biāo)系之間的方向余弦矩陣，/表示單位矩陣，0 =(卩J。 1 1= 69 + -Ox69 + 2辦旋轉(zhuǎn)矢量微分方程為1x（X0）2 （1 cos0）式中，cocox CDy血zF為機(jī)體角速度向量，方程右邊第二項(xiàng)與第 三項(xiàng)之和就是有限轉(zhuǎn)動(dòng)引起的不可交換項(xiàng)，即圓錐誤差。將上式右邊第三項(xiàng)系數(shù)按泰勒級(jí)數(shù)展開，則上式簡(jiǎn)化為11二血+ X0 + x（212上式即為旋轉(zhuǎn)矢量微分方程的常用形式。分析上式可以看出：當(dāng)載體作定軸轉(zhuǎn)動(dòng)時(shí)，旋轉(zhuǎn)矢量的方向與角速度血的方向一致，因 此上式右邊的后兩項(xiàng)為零，這時(shí)相當(dāng)于直接釆用角速度血進(jìn)行姿態(tài)解 算，是不會(huì)產(chǎn)生誤差的。但在實(shí)際情況中，載體運(yùn)

10、動(dòng)的角速度血的方向是不斷變化的，從而導(dǎo) 致某一時(shí)刻角速度血與當(dāng)前時(shí)刻所積累的旋轉(zhuǎn)矢量方向不一致，上式 右邊后兩項(xiàng)不為零，倘若仍然直接采用角速度血直接進(jìn)行積分解算就會(huì) 帶來剛體轉(zhuǎn)動(dòng)的不可交換性誤差。而且，當(dāng)載體作錐運(yùn)動(dòng)時(shí)，旋轉(zhuǎn)矢量和角速度血相互垂直，此時(shí)上 式右邊的后兩項(xiàng)最大，所以錐運(yùn)動(dòng)反映了不可交換誤差最為惡劣的情 況。3.3旋轉(zhuǎn)矢量微分方程的求解在實(shí)際應(yīng)用中，為保證實(shí)時(shí)性并考慮運(yùn)算方便，僅取前兩項(xiàng)，得=血+】xty2在姿態(tài)更新周期h二tk -%-1內(nèi)，通過對(duì)陀螺的角增量進(jìn)行等間隔 采樣，可求得等效旋轉(zhuǎn)矢量的估值。且根據(jù)等間隔釆樣次數(shù)，求解方法 分為單子樣法、雙子樣法、三子樣法與四子樣法。其中

11、，單子樣法就是 四元數(shù)法。（1）等效旋轉(zhuǎn)矢量算法的一般表達(dá)式設(shè)®）為"-1，"時(shí)間段內(nèi)的等效旋轉(zhuǎn)矢量，其中h = tk-tk_v （0） = 0。設(shè)對(duì)內(nèi)陀螺的角增量進(jìn)行N次等間隔采樣，并假 定載體角速度可用關(guān)于時(shí)間f的N -1次多項(xiàng)式進(jìn)行擬合,可得到等效旋 轉(zhuǎn)矢量算法的一般表達(dá)式八NN-l N (&(町二£仇+工乞KijGxOji=l i=l j>i式中，N為子樣數(shù)，0為陀螺輸出的角增量。(2)優(yōu)化準(zhǔn)則對(duì)捷聯(lián)慣導(dǎo)系統(tǒng)來說，錐運(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)境條件。這就

12、是說，如果能確保錐運(yùn)動(dòng)環(huán)境條件下的算法漂移最小, 就一定能確保在其余環(huán)境條件下的算法漂移最小。取圍繞慣性坐標(biāo)系兀軸旋轉(zhuǎn)的圓錐運(yùn)動(dòng)：=a COS（0）a sin（0）旋轉(zhuǎn)矢量算法的優(yōu)化設(shè)計(jì)的誤差準(zhǔn)則是使誤差達(dá)到最小。X(3)姿態(tài)更新在捷聯(lián)系統(tǒng)的實(shí)際姿態(tài)解算中，為了補(bǔ)償由于有限轉(zhuǎn)動(dòng)造成的不可 交換性誤差，采用旋轉(zhuǎn)矢量算法利用角增量計(jì)算姿態(tài)更新周期內(nèi)的姿態(tài) 變化量，但進(jìn)行姿態(tài)更新時(shí)是釆用四元數(shù)實(shí)現(xiàn)。記儀_1時(shí)刻導(dǎo)航系至機(jī)體系的姿態(tài)四元數(shù)為時(shí)刻導(dǎo)航系 至機(jī)體系的姿態(tài)四元數(shù)為Q(tk設(shè)在姿態(tài)更新周期htk-tk_x內(nèi)，機(jī)體系的姿態(tài)變化四元數(shù)為 q(h),則姿態(tài)更新如下側(cè))(A)I 2 丿 0"

13、)1 2 Jq(/z) = cos3.4簡(jiǎn)化形式的旋轉(zhuǎn)矢量?jī)?yōu)化算法假設(shè)在t,t + h間隔內(nèi)，對(duì)陀螺輸出信號(hào)進(jìn)行N次采樣，0表示第i次 釆樣的陀螺輸出角增量信號(hào)，有可第孤嚴(yán)（如口2川2 7 2 Q2(2i 1 )sm ii/ t +hI 2N丿一肋smNC 屮h一 2 sm a sm -2N(2i 1J屮t+h2N丿 cosC 屮h2 sm a sm -2N可得0X0 =-4sMsir?型l2日SI3(2z-lh -cos w f +2N丿r(2z-l Y|A<sir u/ th -sirH2N丿14i/h .A<cos| t +V IriA = -"sm Nsin(ci

14、f)sinih2N )2N丿2N丿"J-'i/h、Jn,由上式可以看出：在0 xq中，X軸分量只與相對(duì)時(shí)間（Z - j）h有關(guān), 而與絕對(duì)時(shí)間無關(guān)；y軸和Z軸分量是絕對(duì)時(shí)間t的余弦、正弦函數(shù)。在本文的圓錐運(yùn)動(dòng)下，能引起漂移的誤差出現(xiàn)在兀軸上。因此，具 有相同時(shí)間間隔的兩個(gè)角增量向量的叉乘對(duì)圓錐誤差的貢獻(xiàn)是相等的， 可以不考慮它們與絕對(duì)時(shí)間的關(guān)系。利用這個(gè)性質(zhì)，可以將圓錐誤差補(bǔ)償公式簡(jiǎn)化為N1=1(NT A k i=l 丿x&n可以看出，簡(jiǎn)化后的算法計(jì)算量大大減小。3.5改進(jìn)旋轉(zhuǎn)矢量?jī)?yōu)化算法的一般形式(1)利用前一周期陀螺角增量輸出的優(yōu)化算法顯然，通過增加對(duì)陀螺的釆樣次

15、數(shù)N,可以提高旋轉(zhuǎn)矢量的估算精 度，但是，與此同時(shí)計(jì)算機(jī)的計(jì)算量和存儲(chǔ)量也增加了。為了進(jìn)一步提高計(jì)算精度，可采用利用前一圓錐補(bǔ)償周期的角增量 輸出值進(jìn)行補(bǔ)償?shù)膱A錐補(bǔ)償方法，補(bǔ)償公式為N/ 1=1P工+i=lN-ii=lx&n式中，©、是加權(quán)系數(shù)，p是要利用的前一圓錐補(bǔ)償周期的角增量的 個(gè)數(shù)，erN_M是前一圓錐補(bǔ)償周期的陀螺輸出角增量。（2）利用前一周期角增量累加和的優(yōu)化算法若是考慮利用前一圓錐補(bǔ)償周期總的角度變化，即前一周期的角增 量累加和，則有如下形式的圓錐補(bǔ)償算法公式N/ =工0 +1=1N1i=lX 軸 + Gff x 0N 式中，&'為前一周期角增量

16、累加和，G為相應(yīng)的加系數(shù)，&二丫耳。1=1（3）改進(jìn)旋轉(zhuǎn)矢量?jī)?yōu)化算法的一般形式推廣到更一般的情形，綜合考慮前兩種改進(jìn)方法，可得到更通用的 圓錐補(bǔ)償算法N1=1pN1媯T+1+ 工®© i=li=lx 0N +G0f x0上述各種形式的圓錐誤差補(bǔ)償算法的系數(shù)如下表所示。子樣數(shù)Ga4«30bib4單子樣1/12一 了舞2/3-1/3011/15一 J 4T1/140-13/210323/420-1/18032/45三子樣9/2027/203/28057/140393/280-1/4201/40157/4201207/8401/1848-31/462061/15

17、401607/462013487/92401/3360243/5601539/1120nnM拌54/10592/105214/105-1/315168/315262/315656/3151/1386-31/34651277/23102762/34657321/3465四J懺-1/600643/18018-733/450455717/1001069337/9009096163/450451/25740-19/3003059/12012-1097/4504577531/360364489/600635303/60060-1/693008992/1732514912/173251696/825五子樣

18、125/25225/24325/2521375/5045/55441355/27722955/27723455/277215335/5544-5/24024215/7207217285/360361090/100187355/72072201335/720721/20592-19/24024295/4804816921/3603653321/4804828451/24024405673/144144-1/8751673/350064-743/40840824845/2450448281287/61261283795/742561422973/1225224693648/24504481/151

19、3512374375/7567561586875/1513512241250/189189518750/1891894圓錐誤差補(bǔ)償算法性能的仿真測(cè)試設(shè)載體沿x軸作典型的圓錐運(yùn)動(dòng)_ o=a cos（2 初） a sin（2 初）圓錐誤差的大小取決于錐運(yùn)動(dòng)參數(shù)，即錐半角Q與錐運(yùn)動(dòng)頻率/, 而各種圓錐誤差補(bǔ)償算法的補(bǔ)償精度與角增量的采樣間隔4有密切關(guān) 系。4. 1仿真實(shí)現(xiàn)流程仿真過程包含以下幾步：(1) 設(shè)定仿真初始條件(2) 求取真實(shí)姿態(tài)角載體作典型錐運(yùn)動(dòng)時(shí)的真實(shí)姿態(tài)四元數(shù)Q (” = qoq <72 §3的精確解是已知的，從而可以求得從導(dǎo)航系n至載體系b的方向余弦矩陣為Qo+Ql

20、 -Q2 -於2（0§2 §093）2（91§3 + §0§2）2（如§2 +903）2 2 2 2 Q0 Q1 + §2 §32（§2§3 一 §00）2（qW3 一 §0§2）2（§2§3 + §0如）2 2 2 2 Qo -Q1 §2 + §3設(shè)從導(dǎo)航系到載體系的三次旋轉(zhuǎn)順序?yàn)椋?(z)T&(y)Ty(“即 按照321的轉(zhuǎn)動(dòng)順序?qū)崿F(xiàn)的，則有51C2C13需=y(x)&(y)0(z)= C21 C

21、22 C23c13 = sin&cn = cos cos c12 = sin cos c21 = cos (p sin sin / - sin (p cos y c22 = sin (p sin & sin 廠 + cos cp cos y c23 = cos sin/c31 = cos (p sin 6 cos / + sin 9 sin y C32 = sin (p sin 0 cos y - cos (p sin y C33 = cos & cos 廠由此可以求得真實(shí)姿態(tài)角為（ 、=arctan 亠1 =c33）arctanf （筆3+摯1衛(wèi)0 - <71

22、02+03 丿0 - - arcsin（Q3） = 一 arcsin（2（3 一 QoQ2）arctanC 7（P - arctan =Wii丿2（qW2 +加3）飛 2221% + <71 一 02 - 如b遲=_泌_-2.2.sin2fa(3)姿態(tài)角估計(jì)值的求取載體繞X軸作錐運(yùn)動(dòng)時(shí)的角速度血"是已知的，即2丿-2吋sin asin（2初）2吋sin acos（2初）對(duì)血”積分得到陀螺的角增量輸出t+Nco(T)dr i = 12 、NT一 2 sin a sin2 . 2 a2rriT sm “ 2 sin 2/( t +2 sin a sinN2T2N2/T211/2N

23、> c ( 2d -、 L I 2N 丿J1接下來，計(jì)算出T+廠時(shí)刻的姿態(tài)四元數(shù)e(r+r)=e(o®(r)最后將e(r + T)代入V表達(dá)式,求出姿態(tài)角估計(jì)值八0與0。(4) 計(jì)算姿態(tài)角誤差將姿態(tài)角誤差定義為姿態(tài)角估計(jì)值與真實(shí)值之差，即A(p =(p-(p姿態(tài)角誤差的大小用于衡量各種補(bǔ)償算法的性能。4. 2仿真條件仿真條件 1： a = l°f f = lHz, 7 = 0.0245, Tf=24s仿真條件 2： a = l°f f = 10Hz,廠二 0.024s, 7）=24$其中，T二N 't為姿態(tài)更新周期（N為一個(gè)姿態(tài)更新周期內(nèi)的釆樣次 數(shù)

24、，即補(bǔ)償算法的子樣數(shù)），為仿真測(cè)試時(shí)間。4. 3仿真算法考察如下6種算法。 算法1：算法2：算法3：算法4：（單子樣算法）=&（單子樣加前一周期角增量算法） = & + £（*&）（雙子樣優(yōu)化算法）2=&1 + &2 + 3 （“I X &2 ）（雙子樣加前一周期角增量算法）=& + &2 +&扌 H &i X §3015算法5：(三子樣優(yōu)化算法)927=&1 +&2 +&3 + 函(&1 "3)+亦&2 x(&3 -&1) 算法

25、6：(四子樣優(yōu)化算法)214315(0 X &2 + &3 x &4 ) +46105(& X &3 + &2 X &4 )54714 z、+ 105如&4)+311(/&3)4. 4仿真結(jié)果仿真結(jié)果如下面的各圖所示。55555520154UUUJUUUiooa1015時(shí)間砂2025-iooa-2UUIJ：JUUU-4000Q1015時(shí)間砂20254UUU：JUUU2UIJIJiooa-iooa-2UUU3UULI-4000Q1015時(shí)間砂2025555555條件I下的真實(shí)姿態(tài)角曲線555510152025時(shí)間於.4.3,

26、20.0 0-O.20.30-0.4510152025時(shí)間於.4.30.0.2,20.30-0.4510152025-2-4-8O246-18單子樣算法在條件1下的姿態(tài)角誤差曲線2510152025時(shí)間於-O.J2 -O.J7 -O.JE:03 4 5 6 o o o o 0 0 0-0.-1.5x W56o 5.-O. sass5101520時(shí)間於25-1.5x 101.510.50-0.5-1510152025時(shí)間於單子樣加前一周期角增量算法在條件1下的姿態(tài)角誤差曲線52加5少10礦5O8642024681 bbbb 000-0.論遐蟄芻醫(yī)雙子樣加前一周期角增量算法在條件1下的姿態(tài)角誤差曲線d-2-3-4-65 W 152025時(shí)間砂5