一種高精度自主式組合導(dǎo)航系統(tǒng)濾波算法

杜學(xué)禹，王茂松，崔加瑞，吳文啟，何曉峰

（國防科技大學(xué) 智能科學(xué)學(xué)院，長沙 410073）

捷聯(lián)慣導(dǎo)系統(tǒng)（Strap-down Inertial Navigation System，SINS）與全球衛(wèi)星導(dǎo)航系統(tǒng)（Global Navigation Satellite System，GNSS）的組合是車載導(dǎo)航定位系統(tǒng)中最常用的組合方式[1-3]，可以提供長時(shí)間的高精度輸出，滿足車輛對(duì)實(shí)時(shí)定位的要求。然而，GNSS 設(shè)備的抗干擾能力較差，當(dāng)處于復(fù)雜電磁干擾或信號(hào)遮擋環(huán)境下時(shí)難以提供有效和連續(xù)的測(cè)量信息，不能滿足車輛適應(yīng)性與自主性的要求。

與GNSS 相比，輪式里程計(jì)（Odometer，OD）能自主地測(cè)量載車的行駛速度和里程，抗干擾性能較好，與SINS 進(jìn)行組合有助于提高車載導(dǎo)航系統(tǒng)的自主導(dǎo)航能力和復(fù)雜環(huán)境下的定位精度[4-6]。但是，傳統(tǒng)的SINS/OD 組合導(dǎo)航系統(tǒng)中，高程定位結(jié)果在缺乏外源信息約束的情況下，容易隨著時(shí)間的推移或路程的累積而逐步發(fā)散。同時(shí)，基于EKF 的慣性基組合導(dǎo)航系統(tǒng)在動(dòng)態(tài)環(huán)境下易出現(xiàn)方差估計(jì)不一致的問題[7]。

為解決上述問題，本文提出了一種基于狀態(tài)變換卡爾曼濾波（State Transformation Extended Kalman Filter,ST-EKF）的慣性/里程計(jì)/氣壓高度計(jì)組合導(dǎo)航算法。與傳統(tǒng)的SINS/OD 組合導(dǎo)航算法相比，該算法首先是采用濾波魯棒性更好的ST-EKF 替代擴(kuò)展卡爾曼濾波（Extended Kalman Filter,EKF）。ST-EKF 對(duì)系統(tǒng)的速度誤差方程進(jìn)行了更嚴(yán)格的定義[8-12]，消除了易受動(dòng)態(tài)環(huán)境干擾的比力項(xiàng)，能有效避免方差不一致問題的產(chǎn)生；其次是利用氣壓高度計(jì)（Barometer，Baro）的量測(cè)信息對(duì)組合導(dǎo)航系統(tǒng)的高程通道進(jìn)行阻尼。氣壓高度計(jì)利用氣壓表測(cè)量載體周圍的大氣壓力，與測(cè)量基準(zhǔn)點(diǎn)的表面溫度和壓力進(jìn)行對(duì)比進(jìn)而估算載體的高度信息，具有較好的高程測(cè)量精度，能有效約束導(dǎo)航系統(tǒng)的高程定位結(jié)果，解決傳統(tǒng)的自主導(dǎo)航算法高程定位誤差易發(fā)散的問題。

1 基于ST-EKF 的慣性/里程計(jì)/氣壓高度計(jì)系統(tǒng)方程

狀態(tài)變換卡爾曼濾波（ST-EKF）在文獻(xiàn)[8]中首次提出，并被擴(kuò)展應(yīng)用在SINS/GNSS、SINS/OD、捷聯(lián)慣性/多普勒測(cè)速儀（SINS/DVL）等組合導(dǎo)航系統(tǒng)中[8-11],實(shí)驗(yàn)表明，ST-EKF 在動(dòng)態(tài)環(huán)境下比EKF 具有更好的濾波魯棒性[8-12]。本節(jié)推導(dǎo)解釋了ST-EKF 框架下的新的速度誤差方程，并依此重新定義了系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣，建立了基于ST-EKF 的SINS/OD/Baro組合導(dǎo)航系統(tǒng)的誤差狀態(tài)模型[8-12]。

1.1 基于ST-EKF 的速度誤差定義

常用的線性速度誤差微分方程可以定義為：

ST-EKF 中將速度誤差在同一坐標(biāo)系下進(jìn)行了更嚴(yán)格的定義[8-12]：

對(duì)新的速度誤差定義求微分可以得到：

從式（3）中可以看出，ST-EKF 中的速度誤差微分方程用重力相關(guān)項(xiàng)替代了比力項(xiàng)，而對(duì)于一般的車載導(dǎo)航過程來說，幾乎為常值，可以有效避免動(dòng)態(tài)環(huán)境中比力量化噪聲和姿態(tài)的劇烈變化帶來的計(jì)算不精確的問題，提高濾波的魯棒性。

1.2 基于ST-EKF 的系統(tǒng)誤差狀態(tài)模型

定義SINS/OD/Baro 組合導(dǎo)航系統(tǒng)的21 維誤差狀態(tài)向量x：

可以構(gòu)建SINS/OD/Baro 組合導(dǎo)航系統(tǒng)的誤差狀態(tài)方程為：

式中x為誤差狀態(tài)向量，F(xiàn)為系統(tǒng)矩陣，G為噪聲轉(zhuǎn)移矩陣，w為過程噪聲向量，它們具體定義如下[8-12]：

2 基于ST-EKF 的慣性/里程計(jì)/氣壓高度計(jì)組合導(dǎo)航量測(cè)方程

為設(shè)計(jì)出能夠精確快速地定位定向，并具有良好的自主性、適應(yīng)性和可靠性的車載導(dǎo)航系統(tǒng)，本文引入輪式里程計(jì)與氣壓高度計(jì)作為輔助導(dǎo)航設(shè)備，利用二者提供的量測(cè)信息，限制慣性導(dǎo)航系統(tǒng)中誤差的累積，從而改進(jìn)導(dǎo)航性能。本文設(shè)計(jì)的組合導(dǎo)航系統(tǒng)的結(jié)構(gòu)原理如圖1所示：

圖1 組合導(dǎo)航系統(tǒng)原理圖Fig.1 Schematic diagram of integrated navigation system

假設(shè)IMU 系b 與車體系m 之間的安裝角是小角度,則方向余弦矩陣可以寫為：

其中,αδ是捷聯(lián)慣導(dǎo)系統(tǒng)與車體之間的安裝誤差。

由捷聯(lián)慣導(dǎo)系統(tǒng)解算的速度在m 系下的投影可以表達(dá)為：

將SINS 解算出的速度信息投影到里程計(jì)坐標(biāo)系下與里程計(jì)的速度相減作為系統(tǒng)的速度觀測(cè)量，即：

在標(biāo)準(zhǔn)大氣模型下，氣壓高度計(jì)測(cè)量公式如下[13]：

在實(shí)驗(yàn)過程中，氣壓高度計(jì)采集當(dāng)?shù)貙?shí)時(shí)氣壓值和溫度值數(shù)據(jù)，以及基準(zhǔn)點(diǎn)的氣壓值和溫度值，由公式（19）可得氣壓高度，將慣導(dǎo)解算得到的高程信息與該量測(cè)高度相減，作為SINS/OD/Baro 組合導(dǎo)航系統(tǒng)的高程觀測(cè)量，即：

結(jié)合式(18)與式(20)，可以得到基于ST-EKF 的SINS/OD/Baro 組合導(dǎo)航系統(tǒng)的觀測(cè)量：

由于基于ST-EKF 的SINS/OD 組合導(dǎo)航系統(tǒng)濾波估計(jì)的是新的速度誤差狀態(tài)，所以濾波后速度狀態(tài)的更新公式應(yīng)為[8-11]：

因?yàn)镾T-EKF 中姿態(tài)失準(zhǔn)角和位置誤差狀態(tài)的定義未作改變，所以姿態(tài)和位置的校正與傳統(tǒng)的EKF 一致。

3 實(shí)驗(yàn)結(jié)果與分析

3.1 實(shí)驗(yàn)設(shè)計(jì)

為了檢驗(yàn)本文提出的基于 ST-EKF 的SINS/OD/Baro 組合導(dǎo)航算法的應(yīng)用效果，對(duì)真實(shí)的長行駛里程車載實(shí)驗(yàn)數(shù)據(jù)進(jìn)行事后處理，并與同等實(shí)驗(yàn)條件下基于EKF 的SINS/OD 組合導(dǎo)航算法、基于ST-EKF 的SINS/OD 組合導(dǎo)航算法的水平定位精度、高程解算結(jié)果作出了對(duì)比。

為了使事后處理結(jié)果更具有參考價(jià)值，本次實(shí)驗(yàn)分別采用了兩組不同行駛條件下的實(shí)驗(yàn)數(shù)據(jù)，其中一組來自于一次沿長沙市環(huán)城高速的車載實(shí)驗(yàn)，行車時(shí)間約為3.7 h，行駛里程總計(jì)154 km，在百度衛(wèi)星地圖下的行車軌跡如圖2所示；另一組來自于一次長沙市至岳陽市的長里程行駛車載實(shí)驗(yàn)，行車時(shí)間約為4.2 h，行駛里程總計(jì)235 km，在百度衛(wèi)星地圖下的行車軌跡如圖3所示。

圖2 二維車輛行駛軌跡圖（第一組）Fig.2 Two dimensional trajectory of the land vehicle experiment(Group 1)

圖3 二維車輛行駛軌跡圖（第二組）Fig.3 Two dimensional trajectory of the land vehicle experiment(Group 2)

實(shí)驗(yàn)車輛配備了導(dǎo)航級(jí)的高精度光纖陀螺IMU、標(biāo)度因數(shù)約為898 p/m 的輪式里程計(jì)和MS5611 型氣壓傳感器，三種傳感器進(jìn)行組合導(dǎo)航的濾波頻率為1 Hz。實(shí)驗(yàn)車輛的傳感器配置和工作模式如圖4所示。

圖4 車載實(shí)驗(yàn)傳感器配置及工作模式Fig.4 Sensor configurations and working mode of the land vehicle field test.

其中，IMU 的輸出頻率為200 Hz，陀螺零偏穩(wěn)定性為0.002 ° /h，角度隨機(jī)游走為；加速度計(jì)零偏穩(wěn)定性為50 μg，速度隨機(jī)游走為。在跑車實(shí)驗(yàn)中，車輛靜止180 s 進(jìn)行初始對(duì)準(zhǔn)。

組合導(dǎo)航過程中采用的氣壓高度測(cè)量值由MS5611 型氣壓傳感器提供的氣壓、溫度數(shù)據(jù)解算得到，兩組實(shí)驗(yàn)中該測(cè)量值與基準(zhǔn)高度的對(duì)比分別如圖5、圖6所示。

圖5 氣壓高度計(jì)輸出高度與基準(zhǔn)高度對(duì)比（第一組）Fig.5 Comparison of barometric altimeter output height and reference height(Group 1)

本節(jié)中采用的基準(zhǔn)位置由慣性/差分衛(wèi)星組合導(dǎo)航事后平滑算法[12]解算得到，數(shù)據(jù)用RTS 表示。精度在1 m 以內(nèi)。

由圖5、圖6 可以看出，在不同的行駛環(huán)境下，氣壓高度計(jì)呈現(xiàn)出不同的測(cè)量精度，這也將影響組合后導(dǎo)航系統(tǒng)的定位精度。本章將分別針對(duì)兩組實(shí)驗(yàn)數(shù)據(jù)的解算結(jié)果，論證該算法在不同行駛環(huán)境下的應(yīng)用效果以及相對(duì)傳統(tǒng)組合導(dǎo)航算法的改進(jìn)效果。

3.2 長沙市環(huán)城高速車載實(shí)驗(yàn)

圖7 為第一組實(shí)驗(yàn)中，三種組合導(dǎo)航算法解算得到的水平定位結(jié)果，分別相對(duì)于基準(zhǔn)位置的定位誤差和精度隨時(shí)間以及行駛里程變化而產(chǎn)生的變化趨勢(shì)。

圖7 水平定位結(jié)果對(duì)比（第一組）Fig.7 Comparison of horizontal positioning results(Group 1)

圖8 為第一組實(shí)驗(yàn)中，三種組合導(dǎo)航算法解算得到的高程以及基準(zhǔn)高度隨時(shí)間的變化曲線。

圖8 高程定位結(jié)果對(duì)比（第一組）Fig.8 Comparison of height positioning results(Group 1)

觀察圖7、圖8 可以發(fā)現(xiàn)，采用ST-EKF 替換傳統(tǒng)的EKF 框架后，SINS/OD 組合導(dǎo)航算法的水平和高程定位精度均得到提高。而在引入氣壓高度計(jì)的量測(cè)信息后，基于ST-EKF 的SINS/OD/Baro 組合導(dǎo)航算法在保持較高的水平定位精度的同時(shí)，高程定位精度也獲得了大幅度提升。

為了更客觀地對(duì)比評(píng)價(jià)三種組合導(dǎo)航算法的導(dǎo)航定位精度，對(duì)三種算法的解算結(jié)果作出了量化統(tǒng)計(jì)，其中，表1 統(tǒng)計(jì)了第一組實(shí)驗(yàn)中三種算法全程水平定位結(jié)果的均方根誤差（RMSE）以及RMSE 占總行駛里程的百分比，表2 則統(tǒng)計(jì)了第一組實(shí)驗(yàn)中三種算法全程高程定位結(jié)果的RMSE。

表1 水平定位誤差統(tǒng)計(jì)表（第一組）Tab.1 Statistical table of horizontal positioning error(Group 1)

表2 高程誤差統(tǒng)計(jì)表（第一組）Tab.2 Statistical table of height error(Group 1)

觀察表1、表2 可以發(fā)現(xiàn)，第一組實(shí)驗(yàn)中基于EKF的SINS/OD 組合導(dǎo)航算法全程的水平定位結(jié)果的RMSE 為 27.0841 m，高程定位結(jié)果的 RMSE 為28.7542 m。而基于ST-EKF 的SINS/OD/Baro 組合導(dǎo)航算法全程的水平定位結(jié)果的RMSE 為17.7685 m，高程定位結(jié)果的RMSE 為6.6669 m，兩種RMSE 分別減少了34.40%和76.81%。

結(jié)合第一組實(shí)驗(yàn)的實(shí)驗(yàn)結(jié)果可以發(fā)現(xiàn)，在環(huán)城高速的行駛過程中，氣候條件變化較小，氣壓高度計(jì)具有較好的測(cè)量精度，其測(cè)量結(jié)果引入本文提出的高精度組合導(dǎo)航算法，對(duì)導(dǎo)航系統(tǒng)的高程通道起到了較好的阻尼效果，有效地抑制高程定位誤差的發(fā)散趨勢(shì)，提高了系統(tǒng)的導(dǎo)航定位精度。

3.3 長沙市至岳陽市直線行駛車載實(shí)驗(yàn)

與第一組實(shí)驗(yàn)不同，第二組是一組跨地市的長里程直線行駛車載實(shí)驗(yàn)，行駛環(huán)境更加復(fù)雜，氣候條件變化較大，氣壓高度計(jì)產(chǎn)生了較高的測(cè)量誤差，對(duì)導(dǎo)航算法的解算精度提出了更高的要求。

與圖7、圖8 相對(duì)應(yīng)，圖9、圖10 分別為第二組實(shí)驗(yàn)中三種組合導(dǎo)航算法的水平定位結(jié)果、高程定位結(jié)果對(duì)比圖。

圖9 水平定位結(jié)果對(duì)比（第二組）Fig.9 Comparison of horizontal positioning results(Group 2)

圖10 高程定位結(jié)果對(duì)比（第二組）Fig.10 Comparison of height positioning results(Group 2)

表3、表4 則分別量化統(tǒng)計(jì)了第二組實(shí)驗(yàn)中三種算法全程的水平定位誤差和高程定位誤差。

表3 水平定位誤差統(tǒng)計(jì)表（第二組）Tab.3 Statistical table of horizontal positioning error(Group 2)

表4 高程誤差統(tǒng)計(jì)表（第二組）Tab.4 Statistical table of height error(Group 2)

結(jié)合圖9-10，表3-4 可以發(fā)現(xiàn)，在跨地市的長里程行駛環(huán)境中，本文提出的高精度組合導(dǎo)航算法的水平定位精度依然相對(duì)傳統(tǒng)的導(dǎo)航算法提高了21%，并且在氣壓高度量測(cè)數(shù)據(jù)不太理想的情況下，減緩了系統(tǒng)高程定位誤差發(fā)散的趨勢(shì)，進(jìn)一步證明了該算法在不同行駛環(huán)境下的普適性。

4 結(jié)論

針對(duì)衛(wèi)星拒止的條件下，長行駛里程任務(wù)中的導(dǎo)航定位精度問題，本文提出了一種基于ST-EKF 的SINS/OD/Baro 高精度車載自主導(dǎo)航濾波算法。實(shí)測(cè)車載實(shí)驗(yàn)數(shù)據(jù)的事后處理表明，該算法在不同的行駛環(huán)境中均能保持較高的導(dǎo)航定位精度，全程的水平定位精度優(yōu)于0.05%D。同時(shí)，該算法能有效利用氣壓高度計(jì)的量測(cè)值對(duì)導(dǎo)航系統(tǒng)的高程通道進(jìn)行阻尼，改善了傳統(tǒng)的基于EKF 的SINS/OD 組合導(dǎo)航系統(tǒng)高程定位誤差易發(fā)散的問題。因此，該算法可以幫助車輛在缺乏衛(wèi)星信號(hào)校正的復(fù)雜環(huán)境下，迅速確定自身所處區(qū)域，使得視覺等傳感器的后續(xù)處理能更加精確，具有良好的工程應(yīng)用價(jià)值。