一種地理系下基于偽線(xiàn)性模型的捷聯(lián)慣導(dǎo)算法

楊小康，嚴(yán)恭敏，李思錦，李四海，劉明雍

(1. 西北工業(yè)大學(xué)自動(dòng)化學(xué)院，西安 710129; 2. 陜西省飛行控制與仿真技術(shù)重點(diǎn)實(shí)驗(yàn)室，西安 710129;3. 西安微電子技術(shù)研究所，西安 710065; 4. 西北工業(yè)大學(xué)航海學(xué)院，西安 710072)

0 引 言

捷聯(lián)式慣導(dǎo)系統(tǒng)與平臺(tái)式不同，需要利用陀螺儀數(shù)據(jù)完成姿態(tài)解算，構(gòu)建虛擬的數(shù)學(xué)平臺(tái)，再利用加速度計(jì)數(shù)據(jù)實(shí)現(xiàn)速度和位置解算。因此捷聯(lián)慣導(dǎo)系統(tǒng)的導(dǎo)航解算算法相較于平臺(tái)系統(tǒng)更加復(fù)雜，也更加重要，捷聯(lián)慣導(dǎo)算法研究也一直是捷聯(lián)慣導(dǎo)系統(tǒng)的研究重點(diǎn)。目前受制于慣導(dǎo)器件精度，慣導(dǎo)算法研究的主要方向是組合導(dǎo)航、冗余慣導(dǎo)系統(tǒng)、長(zhǎng)航時(shí)誤差抑制等。但是原子陀螺慣導(dǎo)系統(tǒng)、空天飛機(jī)、高超音速飛行器等新的應(yīng)用場(chǎng)景對(duì)捷聯(lián)慣導(dǎo)解算算法提出了新的挑戰(zhàn)，需要針對(duì)捷聯(lián)慣導(dǎo)解算的算法結(jié)構(gòu)或原理進(jìn)行改進(jìn)。在傳統(tǒng)捷聯(lián)慣導(dǎo)算法發(fā)展過(guò)程中，隨著對(duì)算法誤差機(jī)理研究的深入，算法精度不斷提高。文獻(xiàn)[6]利用等效旋轉(zhuǎn)矢量完成姿態(tài)解算，建立了捷聯(lián)慣導(dǎo)算法的基礎(chǔ)。在此基礎(chǔ)上，文獻(xiàn)[7-8]又進(jìn)一步提出了多子樣捷聯(lián)慣導(dǎo)算法，通過(guò)補(bǔ)償圓錐誤差和劃槳誤差提升算法精度。

由于構(gòu)成捷聯(lián)慣導(dǎo)系統(tǒng)的陀螺儀和加速度計(jì)精度提升，加之大機(jī)動(dòng)高動(dòng)態(tài)條件下導(dǎo)航需求的出現(xiàn)，近幾年的捷聯(lián)慣導(dǎo)算法研究重點(diǎn)是提升捷聯(lián)慣導(dǎo)算法精度。文獻(xiàn)[9]通過(guò)建立姿態(tài)更新不可交換誤差的高階誤差模型提出了基于角增量四次叉乘補(bǔ)償項(xiàng)的高精度捷聯(lián)慣導(dǎo)算法。文獻(xiàn)[10]提出使用數(shù)值仿真算法求解不可交換誤差高階補(bǔ)償系數(shù)，實(shí)現(xiàn)此類(lèi)算法中補(bǔ)償系數(shù)的快速計(jì)算。文獻(xiàn)[11-12]則是基于函數(shù)迭代方法，構(gòu)建角速度和比力多項(xiàng)式并完成高精度導(dǎo)航解算。

無(wú)論是傳統(tǒng)的多子樣捷聯(lián)慣導(dǎo)算法還是新提出的幾種高精度捷聯(lián)慣導(dǎo)算法，都是利用姿態(tài)陣、四元數(shù)、羅德里格參數(shù)等進(jìn)行姿態(tài)解算，再根據(jù)對(duì)應(yīng)的坐標(biāo)轉(zhuǎn)換計(jì)算規(guī)則，將載體坐標(biāo)系下的加速度計(jì)輸出轉(zhuǎn)換到導(dǎo)航坐標(biāo)系下。因?yàn)橐陨媳硎咀藨B(tài)的變量并不是在線(xiàn)性空間中定義的，所以速度更新方程是非線(xiàn)性的，不便于直接利用捷聯(lián)慣導(dǎo)系統(tǒng)模型進(jìn)行系統(tǒng)分析、濾波等操作。文獻(xiàn)[13]介紹了偽線(xiàn)性系統(tǒng)相關(guān)算法基礎(chǔ)，偽線(xiàn)性系統(tǒng)是指形似線(xiàn)性系統(tǒng)實(shí)則非線(xiàn)性系統(tǒng)的系統(tǒng)。文獻(xiàn)[14-15]介紹了偽線(xiàn)性系統(tǒng)方法在衛(wèi)星姿態(tài)控制、飛行器制導(dǎo)與控制中的應(yīng)用。如果構(gòu)建出捷聯(lián)慣導(dǎo)系統(tǒng)的偽線(xiàn)性系統(tǒng)模型，就可以直接使用線(xiàn)性系統(tǒng)理論完成捷聯(lián)慣導(dǎo)更新，直接使用捷聯(lián)慣導(dǎo)系統(tǒng)模型構(gòu)建線(xiàn)性Kalman濾波器實(shí)現(xiàn)初始對(duì)準(zhǔn)、組合導(dǎo)航等，無(wú)需考慮模型的非線(xiàn)性問(wèn)題。按照此研究路線(xiàn)，文獻(xiàn)[16]提出了基于時(shí)間序列的慣性系下偽線(xiàn)性捷聯(lián)慣導(dǎo)算法，建立了完整的偽線(xiàn)性捷聯(lián)慣導(dǎo)算法，但是此算法建立在慣性系下，不能滿(mǎn)足通常的導(dǎo)航系下定位需求，因此也不能直接將此模型應(yīng)用于初始對(duì)準(zhǔn)和組合導(dǎo)航中。

本文在偽線(xiàn)性模型相關(guān)理論和文獻(xiàn)[16]中偽線(xiàn)性捷聯(lián)慣導(dǎo)算法基礎(chǔ)上，提出地理系下偽線(xiàn)性高精度捷聯(lián)慣導(dǎo)更新算法。與傳統(tǒng)的捷聯(lián)慣導(dǎo)算法相比，偽線(xiàn)性的算法不再將一個(gè)完整的慣導(dǎo)更新過(guò)程分為姿態(tài)更新、速度更新和位置更新三部分，而是使用新構(gòu)建的導(dǎo)航向量統(tǒng)一表示姿態(tài)、速度和位置信息，并且根據(jù)導(dǎo)航向量微分方程設(shè)計(jì)離散的導(dǎo)航向量更新算法。應(yīng)用偽線(xiàn)性捷聯(lián)慣導(dǎo)算法的慣導(dǎo)系統(tǒng)結(jié)構(gòu)如圖1所示，原有的三個(gè)更新回路被導(dǎo)航向量及其線(xiàn)性更新方程代替，更新后的導(dǎo)航向量再轉(zhuǎn)換成姿態(tài)、速度和位置輸出導(dǎo)航結(jié)果。偽線(xiàn)性捷聯(lián)慣導(dǎo)算法因采用了基于泰勒級(jí)數(shù)展開(kāi)的數(shù)值算法，因此在大機(jī)動(dòng)或圓錐運(yùn)動(dòng)等能激勵(lì)出較大不可交換誤差的情況下，理論算法精度遠(yuǎn)高于傳統(tǒng)算法。除此之外，捷聯(lián)慣導(dǎo)系統(tǒng)偽線(xiàn)性模型的另一優(yōu)勢(shì)是可以使用線(xiàn)性的分析與計(jì)算方法。

圖1 偽線(xiàn)性捷聯(lián)慣導(dǎo)系統(tǒng)結(jié)構(gòu)Fig.1 Structure of pseudo-linear SINS

最后，為了驗(yàn)證基于偽線(xiàn)性模型的捷聯(lián)慣導(dǎo)算法與傳統(tǒng)算法的差異，用兩種算法分別完成了大機(jī)動(dòng)條件下導(dǎo)航解算和旋轉(zhuǎn)彈飛行仿真導(dǎo)航解算，并比較兩者的導(dǎo)航誤差。

1 偽線(xiàn)性SINS更新公式

和傳統(tǒng)的捷聯(lián)慣導(dǎo)算法類(lèi)似，偽線(xiàn)性算法模型也將按照姿態(tài)、速度和位置三個(gè)更新過(guò)程進(jìn)行分析。在捷聯(lián)慣導(dǎo)更新算法的推導(dǎo)過(guò)程中，表示慣性坐標(biāo)系，表示載體坐標(biāo)系，表示導(dǎo)航坐標(biāo)系，表示地球坐標(biāo)系。

1.1 姿態(tài)更新

在捷聯(lián)慣導(dǎo)算法中姿態(tài)四元數(shù)微分方程為

(1)

(2)

(3)

(4)

用表示式(1)模型中的系統(tǒng)矩陣，即

(5)

則導(dǎo)航解算的姿態(tài)更新可以表示為

(6)

1.2 速度更新

比力方程是捷聯(lián)慣導(dǎo)解算的核心之一，求解比力方程即可完成速度更新。捷聯(lián)慣導(dǎo)系統(tǒng)導(dǎo)航系下比力方程為

(7)

在式(7)中改用四元數(shù)將系下的比力轉(zhuǎn)換到系下，得到：

(8)

由式(8)右邊第一項(xiàng)可以看出來(lái)，使用四元數(shù)進(jìn)行導(dǎo)航解算時(shí)，速度更新方程是非線(xiàn)性的。

為了得到線(xiàn)性的速度更新方程，對(duì)速度向量進(jìn)行變換，定義新的速度向量——速度四元數(shù)

(9)

對(duì)式(9)兩邊求導(dǎo)

(10)

將式(8)寫(xiě)成四元數(shù)形式

(11)

將式(2)和式(11)代入式(10)，得

(12)

(13)

式(12)可整理成

(14)

式中：

(15)

顯然，式(14)中的速度微分方程是關(guān)于姿態(tài)四元數(shù)和速度四元數(shù)的線(xiàn)性方程。

1.3 位置更新

在地理系下通常計(jì)算載體所在緯度、經(jīng)度和高度作為位置更新結(jié)果，因此定義位置向量

(16)

式中：為緯度，為經(jīng)度，表示高度。其微分方程為

(17)

式中：

(18)

根據(jù)式(18)不能直接得出位置四元數(shù)與速度四元數(shù)之間的線(xiàn)性關(guān)系，所以需要研究新的位置四元數(shù)定義。

由式(9)得

(19)

(20)

(21)

式(20)寫(xiě)成矩陣形式為

(22)

式中：

(23)

通過(guò)構(gòu)建位置四元數(shù)向量，位置更新方程被轉(zhuǎn)換成關(guān)于姿態(tài)四元數(shù)、速度四元數(shù)和位置四元數(shù)的偽線(xiàn)性方程。

2 導(dǎo)航向量及其更新方案

四元數(shù)微分方程為線(xiàn)性方程，比力方程雖然是非線(xiàn)性方程，但是經(jīng)過(guò)變換得到的速度四元數(shù)微分方程是線(xiàn)性的，同理通過(guò)定義位置四元數(shù)得到了偽線(xiàn)性的位置更新方程，因此可以定義包含四元數(shù)、速度四元數(shù)和位置四元數(shù)的導(dǎo)航向量

(24)

(25)

(26)

然后由式(6)、式(14)和式(22)可得導(dǎo)航向量微分方程

(27)

()=++…+-1-1

(28)

式(27)是線(xiàn)性時(shí)變系統(tǒng)，因此在任意更新周期內(nèi)，狀態(tài)轉(zhuǎn)移矩陣滿(mǎn)足：

(29)

為了求解式(29)，將(,0)表示成關(guān)于時(shí)間的次矩陣多項(xiàng)式，即

(,0)=+++…+

(30)

對(duì)式(30)求導(dǎo)可得

(31)

將式(28)、(30)和(31)代入式(29)，得

(32)

整理后得：

(33)

式中：是(,0)的展開(kāi)次數(shù)，決定其計(jì)算精度，根據(jù)式(30)構(gòu)建狀態(tài)轉(zhuǎn)移矩陣的精度可以達(dá)到(+1)，因此在設(shè)計(jì)算法時(shí)可以根據(jù)更新周期、導(dǎo)航算法精度等指標(biāo)選擇合適的展開(kāi)次數(shù)完成計(jì)算。

根據(jù)式(28)、(30)和(33)可以計(jì)算出狀態(tài)轉(zhuǎn)移矩陣(, 0)，然后完成導(dǎo)航向量的更新

()=(, 0)(0)

(34)

最后將更新后的導(dǎo)航向量轉(zhuǎn)換成地理系下的姿態(tài)、速度和位置。

3 算法精度評(píng)估

為了檢驗(yàn)偽線(xiàn)性模型的導(dǎo)航解算效果，在短時(shí)大角速度大加速度條件和設(shè)計(jì)的旋轉(zhuǎn)彈飛行軌跡上進(jìn)行純慣性導(dǎo)航解算，比較偽線(xiàn)性慣導(dǎo)算法與傳統(tǒng)慣導(dǎo)算法的差異，計(jì)算時(shí)統(tǒng)一采用4子樣算法結(jié)構(gòu)。為便于表述評(píng)估結(jié)果，后文中使用“NLNA(Nonlinear navigation algorithm,非線(xiàn)性導(dǎo)航算法)”表示傳統(tǒng)的捷聯(lián)慣導(dǎo)算法，用“PLNA(Pseudo-linear navigation algorithm,偽線(xiàn)性導(dǎo)航算法)”表示新提出的捷聯(lián)慣導(dǎo)算法。

偽線(xiàn)性算法的4子樣解算結(jié)構(gòu)如下：

① 根據(jù)4子樣角增量和速度增量Δ,…,Δ和Δ,…,Δ，計(jì)算角速度和比力多項(xiàng)式系數(shù)，得到角速度和比力函數(shù)：

(35)

(36)

② 根據(jù)式(6)、式(14)和式(22)得到不同次系統(tǒng)矩陣的各個(gè)子矩陣。0次系數(shù)矩陣中各項(xiàng)如下：

(37)

1次及以上系數(shù)矩陣各項(xiàng)為：

(38)

③ 根據(jù)式(27)得到不同次系數(shù)矩陣

(39)

并根據(jù)式(28)得到導(dǎo)航向量系統(tǒng)矩陣的多項(xiàng)式形式：

=+++

(40)

④ 將代入式(33)得到狀態(tài)轉(zhuǎn)移矩陣不同次項(xiàng)，按照式(30)計(jì)算狀態(tài)轉(zhuǎn)移矩陣

(,0)=+++

(41)

⑤ 最后根據(jù)式(34)完成導(dǎo)航向量更新，并根據(jù)式(9)和式(11)中的定義計(jì)算出地理系下的速度和位置信息。

3.1 大機(jī)動(dòng)條件慣導(dǎo)解算仿真

大機(jī)動(dòng)運(yùn)動(dòng)條件對(duì)捷聯(lián)慣導(dǎo)算法有更高的要求，對(duì)于傳統(tǒng)捷聯(lián)慣導(dǎo)算法，在大機(jī)動(dòng)條件下會(huì)激勵(lì)出較大的算法誤差。捷聯(lián)慣導(dǎo)算法研究中一般都采用多項(xiàng)式形式的角速度和加速度模擬大機(jī)動(dòng)條件，并在此條件下檢驗(yàn)算法精度。按照文獻(xiàn)[18-19]中仿真方法，用角速度多項(xiàng)式模擬大機(jī)動(dòng)角運(yùn)動(dòng)，設(shè)置大機(jī)動(dòng)條件角速度和加速度多項(xiàng)式如式(42)～(43)所示，對(duì)應(yīng)的角速度與加速度變化如圖2所示。仿真時(shí)按1 kHz采樣得到慣導(dǎo)仿真數(shù)據(jù)，同時(shí)以100 kHz采樣完成2子樣導(dǎo)航解算，得到姿態(tài)和速度的參考結(jié)果。兩種捷聯(lián)慣導(dǎo)算法的姿態(tài)誤差和速度誤差如圖3～4所示，導(dǎo)航解算誤差如表1所列，因?yàn)椴煌惴ǖ淖藨B(tài)和速度誤差相差較大，因此不必全部列出，選取導(dǎo)航結(jié)束時(shí)刻的三個(gè)軸上誤差的模值作為姿態(tài)誤差和速度誤差。

圖2 大機(jī)動(dòng)條件角速度與加速度Fig.2 Angular rate and acceleration in high-dynamic condition

圖3 大機(jī)動(dòng)條件姿態(tài)誤差Fig.3 Attitude errors in high-dynamic condition

圖4 大機(jī)動(dòng)條件速度誤差Fig.4 Velocity errors in high-dynamic condition

(42)

(43)

從圖3～4和表1中的姿態(tài)誤差與速度誤差對(duì)比結(jié)果可以明顯看出偽線(xiàn)性算法的導(dǎo)航誤差更小。

表1 導(dǎo)航解算仿真誤差Table 1 Errors of navigation solution simulation

這是因?yàn)榛趥尉€(xiàn)性模型的算法在進(jìn)行姿態(tài)和速度更新時(shí)，通過(guò)求解姿態(tài)和速度微分方程的高階導(dǎo)數(shù)，計(jì)算出高精度的導(dǎo)航結(jié)果。在角速度和加速度為關(guān)于時(shí)間的一般多項(xiàng)式的條件下，此算法可以通過(guò)增加泰勒展開(kāi)次數(shù)減小算法誤差，這一特點(diǎn)也使得偽線(xiàn)性捷聯(lián)慣導(dǎo)算法更加適合應(yīng)用于大機(jī)動(dòng)條件。

3.2 旋轉(zhuǎn)彈純慣性導(dǎo)航仿真

旋轉(zhuǎn)彈飛行過(guò)程中含有較長(zhǎng)的大角速度機(jī)動(dòng)，容易激勵(lì)出捷聯(lián)慣導(dǎo)算法誤差，因此和傳統(tǒng)算法相比，偽線(xiàn)性模型捷聯(lián)慣導(dǎo)算法更加適用于旋轉(zhuǎn)彈捷聯(lián)慣導(dǎo)解算。為了驗(yàn)證捷聯(lián)慣導(dǎo)算法性能，設(shè)計(jì)旋轉(zhuǎn)彈飛行過(guò)程仿真軌跡如圖5所示，飛行中彈體角速度從20π rad/s逐漸減速至2π rad/s，慣導(dǎo)數(shù)據(jù)更新頻率為1 kHz。

圖5 旋轉(zhuǎn)彈仿真軌跡Fig.5 Simulation trajectory of spinning projectile

使用4子樣算法捷聯(lián)慣導(dǎo)解算仿真并統(tǒng)計(jì)姿態(tài)誤差、速度誤差和位置誤差的均方根以分析導(dǎo)航誤差，評(píng)估兩種算法的精度。導(dǎo)航誤差對(duì)比如圖6～7所示，全程的均方根誤差如表2所列。

從圖6～7和表2可以得出以下結(jié)論：

表2 旋轉(zhuǎn)彈仿真算法誤差Table 2 Algorithm errors in spinning projectile simulation

圖6 旋轉(zhuǎn)彈仿真姿態(tài)誤差Fig.6 Attitude errors of spinning projectile simulation

(1)基于非線(xiàn)性模型的傳統(tǒng)捷聯(lián)慣導(dǎo)算法在大橫滾角速度條件下算法誤差遠(yuǎn)大于偽線(xiàn)性算法；

(2)從圖7的位置誤差變化圖上可以看出，傳統(tǒng)算法的位置誤差在導(dǎo)航過(guò)程中一直發(fā)散，而偽線(xiàn)性導(dǎo)航算法的位置誤差則在后半程基本不變，這說(shuō)明偽線(xiàn)性算法在旋轉(zhuǎn)彈導(dǎo)航中有明顯的優(yōu)勢(shì)；

圖7 旋轉(zhuǎn)彈仿真速度與位置誤差Fig.7 Velocity and position errors of spinning projectile simulation

(3)仿真結(jié)果表明，對(duì)于旋轉(zhuǎn)彈,傳統(tǒng)捷聯(lián)慣導(dǎo)算法的算法誤差已不能被忽略，因此捷聯(lián)慣導(dǎo)算法誤差模型不再準(zhǔn)確，如果要進(jìn)行組合導(dǎo)航，應(yīng)當(dāng)以偽線(xiàn)性捷聯(lián)慣導(dǎo)算法為基礎(chǔ)，建立組合導(dǎo)航系統(tǒng)線(xiàn)性狀態(tài)空間模型。

4 結(jié) 論

本文在已有的高精度捷聯(lián)慣導(dǎo)算法和偽線(xiàn)性系統(tǒng)理論的基礎(chǔ)上，提出了完整的地理系下基于偽線(xiàn)性模型的捷聯(lián)慣導(dǎo)算法。該算法可以求解線(xiàn)性系統(tǒng)方程實(shí)現(xiàn)捷聯(lián)慣導(dǎo)解算更新，且在大機(jī)動(dòng)條件下，例如有較大橫滾角速度的旋轉(zhuǎn)彈的導(dǎo)航解算中，有較高的算法精度。與傳統(tǒng)的捷聯(lián)慣導(dǎo)算法相比，基于偽線(xiàn)性模型的算法具有精度高、結(jié)構(gòu)簡(jiǎn)單、線(xiàn)性化等優(yōu)點(diǎn)，對(duì)于捷聯(lián)慣導(dǎo)算法研究與工程應(yīng)用有一定的參考價(jià)值。