顏海彬，劉冉冉，臧傳濤

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

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

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

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

1 傳統(tǒng)人工勢(shì)場(chǎng)法

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

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

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

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

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

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

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

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

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

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

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

2 改進(jìn)人工勢(shì)場(chǎng)法

2.1 無(wú)法到達(dá)終點(diǎn)的難題及解決方案

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

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

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

2.2 局部最小值問(wèn)題及解決方法

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

圖3 MR在局部平衡問(wèn)題下的分布

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

圖4 局部平衡比較夾角法

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

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

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

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

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

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

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

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

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

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

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

圖7 人工勢(shì)場(chǎng)法改進(jìn)前后的對(duì)比

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

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

圖8 改進(jìn)勢(shì)場(chǎng)法合力

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

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

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

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

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

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

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

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

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

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

4 結(jié)論

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

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