捷聯(lián)慣導(dǎo)四子樣旋轉(zhuǎn)矢量姿態(tài)更新算法
張澤，段廣仁
（哈爾濱工業(yè)大學(xué)控制理論與制導(dǎo)技術(shù)研究中心，黑龍江哈爾濱15000）
摘 要：姿態(tài)更新算法是捷聯(lián)慣性導(dǎo)航系統(tǒng)的關(guān)鍵算法，目前姿態(tài)更新算法有歐拉角法、四元數(shù)法、方向余弦法和旋轉(zhuǎn)矢量法。旋轉(zhuǎn)矢量法可以采用多子樣算法實(shí)現(xiàn)對不可交換誤差的補(bǔ)償。針對利用陀螺角增量輸出進(jìn)行姿態(tài)更新計(jì)算帶來的不可交換性誤差，考慮到導(dǎo)航坐標(biāo)系在姿態(tài)更新周期內(nèi)旋轉(zhuǎn)比較緩慢的特點(diǎn)，研究了捷聯(lián)慣導(dǎo)姿態(tài)更新的旋轉(zhuǎn)矢量的修正算法，以此為基礎(chǔ)詳細(xì)推導(dǎo)了四子樣的旋轉(zhuǎn)矢量算法，得出利用陀螺角增量求解等效旋轉(zhuǎn)矢量的顯式形式。該顯式形式中直接利用陀螺的增量輸出，便于工程實(shí)際中應(yīng)用。
關(guān)鍵詞：姿態(tài)更新；等效旋轉(zhuǎn)矢量；四子樣；姿態(tài)修正
中圖分類號：tp 27 文獻(xiàn)標(biāo)識碼：a
1引言
姿態(tài)更新是實(shí)時地解算從機(jī)體坐標(biāo)系到導(dǎo)航坐標(biāo)系的方向余弦矩陣，姿態(tài)更新算法是捷聯(lián)慣性導(dǎo)航系統(tǒng)的關(guān)鍵算法。傳統(tǒng)的姿態(tài)更新算法有歐拉角法、方向余弦法和四元數(shù)法，其中，四元數(shù)皮卡算法簡單、計(jì)算量小，因而在工程實(shí)際中常采用。但是該算法對不可交換誤差的補(bǔ)償不****，特別是運(yùn)載體高動態(tài)時，這種誤差更加嚴(yán)重。而等效旋轉(zhuǎn)矢量法對這種誤差作了適當(dāng)補(bǔ)償，特別適用于高動態(tài)的環(huán)境下工作。本文推導(dǎo)了四予樣旋轉(zhuǎn)矢量算法，并推導(dǎo)了利用旋轉(zhuǎn)矢量進(jìn)行姿態(tài)解箅時的修正算法。
2基本關(guān)系式
1)向量坐標(biāo)變換的四元數(shù)乘表示法和坐標(biāo)變換矩陣表示法如果將向量r（向量r在r系中的投影）和r6（向量r在6系中的投影）看作零標(biāo)量的四元數(shù)，則rr和r6聞的變換關(guān)系可采用四元數(shù)乘式中，o表示四元數(shù)乘；q表示從r系到6系的旋轉(zhuǎn)四元數(shù)；q 8表示其共軛四元數(shù)。
而坐標(biāo)變換矩陣表示方法為
式中，cr為從b系到r系的坐標(biāo)變化矩陣。
2)四元數(shù)的三角式及四元數(shù)微分方程q=cos（0/2）+ursiri（0/2），當(dāng)用其描述剛體定點(diǎn)轉(zhuǎn)動，即當(dāng)只關(guān)心b系相對月系的角位置時，可認(rèn)為b系是由r系經(jīng)過無中間過程的一次性等效旋轉(zhuǎn)形成，為瞬時旋轉(zhuǎn)軸和旋轉(zhuǎn)方向，θ為轉(zhuǎn)過的角度。
下面不加證明地給出四元數(shù)的微分方程：
3)姿態(tài)四元數(shù)如果上述中b系表征運(yùn)載體機(jī)體坐標(biāo)系，r系表征運(yùn)載體的導(dǎo)航坐標(biāo)系n，則q為運(yùn)載體的姿態(tài)四元數(shù)。
3旋轉(zhuǎn)矢量和姿態(tài)四元數(shù)的關(guān)系
設(shè)tk時刻的機(jī)體坐標(biāo)系為b(k)，導(dǎo)航坐標(biāo)系為n(k)，tk+1時刻的機(jī)體坐標(biāo)系為b(k+1)，導(dǎo)航坐標(biāo)系為n(k+1)。記6(k)至b(k+1)的旋轉(zhuǎn)四元數(shù)為g(h)，n( k)至b(k)的旋轉(zhuǎn)四元數(shù)q(tk)，即為tk；時刻的姿態(tài)四元數(shù)，n（k十1）至b(k+1)的旋轉(zhuǎn)四元數(shù)為q(tk+1)，即為tk=1
，時刻的姿態(tài)四元數(shù)，n(k)至n(k+1)的旋轉(zhuǎn)四元數(shù)為p(h)，其中，h=tk-1-tk為姿態(tài)更新周期：根據(jù)式(2)可得：
還可以得到：
由式(1)可以得到下式：
依據(jù)式(1)還能得到如下各式：
聯(lián)立以上各式可得到：
四元數(shù)的乘法結(jié)合律，上式可以寫作：
比較上式和式(5)可得：
| 
