嚴(yán)恭敏，楊小康，翁 浚，秦永元

(西北工業(yè)大學(xué)自動(dòng)化學(xué)院，西安 710072)

?

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

嚴(yán)恭敏，楊小康，翁 浚，秦永元

(西北工業(yè)大學(xué)自動(dòng)化學(xué)院，西安 710072)

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

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

0 引 言

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

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

1 角運(yùn)動(dòng)的多項(xiàng)式表示

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

假設(shè)角速度ω(t)為關(guān)于時(shí)間t的(N-1)次多項(xiàng)式，即

(1)

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

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

(2)

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

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

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

(3)

式中：簡(jiǎn)記tj=jh，當(dāng)tj>0時(shí)表示當(dāng)前姿態(tài)更新周期內(nèi)的角增量采樣；而當(dāng)tj≤0時(shí)表示利用了前面姿態(tài)更新周期的角增量信息。

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

Θ=WΓ

(4)

式中：記

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

W=ΘΓ-1

(5)

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

2 不可交換誤差與補(bǔ)償系數(shù)之間關(guān)系推導(dǎo)

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

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

(6)

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

(7)

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

(8)

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

(9)

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

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

(10)

式(10)移項(xiàng)，可得

(11)

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

(12)

簡(jiǎn)記為

ε=bK

(13)

(14)

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

K=(BTB)-1BTE

(15)

3 仿真與分析

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

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

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

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

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

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

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

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

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

表2 6子樣系數(shù)的對(duì)稱(chēng)性

4 結(jié) 論

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

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

[2] 周江華,苗育紅,肖剛. 擴(kuò)展旋轉(zhuǎn)矢量捷聯(lián)姿態(tài)算法[J]. 宇航學(xué)報(bào), 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]. 宇航學(xué)報(bào), 2014, 35(3): 340-344.[WangLi-dong,MengYa-feng,GaoQing.Equivalenceanalysisofrotationvectoralgorithmbasedonangleincrementandangularvelocity[J].JournalofAstronautics, 2014, 35(3): 340-344.]

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

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

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

通信地址：陜西省西安市西北工業(yè)大學(xué)自動(dòng)化學(xué)院183號(hào)信箱(710072)

電話(huà)：(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

航空科學(xué)基金(20165853041)

V249.3

A

1000-1328(2017)07-0723-05

10.3873/j.issn.1000-1328.2017.07.000

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