硬件增強角速率圓錐優(yōu)化算法的姿態(tài)解算精度分析及改進

陳建鋒 陳熙源 祝雪芬

(1 東南大學微慣性儀表與先進導航技術(shù)教育部重點實驗室，南京210096)

(2 江蘇大學電氣信息工程學院，鎮(zhèn)江212013)

姿態(tài)解算是捷聯(lián)慣性導航系統(tǒng)(strapdown inertial navigation system，SINS)算法中最重要的部分.高精度的姿態(tài)解算通常由圓錐優(yōu)化算法和姿態(tài)四元數(shù)更新2 部分構(gòu)成［1］，其中圓錐優(yōu)化算法一直是SINS 研究的熱點內(nèi)容.

現(xiàn)有的圓錐優(yōu)化算法都是在Bortz［2］提出的旋轉(zhuǎn)矢量的基礎(chǔ)上發(fā)展起來的.經(jīng)典的圓錐優(yōu)化算法采用的是基于角增量信號的多子樣算法結(jié)構(gòu)，適用于輸出信號為角增量的陀螺儀.考慮到某些新型陀螺儀的輸出為角速率信號，如果直接采用公式提取角增量，并不能體現(xiàn)多子樣圓錐優(yōu)化算法原有的優(yōu)越性［3］.一種解決的辦法是通過硬件積分器得到角增量信號.在此基礎(chǔ)上，有學者提出了基于角增量加角速率信號(稱為硬件增強角速率)［4-7］的算法結(jié)構(gòu).另一種方法是采用基于純角速率信號［8-9］的算法結(jié)構(gòu).從優(yōu)化過程中的殘留誤差來看，這2 類算法具有同樣優(yōu)越的性能，但是鮮有文獻對這些算法的姿態(tài)解算精度進行分析(特別是姿態(tài)解算的精度極限).此外，國外有學者提出了新的優(yōu)化思路，采用最小二乘法使某一頻率范圍內(nèi)的算法誤差達到最?。?0］.

本文推導了經(jīng)典圓錐運動環(huán)境下旋轉(zhuǎn)矢量變化量的真值，分析了傳統(tǒng)的硬件增強角速率優(yōu)化算法的姿態(tài)解算精度，闡述了算法中周期分量的影響;在此基礎(chǔ)上，提出了一種改進的算法，對傳統(tǒng)算法中的周期分量進行了二次優(yōu)化;最后，利用Matlab進行仿真以驗證理論分析和算法推導的正確性.

1 傳統(tǒng)的硬件增強角速率圓錐算法

經(jīng)典圓錐運動環(huán)境下，表征坐標系之間相對運動的姿態(tài)四元數(shù)q(t)和角速率ω(t)分別為

式中，a 為半錐角;γ=2πf，f 為錐運動頻率［1］.

姿態(tài)四元數(shù)更新通常采用如下的迭代形式:

式中，q(t +ΔT)和q(t)分別為t +ΔT 和t 時刻的姿態(tài)四元數(shù);q(ΔT)為時間間隔［t，t+ΔT］內(nèi)姿態(tài)四元數(shù)的變化量，通常根據(jù)下式計算得到:

式中，Δσ=(Δσ·Δσ)1/2，Δσ 為時間間隔［t，t +ΔT］內(nèi)旋轉(zhuǎn)矢量的變化量［1］.

在傳統(tǒng)的硬件增強角速率圓錐算法中，角增量和角速率信號構(gòu)成了Δσ 的近似形式(記為Δ)，算法的優(yōu)化目標就是在經(jīng)典圓錐運動環(huán)境下根據(jù)Δσ 的理想值Δ最優(yōu)地確定Δ中的一系列系數(shù).Δ可以通過對Bortz 方程［2］積分并取前2 項近似得到.經(jīng)典圓錐運動環(huán)境下，Δ可以表示為

式中，僅x 軸分量不隨時間變化，是非周期性的.

將時間間隔［t，t + ΔT］劃分成N 個寬度為ΔT/N 的子區(qū)間，則可以在N +1 個時間點上從速率陀螺儀得到相應的角速率信號ωi(i =0，1，…，N).考慮到經(jīng)典圓錐運動環(huán)境下所有矢量積ωi×ωj的非周期分量都只與(j－i)的值有關(guān)［4］，傳統(tǒng)的硬件增強角速率圓錐算法通常表示為［4-7］

式中，ki為待優(yōu)化的系數(shù); α 為時間間隔［t，t +ΔT］內(nèi)通過硬件積分器得到的角增量信號.

表1 傳統(tǒng)的硬件增強角速率圓錐算法的優(yōu)化系數(shù)

2 傳統(tǒng)算法的姿態(tài)解算精度分析

傳統(tǒng)的硬件增強角速率圓錐算法的優(yōu)化只涉及非周期分量，如果滿足γΔT ＜1 的條件，則子樣數(shù)N 越大算法的殘留誤差越小［6-7］.考慮到上節(jié)中給出的參考基準是式(5)中的Δ，在只優(yōu)化非周期分量的前提下，算法的姿態(tài)解算精度不可能通過增加子樣數(shù)N 而無限地提高.具體的精度性能可以通過分析傳統(tǒng)算法的各軸分量相對于Δσ 的真值以及Δ的誤差進行確定.

經(jīng)典圓錐運動環(huán)境下，姿態(tài)四元數(shù)的變化量q(ΔT)可推導如下［1］:

根據(jù)式(4)和(7)，可以確定Δσ 的真值，即

式中

式(10)表明，參數(shù)ξ 是關(guān)于半錐角a、錐運動頻率f=γ/(2π)以及姿態(tài)四元數(shù)更新周期ΔT 的多元函數(shù).

定義如下參數(shù):

由式(8)和(12)可知，參數(shù)ηx，ηy，ηz與參數(shù)ξ之間的差距反映了Δ與Δσ 的真值之間的誤差.

類似地，定義如下3 個參數(shù):

則式(6)所示的傳統(tǒng)算法可以表示為

由式(8)和(14)可知，傳統(tǒng)算法的各軸分量相對于Δσ 的真值的誤差可以通過比較參數(shù)μx，μy，μz與參數(shù)ξ 進行確定.

由于式(12)和(14)結(jié)構(gòu)相同，分別比較式(11)和(13)中的x，y，z 軸參數(shù)，即可確定傳統(tǒng)算法的各軸分量相對于Δ的誤差.

表2 表征理想值與真值之間誤差的參數(shù)

表3 表征傳統(tǒng)算法與真值之間誤差的參數(shù)

由表2和表3可知，隨著子樣數(shù)N 的增加，參數(shù)μx逐漸逼近ηx，參數(shù)μy和μz不但偏離參數(shù)ηy和ηz而且表現(xiàn)出振幅越來越大的振蕩.即當N =1時，傳統(tǒng)算法與Δ之間的誤差由x 軸分量決定;當N 達到3 時，傳統(tǒng)算法與Δ之間的誤差則由y軸和z 軸分量共同決定.此外，表3中的三子樣和四子樣算法在y 軸、z 軸方向的差距都不大.因此，可認為當子樣數(shù)N 超過2 時，傳統(tǒng)算法的姿態(tài)解算精度將不再提高.

3 改進的算法

考慮上節(jié)對姿態(tài)解算精度的分析，在傳統(tǒng)硬件增強角速率圓錐優(yōu)化算法的基礎(chǔ)上，對算法中的周期分量進行二次優(yōu)化.由于式(5)、(6)中y 軸和z軸分量的區(qū)別只是時變的正弦、余弦函數(shù)的不同，因此采用如下的系數(shù)矩陣替換式(6)中的待優(yōu)化系數(shù)ki:

式中，λi為二次優(yōu)化系數(shù).

定義如下的二次優(yōu)化誤差準則:

將式(5)和(6)的z 軸分量代入式(16)，可得

確定二次優(yōu)化系數(shù)λi的步驟如下:

①將式(17)中的時變正弦函數(shù)統(tǒng)一展開成關(guān)于γ(t+ΔT/2)的三角函數(shù);

②令關(guān)于γ(t +ΔT/2)的余弦項的系數(shù)之和為零，即

③將關(guān)于γ(t +ΔT/2)的正弦項的系數(shù)之和進行冪級數(shù)展開，然后令某些關(guān)于γΔT 的低階項的系數(shù)為零，可得

④解式(18)和(19)構(gòu)成的方程組，確定λi.

N=1～4 時的二次優(yōu)化系數(shù)如表4所示，其中N=1 時的結(jié)果與表1中傳統(tǒng)算法的優(yōu)化系數(shù)相同.

表4 二次優(yōu)化系數(shù)

4 仿真結(jié)果

在經(jīng)典圓錐運動環(huán)境下，對傳統(tǒng)算法和相應的改進算法的姿態(tài)解算精度進行仿真研究.表5為9 種經(jīng)典圓錐運動的參數(shù)配置; 姿態(tài)四元數(shù)更新周期ΔT=0.01 s.仿真過程中，采用四階冪級數(shù)截斷形式［1］對式(4)中的三角函數(shù)系數(shù)進行近似.

圖1～圖4為8 號仿真環(huán)境下傳統(tǒng)算法和相應改進算法的姿態(tài)解算精度.圖1(a)為單子樣算法的姿態(tài)解算精度，圖1(b)為根據(jù)式(5)中的理想值Δ解算得到的姿態(tài)精度.限于篇幅，省略了與圖1～圖4類似的其他8 種仿真環(huán)境下的結(jié)果.

表5 經(jīng)典圓錐運動參數(shù)配置

圖1 單子樣算法及理想值的姿態(tài)解算精度

圖2 N=2 時的姿態(tài)解算精度

圖3 N=3 時的姿態(tài)解算精度

圖4 N=4 時的姿態(tài)解算精度

圖1～圖4中，當子樣數(shù)超過2 時，傳統(tǒng)算法的姿態(tài)解算精度不再有明顯的提高，這與第2 節(jié)的分析結(jié)果完全一致.子樣數(shù)N =2 時，根據(jù)改進算法解算得到的橫滾角誤差(δγ)和航向角誤差(δΨ)比傳統(tǒng)算法減小了1 個數(shù)量級; 俯仰角誤差(δθ)雖然沒有減小，但是消除了原有的強烈振蕩.這表明針對傳統(tǒng)二子樣算法中的周期分量進行的二次優(yōu)化是有效的，而且改進的二子樣算法的姿態(tài)解算精度主要由x 軸分量的殘留誤差決定.此外，改進的三子樣和四子樣算法的姿態(tài)解算精度與根據(jù)式(5)中的理想值Δˉ解算得到的結(jié)果幾乎完全一致(見圖1(b)、3(b)和4(b)).這是因為傳統(tǒng)三子樣和四子樣算法中的x 軸分量本身就與理想值Δ很接近(見表2和表3).

式(6)中，由于矢量積運算的次數(shù)隨子樣數(shù)N的增加而增加，因此在考慮同樣的精度性能時，改進的三子樣算法更值得推薦.

5 結(jié)語

傳統(tǒng)的硬件增強角速率圓錐算法的優(yōu)化只涉及非周期分量.分析表明:在子樣數(shù)超過2 時，傳統(tǒng)優(yōu)化算法中周期分量是主要的影響因素，相應的姿態(tài)解算精度不再提高.在此基礎(chǔ)上，提出了一種改進的算法，對傳統(tǒng)算法中的周期分量進行了二次優(yōu)化.仿真結(jié)果表明對傳統(tǒng)算法的姿態(tài)解算精度的分析是正確的，而且只需采用改進的三子樣算法就可使最終的姿態(tài)解算精度與根據(jù)理想值得到的結(jié)果幾乎完全一致.

References)

［1］Miller R B.A new strapdown attitude algorithm［J］.Journal of Guidance，Control，and Dynamics，1983，6(4): 287-291.

［2］Bortz J E.A new mathematical formulation for strapdown inertial navigation［J］.IEEE Transactions on Aerospace and Electronic Systems，1971，7(1): 61-66.

［3］黃昊，鄧正隆.角速率輸入下的航姿算法研究［J］.中國慣性技術(shù)學報，2000，8(2): 21-26.

Huang Hao，Deng Zhenglong.Study of navigation attitude algorithms for angular rate input［J］.Journal of Chinese Inertial Technology，2000，8(2): 21-26.(in Chinese)

［4］黃昊，鄧正隆.旋轉(zhuǎn)矢量航姿算法的一種新的表達式［J］.宇航學報，2001，22(3): 92-98.

Huang Hao，Deng Zhenglong.A new expression for rotation vector attitude algorithm［J］.Journal of Astronautics，2001，22(3): 92-98.(in Chinese)

［5］魏曉虹，張春熹，朱奎寶.一種高精度角速率圓錐補償算法［J］.北京航空航天大學學報，2005，31(12):1312-1316.

Wei Xiaohong，Zhang Chunxi，Zhu Kuibao.High accuracy coning compensate algorithm by angular velocity［J］.Journal of Beijing University of Aeronautics and Astronautics，2005，31(12): 1312-1316.(in Chinese)

［6］曾慶化，劉建業(yè)，趙偉，等.激光陀螺慣導系統(tǒng)硬件增強角速率輸入圓錐算法［J］.東南大學學報: 自然科學版，2006，36(5): 746-750.

Zeng Qinghua，Liu Jianye，Zhao Wei，et al.Coning algorithm with hardware-enhanced angle rate of RLG SINS［J］.Journal of Southeast University: Natural Science Edition，2006，36(5): 746-750.(in Chinese)

［7］Wei X Y，F(xiàn)u Z X，Deng Z L.Research on coning compensation algorithms for SINS of angular rate input［J］.Journal of Harbin Institute of Technology:New Series，2007，14(4): 541-544.

［8］曾慶化，劉建業(yè)，熊智，等.一種角速率激光陀螺慣導系統(tǒng)高精度姿態(tài)算法［J］.上海交通大學學報，2006，40(12): 2159-2163.

Zeng Qinghua，Liu Jianye，Xiong Zhi，et al.A highaccuracy attitude algorithm of ring laser gyro strapdown inertial navigation system with pure angle rate output［J］.Journal of Shanghai Jiaotong University，2006，40(12): 2159-2163.(in Chinese)

［9］Ben Y Y，Sun F，Gao W，et al.Generalized method for improved coning algorithms using angular rate［J］.IEEE Transactions on Aerospace and Electronic Systems，2009，45(4): 1565-1572.

［10］Savage P G.Coning algorithm design by explicit frequency shaping［J］.Journal of Guidance，Control，and Dynamics，2010，33(4): 1123-1132.