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

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

0 引言

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

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

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

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

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

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

(1)

式中：

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

(2)

2 捷聯慣導更新新算法的推導

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

(3)

接下來根據捷聯慣導姿態(tài)陣、速度和位置微分方程分別給出相應的數值更新算法。

1)在捷聯慣導算法中，姿態(tài)陣微分方程為

(4)

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

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

(5)

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

(6)

2)捷聯慣導速度微分方程為

(7)

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

(8)

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

(9)

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

3)捷聯慣導位置微分方程為

(10)

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

(11)

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

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

圖1 算法更新流程

3 仿真驗證

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

(12)

(13)

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

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

表1 算法誤差比較 m/s

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

4 結束語

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

[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] 嚴恭敏，嚴衛(wèi)生，徐德民.經典圓錐誤差補償算法中剩余誤差估計的局限性研究[J].中國慣性技術學報，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] 嚴恭敏，翁浚，楊小康，等.基于畢卡迭代的捷聯姿態(tài)更新精確數值解法[J].宇航學報，2017，38(12)：1307-1313.

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

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

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