一種求解姿態(tài)不可交換誤差補償系數(shù)的通用方法

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

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

?

一種求解姿態(tài)不可交換誤差補償系數(shù)的通用方法

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

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

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

捷聯(lián)姿態(tài)更新算法；等效旋轉(zhuǎn)矢量；不可交換誤差；數(shù)值解

0 引 言

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

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

1 角運動的多項式表示

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

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

(1)

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

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

(2)

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

假設陀螺采樣間隔為h，在時間段(-ph,nh]內(nèi)進行了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時表示當前姿態(tài)更新周期內(nèi)的角增量采樣；而當tj≤0時表示利用了前面姿態(tài)更新周期的角增量信息。

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

Θ=WΓ

(4)

式中：記

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

W=ΘΓ-1

(5)

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

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

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

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

(6)

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

(7)

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

(8)

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

(9)

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

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

(10)

式(10)移項，可得

(11)

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

(12)

簡記為

ε=bK

(13)

(14)

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

K=(BTB)-1BTE

(15)

3 仿真與分析

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

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

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

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

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

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

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

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

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

表2 6子樣系數(shù)的對稱性

4 結 論

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

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

[2] 周江華,苗育紅,肖剛. 擴展旋轉(zhuǎn)矢量捷聯(lián)姿態(tài)算法[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] 王立冬，孟亞峰，高慶. 基于角增量和角速率的旋轉(zhuǎn)矢量算法的等效性[J]. 宇航學報, 2014, 35(3): 340-344.[WangLi-dong,MengYa-feng,GaoQing.Equivalenceanalysisofrotationvectoralgorithmbasedonangleincrementandangularvelocity[J].JournalofAstronautics, 2014, 35(3): 340-344.]

[10] 張澤,段廣仁. 捷聯(lián)慣導四子樣旋轉(zhuǎn)矢量姿態(tài)更新算法[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] 嚴恭敏. 高精度捷聯(lián)慣性導航系統(tǒng)Matlab工具箱[EB/OL]. (2013-09-16) [2017-2-20].http://blog.sina.com.cn/s/blog_40edfdc90101heg0.html.

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

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

通信地址：陜西省西安市西北工業(yè)大學自動化學院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-)，男，博士，副教授，主要從事慣性導航與信息融合理論研究。