基于擴展卡爾曼濾波的無人車避障算法研究

江楠　徐健

關鍵詞：擴展卡爾曼；避障算法；A*；自動駕駛

中圖分類號：TP391 文獻標識碼：A

文章編號：1009-3044（2023）08-0016-03

0 引言

隨著科學技術的快速進步，出現了越來越多的智能化設備方便人們的生活，其中自動駕駛技術便是當下的熱點研究問題之一，所有主流車企都在最新的車型上嘗試加入更加智能的自動駕駛算法，并成為品牌的主要賣點之一。自動駕駛技術包含了大量技術，包括對于復雜環境的全方位感知、車輛定位與導航、線路規劃等，其中避障算法是自動駕駛的核心技術之一。優秀的避障算法不僅能夠躲避行駛路線上的靜態障礙物，還能夠對動態的障礙物進行提前預判，根據預判結果選擇合適的規避方法，從而防止碰撞的發生。

本文提出了一種基于拓展卡爾曼濾波的無人車避障算法，當無人車的相機檢測到前方出現動態障礙物時，通過拓展卡爾曼濾波算法對障礙物的運動軌跡進行預測，從而指導無人車規避障礙。

1 卡爾曼濾波及其變種

1.1 相關工作

卡爾曼濾波（Kalman Filter） 也被稱為線性最小均方差估計（Linear Least-Mean-Squares Estimator） ，最初由斯坦利施密特和卡爾曼提出[1]，該算法能夠在線性高斯隨機系統中，將系統的輸入輸出使用線性狀態方程進行運算，并測量在隨機噪聲干擾下的輸出，其中隨機噪聲一般假設滿足高斯分布，并根據輸出對系統進行最優估計。

由于卡爾曼濾波器計算量小、魯棒性強等特點，科研學者針對卡爾曼濾波只能處理線性方程等限制，研究了許多衍生算法。田亞蕾等[2]針對一般目標跟蹤算法在遮擋場景下的退化甚至失效的問題，將核相關濾波算法與卡爾曼濾波預測相結合實現模型的自適應抗遮擋目標跟蹤；周發華[3]利用拍攝道路圖像的連續幀中，車道線發展的連續性提出了一種改進的卡爾曼濾波車道識別方法；易鑫等[4]針對車輛無法確定狀態方程中噪聲分布的問題，將遺傳算法與擴展卡爾曼濾波相結合構建車輛的狀態觀測器?？柭鼮V波的變種算法還能夠應用于物體的位姿估計與狀態修正等場景[5-7]。

1.2 卡爾曼濾波

假設需要預測的系統為高斯線性系統，在線性高斯系統中，運動方程f （x，u）和觀測方程h（x）都為線性方程，并且運動噪聲和觀測噪聲都滿足高斯分布。建立對應的運動方程和觀測方程：

EKF的缺陷主要在于，在一階泰勒展開時可能無法很好地對運動方程和觀測方程進行很好的近似，并且由于馬爾可夫性，EKF只是對前一時刻的狀態對當前進行最優無偏估計，所以容易出現累積誤差，并且算法本身沒有異常值檢測機制，導致估計失效。

2 基于卡爾曼濾波的無人車避障

在檢測到視野中的動態障礙物后，在無人車接近動態障礙物的過程中，可以通過統計一段時間內該障礙物在圖像中的狀態變化，并建立合適的運動方程與狀態觀測方程，之后通過擴展卡爾曼濾波算法結合動態障礙物當前的狀態進行狀態預測，其中預測時間隨著障礙物距離無人車的距離縮短而逐漸減小，動態障礙物的預測位置為無人車需要避讓的位置。

由于無人車和障礙物都在動態地運動，所以無人車需要提前做出動作對障礙物進行躲避，所以需要通過局部路徑規劃算法實現無人車的避障。最早的局部路徑規劃算法是由Khatib在1986年提出的人工勢場法[8]，類似于磁場，相對于車輛障礙物會產生排斥力，而目標點會產生吸引力，在引力場的作用下沿著勢場梯度下降方向行駛實現障礙物躲避。當存在較多障礙物時，由于障礙物排斥力疊加，抵消目標點的吸引力從而導致無解，Fan等[9]在人工勢場法中加入距離修正因子和正則六邊形引導法，從而獲得局部最優解。除此以外，其他學者[10-11]也對人工勢場法避障針對不同應用做出了改進。

除了人工勢場法，圖搜索算法也是局部路徑規劃的主流算法之一。圖搜索算法需要先將地圖進行柵格化生成柵格地圖，并將障礙物通過占用柵格表示，每一個可通行柵格都有通行代價，通過不斷迭代搜索代價最小的柵格路徑進行障礙物躲避。常見的圖搜索算法包括Dijkstra 算法和A* 算法及其衍生算法[12-13]。

上述算法主要解決的是靜態障礙物，針對動態障礙物，我們提出了如圖2所示的一種多層柵格地圖A*算法。擴展卡爾曼濾波根據動態障礙物的當前狀態對未來運動軌跡做出預測，但由于障礙物可能會出現突然加速或減速情況，導致最終的預測發生一定偏差。多層柵格地圖A*算法分為障礙物層、預測軌跡層和膨脹層，其中障礙物層會在柵格地圖中對當前障礙物的位置進行標注，通過拓展卡爾曼濾波得到的障礙物預測軌跡輸入至預測軌跡層，最后根據無人車的尺寸計算膨脹層的膨脹因子，對障礙物層和預測軌跡層進行膨脹，通行代價隨著與障礙物距離增加而減小。最終使用A*算法求解多層代價柵格地圖，獲得最優路徑。

基于擴展卡爾曼濾波的無人車避障方法的整體流程圖如圖3所示。無人車在感知到存在動態障礙物時，通過建立狀態方程和觀測方程，結合擴展卡爾曼濾波算法對目標位置進行預測，在更新卡爾曼增益的過程中不斷對預測進行修正，將獲得的預測軌跡輸入至多層柵格地圖的預測軌跡層，并添加障礙物層和膨脹層后使用A*算法進行求解最優避障路徑。

3 實驗與分析

為了驗證算法的有效性，本文針對不同運動模型的障礙物以及不同數量障礙物進行仿真實驗，并與其他算法進行比較。仿真環境的柵格地圖大小為長200柵格、寬30柵格的矩形，沿途放置靜態障礙物來約束行駛路線，動態障礙物大小為長度不超過3柵格的隨機形狀。

常見的障礙物運動模型分為恒定速度模型（Con?stant Velocity， CV） 和恒定加速度模型（Constant Acce?leration， CA） ，其中恒定速度模型假設障礙物在單位內以恒定速度移動，恒定加速度模型在單位時間內以恒定加速度運行。本文首先對單個動態障礙物的避障進行了實驗，實驗結果如表1所示。由于A*算法和人工勢場算法不存在軌跡預測功能，只能根據動態障礙物的當前位置進行實時更新躲避路線，但是由于躲避路線可能與障礙物的運動軌跡重合，最終導致發生碰撞，而加入卡爾曼濾波預測的算法都能夠很好地在單一障礙物的情況下進行躲避。

為了進一步驗證本文算法的優異性能，在仿真中隨機添加五個動態障礙物，并對比不同算法下的避障性能。實驗結果如表2所示，在多個移動障礙物的干擾下，A*算法和人工勢場法由于沒有預測功能所以總會碰撞某個障礙物，人工勢場法在加入卡爾曼濾波后，由于恒定速度模型中由于速度恒定，線性卡爾曼濾波也能夠進行很好的預測，所以能夠很好地躲避多個動態障礙物。但是由于恒定加速度模型中速度會發生變化，需要對預測模塊進行非線性處理，同時人工勢場法無法獲得局部最優解，導致了碰撞發生。而本文提出的算法能夠很好地解決多個運動模型并實現多個動態障礙物的避障。

4 結束語

性能優秀避障算法對自動駕駛的安全至關重要，本文提出了一種基于擴展卡爾曼濾波的無人車避障算法，針對多種運動模型的動態障礙物，以及多個動態障礙物干擾的復雜環境中都能夠很好的運行?；跀U展卡爾曼濾波算法構建非線性運動方程與觀測方程，對動態障礙物的運動軌跡進行預測，并輸入到基于A*算法的多層柵格代價地圖，在減少計算量的同時，額外的預測軌跡層能夠很好的處理多障礙物情況下產生的干擾問題。