江楠　徐健

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

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

文章編號(hào)：1009-3044（2023）08-0016-03

0 引言

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

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

1 卡爾曼濾波及其變種

1.1 相關(guān)工作

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

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

1.2 卡爾曼濾波

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

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

2 基于卡爾曼濾波的無(wú)人車避障

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

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

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

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

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

3 實(shí)驗(yàn)與分析

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

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

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

4 結(jié)束語(yǔ)

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