一種基于ST-EKF的慣性/里程計滑動窗口濾波技術

杜學禹，王茂松，吳文啟，張 靖

(1.國防科技大學智能科學學院，長沙 410073；2.北京航天發射技術研究所，北京 100076)

0 引言

近年來，隨著智能化水平的不斷提高，無人駕駛技術取得了飛速發展，Google、特斯拉、豐田等廠商紛紛將高新的科研成果投入市場，給人們的生活帶來了許多便利。但是，現階段的無人駕駛技術遠沒有達到十分完善的地步。目前，無人駕駛車輛的導航定位技術廣泛使用全球導航衛星系統(Global Navigation Satellite System，GNSS)。在惡劣天氣、植被遮擋、缺乏基站覆蓋等復雜野外環境下，GNSS難以提供穩定、有效的信號。因此，如何構建在衛星拒止條件下具有更高精度的自主導航定位系統是推動無人駕駛技術進一步發展的關鍵問題。

與GNSS相比，輪式里程計(Odometer，OD)不需要對外發射接收信號，可以通過測量車輪轉過的角度獨立提供車輛的行駛速度和路程，對外部因素的抗干擾能力較強，與捷聯慣導系統(Strapdown Inertial Navigation System，SINS)結合使用有助于提高復雜環境下車載導航系統的定位精度[1-3]。但是，SINS／OD組合導航系統在缺乏GNSS等外源信息校正時，系統的位置定位誤差會隨著時間的推移或路程的增長逐漸累積，使最終解算得到的行駛軌跡偏離實際的路線。同時，當導航平臺處于復雜外部環境時，傳統的基于擴展Kalman濾波(EKF)的捷聯慣性組合導航系統容易出現方差估計不一致的問題[4]，這也將影響車輛的導航定位精度。

RTS平滑算法[5]是Rauth等人基于極大似然估計準則推導出的，對一個區間內的狀態估計和誤差協方差進行最優平滑的固定區間事后處理方法。相比于單向濾波只能基于當前量測信息來估計當前狀態，RTS平滑算法則可以利用區間內所有的量測信息對任一時刻狀態進行估計，常用于慣性衛星組合導航的高精度數據處理與衛星的事后基準確定[6-7]。文獻[8]將RTS平滑器與狀態變換Kalman濾波(ST-EKF)[9-11]結合運用在水下自主潛航器(Autonomous Underwater Vehicle，AUV)的水下勘測航跡修正，有效校正了水下航行過程中AUV的位置誤差。因此，引入RTS平滑器是一種改善復雜環境下無人駕駛車輛導航定位精度的可行方法。但是，由于傳統的RTS平滑器多用于數據的事后處理，缺乏對航跡校正的實時性。同時，傳統的RTS平滑器對于平滑區間的選取不夠靈活，有時不能發揮出最佳的濾波效果。

本文在基于ST-EKF的SINS／OD組合導航算法[11]基礎上，對RTS平滑器的實時性做出改進，提出了一種基于ST-EKF的慣性／里程計滑動窗口濾波技術。先利用系統量測信息進行前向濾波，濾波過程中采用濾波魯棒性更好的ST-EKF替代傳統的EKF，并將解算得到的誤差狀態向量、誤差協方差和狀態轉移矩陣等數據實時存儲在滑動窗口中。當滑動窗口中儲存的信息達到設定長度后，系統利用存儲數據對區間內的所有狀態進行最優平滑。在采用該濾波技術時，可以根據車輛運行軌跡實時改變滑動窗口的長度以取得最優航跡修正效果。

1 基于ST-EKF的慣性/里程計組合導航模型

1.1 ST-EKF與EKF模型對比

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

式(2)中，gn為當地重力矢量，εb為陀螺測量零偏，Δb為加速度計測量零偏，wg和wa分別為陀螺和加速度計誤差的白噪聲向量。

由式(2)可知，其不再含有比力項，而是由重力相關項替代。對于一般的車載導航來說，gn的變化很小。因此，基于ST-EKF的組合導航可以有效避免由于比力量化噪聲和姿態的劇烈變化帶來的計算不精確的問題，避免了誤差的累計，確保了濾波的魯棒性。

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

定義慣性／里程計組合導航系統的21維誤差狀態向量x

可以構建慣性／里程計組合導航系統的誤差狀態方程

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

式(5)～式(7)中，τδk為里程計刻度因子誤差相關時間，τδαy為俯仰安裝誤差角相關時間，w中元素分別為陀螺零偏噪聲、加速度計零偏噪聲、刻度因子誤差Markov過程的白噪聲、俯仰安裝偏角誤差Markov過程的白噪聲，FSTEKF－INS－NED為ST-EKF濾波框架下的15維狀態系統矩陣。FSTEKF－INS－NED的具體定義如下[9-11]

式(9)～式(14)中，vN、vE分別為北向速度、東向速度，RN為子午圈主曲率半徑，RE為卯酉圈主曲率半徑，L、h分別為緯度、 高度，Ω為地球自轉速度。

1.3 基于ST-EKF的系統觀測模型

式(15)中，δα為捷聯慣導系統與車體之間的安裝誤差。

由捷聯慣導系統解算的速度在車體坐標系(m系)下的投影可以表達為

將捷聯慣導系統解算出的速度信息投影到里程計坐標系下與里程計的速度相減作為SINS／OD組合導航系統觀測量δzv， 即

由式(17)可以定義基于ST-EKF的SINS／OD組合導航系統的觀測方程

式(18)中，v4×1為測量噪聲向量，Hv為觀測矩陣。Hv滿足

式(20)中，vD為里程計測量的前向速度。

2 基于ST-EKF的滑動窗口濾波技術

2.1 技術概論與應用流程

本文提出的基于ST-EKF的慣性／里程計滑動窗口濾波技術是在基于ST-EKF的慣性／里程計組合導航算法的基礎上做出了改進，主要由ST-EKF前向濾波器、滑動窗口、RTS平滑器三部分組成[8]。導航平臺開始運動后，首先進行前向濾波，記錄下系統時間更新與量測更新后的誤差狀態向量、誤差協方差矩陣與狀態轉移矩陣，并存儲到預先設置好容量的滑動窗口中。當滑動窗口中的濾波信息達到設定長度時，提取儲存數據對區間內的相關狀態進行RTS反向平滑和航跡修正。同時，為了使平滑時刻的濾波結果也接受優化，利用前一時刻的平滑結果對當前時刻進行第二次濾波。

在實際應用過程中，以滑動窗口中存入的第一組濾波信息到儲存的濾波信息長度達到滑動窗口的設定長度為一個平滑周期，系統利用周期內存儲的所有前向濾波信息進行反向平滑，修正載車航跡，然后初始化滑動窗口空間，繼續進行前向濾波和數據存儲，直到滑動窗口空間達到存儲上限或載車到達行駛終點。

相對于文獻[8]提出的ST-EKF平滑算法，本文根據應用平臺的改變在SINS／OD組合導航系統上對算法進行了重新定義，并針對該算法只能應用于事后處理的問題做出了改進，通過引入滑動窗口的概念使算法成為一種在線平滑方法。相對于傳統的平滑方法，滑動窗口的引進增強了技術的實時性，減小了導航計算機的計算量，滑動窗口中只需要存儲當前平滑周期內的xf，k、xf，k／k－1、Pf，k、Pf，k／k－1、Φk，k－1等前向濾波信息， 反向平滑結束后便可以初始化進入下一個周期。該技術的具體應用流程如圖1所示。

圖1 基于ST-EKF的滑動窗口濾波技術實際應用流程Fig.1 Practicalapplication flow of sliding window filtering technology based on ST-EKF

圖1中，fIMU為IMU的輸出頻率。

2.2 基于ST-EKF的前向濾波與滑動窗口數據存儲

基于ST-EKF的前向濾波過程中的誤差狀態模型與量測模型的建立基本與前文中的基于ST-EKF的SINS／OD組合導航系統相一致，濾波過程可以用以下5個公式表示[5，8]

式(21)～式(25)中，下標f表示前向濾波過程，下標k表示濾波時刻，P為系統的誤差協方差矩陣，Q為系統的過程噪聲方差矩陣，Γ為離散化后的噪聲轉移矩陣，R為系統的量測噪聲方差矩陣，K為濾波增益矩陣。其中，式(21)、式(22)為系統的時間更新過程，式(23)～式(25)為系統的量測更新過程。

系統前向濾波過程中，每次時間更新與量測更新結束后， 將得到的濾波信息xf，k、xf，k／k－1、Pf，k、Pf，k／k－1、Φk，k－1存入滑動窗口并標注好時刻，滑動窗口的長度L可以根據行車里程、故障點的分布時段等實際情況靈活調整。此外，由于只有受噪聲驅動的狀態分量才具有可平滑性，不受噪聲影響的隨機常值狀態分量的平滑精度不會優于單向濾波的精度。為了減少不必要的計算，本文僅將姿態失準角、速度誤差、位置誤差、里程計刻度因子誤差作為平滑狀態存儲到滑動窗口空間中， 即x1－10＝[(φn)T(δvnφ)T(δrn)TδkD]T。

2.3 基于ST-EKF的RTS平滑算法

當滑動窗口空間中實時存儲的濾波信息長度l與滑動窗口長度L相等時，停止前向濾波，以當前的誤差狀態向量xf，k作為初始量xs，k，以當前的誤差協方差矩陣Pf，k作為Ps，k， 利用滑動窗口空間中的濾波信息進行反向RTS平滑，具體過程如式(26)～式(28)所示[5，8]

式(26)～式(28)中，下標s表示RTS平滑過程。

由式(26)～式(28)可知，平滑時刻的誤差狀態向量xs，N和誤差協方差矩陣Ps，k沒有進行平滑優化而是與前向濾波的相關狀態保持一致，不符合無人車導航定位的實時性要求，這也是傳統RTS平滑算法的一個缺陷。為了改善算法的實時性，本文采用平滑時刻k前一時刻的平滑后誤差狀態向量xs，k－1和誤差協方差陣Ps，k－1進行再一次的預測與量測更新， 即采用xs，k－1、Ps，k－1替代式(21)、式(22)中的xf，k－1、Pf，k－1進行第二次前向濾波，使得平滑時刻的濾波狀態在一個平滑周期結束時也能受到優化改善。

3 仿真實驗結果與分析

3.1 實驗設計

為了檢驗本文提出的基于ST-EKF的慣性／里程計滑動窗口濾波技術的實際應用效果，對真實的長行駛里程跑車數據進行事后處理，記錄了組合導航系統水平定位精度隨滑動窗口長度的變化情況，并與同等條件下的基于EKF的滑動窗口濾波技術處理后的系統水平定位精度作出對比。同時，為了觀察將里程計刻度因子誤差作為平滑狀態后對濾波平滑效果的改善性，對分別采用添加里程計刻度因子誤差的十平滑狀態和傳統的九平滑狀態[8]的基于ST-EKF的慣性／里程計滑動窗口濾波技術處理后的系統水平定位精度作出了對比。

本次實驗使用的數據來自北京至河北省的一次長行駛里程跑車實驗[3]，實時行車時間總計7h，在大約3.5h時到達半程(去程)終點，總行程為489.88km。圖2為本次實驗在二維地圖下的行車軌跡。

圖2 二維跑車軌跡圖Fig.2 Two dimensionaltrajectory of the land vehicle experiment

實驗車輛配備了導航級的高精度激光陀螺IMU裝置和比例因子約為53p／m的輪式里程計，IMU的輸出頻率為200Hz，陀螺零偏穩定性為0.01(°)／h，角度隨機游走為0.0003(°)／h1／2，加速度計零偏穩定性為50μg，速度隨機游走為5μg／Hz1／2。在跑車實驗中，車輛的雙位置精對準時間為180s。

3.2 實驗結果

圖3為不進行平滑處理的EKF和ST-EKF組合導航算法解算得到的水平位置分別相對于基準位置的水平定位誤差和精度隨時間以及行駛里程的變化而產生的變化趨勢，圖4為滑動窗口長度L＝6000時采用十平滑狀態平滑處理后的EKF和STEKF組合導航算法的水平定位誤差和精度變化趨勢。基準位置由慣性／衛星組合導航事后平滑算法[11]解算得到。

圖3 不加平滑處理的EKF和ST-EKF組合導航算法的水平定位結果Fig.3 Horizontalpositioning results of EKF and ST-EKF integrated navigation algorithms without smoothing

圖4 L＝6000時十平滑狀態平滑處理后的EKF和ST-EKF組合導航算法的水平定位結果Fig.4 Horizontalpositioning results of EKF and ST-EKF integrated navigation algorithms after ten-smooth state processing when L＝6000

圖3、圖4對比可以發現，平滑處理后的SINS／OD組合導航算法相對于未處理時，水平定位誤差的抑制效果得到了有效提升，誤差曲線更加平滑，跳點大幅度減少。同時，圖3、圖4表明，在非平滑和平滑兩種情況下，ST-EKF都比EKF具有更好的濾波效果，水平定位精度優于行程的0.05%DT(Distance Traveled)。

滑動窗口長度L＝6000時，增加里程計刻度因子誤差為平滑狀態的基于ST-EKF的滑動窗口濾波技術與不增加里程計刻度因子誤差為平滑狀態時的水平定位誤差對比情況如圖5所示。

圖5 L＝6000時分別采用十平滑狀態和九平滑狀態處理后的ST-EKF組合導航算法的水平定位結果Fig.5 Horizontalpositioning results of ST-EKF integrated navigation algorithms after ten-smooth state processing and nine-smooth state processing when L＝6000

由圖5可知，在同等條件下，增加里程計刻度因子誤差為平滑狀態的滑動窗口濾波技術具有更好的航跡修正效果。

在實際應用中，滑動窗口的長度可以根據載車行駛里程、故障點分布時段實時進行調整。為了驗證滑動窗口長度對航跡修正效果的影響，在采用同一組車載實驗數據的前提下，改變L的取值，計算并記錄了在不同條件下基于EKF和STEKF的滑動窗口濾波技術在選取十平滑狀態時的水平定位結果。同時，考慮到本次車載實驗是沿著公路往返行駛，如果只考慮全程的水平定位誤差，可能部分誤差會相互抵消從而影響實驗結果的準確性。因此，為了更客觀地評價本文提出的算法的航跡修正效果，表1和表2分別統計了單程(去程)和全程的水平定位結果。

以行駛軌跡中的半程(去程)終點為界，表1統計了五種情況下的半程(去程)水平定位誤差的均方根誤差(RMSE)以及RMSE相對于這一半行程的百分比，表2則為同等條件下的全程水平定位誤差的RMSE以及RMSE相對于全程的百分比。

表1 水平位置的半程(去程)誤差統計Table 1 Statistics of half-way(outward voyage)errors in horizontalposition

表2 水平位置全程誤差統計表Table 2 Statistics of whole journey error in horizontalposition

由表1、表2可知，基于EKF的慣性／里程計組合導航算法的半程(去程)水平位置的RMSE為57.1512m，全程的RMSE為54.7629m。在滑動窗口長度為6000時，本文提出的采用十平滑狀態的基于ST-EKF的慣性／里程計滑動窗口濾波技術的半程(去程)水平位置的RMSE為32.6988m，全程的RMSE為28.6417m，相對于基于EKF的慣性／里程計組合導航算法分別減少了42.79%和47.70%，進一步驗證了該技術的有效性與實用性。同時，由表1、表2可知，在樣本長度允許的前提下，適當加大滑動窗口的長度可以使平滑取得更好的航跡修正效果，提高了系統的水平定位精度。

4 結論

針對長行駛里程條件下無人駕駛車輛的導航定位精度優化問題，本文提出了一種基于ST-EKF的慣性／里程計滑動窗口濾波技術。在基于ST-EKF的慣性／里程計組合導航方案的基礎上引入了滑動窗口和RTS平滑器，利用實時存儲的前向濾波數據進行航跡修正，提高了車輛的導航定位精度。實驗表明，該技術相比于傳統的EKF組合導航方案具有更好的濾波魯棒性，并且在經過RTS平滑處理后，取得了較好的航跡修正效果，半程(去程)水平位置的RMSE減少了42.79%，全程水平位置的RMSE減少47.70%。因此，在衛星拒止的復雜野外環境下，該技術可以幫助無人車迅速確定自身所處區域，使得視覺等傳感器的后續處理能更加精確，具有良好的工程應用價值。