改進(jìn)人工勢場法的移動小車動態(tài)避障路徑規(guī)劃

唐小潔 丁一航 申勤 賈成芬

摘要：為使移動小車更好地適應(yīng)復(fù)雜的實際行車道路環(huán)境，實現(xiàn)動態(tài)障礙物環(huán)境下的路徑規(guī)劃，并解決傳統(tǒng)人工勢場法中的局部最小值問題，提高駕駛安全性，改進(jìn)傳統(tǒng)人工勢場法中的障礙物斥力勢場模型，并增加了速度斥力勢場模型和道路邊界約束斥力勢場模型，同時將障礙物連鎖網(wǎng)絡(luò)結(jié)構(gòu)與隨機目標(biāo)點法相結(jié)合，加入傳統(tǒng)人工勢場算法中。仿真結(jié)果證明了該改進(jìn)方法的有效性，移動小車能避開局部最小值陷阱到達(dá)目標(biāo)點，實現(xiàn)動態(tài)避障路徑規(guī)劃。

關(guān)鍵詞：路徑規(guī)劃;人工勢場法;動態(tài)環(huán)境;避障

DOI：10.11907/rjdk.192172開放科學(xué)（資源服務(wù)）標(biāo)識碼（OSID）：

中圖分類號：TP319文獻(xiàn)標(biāo)識碼：A 文章編號：1672-7800（2019）010-0152-05

0引言

路徑規(guī)劃是指在已知或未知的障礙物環(huán)境中，搜索出一條從起始位置到目標(biāo)位置、最優(yōu)或近似最優(yōu)的無碰撞安全路徑。目前，已知環(huán)境的路徑規(guī)劃技術(shù)已十分成熟，但在真實、復(fù)雜多變的未知行車道路環(huán)境中，對動態(tài)障礙物的避障問題仍有待進(jìn)一步研究。

人工勢場法是路徑規(guī)劃算法中一種常用的、發(fā)展已較為成熟高效的方法，其具有數(shù)學(xué)原理簡單、計算量小、響應(yīng)速度快、實時控制性能好等優(yōu)點，因此常用于未知環(huán)境的局部路徑規(guī)劃，但也存在目標(biāo)不可達(dá)、容易陷入局部最優(yōu)、對動態(tài)環(huán)境適應(yīng)性較差等問題。針對這些問題，文獻(xiàn)提出一種輔助斥力環(huán)法，強調(diào)預(yù)防性避障，以實現(xiàn)對動態(tài)目標(biāo)的追蹤以及對攔截型障礙物的避障，但并未提及如何解決局部最小值問題;文獻(xiàn)將其它規(guī)劃算法與人工勢場法相結(jié)合，在路徑規(guī)劃時間和路徑平滑性上取得了一定的改進(jìn)效果;文獻(xiàn)為解決局部最小值問題，針對常見的4種環(huán)境場景，提出一種斥力偏轉(zhuǎn)模型。但在以上文獻(xiàn)中，避障環(huán)境均假設(shè)為無邊界平面，因此其實用性仍有待提高。

本文考慮到實際行車道路環(huán)境，首先在人工勢場算法中增加了道路邊界約束以提高其實用性和安全性;其次將動態(tài)障礙物也納入到障礙物環(huán)境中，充分考慮到動態(tài)障礙物速度，建立速度斥力場;之后改進(jìn)障礙物斥力場模型，增加移動小車與目標(biāo)點之間的距離，解決目標(biāo)不可達(dá)問題;采用障礙物連鎖網(wǎng)絡(luò)結(jié)構(gòu)，將密集的障礙物群體連鎖為新的障礙物結(jié)構(gòu)，使移動小車能繞開該群體，無碰撞地到達(dá)目標(biāo)位置;最后為避免移動小車在陷阱區(qū)域因所受合力為零陷入局部最小值，采用隨機目標(biāo)點法使移動小車跳出陷阱到達(dá)目標(biāo)點。

1傳統(tǒng)人工勢場法

人工勢場的基本思想是將移動小車所處障礙物環(huán)境虛擬為引力場和斥力場。其中目標(biāo)點對移動小車產(chǎn)生引力，使移動小車向目標(biāo)點移動，障礙物對移動小車有斥力，使移動小車不與其發(fā)生碰撞。移動小車在路徑上每一點所受合力即為引力與斥力的矢量和。在合力的引導(dǎo)下，移動小車從初始位置沿著勢場下降最快的方向到達(dá)目標(biāo)點。

1.2斥力場

斥力勢場函數(shù)為：

1.3主要問題

1.3.1不符合實際行車道路環(huán)境

傳統(tǒng)人工勢場法的障礙物環(huán)境是假定在無限大的區(qū)域平面內(nèi)，而實際行車道路是有限的，所以實用性較差。

1.3.2動態(tài)環(huán)境路徑規(guī)劃能力差

由式（1）-式（4）可知，傳統(tǒng)人工勢場法只考慮了小車與目標(biāo)點，以及障礙物之間的距離信息，因此通常用于靜態(tài)環(huán)境的路徑規(guī)劃，對動態(tài)環(huán)境的適應(yīng)性較差。當(dāng)出現(xiàn)動態(tài)障礙物或動態(tài)目標(biāo)點時，傳統(tǒng)人工勢場法會因為實時性較差而無法順利到達(dá)目標(biāo)點。

1.3.3目標(biāo)不可達(dá)問題

當(dāng)小車與目標(biāo)點距離很近時，若目標(biāo)點附近有障礙物，小車所受引力相對于此時極大的斥力近似為零，小車將一直在目標(biāo)點附近徘徊而無法到達(dá)目標(biāo)點。

1.3.4局部最小值問題

當(dāng)局部區(qū)域出現(xiàn)多個障礙物時，若小車所受引力和斥力矢量和即合力為零，此時小車陷入局部勢能最低點，產(chǎn)生局部最優(yōu)解，導(dǎo)致小車在該處徘徊往復(fù)、停滯不前，路徑規(guī)劃失敗。

1.3.5對復(fù)雜障礙物環(huán)境適應(yīng)性差

當(dāng)多個障礙物形成U型或C型的環(huán)繞型狹窄區(qū)域時，小車在所受合力引導(dǎo)下，極易掉入障礙物陷阱中發(fā)生碰撞而無法逃離。

2改進(jìn)人工勢場法

2.1斥力場改進(jìn)

2.1.1道路邊界約束斥力場建立

為解決上述傳統(tǒng)人工勢場法不符合實際道路行車環(huán)境的問題，建立道路邊界約束斥力勢場，通過勢場給移動小車施加虛擬斥力，使其在道路邊界線內(nèi)行駛，從而限制了移動小車行駛區(qū)域?？紤]到小車本身具有一定寬度，因此道路左右邊界勢場公式分別為：

2.1.2障礙物斥力場改進(jìn)

由人工勢場法中的引力與斥力公式可知，引力隨著移動小車與目標(biāo)點之間距離的縮短而減小，斥力會隨著移動小車與障礙物之間距離的縮短而增大，因此小車得以避開障礙物到達(dá)正確的目標(biāo)位置。但當(dāng)目標(biāo)點附近有障礙物時，移動小車會因為所受斥力太大、引力太小而產(chǎn)生目標(biāo)不可達(dá)問題，所以在傳統(tǒng)人工勢場法的障礙物斥力場模型中，將移動小車與目標(biāo)點之間距離也考慮進(jìn)來，使移動小車在靠近目標(biāo)點過程中，斥力也逐漸減小至零，以解決目標(biāo)不可達(dá)問題。

2.1.3

速度斥力場建立

為改善人工勢場法的動態(tài)環(huán)境規(guī)劃能力，提高小車對動態(tài)障礙物的避障能力，以適應(yīng)實際行車環(huán)境，將障礙物速度考慮進(jìn)來，建立速度斥力場。速度斥力場函數(shù)為：

2.2障礙物連鎖網(wǎng)絡(luò)結(jié)構(gòu)

當(dāng)小車在障礙物比較密集的狹窄環(huán)境中行駛時，在目標(biāo)點引力與多個障礙物斥力的共同作用下，極易進(jìn)入狹窄區(qū)域，掉入陷阱中發(fā)生碰撞，或達(dá)到力平衡后陷入局部最小值。當(dāng)多個障礙物呈U型或C型分布時，該情況尤其容易發(fā)生。針對該問題，本文擬采用障礙物連鎖網(wǎng)絡(luò)結(jié)構(gòu)，將近距離密集分布的多個障礙物連鎖起來，形成一個新的大障礙物，進(jìn)行大范圍區(qū)域避障。具體操作方法為：

（1）設(shè)定允許連鎖距離D，搜索障礙物1周圍是否存在與其距離小于D的其它障礙物，如果有，則與障礙物1距離最小的其它障礙物建立連鎖。如圖2所示，障礙物1與障礙物2建立連鎖。

（2）分別作移動小車到障礙物1和移動小車到障礙物2的連接線并延長，形成圖2中的斜線陰影區(qū)域。若有其它障礙物在該區(qū)域內(nèi)，則視其被遮擋，不進(jìn)行連鎖。圖2中障礙物3被遮擋，而障礙物4未被遮擋。該過程在算法中的實現(xiàn)方法如圖3所示，連接OA、OB、AC、BC，然后進(jìn)行向量叉乘。若向量OA x OB與向量CA x CB異號，且向量AO x AC與向量Bo x BC異號，則可判定C點處的障礙物3被遮擋。

（3）選擇其它未被連鎖或遮擋的障礙物，重復(fù)步驟（1）和（2）。

2.3隨機目標(biāo)點法

在復(fù)雜的障礙物環(huán)境中，為更好地解決局部最小值問題，除采用障礙物連鎖網(wǎng)絡(luò)結(jié)構(gòu)外，還可結(jié)合應(yīng)用隨機目標(biāo)點法。由于局部最小值的出現(xiàn)是因為移動小車所受合力為零，所以只要打破力的平衡，即可使小車逃出陷阱。本文在道路終點處原有目標(biāo)點左右兩側(cè)各取一段距離，當(dāng)與目標(biāo)點距離大于設(shè)定閾值G時，用該距離范圍內(nèi)的隨機點代替原有目標(biāo)點，以此改變目標(biāo)點對小車的引力值，破壞力的平衡，從而使移動小車總體仍朝著目標(biāo)方向前進(jìn)。當(dāng)與目標(biāo)點距離小于設(shè)定閾值G后，則再次更改回原有目標(biāo)點。

3算法實現(xiàn)與仿真分析

3.1算法實現(xiàn)

為彌補傳統(tǒng)人工勢場法的不足，本文對斥力勢場函數(shù)進(jìn)行改進(jìn)，以適應(yīng)復(fù)雜的障礙物環(huán)境，并結(jié)合障礙物連鎖網(wǎng)絡(luò)結(jié)構(gòu)與隨機目標(biāo)點法改善局部最小點問題。算法實現(xiàn)步驟如下：

第1步：建立移動小車定位坐標(biāo)系。

第2步：設(shè)置障礙物，初始化移動小車各項參數(shù)，確定引力場系數(shù)、斥力場系數(shù)、與目標(biāo)點設(shè)定距離閾值G和障礙物允許連鎖距離D。

第3步：統(tǒng)計移動小車傳感器探測范圍內(nèi)的障礙物，對障礙物進(jìn)行連鎖，將被遮擋障礙物設(shè)置為傳感器無法探測。

第4步：計算移動小車與目標(biāo)點之間的引力。

第5步：計算障礙物位置斥力和速度斥力。

第6步：計算道路左右邊界斥力。

第7步：根據(jù)虛擬力值確定移動小車下一步坐標(biāo)值。

第8步：計算移動小車到目標(biāo)點距離。如果小車到目標(biāo)點距離大于給定值，則利用隨機目標(biāo)點法使目標(biāo)點位置進(jìn)行隨機變動，由此使小車逃離局部最優(yōu)解。

第9步：判斷移動小車到目標(biāo)點距離是否小于設(shè)定閾值G，如果是，則認(rèn)為已到達(dá)目標(biāo)點，如果不是，則轉(zhuǎn)到第2步。

3.2算法仿真分析

為驗證改進(jìn)方法是否有效，本文采用MATLAB軟件進(jìn)行仿真分析。在軟件中建立長40m、寬7m的黃色車道，左右紅色三角形符號分別代表起始點和目標(biāo)點，黑色圓形代表靜態(tài)障礙物，最小的藍(lán)色小圓點代表動態(tài)障礙物，例如道路上的行人。動態(tài)障礙物運動并留下藍(lán)色軌跡線，障礙物信息由移動小車車身上所帶傳感器獲取。

圖5為只改進(jìn)了斥力勢場函數(shù)后的仿真結(jié)果，紅色線為移動小車運動軌跡線，可以看到在既有靜態(tài)障礙物也有動態(tài)障礙物的復(fù)雜環(huán)境中，移動小車成功規(guī)劃出一條從起始點到目標(biāo)點的安全無碰撞路徑（彩圖見封三）。

圖6（a）為當(dāng)車道中增加障礙物A，形成障礙物陷阱1后，僅改變了斥力場函數(shù)的移動小車掉入陷阱中發(fā)生碰撞，路徑規(guī)劃失敗。但在算法中添加障礙物連鎖網(wǎng)絡(luò)結(jié)構(gòu)后，障礙物陷阱中的各個障礙物連鎖成為一個整體，小車安全繞過該陷阱，如圖6（b）所示。

圖7為當(dāng)移動小車運動到障礙物陷阱2處后，小車受到的合力為零，陷入局部最小值。小車在此處繞圈且停滯不前，無法到達(dá)最終目標(biāo)點。圖8中紅色叉形符號代表起始點和隨機目標(biāo)點，其仿真結(jié)果說明在采用沿原目標(biāo)點上下跳動的隨機目標(biāo)點后，由于目標(biāo)點位置已發(fā)生改變，小車所受引力值和斥力值之和不再為零，因此小車得以確定下一步前進(jìn)方向，從而跳出陷阱區(qū)域2。在行駛至離原目標(biāo)點距離小于設(shè)定閾值后，移動小車重新更改為以原目標(biāo)點為終點，最終路徑規(guī)劃成功。

4結(jié)語

為解決傳統(tǒng)人工勢場法動態(tài)環(huán)境路徑規(guī)劃能力差、容易陷入局部最小值及目標(biāo)不可達(dá)等問題，更好地適應(yīng)復(fù)雜的實際行車道路環(huán)境，本文作了以下幾方面改進(jìn)：首先建立道路邊界約束勢場，改進(jìn)障礙物位置斥力勢場，增加障礙物速度斥力勢場，以完成對動態(tài)障礙物的避障，并解決目標(biāo)不可達(dá)問題;其次針對密集障礙物群形成的U型或C型障礙物陷阱，采用障礙物連鎖網(wǎng)絡(luò)結(jié)構(gòu)，將障礙物群連鎖為一個新的障礙物，以避免智能小車掉入陷阱發(fā)生碰撞或陷入局部最小值;最后為更好地解決局部最小值問題，采用隨機目標(biāo)點法改變目標(biāo)點位置，從而使小車在到達(dá)目標(biāo)點前所受合力不為零。通過Matlab仿真結(jié)果可知，該改進(jìn)方法是有效的，但其在復(fù)雜環(huán)境下規(guī)劃出的路徑仍然存在不夠光滑及非最短等問題，有待后續(xù)進(jìn)一步完善。