唐小潔 丁一航 申勤 賈成芬
摘要:為使移動小車更好地適應復雜的實際行車道路環(huán)境,實現(xiàn)動態(tài)障礙物環(huán)境下的路徑規(guī)劃,并解決傳統(tǒng)人工勢場法中的局部最小值問題,提高駕駛安全性,改進傳統(tǒng)人工勢場法中的障礙物斥力勢場模型,并增加了速度斥力勢場模型和道路邊界約束斥力勢場模型,同時將障礙物連鎖網(wǎng)絡結(jié)構(gòu)與隨機目標點法相結(jié)合,加入傳統(tǒng)人工勢場算法中。仿真結(jié)果證明了該改進方法的有效性,移動小車能避開局部最小值陷阱到達目標點,實現(xiàn)動態(tài)避障路徑規(guī)劃。
關(guān)鍵詞:路徑規(guī)劃;人工勢場法;動態(tài)環(huán)境;避障
DOI:10.11907/rjdk.192172開放科學(資源服務)標識碼(OSID):
中圖分類號:TP319文獻標識碼:A 文章編號:1672-7800(2019)010-0152-05
0引言
路徑規(guī)劃是指在已知或未知的障礙物環(huán)境中,搜索出一條從起始位置到目標位置、最優(yōu)或近似最優(yōu)的無碰撞安全路徑。目前,已知環(huán)境的路徑規(guī)劃技術(shù)已十分成熟,但在真實、復雜多變的未知行車道路環(huán)境中,對動態(tài)障礙物的避障問題仍有待進一步研究。
人工勢場法是路徑規(guī)劃算法中一種常用的、發(fā)展已較為成熟高效的方法,其具有數(shù)學原理簡單、計算量小、響應速度快、實時控制性能好等優(yōu)點,因此常用于未知環(huán)境的局部路徑規(guī)劃,但也存在目標不可達、容易陷入局部最優(yōu)、對動態(tài)環(huán)境適應性較差等問題。針對這些問題,文獻提出一種輔助斥力環(huán)法,強調(diào)預防性避障,以實現(xiàn)對動態(tài)目標的追蹤以及對攔截型障礙物的避障,但并未提及如何解決局部最小值問題;文獻將其它規(guī)劃算法與人工勢場法相結(jié)合,在路徑規(guī)劃時間和路徑平滑性上取得了一定的改進效果;文獻為解決局部最小值問題,針對常見的4種環(huán)境場景,提出一種斥力偏轉(zhuǎn)模型。但在以上文獻中,避障環(huán)境均假設為無邊界平面,因此其實用性仍有待提高。
本文考慮到實際行車道路環(huán)境,首先在人工勢場算法中增加了道路邊界約束以提高其實用性和安全性;其次將動態(tài)障礙物也納入到障礙物環(huán)境中,充分考慮到動態(tài)障礙物速度,建立速度斥力場;之后改進障礙物斥力場模型,增加移動小車與目標點之間的距離,解決目標不可達問題;采用障礙物連鎖網(wǎng)絡結(jié)構(gòu),將密集的障礙物群體連鎖為新的障礙物結(jié)構(gòu),使移動小車能繞開該群體,無碰撞地到達目標位置;最后為避免移動小車在陷阱區(qū)域因所受合力為零陷入局部最小值,采用隨機目標點法使移動小車跳出陷阱到達目標點。
1傳統(tǒng)人工勢場法
人工勢場的基本思想是將移動小車所處障礙物環(huán)境虛擬為引力場和斥力場。其中目標點對移動小車產(chǎn)生引力,使移動小車向目標點移動,障礙物對移動小車有斥力,使移動小車不與其發(fā)生碰撞。移動小車在路徑上每一點所受合力即為引力與斥力的矢量和。在合力的引導下,移動小車從初始位置沿著勢場下降最快的方向到達目標點。
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)人工勢場法只考慮了小車與目標點,以及障礙物之間的距離信息,因此通常用于靜態(tài)環(huán)境的路徑規(guī)劃,對動態(tài)環(huán)境的適應性較差。當出現(xiàn)動態(tài)障礙物或動態(tài)目標點時,傳統(tǒng)人工勢場法會因為實時性較差而無法順利到達目標點。
1.3.3目標不可達問題
當小車與目標點距離很近時,若目標點附近有障礙物,小車所受引力相對于此時極大的斥力近似為零,小車將一直在目標點附近徘徊而無法到達目標點。
1.3.4局部最小值問題
當局部區(qū)域出現(xiàn)多個障礙物時,若小車所受引力和斥力矢量和即合力為零,此時小車陷入局部勢能最低點,產(chǎn)生局部最優(yōu)解,導致小車在該處徘徊往復、停滯不前,路徑規(guī)劃失敗。
1.3.5對復雜障礙物環(huán)境適應性差
當多個障礙物形成U型或C型的環(huán)繞型狹窄區(qū)域時,小車在所受合力引導下,極易掉入障礙物陷阱中發(fā)生碰撞而無法逃離。
2改進人工勢場法
2.1斥力場改進
2.1.1道路邊界約束斥力場建立
為解決上述傳統(tǒng)人工勢場法不符合實際道路行車環(huán)境的問題,建立道路邊界約束斥力勢場,通過勢場給移動小車施加虛擬斥力,使其在道路邊界線內(nèi)行駛,從而限制了移動小車行駛區(qū)域??紤]到小車本身具有一定寬度,因此道路左右邊界勢場公式分別為:
2.1.2障礙物斥力場改進
由人工勢場法中的引力與斥力公式可知,引力隨著移動小車與目標點之間距離的縮短而減小,斥力會隨著移動小車與障礙物之間距離的縮短而增大,因此小車得以避開障礙物到達正確的目標位置。但當目標點附近有障礙物時,移動小車會因為所受斥力太大、引力太小而產(chǎn)生目標不可達問題,所以在傳統(tǒng)人工勢場法的障礙物斥力場模型中,將移動小車與目標點之間距離也考慮進來,使移動小車在靠近目標點過程中,斥力也逐漸減小至零,以解決目標不可達問題。
2.1.3
速度斥力場建立
為改善人工勢場法的動態(tài)環(huán)境規(guī)劃能力,提高小車對動態(tài)障礙物的避障能力,以適應實際行車環(huán)境,將障礙物速度考慮進來,建立速度斥力場。速度斥力場函數(shù)為:
2.2障礙物連鎖網(wǎng)絡結(jié)構(gòu)
當小車在障礙物比較密集的狹窄環(huán)境中行駛時,在目標點引力與多個障礙物斥力的共同作用下,極易進入狹窄區(qū)域,掉入陷阱中發(fā)生碰撞,或達到力平衡后陷入局部最小值。當多個障礙物呈U型或C型分布時,該情況尤其容易發(fā)生。針對該問題,本文擬采用障礙物連鎖網(wǎng)絡結(jié)構(gòu),將近距離密集分布的多個障礙物連鎖起來,形成一個新的大障礙物,進行大范圍區(qū)域避障。具體操作方法為:
(1)設定允許連鎖距離D,搜索障礙物1周圍是否存在與其距離小于D的其它障礙物,如果有,則與障礙物1距離最小的其它障礙物建立連鎖。如圖2所示,障礙物1與障礙物2建立連鎖。
(2)分別作移動小車到障礙物1和移動小車到障礙物2的連接線并延長,形成圖2中的斜線陰影區(qū)域。若有其它障礙物在該區(qū)域內(nèi),則視其被遮擋,不進行連鎖。圖2中障礙物3被遮擋,而障礙物4未被遮擋。該過程在算法中的實現(xiàn)方法如圖3所示,連接OA、OB、AC、BC,然后進行向量叉乘。若向量OA x OB與向量CA x CB異號,且向量AO x AC與向量Bo x BC異號,則可判定C點處的障礙物3被遮擋。
(3)選擇其它未被連鎖或遮擋的障礙物,重復步驟(1)和(2)。
2.3隨機目標點法
在復雜的障礙物環(huán)境中,為更好地解決局部最小值問題,除采用障礙物連鎖網(wǎng)絡結(jié)構(gòu)外,還可結(jié)合應用隨機目標點法。由于局部最小值的出現(xiàn)是因為移動小車所受合力為零,所以只要打破力的平衡,即可使小車逃出陷阱。本文在道路終點處原有目標點左右兩側(cè)各取一段距離,當與目標點距離大于設定閾值G時,用該距離范圍內(nèi)的隨機點代替原有目標點,以此改變目標點對小車的引力值,破壞力的平衡,從而使移動小車總體仍朝著目標方向前進。當與目標點距離小于設定閾值G后,則再次更改回原有目標點。
3算法實現(xiàn)與仿真分析
3.1算法實現(xiàn)
為彌補傳統(tǒng)人工勢場法的不足,本文對斥力勢場函數(shù)進行改進,以適應復雜的障礙物環(huán)境,并結(jié)合障礙物連鎖網(wǎng)絡結(jié)構(gòu)與隨機目標點法改善局部最小點問題。算法實現(xiàn)步驟如下:
第1步:建立移動小車定位坐標系。
第2步:設置障礙物,初始化移動小車各項參數(shù),確定引力場系數(shù)、斥力場系數(shù)、與目標點設定距離閾值G和障礙物允許連鎖距離D。
第3步:統(tǒng)計移動小車傳感器探測范圍內(nèi)的障礙物,對障礙物進行連鎖,將被遮擋障礙物設置為傳感器無法探測。
第4步:計算移動小車與目標點之間的引力。
第5步:計算障礙物位置斥力和速度斥力。
第6步:計算道路左右邊界斥力。
第7步:根據(jù)虛擬力值確定移動小車下一步坐標值。
第8步:計算移動小車到目標點距離。如果小車到目標點距離大于給定值,則利用隨機目標點法使目標點位置進行隨機變動,由此使小車逃離局部最優(yōu)解。
第9步:判斷移動小車到目標點距離是否小于設定閾值G,如果是,則認為已到達目標點,如果不是,則轉(zhuǎn)到第2步。
3.2算法仿真分析
為驗證改進方法是否有效,本文采用MATLAB軟件進行仿真分析。在軟件中建立長40m、寬7m的黃色車道,左右紅色三角形符號分別代表起始點和目標點,黑色圓形代表靜態(tài)障礙物,最小的藍色小圓點代表動態(tài)障礙物,例如道路上的行人。動態(tài)障礙物運動并留下藍色軌跡線,障礙物信息由移動小車車身上所帶傳感器獲取。
圖5為只改進了斥力勢場函數(shù)后的仿真結(jié)果,紅色線為移動小車運動軌跡線,可以看到在既有靜態(tài)障礙物也有動態(tài)障礙物的復雜環(huán)境中,移動小車成功規(guī)劃出一條從起始點到目標點的安全無碰撞路徑(彩圖見封三)。
圖6(a)為當車道中增加障礙物A,形成障礙物陷阱1后,僅改變了斥力場函數(shù)的移動小車掉入陷阱中發(fā)生碰撞,路徑規(guī)劃失敗。但在算法中添加障礙物連鎖網(wǎng)絡結(jié)構(gòu)后,障礙物陷阱中的各個障礙物連鎖成為一個整體,小車安全繞過該陷阱,如圖6(b)所示。
圖7為當移動小車運動到障礙物陷阱2處后,小車受到的合力為零,陷入局部最小值。小車在此處繞圈且停滯不前,無法到達最終目標點。圖8中紅色叉形符號代表起始點和隨機目標點,其仿真結(jié)果說明在采用沿原目標點上下跳動的隨機目標點后,由于目標點位置已發(fā)生改變,小車所受引力值和斥力值之和不再為零,因此小車得以確定下一步前進方向,從而跳出陷阱區(qū)域2。在行駛至離原目標點距離小于設定閾值后,移動小車重新更改為以原目標點為終點,最終路徑規(guī)劃成功。
4結(jié)語
為解決傳統(tǒng)人工勢場法動態(tài)環(huán)境路徑規(guī)劃能力差、容易陷入局部最小值及目標不可達等問題,更好地適應復雜的實際行車道路環(huán)境,本文作了以下幾方面改進:首先建立道路邊界約束勢場,改進障礙物位置斥力勢場,增加障礙物速度斥力勢場,以完成對動態(tài)障礙物的避障,并解決目標不可達問題;其次針對密集障礙物群形成的U型或C型障礙物陷阱,采用障礙物連鎖網(wǎng)絡結(jié)構(gòu),將障礙物群連鎖為一個新的障礙物,以避免智能小車掉入陷阱發(fā)生碰撞或陷入局部最小值;最后為更好地解決局部最小值問題,采用隨機目標點法改變目標點位置,從而使小車在到達目標點前所受合力不為零。通過Matlab仿真結(jié)果可知,該改進方法是有效的,但其在復雜環(huán)境下規(guī)劃出的路徑仍然存在不夠光滑及非最短等問題,有待后續(xù)進一步完善。