一種高精度自主式組合導航系統濾波算法

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

（國防科技大學 智能科學學院，長沙 410073）

捷聯慣導系統（Strap-down Inertial Navigation System，SINS）與全球衛星導航系統（Global Navigation Satellite System，GNSS）的組合是車載導航定位系統中最常用的組合方式[1-3]，可以提供長時間的高精度輸出，滿足車輛對實時定位的要求。然而，GNSS 設備的抗干擾能力較差，當處于復雜電磁干擾或信號遮擋環境下時難以提供有效和連續的測量信息，不能滿足車輛適應性與自主性的要求。

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

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

1 基于ST-EKF 的慣性/里程計/氣壓高度計系統方程

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

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

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

ST-EKF 中將速度誤差在同一坐標系下進行了更嚴格的定義[8-12]：

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

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

1.2 基于ST-EKF 的系統誤差狀態模型

定義SINS/OD/Baro 組合導航系統的21 維誤差狀態向量x：

可以構建SINS/OD/Baro 組合導航系統的誤差狀態方程為：

式中x為誤差狀態向量，F為系統矩陣，G為噪聲轉移矩陣，w為過程噪聲向量，它們具體定義如下[8-12]：

2 基于ST-EKF 的慣性/里程計/氣壓高度計組合導航量測方程

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

圖1 組合導航系統原理圖Fig.1 Schematic diagram of integrated navigation system

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

其中,αδ是捷聯慣導系統與車體之間的安裝誤差。

由捷聯慣導系統解算的速度在m 系下的投影可以表達為：

將SINS 解算出的速度信息投影到里程計坐標系下與里程計的速度相減作為系統的速度觀測量，即：

在標準大氣模型下，氣壓高度計測量公式如下[13]：

在實驗過程中，氣壓高度計采集當地實時氣壓值和溫度值數據，以及基準點的氣壓值和溫度值，由公式（19）可得氣壓高度，將慣導解算得到的高程信息與該量測高度相減，作為SINS/OD/Baro 組合導航系統的高程觀測量，即：

結合式(18)與式(20)，可以得到基于ST-EKF 的SINS/OD/Baro 組合導航系統的觀測量：

由于基于ST-EKF 的SINS/OD 組合導航系統濾波估計的是新的速度誤差狀態，所以濾波后速度狀態的更新公式應為[8-11]：

因為ST-EKF 中姿態失準角和位置誤差狀態的定義未作改變，所以姿態和位置的校正與傳統的EKF 一致。

3 實驗結果與分析

3.1 實驗設計

為了檢驗本文提出的基于 ST-EKF 的SINS/OD/Baro 組合導航算法的應用效果，對真實的長行駛里程車載實驗數據進行事后處理，并與同等實驗條件下基于EKF 的SINS/OD 組合導航算法、基于ST-EKF 的SINS/OD 組合導航算法的水平定位精度、高程解算結果作出了對比。

為了使事后處理結果更具有參考價值，本次實驗分別采用了兩組不同行駛條件下的實驗數據，其中一組來自于一次沿長沙市環城高速的車載實驗，行車時間約為3.7 h，行駛里程總計154 km，在百度衛星地圖下的行車軌跡如圖2所示；另一組來自于一次長沙市至岳陽市的長里程行駛車載實驗，行車時間約為4.2 h，行駛里程總計235 km，在百度衛星地圖下的行車軌跡如圖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)

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

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

其中，IMU 的輸出頻率為200 Hz，陀螺零偏穩定性為0.002 ° /h，角度隨機游走為；加速度計零偏穩定性為50 μg，速度隨機游走為。在跑車實驗中，車輛靜止180 s 進行初始對準。

組合導航過程中采用的氣壓高度測量值由MS5611 型氣壓傳感器提供的氣壓、溫度數據解算得到，兩組實驗中該測量值與基準高度的對比分別如圖5、圖6所示。

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

本節中采用的基準位置由慣性/差分衛星組合導航事后平滑算法[12]解算得到，數據用RTS 表示。精度在1 m 以內。

由圖5、圖6 可以看出，在不同的行駛環境下，氣壓高度計呈現出不同的測量精度，這也將影響組合后導航系統的定位精度。本章將分別針對兩組實驗數據的解算結果，論證該算法在不同行駛環境下的應用效果以及相對傳統組合導航算法的改進效果。

3.2 長沙市環城高速車載實驗

圖7 為第一組實驗中，三種組合導航算法解算得到的水平定位結果，分別相對于基準位置的定位誤差和精度隨時間以及行駛里程變化而產生的變化趨勢。

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

圖8 為第一組實驗中，三種組合導航算法解算得到的高程以及基準高度隨時間的變化曲線。

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

觀察圖7、圖8 可以發現，采用ST-EKF 替換傳統的EKF 框架后，SINS/OD 組合導航算法的水平和高程定位精度均得到提高。而在引入氣壓高度計的量測信息后，基于ST-EKF 的SINS/OD/Baro 組合導航算法在保持較高的水平定位精度的同時，高程定位精度也獲得了大幅度提升。

為了更客觀地對比評價三種組合導航算法的導航定位精度，對三種算法的解算結果作出了量化統計，其中，表1 統計了第一組實驗中三種算法全程水平定位結果的均方根誤差（RMSE）以及RMSE 占總行駛里程的百分比，表2 則統計了第一組實驗中三種算法全程高程定位結果的RMSE。

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

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

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

結合第一組實驗的實驗結果可以發現，在環城高速的行駛過程中，氣候條件變化較小，氣壓高度計具有較好的測量精度，其測量結果引入本文提出的高精度組合導航算法，對導航系統的高程通道起到了較好的阻尼效果，有效地抑制高程定位誤差的發散趨勢，提高了系統的導航定位精度。

3.3 長沙市至岳陽市直線行駛車載實驗

與第一組實驗不同，第二組是一組跨地市的長里程直線行駛車載實驗，行駛環境更加復雜，氣候條件變化較大，氣壓高度計產生了較高的測量誤差，對導航算法的解算精度提出了更高的要求。

與圖7、圖8 相對應，圖9、圖10 分別為第二組實驗中三種組合導航算法的水平定位結果、高程定位結果對比圖。

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

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

表3、表4 則分別量化統計了第二組實驗中三種算法全程的水平定位誤差和高程定位誤差。

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

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

結合圖9-10，表3-4 可以發現，在跨地市的長里程行駛環境中，本文提出的高精度組合導航算法的水平定位精度依然相對傳統的導航算法提高了21%，并且在氣壓高度量測數據不太理想的情況下，減緩了系統高程定位誤差發散的趨勢，進一步證明了該算法在不同行駛環境下的普適性。

4 結論

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