基于橢圓模型人工勢場法的移動機器人路徑規(guī)劃研究

顏海彬，劉冉冉，臧傳濤

（江蘇理工學院 汽車與交通工程學院，江蘇 常州 213001）

作為移動機器人（MR）研究的重要技術(shù)之一，路徑規(guī)劃在人工智能的各個方面均有所體現(xiàn)。路徑規(guī)劃是從起始位置到目標位置，尋找一條最合適快捷的路線[1]，其常用方法有遺傳算法（GA）、快速搜索隨機樹算法（RRT）、人工勢場法（APF）等。

RRT算法是以采樣為基礎的，在進行路徑規(guī)劃的時候需要對空間環(huán)境進行采樣，通過搜索樹向空白空間不斷擴展，形成完整狀態(tài)空間模型，以避免對狀態(tài)空間的建模。由于該算法使用均勻的采樣策略，因此，導致計算量大、算法運行較慢；且最鄰近點的選擇只考慮了離隨機樹中間節(jié)點的距離，因而在一定程度上影響了算法的搜索結(jié)果[2]。GA算法屬于一種進化算法，它尋找最優(yōu)解的方式類似于自然界物種的遺傳與進化機理。GA算法具備較高的搜索效率，同時，易與其他算法相結(jié)合使用；但實現(xiàn)較為復雜，需要先對所求解的問題編碼，解算出來后再對問題進行解碼[3]。APF算法是一種根據(jù)梯度下降法求最優(yōu)解的方法，其結(jié)構(gòu)簡單、程序易于實現(xiàn)，能夠快速規(guī)劃出一條安全的平滑路徑；因此，廣泛應用于規(guī)避障礙物控制和平滑軌跡方面[4]。APF算法將環(huán)境函數(shù)分成引力場和斥力場，機器人在兩種不同勢場共同作用下從高勢場點位置運動到終點位置來計算路線。但是，在復雜環(huán)境中，容易出現(xiàn)斥力、引力平衡和在目標點附近斥力遠大于引力的情況，造成局部極小值和目標點不可達的難題。為此，Saeid等人[5]針對障礙物周圍終點不能到達的問題，提出了沿墻跟蹤的方法。Lee等人[6]針對路徑規(guī)劃的局部平衡問題，采用設置虛擬障礙物的方法來跳出局部平衡，此方法在規(guī)劃前考慮到機器人的自身尺寸，在MR協(xié)力最大點設置一個虛擬障礙物，從而改變當下機器人所處的勢場，進而改變MR下一步的移動方向；然而，這種逃逸局部平衡問題的方法，需要根據(jù)不同的MR形態(tài)建立不同的模型，移植性低。2020年，劉玉等人[7]提出具有隨機可達集的人工勢場算法，從而提高了在動態(tài)環(huán)境中路徑規(guī)劃的成功率，由于該方法是在采樣基礎上進行的，因此規(guī)劃出的路線平滑度較低。魏智紅[8]在研究多機器人的編隊問題時，就APF算法規(guī)劃路徑出現(xiàn)局部平衡的難題，提出了全局勢力最小點的范圍，在該范圍中，以MR位置與終點位置連線的正交線方向作為MR下一步的移動方向，從而繞過局部平衡點的位置，解決陷入局部平衡的問題；但是，該方法在面對兩邊對稱的通道時，無法從中間通過，因此，跳出局部平衡的路線并非最優(yōu)。

本文在傳統(tǒng)APF算法的基礎上，采用修改斥力函數(shù)和建立橢圓模型設置虛擬目標點的方法，以解決勢場法在規(guī)劃路徑過程中可能遇到的無法到達目標點和陷入局部平衡的問題。該算法通過增加終點位置到障礙物位置的距離作為修改斥力勢場函數(shù)的因子，從而解決MR運動到障礙物附近時，因所受引力小于斥力而造成的無法向終點位置靠近的問題；通過構(gòu)建橢圓模型設置虛擬目標點，改變原有的勢場分布，從而改變MR的受力情況，使MR離開局部最小點的位置；最后，對算法進行仿真驗證，得到一條從起始點到終點位置的安全、有效路線。

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

20世紀80年代后期，Khatib根據(jù)物理學中的思想提出了APF算法[9]。該方法最初用于機械臂抓取物體時防止與其他物體存在干涉。假設機械臂的工作環(huán)境是一個虛擬力場，需要抓取的物體周圍存在虛擬的引力，則其他物體周圍存在虛擬的斥力場，機械臂在引力和斥力的共同作用下產(chǎn)生運動。APF就是利用物理中的力學理論來形容人類思維的方式[10]。隨后，研究者們發(fā)現(xiàn)，該方法在移動機器人的路徑規(guī)劃中有不錯的效果，使得到的運動曲線相對平滑了許多。

MR在有障礙物的環(huán)境中進行路徑規(guī)劃時，假設在各個障礙物四周都存在一個一定范圍的排斥力場，在終點位置四周存在一個吸引力場，機器人沿著勢場函數(shù)下降的方向?qū)ふ医K點，完成路徑規(guī)劃。如圖1所示，為MR在環(huán)境中的受力分布。

圖1 MR在環(huán)境中的受力分布

斥力是由障礙物指向MR方向的力，從MR指向終點位置的力是引力方向，根據(jù)力學理論，合并兩個力即得到機器人的下一步移動方向。為得到排斥力和吸引力，需要先設置引力勢場和斥力勢場函數(shù)，以距離作為力場函數(shù)的因變量。設置：MR的位置X=(x,y)；引力勢場函數(shù)Uatt(X)；斥力勢場函數(shù)Urep(X)；兩者的合力勢場函數(shù)Ftotal(X)。

引力場函數(shù)表示為：

式中：Xg表示目標點的位置(xg,yg)；λ1表示引力常數(shù)；ρ(X,Xg)是MR當前位置和終點位置的幾何距離。

對引力勢力函數(shù)中的距離求一階導數(shù)所得到的即為引力：

同理，斥力勢函數(shù)為：

斥力函數(shù)對距離求一階導數(shù)即為斥力：

所以，MR在空間中所受到的合力Ftotal(X)為：

就環(huán)境中的障礙點而言，這些障礙物對機器人形成了排斥力場。當機器人往障礙物方向移動時，MR所受到的斥力會慢慢變大；同樣，當MR向遠離障礙物方向移動時，其所受到的斥力會逐漸減小；而當MR駛離障礙物的影響范圍后，其所受到的斥力不存在。

2 改進人工勢場法

2.1 無法到達終點的難題及解決方案

在龐雜環(huán)境中，MR、障礙物和終點的特殊位置會導致規(guī)劃路徑失敗。例如：在一個狹窄的過道里，MR會來回抖動陷入勢場局部平衡的狀況；當障礙物到終點的距離小于障礙物的影響范圍時，MR將難以找到終點位置；當障礙物處于終點位置附近，MR尚未進入障礙物的影響范圍時，若MR繼續(xù)向障礙物位置前進，障礙物將產(chǎn)生對機器人的斥力，而隨著MR不斷接近，此時勢場中產(chǎn)生的斥力遠大于引力，MR向遠離目標點的方向運動，之后繼續(xù)受到引力影響向目標點位置靠近，如此反復，MR將無法準確地到達目標點位置。

通過傳統(tǒng)APF算法的斥力和引力函數(shù)可知，引力隨機器人與終點坐標距離的增大而增大，斥力隨機器人到障礙物間距的增大而減小[11]。如圖2所示，當終點位置即目標點附近存在障礙物時，機器人所受到的斥力大于引力，致使MR無法成功抵達目標點的位置，在目標點周圍抖動。

目標點位置無法抵達的問題是由于MR在目標點周圍的障礙物所產(chǎn)生的斥力大于目標點的引力；因此，在傳統(tǒng)的斥力函數(shù)基礎上，引入當前點到目標點之間的距離ρ( )X,Xg，根據(jù)不同的情形降低斥力值的大小。

2.2 局部最小值問題及解決方法

在較為復雜的環(huán)境中，有可能出現(xiàn)MR在未抵達終點位置時，其所受到的引力和斥力大小相等、方向相反、合力為0的情況，這將導致MR在目前位置的來回振蕩[12]。常見的局部最小情況如圖3所示。

圖3 MR在局部平衡問題下的分布

近年來，研究人員提出了多種辦法解決局部平衡的問題。如圖4所示，裴以建等人[13]先確定造成局部最小點的障礙物位置信息，判斷機器人與終點位置構(gòu)成直線左右兩邊障礙物的數(shù)目，在較少障礙物的一側(cè)，算出每個障礙物與平衡點的連線與地圖坐標系X軸所構(gòu)成的夾角θi以及終點位置與該平衡點的連線與地圖坐標系X軸正方向的夾角θ，通過對比大小，擬定虛擬目標點的大體位置。由于該方法計算量過于龐大，導致MR跳出局部最小區(qū)域的時間過大，并且生成的路徑并不一定是最優(yōu)的。鄧永娣等人[14]采用一種簡便的局部極小值的跳出方案，設定MR陷入局部最小區(qū)域時，將沿著機器人到目標的正交方向移動，實際上，相當于在正交方向上設置了一個固定的虛擬目標點。該方法雖然可以跳出局部最小區(qū)域，但是，在面對狹長的通道時，MR做了無用的避障且路線不一定是最佳的。理論上，增加一個適當?shù)奶摂M勢場可以有效解決局部平衡的難題。鑒于以上方法存在計算量大、路徑不是最優(yōu)等問題，本文提出建立橢圓模型來設置虛擬目標點的方法。

圖4 局部平衡比較夾角法

如圖5所示，針對障礙物對稱分布在MR與終點位置連線兩側(cè)的情況，用橢圓模型來確定虛擬目標點的最佳位置。虛擬目標點產(chǎn)生的引力將改變機器人在局部最小值的狀態(tài)，通過在橢圓模型上不斷修正虛擬目標點的位置，從而使MR跳出局部平衡區(qū)域。

圖5 橢圓模型建立虛擬目標點

根據(jù)人工勢場法原理，MR距離終點位置越遠，產(chǎn)生的引力就越大；因此，在面對上述情況時，設置的虛擬目標點給機器人帶來的引力要大于實際目標所施加的引力，并且方向保持不變。MR在障礙物和虛擬目標點的勢場下，斥力大小和方向不變，引力增大，原來的平衡力被打破，此時機器人向虛擬的終點移動。

在圖5坐標系中，假設機器人陷入局部平衡的坐標為Pmin(xmin,ymin)，終點位置坐標為Pgoal(xgoal,ygoal)，則這兩點的中點坐標位置Pb為：

以Pmin和Pgoal兩點的連線作為新軸X，，以Pb為原點，建立過原點垂直于X，的軸Y，，新的坐標系是由原坐標系經(jīng)過旋轉(zhuǎn)平移得到的，則：

據(jù)此，橢圓模型進一步確定虛擬目標點的可行位置，以橢圓與X，軸的正交點為初始的虛擬目標點，機器人按此虛擬目標點行走；若檢測到再次陷入局部平衡時，則構(gòu)建新的橢圓模型，尋找新的虛擬目標點。對規(guī)劃后的路徑再進行二次優(yōu)化，從起點開始依次檢測路徑節(jié)點，判斷是否能夠直線到達。若能直線到達，則刪除起點到該節(jié)點的路徑；若不能直線到達，則將該節(jié)點位置當作起點位置繼續(xù)做能否直線到達的判斷，從而簡化路徑。

如圖6所示，為改進勢場法（APF-N）流程。

圖6 路徑規(guī)劃APF-N算法流程圖

3 仿真結(jié)果及分析

如圖7所示，將傳統(tǒng)APF算法與改進后的方法進行仿真對比分析，構(gòu)建12×12的環(huán)境地圖來驗證改進算法在處理目標點不可達問題中的可行性。環(huán)境參數(shù)如表1所示。通過仿真對比分析可以看到：改進后的人工勢場法和傳統(tǒng)勢場法規(guī)劃出來的路線略有不同，在目標點附近，傳統(tǒng)人工勢場法的紅色路線并沒有準確到達終點位置，而改進后的勢場法藍色路線成功到達了終點位置。

表1 處理目標點不可達問題的環(huán)境參數(shù)

圖7 人工勢場法改進前后的對比

如表2所示，為路徑規(guī)劃代價值。雖然，改進后的人工勢場法很好地解決了無法到達終點的難題，但是從仿真的時間和迭代次數(shù)來看，改進后的人工勢場法在速度和節(jié)點的搜索次數(shù)上都是有明顯劣勢的。如圖8改進勢場法合力圖中顯示，合力在起點的時候處于最大值，原因在于目標點距離起點很遠，引力遠遠大于斥力，而起始點到最近障礙物的距離大于一個單位，斥力較小。此時，根據(jù)梯度下降法尋找合力函數(shù)的局部最小值，搜索到目標點后停止迭代搜索，在目標點處合力達到局部最小值，完成路徑規(guī)劃。

表2 路徑規(guī)劃代價值

圖8 改進勢場法合力

針對局部最小值的情況，傳統(tǒng)人工勢場法會造成陷入一點無法移動，目前常用的方法是設置中間虛擬目標點來跳出局部最小值。本文通過局部最小值點和目標點構(gòu)建橢圓模型，設置虛擬目標點來解決全局最小值的問題，再對規(guī)劃后的路線進行優(yōu)化，簡化路線，去掉不必要的位置信息。環(huán)境參數(shù)如表3所示。

表3 處理局部最小問題的環(huán)境參數(shù)

如圖9所示，為采用傳統(tǒng)人工勢場法在該地圖環(huán)境中規(guī)劃路徑時的效果?？梢钥闯?，該方法在規(guī)劃的初始階段便陷入了局部平衡的狀態(tài)，無法繼續(xù)規(guī)劃路徑。文獻[13]中采取夾角比較的方法，如圖10所示，為對該方法進行仿真得到的規(guī)劃路線。比較圖9和圖10可以看出，后者成功解決了局部最小值問題。如表4所示，為夾角設置法與APF-N法的參數(shù)對比。

表4 夾角設置法與APF-N法的參數(shù)對比

圖9 APF算法在龐雜環(huán)境下搜索路徑

圖10 夾角比較路徑規(guī)劃

如圖11所示，為采用橢圓模型設置虛擬目標點的方法對該環(huán)境進行規(guī)劃仿真，并對規(guī)劃出的路線進行優(yōu)化。綠色點處是APF-N方法在規(guī)劃路徑的過程中出現(xiàn)局部平衡的位置，藍色線是該方法規(guī)劃出的路線?？梢钥闯?，規(guī)劃出的路徑并不是最優(yōu)的，其多次出現(xiàn)棱角。通過路徑優(yōu)化方法，刪除不必要的節(jié)點，簡化路徑，可以得到一條從起始點到目標點的簡化路線。優(yōu)化路線參數(shù)如表5所示。

表5 優(yōu)化路線參數(shù)

圖11 橢圓模型勢場法路徑規(guī)劃及曲線優(yōu)化

仿真實驗數(shù)據(jù)顯示，采用APF-N法規(guī)劃路徑的時間明顯較短，且規(guī)劃出的路徑距離基本一致。優(yōu)化路線的算法雖然由于在APF-N算法執(zhí)行的過程中加入了優(yōu)化程序，使規(guī)劃時間多出了0.116 0 s；但是，從規(guī)劃出的路徑來看，距離相對于初始APF-N算法節(jié)省了11.19 m，大大減少了路徑規(guī)劃的長度。

4 結(jié)論

針對在移動機器人路徑規(guī)劃中，傳統(tǒng)人工勢場法存在終點位置無法準確到達和容易陷入局部平衡的問題，本文采用完善斥力函數(shù)和建立橢圓模型確定虛擬目標點的方法加以解決。（1）根據(jù)移動機器人位置與障礙物之間的距離，在0到障礙物影響范圍上限值的一半、障礙物影響范圍的一半到上限值、超出上限值三種情況，增設當前點到目標點的距離因子，構(gòu)建斥力勢場函數(shù)，從而保證MR能夠安全到達終點位置。（2）通過構(gòu)建橢圓模型確定虛擬目標點的位置，增大目標點的引力，打破平衡，從而克服局部平衡問題。

仿真分析顯示，上述改進算法成功地解決了無法到達終點和局部平衡的問題，并且該方法在路徑規(guī)劃用時上，比夾角設置法節(jié)約了近5倍時間，大大降低了計算成本；同時，優(yōu)化路線在保障安全的前提下縮短了約11 m。改進算法不僅可以應用到掃地機器人、機械臂等復雜環(huán)境下的路徑規(guī)劃問題，而且還可以拓展到智能車路徑規(guī)劃的參考算法。然而，該方法也存在一定不足，如：模型僅考慮單個質(zhì)點機器人的路徑規(guī)劃，忽略了機器人的自身尺寸對整個環(huán)境的影響；僅考慮靜態(tài)環(huán)境中的路徑規(guī)劃，而忽略了在動態(tài)環(huán)境中的相關因素。在實際的路徑規(guī)劃中，許多動態(tài)不確定因素都是潛在的，從而影響到規(guī)劃結(jié)果的準確性，在以后的研究中，應對此加以關注。