一種基于回溯過(guò)程的里程計(jì)輔助SINS 初始對(duì)準(zhǔn)方法

林宇森，繆玲娟，周志強(qiáng)

（北京理工大學(xué) 自動(dòng)化學(xué)院，北京 100081）

捷聯(lián)慣性導(dǎo)航系統(tǒng)（Strapdown Inertial Navigation System,SINS）是應(yīng)用最廣泛的導(dǎo)航系統(tǒng)之一[1]。由于越來(lái)越多的陸地車輛需要SINS 在行駛過(guò)程中完成初始對(duì)準(zhǔn)，因此行進(jìn)間對(duì)準(zhǔn)已經(jīng)成為了一個(gè)備受關(guān)注的問(wèn)題。通常使用的外部導(dǎo)航傳感器包括全球定位系統(tǒng)（Global Positioning System,GPS）、里程計(jì)（Odometer,OD）等，它們用來(lái)為SINS 的行進(jìn)間對(duì)準(zhǔn)提供輔助信息。GPS 的信號(hào)易受干擾，同時(shí)在掩蔽環(huán)境中無(wú)法使用。而OD 可以獨(dú)立自主地測(cè)量車輛路程增量，以此為觀測(cè)信息完成SINS 陸上對(duì)準(zhǔn)。因此，陸用定位定向系統(tǒng)中使用基于OD 輔助的SINS 行進(jìn)間初始對(duì)準(zhǔn)，增強(qiáng)了陸用裝備的戰(zhàn)場(chǎng)適應(yīng)性，引起了廣泛的關(guān)注。

初始對(duì)準(zhǔn)通常分為兩個(gè)階段，即粗對(duì)準(zhǔn)階段和精對(duì)準(zhǔn)階段。理想情況下，粗對(duì)準(zhǔn)階段提供初始姿態(tài)以使得SINS 在線性濾波模型下進(jìn)行精對(duì)準(zhǔn)。使用慣性參考系中的重力測(cè)量值計(jì)算初始姿態(tài)是一種有效的粗對(duì)準(zhǔn)方法。文獻(xiàn)[2]提出了一種TRIAD 算法，該算法使用兩個(gè)不同時(shí)刻的矢量觀測(cè)來(lái)計(jì)算姿態(tài)。為了充分利用測(cè)量值，文獻(xiàn)[3]將粗對(duì)準(zhǔn)問(wèn)題轉(zhuǎn)化為基于無(wú)限矢量觀測(cè)的姿態(tài)確定問(wèn)題，這種姿態(tài)確定問(wèn)題被稱為優(yōu)化對(duì)準(zhǔn)方法，通常通過(guò)q-method 方法解決這類問(wèn)題[4]。慣性系解析對(duì)準(zhǔn)方法可以進(jìn)一步用于OD 輔助SINS的行進(jìn)間對(duì)準(zhǔn)。文獻(xiàn)[5]結(jié)合慣性系解析對(duì)準(zhǔn)方法提出了行進(jìn)間粗對(duì)準(zhǔn)方法，但該方法未進(jìn)行載體即時(shí)位置的更新，在計(jì)算矢量觀測(cè)時(shí)進(jìn)行了大量近似。為了處理OD 固有的嚴(yán)重干擾，文獻(xiàn)[6]加入了一個(gè)低通有限脈沖響應(yīng)數(shù)字濾波器來(lái)衰減OD 中的干擾。為了補(bǔ)償陀螺儀動(dòng)態(tài)誤差，文獻(xiàn)[7]提出了一種基于卡爾曼濾波的OD 輔助SINS 粗對(duì)準(zhǔn)方法。為了提高速度和加速度的跟蹤能力，文獻(xiàn)[8]中慣性元件和OD 在一個(gè)采樣間隔內(nèi)的輸出采用直線擬合而不是假設(shè)成為常數(shù)。對(duì)于OD 輔助SINS 的行進(jìn)間精對(duì)準(zhǔn)，文獻(xiàn)[9]提出了一種改進(jìn)的ACKF/KF 的初始對(duì)準(zhǔn)方法用于大失準(zhǔn)角初始對(duì)準(zhǔn)。上述研究的主要關(guān)注點(diǎn)是提高對(duì)準(zhǔn)的最終精度，而對(duì)準(zhǔn)的快速性也應(yīng)是行進(jìn)間對(duì)準(zhǔn)中非常重要的評(píng)價(jià)指標(biāo)。

近年來(lái)，隨著導(dǎo)航計(jì)算機(jī)技術(shù)的發(fā)展，人們對(duì)回溯過(guò)程進(jìn)行了研究，以提高短時(shí)間內(nèi)初始對(duì)準(zhǔn)的精度。為了解決在沒(méi)有GPS 情況下的行進(jìn)間對(duì)準(zhǔn)的定位問(wèn)題，文獻(xiàn)[10]研究了一種基于回溯方案的OD 輔助行進(jìn)間精對(duì)準(zhǔn)方法(Optimization-Based Alignment,OBA)。在此基礎(chǔ)上，文獻(xiàn)[11]給出了一種基于回溯方案的大失準(zhǔn)角OD 輔助行進(jìn)間精對(duì)準(zhǔn)方法。對(duì)于OD的異常值問(wèn)題，文獻(xiàn)[12]研究了一種用于OD 輔助SINS 的魯棒行進(jìn)間精對(duì)準(zhǔn)方法。然而，上述對(duì)準(zhǔn)方法僅將回溯過(guò)程用于精對(duì)準(zhǔn)階段，對(duì)粗對(duì)準(zhǔn)階段的回溯過(guò)程沒(méi)有涉及。在粗對(duì)準(zhǔn)和精對(duì)準(zhǔn)中同時(shí)應(yīng)用回溯過(guò)程對(duì)于快速高精度的初始對(duì)準(zhǔn)更為有效。

本文提出了一種增加粗對(duì)準(zhǔn)回溯的OD輔助SINS行進(jìn)間對(duì)準(zhǔn)方法，該方法可以快速自主地獲得高精度的姿態(tài)和位置。所提出的行進(jìn)間對(duì)準(zhǔn)方法由粗對(duì)準(zhǔn)和精對(duì)準(zhǔn)組成。在粗對(duì)準(zhǔn)階段，結(jié)合包含即時(shí)位置更新的慣性系解析對(duì)準(zhǔn)方案，提出了基于回溯過(guò)程的粗對(duì)準(zhǔn)方法；然后利用粗對(duì)準(zhǔn)的對(duì)準(zhǔn)結(jié)果對(duì)卡爾曼濾波參數(shù)進(jìn)行初始化，提出了基于回溯過(guò)程的精對(duì)準(zhǔn)方法，進(jìn)一步提高姿態(tài)和位置的精度。最后通過(guò)仿真試驗(yàn)證了所研究方法的有效性。

1 粗對(duì)準(zhǔn)和精對(duì)準(zhǔn)介紹

本文所使用的坐標(biāo)系定義如下：

i：地心慣性系。其原點(diǎn)位于地心，zi軸沿地球自轉(zhuǎn)方向，x i軸在赤道平面內(nèi)指向春分點(diǎn)，y i軸與xi、zi軸構(gòu)成右手坐標(biāo)系。

e：地球坐標(biāo)系。其原點(diǎn)位于地心，ze軸沿地球自轉(zhuǎn)方向，xe軸在赤道平面內(nèi)指向載體所在點(diǎn)的子午線，ye軸與xe、ze軸構(gòu)成右手坐標(biāo)系。

e0：將對(duì)準(zhǔn)開(kāi)始時(shí)刻的地球坐標(biāo)系凝固后形成的慣性系。

n：導(dǎo)航坐標(biāo)系（東-北-天）。

n0：將對(duì)準(zhǔn)開(kāi)始時(shí)刻的導(dǎo)航坐標(biāo)系凝固后形成的慣性系。

b：載體坐標(biāo)系（右-前-上）。

b0：將對(duì)準(zhǔn)開(kāi)始時(shí)刻的載體坐標(biāo)系凝固后形成的慣性系。

1.1 改進(jìn)的慣性系解析粗對(duì)準(zhǔn)

1.1.1 姿態(tài)矩陣的分解

在慣性系解析粗對(duì)準(zhǔn)中，姿態(tài)矩陣可以分解為：

其中，ω ie表示地球自轉(zhuǎn)角速率，L0和λ0分別表示初始的緯度和經(jīng)度，L和λ分別表示t時(shí)刻的緯度和經(jīng)度，計(jì)算方法在1.1.3 節(jié)介紹。

1.1.2 慣性系解析粗對(duì)準(zhǔn)

在n系下比力方程可以表示為[1]：

其中，

由式(16)可知，當(dāng)α和β已知時(shí)，的求解已經(jīng)轉(zhuǎn)化為了Wahba 問(wèn)題[13]。關(guān)于Wahba 問(wèn)題的求解在文獻(xiàn)[14]中有相應(yīng)的細(xì)節(jié)，本文不再重復(fù)。

1.1.3 即時(shí)位置更新

對(duì)式(9)進(jìn)行積分，可得位置增量如下：

1.2 卡爾曼濾波精對(duì)準(zhǔn)

OD 輔助SINS 精對(duì)準(zhǔn)的誤差模型可以寫為[15]

設(shè)置狀態(tài)向量X如下：

2 基于回溯過(guò)程的粗對(duì)準(zhǔn)和精對(duì)準(zhǔn)

2.1 回溯過(guò)程

假設(shè)從位置A導(dǎo)航到位置B為前向過(guò)程，則從位置B回到位置A為逆向過(guò)程，將前向過(guò)程和逆向過(guò)程結(jié)合起來(lái)稱為回溯過(guò)程。將回溯過(guò)程引入初始對(duì)準(zhǔn)可以對(duì)測(cè)量數(shù)據(jù)進(jìn)行多次處理，充分利用有限的信息，等效于延長(zhǎng)了對(duì)準(zhǔn)時(shí)間，從而提高對(duì)準(zhǔn)精度[16]。

前向過(guò)程中SINS 的姿態(tài)和速度更新方程為：

假設(shè)SINS 的更新周期為Δt，式(36)(37)可離散化為：

逆向過(guò)程利用前向過(guò)程中存儲(chǔ)的陀螺儀、加速度計(jì)和OD 的測(cè)量值進(jìn)行逆向計(jì)算，得到逆向的姿態(tài)、速度和位置信息。由式(38)可得逆向過(guò)程的姿態(tài)更新方程：

由式(39)可得逆向過(guò)程的速度更新方程為：

由式(40)(41)可知，逆向過(guò)程中測(cè)量數(shù)據(jù)的生成方式為：（1）將前向陀螺儀、加速度計(jì)和OD 的測(cè)量值逆序；（2）將陀螺儀與OD 的測(cè)量值和地球旋轉(zhuǎn)速率的符號(hào)取負(fù)。在對(duì)準(zhǔn)時(shí)間較短或測(cè)量數(shù)據(jù)較少的情況下，引入回溯過(guò)程可以人為地增加測(cè)量數(shù)據(jù)，等效延長(zhǎng)對(duì)準(zhǔn)時(shí)間。隨著對(duì)準(zhǔn)時(shí)間的增加，濾波器有充足的時(shí)間更容易收斂于穩(wěn)態(tài)誤差，從而提高初始對(duì)準(zhǔn)精度。

2.2 基于回溯過(guò)程的粗對(duì)準(zhǔn)

基于回溯過(guò)程的粗對(duì)準(zhǔn)是在前向過(guò)程粗對(duì)準(zhǔn)之后繼續(xù)進(jìn)行逆向過(guò)程粗對(duì)準(zhǔn)?；诨厮葸^(guò)程的粗對(duì)準(zhǔn)示意圖如圖1 所示，其中t1表示對(duì)準(zhǔn)測(cè)量數(shù)據(jù)總時(shí)長(zhǎng)。

圖1 基于回溯過(guò)程的粗對(duì)準(zhǔn)示意圖Fig.1 Diagram of coarse alignment based on backtracking process

前向慣性系解析粗對(duì)準(zhǔn)由式(1)和式(16)組成。而構(gòu)成式(16)的矢量觀測(cè)α和β如式(17)(18)所示。對(duì)于逆向慣性系解析粗對(duì)準(zhǔn)，除了通過(guò)2.1 節(jié)的方法獲得測(cè)量數(shù)據(jù)外，β的獲取方式也與前向慣性系解析粗對(duì)準(zhǔn)不同。

由式(17)可知，構(gòu)成式(16)的矢量觀測(cè)β表示b0系下的速度變化量。如果直接使用式(17)來(lái)計(jì)算逆向慣性系解析粗對(duì)準(zhǔn)的矢量觀測(cè)β，那么會(huì)缺少由前向慣性系解析粗對(duì)準(zhǔn)到逆向慣性系解析粗對(duì)準(zhǔn)轉(zhuǎn)向時(shí)刻的速度變化，從而產(chǎn)生誤差，因此需要利用OD 輸出進(jìn)行矢量觀測(cè)修正。

在前向慣性系解析粗對(duì)準(zhǔn)轉(zhuǎn)向到逆向慣性系解析粗對(duì)準(zhǔn)時(shí)，b0系下的速度由變?yōu)榱?，因此在逆向慣性系解析粗對(duì)準(zhǔn)中，矢量觀測(cè)β應(yīng)修正為：

基于回溯過(guò)程的粗對(duì)準(zhǔn)步驟如下：首先進(jìn)行前向慣性系解析粗對(duì)準(zhǔn)，由式(16)(17)(18)求得，通過(guò)式(1)求得；然后進(jìn)行逆向過(guò)程測(cè)量數(shù)據(jù)的生成；最后進(jìn)行逆向慣性系解析粗對(duì)準(zhǔn)，由式(16)、式(18)和式(42)求得，通過(guò)式(1)求得。

相比于僅有前向的慣性系解析粗對(duì)準(zhǔn)，基于回溯過(guò)程的粗對(duì)準(zhǔn)增加了逆向慣性系解析粗對(duì)準(zhǔn)階段，延長(zhǎng)了粗對(duì)準(zhǔn)的時(shí)間。在初始對(duì)準(zhǔn)時(shí)間較短的情況下，基于回溯過(guò)程的粗對(duì)準(zhǔn)有利于提高粗對(duì)準(zhǔn)精度。

2.3 基于回溯過(guò)程的精對(duì)準(zhǔn)

為了實(shí)現(xiàn)快速高精度初始對(duì)準(zhǔn)，將回溯過(guò)程引入到精對(duì)準(zhǔn)之中。

由于粗對(duì)準(zhǔn)結(jié)束后得到了對(duì)準(zhǔn)開(kāi)始時(shí)刻的姿態(tài)矩陣，因此精對(duì)準(zhǔn)從對(duì)準(zhǔn)開(kāi)始時(shí)刻進(jìn)行?；诨厮葸^(guò)程的精對(duì)準(zhǔn)共進(jìn)行了三個(gè)階段：第一階段是前向卡爾曼濾波精對(duì)準(zhǔn)；第二階段是逆向卡爾曼濾波精對(duì)準(zhǔn)，所使用的是逆向過(guò)程的測(cè)量數(shù)據(jù)；第三階段再次進(jìn)行前向卡爾曼濾波精對(duì)準(zhǔn)。

因此，整個(gè)初始對(duì)準(zhǔn)過(guò)程包括兩個(gè)部分：基于回溯過(guò)程的兩階段粗對(duì)準(zhǔn)及基于回溯過(guò)程的三階段精對(duì)準(zhǔn)。初始對(duì)準(zhǔn)的完整示意圖如圖2 所示。

圖2 基于回溯過(guò)程的初始對(duì)準(zhǔn)示意圖Fig.2 Diagram of initial alignment based on backtracking process

前向粗對(duì)準(zhǔn)過(guò)后，需要一定的時(shí)間延遲Δt用于計(jì)算逆向粗對(duì)準(zhǔn)和三階段精對(duì)準(zhǔn)。對(duì)于現(xiàn)代導(dǎo)航計(jì)算機(jī)的強(qiáng)大計(jì)算能力，延遲時(shí)間很短。由于對(duì)準(zhǔn)測(cè)量數(shù)據(jù)總時(shí)長(zhǎng)t1遠(yuǎn)大于精對(duì)準(zhǔn)計(jì)算所需時(shí)間，因此時(shí)間延遲Δt后，第二次前向卡爾曼濾波精對(duì)準(zhǔn)可以實(shí)時(shí)輸出當(dāng)前時(shí)刻t2的姿態(tài)和位置，在精對(duì)準(zhǔn)結(jié)束后可以立即轉(zhuǎn)入導(dǎo)航階段。

基于回溯過(guò)程的初始對(duì)準(zhǔn)完整步驟如下：

（1）粗對(duì)準(zhǔn)

①進(jìn)行前向慣性系解析粗對(duì)準(zhǔn)，同時(shí)記錄陀螺儀、加速度計(jì)和OD 的測(cè)量數(shù)據(jù)作為前向數(shù)據(jù)；

②使用前向數(shù)據(jù)生成逆向過(guò)程數(shù)據(jù)；

③利用逆向過(guò)程數(shù)據(jù)進(jìn)行逆向慣性系解析粗對(duì)準(zhǔn)，并修正由回溯帶來(lái)的矢量觀測(cè)誤差。

（2）精對(duì)準(zhǔn)

①進(jìn)行前向卡爾曼濾波精對(duì)準(zhǔn)，速度和位置的狀態(tài)初始值設(shè)置為已知的初始速度和位置；

②利用逆向過(guò)程數(shù)據(jù)進(jìn)行逆向卡爾曼濾波精對(duì)準(zhǔn)；

③再次進(jìn)行前向卡爾曼濾波精對(duì)準(zhǔn)，同時(shí)利用已知的初始速度和位置對(duì)估計(jì)量進(jìn)行修正；

④當(dāng)前向卡爾曼濾波精對(duì)準(zhǔn)的回溯追趕上實(shí)時(shí)輸入后，即滿足導(dǎo)航狀態(tài)的轉(zhuǎn)換條件，可轉(zhuǎn)入導(dǎo)航階段。

3 仿真試驗(yàn)

3.1 仿真環(huán)境設(shè)置

通過(guò)仿真試驗(yàn)來(lái)評(píng)估所提出的初始對(duì)準(zhǔn)方法的性能。設(shè)置SINS 中陀螺儀零偏為0.02°/h，陀螺儀隨機(jī)游走為，加速度計(jì)偏值為100μg，加速度計(jì)隨機(jī)游走為。設(shè)置OD 俯仰和方位安裝誤差角分別為15'和10'，標(biāo)度因數(shù)誤差為0.005。設(shè)置對(duì)準(zhǔn)開(kāi)始時(shí)刻，前向粗對(duì)準(zhǔn)的初始航向角誤差為30°。

考慮到載體的轉(zhuǎn)向與速度變化可以提高狀態(tài)變量的可觀測(cè)性，設(shè)計(jì)了車輛運(yùn)動(dòng)軌跡如圖3 所示，車輛的姿態(tài)和速度如圖4-5 所示，其中θ、γ和ψ分別表示車輛的俯仰角、橫滾角和航向角；VE、VN和VU分別表示車輛的東向速度、北向速度和天向速度。車輛在對(duì)準(zhǔn)開(kāi)始時(shí)刻速度為零，開(kāi)始對(duì)準(zhǔn)后車輛立即行駛，對(duì)準(zhǔn)時(shí)間共180s。車輛在90s 時(shí)航向角發(fā)生變化，變化幅度為5°。

圖3 車輛的運(yùn)動(dòng)軌跡Fig.3 Trajectory of the vehicle

圖4 車輛的運(yùn)動(dòng)姿態(tài)Fig.4 Attitude of the vehicle

圖5 車輛的運(yùn)動(dòng)速度Fig.5 Velocity of the vehicle

3.2 不同行進(jìn)間粗對(duì)準(zhǔn)方法的比較

根據(jù)仿真得到的IMU 和OD 測(cè)量數(shù)據(jù)，使用慣性系解析粗對(duì)準(zhǔn)方法（OBA）[10]和本文所提出基于回溯過(guò)程的粗對(duì)準(zhǔn)方法（OBA Based on Backtracking Process,BOBA）進(jìn)行粗對(duì)準(zhǔn)。兩種方法的姿態(tài)誤差、速度誤差和位置誤差分別如圖6-8 所示。圖的左側(cè)表示前向慣性系解析粗對(duì)準(zhǔn)方法，即OBA 方法；圖的右側(cè)表示逆向慣性系解析粗對(duì)準(zhǔn)方法，圖的左側(cè)和右側(cè)共同構(gòu)成BOBA 方法。兩種方法結(jié)束時(shí)刻的姿態(tài)誤差、速度誤差和位置誤差如表1 所示。

表1 不同行進(jìn)間粗對(duì)準(zhǔn)方法的對(duì)準(zhǔn)結(jié)果Tab.1 Alignment results of different coarse alignment methods

由圖6-8 可以看出，OBA 方法和BOBA 方法都可以完成行進(jìn)間初始對(duì)準(zhǔn)。這是由于兩種方法都通過(guò)式(22)實(shí)現(xiàn)每一時(shí)刻的位置更新進(jìn)而求取姿態(tài)，因此都適用于行進(jìn)間的粗對(duì)準(zhǔn)。而對(duì)于對(duì)準(zhǔn)精度，無(wú)論姿態(tài)誤差、速度誤差和位置誤差，BOBA 方法的誤差均小于OBA 方法。這是由于在初始對(duì)準(zhǔn)時(shí)間較短的情況下，OBA 方法的對(duì)準(zhǔn)結(jié)果無(wú)法充分收斂，因此對(duì)準(zhǔn)精度較低。而B(niǎo)OBA 方法使用相同的測(cè)量數(shù)據(jù)又進(jìn)行了一個(gè)逆向過(guò)程粗對(duì)準(zhǔn)，延長(zhǎng)了一倍的對(duì)準(zhǔn)時(shí)間，可以使對(duì)準(zhǔn)結(jié)果繼續(xù)收斂，因此對(duì)準(zhǔn)精度高。

圖6 兩種方法的姿態(tài)誤差Fig.6 Attitude error of the two methods

圖7 兩種方法的速度誤差Fig.7 Velocity error of the two methods

圖8 兩種方法的位置誤差Fig.8 Position error of the two methods

表1 中BOBA 方法位置誤差較小的原因在于經(jīng)回溯過(guò)程粗對(duì)準(zhǔn)后，OD 標(biāo)度因數(shù)誤差引起的前向位置誤差與逆向位置誤差相抵消，最終計(jì)算得到的位置接近初始時(shí)刻位置，因此位置誤差較小。

3.3 不同行進(jìn)間精對(duì)準(zhǔn)方法的比較

根據(jù)仿真得到的IMU 和OD 測(cè)量數(shù)據(jù)，比較了傳統(tǒng)的基于回溯過(guò)程的精對(duì)準(zhǔn)方法（Kalman Filter Based on Backtracking Process,BKF）[10]和本文改進(jìn)的基于回溯過(guò)程的精對(duì)準(zhǔn)方法（Improved Kalman Filter Based on Backtracking Process,IBKF）。為了進(jìn)一步提高精對(duì)準(zhǔn)的精度，使用3.2 節(jié)得到的各自粗對(duì)準(zhǔn)結(jié)果初始化兩種方法的濾波參數(shù)。同時(shí)為了便于比較，省略了非常短的延遲時(shí)間Δt。圖9 所示為兩種方法OD 相關(guān)誤差的估計(jì)與誤差真值，包括OD 俯仰安裝誤差角、方位安裝誤差角和標(biāo)度因數(shù)誤差。兩種方法的姿態(tài)誤差、速度誤差和位置誤差如圖10-12 所示，BKF 先從180s開(kāi)始進(jìn)行逆向卡爾曼濾波精對(duì)準(zhǔn)，然后進(jìn)行前向卡爾曼濾波精對(duì)準(zhǔn)。IBKF 先由0 時(shí)刻開(kāi)始進(jìn)行前向卡爾曼濾波精對(duì)準(zhǔn)，然后進(jìn)行逆向卡爾曼濾波精對(duì)準(zhǔn)和第二次前向卡爾曼濾波精對(duì)準(zhǔn)。兩種方法對(duì)準(zhǔn)結(jié)束時(shí)刻的姿態(tài)誤差、速度誤差和位置誤差如表2 所示。

圖9 兩種方法的OD 相關(guān)誤差Fig.9 OD related error of the two methods

圖10 兩種方法的姿態(tài)誤差Fig.10 Attitude error of the two methods

表2 不同行進(jìn)間精對(duì)準(zhǔn)方法的對(duì)準(zhǔn)結(jié)果Tab.2 Alignment results of different fine alignment methods

由圖9 可以看出，兩種方法均可以估計(jì)出OD 相關(guān)誤差，其中IBKF 的估計(jì)精度高于BKF。由圖10–圖12 可以看出，IBKF 的對(duì)準(zhǔn)效果優(yōu)于BKF。這主要是由于初始對(duì)準(zhǔn)時(shí)間較短，BKF 的誤差無(wú)法充分收斂，而IBKF 經(jīng)過(guò)兩次前向卡爾曼濾波和一次逆向卡爾曼濾波，延長(zhǎng)了對(duì)準(zhǔn)時(shí)間，使得誤差曲線更為穩(wěn)定，估計(jì)精度更高。在圖11-12 中，曲線在0 時(shí)刻有瞬間的跳變，這主要是由于在逆向卡爾曼濾波轉(zhuǎn)到前向卡爾曼濾波時(shí)，利用已知的初始速度和位置進(jìn)行了狀態(tài)修正，因此誤差會(huì)有跳變。

圖11 兩種方法的速度誤差Fig.11 Velocity error of the two methods

圖12 兩種方法的位置誤差Fig.12 Position error of the two methods

同時(shí)由表2 可以看出，IBKF 的位置估計(jì)誤差明顯小于BKF。這主要是因?yàn)镮BKF 首次精對(duì)準(zhǔn)采用的是前向精對(duì)準(zhǔn)，其初始速度和位置為精確的速度和位置。而B(niǎo)KF 首次精對(duì)準(zhǔn)采用逆向精對(duì)準(zhǔn)濾波，其初始速度和位置是由粗對(duì)準(zhǔn)提供的，存在誤差，因此BKF 的估計(jì)精度要低于IBKF。

綜上所述，與現(xiàn)有的行進(jìn)間粗對(duì)準(zhǔn)和精對(duì)準(zhǔn)方法相比，本文方法可以快速自主地獲得精度更高的姿態(tài)和位置對(duì)準(zhǔn)結(jié)果。相比于現(xiàn)有方法，所提方法航向角估計(jì)精度提高了45.1%，位置估計(jì)精度提高了58.1%。

4 結(jié)論

本文提出了一種高精度OD 輔助的SINS 行進(jìn)間對(duì)準(zhǔn)方法。通過(guò)將回溯過(guò)程引入到粗對(duì)準(zhǔn)和精對(duì)準(zhǔn)中，粗對(duì)準(zhǔn)和精對(duì)準(zhǔn)都獲得了較高的對(duì)準(zhǔn)精度。在粗對(duì)準(zhǔn)中，通過(guò)改進(jìn)的慣性系解析對(duì)準(zhǔn)算法，減小了由位置變化引起的粗對(duì)準(zhǔn)誤差。同時(shí)，通過(guò)載體轉(zhuǎn)向時(shí)矢量觀測(cè)的修正，提高了回溯過(guò)程的精度。在精對(duì)準(zhǔn)中，為了保證對(duì)準(zhǔn)結(jié)果充分收斂以及對(duì)準(zhǔn)過(guò)程的連續(xù)性，引入了兩次前向精對(duì)準(zhǔn)和一次逆向精對(duì)準(zhǔn)，等效延長(zhǎng)對(duì)準(zhǔn)時(shí)間達(dá)到提高精對(duì)準(zhǔn)估計(jì)精度的目的。仿真試驗(yàn)證明了所提出的行進(jìn)間對(duì)準(zhǔn)方法的正確性和有效性。相比于傳統(tǒng)前向?qū)?zhǔn)方法，在一定程度上可縮短對(duì)準(zhǔn)時(shí)間，提高初始對(duì)準(zhǔn)精度。在載體航向變化明顯的機(jī)動(dòng)條件下，本文方法有很好的應(yīng)用前景。