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

江楠　徐健

關(guān)鍵詞：擴(kuò)展卡爾曼；避障算法；A*；自動駕駛

中圖分類號：TP391 文獻(xiàn)標(biāo)識碼：A

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

0 引言

隨著科學(xué)技術(shù)的快速進(jìn)步，出現(xiàn)了越來越多的智能化設(shè)備方便人們的生活，其中自動駕駛技術(shù)便是當(dāng)下的熱點研究問題之一，所有主流車企都在最新的車型上嘗試加入更加智能的自動駕駛算法，并成為品牌的主要賣點之一。自動駕駛技術(shù)包含了大量技術(shù)，包括對于復(fù)雜環(huán)境的全方位感知、車輛定位與導(dǎo)航、線路規(guī)劃等，其中避障算法是自動駕駛的核心技術(shù)之一。優(yōu)秀的避障算法不僅能夠躲避行駛路線上的靜態(tài)障礙物，還能夠?qū)討B(tài)的障礙物進(jìn)行提前預(yù)判，根據(jù)預(yù)判結(jié)果選擇合適的規(guī)避方法，從而防止碰撞的發(fā)生。

本文提出了一種基于拓展卡爾曼濾波的無人車避障算法，當(dāng)無人車的相機(jī)檢測到前方出現(xiàn)動態(tài)障礙物時，通過拓展卡爾曼濾波算法對障礙物的運動軌跡進(jìn)行預(yù)測，從而指導(dǎo)無人車規(guī)避障礙。

1 卡爾曼濾波及其變種

1.1 相關(guān)工作

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

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

1.2 卡爾曼濾波

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

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

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

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

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

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

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

基于擴(kuò)展卡爾曼濾波的無人車避障方法的整體流程圖如圖3所示。無人車在感知到存在動態(tài)障礙物時，通過建立狀態(tài)方程和觀測方程，結(jié)合擴(kuò)展卡爾曼濾波算法對目標(biāo)位置進(jìn)行預(yù)測，在更新卡爾曼增益的過程中不斷對預(yù)測進(jìn)行修正，將獲得的預(yù)測軌跡輸入至多層?xùn)鸥竦貓D的預(yù)測軌跡層，并添加障礙物層和膨脹層后使用A*算法進(jìn)行求解最優(yōu)避障路徑。

3 實驗與分析

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

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

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

4 結(jié)束語

性能優(yōu)秀避障算法對自動駕駛的安全至關(guān)重要，本文提出了一種基于擴(kuò)展卡爾曼濾波的無人車避障算法，針對多種運動模型的動態(tài)障礙物，以及多個動態(tài)障礙物干擾的復(fù)雜環(huán)境中都能夠很好的運行?；跀U(kuò)展卡爾曼濾波算法構(gòu)建非線性運動方程與觀測方程，對動態(tài)障礙物的運動軌跡進(jìn)行預(yù)測，并輸入到基于A*算法的多層?xùn)鸥翊鷥r地圖，在減少計算量的同時，額外的預(yù)測軌跡層能夠很好的處理多障礙物情況下產(chǎn)生的干擾問題。