新型樹啟發式搜索算法的機器人路徑規劃

胡曉敏，梁天毅，王明豐，李 敏，2

1.廣東工業大學 計算機學院，廣州510006

2.廣東工業大學 信息工程學院，廣州510006

1 引言

移動機器人是智能工業領域的重要輔助工具，代替人類執行危險任務等。其性能主要由三個部分決定：感知外界環境信息的傳感器技術[1]、定位和控制機器人位置的GPS 導航技術[2]以及引導機器人智能化作業的智能算法[3]，共同實現移動機器人的路徑規劃。在該技術中，要解決機器人從某個起始點到某個終止點的最短路徑算法求解問題，本文將對路徑規劃問題中的智能算法進行研究。

現有的路徑規劃算法主要包含兩部分：地圖建模和路徑搜索算法。算法先對環境信息進行建模，在處理后的地形信息上使用搜索算法。三維空間存在障礙物，建模時常把三維空間柵格化為可行點與不可達的障礙點。已有很多學者提出了兩點之間最短路徑的規劃算法。傳統的方法有快速配對算法FM[4]，其效率與空間的總點數相關，運行時間隨著空間的點數增多顯著增加；混合整數線性規劃[5]算法只適用于小規模空間地圖；搜索路徑較為曲折的A*算法[6]等，這些算法基于貪心選擇的方式構建路徑。

在復雜障礙物的環境下，移動機器人的路徑規劃已被證明為NP 難問題[7]。Fu 等人[8]提出量子粒子群優化（QPSO）的方法解決無人機在三維空間規避障礙物的路徑規劃問題，該空間采用連續的方式建立模型。A*算法作為一種確定性的搜索算法，結合了啟發式和最短路徑搜索的特點，已被成功應用于各種調度[9-10]、自動駕駛[11-12]、電路配置[13]等問題。Yu等人[14]將A*算法和蟻群算法相結合，利用蟻群算法優化連接三維空間中多個目標點的最短回路，類似求解旅行商問題，然而兩個目標點之間的最短路徑仍采用A*算法實現。曾有學者提出快速擴展樹算法[15]以解決未知連續空間的路徑規劃問題，但其由于樹形結構過于龐大且樹節點是隨機生成的，在包含狹窄通道的環境中難以搜索到路徑，導致得出的最短路徑較長。

本文受擴展樹結構的啟發，對樹結構修改成適用于離散已知空間的地圖并確定性選出有效點來解決路徑規劃問題。綜合搜索空間壓縮和搜索方向引導兩方面，本文提出了一種基于可靠邊緣點模型的樹擴散啟發式算法，記為Tree-EP，其創新點包括：

（1）構建可靠邊緣點模型，將空間搜索域壓縮到可靠邊緣點搜索域；

（2）提出樹擴散架構選出潛力點以提高引導搜索方向的智能性；

（3）提出潛力點判定范圍H ，合適的H 范圍設置能增強搜索的導向；

（4）對算法引入局部回溯優化。

實驗中測試了Tree-EP算法求解多種帶障礙的三維空間路徑規劃實例的性能，并對比了QPSO 算法，傳統A*算法和添加了局部調整操作的改進Tree-EP 算法。實驗結果顯示，改進Tree-EP具有更佳的路徑尋優能力。

2 傳統A＊算法

A*算法是路徑規劃領域里比較流行的啟發式搜索算法，該算法的特點是在圖G=(V,E)中，從起始點開始，逐步通過探索已知點的鄰域以到達終止點，期間在評價已知的路徑節點時除了含有當前點到起始點的信息，還添加了到達終止點的啟發式信息。評價路徑節點的公式為：

其中，f(x)是路徑點x 的評估值，g(x)是路徑點x 到起始點的距離，h(x)是路徑點x 到終止點的啟發式距離。算法的運行流程如下：

步驟1 初始化地圖所有點的評估值f 為無限大，設置起始點st 的評估值f(st)=0，并將起始點st 放入候選集Q 中。

步驟2 將候選集Q 中評估值f 最小的路徑點u 作為下一個路徑點，并把該點移出候選集Q。

步驟3 若步驟2 中被移出候選集Q 的點u 為終止點，則結束算法。

步驟4 將點u 鄰域里的每一個未加入過候選集Q的點m 加入到Q 中，若g(u)+length(u,m)＜g(m)，則修改g(m)=g(u)+length(u,m)，回到步驟2。

3 空間可靠邊緣點模型

傳統柵格路徑搜索方法搜索所有空間點構建路徑解，假設在一個三維空間中，每一維取固定步長分割為n 個點，需對n3個空間點進行搜索，實際上可以篩選出部分空間點以縮小搜索范圍。以二維平面為例，如圖1，離散后的空間有6 個離散空間點，中間的矩形為障礙物，所圍的黑色實心點為障礙點，灰色點為可行點，點A為起始點，點B 為終止點，當點A 和點B 不能直連時，其最短路徑的中間點往往經過障礙的邊緣附近，如圖中的點M 。意味著只需搜索障礙物邊緣的位置點。

圖1 障礙物空間中A、B點之間最短路徑

空間可靠邊緣點模型的設計思路，就是通過對地圖空間中的點進行篩選，根據路徑安全情況，篩選出障礙物的可靠邊緣點信息。以障礙物周圍的可靠邊緣點構建路徑搜索空間，大大縮小搜索的柵格點數以及調整路徑的碰撞風險。

3.1 障礙可靠邊緣點構建

本文的方法是構建長寬高分別為lx、ly、lz的三維空間，并對該空間進行等距離的離散化操作，在三維空間中構建O-XYZ 坐標，生成sx×sy×sz個空間點，每個點的坐標值如下：其中sxsysz分別表示每一維上的點密度。通過給定障礙的安全距離δ，得到用于路徑構建的障礙周圍的可靠邊緣點集，該構建步驟為：首先，擴展障礙物周圍安全距離δ 內的可行點為非可行點，其余的空間點標記為可行點；其次選出可靠邊緣點。對于可行點是否為可靠邊緣點的判斷方式為：若該可行點周圍26 個鄰居點里至少存在一個非可行點，則把該可行點定義為可靠邊緣點。注意這里的26個鄰居點指的是三維空間上當前點(xi,yj,zk)周圍的鄰居柵格點，坐標分別為(xi+Δx,yj+Δy,zk+Δz)，Δx ∈{-1/sx,0,1/sx}，Δy ∈{-1/sy,0,1/sy}，Δz ∈{-1/sz,0,1/sz}。整個設置過程的偽代碼如算法1。

算法1 障礙可靠邊緣點構建

For 每一個障礙物

將障礙物所占空間的最小x y z 坐標值各減(δ+1)后標記為min_x,min_y,min_z；

將障礙物所占空間的最大x y z 坐標值各加(δ+1)后標記為max_x,max_y,max_z；

將上述6個更新后的坐標所圍空間標記為S；

For 空間S 內的點

If 該點在障礙物周圍安全距離δ 內

標記該點為非可行點；

End if

End for

For 空間S 內的可行點

If 該點周圍26 個鄰居點至少存在一個非可行點把該點定義為可靠邊緣點；

End if

End for

End for

3.2 空間兩點是否直連的判別方式

空間兩點之間的關系可劃分為可直連和不可直連。若兩點之間的線段不觸碰障礙物，則標記這兩點為可直連關系；否則，標記為不可直連關系。在判斷兩點間關系時，對兩點間線段上的點進行沿各個坐標軸的等間距抽樣，并對抽樣后的點歸約到離之最近的離散點，若出現任何一個歸約后的離散點是不可行點，則判定這兩點的連線觸碰到了障礙物，并標記這兩點的關系為不可直連，實現的偽代碼如算法2。

算法2 空間兩個可行點P 與Q 的直連關系判斷

For 線段PQ 上滿足x y 或z 坐標中至少有一個坐標為整數的連續空間點M(x,y,z)

將連續空間點M(x,y,z)歸約到與之最近的離散空間點N(x′,y′,z′)；

If 點N 為不可行點

標記點P 和點Q 為不可直連關系；

Break；

End if

End for

If 點P 和點Q 不為不可直連關系

標記點P 和點Q 為可直連關系；

End if

4 基于可靠邊緣點模型的Tree-EP算法

4.1 樹擴散基本架構

經過空間可靠邊緣點模型對空間的壓縮后，算法的搜索范圍縮小到邊緣點上，但對這些邊緣點如何排列組合成一條最短路徑，需要搜索方向的引導。本文提出樹擴散架構的基本思想為：從起始點開始不斷擴散探索障礙的邊緣，選取出潛力邊緣點添加到集合中，直到找到目的點或集合中所有潛力點擴散完畢為止。這里的潛力邊緣點必須滿足三個要求，分別是：（1）屬于邊緣點，（2）該點能與擴散點直連，（3）該點判斷范圍H 內至少存在一個不能與擴散點直連的可行點。

圖2 舉例說明了選擇這些邊緣點的原因。圖中有三個圓形障礙物，起始點為S，終止點為E。對于起始點S 來說，其擴散的可見邊緣點為圖中灰色點組成的弧AB，除了點A 和點B 外，弧中的其余點對路徑尋優是沒有作用的。點A 和點B 除了是點S 的可見邊緣點外，還有一個共同特點就是都存在點S 不可直連的可行鄰居點。因此滿足以上三個要求的邊緣點，才值得下一步擴散。

圖2 樹擴散基本架構

圖2 中黑色實心點A B 為點S 擴散的潛力點并被納入為其孩子節點，點C D 為點A 擴散的潛力點，點F G 為點B 擴散的潛力點，點I 為點G 擴散的潛力點。最后點C D F G I 均能直連終止點，作為樹的葉子節點，整個樹構建完畢。

4.2 潛力點判斷范圍H的設置

由樹擴散基本架構可知，潛力點的判斷影響著整個樹形搜索結構。假設有任意潛力點P(x0,y0,z0)，其判斷范圍H 內的空間點為(x,y,z)，相關設置如下：

l 為柵格的單位距離，n 為放大的倍數。潛力點判斷范圍H 如圖3所示，H1為n=1 時的范圍，H2為n=2 時的范圍，n 越大，H 越大，潛力點判斷越寬松，得到的潛力點也越多。此處通過圖4、圖5和圖6對比n 在不同設置時被選中的潛力點。圖中的黑色矩形為障礙物，黑色實心點為障礙點，點S 為擴散點，空心點為所屬點S 的潛力點。可見n=1，2，3時，潛力點的個數分別為3，7和9個，n 越大，潛力點個數越多，也意味著之后需要擴散的點數也越多，樹結構越龐大和復雜。為了壓縮搜索空間，準確指導搜索方向和精簡樹擴散架構，本文將n 設置為1。

圖3 潛力點判斷范圍H

圖4 n=1 時的潛力點

圖5 n=2 時的潛力點

圖6 n=3 時的潛力點

4.3 潛力點擴散機制

Tree-EP 把擴散得到的潛力點納入候選集，從集合中選出評價值最好的潛力點進行擴散。Tree-EP對每個可行點的評價函數如公式（1），每個潛力點x 的g(x)和h(x)設置為：

cost(x)表示點x 在樹結構中不斷沿著父節點追蹤到根節點的沿途的路徑長度。

妊娠期高血壓疾病是妊娠期常見并發癥之一，主要臨床癥狀為高血壓、水腫以及蛋白尿等，如果在發病之初未能及時有效治療，會導致疾病累及全身器官，甚至造成孕產婦和圍生兒的死亡[1-4]。以往硫酸鎂是妊娠期高血壓疾病的常用首選藥物，其能有效緩解微血管痙攣，但降壓效果不好[5]。本研究探究拉貝洛爾聯合硝苯地平治療妊娠期高血壓疾病的降壓效果及臨床意義。現將研究結果報道如下。

length(x,ed)表示點x 到終止點ed 的直線距離。Tree-EP的擴散操作偽代碼如下所示：

算法3 Tree-EP的擴散操作Extend(x)

If 點x 可以直連終止點

計算點x 的評價值f ；

構建點x 對應的完整路徑；

flag=true； //標記已找到最短路

return；

End if

從可靠邊緣點集中提取出關于點x 的潛力點集J ；

For 集合J 里的所有潛力點

If 該潛力點已執行擴散操作

continue；

Else

If 該潛力點不在樹結構中

把該點作為點x 的孩子節點；

End if

局部回溯優化；

計算該潛力點的評價值f ；

把點x 加入到候選集Q 中；

End if

End for

如圖7，引入潛力點擴散機制后，點S 擴散得到點A 和B，均放入擴散候選集Q，發現點A 的評價值比點B 的要小，即評估距離SAE 比SBE 短，于是從集合Q中選出點A 進行擴散，得到點C 和點D，發現點D 的評價值最小，于是從集合Q 中選出D，且發現點D 能直連點E ，輸出最短路徑SADE 并停止其他節點的擴散。最終整個樹架構得到精簡，只留下節點SABCD。

圖7 引入潛力點擴散機制的樹擴散架構

4.4 局部回溯優化

Tree-EP 在進行搜索時，任意點擴散得到的某個沒有孩子節點的潛力點分成兩類情況，分別是潛力點不在擴散候選集Q 和已在擴散候選集Q。

4.4.1 潛力點不在候選集Q 時的優化

如果潛力點不在候選集Q，意味著該點不在樹中，若該潛力點能與所屬擴散點的父節點直連，則在樹中將所屬擴散點替換成潛力點。如圖8，點F 擴散得到點G，點F 的父節點為點E，EF 和FG 的路徑長度均為0.3，發現點G 可與F 的父節點E 直連且EG 路徑長度0.5 比EFG 的路徑長度0.6 要短，把點F 從樹中刪除，將點G 作為點E 的孩子節點。

圖8 潛力點不在候選集Q時的優化

4.4.2 潛力點在候選集Q 時的優化

如果該點沿著新擴散點回溯到根節點的路徑長度比沿著父節點回溯到根節點的路徑長度要短，則在樹中將該潛力點的父節點設為新擴散點。由于該潛力點的父節點變更了，判斷該潛力點能否與父節點的父節點直連。如圖9，點S 擴散得到A 和B，點A 擴散得到C 和D ，點B 也擴散得到C 和D ，但SBD 的路徑長度比SAD 短，因此把點D 的父節點更改為點B。

4.5 Tree-EP的算法流程

圖9 潛力點在候選集Q 時的優化

整體框架如算法4，首先構建地圖空間和可靠邊緣點模型，然后將起始點納入到擴散候選集Q 作為初始化，進入循環。從集合Q 中選出評價值f 最小的點x并對該點執行擴散操作extend(x)，擴散完畢后如果找到能與終止點直連的葉子節點，則退出循環，否則繼續循環直到候選集Q 為空集。循環結束后輸出最短路徑。

算法4 Tree-EP

構建地圖空間；

構建可靠邊緣點模型；

將起始點納入擴散候選集Q；

While 擴散候選集Q 不為空

從擴散候選集Q 中選出評價值f 最小的點x；

Extend(x)；

If flag==true

Break； //已找到最短路徑

End if

End while

輸出最短路徑

4.6 Tree-EP與A＊的搜索過程對比

在兩點之間求解最短路徑的路徑規劃問題中，A*算法的路徑搜索方向是依靠空間點到終止點的啟發式數值和到起始點的實際路徑距離引導的，其擴散過程是不斷將鄰居點納入候選集，再不斷從候選集中選出評價最好的點進行路徑構建引導算法進行搜索。盡管A*的整體搜索方向總是指向終止點，如果指向終止點的方向有障礙物阻擋，各條路徑會在觸碰到障礙之后才改變方向繼續構建路徑，這種預測性不強和碰到阻礙才改變搜索方向的缺陷使得A*算法構建出來的路徑較長。A*算法的搜索過程中納入大量的鄰居點，沒有充分利用空間中的障礙信息。

本文提出的Tree-EP算法根據受障礙阻擋的兩點間最短路徑往往觸碰障礙物邊緣的原則，設計出利用潛力邊緣點進行最短路徑構建的擴散機制，擴散得到的潛力點納入候選集，從集合中選出評價值最好的潛力點進行擴散。這種方法利用了障礙邊緣信息，把路徑搜索直接指向最有潛力擴散的邊緣點，提前規劃避開障礙物的搜索方向，能帶有預測性地引導路徑的構建。圖10 舉例比較了兩種算法的搜索過程。

圖10 Tree-EP和A*的搜索過程

圖10 中點S 為起始點，點E 為終止點，黑色實心多邊形為障礙物，灰色點為Tree-EP搜索時忽略的點，黑色實心點為樹結構中的節點，也即潛力點，黑色帶箭頭的實線表示擴散過程，帶箭頭的虛線表示A*的搜索方向，旁邊的標號代表A*搜索方向的變化。對于A*算法，根據其特性會先往直線SE 方向搜索，即方向①，然后觸碰墻壁后，搜索方向稍微往SE的垂直方向靠近，再次觸碰墻壁后繼續調整搜索方向，直到找到搜索的出口，其在障礙物包圍區域里的搜索方向按編號①②③④的順序變化，可見A*算法要經歷多次搜索才找到障礙物包圍空間的出口。而Tree-EP 算法先提取出障礙邊緣點，在擴散過程中判斷圖中的灰色點均是無意義點，逐步將潛力點構建到樹結構中，最后得出最短路徑。圖中的黑色帶箭頭的實線表示Tree-EP的搜索方向，可見比A*算法的搜索要更加準確。

Tree-EP 在進行邊緣點確認的過程中，其判定對象是空間中障礙物周圍的邊緣點，基于空間中的障礙位置進行潛力邊緣點的選擇，目前激光測距、聲吶探測的技術都為障礙位置的判定提供了方法，無需對全圖的空間點進行分析判斷。

5 實驗設計及數據分析

5.1 測試用例

本文選取了圓形、矩形和山脈三類障礙物作為用例。為了便于比較，本實驗中三個坐標軸的柵格密度都取相同值。圓形和矩形實例相當于在三維空間上有圓柱和矩形柱的障礙，把起始點與目標點的z 坐標都設置為0，該三維實例轉換為二維表示。這兩個實例用于比較不同形狀的障礙邊緣對算法尋找最優路徑的影響。圓形和矩形實例在密度為20下的障礙位置如表1和表2所示。其中表1 顯示該實例有11 個障礙，屬性中的(x,y,r)分別代表障礙的圓心和半徑值，在圓形內的空間都屬于障礙物。

表1 圓形實例的障礙設置（密度20）

表2 矩形實例的障礙設置（密度20）

表2 給出了矩形實例的障礙位置數據，共有9 個矩形柱障礙，其中表中的屬性數據(x,y,lx,ly)表示該矩形在密度為20下的左下角x 和y 坐標，lx和ly表示矩形在該坐標下往x 軸和y 軸正方向的長和寬度值。

本文在不同柵格密度下的圓形和矩形實例都是通過對表1 和表2 中的坐標數據進行縮放獲得的。山脈實例采用遠大于柵格密度數量的隨機點插值出地形高度的方式生成三維地形。因此本實驗能反映出不同柵格密度下，算法對同一個三維帶障礙空間的路徑搜索能力。

圓形和矩形實例的起始點和目標點的坐標都分別取值為（0，0）和(sx-1,sy-1)，山脈實例的起始點和目標點的x 和y 坐標取值同上，而z 坐標則取對應位置的地形高度以及安全距離之和以上的柵格點高度。每類障礙物在不同密度下的總像素點數和邊緣點數如表3到表5 所示。從表中看到，構建障礙可靠邊緣點過程后，搜索的潛在空間點數大大減少。

表3 圓形實例

表4 矩形實例

表5 山脈實例

邊緣點數所占空間總點數的比例隨著密度的上升而下降。特別是在柵格密度增大的情況下，比如每一維都為所測最大密度時，邊緣點的數量在三個實例中只有原始空間總點數的5.72%～15.7%。

5.2 算法設置與數據表示

本文比較了量子粒子群優化（QPSO）[8]、傳統A*、添加了局部調整操作的改進A*、Tree-EP 和改進Tree-EP這五種算法對最短路徑求解的性能。其中QPSO 為群體智能優化算法，用于對比不同算法的路徑尋優能力，其參數設置與文獻[8]一致，本實驗給出的數據均為QPSO在30次獨立運行中的最優解。

為了比較不同密度值對最優路徑長度的影響，實驗結果中對路徑長度進行了歸一化：首先，把三維空間柵格化后，每個柵格的步長為1，得到一個新的柵格坐標；然后，通過算法搜索出最優路徑，計算柵格空間中路徑點之間的歐幾里德距離，得到該最短路徑的長度；第三，把路徑長度值除以密度值，得到去除密度值影響的歸一化后的路徑長度值。

5.3 不同柵格密度下路徑長度的比較

表6 比較了五種算法對圓形實例的標準化路徑長度值。從表中可以看到，QPSO在全部所測密度下的路徑長度比A*的要好，在密度為20以外時的結果比Tree-EP和改進Tree-EP要差。全部算法當中，結果最差的是A*算法，但是經過局部回溯優化后的改進A*比A*的結果好很多，說明A*算法在路徑構建時過于粗糙，其自身有很大的優化空間。但是經過改進后的A*算法性能仍比Tree-EP和改進Tree-EP都要差，說明加入樹擴散架構的Tree-EP 和改進Tree-EP 的路徑搜索性能得到了質的提高。

表6 各算法標準化路徑長度的比較（圓形實例）

從表7 給出的矩形實例的測試結果可知，Tree-EP和改進Tree-EP 同樣表現出優越的性能，在各個密度的路徑長度都比QPSO、A*和改進A*的好。

表7 各算法標準化路徑長度的比較（矩形實例）

對于地形細節較多和較復雜的三維山脈實例，如表8在相同密度比較下，QPSO的路徑均比A*的要好，但比改進A*、Tree-EP 和改進Tree-EP 要差。A*算法在引入局部回溯優化后的改進A*結果比原來要好，在Tree-EP中加入該優化操作后的改進Tree-EP 也得到更好的結果，說明了局部回溯優化在復雜和崎嶇的地形下效果顯著。

表8 各算法標準化路徑長度的比較（山脈實例）

5.4 柵格密度影響的進一步分析

空間的柵格密度影響算法最終得到的最短路徑。由于Tree-EP 和A*算法都是基于空間柵格進行路徑構建，柵格點密度過小會導致位于障礙之間的最優路徑被忽略。圖11比較了算法在圓形實例柵格密度為20和40得到的最優路線。圖中實心點代表該柵格點是不可行點。當密度為20 時，過小的密度導致算法忽略了中間障礙密集的區域，當柵格密度增大到40 時算法才發現障礙間的更優路徑。因此，柵格密度的下限值應與空間中任意兩個障礙之間扣除兩邊安全距離后得到的最小距離有關。

另一方面，從表7 的數據看出，增大柵格密度有可能提高算法找到更優路線的能力，但由于存在舍入誤差，并不保證算法可以得到更短的路徑長度。例如，密度為180 時得到的長度比密度為160 時要長。圖12 比較了算法在矩形實例柵格密度為60 和180 時的路徑圖。可見當密度增大后，在保證安全距離前提下路徑更貼近障礙，路徑長度更短，而且Tree-EP算法總能找到比其他算法更短的路線。

對于山脈實例來說，得到的最短路徑值隨著柵格密度增大呈遞減的趨勢。圖13對比了算法在柵格密度為15 和45 的路線圖。柵格密度增大后，柵格空間表現的空間高度差異更加細化，改善了障礙之間部分的最優路徑被忽略的問題。然而空間密度的增大會增加搜索空間的點數量，延長運算時間。從表8 的結果看出，柵格密度取值為45 和50 時改進的Tree-EP 算法得到的標準化路徑長度的改善程度已經小于0.5×10-3，是否加大柵格密度取決于實際問題的精度需要。

圖11 圓形實例下密度為20和40的路徑圖示

圖12 矩形實例下密度為60和180的路徑圖示

圖13 山脈實例下密度為15和45的等高線圖

6 結束語

本文提出了一種用于縮小搜索空間的可靠邊緣點模型，能將整個空間壓縮到邊緣點搜索空間，在此基礎上再提出樹擴散架構，該架構從邊緣點中提取出能有效指導搜索方向的潛力點。另外將潛力邊緣點搜索機制和樹擴散架構相結合，提出了Tree-EP算法，該算法能搜索出空間中的有效路徑點，最終得出更好的最短路徑。此外，還對算法引入局部調整策略得到新型的改進Tree-EP 算法，結果顯示該優化機制有局部修正路徑的功能，能進一步縮短所得路徑的長度。實驗對QPSO、A*、改進A*、Tree-EP 和改進Tree-EP 的性能進行比較，總的來說，Tree-EP和改進Tree-EP的最短路徑搜索性能很好，對兩點間最短路徑搜索的路徑規劃問題有著重要的意義。