王　真，高鳳岐，高　敏，高偉偉

（軍械工程學(xué)院，河北　石家莊　050003）

旋轉(zhuǎn)矢量多迭代捷聯(lián)姿態(tài)解算誤差補(bǔ)償算法

王真，高鳳岐，高敏，高偉偉

（軍械工程學(xué)院，河北石家莊050003）

為克服高動(dòng)態(tài)條件下的捷聯(lián)姿態(tài)解算存在不可交換性誤差的問題，達(dá)到進(jìn)一步增強(qiáng)捷聯(lián)姿態(tài)誤差抑制效果的目的，基于角速率的輸出提出了等效旋轉(zhuǎn)矢量三子樣二次迭代優(yōu)化算法，推導(dǎo)對(duì)應(yīng)的圓錐補(bǔ)償算法方程及其表達(dá)式。分別在不同圓錐運(yùn)動(dòng)頻率情況下和不同姿態(tài)更新頻率情況下，展開仿真驗(yàn)證算法的漂移誤差和俯仰角誤差，以傳統(tǒng)的四元數(shù)法、三子樣算法為對(duì)照，分析仿真數(shù)據(jù)曲線，得出本改進(jìn)算法在精度和穩(wěn)定性方面均有較大提高。在單軸速率轉(zhuǎn)臺(tái)上進(jìn)行光纖陀螺的實(shí)測(cè)驗(yàn)證中，通過調(diào)整圓錐運(yùn)動(dòng)半偏角和頻率，測(cè)量獲取光纖陀螺慣組輸出情況，結(jié)果表明：該算法在高動(dòng)態(tài)條件下受圓錐半角、圓錐運(yùn)動(dòng)頻率的影響較小，性能更加優(yōu)越。

捷聯(lián)姿態(tài)測(cè)量；高動(dòng)態(tài)；圓錐誤差補(bǔ)償；等效旋轉(zhuǎn)矢量

0　引言

捷聯(lián)慣性導(dǎo)航系統(tǒng)一般與載體固聯(lián)，直接感測(cè)載體的運(yùn)動(dòng)［1］，姿態(tài)實(shí)時(shí)計(jì)算是捷聯(lián)慣導(dǎo)的關(guān)鍵技術(shù)［2］。在導(dǎo)彈等高過載、高頻姿態(tài)變化的高動(dòng)態(tài)環(huán)境下，傳感器采集的離散點(diǎn)信息較難反映真實(shí)的載體空間轉(zhuǎn)動(dòng)次序，圓錐運(yùn)動(dòng)時(shí)尤為明顯，故也稱圓錐誤差。圓錐誤差是姿態(tài)解算的重難點(diǎn)問題，方向余弦法、四元數(shù)法［3］、三子樣算法［4］等傳統(tǒng)圓錐補(bǔ)償算法會(huì)引起較大的不可交換性誤差［5］。作為高動(dòng)態(tài)情形下的誤差抑制重難點(diǎn)，圓錐誤差的有效抑制對(duì)導(dǎo)彈等載體的姿態(tài)解算準(zhǔn)確度提高有顯著意義。

陳建鋒等［6］提出一種角速率輸入下的二次優(yōu)化補(bǔ)償算法，湯傳業(yè)等［7］引入最小二乘方法進(jìn)行圓錐誤差補(bǔ)償優(yōu)化，Song等［8］進(jìn)一步討論了傳統(tǒng)的圓錐誤差補(bǔ)償算法在機(jī)動(dòng)條件下的實(shí)用性問題，黃磊等［9］提出了基于高階補(bǔ)償模型的新圓錐算法。

本文針對(duì)高動(dòng)態(tài)圓錐誤差補(bǔ)償?shù)碾y題，以等效旋轉(zhuǎn)矢量三子樣算法為基礎(chǔ)，基于增加旋轉(zhuǎn)矢量迭代次數(shù)提出了一種基于角速率輸出的二次迭代改進(jìn)算法，并優(yōu)化了旋轉(zhuǎn)矢量計(jì)算系數(shù)。通過上述算法改進(jìn)，減小姿態(tài)更新過程中的計(jì)算量，有利于提高姿態(tài)解算準(zhǔn)確度，尤其是在高動(dòng)態(tài)條件下提高算法性能。

1　改進(jìn)型算法的提出

基于陀螺角增量輸出是傳統(tǒng)的等效旋轉(zhuǎn)矢量法的研究前提，這種方法需要先轉(zhuǎn)換角增量信息為角速率信息，再計(jì)算等效旋轉(zhuǎn)矢量，這無(wú)疑增加了算法計(jì)算量且引入誤差。直接針對(duì)基于角速率輸出的算法研究并不多，本文旨在破除轉(zhuǎn)換環(huán)節(jié)，直接面向角速率輸出，從而提升算法性能和解算準(zhǔn)確度。

1.1角速率輸出的等效旋轉(zhuǎn)矢量推導(dǎo)

下面推導(dǎo)針對(duì)角速率輸出的等效旋轉(zhuǎn)矢量，其過程如下：

記tk是第k次采樣時(shí)刻，設(shè)h為姿態(tài)更新的周期，且h=tk-tk-1，在姿態(tài)更新周期內(nèi)，記等效旋轉(zhuǎn)矢量為Φ（h），在h=0處用泰勒級(jí)數(shù)法將Φ（h）展開：

Φ（0）顯然為0。ω是載體系相對(duì)于參考系的轉(zhuǎn)動(dòng)角速率，運(yùn)用三次型拋物線對(duì)其進(jìn)行擬合，過程如下：

令在tk-1、tk-1+h/3、tk-1+2h/3、tk時(shí)刻計(jì)算出的角速率分別對(duì)應(yīng)為ω1、ω2、ω3、ω4，將其代入上面的公式，得到四元數(shù)q（h）：

由此可推得ω（tk-1+），Δθ（）在=0處及各階導(dǎo)數(shù)為

將上式代入Φ（h）在h=0處的泰勒展開式，由角速率系數(shù)與角速率的關(guān)系，得到基于角速率輸出的等效旋轉(zhuǎn)矢量估計(jì)值為

上式是對(duì)角速率進(jìn)行三次曲線擬合后得到的，于是稱其為基于角速率輸出的等效旋轉(zhuǎn)矢量三子樣算法。得到等效旋轉(zhuǎn)矢量Φ后，可計(jì)算出姿態(tài)更新周期內(nèi)的變化四元數(shù)q（h），表示如下：

在下個(gè)姿態(tài)更新周期的初始時(shí)刻，其四元數(shù)Q（t+h）可算出，并有如下表示（?為四元數(shù)乘）：

在姿態(tài)更新周期內(nèi)，四元數(shù)更新由此實(shí)現(xiàn)。選取在平衡位置附近旋轉(zhuǎn)的圓錐運(yùn)動(dòng)，其等效旋轉(zhuǎn)矢量：

式中：α——圓錐角；

Ω——角頻率。

在上面描述的運(yùn)動(dòng)情況下，其四元數(shù)為

根據(jù)式（11），得到：

那么，相對(duì)應(yīng)的角速率ω（t）可表示為

若姿態(tài)更新頻率較大時(shí)，sin（Φ/2）≈Φ/2，近似簡(jiǎn)化后的等效旋轉(zhuǎn)矢量：

旋轉(zhuǎn)矢量誤差Φε=Φ-。若想讓?duì)郸抛钚。枰O(shè)置低次冪項(xiàng)為零，從而得出在圓錐運(yùn)動(dòng)情況下，在沿著圓錐軸軸向的等效旋轉(zhuǎn)矢量的誤差估計(jì)值為

1.2等效旋轉(zhuǎn)矢量算法的改進(jìn)

在姿態(tài)更新頻率較高的條件下，采用多次迭代辦法更新旋轉(zhuǎn)矢量，對(duì)抑制圓錐誤差有積極影響。按照這一思想，在基于角速率輸出情況，推導(dǎo)了等效旋轉(zhuǎn)矢量的三子樣二次迭代優(yōu)化算法。

二次迭代更新時(shí)，設(shè)h為姿態(tài)更新周期，其迭代周期T=h/2，［tk-1，tk-1+T］和［tk-1+T，tk-1+2T］則分別為兩段迭代周期。在迭代周期［tk-1，tk-1+T］內(nèi)，相對(duì)參考系下的載體系角速率可表示為［10］

用泰勒級(jí)數(shù)展開Φ（tk-1+T）。

Φ（tk-1）=0，則Φ 在t=tk-1時(shí)刻各階導(dǎo)數(shù)皆可知，綜上可得：

在［tk-1+T，tk-1+2T］周期內(nèi)，其表達(dá)式亦同理可得。

綜合上面的內(nèi)容，在姿態(tài)更新周期內(nèi)，關(guān)于旋轉(zhuǎn)矢量的估計(jì)結(jié)果表示如下：

旋轉(zhuǎn)矢量增量為

Mij，Nij分別作為角速率叉乘項(xiàng)的待定系數(shù)。

對(duì)系數(shù)優(yōu)化過程如下：

在典型的圓錐情況時(shí)，ωi×ωj僅僅與相對(duì)時(shí)間間隔h（j-i）有關(guān)聯(lián)。故將上式簡(jiǎn)化為

在典型圓錐運(yùn)動(dòng)條件下，算法的誤差漂移［11］：

用泰勒公式將上式展開，可以獲到Ωh的各階系數(shù)，令（Ωh）3，（Ωh）5，（Ωh）7的系數(shù)為零。算法可表示為如下形式：

其中

表示姿態(tài)更新周期內(nèi)角增量值［12］。

2　仿真實(shí)驗(yàn)分析

開展仿真實(shí)驗(yàn)的總體思路是將四元數(shù)法［13］、三子樣算法［14］還有改進(jìn)算法進(jìn)行對(duì)比，來(lái)驗(yàn)證本文改進(jìn)算法的效果，實(shí)驗(yàn)過程中預(yù)設(shè)圓錐角α=1.5°。

2.1驗(yàn)證算法漂移誤差

首先，將圓錐運(yùn)動(dòng)的頻率固定為4Hz，隨著姿態(tài)更新頻率的變化，觀察算法漂移誤差（X軸方向分量），如圖1所示。然后，將姿態(tài)更新頻率固定為100Hz，隨著圓錐頻率的變化，觀察算法漂移誤差（X軸方向分量）變化情況，如圖2所示。

圖1　不同姿態(tài)更新頻率下的算法誤差

圖2　不同圓錐運(yùn)動(dòng)頻率下的算法誤差

由圖知，算法漂移誤差與圓錐運(yùn)動(dòng)頻率呈正比，與姿態(tài)更新頻率呈反比，改進(jìn)算法在抑制圓錐運(yùn)動(dòng)誤差方面性能更佳。

2.2驗(yàn)證俯仰角誤差

以俯仰角解算為例［15］，以期望更加直觀地分析圓錐運(yùn)動(dòng)施加于姿態(tài)解算的影響。在較典型圓錐運(yùn)動(dòng)條件［16］下，分別采用3種算法計(jì)算相對(duì)參考系的載體俯仰角。

設(shè)α=1.5°，仿真時(shí)間t=300s，分別在錐頻率4Hz、8 Hz條件下，采用以上算法得到的俯仰角誤差結(jié)果如圖3和圖4所示。

2.3分析仿真結(jié)果

在設(shè)定好的圓錐運(yùn)動(dòng)情況下，四元數(shù)法解算出的俯仰角誤差相較之下明顯較大；當(dāng)圓錐頻率比較低時(shí)，關(guān)于俯仰角解算誤差，改進(jìn)算法優(yōu)于三子樣算法但在精度與穩(wěn)定性方面相差不是很大，但當(dāng)圓錐頻率明顯增大時(shí)，改進(jìn)算法在精度與穩(wěn)定性方面的優(yōu)勢(shì)更加顯著。

3　圓錐誤差補(bǔ)償實(shí)測(cè)數(shù)據(jù)驗(yàn)證

3.1試驗(yàn)平臺(tái)及試驗(yàn)條件

為更好地驗(yàn)證本改進(jìn)型補(bǔ)償算法在圓錐運(yùn)動(dòng)條件下的性能，搭建實(shí)測(cè)試驗(yàn)平臺(tái)，其主要組成是被測(cè)光纖陀螺和單軸速率轉(zhuǎn)臺(tái)。

在單軸速率轉(zhuǎn)臺(tái)上，運(yùn)用某型光纖陀螺慣組來(lái)模擬載體的圓錐運(yùn)動(dòng)。用安裝偏角模擬半錐角α，圓錐運(yùn)動(dòng)頻率f由設(shè)定速率轉(zhuǎn)臺(tái)參數(shù)來(lái)模擬，姿態(tài)更新周期h是在姿態(tài)解算算法中設(shè)定的。具體實(shí)驗(yàn)條件如表1所示。

圖3　錐頻率4Hz下的俯仰角誤差

圖4　錐頻率8Hz下的俯仰角誤差

表1　實(shí)測(cè)的實(shí)驗(yàn)條件

3.2實(shí)測(cè)數(shù)據(jù)情況

實(shí)測(cè)試驗(yàn)開展的思路是，調(diào)整圓錐運(yùn)動(dòng)情況，獲取光纖陀螺慣組輸出情況，圓錐運(yùn)動(dòng)的調(diào)整分別由不同α、f來(lái)實(shí)現(xiàn)。需要指出的是，在實(shí)驗(yàn)中沒必要對(duì)半錐角詳細(xì)羅列，僅區(qū)分較小與較大兩大類情況即可，并且分別由as和am表示。

圖5　不同模擬圓錐運(yùn)動(dòng)下方位角誤差

實(shí)測(cè)3組數(shù)據(jù)，兩兩互為對(duì)比，具體條件設(shè)定如表2所示。

表2　實(shí)測(cè)的條件設(shè)定情況1）

在單軸速率轉(zhuǎn)臺(tái)上固定慣組后，通過初始精對(duì)準(zhǔn)得出兩個(gè)半錐角數(shù)值：as=0.9527°，am=3.9318°。以方位角為例，對(duì)比3種算法在3組條件下的姿態(tài)解算誤差情況，結(jié)果如圖5所示。

3.3實(shí)測(cè)結(jié)果分析

在同一種圓錐運(yùn)動(dòng)情況下對(duì)比，本改進(jìn)算法的姿態(tài)解算精度比其他兩種算法大幅提升，幾乎提升了一個(gè)數(shù)量級(jí)。在圓錐半角一樣時(shí)，圓錐運(yùn)動(dòng)頻率越大則引起更大的姿態(tài)解算誤差；在圓錐頻率一樣時(shí)，圓錐半角越大則引起更大的姿態(tài)解算誤差。本改進(jìn)型算法相比前兩種算法來(lái)說(shuō)，受圓錐半角、圓錐運(yùn)動(dòng)頻率的影響很小，算法性能更加穩(wěn)定。

4　結(jié)束語(yǔ)

為有針對(duì)性地解決高動(dòng)態(tài)條件下傳統(tǒng)圓錐補(bǔ)償算法有不可交換性誤差的問題，探討了基于角速率輸出的等效旋轉(zhuǎn)矢量的三子樣二次迭代優(yōu)化算法，推導(dǎo)了相關(guān)的圓錐補(bǔ)償算法方程及表達(dá)式。

通過仿真計(jì)算和試驗(yàn)驗(yàn)證，對(duì)比四元數(shù)法和三子樣法，證明了本改進(jìn)算法的優(yōu)越性。首先，在不同的圓錐頻率與不同更新頻率情況下，本改進(jìn)算法的誤差精度方面均有較明顯提高；其次，本改進(jìn)算法受圓錐半角、圓錐頻率帶來(lái)的影響更小，高動(dòng)態(tài)下性能更優(yōu)與穩(wěn)定性更好。此外，本算法的研究能針對(duì)性地解決高動(dòng)態(tài)姿態(tài)解算的不可交換性誤差補(bǔ)償?shù)膯栴}，解決了算法復(fù)雜度與計(jì)算實(shí)時(shí)性的矛盾，改善整體性能，為下一步繼續(xù)深入挖掘圓錐誤差補(bǔ)償算法提供有益的理論參考和實(shí)驗(yàn)依據(jù)。

［1］程承，潘泉，李漢舟.一種新的捷聯(lián)慣導(dǎo)系統(tǒng)圓錐誤差補(bǔ)償算法研究［J］.彈箭與制導(dǎo)學(xué)報(bào)，2014，34（1）：1-4.

［2］李海濤，曹詠弘，祖靜.等效旋轉(zhuǎn)矢量法在旋轉(zhuǎn)彈姿態(tài)解算中的應(yīng)用［J］.測(cè)試技術(shù)學(xué)報(bào)，2011，25（4）：287-291.

［3］張榮輝，賈宏光.基于四元數(shù)法的捷聯(lián)式慣性導(dǎo)航系統(tǒng)的姿態(tài)解算［J］.光學(xué)精密工程，2008，16（10）：1963-1970.

［4］MILLER R B.A new strapdown attitude algorithm［J］.Journal of Guidance Control and Dynamics，1983，6（4）：287-291.

［5］黃磊，劉建業(yè)，曾慶化.基于高階補(bǔ)償模型的新圓錐算法［J］.中國(guó)慣性技術(shù)學(xué)報(bào)，2013，21（1）：37-41.

［6］陳建鋒，陳熙源，祝雪芬.硬件增強(qiáng)角速率圓錐優(yōu)化算法的姿態(tài)解算精度分析及改進(jìn)［J］.東南大學(xué)學(xué)報(bào)（自然科學(xué)版），2012，42（4）：632-636.

［7］湯傳業(yè)，陳熙源，李建利.一種角速率輸入的圓錐算法設(shè)計(jì)［J］.中國(guó)慣性技術(shù)學(xué)報(bào)，2013，21（4）：456-461.

［8］SONG M，WU W Q，PAN X F.Approach to recovering maneuveraccuracyinclassicalconingalgorithms［J］. Journal of Guidance Control and Dynamics，2013，36（6）：1872-1880.

［9］黃磊，劉建業(yè)，曾慶化.基于高階補(bǔ)償模型的新圓錐算法［J］.中國(guó)慣性技術(shù)學(xué)報(bào)，2013，21（1）：37-41.

［10］屈熠，趙忠.增強(qiáng)型等效旋轉(zhuǎn)矢量算法在圓錐運(yùn)動(dòng)下的仿真［J］.計(jì)算機(jī)仿真，2010，27（11）：25-27.

［11］李海濤，曹詠弘，祖靜.等效旋轉(zhuǎn)矢量法在旋轉(zhuǎn)彈姿態(tài)解算中的應(yīng)用［J］.測(cè)試技術(shù)學(xué)報(bào)，2011，25（4）：287-291.

［12］袁張賢，王鑫偉.斜直圓筒內(nèi)基于Euler-Rodrigues參數(shù)的細(xì)長(zhǎng)桿螺旋屈曲研究［J］.計(jì)算物理，2012，29（4）：549-556.

［13］梁鋒，周衛(wèi)東，馬薈.基于四元數(shù)的艦船捷聯(lián)慣導(dǎo)粗對(duì)準(zhǔn)方法研究［J］.艦船科學(xué)技術(shù)，2013，35（8）：53-56.

［14］湯傳業(yè)，陳熙源.一種角速率輸入的捷聯(lián)慣導(dǎo)姿態(tài)算法［J］.東南大學(xué)學(xué)報(bào)，2014，44（3）：543-549.

［15］彭孝東，張鐵民，李繼宇，等.基于傳感器校正與融合的農(nóng)用小型無(wú)人機(jī)姿態(tài)估計(jì)算法［J］.自動(dòng)化學(xué)報(bào)，2015，41（4）：854-860.

［16］HUANGL，LIU J Y，ZENG Q H.Optimized strapdown coning correctionalgorithm［J］.Transactions of Nanjing University of Aeronautics&Astronautics，2013，30（4）：343-349.

（編輯：李妮）

Research on error compensation improved algorithm of strapdown attitude&heading measurement based on multiple iteration rotation vector

WANG Zhen，GAO Fengqi，GAO Min，GAO Weiwei
（Ordnance Engineering College，Shijiazhuang 050003，China）

To reduce the traditional coning error compensation algorithm noncommutativity errors under high dynamic conditions and to restrain the coning error，proposed the three-sample-andtwo-time iteration algorithm of equivalent rotation vector based on angular rate，and deduced the expression of coning error compensation algorithm.At different coning motion frequency and update frequency，simulation validation showed this algorithm had advantages in accuracy and stability than quaternion and three sub-sample algorithms.Experimental results show that there was smaller influence of coning motion frequency and cone angle on this algorithm，thus this algorithm is better than traditional algorithm under high dynamic conditions.

strapdown attitude&heading measurement；high dynamic；coning error compensation；equivalent rotation vector

A

1674-5124（2016）08-0113-05

10.11857/j.issn.1674-5124.2016.08.023

2015-09-10；

2015-10-11

裝備預(yù)研共用技術(shù)基金項(xiàng)目（9140A05040213JB34069）

王真（1991-），男，河北唐山市人，碩士研究生，專業(yè)方向?yàn)榫軆x器與微系統(tǒng)。