杜學(xué)禹，王茂松，吳文啟，張 靖

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

0 引言

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

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

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

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

1 基于ST-EKF的慣性/里程計(jì)組合導(dǎo)航模型

1.1 ST-EKF與EKF模型對比

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

式(2)中，gn為當(dāng)?shù)刂亓κ噶?，εb為陀螺測量零偏，Δb為加速度計(jì)測量零偏，wg和wa分別為陀螺和加速度計(jì)誤差的白噪聲向量。

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

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

定義慣性／里程計(jì)組合導(dǎo)航系統(tǒng)的21維誤差狀態(tài)向量x

可以構(gòu)建慣性／里程計(jì)組合導(dǎo)航系統(tǒng)的誤差狀態(tài)方程

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

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

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

1.3 基于ST-EKF的系統(tǒng)觀測模型

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

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

將捷聯(lián)慣導(dǎo)系統(tǒng)解算出的速度信息投影到里程計(jì)坐標(biāo)系下與里程計(jì)的速度相減作為SINS／OD組合導(dǎo)航系統(tǒng)觀測量δzv， 即

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

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

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

2 基于ST-EKF的滑動窗口濾波技術(shù)

2.1 技術(shù)概論與應(yīng)用流程

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

在實(shí)際應(yīng)用過程中，以滑動窗口中存入的第一組濾波信息到儲存的濾波信息長度達(dá)到滑動窗口的設(shè)定長度為一個平滑周期，系統(tǒng)利用周期內(nèi)存儲的所有前向?yàn)V波信息進(jìn)行反向平滑，修正載車航跡，然后初始化滑動窗口空間，繼續(xù)進(jìn)行前向?yàn)V波和數(shù)據(jù)存儲，直到滑動窗口空間達(dá)到存儲上限或載車到達(dá)行駛終點(diǎn)。

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

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

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

2.2 基于ST-EKF的前向?yàn)V波與滑動窗口數(shù)據(jù)存儲

基于ST-EKF的前向?yàn)V波過程中的誤差狀態(tài)模型與量測模型的建立基本與前文中的基于ST-EKF的SINS／OD組合導(dǎo)航系統(tǒng)相一致，濾波過程可以用以下5個公式表示[5，8]

式(21)～式(25)中，下標(biāo)f表示前向?yàn)V波過程，下標(biāo)k表示濾波時刻，P為系統(tǒng)的誤差協(xié)方差矩陣，Q為系統(tǒng)的過程噪聲方差矩陣，Γ為離散化后的噪聲轉(zhuǎn)移矩陣，R為系統(tǒng)的量測噪聲方差矩陣，K為濾波增益矩陣。其中，式(21)、式(22)為系統(tǒng)的時間更新過程，式(23)～式(25)為系統(tǒng)的量測更新過程。

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

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

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

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

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

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

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

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

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

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

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

3.2 實(shí)驗(yàn)結(jié)果

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

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

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

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

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

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

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

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

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

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

表2 水平位置全程誤差統(tǒng)計(jì)表Table 2 Statistics of whole journey error in horizontalposition

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

4 結(jié)論

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