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

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

捷聯(lián)慣導(dǎo)中求解圓錐誤差系數(shù)的通用算法

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

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

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

捷聯(lián)姿態(tài)更新算法；優(yōu)化圓錐誤差補(bǔ)償算法；等效旋轉(zhuǎn)矢量；卷積運(yùn)算；數(shù)值解

0 引言

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

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

1 算法推導(dǎo)

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

(1)

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

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

(2)

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

(3)

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

以下在圓錐運(yùn)動條件下由角增量多子樣采樣估計(jì)圓錐誤差積分，從而求解圓錐誤差補(bǔ)償系數(shù)。

首先，假設(shè)圓錐運(yùn)動的角速度表達(dá)式為

(4)

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

(5)

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

(6)

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

記姿態(tài)等效旋轉(zhuǎn)矢量更新時(shí)間為[0,T]，根據(jù)式(5)和式(6)計(jì)算圓錐誤差積分為

(7)

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

(8)

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

(9)

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

計(jì)算任意2個(gè)子樣之間的叉乘積，可得

(10)

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

(11)

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

(12)

式中kin即為N-1個(gè)待求解的優(yōu)化圓錐誤差補(bǔ)償系數(shù)。

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

(13)

如將式(13)簡記為

Z=BK，

(14)

則可立即求得優(yōu)化圓錐誤差補(bǔ)償系數(shù)

K=B-1Z。

(15)

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

(16)

考慮到實(shí)際圓錐運(yùn)動參數(shù)半錐角α、角頻率Ω及姿態(tài)等效旋轉(zhuǎn)矢量更新周期T，漂移系數(shù)ρN與以rad/s表示的圓錐誤差補(bǔ)償算法漂移εN之間的關(guān)系[9]為

(17)

至此，完成算法推導(dǎo)。

2 仿真與分析

根據(jù)前一小節(jié)的公式推導(dǎo)，下面給出求解優(yōu)化圓錐誤差補(bǔ)償系數(shù)的主要步驟：

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

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

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

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

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

表1 各子樣(N=1～6)優(yōu)化圓錐誤差補(bǔ)償系數(shù)及算法漂移

3 結(jié)束語

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

[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] 秦永元，張士邈.捷聯(lián)慣導(dǎo)姿態(tài)更新的四子樣旋轉(zhuǎn)矢量優(yōu)化算法研究[J].中國慣性技術(shù)學(xué)報(bào)，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] 王立冬，孟亞峰，高慶.基于角增量和角速率的旋轉(zhuǎn)矢量算法的等效性[J].宇航學(xué)報(bào)，2014，35(3)：340-344.

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

[10]嚴(yán)恭敏.高精度捷聯(lián)慣性導(dǎo)航系統(tǒng)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—)，男，福建建甌人，博士，副教授，研究方向?yàn)閼T性導(dǎo)航與多源信息融合。

嚴(yán)恭敏，楊小康，翁浚，等.捷聯(lián)慣導(dǎo)中求解圓錐誤差系數(shù)的通用算法[J].導(dǎo)航定位學(xué)報(bào)，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