捷聯(lián)慣導(dǎo)準(zhǔn)靜基座大失準(zhǔn)角線性初始對(duì)準(zhǔn)方法研究

狄靜波，常路賓

(海軍工程大學(xué)電氣工程學(xué)院，武漢 430033)

0 引言

初始對(duì)準(zhǔn)是捷聯(lián)慣導(dǎo)工作的起始和必要過(guò)程。常用的初始對(duì)準(zhǔn)方法是傳統(tǒng)的兩步初始對(duì)準(zhǔn)方法，即先利用解析方法進(jìn)行粗對(duì)準(zhǔn)，快速地獲得概略已知姿態(tài)信息，進(jìn)而利用該姿態(tài)信息作為初始值進(jìn)行線性卡爾曼濾波精對(duì)準(zhǔn)。如果能夠獲得較為精確的初始姿態(tài)，捷聯(lián)慣導(dǎo)線性誤差模型能夠有效地完成各種條件下的精對(duì)準(zhǔn)。由于在角運(yùn)動(dòng)/線運(yùn)動(dòng)干擾下，傳統(tǒng)的解析粗對(duì)準(zhǔn)不再適用。針對(duì)上述難題，目前有兩類解決方案：一是利用人為定義慣性系作為過(guò)渡坐標(biāo)系的優(yōu)化對(duì)準(zhǔn)方法；二是推導(dǎo)大失準(zhǔn)角條件下的非線性誤差模型，直接利用非線性濾波進(jìn)行對(duì)準(zhǔn)。優(yōu)化對(duì)準(zhǔn)是近年來(lái)較為盛行的初始對(duì)準(zhǔn)方法，無(wú)論是在學(xué)術(shù)研究還是在工程應(yīng)用角度都產(chǎn)生了較大影響?？梢哉f(shuō)，優(yōu)化粗對(duì)準(zhǔn)+卡爾曼濾波精對(duì)準(zhǔn)能夠解決絕大部分應(yīng)用場(chǎng)景下的初始對(duì)準(zhǔn)問(wèn)題。而非線性初始對(duì)準(zhǔn)一直嘗試將傳統(tǒng)的兩步對(duì)準(zhǔn)方案統(tǒng)一，直接實(shí)現(xiàn)一步對(duì)準(zhǔn)。但非?？上У氖?，由于非線性誤差模型的固有缺陷，非線性初始對(duì)準(zhǔn)在對(duì)準(zhǔn)速度和穩(wěn)態(tài)精度上一直沒(méi)有達(dá)到傳統(tǒng)的兩步對(duì)準(zhǔn)效果。很多時(shí)候，非線性初始對(duì)準(zhǔn)僅僅是作為驗(yàn)證新型非線性濾波算法的一種算例，較少有研究涉及相關(guān)模型的優(yōu)化改進(jìn)。

近年來(lái)興起的不變擴(kuò)展卡爾曼濾波(Invariant Extended Kalman Filter，IEKF)算法在慣性基領(lǐng)域得到了廣泛的應(yīng)用，如姿態(tài)估計(jì)、同時(shí)定位與建圖、視覺(jué)里程計(jì)等。不變卡爾曼濾波可以有效解決因狀態(tài)誤估計(jì)導(dǎo)致的系統(tǒng)可觀測(cè)性與實(shí)際不符的問(wèn)題。相關(guān)學(xué)者也將其引入到捷聯(lián)慣性基組合導(dǎo)航、系統(tǒng)標(biāo)定等領(lǐng)域，實(shí)驗(yàn)結(jié)果表明，該算法可進(jìn)一步提高定位精度。不變卡爾曼濾波的優(yōu)勢(shì)在于通過(guò)定義在李群上的狀態(tài)，構(gòu)造相應(yīng)的非線性誤差量，進(jìn)而推導(dǎo)出獨(dú)立于狀態(tài)估計(jì)值的線性狀態(tài)空間模型。由于狀態(tài)空間模型獨(dú)立于狀態(tài)估計(jì)值，因此能夠做到“不變”，進(jìn)而有效解決誤差可觀測(cè)性問(wèn)題。獨(dú)立于狀態(tài)估計(jì)值的狀態(tài)空間模型的另外一個(gè)突出優(yōu)勢(shì)是，基于該線性模型可以精確反推出李群上的非線性誤差，即對(duì)于初始狀態(tài)估計(jì)遠(yuǎn)遠(yuǎn)偏離真實(shí)值的情形，利用該線性模型同樣可以做到濾波收斂。上述優(yōu)勢(shì)為利用線性誤差模型實(shí)現(xiàn)大失準(zhǔn)角初始對(duì)準(zhǔn)提供了思路，即針對(duì)慣導(dǎo)初始對(duì)準(zhǔn)應(yīng)用場(chǎng)景下的慣導(dǎo)系統(tǒng)模型，能否推導(dǎo)或設(shè)計(jì)出獨(dú)立于狀態(tài)估計(jì)的誤差狀態(tài)空間模型。2015年,A.Barrau在其博士論文中首次報(bào)道了IEKF在慣導(dǎo)初始對(duì)準(zhǔn)中的應(yīng)用，相對(duì)于傳統(tǒng)的初始對(duì)準(zhǔn)方法，該線性化方法在大失準(zhǔn)角下依然適用。

根據(jù)上述思路，本文利用李群理論中的仿射性分析方法，系統(tǒng)研究了準(zhǔn)靜基座條件下捷聯(lián)慣導(dǎo)姿態(tài)和速度微分方程。分析指出，捷聯(lián)慣導(dǎo)姿態(tài)微分方程滿足仿射性條件，從而為利用其線性誤差方程實(shí)現(xiàn)大失準(zhǔn)角初始對(duì)準(zhǔn)提供了可能。同時(shí)，通過(guò)分析(3)+和(3)兩種狀態(tài)描述下的誤差模型，指出兩種模型中速度誤差模型都與狀態(tài)估計(jì)值有關(guān)，而且(3)狀態(tài)描述下的觀測(cè)方程也與姿態(tài)誤差有關(guān)。在上述分析的基礎(chǔ)上，直接對(duì)(3)+狀態(tài)描述下的速度誤差方程進(jìn)行改造，用重力矢量替換原有誤差方程中的比力項(xiàng)，進(jìn)而實(shí)現(xiàn)整體誤差狀態(tài)模型的不變性。仿真實(shí)驗(yàn)和車載晃動(dòng)實(shí)驗(yàn)也驗(yàn)證了利用所研究的線性模型能夠有效實(shí)現(xiàn)大失準(zhǔn)角初始對(duì)準(zhǔn)。

由于本文所研究的模型未考慮位置狀態(tài)，因此只適用于準(zhǔn)靜基座條件下的初始對(duì)準(zhǔn)；同時(shí)，狀態(tài)空間模型未考慮慣性器件誤差，因此所研究的對(duì)準(zhǔn)算法仍然定位于粗對(duì)準(zhǔn)。

1 準(zhǔn)靜基座下慣導(dǎo)模型分析

1.1 準(zhǔn)靜基座慣導(dǎo)微分方程

記地心慣性坐標(biāo)系為系，載體坐標(biāo)系(右-前-上)為系，導(dǎo)航坐標(biāo)系(東-北-天)為系，地球坐標(biāo)系為系。準(zhǔn)靜基座條件下，載體位置信息可以認(rèn)為是已知且不變，因此對(duì)于粗對(duì)準(zhǔn)而言，可以只考慮姿態(tài)和速度信息。導(dǎo)航坐標(biāo)系姿態(tài)和速度微分方程為

(1)

需要指出的是，式(1)中的慣導(dǎo)微分方程本質(zhì)上是一個(gè)阻尼微分方程，即式(1)中已經(jīng)默認(rèn)代入了速度為零、位置為已知常值的條件。后續(xù)的線性誤差模型都是在該阻尼慣導(dǎo)微分方程基礎(chǔ)上推導(dǎo)的。因此，如果進(jìn)行間接式初始對(duì)準(zhǔn)，即先慣導(dǎo)解算、再濾波估計(jì)狀態(tài)誤差、進(jìn)而反饋修正，那么在慣導(dǎo)解算階段必須用該阻尼微分方程，而不能利用未簡(jiǎn)化的慣導(dǎo)微分方程。

1.2 慣導(dǎo)微分方程仿射性分析

根據(jù)李群理論，對(duì)于定義在群上的狀態(tài)∈，其對(duì)應(yīng)的狀態(tài)模型為

(2)

其中，表示已知輸入量。文獻(xiàn)[14, 18]指出，對(duì)于,∈，如果()滿足如下關(guān)系式

()=()+()-

()

(3)

則()表示的模型是一個(gè)仿射模型，其歐幾里得空間下的線性化模型獨(dú)立于狀態(tài)估計(jì)值，同時(shí)可以根據(jù)該線性化模型精確反推出李群上的非線性誤差。

上述理論為利用線性模型實(shí)現(xiàn)任意失準(zhǔn)角下的初始對(duì)準(zhǔn)提供了思路，即通過(guò)對(duì)慣導(dǎo)微分方程中涉及的狀態(tài)在李群上進(jìn)行建模，進(jìn)而分析其對(duì)應(yīng)的模型是否滿足上述仿射性條件。

對(duì)于模型(1)中的姿態(tài)和速度，一般有兩種狀態(tài)定義方式。慣導(dǎo)初始對(duì)準(zhǔn)領(lǐng)域傳統(tǒng)的定義方式為(3)+，即將姿態(tài)定義在李群上，而速度定義在歐幾里得空間下。另外一種定義方式是將姿態(tài)和速度集成定義為特殊歐式群(3)的元素，即

(4)

對(duì)于式(4)中的李群狀態(tài)，其對(duì)應(yīng)的系統(tǒng)模型為

()=

(5)

接下來(lái)分析模型(5)是否滿足式(3)。根據(jù)定義(4)可得

(6)

令=()，則

(7)

令=()，則

(8)

令=()，則

(9)

令=()，則

(10)

通過(guò)驗(yàn)證可知

(1,1)=(1,1)+(1,1)-(1,1)

(11)

但是由于

(12)

可以驗(yàn)證

(1,2)≠(1,2)+(1,2)-(1,2)

(13)

2 獨(dú)立于狀態(tài)估計(jì)的慣導(dǎo)線性誤差模型設(shè)計(jì)

對(duì)于(3)+狀態(tài)定義，定義狀態(tài)誤差量為

(14)

其中

(15)

(16)

則δ所對(duì)應(yīng)的狀態(tài)空間模型為

(17)

(18)

(19)

如果選取速度誤差作為觀測(cè)量，則對(duì)應(yīng)的觀測(cè)模型為

(20)

對(duì)于(3)狀態(tài)定義，定義狀態(tài)誤差量為

(21)

可以看出，姿態(tài)誤差定義同式(15)是一致的。速度誤差定義可近似為

(22)

可以看出，式(22)中的速度誤差與文獻(xiàn)[15-18]中的變換速度誤差是內(nèi)在一致的，事實(shí)上，文獻(xiàn)[15-18]中的變換速度誤差也是受IEKF算法啟發(fā)而來(lái)的。

定義狀態(tài)量為

(23)

其對(duì)應(yīng)的狀態(tài)空間模型為

(24)

(25)

(26)

如果選取速度誤差作為觀測(cè)量，則對(duì)應(yīng)的觀測(cè)模型為

(27)

事實(shí)上，(3)+和(3)作為兩種狀態(tài)定義方式，在理論基礎(chǔ)上并沒(méi)有優(yōu)劣之分，主要還是看基于其所構(gòu)造的誤差模型的呈現(xiàn)形式。文獻(xiàn)[19]指出，狀態(tài)誤差量的構(gòu)建并不拘泥于特定的流形上狀態(tài)的定義。通過(guò)對(duì)比分析式(18)和式(25)可以看出，不同的速度誤差定義可以產(chǎn)生不同的速度誤差模型。由于姿態(tài)微分方程本身具有仿射性，其對(duì)應(yīng)的姿態(tài)誤差模型與狀態(tài)估計(jì)無(wú)關(guān)。因此，可以對(duì)速度誤差方程進(jìn)行改造，目的是使其模型同樣獨(dú)立于狀態(tài)估計(jì)，從而最大程度減小速度模型的非仿射性對(duì)整體系統(tǒng)誤差估計(jì)的影響。基于上述認(rèn)識(shí)，對(duì)狀態(tài)空間模型(17)進(jìn)行改造。在準(zhǔn)靜基座條件下，根據(jù)模型(1)可得

(28)

因此，式(17)可改寫(xiě)為

(29)

其中

(30)

3 實(shí)驗(yàn)研究

為了驗(yàn)證所研究模型的有效性，分別設(shè)計(jì)了仿真實(shí)驗(yàn)和車載晃動(dòng)實(shí)驗(yàn)。參與比較的算法模型分別為：狀態(tài)空間模型(17)和(20)記為模型1；狀態(tài)空間模型(24)和(27)記為模型2；狀態(tài)空間模型(29)和(20)記為模型3。

3.1 仿真實(shí)驗(yàn)

(a)

(a)

圖3 仿真實(shí)驗(yàn)航向角對(duì)準(zhǔn)誤差

3.2 車載實(shí)驗(yàn)

圖4 車載實(shí)驗(yàn)俯仰角對(duì)準(zhǔn)結(jié)果

圖5 車載實(shí)驗(yàn)橫滾角對(duì)準(zhǔn)結(jié)果

圖6 車載實(shí)驗(yàn)航向角對(duì)準(zhǔn)結(jié)果

4 結(jié)論

本文從仿射性分析角度,系統(tǒng)研究了準(zhǔn)靜基座條件下捷聯(lián)慣導(dǎo)系統(tǒng)的初始對(duì)準(zhǔn)問(wèn)題。分析指出，慣導(dǎo)姿態(tài)微分方程滿足仿射性條件，具備利用其線性模型進(jìn)行大失準(zhǔn)角初始對(duì)準(zhǔn)的可能。但是當(dāng)增加速度作為狀態(tài)量后，整體模型不再滿足仿射性條件，因此無(wú)論是基于(3)+還是(3)的狀態(tài)描述，都不能直接推導(dǎo)出完全獨(dú)立于狀態(tài)估計(jì)值的狀態(tài)空間模型。在上述分析的基礎(chǔ)上，本文直接將(3)+狀態(tài)描述下的速度誤差模型中的比力項(xiàng)用重力矢量代替，從而構(gòu)造出完全獨(dú)立于狀態(tài)估計(jì)值的狀態(tài)轉(zhuǎn)移矩陣。仿真實(shí)驗(yàn)和車載晃動(dòng)實(shí)驗(yàn)結(jié)果表明，利用新構(gòu)造的線性狀態(tài)空間模型能夠?qū)崿F(xiàn)大失準(zhǔn)角下的初始對(duì)準(zhǔn)，初始對(duì)準(zhǔn)速度和精度都優(yōu)于傳統(tǒng)模型。

基于本文的分析和實(shí)驗(yàn)結(jié)果，可以得到以下結(jié)論:

1)在準(zhǔn)靜基座條件下，采用優(yōu)化對(duì)準(zhǔn)或者本文提出的線性大失準(zhǔn)角對(duì)準(zhǔn)都能夠較為準(zhǔn)確地為后續(xù)卡爾曼濾波精對(duì)準(zhǔn)提供初始姿態(tài)信息。

2)本文所研究的方法也能夠?yàn)楹罄m(xù)的線性卡爾曼濾波精對(duì)準(zhǔn)提供姿態(tài)方差初始信息，從而縮短相應(yīng)的階躍響應(yīng)時(shí)間。

由于本文的分析是在準(zhǔn)靜基座前提下展開(kāi)的，相關(guān)模型和結(jié)論并不適用于動(dòng)基座初始對(duì)準(zhǔn)問(wèn)題。由于增加位置作為狀態(tài)后，會(huì)進(jìn)一步影響模型整體的仿射性，如何參照本文思路推導(dǎo)獨(dú)立于狀態(tài)估計(jì)值的狀態(tài)空間模型，進(jìn)而實(shí)現(xiàn)動(dòng)基座大失準(zhǔn)角線性卡爾曼濾波對(duì)準(zhǔn)是接下來(lái)需要開(kāi)展的工作。

感謝西北工業(yè)大學(xué)嚴(yán)恭敏老師提供的開(kāi)源高精度捷聯(lián)慣導(dǎo)算法(Precise Strapdown Inertial Navigation System，PSINS)，相關(guān)程序代碼加深了作者對(duì)慣導(dǎo)相關(guān)算法的學(xué)習(xí)理解，加快了對(duì)相關(guān)算法的改進(jìn)和驗(yàn)證。感謝上海交通大學(xué)武元新老師在狀態(tài)表示方面對(duì)作者的指導(dǎo)。同時(shí)，作者非常感謝嚴(yán)恭敏老師提供的車載激光陀螺慣導(dǎo)數(shù)據(jù)，該數(shù)據(jù)應(yīng)用于本文中車載實(shí)驗(yàn)驗(yàn)證一節(jié)。