一種無誤差的捷聯(lián)慣導(dǎo)數(shù)值更新新算法

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

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

0 引言

自二十世紀(jì)六七十年代開始，捷聯(lián)慣導(dǎo)的傳統(tǒng)算法經(jīng)過了幾十年的發(fā)展，其主流設(shè)計方法及細(xì)節(jié)已經(jīng)完成[1-6]。姿態(tài)更新是整個捷聯(lián)算法的核心，為了補償姿態(tài)不可交換誤差，基于等效旋轉(zhuǎn)矢量概念提出了多子樣及其優(yōu)化算法；利用對偶性原理，可以直接將姿態(tài)多子樣算法應(yīng)用于速度更新算法[7]；與前兩者相比，位置更新算法的算法誤差影響相對較小些，在許多應(yīng)用中一般可以不予考慮。

然而，傳統(tǒng)的不可交換誤差補償多子樣算法是在等效旋轉(zhuǎn)矢量方程(Bortz方程[3])進行二階近似的基礎(chǔ)上推導(dǎo)的，它本質(zhì)上存在原理性誤差，特別在大機動環(huán)境下計算誤差不容忽略，有時高子樣算法的誤差反而比低子樣的大[8]。即使考慮得更加周全，保留Bortz方程的高階近似[9]，也還依然存在原理性誤差，且高階補償算法的推導(dǎo)過程會變得非常復(fù)雜。最近，一些學(xué)者研究了無原理性算法誤差的捷聯(lián)姿態(tài)更新方法，徹底解決了大機動環(huán)境下的姿態(tài)更新問題[10-11]。

本文根據(jù)捷聯(lián)慣導(dǎo)的姿態(tài)陣、速度和位置微分方程，直接采用泰勒級數(shù)展開方法，給出了全套捷聯(lián)慣導(dǎo)更新數(shù)值算法，在陀螺輸出角速度和加速度計輸出比力滿足多項式形式假設(shè)的條件下(根據(jù)數(shù)學(xué)知識，平滑的角運動或線運動總是可以用多項式無限逼近的)，該算法不存在任何原理性誤差，隱含了對姿態(tài)、速度和位置更新不可交換誤差的完美補償。通常在地球表面上實施的慣性導(dǎo)航需同時考慮地球自轉(zhuǎn)和有害加速度的補償，相對于大機動而言這些補償量均為緩變量，計算誤差很小。為了理論研究和對比分析方便，暫且以慣性坐標(biāo)系作為導(dǎo)航參考系，給出了簡潔的捷聯(lián)慣導(dǎo)數(shù)值更新算法，最后，通過仿真實驗驗證了算法不存在原理性誤差，具有很高的計算精度。

1 由角增量(速度增量)擬合角速度(比力)的方法

捷聯(lián)慣導(dǎo)系統(tǒng)中的慣性器件通常是角增量(或速度增量)輸出形式的，本文的算法要以角速度(或比力)作為輸入，為此，先給出由角增量(速度增量)轉(zhuǎn)換為角速度(比力)的計算方法。

由文獻[12]知，若陀螺角增量的采樣間隔為h，在時間段(-ph,nh]內(nèi)進行了N次采樣(p,n均為整數(shù)，p≥0,n>0且p+n=N)，角增量分別記為Δθj(j=-p+1,-p+2,…,n)，則可得角速度ω的關(guān)于時間t的(N-1)次多項式擬合為

(1)

式中：

同理，可得比力f的多項式擬合為

(2)

2 捷聯(lián)慣導(dǎo)更新新算法的推導(dǎo)

首先給出兩矩陣之乘積的求導(dǎo)公式，即若Z=XY，則有

(3)

接下來根據(jù)捷聯(lián)慣導(dǎo)姿態(tài)陣、速度和位置微分方程分別給出相應(yīng)的數(shù)值更新算法。

1)在捷聯(lián)慣導(dǎo)算法中，姿態(tài)陣微分方程為

(4)

式中：C為姿態(tài)矩陣；(ω×)表示由向量ω構(gòu)成的反對稱陣。

假設(shè)姿態(tài)更新時間間隔為(0,T]且T=nh，將T時刻的姿態(tài)陣CT在0時刻展開成泰勒級數(shù)形式，并將式(4)代入，可得

(5)

根據(jù)式(3)和式(4)，有

(6)

2)捷聯(lián)慣導(dǎo)速度微分方程為

(7)

將T時刻的速度VT展開成泰勒級數(shù)形式為

(8)

根據(jù)式(3)和式(7)有

(9)

式(8)右端的第三項(求和項)也隱含了對速度不可交換誤差(劃船誤差)的精確補償。

3)捷聯(lián)慣導(dǎo)位置微分方程為

(10)

將T時刻的位置PT展開成泰勒級數(shù)形式為

(11)

式(11)右端的第三項(求和項)隱含了對位置不可交換誤差(渦卷誤差)的精確補償。

至此獲得完整的包含姿態(tài)陣更新、速度更新和位置更新的全套捷聯(lián)慣導(dǎo)數(shù)值更新算法，其計算流程可總結(jié)如圖1所示。算法以0時刻的姿態(tài)陣C0、速度V0和位置P0，以及陀螺角增量Δθj和加速度計速度增量ΔVj為輸入，經(jīng)過迭代求導(dǎo)與累加求和后，輸出T時刻的姿態(tài)陣CT、速度VT和位置PT。當(dāng)角速度ω和比力f可以用多項式描述且泰勒級數(shù)展開項數(shù)足夠多時，理論上算法誤差僅取決于計算機的數(shù)值計算精度，全套算法不存在任何原理性誤差。

圖1 算法更新流程

3 仿真驗證

假設(shè)飛機做類似于“普加喬夫眼鏡蛇機動”的飛行動作[13]，捷聯(lián)慣導(dǎo)輸出的角速度ω和比力f可用二次多項式描述，在載體坐標(biāo)系下按“x(右)—y(前)—z(上)”順序表示的多項式系數(shù)矩陣分別為

(12)

(13)

式中：角速度ω的單位為rad/s，比力f的單位為m/s2。顯然，飛機只在x軸方向上有角速度機動，而在y軸和z軸方向上均有加速度機動，飛機的飛行軌跡始終在同一鉛垂面上。

設(shè)置慣性傳感器采樣頻率100 Hz，仿真時長1 s，飛機初始姿態(tài)為0，速度300 m/s。經(jīng)過傳感器數(shù)據(jù)生成和捷聯(lián)慣導(dǎo)更新解算仿真，本文新算法與傳統(tǒng)多子樣算法相比，在1 s時導(dǎo)航速度誤差如表1所列。

表1 算法誤差比較 m/s

由表1可見，由于同時存在強烈的角速度和加速度機動耦合，傳統(tǒng)算法的速度誤差較大，高子樣算法的誤差反而大于低子樣算法，其中4子樣算法的z軸速度誤差超過了0.1 m/s。然而對于新算法而言，其速度誤差非常小，幾乎可以忽略不計，近似為0。

4 結(jié)束語

由于精確的Bortz方程非常復(fù)雜，不便于直接應(yīng)用，在傳統(tǒng)的姿態(tài)不可交換誤差補償多子樣算法推導(dǎo)過程中，必須對Bortz方程進行簡化，忽略高階項的影響，從而使得傳統(tǒng)的多子樣補償算法總是近似成立的，特別在大機動環(huán)境下容易引起較大的原理性誤差。本文放棄了求解一組固定的不可交換誤差補償系數(shù)的做法，直接采用泰勒級數(shù)展開式求解捷聯(lián)慣導(dǎo)姿態(tài)陣、速度和位置的數(shù)值更新算法，從推導(dǎo)過程中可以看出，當(dāng)陀螺角速度和加速度計比力能用多項式形式描述時，新算法不存在任何原理性誤差，仿真實驗結(jié)果也表明了新算法是無誤差的。就目前的計算機運算能力而言，類似于文獻[10]的分析，新算法的計算量是可以接受的。當(dāng)然，在本文理論推導(dǎo)的基礎(chǔ)上，讀者通過一些簡單的修改即可擴展成指北方位捷聯(lián)慣導(dǎo)系統(tǒng)的數(shù)值更新算法，實現(xiàn)高精度的導(dǎo)航解算。

[1] SAVAGE P G.A new second-order solution for strapped-down attitude computation[EB/OL].[2017-10-28].http：//www.ijicic.org/ijicic-12-08014.pdf.

[2] JORDAN J W.An accurate strapdown direction cosine algorithm[EB/OL].[2017-10-28].https：//www.researchgate.net/publication/24331776_An_accurate_strapdown_direction_cosine_algorithm.NASA TN-D-5384，1969，9.

[3] BORTZ J E.A new mathematical formulation for strapdown inertial navigation[J].IEEE Transactions on Aerospace and Electronic Systems，1971，7(1)：61-66.

[4] MILLER R.A new strapdown attitude algorithm[J].Journal of Guidance，Control，and Dynamics，1983，6(4)：287-291.

[5] 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.

[6] 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.

[7] ROSCOE K M.Equivalency between strapdown inertial navigation coning and sculling integrals/algorithms[J]. Journal of Guidance，Control，and Dynamics，2001，24(2)：201-205.

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

[9] WANG M S，WU W Q，WANG J L，et al.High-order attitude compensation in coning and rotation coexisting environment[J].IEEE Transactions on Aerospace and Electronic Systems，2015，51(2)：1178-1190.

[10] 嚴(yán)恭敏，翁浚，楊小康，等.基于畢卡迭代的捷聯(lián)姿態(tài)更新精確數(shù)值解法[J].宇航學(xué)報，2017，38(12)：1307-1313.

[11] 武元新，郁文賢.一種基于函數(shù)迭代積分的剛體姿態(tài)解算方法：201710273489.3[P].2017-11-10.

[12] 嚴(yán)恭敏，楊小康，翁浚，等.一種求解姿態(tài)不可交換誤差補償系數(shù)的通用方法[J].宇航學(xué)報，2017，38(7)：723-727.

[13] 李中華.Su—27飛機眼鏡蛇機動及其戰(zhàn)術(shù)意義[J].飛行力學(xué)，2000，18(1)：54-57.