基于障礙物運動預測的移動機器人路徑規劃

楊奇峰，曲道奎，徐 方

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

0 引 言

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

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

1 基于D*lite的路徑規劃

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

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

(1)

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

圖1 D*lite算法

D*lite算法引入k(s) 來進行各柵格節點的距離代價的評估，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*lite算法可以快速更新該障礙物周邊節點的信息，并對由此而導致不連續的節點進行快速重規劃。但是每次機器人遇到新的障礙物進行避障時，機器人總是只考慮障礙物當前的位置，并不考慮障礙物的動態性，因此對于動態障礙物的避障，所規劃出的路徑并不是最優的。另一方面很有可能機器人選擇的避障路徑與障礙物的運動方向一致，增加了機器人與障礙物碰撞的風險，本文提出基于障礙物運動趨勢的改進D*lite算法，極大提高了局部動態障礙物避障路徑規劃的規劃效率及安全性。

2 基于動態障礙物運動趨勢的避障路徑規劃

2.1 基本原理

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

圖2 動態避障路徑規劃

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

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

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

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

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

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

2.2 動態障礙物的探測與運動趨勢預測

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

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

圖3 動態障礙物運行趨勢

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

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

(4)

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

(5)

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

(6)

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

(7)

(8)

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

(9)

(10)

模型建立后，即可計算出此后任意時刻tn運動目標的位置 (tn,xn,yn), 當機器人運行過程中獲得新的動態障礙物位置，設為 (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)，即可得到動態障礙物今后某一時刻tn新的預測位置 (tn,xn,yn)

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

圖4 動態障礙物運動預測

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

2.3 基于改進D*lite算法的路徑規劃算法

在柵格地圖中，設機器人當前節點為S，起始節點為sstart， 目標節點為sgoal， 動態障礙物的節點為O(O1,O2…On)。

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

DisV(o)=K

(14)

為了避免機器人在避障運行時，規劃出的路徑離機器人距離太近，我們對障礙物進行適當的膨脹，膨脹后障礙物周邊的柵格具有一定數值的距離代價，設柵格離障礙物之間的街區距離為Ro，膨脹區域為領域距離R，可得到障礙物周邊柵格的距離代價函數如下式所示

(15)

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

圖5 障礙物膨脹建模

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

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

(16)

當前節點s經過后繼節點到達目標節點的路徑代價為rhs(s)， 代表了s父節點的g(s)，rhs(s) 由下式表示

(17)

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

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

(18)

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

圖6 動態障礙物代價距離建模

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

圖7 改進D*lite算法流程

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

圖8 最短路徑算法流程

3 仿真實驗與驗證

3.1 障礙物膨脹

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

圖9 障礙物膨脹

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

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

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

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

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

圖12 多障礙物與路徑規劃算法對比

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

3.2 實驗結果對比

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

4 結束語

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