基于無跡卡爾曼濾波的定位融合與校驗算法研究*

李興佳，李建芬，朱 敏，彭能嶺，左 帥

（鄭州宇通客車股份有限公司，鄭州 450061）

前言

定位技術是自動駕駛領域的關鍵基礎技術之一，定位精度直接影響自動駕駛車輛的行駛安全［1］。常用的定位技術有組合GPS（global positioning sys?tem）和SLAM（simultaneous localization and mapping）定位等［2-3］。對于組合GPS定位技術，實際行駛環境中存在的遮擋和接收機噪聲等因素會導致定位數據發生一定程度的漂移［4］，這種漂移會極大地影響自動駕駛車輛的控制精度。通過小波去噪等方法對定位結果進行濾波，能夠提高車輛定位精度［5-6］，但是該方法無法檢測出定位發生的緩慢偏移。相比而言，SLAM方法通過相對定位方式避免信號遮擋影響，但存在一定的積累誤差，而其自身難以識別、消除這種積累誤差［7］。采用航跡推算與傳感器定位融合的方式也可以在一定程度上提高定位精度［8］。車道保持系統通過相機檢測車輛在車道內的相對位置，控制方向盤轉角使車輛保持在當前車道內行駛［9-10］，能夠避免對GPS全局定位的依賴，但在無明顯車道線路面上，或車輛處于換道駕駛任務時，難以獲取車輛的位置信息［11］。為融合不同定位方法的優勢，Huang等［12］通過多傳感器融合方法提出了一種高精度組合定位策略，能夠有效削弱車輛突變型橫向定位誤差，但是難以應對緩慢定位偏移導致的影響。綜上所述，無論采用何種定位方式，對于突變的定位失效，系統能夠及時察覺，從而及時采取一定的措施（如人工接管等）；而對于緩慢變化的定位失效難以及時察覺，這給自動駕駛帶來了很大的安全隱患。

針對這一問題，本文中采用無跡卡爾曼濾波算法融合航跡推算定位結果與實際傳感器定位結果，在提高定位精度的同時進行定位校驗，判斷定位是否發生緩慢偏移失效，同時提出主動避險控制算法，在定位失效后提高自動駕駛車輛行駛安全性。

1 基于無跡卡爾曼濾波的定位融合算法

1.1 基于車輛航跡推算的定位預測

航跡推算基于車輛運動模型建立車輛航跡推算模型，從而根據速度、航向角等信息預測未來一段時間內車輛的行駛軌跡，是一種在短時間短距離內精度較高的輔助定位方法。

圖1為車輛運動學模型，車輛質心在后軸中心點處，前輪為轉向輪。記大地坐標系為XOY，車輛定位信息為(xk，yk)，P為車輛瞬時旋轉中心，ds為相鄰兩時刻車輛行駛的距離，車輛航向角為φk，車輛瞬時速度vk、加減速度ak和橫擺角速度ωk可由車身其他傳感器獲得，相鄰兩時刻之間的時間間隔為Δt，則根據圖示位置關系，相鄰時刻間的車輛位置關系可以表示為

圖1 車輛運動學模型

1.2 基于無跡卡爾曼濾波的定位融合與校驗算法

無跡卡爾曼濾波是通過無跡變換的方法來解決卡爾曼濾波在推演過程中的非線性傳遞問題，方程分為狀態方程和觀測方程兩部分，狀態方程反映了系統狀態量的推演過程，觀測方程反映了可觀測量與狀態量之間的關系。定義狀態變量x∈R n，系統的狀態方程可以表示為

系統的觀測方程可以表示為

式中:X k為第k時刻狀態變量；U k為第k時刻系統輸入量；Q和R分別為過程激勵噪聲和觀測噪聲，二者相互獨立且服從高斯分布；Z k+1為第k+1時刻觀測量矩陣。

通過無跡卡爾曼濾波算法將航跡推算預測定位結果與實際傳感器定位結果非線性融合，則根據式（1）和式（2），實際系統的狀態方程可表示為

式中:Xˉ0為狀態量初始均值；PX，0為狀態量初始方差矩陣。

（2）通過無跡變換，獲取狀態量采樣點集和對應的權值，各采樣點可以表示為

各采樣點對應的權值可以表示為

（4）對先驗估計值繼續使用無跡變換，獲得新的采樣點集和對應權值

表1 傳感器定位數據狀態判斷表

（6）觀測更新與狀態后驗估計

①如果傳感器數據發生緩慢或突變的定位偏移，則用航跡推算數據，經采樣預測后對各估計值進行更新:

式中:Xˉk+1為狀態量估計輸出矩陣；Kg k+1為無跡卡爾曼增益矩陣。

如果傳感器定位數據連續FN幀出現緩慢偏移或單幀出現突變偏移，則說明傳感器定位數據發生偏移失效，當傳感器定位數據發生定位偏移失效時，需要采取進一步的措施以保證車輛安全。

2 主動避險控制算法

傳感器定位正常時，車輛采用融合后的定位數據以保證其正常行駛；當傳感器定位數據發生偏移失效時，已知車輛定位失效前位置和參考軌跡，通過航跡推算結合無跡卡爾曼濾波算法預測車輛定位信息，使車輛能夠繼續按照參考軌跡線行駛直至停車。

當定位失效后，系統讀取定位失效前保存的軌跡（用等間距離散點表示），如圖2虛線所示。圖中，Xtraj(k)（其中，Xtraj(k)=(xtraj(k)，ytraj(k))，k=1，2，…，Ltraj）為 軌跡上第k點坐標，Ltraj為參考軌跡長度，t1、t2為設定最短、最長預瞄時間，Lstatic_min、Lstatic_max分別為最短、最長固定預瞄距離（A和C分別為軌跡上對應的點），P為預瞄點個數，Lpredis(1)、Lpredis(P)分別為最近、最遠預瞄距離（B和D分別為軌跡上對應的點），G點為車輛質心位置（也即定位點位置），假設車輛當前坐標為（x，y），車速為v，航向角為φ。

圖2 主動避險車輛控制示意圖

車輛采用多點預瞄+最優曲率的方式進行橫向控制，預瞄距離采用固定預瞄距離+速度·時間的方式［14］，可以計算得到最近和最遠預瞄距離分別為

根據式（15）計算得到的多個預瞄距離在軌跡上查找對應的預瞄點并存于數組Xdot中，其中Xdot(i)=(xdot(i)，ydot(i))，i=1，2，…，P，根據曲率計算公式，計算各預瞄點對應的曲率ρ(i)，則有

根據轉向盤轉角與橫擺角速度的轉換關系，計算轉向盤轉角控制量，進行主動避險的橫向控制。

主動避險過程中，實時獲取車輛反饋橫擺角速度ωact和實車減速度aact，根據式（1）預測的車輛定位信息，實現車輛沿參考軌跡主動避險，直至車輛減速停車或人工接管。

3 算法仿真結果與分析

3.1 測試條件及相關參數設置

為驗證定位失效檢測算法效果，也便于控制算法向實車實驗平臺移植，本文中使用標準C語言將算法編寫成動態鏈接庫，并搭建了MATLAB/Simulink與TruckSim聯合仿真平臺，調用該算法庫進行仿真測試，如圖3所示，仿真參數設置與表2實車參數設置保持一致。

圖3 定位失效仿真系統示意圖

3.2 定位校驗算法仿真結果

3.2.1 緩慢偏移定位失效仿真結果

為驗證該算法，手動從200 s處開始給定位加上緩慢偏移，偏移在300 s的位置結束，定位累計偏差為0.5 m。圖4（a）為車輛行駛軌跡示意圖，X軸為車輛縱向坐標，Y為車輛橫向坐標，可以看出，當定位發生緩慢偏移時，車輛開始偏離原始行駛軌跡，當定位恢復正常時，車輛也慢慢恢復到正常行駛狀態。圖4（b）為仿真設定傳感器累積偏差e1與實際傳感器數據偏差（實際傳感器輸出值與估計值偏差）e2示意圖（以行駛方向橫向偏差為例）。從圖4（c）偏差放大示意圖可以看到，定位正常時，偏差在0.01 m以內；當定位發生偏移時，e2隨之增大，可達到0.27 m；當定位再次正常時，e2恢復正常值。由此說明，通過判斷傳感器數據偏差的變化即可判斷出定位正在發生的緩慢偏移。

圖4 設定偏差與傳感器數據偏差示意圖

3.2.2 突變定位失效仿真結果

在100 s處給定一個突變偏差，偏差幅度為0.5 m。圖5（a）為給定傳感器偏差e1與實際傳感器偏差（實際傳感器輸出值與估計值偏差）e2示意圖（以行駛方向橫向偏差為例）。圖5（b）為偏差放大示意圖，可以看到，當仿真設置偏差較大即定位發生突變失效時，實際傳感器數據偏差e2也隨之發生突變，由此可以判斷出傳感器定位數據正在發生的突變偏移。

圖5 定位偏差與傳感器數據偏差示意圖

4 算法實車測試結果與分析

4.1 測試條件及相關參數設置

為進一步驗證算法的有效性，基于普通平直瀝青路面進行了實車測試。實車試驗平臺如圖6所示，該平臺使用激光雷達、毫米波雷達、超聲波雷達、視覺傳感器等多種傳感器來感知周圍環境，使用組合導航加高精定位的方式進行系統定位，使用線控驅動、制動和線控轉向等響應智能控制器發出的控制指令，并使用相應的傳感器來檢測轉向盤實際轉向盤轉角、速度、加減速度、橫擺角速度等信息。

圖6 實車測試平臺

實車測試的車輛、道路等相關參數設置如表2所示。

表2 相關參數設置

4.2 定位融合算法測試結果與分析

圖7所示為基于無跡卡爾曼濾波的定位融合結果示意圖，航跡推算定位預測周期與偏差積累周期一致，測量噪聲設置為Q=[0.001，0.001，0.1，0.1]，觀測噪聲設置為R=[0.14，0.1]。圖7中，點實線為實際傳感器定位數據曲線，虛線為航跡推算定位曲線，黑色實線為融合后定位曲線，可以看到定位融合算法能夠在一定程度上提升定位準確度。

圖7 定位融合結果示意圖

4.3 定位校驗實車測試結果與分析

圖8（a）為車輛實際行駛軌跡示意圖。從圖中可知定位在（-397.7，-5.5）附近開始發生緩慢偏移，在（-330.8，-30.35）處跳回正常值。圖8（b）為實車測試時偏差示意圖（以行駛方向橫向偏差為例）。從圖8（c）偏差放大示意圖可以看到，傳感器定位正常時，偏差（測量與估計值偏差）在0.1 m以內；當定位開始發生緩慢偏移時，偏差也隨之緩慢增大，增加到0.25 m左右。當定位突然跳變為正常值時，相應的偏差也發生了跳變。由此可以說明，通過判斷傳感器數據偏差變化即可判斷出定位正在發生的緩慢偏移。

圖8 傳感器數據偏差示意圖

4.4 主動避險控制實車測試結果與分析

為了更好地驗證所提出的主動避險控制方法的有效性和實車測試結果的可靠性，分別在直線行駛工況和換道行駛工況下進行實車測試。測試過程中在車速達到試驗車速后切斷定位數據，采用無跡卡爾曼濾波結合航跡推算定位算法推測未來一段時間內的車輛位置信息（航跡推算周期與定位偏差周期一致），直至車輛完全停車，每組車速測試10次，實車能夠響應的最小減速度為-0.3 m/s2。

4.4.1 直線行駛工況下測試結果

表3所示為直線行駛工況下的測試結果，分別測試了10、20、30、40、50 km/h 5種速度下實車的控制效果，該速度為車輛定位失效時的車速。從表中可以看到，實際可跟隨平均軌跡長度為59.8 m，給定絕對減速度大于絕對安全減速度，且均未超過車輛自身限制最大減速度極限值。隨著車速的增加，車輛的平均制動距離增大，橫向誤差也隨著制動距離和車速的增加而增大，但均未偏離出當前車道（車輛沿車道中心線行駛，車道寬度一般約3.6 m，車寬2.5 m）。說明基于無跡卡爾曼濾波的航跡推算定位預測定算法能夠用于一定時間內的定位預測，同時主動避險控制方法對于直線行駛工況可以取得很好的控制效果，滿足車輛的安全行駛要求。

表3 直線行駛時測試結果

4.4.2 換道行駛測試結果

表4所示為換道行駛工況下測試結果，分別測試了10、15、20、25、30 km/h 5種速度下實車的控制效果。由表中數據可以看出，對于制動距離，由于換道影響因素較多，不同速度下換道軌跡長度也差別較大。結合圖9可以看出，測試車輛能夠在定位失效后正常控制車輛按照之前存儲的參考軌跡繼續安全行駛直至停車，其軌跡偏差也在允許的范圍之內。因此，該定位預測算法與主動避險控制方法對于換道行駛工況同樣具有較好的控制效果。

表4 換道行駛時測試結果

圖9 換道軌跡示意圖

5 結論

本文中基于車輛運動模型建立航跡推算定位預測模型，通過無跡卡爾曼濾波實現了預測定位與實際傳感器定位數據的非線性融合，通過定位校驗算法檢驗定位是否出現偏移，最后研究了主動避險控制算法對車輛行駛安全性的提升，仿真和實車測試結果表明:

（1）基于無跡卡爾曼濾波的定位融合算法能夠在一定程度上提高車輛實際定位準確性；

（2）定位校驗算法不僅能夠識別出突變的定位偏移，也能夠識別出緩慢變化的定位偏移；

（3）車輛主動避險控制算法使車輛保持定位失效前軌跡繼續行駛直至停車，適用于直線行駛、換道等不同場景，能夠提高車輛行駛安全性。