顏海彬，劉冉冉，臧傳濤

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

2.1 無法到達(dá)終點的難題及解決方案

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

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

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

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

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

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

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

圖4 局部平衡比較夾角法

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

圖5 橢圓模型建立虛擬目標(biāo)點

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

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

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

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

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

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

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

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

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

圖7 人工勢場法改進(jìn)前后的對比

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

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

圖8 改進(jìn)勢場法合力

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

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

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

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

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

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

如圖11所示，為采用橢圓模型設(shè)置虛擬目標(biāo)點的方法對該環(huán)境進(jìn)行規(guī)劃仿真，并對規(guī)劃出的路線進(jìn)行優(yōu)化。綠色點處是APF-N方法在規(guī)劃路徑的過程中出現(xiàn)局部平衡的位置，藍(lán)色線是該方法規(guī)劃出的路線?？梢钥闯?，規(guī)劃出的路徑并不是最優(yōu)的，其多次出現(xiàn)棱角。通過路徑優(yōu)化方法，刪除不必要的節(jié)點，簡化路徑，可以得到一條從起始點到目標(biāo)點的簡化路線。優(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)人工勢場法存在終點位置無法準(zhǔn)確到達(dá)和容易陷入局部平衡的問題，本文采用完善斥力函數(shù)和建立橢圓模型確定虛擬目標(biāo)點的方法加以解決。（1）根據(jù)移動機器人位置與障礙物之間的距離，在0到障礙物影響范圍上限值的一半、障礙物影響范圍的一半到上限值、超出上限值三種情況，增設(shè)當(dāng)前點到目標(biāo)點的距離因子，構(gòu)建斥力勢場函數(shù)，從而保證MR能夠安全到達(dá)終點位置。（2）通過構(gòu)建橢圓模型確定虛擬目標(biāo)點的位置，增大目標(biāo)點的引力，打破平衡，從而克服局部平衡問題。

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