捷聯慣導中求解圓錐誤差系數的通用算法

嚴恭敏，楊小康，翁 浚，秦永元

(西北工業大學 自動化學院，西安 710072)

捷聯慣導中求解圓錐誤差系數的通用算法

嚴恭敏，楊小康，翁 浚，秦永元

(西北工業大學 自動化學院，西安 710072)

在捷聯慣導姿態更新算法中，針對傳統的由等效旋轉矢量微分方程求解多子樣優化圓錐誤差補償系數的推導過程比較繁瑣的問題，提出了一種新的計算任意子樣數優化圓錐誤差補償系數的數值算法。在圓錐角運動條件下，新算法對角速度和角增量三角函數作泰勒級數展開，再根據圓錐誤差積分和不同子樣角增量叉乘計算過程中的多項式系數向量叉乘特點，將叉乘運算轉化為多項式系數的卷積運算，新算法易于軟件編程實現。最后，通過仿真計算給出了所有共計21種1～6子樣優化圓錐誤差補償系數及相應的算法漂移。

捷聯姿態更新算法；優化圓錐誤差補償算法；等效旋轉矢量；卷積運算；數值解

0 引言

目前，捷聯慣導系統的姿態更新算法中普遍采取的思路[1]是：根據不可交換誤差補償算法，使用陀螺角增量的多子樣采樣構造等效旋轉矢量，盡量消除轉動不可交換誤差，再利用等效旋轉矢量計算姿態更新四元數，實現姿態更新。等效旋轉矢量多子樣算法的數學理論基礎是等效旋轉矢量微分方程(Bortz方程)[2]。由Bortz方程求解多子樣不可交換誤差補償系數的方法主要有兩類：一是在多項式角運動假設條件下，基于等效旋轉矢量泰勒級數展開法的不可交換誤差補償系數求解[3-5]；二是在純圓錐運動假設條件下求解的所謂優化圓錐誤差補償系數[6-7]，本文主要討論后者。

雖然優化圓錐誤差補償系數的求解已得到了比較圓滿的解決，理論上可獲得任意子樣系數的通式，但是它的推導過程略顯繁瑣[7]。本文放棄了傳統的采用解析推導方法求解優化圓錐誤差補償系數的思路，在圓錐運動條件下將角速度和角增量三角函數展開成泰勒級數形式，通過仔細分析圓錐誤差積分的多項式數值運算特點，給出了求解優化圓錐誤差補償系數及剩余漂移誤差的通用數值算法，算法簡潔，易于軟件編程實現。經過仿真計算，本文給出了所有共計21種1～6子樣優化圓錐誤差補償系數和漂移誤差。

1 算法推導

等效旋轉矢量微分方程(Bortz方程)[2]為

(1)

式中：t為時間參數；φ(t)、ω(t)分別表示等效旋轉矢量、角速度。

式(1)在理論上嚴格成立，但比較復雜，不便于工程使用，通行的做法是將右端第三項視為小量，并將第二項中的等效旋轉矢量近似為角增量[6-8]，從而近似有

(2)

式中Δθ(t)為角增量。對式(2)右端第二項進行積分得

(3)

通常稱δφ(t)為圓錐誤差積分。

以下在圓錐運動條件下由角增量多子樣采樣估計圓錐誤差積分，從而求解圓錐誤差補償系數。

首先，假設圓錐運動的角速度表達式為

(4)

式中：α為半錐角；Ω為圓錐運動角頻率。從后面推導過程中將會看到參數α和Ω均跟圓錐誤差補償系數的求解無關，所以為了書寫簡潔不妨把它們都作歸一化處理，將角速度ω(t)展開為關于時間t的無窮級數形式，得

(5)

由式(5)積分，可得角增量

(6)

式中：Wx、Wy、Ax和Ay均為行向量，分別有：

記姿態等效旋轉矢量更新時間為[0,T]，根據式(5)和式(6)計算圓錐誤差積分為

(7)

式中：運算符“*”表示兩多項式系數向量之間的卷積運算。注意到，Ax和Wy的零次項系數為0、Ay的零次及一次項系數均為0，因而(Ax*Wy-Ay*Wx)/2的零次及一次項系數也為0，將其再積分后，U的零次～二次項系數均為0；再者，由于Ax和Wy(Ay和Wx)的偶(奇)次項系數均為0，因而(Ax*Wy-Ay*Wx)/2的偶次項系數也為0，將其再積分后，U的奇次項系數必為0。由上述分析可見，式(7)的z軸分量有

(8)

式中：Ui(i=3,5,7,…)為行向量U的非零元素。

(9)

式中：有h=T/n和tj=jh，當tj>0時表示當前姿態更新周期內的角增量子樣采樣；而當tj≤0時表示利用了前面姿態更新周期的子樣信息。

計算任意2個子樣之間的叉乘積，可得

(10)

類似于式(8)對行向量U的分析，不難知道Vij的非零元素位置分布與U完全相同，則式(10)的z軸分量可寫成

(11)

最后，對比式(8)和式(11)的行向量系數，利用N-1個子樣叉乘積Δθi×Δθn(i=-p+1,-p+2,…,n-1)的線性組合對圓錐誤差積分進行估計，可得系數關系式

(12)

式中kin即為N-1個待求解的優化圓錐誤差補償系數。

若令式(12)中前N-1階非零低次項系數對應相等，可得N-1維線性方程為

(13)

如將式(13)簡記為

Z=BK，

(14)

則可立即求得優化圓錐誤差補償系數

K=B-1Z。

(15)

此外，若在式(13)成立的情況下將式(12)兩邊第2N+1次項系數的差異定義為N子樣剩余圓錐誤差漂移系數ρN，則有

(16)

考慮到實際圓錐運動參數半錐角α、角頻率Ω及姿態等效旋轉矢量更新周期T，漂移系數ρN與以rad/s表示的圓錐誤差補償算法漂移εN之間的關系[9]為

(17)

至此，完成算法推導。

2 仿真與分析

根據前一小節的公式推導，下面給出求解優化圓錐誤差補償系數的主要步驟：

1)根據式(5)和式(6)，設定角速度和角增量系數向量Wx、Wy、Ax和Ay；

2)根據式(7)計算圓錐誤差積分系數向量U；

3)按需求選擇子樣參數n和p，根據式(9)和式(10)計算角增量子樣Δθj及叉乘積系數向量Vij；

4)構造方程(13)，由式(15)～式(17)求解優化圓錐誤差補償系數kin、漂移系數ρN和算法漂移εN，完畢。

按照上述步驟，筆者編寫了Matlab仿真計算程序[10]，表1列出了當N=1～6時的所有共計21種子樣數的優化圓錐補償系數及算法漂移的仿真結果，其中在計算算法漂移εN時，假設半錐角α=1°、錐頻率Ω=2π rad/s且采樣間隔h=0.01 s。為了表示簡潔，使用Matlab的“format rat”命令將優化圓錐誤差補償系數kin和漂移系數ρN轉換成分數形式，算法漂移εN僅保留了3位有效數字。表1中序號右上標未標注“*”號的結果與已有文獻的結果完全一致[6-7]，而標注“*”號的結果是本文首次給出的。從表1算法漂移εN一列數據可以看出，在圓錐運動參數α、Ω和h相同的情況下，子樣數N=n+p越多則算法漂移εN越小；而在子樣數N一定的條件下，不論如何分配當前子樣數n和以往子樣數p， 都具有相同的算法漂移。顯然，如果不考慮計算量的話，選擇較大的N和較小的n， 有利于減小算法漂移并提高姿態更新輸出頻率。

表1 各子樣(N=1～6)優化圓錐誤差補償系數及算法漂移

3 結束語

傳統求解優化圓錐誤差補償系數的理論思路并不復雜，但公式推導過程稍顯繁瑣，本文通過三角函數的泰勒級數展開，采用卷積算法進行多項式向量的叉乘運算，給出求解優化圓錐誤差補償系數的方法和軟件編程都非常簡便，當然，讀者還可根據需要任意設置子樣數N(包括p和n)，運行程序后便可立即得到相應的系數，避免了繁瑣的公式推導過程。最后，文中給出了共計21種1～6子樣優化圓錐誤差補償系數及相應的算法漂移，為實際應用和算法選擇提供了更多的參考和便利。

[1] SAVAGE P G.Strapdown inertial navigation integration algorithm design part 1：attitude algorithms[J].Journal of Guidance，Control，and Dynamics，1998，21(1)：19-28.

[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] MILLER R.A new strapdown attitude algorithm[J].Journal of Guidance，Control，and Dynamics，1983，6(4)：287-291.

[4] LEE J G，YOON Y J，MARK J G，et al.Extension of strapdown attitude algorithm for high-frequency base motion[J].Journal of Guidance，Control，and Dynamics，1990，13(4)：738-743.

[5] 秦永元，張士邈.捷聯慣導姿態更新的四子樣旋轉矢量優化算法研究[J].中國慣性技術學報，2001，9(4)：1-7.

[6] IGNAGNI M B.Efficient class of optimized coning compensation algorithms[J].Journal of Guidance，Control，and Dynamics，1996，19(2)：424-429.

[7] PARK C G，KIM K J，LEE J G，et al.Formalized approach to obtaining optimal coefficients for coning algorithms[J].Journal of Guidance，Control，and Dynamics，1999，22(1)：165-168.

[8] 王立冬，孟亞峰，高慶.基于角增量和角速率的旋轉矢量算法的等效性[J].宇航學報，2014，35(3)：340-344.

[9] 嚴恭敏，嚴衛生，徐德民.經典圓錐誤差補償算法中剩余誤差估計的局限性研究[J].中國慣性技術學報，2008，16(4)：379-385.

[10]嚴恭敏.高精度捷聯慣性導航系統Matlab工具箱[EB/OL].(2013-09-16) [2017-4-12].http：//blog.sina.com.cn/s/blog_40edfdc90101heg0.html.

A general numerical method to obtaining optimized coning compensationcoefficients for strapdown attitude algorithm

YANGongmin，YANGXiaokang，WENGJun，QINYongyuan

(School of Automation，Northwestern Polytechnical University，Xi’an 710072，China)

In strapdown attitude updating algorithm，the traditional deduction process from equivalent rotation vector differential equation to obtaining multi-sample optimized coning compensation coefficients is generally much cumbersome.Under the condition of coning motion，the trigonometric functions of angular velocity and angular increment are developed with the Taylor series expansion.According to coning error integral and the characteristic of polynomial coefficient vector cross production operation，contained in the process of different multi-sample angular increment cross production，the cross production is converted into convolution operation of polynomial coefficients.A new method to obtain optimized coning compensation coefficients of arbitrary multi-sample is deducted，and it is easy to implement with computer programming.Finally，a total of 21 kinds of 1～6-sample optimized coning compensation coefficients and the corresponding algorithm drift error are proposed via numerical simulation.

strapdown attitude algorithm；optimized coning compensation algorithm；equivalent rotation vector；convolution operation；numerical solution

2017-02-18

(1977—)，男，福建建甌人，博士，副教授，研究方向為慣性導航與多源信息融合。

嚴恭敏，楊小康，翁浚，等.捷聯慣導中求解圓錐誤差系數的通用算法[J].導航定位學報，2017，5(3)：1-4,23.(YAN Gongmin，YANG Xiaokang，WENG Jun，et al.A general numerical method to obtaining optimized coning compensation coefficients for strapdown attitude algorithm[J].Journal of Navigation and Positioning，2017，5(3)：1-4,23.)

10.16547/j.cnki.10-1096.20170301.

V249.3

A

2095-4999(2017)03-0001-05