基于人工勢場法的移動(dòng)機(jī)器人局部路徑規(guī)劃

王迪，李彩虹，郭娜，張寧

(山東理工大學(xué) 計(jì)算機(jī)科學(xué)與技術(shù)學(xué)院，山東 淄博 255049)

在有障礙物的環(huán)境中，按照一定的評價(jià)標(biāo)準(zhǔn)，尋找一條安全無碰的繞過障礙物、從起始點(diǎn)到目標(biāo)點(diǎn)的路徑，是移動(dòng)機(jī)器人路徑規(guī)劃要解決的問題。路徑規(guī)劃用于GPS導(dǎo)航、水下導(dǎo)航系統(tǒng)、無人機(jī)、移動(dòng)機(jī)器人避障系統(tǒng)等，具有廣泛的應(yīng)用領(lǐng)域及較高的應(yīng)用價(jià)值。

路徑規(guī)劃分為全局路徑規(guī)劃和局部路徑規(guī)劃，即環(huán)境信息已知的和環(huán)境信息未知的路徑規(guī)劃。全局路徑規(guī)劃算法有蟻群算法、遺傳算法、A*算法、柵格法等，局部路徑規(guī)劃算法有模糊控制[1]、人工勢場法、神經(jīng)網(wǎng)絡(luò)[2]等，其中人工勢場法以其簡單的原理、少量的計(jì)算、實(shí)時(shí)性好以及平滑的路徑等優(yōu)點(diǎn)得到廣泛應(yīng)用。但是傳統(tǒng)人工勢場法存在局部極小點(diǎn)和目標(biāo)不可達(dá)的問題。局部極小點(diǎn)問題是移動(dòng)機(jī)器人因在某一范圍內(nèi)產(chǎn)生勢場的局部極小點(diǎn)而無法到達(dá)目標(biāo)點(diǎn)。目標(biāo)不可達(dá)問題是移動(dòng)機(jī)器人在障礙物或目標(biāo)點(diǎn)前因斥力大于引力使得移動(dòng)機(jī)器人無法到達(dá)目標(biāo)點(diǎn)。國內(nèi)外學(xué)者針對局部極小點(diǎn)問題提出了兩種解決方案。一種方案是將人工勢場法與其他算法結(jié)合起來克服人工勢場法的缺陷。韓偉[3]提出了模糊人工勢場法來解決局部極小點(diǎn)問題；劉芳[4]將人工勢場法與免疫算法相結(jié)合來解決因局部極小值而陷入死鎖的情況；Chen等[5]加入混沌優(yōu)化方法對人工勢場法進(jìn)行改進(jìn)，解決了局部極小點(diǎn)和目標(biāo)不可達(dá)問題。另一種方案是在傳統(tǒng)人工勢場法的基礎(chǔ)上做改進(jìn)。紀(jì)迪[6]采用多方向沿墻走的方法，從五個(gè)方向中選出合適的方向跳出局部極小點(diǎn)陷阱；溫素芳等[7]提出障礙物影響范圍分層的思想，使移動(dòng)機(jī)器人順利到達(dá)目標(biāo)點(diǎn)；魯新軍等[8]提出水流解決法；梁獻(xiàn)霞等[9]提出通過扇區(qū)劃分的方法在合適的位置添加虛擬障礙物，從而改變合力大小，打破局部極小點(diǎn)僵局；Lee等[10]提出了NP-APF，即在無障礙物的環(huán)境設(shè)置虛擬目標(biāo)點(diǎn)來克服局部極小點(diǎn)和路徑無效率問題。

本文采用基于人工勢場法的左轉(zhuǎn)勢場法和虛擬目標(biāo)點(diǎn)法解決移動(dòng)機(jī)器人在障礙物前震蕩、無法到達(dá)目標(biāo)點(diǎn)的缺陷。通過計(jì)算移動(dòng)機(jī)器人當(dāng)前位置與之前走過位置的距離來判斷移動(dòng)機(jī)器人是否陷入局部極小點(diǎn)陷阱。在未陷入局部極小點(diǎn)陷阱時(shí)移動(dòng)機(jī)器人沿其所受的引力與斥力的合力方向每次移動(dòng)一個(gè)步長，若陷入局部極小點(diǎn)陷阱則通過改變移動(dòng)機(jī)器人的移動(dòng)方向，使其跳出局部極小點(diǎn)陷阱，順利到達(dá)目標(biāo)點(diǎn)。

1 人工勢場法

傳統(tǒng)人工勢場法路徑規(guī)劃是由Khatib于1986年提出的一種虛擬力法[11]。該方法將機(jī)器人、目標(biāo)點(diǎn)、障礙物看做質(zhì)點(diǎn)，目標(biāo)點(diǎn)對機(jī)器人產(chǎn)生引力，障礙物對機(jī)器人產(chǎn)生斥力，由引力與斥力的合力決定機(jī)器人的下一步前進(jìn)坐標(biāo)。實(shí)質(zhì)是在移動(dòng)機(jī)器人運(yùn)動(dòng)的環(huán)境中人為地定義一個(gè)勢力場，目標(biāo)點(diǎn)產(chǎn)生的引力場與障礙物產(chǎn)生的斥力場疊加，共同對移動(dòng)機(jī)器人產(chǎn)生作用，使移動(dòng)機(jī)器人繞開障礙物到達(dá)目標(biāo)點(diǎn)。人工勢場定義為

U=Uatt+Urep

(1)

式中:U為勢場和;Uatt為目標(biāo)點(diǎn)對移動(dòng)機(jī)器人產(chǎn)生的引力場;Urep為障礙物對移動(dòng)機(jī)器人產(chǎn)生的斥力場。定義目標(biāo)點(diǎn)對移動(dòng)機(jī)器人的引力和障礙物對移動(dòng)機(jī)器人的斥力分別對應(yīng)引力場和斥力場的負(fù)梯度，根據(jù)偏導(dǎo)得出人工勢場對機(jī)器人的作用力F的大小為

F=Fatt+Frep

(2)

式中：F為合力；Fatt為目標(biāo)點(diǎn)對移動(dòng)機(jī)器人產(chǎn)生的引力；Frep為障礙物對移動(dòng)機(jī)器人產(chǎn)生的斥力。

1.1 引力場

設(shè)目標(biāo)點(diǎn)的位置為EP，機(jī)器人的位置為X，引力勢場函數(shù)定義為

(3)

式中：k為引力增益系數(shù)；‖X-EP‖為機(jī)器人到目標(biāo)點(diǎn)的歐幾里得距離。目標(biāo)點(diǎn)對機(jī)器人的引力為引力勢場函數(shù)的負(fù)梯度，即

Fatt=k‖X-EP‖

(4)

1.2 斥力場

設(shè)機(jī)器人的位置為X，在傳統(tǒng)人工勢場法的斥力場函數(shù)中添加了機(jī)器人與目標(biāo)點(diǎn)之間的距離[12]。添加機(jī)器人與目標(biāo)點(diǎn)的距離可以保證目標(biāo)點(diǎn)處的勢場為全局最小點(diǎn)，添加距離后的斥力勢場函數(shù)定義為

(5)

式中：m為斥力增益系數(shù);d為機(jī)器人與障礙物的距離;d0為障礙物影響距離;da為機(jī)器人與目標(biāo)點(diǎn)之間的距離。障礙物對機(jī)器人的斥力為

(6)

1.3 受力分析

根據(jù)目標(biāo)點(diǎn)對移動(dòng)機(jī)器人的引力和障礙物對移動(dòng)機(jī)器人的斥力的合力決定移動(dòng)機(jī)器人前進(jìn)的方向。其中，五角星代表機(jī)器人，方形代表障礙物，十字代表目標(biāo)點(diǎn)。受力分析如圖1所示。

圖1 移動(dòng)機(jī)器人受力分析圖Fig.1 Force analysis diagram of mobile robot

2 解決局部極小點(diǎn)的方法

當(dāng)移動(dòng)機(jī)器人遇到某范圍內(nèi)勢場或合力的局部極小點(diǎn)時(shí)就陷入了局部極小點(diǎn)陷阱。局部極小點(diǎn)陷阱體現(xiàn)為移動(dòng)機(jī)器人在某一范圍內(nèi)反復(fù)震蕩。根據(jù)沿墻走的思想，使移動(dòng)機(jī)器人沿著障礙物前進(jìn)，繞開障礙物，跳出局部極小點(diǎn)陷阱。本文采用了兩種方法來解決局部極小點(diǎn)問題：一種是左轉(zhuǎn)勢場法,另一種是虛擬目標(biāo)點(diǎn)法。

2.1 左轉(zhuǎn)勢場法

移動(dòng)機(jī)器人進(jìn)入局部極小點(diǎn)陷阱時(shí)，傳統(tǒng)的人工勢場法以及單純地改變勢場函數(shù)不能很好解決這個(gè)問題，如圖2所示，這是比較經(jīng)典的局部極小點(diǎn)問題。其中五角星為機(jī)器人的起始位置，十字是目標(biāo)點(diǎn)，方形是障礙物，移動(dòng)機(jī)器人、障礙物、目標(biāo)點(diǎn)在一條直線上，雖然合力不為零，但是引力與斥力分量Frep1方向相反，移動(dòng)機(jī)器人未靠近障礙物時(shí)引力大，斥力為零，合力方向指向引力方向，靠近障礙物時(shí)斥力分量Frep1大，合力方向指向Frep1方向，陷入了局部極小點(diǎn)陷阱，導(dǎo)致移動(dòng)機(jī)器人一直在小范圍內(nèi)上下移動(dòng)。左轉(zhuǎn)勢場法利用移動(dòng)機(jī)器人自帶的傳感器(掃描角度為360°)來躲避障礙物，通過計(jì)算目標(biāo)點(diǎn)、障礙物與移動(dòng)機(jī)器人的距離，計(jì)算移動(dòng)機(jī)器人受到的各個(gè)力的大小和方向。當(dāng)檢測到移動(dòng)機(jī)器人在某一范圍內(nèi)徘徊時(shí)，根據(jù)此時(shí)移動(dòng)機(jī)器人所受到的斥力分量Frep1的方向，向左偏轉(zhuǎn)90°移動(dòng)，移動(dòng)機(jī)器人就跳出了局部極小點(diǎn)陷阱。此時(shí)移動(dòng)機(jī)器人所受斥力的分量Frep1的方向如圖3所示。

圖2 局部極小點(diǎn)情況 Fig.2 Local minimum case

圖3 移動(dòng)機(jī)器人所受斥力分量 Frep1方向 Fig.3 The direction of the repulsive force component Frep1 received by the mobile robot

基于左轉(zhuǎn)勢場法的移動(dòng)機(jī)器人遇到局部極小點(diǎn)陷阱時(shí)，下一步移動(dòng)位置的公式為

(7)

式中:(xi+1,yi+1)為移動(dòng)機(jī)器人下一步移動(dòng)位置的坐標(biāo)；(xi,yi)為當(dāng)前移動(dòng)機(jī)器人所在位置的坐標(biāo)；h為步長;ang(Frep1)為Frep1的角度。此時(shí)移動(dòng)機(jī)器人的移動(dòng)方向如圖4中箭頭所指方向，此種方法在遇到U型障礙物和一字型障礙物時(shí)都能很好地避開。因?yàn)檎系K物的影響距離與移動(dòng)機(jī)器人步長大小懸殊，所以移動(dòng)機(jī)器人不會碰到障礙物。

圖4 左轉(zhuǎn)勢場法移動(dòng)機(jī)器人移動(dòng)方向Fig.4 The direction of movement of a mobile robot in the left-turn potential field method

采用左轉(zhuǎn)勢場法的基本流程如圖5所示。

圖5 左轉(zhuǎn)勢場法流程Fig.5 Flow chart of the left-turn potential field method

2.2 虛擬目標(biāo)點(diǎn)法

基于上一小節(jié)的思想，當(dāng)移動(dòng)機(jī)器人在障礙物前陷入局部極小點(diǎn)陷阱而徘徊時(shí)，在移動(dòng)機(jī)器人所受斥力分量Frep1方向向左偏轉(zhuǎn)90°的方向上的合適位置添加一個(gè)虛擬目標(biāo)點(diǎn)，此時(shí)忽略目標(biāo)點(diǎn)對移動(dòng)機(jī)器人的引力和障礙物對移動(dòng)機(jī)器人的斥力，使移動(dòng)機(jī)器人沿虛擬目標(biāo)點(diǎn)的引力方向移動(dòng)，移動(dòng)后撤銷虛擬目標(biāo)點(diǎn)，恢復(fù)障礙物與目標(biāo)點(diǎn)的作用。重復(fù)此步驟，直到移動(dòng)機(jī)器人跳出局部極小點(diǎn)陷阱。虛擬目標(biāo)點(diǎn)法與左轉(zhuǎn)勢場法的區(qū)別是移動(dòng)機(jī)器人在陷入局部極小點(diǎn)陷阱時(shí)，虛擬目標(biāo)法中的轉(zhuǎn)向是因?yàn)橐苿?dòng)機(jī)器人受到虛擬目標(biāo)點(diǎn)的引力作用，而左轉(zhuǎn)勢場法是強(qiáng)制轉(zhuǎn)向，此時(shí)不受任何力的作用。移動(dòng)機(jī)器人陷入局部極小點(diǎn)時(shí)，采用虛擬目標(biāo)點(diǎn)法的下一步移動(dòng)位置由式(7)計(jì)算，移動(dòng)機(jī)器人所受斥力分量Frep1的方向及下一步移動(dòng)方向如圖6所示。虛擬目標(biāo)點(diǎn)法流程如圖7所示。

圖6 移動(dòng)機(jī)器人所受斥力分量Frep1的方向及其移動(dòng)方向Fig.6 The direction of the repulsive force component Frep1 received by the mobile robot and the moving direction of the mobile robot

圖7 虛擬目標(biāo)點(diǎn)法流程Fig.7 Flow chart of the virtual object point method

3 仿真與分析

本文利用Matlab模擬現(xiàn)實(shí)生活中的移動(dòng)機(jī)器人在前進(jìn)過程中遇到障礙物時(shí)如何避開障礙物到達(dá)目標(biāo)點(diǎn)的情形，分別在一字型障礙物、U型障礙物以及混合型障礙物環(huán)境中進(jìn)行。圖8是移動(dòng)機(jī)器人遇到一字型障礙物時(shí)分別基于改進(jìn)勢場函數(shù)的人工勢場法、左轉(zhuǎn)勢場法、虛擬目標(biāo)點(diǎn)法規(guī)劃路徑的仿真圖，其中，紅色十字是虛擬目標(biāo)點(diǎn)。圖9為一字型障礙物環(huán)境下基于左轉(zhuǎn)勢場法、虛擬目標(biāo)點(diǎn)法時(shí)障礙物產(chǎn)生的斥力分量Frep1的大小，可以看出障礙物前形成了局部極小點(diǎn)陷阱。圖10為移動(dòng)機(jī)器人遇到U型障礙物時(shí)分別基于改進(jìn)勢場函數(shù)的人工勢場法、左轉(zhuǎn)勢場法、虛擬目標(biāo)點(diǎn)法的仿真圖，與只改進(jìn)勢場函數(shù)的人工勢場法比較，后兩種方法能夠繞開障礙物且路徑比較平滑。圖11為U型障礙物環(huán)境下移動(dòng)機(jī)器人基于左轉(zhuǎn)勢場法、虛擬目標(biāo)點(diǎn)法進(jìn)行路徑規(guī)劃時(shí)障礙物產(chǎn)生的的斥力分量Frep1的大小，可以看出在U型障礙物內(nèi)形成了局部極小點(diǎn)陷阱。圖12為混合型環(huán)境中移動(dòng)機(jī)器人采用左轉(zhuǎn)勢場法和虛擬目標(biāo)點(diǎn)法規(guī)劃的路徑仿真圖。由路徑仿真圖可以看出，移動(dòng)機(jī)器人可以快速繞過由散點(diǎn)障礙物、U型障礙物和一字型障礙物等混合組成的障礙物環(huán)境，較平滑地到達(dá)目標(biāo)點(diǎn)。

(a) 基于人工勢場法 (b) 基于左轉(zhuǎn)勢場法 (c) 基于虛擬目標(biāo)點(diǎn)法圖8 移動(dòng)機(jī)器人遇到一字型障礙物時(shí)基于不同方法的路徑規(guī)劃Fig.8 Path planning diagram based on different methods when mobile robot encounters a linear obstacle

(a)基于左轉(zhuǎn)勢場法 (b) 基于虛擬目標(biāo)點(diǎn)法圖9 移動(dòng)機(jī)器人遇到一字型障礙物時(shí)受到的斥力分量Frep1的大小 Fig.9 The magnitude of the repulsive force component Frep1 when mobile robot encounters a linear obstacle

(a) 基于人工勢場法 (b)基于左轉(zhuǎn)勢場法 (c)基于虛擬目標(biāo)點(diǎn)法圖10 移動(dòng)機(jī)器人遇到U型障礙物時(shí)基于不同方法的路徑規(guī)劃Fig.10 Path planning diagram based on different methods when mobile robot encounters a U-shaped obstacle

(a)基于左轉(zhuǎn)勢場法(b) 基于虛擬目標(biāo)點(diǎn)法圖11 移動(dòng)機(jī)器人遇到U型障礙物時(shí)受到的斥力分量Frep1的大小Fig.11 The magnitude of the repulsive force component Frep1 when the mobile robot encounters a U-shaped obstacle

(a)基于左轉(zhuǎn)勢場法(b) 基于虛擬目標(biāo)點(diǎn)法圖12 混合環(huán)境下移動(dòng)機(jī)器人的的路徑規(guī)劃 Pig.12 Path planning of mobile robot in a mixed environment

4 結(jié)束語

本文設(shè)計(jì)了左轉(zhuǎn)勢場法和虛擬目標(biāo)點(diǎn)法的避障策略來解決移動(dòng)機(jī)器人采用傳統(tǒng)人工勢場法路徑規(guī)劃存在的局部極小點(diǎn)陷阱問題。通過在Matlab平臺上仿真可以看出，這兩種方法規(guī)劃的路徑均能走出局部極小點(diǎn)陷阱且路徑規(guī)劃效果較好。左轉(zhuǎn)勢場法無需外力，通過強(qiáng)制轉(zhuǎn)向來跳出局部極小點(diǎn)；而虛擬目標(biāo)點(diǎn)法相當(dāng)于將路徑劃分為若干個(gè)階段，虛擬目標(biāo)點(diǎn)是移動(dòng)機(jī)器人前往目標(biāo)點(diǎn)的中轉(zhuǎn)站。盡管兩種方法的最終路徑規(guī)劃效果一樣，但是相對而言，左轉(zhuǎn)勢場法比虛擬目標(biāo)點(diǎn)法更適合于障礙物密集的環(huán)境，在需要頻繁采取轉(zhuǎn)向策略的環(huán)境下，左轉(zhuǎn)勢場法計(jì)算簡單，移動(dòng)方向?qū)Ｒ?，速度快。兩種方法都是通過轉(zhuǎn)向策略跳出局部極小點(diǎn)，方法是對稱的，無論是向左轉(zhuǎn)還是向右轉(zhuǎn)，都能解決相同的問題，但這兩種方法在遇到近似封閉的障礙物環(huán)境時(shí)無法到達(dá)目標(biāo)點(diǎn)，存在局限性。