基于A*算法和人工勢(shì)場(chǎng)法的移動(dòng)機(jī)器人路徑規(guī)劃

王洪斌 郝 策 張 平 張明泉 尹鵬衡 張永順

1.燕山大學(xué)工業(yè)計(jì)算機(jī)控制工程河北省重點(diǎn)實(shí)驗(yàn)室，秦皇島，0660042.燕山大學(xué)國(guó)家冷軋帶鋼裝備及工藝工程技術(shù)研究中心，秦皇島，066004

0 引言

近年來，移動(dòng)機(jī)器人的路徑規(guī)劃問題成為研究熱點(diǎn)，其主要思路是[1]根據(jù)能耗、路程、時(shí)間等性能指標(biāo)，保證在存在障礙物的環(huán)境下，規(guī)劃出一條從初始位置到目標(biāo)位置的最優(yōu)路徑。在以往的研究中，根據(jù)環(huán)境信息的完全已知、部分未知或者完全未知等不同特點(diǎn)，通常把移動(dòng)機(jī)器人的路徑規(guī)劃分為兩部分：全局路徑規(guī)劃[2-3]和局部路徑規(guī)劃[4-5]。

環(huán)境建模和路徑搜索是全局路徑規(guī)劃研究中的兩個(gè)重要問題。在研究環(huán)境建模時(shí)主要應(yīng)用可視圖法[6]、拓?fù)浞ê蜄鸥穹?。柵格法[7]是由柵格構(gòu)成一個(gè)連通圖，依據(jù)障礙物的分布，用搜索算法(A*算法[8]、遺傳算法[9]、粒子群算法[10]、蟻群算法[11]等)生成從初始節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的最優(yōu)路徑。粒子群算法[12]利用較小種群模式，通過單向信息流機(jī)制搜索最優(yōu)路徑。文獻(xiàn)[13]融合了最大最小蟻群算法與蟻群系統(tǒng)算法的信息素更新方式，設(shè)計(jì)了與環(huán)境模型相符合的適應(yīng)度函數(shù)，實(shí)現(xiàn)了路徑規(guī)劃。但上述算法數(shù)據(jù)復(fù)雜，計(jì)算量大，很難快速規(guī)劃出一條平滑的最優(yōu)路徑。作為一種典型的啟發(fā)式搜索算法，A*算法能夠遍歷所有可達(dá)節(jié)點(diǎn)，再根據(jù)估價(jià)函數(shù)來搜索較優(yōu)路徑。文獻(xiàn)[14]提出跳點(diǎn)搜索策略，減少了遍歷過程中所需訪問的節(jié)點(diǎn)數(shù)，運(yùn)行速度很快，但路徑中仍存在轉(zhuǎn)折點(diǎn)。由于移動(dòng)機(jī)器人自身存在非完整約束，在轉(zhuǎn)折點(diǎn)處不能連續(xù)運(yùn)動(dòng)，為了解決路徑平滑問題，文獻(xiàn)[15]引入多項(xiàng)式曲線實(shí)現(xiàn)平滑路徑的生成，但存在曲率峰值過大問題。文獻(xiàn)[16-17]分別提出改進(jìn)的三階Bezier曲線和改進(jìn)Pseudo-Dubins曲線方法，這兩種方法均能實(shí)現(xiàn)路徑平滑，但算法的過程繁瑣，效率低。

規(guī)劃?rùn)C(jī)器人的運(yùn)動(dòng)路徑時(shí)，若存在結(jié)果不可行的情況，一般會(huì)在全局規(guī)劃后進(jìn)一步采用局部路徑規(guī)劃的算法，這種方法能夠有效提高系統(tǒng)的實(shí)時(shí)性。目前，主要應(yīng)用模糊邏輯算法、遺傳算法和人工勢(shì)場(chǎng)法等方法來進(jìn)行局部路徑規(guī)劃。其中，人工勢(shì)場(chǎng)法結(jié)構(gòu)較為簡(jiǎn)單，易于實(shí)現(xiàn)實(shí)時(shí)控制等優(yōu)點(diǎn)，但該算法存在局部極小值問題。針對(duì)上述缺點(diǎn)，文獻(xiàn)[18-20]分別提出了建立多行為體系結(jié)構(gòu)、添加虛擬水流法和增設(shè)虛擬中間目標(biāo)點(diǎn)方法來解決局部極小值問題。但是針對(duì)上述方法的研究中，并沒有綜合考慮路徑長(zhǎng)度、路徑安全性、轉(zhuǎn)彎角度等因素來對(duì)路徑進(jìn)行處理，因此，由這種算法得出的路徑綜合適應(yīng)性不高。

在動(dòng)態(tài)復(fù)雜的未知環(huán)境中，傳統(tǒng)的路徑規(guī)劃方法很難達(dá)到最優(yōu)路徑指標(biāo)，因此，本文提出了一種將全局路徑規(guī)劃與局部路徑規(guī)劃相結(jié)合的混合路徑規(guī)劃算法。在該算法中，需要首先完成全局路徑規(guī)劃，然后在全局路徑的基礎(chǔ)上利用二次A*搜索和動(dòng)態(tài)切點(diǎn)調(diào)整法優(yōu)化已規(guī)劃路徑，最后根據(jù)環(huán)境信息的變化狀況，在全局路徑規(guī)劃的結(jié)果上，采用基于自適應(yīng)步長(zhǎng)調(diào)節(jié)方法的人工勢(shì)場(chǎng)法進(jìn)行局部?jī)?yōu)化。該混合方法不但能根據(jù)已知的環(huán)境信息規(guī)劃出全局最優(yōu)路徑，同時(shí)能在局部變化的環(huán)境中實(shí)現(xiàn)實(shí)時(shí)避障和動(dòng)態(tài)目標(biāo)追蹤，較好地克服了傳統(tǒng)算法復(fù)雜、計(jì)算量大、效率不高等問題。

本文以3自由度非完整輪式移動(dòng)機(jī)器人作為研究對(duì)象，相比2自由度機(jī)器人，移動(dòng)機(jī)器人不能實(shí)現(xiàn)全向移動(dòng)，轉(zhuǎn)折點(diǎn)處不能連續(xù)直線運(yùn)動(dòng)，需要轉(zhuǎn)動(dòng)一定角度調(diào)整姿態(tài)，對(duì)路徑規(guī)劃有了更多的限制，例如路徑中存在的轉(zhuǎn)折點(diǎn)個(gè)數(shù)以及轉(zhuǎn)動(dòng)角度等，因此，研究非完整移動(dòng)機(jī)器人的路徑規(guī)劃意義重大。

1 A*全局路徑規(guī)劃算法

1.1 環(huán)境模型描述

機(jī)器人的環(huán)境模型是對(duì)其進(jìn)行控制的基礎(chǔ)，為了有效地描述機(jī)器人所處環(huán)境，將被控目標(biāo)看作在二維平臺(tái)上移動(dòng)的點(diǎn)狀物體，于是目標(biāo)的運(yùn)動(dòng)軌跡可以為柵格地圖上的黑色塊，而可行區(qū)域則由白色塊表示。環(huán)境M由柵格Mij構(gòu)成：

M={Mij,Mij=0,1,2,3}

其中，Mij=0表示移動(dòng)機(jī)器人的起始位置單元柵格；Mij=1表示無障礙區(qū)域單元柵格；Mij=2表示障礙區(qū)域單元柵格；Mij=3表示目標(biāo)所處位置。

柵格大小的選取對(duì)于整個(gè)路徑規(guī)劃是很重要的，柵格選取較大，計(jì)算量就會(huì)減少，但得到的路徑長(zhǎng)度可能會(huì)增大；相反，柵格選取太小，路徑規(guī)劃的準(zhǔn)確度就會(huì)提高，但規(guī)劃過程緩慢。柵格大小主要由實(shí)驗(yàn)環(huán)境確定。柵格長(zhǎng)度

其中，r為障礙物半徑，R為機(jī)器人半徑，δ為設(shè)定的安全距離。綜上所述，可以將移動(dòng)機(jī)器人的路徑規(guī)劃問題概述為通過在已得到的環(huán)境模型中的無障礙區(qū)域搜索，得到一條由柵格組成的連續(xù)的最短路徑。

1.2 A*全局路徑規(guī)劃算法

A*算法[21]是一類搜索算法，它根據(jù)所定義的估價(jià)函數(shù)大小來確定最優(yōu)路徑。代價(jià)函數(shù)表示為

f(n)=g(n)+h(n)

(1)

其中，n為當(dāng)前節(jié)點(diǎn)；f(n)為節(jié)點(diǎn)n的估價(jià)函數(shù)；g(n)為從起始點(diǎn)到當(dāng)前節(jié)點(diǎn)n的實(shí)際代價(jià)值；h(n)為當(dāng)前節(jié)點(diǎn)n到目標(biāo)點(diǎn)的估計(jì)代價(jià)值。一般來說h(n)為歐氏距離，其定義如下：

其中，(xn,yn)表示當(dāng)前位置處柵格的中心坐標(biāo)；(xgoal,ygoal)則表示目標(biāo)柵格的中心。通過優(yōu)化可以得到當(dāng)前位置到目標(biāo)位置的最短路徑。

傳統(tǒng)A*算法能夠有效地對(duì)目標(biāo)進(jìn)行全局路徑規(guī)劃，但其優(yōu)化后得到的路徑冗余點(diǎn)較多，且該方法得到的運(yùn)動(dòng)路線折線多、轉(zhuǎn)折次數(shù)多、轉(zhuǎn)折角度大，這些缺陷嚴(yán)重影響了路徑規(guī)劃的效果。本文根據(jù)傳統(tǒng)A*算法的原理，進(jìn)一步提出了一種新型A*算法。該算法利用二次A*搜索優(yōu)化了目標(biāo)路徑的長(zhǎng)度，這種方法能夠僅保留路徑中的起點(diǎn)、轉(zhuǎn)折點(diǎn)和終點(diǎn)。對(duì)于存在非完整約束的移動(dòng)機(jī)器人，其特性造成移動(dòng)機(jī)器人不能在路徑中的轉(zhuǎn)折處進(jìn)行直線運(yùn)動(dòng)，需要不斷轉(zhuǎn)動(dòng)來調(diào)整自身的運(yùn)動(dòng)姿態(tài)，從而引起目標(biāo)運(yùn)動(dòng)的角速度以及向心加速度發(fā)生變化。對(duì)此，本文提出了動(dòng)態(tài)切點(diǎn)調(diào)整算法對(duì)路徑進(jìn)行平滑處理。

1.2.1路徑點(diǎn)序列優(yōu)化

在實(shí)現(xiàn)路徑優(yōu)化過程中，為了縮短路徑長(zhǎng)度，引入二次A*搜索算法。首先，將式(1)得到的估價(jià)函數(shù)作為初始值，并選取擴(kuò)展點(diǎn)，擴(kuò)展點(diǎn)的選擇方法是選取與路徑節(jié)點(diǎn)不相鄰的后續(xù)路徑點(diǎn)；進(jìn)一步，以代價(jià)函數(shù)大小和是否穿過障礙作為判斷依據(jù)，來確定上述兩個(gè)節(jié)點(diǎn)之間是否存在新的路徑。判斷標(biāo)準(zhǔn)是，如果滿足代價(jià)函數(shù)小于初始值、無障礙物這兩個(gè)條件，則兩點(diǎn)之間構(gòu)成新的路徑，并剔除中間節(jié)點(diǎn)，否則，路徑為原有路徑。最后，所生成的路徑為最短路徑，僅包含起點(diǎn)、轉(zhuǎn)折點(diǎn)和目標(biāo)點(diǎn)。

對(duì)兩種A*算法在柵格地圖為10 m×10 m的情況下進(jìn)行仿真對(duì)比，結(jié)果如圖1、圖2和表1所示。

圖1 一次A*算法尋優(yōu)Fig.1 Linear A* algorithm optimization

圖2 二次A*算法尋優(yōu)Fig.2 Twice A* algorithm optimization

機(jī)制累計(jì)轉(zhuǎn)彎角度(°)路徑長(zhǎng)度(m)路徑長(zhǎng)度降低率(%)轉(zhuǎn)彎角度降低率(%)一次A?算法18010.07二次A?算法112.629.842.2137.43

比較圖1、圖2和表1的仿真數(shù)據(jù)可知，二次A*算法有效剔除了冗余點(diǎn)和折線，減小了規(guī)劃路徑長(zhǎng)度，有效減少了轉(zhuǎn)彎角度，實(shí)現(xiàn)了路徑優(yōu)化。

1.2.2路徑平滑處理

二次A*算法可以規(guī)劃出一條由線段組成的完整路徑，而平滑的路徑更便于移動(dòng)機(jī)器人的控制。本文利用動(dòng)態(tài)切點(diǎn)調(diào)整算法去除凹凸點(diǎn)，該方法能夠得到既有曲率連續(xù)性，同時(shí)滿足幾何特性的路徑。路徑平滑示意圖見圖3。

圖3 路徑平滑示意圖Fig.3 Schematic diagram of the path smooth

機(jī)器人的初始位置為A1(x1,y1)，終點(diǎn)位置為An(xn,yn)。從初始位置開始，依次對(duì)Ai(xi,yi)(i=2,3,…,n-1)轉(zhuǎn)折點(diǎn)進(jìn)行平滑處理。圖3中，單調(diào)地使用固定切點(diǎn)會(huì)使機(jī)器人陷入包含障礙物的死區(qū)，因此，本文將固定切點(diǎn)改為動(dòng)態(tài)切點(diǎn)，提出了動(dòng)態(tài)切點(diǎn)調(diào)整算法，具體步驟如下：

(1)比較Ai-1Ai和AiAi+1兩線段長(zhǎng)度，選擇較短邊的端點(diǎn)P(xp,yp)為初始切點(diǎn)，過點(diǎn)P作垂線，與∠Ai-1AiAi+1(i=2,3,…,n-1)的角平分線AiQi-1相交于點(diǎn)Oi-1(x0,y0)：

x0=(xp+k01yp+k01(k0x2-y2))/(1+k0k01)

(2)

y0=k0(x0-x2)+y2

(3)

相切圓的半徑R可表示為

(4)

相切圓方程為

(x-x0)2+(y-y0)2=R2

(5)

式中，k01為較短邊斜率；k0為角平分線斜率。

(2)判斷相切圓是否與長(zhǎng)邊之間有交點(diǎn)S，如果有，則轉(zhuǎn)至步驟(3)；否則，轉(zhuǎn)至步驟(4)。

(3)判斷圓弧PS上是否存在障礙物，如果存在，則轉(zhuǎn)至步驟(4)；否則，用圓弧PS替代Ai-1AiAi+1，并轉(zhuǎn)至步驟(5)；

(4)切點(diǎn)P(xp,yp)沿著所在線段移動(dòng)到P2(xp2,yp2)，xp2可表示為

xp2=xp+λ|x2-xp|λ∈(0,1)

(6)

其中，λ依據(jù)實(shí)際情況設(shè)置。同時(shí)將P2設(shè)置為初始切點(diǎn)，并返回步驟(1)。

(5)判斷此步驟確定的路徑是否已經(jīng)在環(huán)境模型中遍歷所有的路徑節(jié)點(diǎn)，若是，則返回步驟(1)，否則結(jié)束。

2 改進(jìn)人工勢(shì)場(chǎng)局部路徑規(guī)劃算法

在機(jī)器人的移動(dòng)運(yùn)行過程中，能夠通過機(jī)器人機(jī)身攜帶的傳感器完成對(duì)所處局部環(huán)境變化的感知，根據(jù)實(shí)時(shí)檢測(cè)的障礙物或目標(biāo)的變化情況，采用人工勢(shì)場(chǎng)法做出行為決策，及時(shí)避開障礙物，實(shí)現(xiàn)目標(biāo)跟蹤。

人工勢(shì)場(chǎng)[22]由斥力勢(shì)場(chǎng)和引力勢(shì)場(chǎng)組成。在復(fù)雜的工作環(huán)境中，環(huán)境信息不斷變化，為了進(jìn)一步滿足當(dāng)前路徑規(guī)劃的需求，不能僅僅考慮機(jī)器人、目標(biāo)位置和障礙三者之間的相對(duì)位置關(guān)系。為此，本文將機(jī)器人、目標(biāo)與障礙三者間的相對(duì)速度關(guān)系作為優(yōu)化路徑的指標(biāo)。引力勢(shì)場(chǎng)函數(shù)Uatt可定義為

Uatt(q)=[mρ2(q,qg)+kv1ρ2(V,Vg)]/2

(7)

相應(yīng)的引力Fatt可表示為

Fatt(q)=mρ(q,qg)+kv1ρ(V,Vg)

(8)

式中，m、kv1為引力場(chǎng)正常量；ρ(q,qg)為機(jī)器人與目標(biāo)點(diǎn)之間的歐氏距離；ρ(V,Vg)為機(jī)器人和目標(biāo)點(diǎn)之間的相對(duì)速度。

斥力勢(shì)場(chǎng)函數(shù)Urep可定義為

(9)

Urepv=kv2ρ2(V,Vobs)/2

(10)

(11)

相應(yīng)斥力Frep可表示為

(12)

Frepv=kv2ρ(V,Vobs)

(13)

(14)

其中，k、kv2為斥力場(chǎng)正常量；ρ(q,qobs)表示機(jī)器人與障礙物兩點(diǎn)之間的歐氏距離；ρ(V,Vobs)表示機(jī)器人和動(dòng)態(tài)障礙物之間的相對(duì)速度；ρ0為常數(shù)，表示障礙物對(duì)機(jī)器人產(chǎn)生作用的最大范圍；α為障礙物的相對(duì)移動(dòng)方向和移動(dòng)機(jī)器人與障礙物連線的夾角，當(dāng)α∈(-π/2,π/2)時(shí)，障礙物的移動(dòng)方向確定為靠近移動(dòng)機(jī)器人。

2.1 虛擬子目標(biāo)的增設(shè)

移動(dòng)機(jī)器人在行進(jìn)過程中，存在一種嚴(yán)重影響運(yùn)行的局部最小值陷阱，即障礙物和目標(biāo)點(diǎn)對(duì)移動(dòng)機(jī)器人產(chǎn)生的排斥力和吸引力相等時(shí)，使得移動(dòng)機(jī)器人不能順利抵達(dá)目標(biāo)點(diǎn)。因此，本文提出增設(shè)虛擬子目標(biāo)法。借助虛擬外力，使機(jī)器人擺脫陷阱，成功抵達(dá)目標(biāo)點(diǎn)。具體方法如下。

(1)首先依據(jù)移動(dòng)機(jī)器人的狀態(tài)信息判斷是否進(jìn)入局部最小值陷阱，以連續(xù)五個(gè)步長(zhǎng)作為判斷依據(jù)，當(dāng)總的移動(dòng)距離小于βL(L為步長(zhǎng)，β∈[2,4])時(shí)，改變?chǔ)?，移?dòng)機(jī)器人可以提前檢測(cè)到自身陷入局部極小值陷阱。

(2)程序跳轉(zhuǎn)到虛擬子目標(biāo)增設(shè)模塊，存儲(chǔ)使移動(dòng)機(jī)器人陷入局部最小值陷阱的障礙物位置信息，形成障礙物群，充分考慮目標(biāo)點(diǎn)位置與障礙物群位置信息，判斷障礙物在移動(dòng)機(jī)器人與目標(biāo)點(diǎn)連接線左右兩端數(shù)目多少，選擇障礙物群左側(cè)或右側(cè)為目標(biāo)障礙物，然后利用下式得到虛擬子目標(biāo)位置：

(15)

式中，(xob,yob)為目標(biāo)障礙物位置；(xg,yg)為原目標(biāo)所處位置；(x,y)為機(jī)器人的當(dāng)前位置；β1,β2為可調(diào)參數(shù)，β1,β2>0。

移動(dòng)機(jī)器人擺脫上述陷阱后，將撤出虛擬子目標(biāo)，移動(dòng)機(jī)器人不再受外力作用。移動(dòng)機(jī)器人將在原目標(biāo)和障礙物的合力作用下向原目標(biāo)點(diǎn)靠近。

2.2 自適應(yīng)步長(zhǎng)調(diào)節(jié)

通過控制步長(zhǎng)的變化，能夠使搜索更加靈活。在障礙物較少的情況下，增大步長(zhǎng)可以降低機(jī)器人調(diào)整位姿的頻率，縮短運(yùn)動(dòng)時(shí)間。但是步長(zhǎng)不能隨意改變，盲目增大步長(zhǎng)會(huì)增加碰撞的概率。為此，本文使用自適應(yīng)步長(zhǎng)調(diào)節(jié)的方法來減少碰撞的概率。

這種算法需要選擇補(bǔ)償，主要考慮環(huán)境的復(fù)雜度以及機(jī)器人與目標(biāo)的相對(duì)距離。機(jī)器人機(jī)身攜帶的傳感器，能夠探測(cè)到周圍障礙物數(shù)目以及障礙物與機(jī)器人自身的位置關(guān)系，由此判斷環(huán)境的復(fù)雜程度。本文設(shè)立的判定標(biāo)準(zhǔn)如下：當(dāng)機(jī)器人探測(cè)到的障礙物數(shù)目大于設(shè)定值或機(jī)器人探測(cè)到障礙物與自身相對(duì)位置小于設(shè)定值時(shí)，環(huán)境即為復(fù)雜環(huán)境。在這種情況下，機(jī)器人步長(zhǎng)取Lmin；而當(dāng)機(jī)器人離目標(biāo)很近時(shí)，為避免出現(xiàn)步長(zhǎng)太大而超過目標(biāo)，或者步長(zhǎng)太小而無法跟蹤目標(biāo)的情況，步長(zhǎng)取Lmid；當(dāng)機(jī)器人處于簡(jiǎn)單環(huán)境時(shí)，步長(zhǎng)取Lmax。這樣既節(jié)省運(yùn)行時(shí)間，還能優(yōu)化路徑。

采用改進(jìn)的人工勢(shì)場(chǎng)法有效避免了傳統(tǒng)人工勢(shì)場(chǎng)法中目標(biāo)不可達(dá)、易陷入局部極小值等問題，在動(dòng)靜態(tài)障礙物干擾下，實(shí)現(xiàn)對(duì)動(dòng)態(tài)目標(biāo)的快速跟蹤。

3 仿真與分析

利用MATLAB仿真軟件對(duì)本文算法進(jìn)行仿真，并與傳統(tǒng)算法進(jìn)行仿真對(duì)比。

3.1 靜態(tài)障礙物+靜態(tài)目標(biāo)

采用新型A*全局路徑規(guī)劃算法和基于三次B樣條函數(shù)的A*算法在柵格地圖為10 m×10 m的情況下進(jìn)行仿真，結(jié)果如圖4、圖5所示，其中系數(shù)λ=0.1。

由圖4、圖5可以發(fā)現(xiàn)，利用兩種算法均可實(shí)現(xiàn)規(guī)劃一條起點(diǎn)至終點(diǎn)的平滑路徑。由圖5中的局部路徑放大圖可以看出，A*算法與三次B樣條函數(shù)法的路徑規(guī)劃算法在處理路徑平滑過程中，單獨(dú)使用三次B樣條函數(shù)會(huì)導(dǎo)致移動(dòng)機(jī)器人陷入障礙物死區(qū)，并與障礙物發(fā)生碰撞，而本文提出的改進(jìn)的A*全局路徑規(guī)劃算法在處理轉(zhuǎn)折點(diǎn)時(shí)，采用動(dòng)態(tài)切點(diǎn)調(diào)整方法，因此可以規(guī)劃出平滑、安全的最優(yōu)路徑。

圖4 改進(jìn)的A*算法的路徑規(guī)劃Fig.4 Path planning for improved A* algorithm

圖5 三次B樣條函數(shù)和A*算法的路徑規(guī)劃Fig.5 Path planning of cubic B-splines function and A* algorithm

3.2 動(dòng)靜態(tài)障礙物+動(dòng)態(tài)目標(biāo)

利用MATLAB軟件對(duì)比改進(jìn)的人工勢(shì)場(chǎng)法與傳統(tǒng)方法，其中參數(shù)選取根據(jù)經(jīng)驗(yàn)法[23-25]、仿真環(huán)境和實(shí)驗(yàn)環(huán)境得到。為了有效對(duì)比算法的先進(jìn)性，仿真過程兩種算法的參數(shù)選取如下：m=2000，kv1=1000，k=100，kv2=1000，ρ0=0.5m，其他參數(shù)見表2，仿真結(jié)果如圖6、圖7和表3所示。

表2 實(shí)驗(yàn)參數(shù)

圖6 改進(jìn)的人工勢(shì)場(chǎng)法的路徑規(guī)劃Fig.6 Path planning for improved artificial potential field algorithm

圖7 傳統(tǒng)人工勢(shì)場(chǎng)法的路徑規(guī)劃Fig.7 Path planning for traditional artificial potential field algorithm

圖7中，移動(dòng)機(jī)器人在障礙物附近存在不良震蕩現(xiàn)象。最終，運(yùn)行步數(shù)為154。由圖6的仿真結(jié)果可以發(fā)現(xiàn)，采用自適應(yīng)步長(zhǎng)調(diào)節(jié)算法既可以安全躲避障礙物，提高實(shí)時(shí)性，又能節(jié)省運(yùn)行時(shí)間?？傔\(yùn)行步數(shù)為54。由表3可以看出，相比傳統(tǒng)人工勢(shì)場(chǎng)法，所提算法運(yùn)行時(shí)間和路徑長(zhǎng)度分別減少了18.6%和51.3%。

表3 算法仿真數(shù)據(jù)比較

3.3 動(dòng)靜態(tài)障礙物+動(dòng)態(tài)目標(biāo)(特殊環(huán)境)

機(jī)器人、障礙物和目標(biāo)位置處于同一條直線，且障礙物位于中間位置時(shí)，機(jī)器人很容易陷入局部極小值陷阱。采用改進(jìn)的人工勢(shì)場(chǎng)法進(jìn)行仿真實(shí)驗(yàn)，結(jié)果如圖8、圖9和表3所示。

圖8 改進(jìn)的人工勢(shì)場(chǎng)法的路徑規(guī)劃Fig.8 Path planning for improved artificial potential field algorithm

圖9 傳統(tǒng)人工勢(shì)場(chǎng)法的路徑規(guī)劃Fig.9 Path planning for traditional artificial potential field algorithm

由圖9可以看出，當(dāng)引力和斥力一直處于同一條直線時(shí)，機(jī)器人無法有效地避開障礙物，一直在障礙物附近徘徊，無法向目標(biāo)點(diǎn)移動(dòng)。總移動(dòng)步數(shù)為240。而在圖8中，當(dāng)移動(dòng)機(jī)器人檢測(cè)到陷入局部極小值陷阱時(shí)，增設(shè)的虛擬子目標(biāo)及時(shí)的對(duì)移動(dòng)機(jī)器人產(chǎn)生外力，幫助移動(dòng)機(jī)器人擺脫了局部極小值陷阱，同時(shí)避免在障礙物群附近產(chǎn)生震蕩。總的移動(dòng)步數(shù)為51。由表3不難看出，相比傳統(tǒng)人工勢(shì)場(chǎng)法，改進(jìn)的人工勢(shì)場(chǎng)法在運(yùn)行時(shí)間上縮短了59.13%，在最優(yōu)路徑長(zhǎng)度上也減少了52.3%。

4 實(shí)驗(yàn)

本實(shí)驗(yàn)采用綠色圓形紙盒作為靜態(tài)障礙物，兩個(gè)3自由度QUANSER QBot2作為追蹤機(jī)器人和動(dòng)態(tài)目標(biāo)，整個(gè)實(shí)驗(yàn)場(chǎng)地大小為2.5 m×3.0 m，柵格大小設(shè)定為0.6 m×0.6 m。機(jī)器人半徑為0.17 m，障礙物半徑為0.11 m，δ=0.15 m，實(shí)驗(yàn)參數(shù)設(shè)定m=1 200，kv1=1 000，k=200，kv2=1 000，ρ0=0.3 m，其余參數(shù)設(shè)定同表2。其中，紅外攝像頭實(shí)時(shí)傳輸目標(biāo)機(jī)器人的位置信息。實(shí)驗(yàn)環(huán)境和實(shí)驗(yàn)情況如圖10和圖11所示。

圖10 實(shí)驗(yàn)設(shè)備Fig.10 Experimental Equipment

圖11 路徑規(guī)劃實(shí)驗(yàn)情況Fig.11 Path planning experiments

圖11中，機(jī)器人從起點(diǎn)(圖11a)開始，在目標(biāo)引力與障礙物斥力的合力作用下，實(shí)時(shí)完成避障(圖11b)，而且迅速跟蹤移動(dòng)的目標(biāo)機(jī)器人。值得注意的是，機(jī)器人自身存在非完整約束，且試驗(yàn)場(chǎng)地地面較為光滑，故本實(shí)驗(yàn)中，機(jī)器人運(yùn)行的轉(zhuǎn)角較大。

5 結(jié)論

本文主要研究動(dòng)態(tài)復(fù)雜非結(jié)構(gòu)化環(huán)境下移動(dòng)機(jī)器人的路徑規(guī)劃問題，提出了將全局規(guī)劃算法與局部規(guī)劃算法相融合的路徑規(guī)劃方法?；谛滦虯*算法，實(shí)現(xiàn)了移動(dòng)機(jī)器人路徑規(guī)劃，利用二次A*算法與動(dòng)態(tài)切點(diǎn)調(diào)整法對(duì)路徑進(jìn)行平滑處理，同時(shí)依據(jù)機(jī)器人環(huán)境感知信息和全局路徑規(guī)劃結(jié)果，采用基于自適應(yīng)步長(zhǎng)調(diào)節(jié)算法的人工勢(shì)場(chǎng)法，實(shí)現(xiàn)了移動(dòng)機(jī)器人局部路徑的動(dòng)態(tài)規(guī)劃。最終移動(dòng)機(jī)器人實(shí)現(xiàn)以較快速度沿著較短平滑路徑追蹤目標(biāo)。通過對(duì)比分析，證明了本文所提算法的有效性。在后續(xù)的工作中，將結(jié)合圖像對(duì)更多復(fù)雜環(huán)境進(jìn)行進(jìn)一步改進(jìn)分析。