一種高效的無角度約束移動機器人路徑規劃方法

楊建禹，白國振

(上海理工大學機械工程學院，上海200093)

移動機器人路徑規劃問題是指在有障礙物存在的地理環境中，如何尋找一條從給定起點到終點的路徑，使機器人沿該路徑移動的過程中不與障礙物發生碰撞且路程最短或移動代價最小。國內外學者在該問題上做了大量的研究，在眾多的路徑規劃算法中采用柵格化地圖的方法，由于其模型簡單且方便建立，從而得到廣泛的應用和發展，同時柵格化的地圖經常作為即時定位與地圖構建(SLAM)算法的結果輸出形式[1]。在柵格化地圖的應用方面，中國目前有曲道奎提出的將A*與人工勢場相融合的方法[2]、楊晶東等提出的移動機器人行為融合的避障方法[3]、葉煒垚提出的基于虛擬障礙物與實際障礙物融合的方法[4]，均取得了一定的效果。

國外基于柵格化地圖移動機器人路徑規劃方面有Dijkstra[5]和A*[6]等適合處理靜態地理環境的方法；同時還有D*[7]，Focused D*[8]，D* Lite[9]等適合應對動態地理環境或未知地理環境的方法，但是上述這些算法所獲得的路徑在真實的連續環境中卻不是最優的路徑。大多數情況下，這些算法所得路徑上某兩點間的路徑還可以用兩點間的直線來代替，這是因為這些算法均采用了固定角度步長(π/4)的方式向其相鄰節點傳遞固定的路徑里程信息，導致其獲得的路徑中轉向點處路徑轉過的角度一定是π/4的整數倍，從而約束了最優路徑的選擇。為解決該問題，國際上又出現了一類無角度約束路徑規劃(any-angle path planning)算法，典型的有Field D*[10]，Theta*[11]，Block A*，Cwave[1]等。但是，這些算法中，有的運算速度慢，有的需要前期預處理，有的又過于復雜。為解決該問題，本文提出了一種易于實現且運算高效的路徑規劃方法WG(wave gate)。

1 基本符號和定義

1.1 符號約定

柵格地圖M中所有柵格上的點組成1個集合，記為set-m(map)。定義由起始節點向終點不斷搜索的過程中，某時刻已經被算法遍歷的節點所組成的集合，記為set-c(close)；而地圖上未被遍歷的節點集合，記為set-v。由set-c中全部節點構成的區域稱作已遍歷區域，用area-set-c表示；由set-v中全部節點構成的塊區域，稱作待遍歷區域，用area-set-v表示。set-c中位于area-set-c與area-set-v交界線上的所有節點所組成的集合，記為set-f(fringe)，其中元素稱作前緣節點。柵格化地圖前緣節點示意如圖1所示，area-set-v與area-set-c的交界線上所有的節點都以“X”形符號標出，它們即為此時的前緣點，節點a，t，m均屬于set-f(灰色區域表示為障礙物區)。

圖1 柵格化地圖前緣節點示意

(1)

同時記

(2)

(3)

位于柵格地圖M中的任意節點v均定義1個父節點，記為P(v)。在算法執行的過程中，節點v到地圖上起點s的優化路徑可由v到P(v)的直線與P(v)到s的優化路徑組合而成。所以算法最終求得的路徑可由終點g開始向其父節點進行迭代性的連線，即父節點再向父節點的父節點進行連線，直至迭代到起點s時為止而得到。在算法初始化時，地圖上除起始節點的父節點定義為節點本身外，其他所有節點的父節點都定義為無窮大。同時，定義任意節點i到起始點的距離為Gi，若P(i)=j，由父節點的定義有：

Gi=Gj+Di-j

(4)

式中：Di-j——節點i到節點j的直線距離。

按式(5)定義評價函數K(i)，其中i為地圖上任意節點，并從集合set-f中選擇評價函數數值最小的節點t作為拓展基點，即K(t)=min(K(i))，記cv=t，由該節點將產生新的前緣節點：

K(i)=Gi+Di-g

(5)

式中：Di-g——節點i到終點g的直線距離。

1.2 柵邊節點

假設搜索算法已經完成了節點t的計算，并以節點t為基礎擬求出其所有鄰近節點中未被算法遍歷過的節點距離數據，此時稱節點t為柵邊基點，記為gv此時有gv=t，并稱t節點的父節點，即P(t)為set-ngb(t)任意元素的備選父節點，記為cp，則此時有cp=P(t)，用函數形式表示為

cp=CP(gv，vi)

(6)

式中：vi——set-ngb(t)中未被算法遍歷過的任意元素；CP(gv，vi)——以gv為柵邊基點時求取節點vi的備選父節點的函數。

set-ngb(t)中未被算法遍歷過的元素vi與節點P(t)(即節點vi的備選父節點CP(t，vi))構成的線段Lvi-cp必然與節點vi的某條鄰近柵邊相交，則稱該鄰近柵邊為節點vi在t為柵邊基點時的柵邊，組成該柵邊的節點稱為節點vi在t為柵邊基點時的柵邊節點，其中與節點vi構成對角關系的柵邊節點記為v1，另外1個柵邊節點記為v2，同時記：

(v1，v2)=Get_gate(t，vi)

(7)

圖1中已知P(a)=P(t)=j，顯然可知當t節點為柵邊基點時，set-ngb(t)中節點c的兩柵邊節點分別為a節點(v1)與t節點(v2)。

定義set-m中任意節點v有set-ngb(v)中任意未被算法遍歷的元素vi；以節點q為柵邊基點時，相應的柵邊節點v1與v2組成的柵邊Lv1-v2與Lvi-cp(其中cp=CP(q，vi))的交點與節點vi若存在直通路徑，即該交點與vi的連線不穿越障礙區域時，稱Lvi-cp能穿越Lv1-v2，記為

Lvi-cp>>Lv1-v2

(8)

反之若該交點與vi不存在直通路徑時即該交點與vi的連線穿越障礙區域時，稱Lvi-CP(vi)不能穿越Lv1-v2記為

Lvi-cp>

(9)

顯然對set-ngb(v)中任意節點vi，其柵邊節點v1與v2若滿足P(v1)=P(v2)=CP(v，vi)，且滿足Lvi-cp>>Lv1-v2則必有：

P(vi)=CP(v，vi)=P(v1)

(10)

即節點v1與v2的父節點P(v1)與節點vi間必然存在直通路徑。如圖1中以a節點為柵邊基點時，求得c的柵邊為La-t，其與Lj-c交點為u，又已知P(a)=P(t)=CP(a，c)，且交點u與節點c間的連線不穿越障礙區域，即Lc-cp>>La-t，則必然有P(c)=CP(a，c)，這與柵格地圖M中的實際情況相符。

2 柵格地圖上父節點的確定

2.1 鄰近節點的分類和柵邊節點的確定

依據式(1)和(2)，當節點v為柵邊基點時：

1)?vi∈set-ngb(v)若滿足式(11)的條件，則對應的節點稱為節點v的第二類鄰近節點。

(11)

2)?vi∈set-ngb(v)若滿足式(12)的條件，則對應的節點稱為節點v的第一類鄰近節點。

(12)

3)?vi∈set-ngb(v)若滿足式(13)的條件，則對應的節點稱為節點v的第零類鄰近節點。

(13)

對于不滿足式(11)～(13)條件的節點，本文不進行討論。式(11)～(13)中假定橫向柵格間距等于縱向柵格間距。

對于節點v的第二類鄰近節點vi，顯然節點v與vi必為對角關系，故v必為節點vi的柵邊節點，且v1=v，而對于v2的坐標[xv2，yv2]可由下式求得：

(14)

2.2 障礙物形成的分界線附近節點的父節點確定

障礙物形成的分界線附近的節點間的位置關系如圖2所示。圖2中假設節點j為起點，圖中灰色陰影部分為障礙物，為實現節點到起始節點j的路徑最短的要求，顯然過節點j與節點c的直線lj-c將該直線附近的節點的父節點劃分成節點j與節點c。即從節點j到節點c的方向上，自節點c以后位于直線lj-c上或上方的節點vi都有P(vi)=j，如P(b)=j；同樣，該方向上自節點c以后位于lj-c下方的lj-c附近的節點vi都有P(vi)=c，如P(a)=c。該類節點通常滿足P(v1)=P(P(v2))或P(v2)=P(P(v1))，例如以節點a為柵邊基點時，節點d的柵邊節點v1=a，v2=b，且明顯有P(b)=P(P(a))=j。

圖2 障礙物形成的分界線附近的節點示意

(15)

記滿足式(15)的節點vi的兩個柵邊節點對應的父節點，即P(v1)與P(v2)中距離vi近的父節點為p1，另外一個節點為p2。例如圖2中對于節點d有p1=c，p2=j。同時容易證明，若節點vi的兩柵邊節點滿足Lvi-cp>>Lv1-v2，即Lvi-cp能穿越Lv1-v2時，節點vi的父節點可按下述方法確定： 首先節點vi位于分界線lp2-p1上，則P(vi)=p2；其次若節點vi與節點v1位于lp2-p1的同一側，則P(vi)=P(v1)；最后若節點vi與節點v2位于lp2-p1的同一側，則有P(vi)=P(v2)。由式(3)可知，當vi位于分界線lp2-p1上時，可用函數的形式表達為

(16)

同樣由式(3)可知，節點vi與節點v1位于lp2-p1的同一側，不包括v1位于lp2-p1上的情況，用函數的形式表達為

(17)

同理可知，節點vi與節點v2位于lp2-p1的同一側，不包括v2位于lp2-p1上的情況，用函數的形式表達為

(18)

(19)

同理，式(17)與式(18)可分別等效為

(20)

(21)

綜上所述，滿足式(15)且同時滿足式(8)的節點vi的父節點，可依據式(22)確定：

(22)

式(22)中，依次用式(19)～(21)等效替代式(16)～(18)，主要是考慮移動機器人上工作的大多數嵌入式系統在處理乘法時的速度比除法要快許多。

3 算法實現

3.1 節點更新函數

定義函數(Gtemp，Ptemp，Ktemp)=UpdateVertice(vi，gv)，其中節點vi為待更新節點，gv為更新vi節點的柵邊基點。先由柵邊基點gv求出vi的兩柵邊節點v1和v2，對于滿足Lvi-cp>>Lv1-v2的節點可依據式(10)和(22)分別確定其父節點；對于Lvi-cp>

3.2 算法主要步驟

1)在前緣節點的集合set-f中選擇滿足K(t)=min(K(i))的節點t為拓展基點，即cv=t。從set-ngb(t)中依次選擇已經被算法遍歷過的節點q作為柵邊基點，調用節點更新函數UpdateVertice(t，q)求得Gtemp，Ptemp，Ktemp。若滿足Gtemp

2)以節點t為柵邊基點對set-ngb(t)中未被遍歷的鄰近節點vi進行分類，并按照第零類鄰近節點、第一類鄰近節點和第二類鄰近節點，按由高到低優先級依次調用節點更新函數UpdateVertice(vi，t)，并將計算結果更新到節點vi中，即G(vi)=Gtemp，P(vi)=Ptemp，K(vi)=Ktemp。

3)對于步驟2)中能確定父節點的節點vi，插入到set-f中供下次迭代時使用，同時將節點t從set-f中移除，并返回步驟1)繼而進行下一次迭代，直到終點節點g從set-f中被移出，或K(t)為無窮大時終止迭代。

4 計算結果與比較

WG算法與Field D*算法在同樣的地圖環境中進行路徑搜索的結果比較見表1所列，兩種算法的代碼都以C++寫成，并在同一臺主頻為3.7 GHz的計算機上進行運算。每類地圖的統計數據均由100幅隨機生成的同類型地圖運算后取平均所得，表1中地圖名為100×100(5)的地圖，表示地圖大小為100×100的柵格，且地圖中障礙物塊的比例為5%。

表1 WG算法與Field D*算法比較

從表1中的統計結果中不難發現： WG算法較Field D*在求解路徑所需的時間以及所求的路徑上轉向點的個數等指標上都有很大的進步。圖3中直觀地對比了兩種算法在不同地圖規模以及不同障礙物塊比例的地圖中的計算結果，可以看出WG算法在運算速度上有較大的優勢。由于Field D*算法采用線性插值的方式進行路徑信息的傳遞，所以需要消耗較大的計算資源；另外，由于實際的地圖環境中，距離起點的路徑長度分布基本不會在2節點間呈線性分布，所以Field D*求得的路徑長度較理論最短路徑還存在不少的優化空間。相比之下，因為WG算法是通過分析兩柵邊節點的父節點的特點來確定被計算的節點的父節點，所以從這個角度上看，WG求解出的路徑長度也將優于Field D*；其次由于Field D*算法是在1個柵格內進行線性插值導致其求得路徑上的轉向點增多，圖4中可以很明顯地顯示出了它的這個缺點。

圖3 WG算法與Field D*算法運行時間和求得路徑長度比較

圖4 WG算法與Field D*算法求得路徑上的轉向點比較

5 結束語

綜上所述，WG算法之所以能在計算速度、路徑轉向點以及路徑長度上較Field D*有明顯提高，主要得益于：

1)通過建立柵邊節點的概念來簡化算法在搜索無障礙區域時的計算速度，因為它僅需比較兩柵邊節點的父節點是否與柵邊基點的父節點為同一節點即可。

2)建立“障礙物形成的分界線附近節點的父節點的確定”的方法，利用兩柵邊節點的父節點以及待計算節點在障礙物形成的分界線上的同側關系，簡化復雜的角度求解計算(Theta*算法)。

但是為加快搜索速度，算法在柵邊節點的結構下引入A*算法中啟發函數的思想，某些特殊情況下將導致算法在遍歷部分節點時，存在柵邊節點的父節點還未被定義的情況，該算法在初始化時，將所有節點的父節點均定義為無窮大。目前的解決辦法是將這些柵邊節點插入Set-f中，并賦予比較高的優先級，使其在下輪迭代中成為拓展基點。然而，在某些實際應用中可以不用這樣處理，繼而避免過多的節點被引入set-f而降低算法的搜索效率，這也是后續需要研究和優化的一個方向。其次由于建立柵邊節點的過程中，通過式(14)來確定兩個柵邊節點中的v2的坐標，實際是利用柵邊節點v1與被計算節點間的對角關系(假定地圖上2個柵格方向上的柵格間距相等)，即v1與被計算節點的連線與坐標軸夾角為45°的前提條件，從而很容易地確定出v2的坐標，然而該方法并不能適用于非正方形柵格的地圖上，所以后續在非正方形柵格地圖上的計算也是需要繼續研究的問題。