基于障礙物運動預(yù)測的移動機器人路徑規(guī)劃

楊奇峰，曲道奎，徐 方

(1.中國科學(xué)院沈陽自動化研究所機器人學(xué)國家重點實驗室，遼寧 沈陽 110016；2.中國科學(xué)院機器人與智能制造創(chuàng)新研究院，遼寧 沈陽 110169；3.中國科學(xué)院大學(xué)，北京 100049；4.沈陽新松機器人自動化股份有限公司，遼寧 沈陽 110168)

0 引 言

移動機器人路徑規(guī)劃技術(shù)是其研究的關(guān)鍵技術(shù)問題之一，移動機器人局部路徑規(guī)劃方法可以歸類為3類方法，基于圖搜索的路徑規(guī)劃技術(shù)[1-3]，構(gòu)建人工勢場進行路徑規(guī)劃的技術(shù)[4]，基于各類人工智能算法的路徑規(guī)劃技術(shù)[5-8]，各種算法各有優(yōu)缺點[9]。

劉軍等提出有向D*算法。該算法考慮目標(biāo)點與障礙物信息，引入關(guān)鍵節(jié)點概念，較傳統(tǒng)算法能更好地兼顧局部搜索與全局最優(yōu)性[10]；Le AT等基于D*lite在復(fù)雜環(huán)境中的效率問題，提出了一種新的算法D*LR(D*lite with reset)，在復(fù)雜環(huán)境下，D*LR的性能比D*lite有明顯的提高并在ROS上進行了驗證[11]；黃魯?shù)纫霊卸枰暰€算法，結(jié)合距離變換對D*lite算法進行改進。使得重規(guī)劃得到的路徑遠離突現(xiàn)的障礙物[12]。楊俊駒通過改進傳統(tǒng)人工勢場法作為機器人局部路徑規(guī)劃算法，使機器人能迅速避開動態(tài)障礙物[13]。吳運雄等提出基于深度強化學(xué)習(xí)的動態(tài)避障算法，較好地解決了傳統(tǒng)算法存在的容易陷入局部最優(yōu)的問題[14]。這些算法在進行動態(tài)路徑規(guī)劃時，在每一個更新的時刻，機器人都是根據(jù)障礙物的當(dāng)前位置進行后續(xù)路徑規(guī)劃，并依次更新迭代，在機器人動態(tài)的更新路徑時，并不考慮動態(tài)障礙物的運動趨勢，因此我們希望在移動機器人動態(tài)路徑規(guī)劃時，引入動態(tài)障礙物的運動預(yù)測機制，優(yōu)化動態(tài)避障路徑規(guī)劃策略。在移動機器人的動態(tài)避障路徑規(guī)劃過程中，評估障礙物的移動軌跡，提出改進的D*lite路徑規(guī)劃算法，使機器人的動態(tài)避障算法更接近于人的避障策略，提高機器人動態(tài)避障路徑規(guī)劃的效率和安全性。

1 基于D*lite的路徑規(guī)劃

D*lite是一種基于柵格模型適應(yīng)動態(tài)環(huán)境的路徑規(guī)劃算法。D*lite算法由D*算法改進而來，運用LPA*算法的思想，使得移動機器人在未知動態(tài)環(huán)境下可以快速重規(guī)劃。

D*lite算法與D*算法一樣，搜索方式采用從目標(biāo)節(jié)點到起始節(jié)點，以適應(yīng)起始點改變的情況，D*lite算法維護每一個節(jié)點的g(s)， 并且引入了LPA*算法中rhs(s) 的定義

(1)

式(1)中用Succ(s) 表示所有節(jié)點s的后繼節(jié)點，用g(s′) 表示節(jié)點s′到目標(biāo)節(jié)點的距離代價。 D*lite維護每個柵格的g(s) 與rhs(s)， 當(dāng)兩個變量相等時，定義該節(jié)點為一致狀態(tài)，否則為不一致態(tài)。只有處于一致狀態(tài)下的節(jié)點是已經(jīng)擴展完畢、可以通行的，不一致狀態(tài)表示該節(jié)點的路徑信息是不準(zhǔn)確的。 D*lite算法如圖1所示。

圖1 D*lite算法

D*lite算法引入k(s) 來進行各柵格節(jié)點的距離代價的評估，k(s) 包含兩個值[k1(s);k2(s)]， 這里的k1(s)，k2(s) 由如式(2)、式(3)計算

k1(s)=min(g(s),rhs(s))+h(s,sgoal)

(2)

k2(s)=min(g(s),rhs(s))

(3)

當(dāng)環(huán)境中出現(xiàn)新的障礙物時，D*lite算法可以快速更新該障礙物周邊節(jié)點的信息，并對由此而導(dǎo)致不連續(xù)的節(jié)點進行快速重規(guī)劃。但是每次機器人遇到新的障礙物進行避障時，機器人總是只考慮障礙物當(dāng)前的位置，并不考慮障礙物的動態(tài)性，因此對于動態(tài)障礙物的避障，所規(guī)劃出的路徑并不是最優(yōu)的。另一方面很有可能機器人選擇的避障路徑與障礙物的運動方向一致，增加了機器人與障礙物碰撞的風(fēng)險，本文提出基于障礙物運動趨勢的改進D*lite算法，極大提高了局部動態(tài)障礙物避障路徑規(guī)劃的規(guī)劃效率及安全性。

2 基于動態(tài)障礙物運動趨勢的避障路徑規(guī)劃

2.1 基本原理

人在行走過程中，如果在自己的行走路線上出現(xiàn)諸如行人、汽車等運動障礙物，會首先基于運動障礙物的歷史移動軌跡及當(dāng)前狀態(tài)分析其運動意圖，之后根據(jù)運動障礙物的運動意圖合理規(guī)劃下一步路線。本文提出了基于運動障礙物運動意圖的動態(tài)障礙物路徑規(guī)劃方法，如圖2所示，模仿人遇到動態(tài)障礙物的避障策略。在進行運動障礙物避障時，首先分析運動障礙物的意圖，以提高避障的效率和安全性。

圖2 動態(tài)避障路徑規(guī)劃

機器人通過對運動障礙物歷史狀態(tài)的分析，估計運動障礙物速度及在避障過程中運動方向和運動距離，將運動障礙物的可能行走軌跡作為待避障區(qū)域的一部分進行整體規(guī)劃。在所有可行的軌跡中，采用時間維度上路徑最優(yōu)的策略確定優(yōu)先級最高的路徑，機器人運行過程中實時判斷運動障礙物的狀態(tài)，并不斷調(diào)整運行軌跡。局部動態(tài)避障算法步驟如下：

(1)機器人實時采集環(huán)境中障礙物的信息，并計算障礙物在環(huán)境坐標(biāo)系下的歷史位置；

(2)對障礙物進行合理的膨脹，提高機器人避障路徑規(guī)劃的安全性；

(3)根據(jù)障礙物的歷史位置，基于障礙物運動預(yù)測模型預(yù)測機器人運行過程中障礙物的行進路徑；

(4)采用改進D*lite算法實時規(guī)劃機器人行進的路徑，控制機器人按照規(guī)劃路徑運行；

(5)機器人運行過程中根據(jù)障礙物狀態(tài)(歷史軌跡)的變化，實時調(diào)整機器人的運行軌跡。

2.2 動態(tài)障礙物的探測與運動趨勢預(yù)測

機器人可以通過自身搭載的攝像頭、機載雷達等傳感器實時獲取環(huán)境中的點云信息，采用聚類的方法對環(huán)境中的物體點云進行歸類，并實時對環(huán)境中的障礙物進行匹配分析，不同幀數(shù)據(jù)中障礙物的位置變化表征了動態(tài)障礙物的歷史移動軌跡。

如圖3所示， R為移動器人， O為機器人探測到的前方障礙物。在機器人R運行過程中，障礙物O同時也在運動，機器人當(dāng)前的位置為Pr， 速度為Vr， 動態(tài)障礙物當(dāng)前的位置為Po， 速度為VO， 下一時刻動態(tài)障礙物運行的可能位置為P′o， 對運動障礙物進行運行軌跡預(yù)測可以提高機器人的路徑規(guī)劃效率。

圖3 動態(tài)障礙物運行趨勢

設(shè)運動障礙物任意時刻的位置為Pi=(xi,yi)， 運用拉格朗日插值法建立移動目標(biāo)的運動軌跡模型。

采用拉格朗日插值法可以構(gòu)建一個多項式，該多項式在各個觀測點通過觀測到的值，對于任意K+1個點A0(x0,y0),A1(x1,y1),…,Ak+1(xk+1,yk+1), 存在多項式(4)

(4)

其中，li(x) 為拉格朗日基本多項式，也稱為插值基函數(shù)，其表達式為(5)

(5)

拉格朗日插值多項式fk(x) 的截斷誤差為公式

(6)

其中，cn+1(x)=(x-x0)(x-x1)…(x-xn)， 因此，三點外插公式為式(7)和式(8)

(7)

(8)

設(shè)機器人在運行過程中，通過掃描匹配獲得動態(tài)障礙物的先后3個位置為(ti,xi,yi)，(ti+1,xi+1,yi+1),(ti+2,xi+2,yi+2)， 由上式可以得出動態(tài)障礙物tn時刻x方向和y方向的運動預(yù)測計算公式，分別為式(9)、式(10)

(9)

(10)

模型建立后，即可計算出此后任意時刻tn運動目標(biāo)的位置 (tn,xn,yn), 當(dāng)機器人運行過程中獲得新的動態(tài)障礙物位置，設(shè)為 (ti+3,xi+3,yi+3)， 這時令

ti=ti+1,xi=xi+1,yi=yi+1

(11)

ti+1=ti+2,xi+1=xi+2,yi+1=yi+2

(12)

ti+2=ti+3,xi+2=xi+3,yi+2=yi+3

(13)

代入式(9)、式(10)，即可得到動態(tài)障礙物今后某一時刻tn新的預(yù)測位置 (tn,xn,yn)

如圖4所示，機器人運行過程中先后探測到位置為P0，P1，P2，P3動態(tài)障礙物，黑色實線軌跡f1(x) 為機器人依據(jù)探測到的障礙物位置P0，P1，P2預(yù)測的動態(tài)障礙物運動軌跡，P′3，P′4為機器人預(yù)測的后續(xù)兩時刻動態(tài)障礙物的位置，當(dāng)機器人探測到t3時刻實際位置P3的動態(tài)障礙物時，機器人根據(jù)動態(tài)障礙物的運動位置P1，P2，P3對其后續(xù)運行軌跡進行重新預(yù)測，圖中淺灰色虛線便是重新預(yù)測后的軌跡曲線，P″4為基于新預(yù)測軌跡曲線計算獲得的動態(tài)障礙物后續(xù)位置，由于P′3與P3存在偏差，因此預(yù)測出的后續(xù)節(jié)點P′4與P″4也存在偏差，機器人在每一個規(guī)劃周期T，更新環(huán)境中的障礙物信息，迭代預(yù)測障礙物后續(xù)軌跡，并更新機器人的規(guī)劃軌跡。

圖4 動態(tài)障礙物運動預(yù)測

雖然環(huán)境中的運動目標(biāo)在中長期內(nèi)其運動具有很大的隨機性，但在短期內(nèi)的運動還是有一定規(guī)律可循。比如室外園區(qū)中常見動態(tài)障礙物如行人、車輛由于受到社會規(guī)則或其本身運動學(xué)模型的約束，其軌跡具有光滑性、連續(xù)性的特點，不會突然逆方向運動；或者即使有這種情況存在，但其概率也比較小。因此在一個較短時間內(nèi)，通過構(gòu)建拉格朗日運動預(yù)測模型，對園區(qū)動態(tài)障礙物的運動進行一定精度的預(yù)測是可行的。

2.3 基于改進D*lite算法的路徑規(guī)劃算法

在柵格地圖中，設(shè)機器人當(dāng)前節(jié)點為S，起始節(jié)點為sstart， 目標(biāo)節(jié)點為sgoal， 動態(tài)障礙物的節(jié)點為O(O1,O2…On)。

柵格地圖中的障礙物具有阻礙機器人通過的特性，我們稱之為機器人通過該障礙物所占柵格的距離代價，用距離代價函數(shù)DisV(o) 表示，障礙物的距離代價值取相對較大的值進行表征。用下式表示

DisV(o)=K

(14)

為了避免機器人在避障運行時，規(guī)劃出的路徑離機器人距離太近，我們對障礙物進行適當(dāng)?shù)呐蛎?，膨脹后障礙物周邊的柵格具有一定數(shù)值的距離代價，設(shè)柵格離障礙物之間的街區(qū)距離為Ro，膨脹區(qū)域為領(lǐng)域距離R，可得到障礙物周邊柵格的距離代價函數(shù)如下式所示

(15)

通過障礙膨脹函數(shù)，可以對地圖中的柵格逐一進行距離代價的計算，柵格地圖中存在多個障礙物的情況下，每一個柵格取該柵格所有領(lǐng)域障礙物膨脹距離代價的最大值作為該柵格的距離代價值。障礙物膨脹如圖5所示。

圖5 障礙物膨脹建模

地圖中任意兩點間的路徑可能有多條，地圖中任意兩個節(jié)點S1，S2之間的可行路徑組成一個路徑集，用dis(S1,S2) 表示路徑集的距離代價。

與D*lite算法一致，當(dāng)前節(jié)點s到目標(biāo)節(jié)點sgoal的啟發(fā)距離值為g(s)， 不同的是，令g(s) 為當(dāng)前節(jié)點s到目標(biāo)節(jié)點sgoal之間的最短路徑加上節(jié)點s的距離代價。g(s) 計算公式如下式所示

(16)

當(dāng)前節(jié)點s經(jīng)過后繼節(jié)點到達目標(biāo)節(jié)點的路徑代價為rhs(s)， 代表了s父節(jié)點的g(s)，rhs(s) 由下式表示

(17)

這里c(s,s′) 為s到節(jié)點s′的路徑代價，當(dāng)變量rhs(s)=g(s) 時，該節(jié)點處于一致狀態(tài)，不需要更新；否則處于不一致狀態(tài)，需要更新。

當(dāng)前障礙節(jié)點是由機器人運行過程中實時探測得到，其置信度高；而障礙物后續(xù)運動軌跡是依據(jù)障礙物當(dāng)前運動狀態(tài)預(yù)測得到，因此其軌跡上的障礙物置信度較低。而且隨著預(yù)測時間的增加，可信度將減小，將預(yù)測節(jié)點的距離代價函數(shù)定義如下

(18)

這里DisV(on) 表示障礙物節(jié)點o在時間n時的距離代價值，N表示預(yù)測步數(shù)范圍。動態(tài)障礙物預(yù)測代價距離建模如圖6所示。

圖6 動態(tài)障礙物代價距離建模

基于動態(tài)障礙物運動趨勢的改進D*lite避障路徑規(guī)劃算法流程如圖7所示。

圖7 改進D*lite算法流程

該算法中的計算最短路徑的算法流程如圖8所示。

圖8 最短路徑算法流程

3 仿真實驗與驗證

3.1 障礙物膨脹

仿真實驗過程中， P(1,1) 灰色點代表機器人的起始點， P(30,30) 黑色點代表機器人下一個將要到達的目標(biāo)點，圖中黑色方塊代表機器人運行過程中遇到的障礙物，為了使機器人避障過程中更安全，首先將障礙物進行合理的膨脹，實驗過程中障礙物膨脹半徑R=2， 圖9(a)中的障礙物為機器人探測到的實際障礙物，中間圖9(b)中灰色部分為障礙物的膨脹區(qū)域，圖9(c)中障礙物為實際障礙物膨脹后變大的障礙物模型。障礙物膨脹后，本文對動態(tài)障礙物幾種典型的運動進行了模擬驗證，障礙物的膨脹過程如圖9所示。

圖9 障礙物膨脹

實驗1，動態(tài)障礙物運動趨勢如圖10(a)所示，由黑色障礙物位置P(21,13) 向灰色障礙物位置P(11,23) 移動，圖10(b)是障礙物移動的實際軌跡，圖10(c)是機器人遇到圖中的動態(tài)障礙物時，按照D*lite算法實時規(guī)劃出的從機器人運動起始點到終點的路徑，可以看出機器人在運行到點P(14,14) 時遇到障礙物，之后隨著障礙物的運動，機器人一直處于動態(tài)避障狀態(tài)，并計算代價最小的后續(xù)節(jié)點，但是由于機器人并不判斷障礙物的后續(xù)運動位置，機器人會一直沿著障礙物的運動方向進行避障，直到障礙物停止運動(或者機器人超過或落后于障礙物)，機器人才可以完成障礙物避障，整個過程中機器人與障礙物的運動方向一致，且機器人速度大于障礙物時，機器人會從障礙物前方繞過，這樣的避障方式會使發(fā)生碰撞的風(fēng)險大大增大，整個避障過程經(jīng)過的節(jié)點數(shù)為12。采用本文提出的方法進行動態(tài)障礙物避障，規(guī)劃的避障路徑如圖10(d)所示，從圖中可以看出，本文所提方法不僅考慮了障礙物當(dāng)前的位置，而且會預(yù)測障礙物之后運動位置，當(dāng)機器人遇到障礙物后，與機器人運動方向一致的避障方向代價增大，機器人會選擇障礙物運動方向相反的路徑進行避障，這樣的避障方法更符合人類遇到障礙物的避障策略，根據(jù)障礙物的運動趨勢，選擇最優(yōu)的方向與路徑進行避障，提高避障安全性的同時，縮短避障的路徑代價，整個避障過程機器人經(jīng)過的節(jié)點數(shù)為9，經(jīng)過的節(jié)點數(shù)相比D*lite算法減小25%。實驗1的結(jié)果如圖10所示。

圖10 單障礙物與路徑規(guī)劃算法對比1

實驗2，移動的機器人移動過程中，體積較小動態(tài)障礙物由黑色障礙物位置P(15，19) 移動到灰色障礙物位置P(24,7)， 移動機器人在運動到位置P(14,14) 時與動態(tài)障礙物相遇，進入避障區(qū)域，按照D*lite算法規(guī)劃出的避障路徑如圖11(c)所示，可以看出隨著障礙物的移動，移動機器人規(guī)劃出的避障路徑與機器人的運動方向相同，直到障礙物停止(或者機器人超過或落后于障礙物)，機器人完成避障，在整個運行過程中移動機器人一直與運動障礙物伴行，避障安全性無法保障，整個避障過程的節(jié)點數(shù)為14。本文所提算法規(guī)劃出的避障路徑如圖11(d)所示，機器人遇到動態(tài)障礙物時，繞過運動障礙物，從運動障礙物的運動反方向進行避障運行，成功實現(xiàn)避障過程且可以大大提高機器人的避障安全性，整個避障過程的節(jié)點數(shù)為7，是D*lite算法避障節(jié)點數(shù)的50%，效率提高了一倍。實驗2的結(jié)果如圖11所示。

圖11 單障礙物與路徑規(guī)劃算法對比2

實驗3，移動機器人在運動過程中遇到多個動態(tài)障礙物(圖中為兩個障礙物)，圖12(a)中稍大的障礙物由黑色障礙物位置P(8,13) 運動到灰色障礙物位置P(14,8)， 尺寸偏小的障礙物由黑色障礙物位置P(26,13) 移動到灰色障礙物位置P(19,24)， 在移動機器人向目標(biāo)點運動過程中，基于D*lite避障算法如圖12(c)所示，移動機器人首先在位置P(8,9) 遇到大障礙物，之后在位置P(23,11) 遇到小障礙物，機器人首先伴行大障礙物進行避障，繞或大障礙物后，在位置P(23,11) 遇到小障礙物，繼續(xù)伴行小障礙物運行，直到繞過小障礙物，完成避障路徑規(guī)劃運行，整個避障過程中移動機器人避障移動的節(jié)點數(shù)為25。采用本文所提方法進行避障路徑規(guī)劃，如圖12(d)所示，機器人基于動態(tài)障礙物的運動趨勢進行障礙物位置預(yù)測，所規(guī)劃出的避障路徑沿大動態(tài)障礙物運動反方向，當(dāng)機器人遇到小動態(tài)障礙物時，基于動態(tài)障礙物的運動趨勢與移動機器人當(dāng)前位置，規(guī)劃出避障代價較小的路徑實現(xiàn)了動態(tài)避障，整個避障過程的安全性大大提高，并且避障經(jīng)過的路徑節(jié)點減小到14，相比D*lite算法，避障節(jié)點數(shù)減少44%。實驗3的結(jié)果如圖12所示。

圖12 多障礙物與路徑規(guī)劃算法對比

另外值得注意的是，基于本文算法在避障過程中顯示的障礙物不完全是實際的障礙物，也可能是避障時算法預(yù)測的障礙物未來出現(xiàn)的位置，例如最后一個實驗中機器人遇到第二個障礙物實際上是算法對障礙物的預(yù)測位置。

3.2 實驗結(jié)果對比

從以上仿真實驗結(jié)果可以看出，在移動機器人遇到圖10、圖11中的單動態(tài)障礙物時，采用本文提出的改進算法相比D*lite算法其避障代價分別降低了25%和50%，而且所規(guī)劃出的避障路徑均與動態(tài)障礙物運動方向相反，當(dāng)機器人遇到圖12中的多動態(tài)障礙物時，采用本文所提算法，避障代價減少了44%，而且與動態(tài)障礙物的重合路徑大幅減少。移動機器人采用本文所提方法對比D*lite方法對于動態(tài)障礙物的避障無論從避障安全性還是避障效率上均有顯著提升。

4 結(jié)束語

本文提出了移動機器人基于動態(tài)障礙物運動趨勢的改進D*lite避障路徑規(guī)劃算法，采用了類似人在進行動態(tài)障礙物避障類似的策略，提出了基于動態(tài)障礙物歷史軌跡的運動預(yù)測算法，優(yōu)化了D*lite算法在障礙物避障時只考慮當(dāng)前狀態(tài)，不考慮運動趨勢的問題，建立了預(yù)測障礙物運動位置代價函數(shù)模型，改進了D*lite算法，實現(xiàn)了機器人基于動態(tài)障礙物運動預(yù)測的避障路徑規(guī)劃。通過搭建仿真環(huán)境，選擇典型的單動態(tài)障礙物、多動態(tài)障礙物場景進行仿真驗證，實驗結(jié)果顯示本文所提算法在進行動態(tài)障礙物避障路徑規(guī)劃時，無論避障的效率還是避障的安全性均比D*lite算法有顯著提升。