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

楊建禹，白國振

(上海理工大學(xué)機(jī)械工程學(xué)院，上海200093)

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

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

1 基本符號和定義

1.1 符號約定

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

圖1 柵格化地圖前緣節(jié)點(diǎn)示意

(1)

同時記

(2)

(3)

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

Gi=Gj+Di-j

(4)

式中：Di-j——節(jié)點(diǎn)i到節(jié)點(diǎn)j的直線距離。

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

K(i)=Gi+Di-g

(5)

式中：Di-g——節(jié)點(diǎn)i到終點(diǎn)g的直線距離。

1.2 柵邊節(jié)點(diǎn)

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

cp=CP(gv，vi)

(6)

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

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

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

(7)

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

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

Lvi-cp>>Lv1-v2

(8)

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

Lvi-cp>

(9)

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

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

(10)

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

2 柵格地圖上父節(jié)點(diǎn)的確定

2.1 鄰近節(jié)點(diǎn)的分類和柵邊節(jié)點(diǎn)的確定

依據(jù)式(1)和(2)，當(dāng)節(jié)點(diǎn)v為柵邊基點(diǎn)時：

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

(11)

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

(12)

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

(13)

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

對于節(jié)點(diǎn)v的第二類鄰近節(jié)點(diǎn)vi，顯然節(jié)點(diǎn)v與vi必為對角關(guān)系，故v必為節(jié)點(diǎn)vi的柵邊節(jié)點(diǎn)，且v1=v，而對于v2的坐標(biāo)[xv2，yv2]可由下式求得：

(14)

2.2 障礙物形成的分界線附近節(jié)點(diǎn)的父節(jié)點(diǎn)確定

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

圖2 障礙物形成的分界線附近的節(jié)點(diǎn)示意

(15)

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

(16)

同樣由式(3)可知，節(jié)點(diǎn)vi與節(jié)點(diǎn)v1位于lp2-p1的同一側(cè)，不包括v1位于lp2-p1上的情況，用函數(shù)的形式表達(dá)為

(17)

同理可知，節(jié)點(diǎn)vi與節(jié)點(diǎn)v2位于lp2-p1的同一側(cè)，不包括v2位于lp2-p1上的情況，用函數(shù)的形式表達(dá)為

(18)

(19)

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

(20)

(21)

綜上所述，滿足式(15)且同時滿足式(8)的節(jié)點(diǎn)vi的父節(jié)點(diǎn)，可依據(jù)式(22)確定：

(22)

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

3 算法實(shí)現(xiàn)

3.1 節(jié)點(diǎn)更新函數(shù)

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

3.2 算法主要步驟

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

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

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

4 計算結(jié)果與比較

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

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

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

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

圖4 WG算法與Field D*算法求得路徑上的轉(zhuǎn)向點(diǎn)比較

5 結(jié)束語

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

1)通過建立柵邊節(jié)點(diǎn)的概念來簡化算法在搜索無障礙區(qū)域時的計算速度，因?yàn)樗鼉H需比較兩柵邊節(jié)點(diǎn)的父節(jié)點(diǎn)是否與柵邊基點(diǎn)的父節(jié)點(diǎn)為同一節(jié)點(diǎn)即可。

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

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