一種求解姿態不可交換誤差補償系數的通用方法

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

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

?

一種求解姿態不可交換誤差補償系數的通用方法

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

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

針對捷聯慣導姿態更新算法中的不可交換誤差補償系數求解問題，論文在多項式角運動條件下建立了角速度多項式、角增量多項式與多子樣角增量采樣之間的線性關系，根據等效旋轉矢量微分方程中不可交換誤差多項式的向量叉乘特點，將叉乘轉化為多項式系數的卷積運算，推導給出了計算任意子樣數不可交換誤差補償系數的數值方法，新方法易于軟件編程實現。最后，通過仿真計算給出了2～6子樣誤差補償系數，其中2～4子樣結果與已有文獻完全相同，而5、6子樣為首次給出。

捷聯姿態更新算法；等效旋轉矢量；不可交換誤差；數值解

0 引 言

目前，捷聯慣導系統的姿態更新算法中普遍采取的思路是[1-2]：根據不可交換誤差補償算法，使用陀螺角增量的多子樣采樣構造等效旋轉矢量，盡量消除轉動不可交換誤差，再利用等效旋轉矢量計算姿態更新四元數，實現姿態更新。等效旋轉矢量多子樣算法的數學理論基礎是等效旋轉矢量微分方程(Bortz方程)[3]。由Bortz方程求解多子樣不可交換誤差補償系數的方法主要有兩類：一是在純圓錐運動假設條件下求解的所謂優化圓錐誤差補償系數，該問題業已得到了比較圓滿的解決，理論上獲得了求解任意子樣系數的通式[4-6]；二是在多項式角運動假設條件下，基于等效旋轉矢量泰勒級數展開法的不可交換誤差補償系數求解，雖然研究者們求得了一系列不同子樣數的誤差補償系數[7-10]，甚至有研究者考慮了Bortz方程不可交換誤差高階項的影響，提出了高階補償算法[11]，但是目前還沒有得到這些方法的求解通式，每當遇到一種新的子樣數算法就得重新推導補償系數，特別在高子樣數情況下，推導過程異常繁瑣。

在多項式角運動條件下，論文放棄了傳統的冗長而繁瑣的公式推導思路，而通過仔細分析基于等效旋轉矢量泰勒級數展開法求解不可交換誤差補償系數的特點，給出了求解補償系數的通用數值算法，新算法簡潔，易于軟件編程實現。論文求解不可交換誤差系數的方法與傳統方法相比，基本理論是一樣的，都是基于Bortz方程的一階近似，在圓錐誤差積分中以角增量代替等效旋轉矢量，通過角速度的多項式假設和等效旋轉矢量的泰勒級數展開，比較級數低階項與角增量叉乘的系數，聯立起來建立方程組，從而求得誤差補償系數；但是論文提出的數值求解方法，在建立方程組和求解系數時都非常方便，省去了傳統采用公式直接推導的繁瑣，具有很好的通用性。論文通過仿真計算，首次正確給出了5子樣和6子樣誤差補償系數。

1 角運動的多項式表示

在實際捷聯慣導系統中，大多數陀螺采樣直接獲得的是角增量信息，而姿態等效旋轉矢量更新算法的推導過程需要用到角速度作為輸入，因此，在角運動為多項式形式假設條件下先給出由角增量信息構造角速度的方法。

假設角速度ω(t)為關于時間t的(N-1)次多項式，即

(1)

式中：wj(j=N-1,N-2,…,0)為列向量，Wi(i=x,y,z)為行向量，W為3×N系數矩陣。

對式(1)積分，可得相對于0時刻的總角增量為

(2)

式中：aj為列向量且有aj+1=wj/(j+1)，Ai為行向量，A為3×N系數矩陣。

假設陀螺采樣間隔為h，在時間段(-ph,nh]內進行了N次角增量采樣(p≥0,n>0且p+n=N)，分別記為Δθj(j=-p+1,-p+2,…,n)，對角速度積分可得角增量采樣，如下

wN-2+…+(tj-tj-1)w0

(3)

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

根據式(3)，將相繼N次角增量采樣合并在一起寫成矩陣形式，有

Θ=WΓ

(4)

式中：記

由式(4)容易求解得到以角增量表示的多項式系數矩陣

W=ΘΓ-1

(5)

由此可見，根據相繼的N次角增量采樣，通過式(5)和式(1)可以構造一個N-1次的多項式角速度擬合；顯然，角速度系數wj及角增量系數aj都是所有角增量采樣Δθj的線性函數。

2 不可交換誤差與補償系數之間關系推導

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

[φ(t)×]2ω(t)

(6)

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

(7)

若將式(1)和式(2)的列向量系數多項式表示法代入式(7)，可得

(8)

由式(1)和式(2)的行向量系數多項式表示法代入式(7)，考慮到叉乘運算規則，可得

(9)

式中：運算符“*”表示兩多項式系數向量之間的卷積運算。與式(8)相比，利用式(9)卷積計算系數矩陣U較方便。

記T=nh為等效旋轉矢量更新周期，將φ(T)在時間t=0處展開成泰勒級數，考慮到φ(0)=Δθ(0)=0及式(7)、式(8)，則有

(10)

式(10)移項，可得

(11)

由第1節分析可知，wj和aj均為角增量采樣Δθj的線性函數；由式(8)可知，um可表示為ai和wj叉乘和，因此um可以表示為各個角增量Δθj之間的叉乘和形式，從而有

(12)

簡記為

ε=bK

(13)

(14)

從式(14)可求得誤差補償系數向量K的最小二乘解

K=(BTB)-1BTE

(15)

3 仿真與分析

綜合第1～2節的公式推導，下面給出求解不可交換誤差補償系數的詳細步驟：

1)按需求給定子樣數N(包括p和n的值)，不妨將等效旋轉矢量更新周期作歸一化處理，即令T=1和h=1/n，預先計算式(4)中的系數矩陣Γ和Γ-1備用；

3)根據式(9)卷積計算系數矩陣U；

4)根據式(12)或式(13)計算不可交換誤差ε和角增量叉乘矩陣b，確定一個量測方程；

5)重復步驟2～4，建立一組如式(14)的量測方程組，一般可選擇l=N；

6)按式(15)求解不可交換誤差補償系數向量K，完畢。

按照上述步驟，編寫了MatlabPSINS工具箱仿真計算程序[12]，表1列出了子樣數N=2～6(p=0)的仿真結果，為簡潔，誤差補償系數kij僅保留了6位小數，完全可滿足實際應用對算法精度的要求。若不計數值表示的舍入誤差，表1中2～4子樣結果與文獻[4,7-10]的結果完全一致(文獻中通常以分數形式給出)。傳統基于等效旋轉矢量泰勒級數展開法求解誤差補償系數的理論思路并不復雜，但公式推導過程比較繁瑣，文獻[13]給出了5子樣的推導過程。然而與本文相比,文獻[13]中式(20)明顯是錯的，原因在于文獻[13]中式(10)的系數推導失誤，1/12應為1/14。本文求解誤差補償系數的方法和軟件編程更簡便，表1列出了6子樣誤差補償系數，讀者還可根據需要任意設置子樣數N(包括p和n)，運行程序后便可立即得到相應的系數，避免了繁瑣的公式推導過程。

表1 2～6子樣不可交換誤差補償系數

從表1不難看出，對于任意一子樣數N(p=0)，不可交換誤差補償系數存在對稱性，即有ki,j=kN-j+1,N-i+1，以6子樣為例，將表1中的6子樣系數重新整理成表2形式，可見誤差系數關于副對角線是對稱的。

表2 6子樣系數的對稱性

4 結 論

在傳統的捷聯慣導姿態不可交換誤差補償算法中，不論是基于純圓錐運動假設的優化算法還是基于多項式角運動的一般補償算法，都是在一定近似假設條件下進行推導的，比如前者假設錐角為小角度而后者忽略了Bortz方程中高階項的影響，從而使得結果也是近似的，特別對于高子樣算法而言，其誤差補償往往達不到理論宣稱的效果，有時高子樣算法的精度反而不如低子樣算法[14]。顯然，片面追求高子樣數的算法是不合適的，不一定能帶來實用效果的明顯改善。但是，從理論研究角度看，本文給出了求解各種子樣數誤差補償系數的通用方法，為實際應用和算法選擇提供了更多的參考和便利。

[1]SavagePG.Strapdowninertialnavigationintegrationalgorithmdesignpart1:attitudeagorithms[J].JournalofGuidance,Control,andDynamics, 1998,21(1): 19-28.

[2] 周江華,苗育紅,肖剛. 擴展旋轉矢量捷聯姿態算法[J]. 宇航學報, 2003, 24(4): 414-417.[ZhouJiang-hua,MiaoYu-hong,XiaoGang.Extendedrotationvectorbasedstrapdownattitudealgorithms[J].JournalofAstronautics, 2003, 24(4): 414-417.]

[3]BortzJE.Anewmathematicalformulationforstrapdowninertialnavigation[J].IEEETransactionsonAerospaceandElectronicSystems, 1971, 7(1): 61-66.

[4]IgnagniMB.Efficientclassofoptimizedconingcompensationalgorithms[J].JournalofGuidance,Control,andDynamics, 1996, 19(2): 424-429.

[5]ParkCG,KimKJ,LeeJG,etal.Formalizedapproachtoobtainingoptimalcoefficientsforconingalgorithms[J].JournalofGuidance,Control,andDynamics, 1999, 22(1): 165-168.

[6]TangCY,ChenXY.Ageneralizedconingcorrectionstructureforattitudealgorithms[J].MathematicalProblemsinEngineering, 2014:1-15.

[7]MillerR.Anewstrapdownattitudealgorithm[J].JournalofGuidance,Control,andDynamics, 1983, 6(4): 287-291.

[8]LeeJG,YoonYJ,MarkJG,etal.Extensionofstrapdownattitudealgorithmforhigh-frequencybasemotion[J].JournalofGuidance,Control,andDynamics, 1990, 13(4): 738-743.

[9] 王立冬，孟亞峰，高慶. 基于角增量和角速率的旋轉矢量算法的等效性[J]. 宇航學報, 2014, 35(3): 340-344.[WangLi-dong,MengYa-feng,GaoQing.Equivalenceanalysisofrotationvectoralgorithmbasedonangleincrementandangularvelocity[J].JournalofAstronautics, 2014, 35(3): 340-344.]

[10] 張澤,段廣仁. 捷聯慣導四子樣旋轉矢量姿態更新算法[J]. 控制工程, 2010, 17(3): 272-274.[ZhangZe,DuanGuang-ren.Onfour-samplerotationvectoralgorithmforSINSattitudeupdating[J].ControlEngineeringofChina, 2010, 17(3): 272-274.]

[11]WangMS,WuWQ,WangJL,etal.High-orderattitudecompensationinconingandrotationcoexistingenvironment[J].IEEETransactionsonAerospaceandElectronicSystems, 2015,51(2):1178-1190.

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

[13] 靳聰,趙修斌,許云達,等. 捷聯姿態解算五子樣等效旋轉矢量算法研究[J]. 科學技術與工程, 2014, 14(29): 96-99.[JinCong,ZhaoXiu-bin,XuYun-da,etal.Researchonfive-samplealgorithmforattitudeupdatingofSINS[J].ScienceTechnologyandEngineering, 2014, 14(29): 96-99.]

[14] 嚴恭敏,嚴衛生,徐德民. 經典圓錐誤差補償算法中剩余誤差估計的局限性研究[J]. 中國慣性技術學報, 2008, 16(4): 379-385.[YanGong-min,YanWei-sheng,XuDe-min.Limitationsoferrorestimationforclassicconingcompensationalgorithm[J].JournalofChineseInertialTechnology, 2008, 16(4): 379-385.]

通信地址：陜西省西安市西北工業大學自動化學院183號信箱(710072)

電話：(029)88431369

E-mail:yangongmin@163.com

A General Method to Obtain Noncommutativity Error Compensation Coefficients for Strapdown Attitude Algorithm

YAN Gong-min, YANG Xiao-kang, WENG Jun, QIN Yong-yuan

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

In a strapdown attitude updating algorithm, a general numerical method for obtaining the multi-sample noncommutativity error compensation coefficients is presented. Under the condition of the angular motion expressed as polynomial, the linear relationships between the angular velocity polynomial, angular increment polynomial and multi-sample angular increments are established in this paper. According to the vector cross product operation of the noncommutativity error polynomial in the equivalent rotation vector differential equation, the cross product is converted into the convolution operation of the polynomial coefficients. Then, the numerical methods to compute the noncommutativity error compensation coefficients of the arbitrary multiple samples are presented in deduction, which are easy to be implemented with computer programming. Finally, simulations are carried out to obtain the 2～6-sample compensation coefficients, in which the 2～4-sample coefficients are consistent with the existing literatures and the 5 or 6-sample coefficients are firstly proposed by the authors.

Strapdown attitude algorithm; Equivalent rotation vector; Noncommutativity error; Numerical solution

2017-03-14;

2017-05-15

航空科學基金(20165853041)

V249.3

A

1000-1328(2017)07-0723-05

10.3873/j.issn.1000-1328.2017.07.000

嚴恭敏(1977-)，男，博士，副教授，主要從事慣性導航與信息融合理論研究。