改進型RRT*算法的水下機器人三維全局路徑規劃

師穎慧，張 冰，趙 強

（江蘇科技大學電子信息學院，江蘇鎮江 212003）

0 引言

自主水下航行器（Autonomous Underwater Vehicle，AUV）能夠按照需求探測水下環境并自主完成作業任務。路徑規劃技術是自主水下機器人關鍵技術之一，指為到達某個目標或完成某個任務對所規劃設備的航行方向、航行路線等進行預先計算、設定、優化的過程。路徑規劃技術在一定程度上標志著水下機器人智能化程度的高低。

水下航行器路徑規劃需要在三維空間中尋找可行路徑，是一項非常困難和具有挑戰性的任務。目前全局路徑規劃算法有Dijkstra算法、A*算法、RRT算法、粒子群優化算法（PSO）、蟻群優化算法（ACO）等。Dijkstra算法是典型的單源最短路徑算法，用于計算一個節點到其他所有節點的最短路徑，雖然其獲得最短路徑的成功率高、魯棒性好，但在大規模復雜路徑拓撲網絡中，節點遍歷和效率低下是其致命缺陷，所以不適合三維的空間環境；A*算法是求解靜態路網中最短路徑最有效的直接搜索解，也是求解其他問題的一種常見的啟發式算法，優點是低成本、啟發式最優解路徑且在規劃過程中可以及時中斷和恢復，但A*算法是通過比較當前路徑鄰近網格的啟發式函數值逐步確定下一個路徑網格，當存在多個最小值時A*算法不能保證搜索到最優路徑，即A*算法容易陷入極小值點，不適合解決三維空間中的路徑規劃問題；粒子群優化算法（PSO）是一種基于鳥類種群捕食和回歸的啟發式算法，在搜索的初始階段收斂速度較快，但在搜索的后期收斂速度較慢；蟻群算法優點是可應用于水下三維路徑搜索問題，其參數相對較少，不需要人工調整，但是收斂速度慢，容易陷入局部最優。幾種全局路徑規劃算法各有優缺點，都能實現對全局避障路徑的搜尋，但基本前提是要對環境空間進行路徑建模，之后在已有路徑基礎上運用算法搜尋最優路徑，Canny已經證明隨著維數的增長這些算法速度會大大下降。針對這些問題，本文引入基于采樣的隨機搜索，如快速搜索樹算法（Rapid-exploring random tree，RRT），在復雜的水面環境下，由于不需要提前對環境空間進行路徑建模，更方便搜尋全局避障路徑。RRT算法是基于采樣的單一查詢路徑規劃算法，其不需要對狀態空間進行預處理，且搜索樹能快速朝著未知的狀態空間部分搜索。文獻［9-13］指出，所有基于RRT 的方法幾乎都是次優的，即使用RRT算法規劃出的路徑不是最優的，因此引入RRT*。RRT*擴展了RRT 求漸近最優解，即當樣本個數趨于無窮時，它保證收斂于最優解。然而，水下環境的特點是障礙物很分散，包括地形障礙物和漂浮障礙物，這與地面上的障礙物不同。RRT*中全局均勻隨機搜索導致搜索效率低、收斂速度慢、存儲要求高，特別是在水下環境和高維空間中。

本文針對RRT*中出現的問題對其進行改進，提出NT-RRT*算法（正態采樣、三角裁剪的快速擴展隨機樹的改進算法），利用正態分布的空間采樣策略代替RRT*中的全局均勻隨機采樣，使算法具有目標偏置性，加快了算法的收斂速度。針對規劃出的路徑過于曲折、轉角過多，不適合AUV 的實際運動情況，又引入基于三角不等式的幾何修剪算法，在算法中用鄰近節點的父節點代替臨近點，然后與新加入的節點相連，使距離代價更小，規劃出的路徑更加平滑。仿真結果表明，改進的RRT*方法通過改變采樣策略和減少節點數，提高了水下路徑規劃中的路徑搜索效率和收斂速度，降低了路徑代價，減少了路徑的彎曲程度。

1 改進的RRT*算法

1.1 環境建模

本文利用柵格法將環境地圖分為725×648格，每個格子不僅表示自身的像素信息，還用8 位二進制數表示其顏色信息，分別用不同顏色來標注障礙物區域和非障礙物區域。因為不同的顏色代表不同的RGB 值，這樣在算法中很容易識別障礙物，對地圖進行預處理，避免了算法執行過程中對路徑進行避障距離檢測，減少了算法的計算時間。

1.2 正態分布空間采樣策略

假設環境空間為D，Dfree為環境空間中的無障礙區域，D為環境空間的障礙區域，q∈D為起始點，q∈D為目標點。

Fig.1 Two-dimensional normal distribution image with mean 25，25 and variance 25 and 25圖1 均值25、25，方差為25、25 的二維正態分布圖像

圖1 中，空間中的點以均值25、25 為中心呈現正態分布，且數據的分布區間由標準差25、25 決定，說明通過設置正態分布函數的均值，標準差可以決定數據的分布情況。所以本文在算法中利用MATLAB 中的二維正態概率密度的離散函數對空間的點進行采樣，使離散點符合以目標點為均值、目標點與起始點的大致范圍設為標準差的正態分布，這樣算法在空間中采的點就不是服從均勻分布而是以目標點為正態分布的最高點。從起始點到目標點均服從正態分布，從而使算法具有目標偏置性，加快了算法的收斂速度。改進后的N-RRT*（正態采樣RRT*算法）詳細內容見下面的N-RRT*算法步驟Algorithm 1，改進的RRT*方法描述如下：

函數Samplenodes 是二維正態采樣點函數，其返回以采樣點為均值，以起始點和目標的大致范圍為方差，協方差為0 的離散點；函數FeasiblePoint 用來判斷點是否在地圖中或者是否與障礙物相碰撞；函數linepath 用來連接拓展的頂點和分支。

N-RRT*算法步驟如下：

1.3 基于三角不等式的幾何修剪算法

RRT*算法在RRT算法基礎上引入代價函數來優化規劃出的路徑，經過逐次迭代改善之前的路徑，從而得到一條最優或準優路徑。與RRT 不同的是，RRT*在擴展新節點時要分別計算最近鄰點集合中的所有點到新頂點的距離，加上新頂點到起始點的距離作為路徑代價，選擇出一條最短路徑，而RRT 是直接由起始點到最鄰近點再到新頂點進行擴展，不考慮路徑代價。

三角不等式的幾何修剪算法（T-RRT*）在RRT*的基礎上進行路徑代價的改進，其不僅考慮鄰近點集合中的點，還把臨近點的父節點放入集合中，使其有共享父節點的趨勢。圖2、圖3 分別是RRT、RRT*、T-RRT*三種算法的路徑擴展方式，其基于q為算法的起始點，q為新加入的頂點，q為以q為球心、以r 為半徑的球內的點，q為隨機樹中離q最近的點。

圖4 中實線表示隨機數原來的擴展路徑，虛線表示重新選擇父節點后的路徑。由三角形中任意兩邊之和大于第三邊定理可知，q到q的距離加上q到其父節點的距離（即圖中黑色虛線所示）大于q到q父節點的距離（即圖中黑色虛線所示），所以NT-RRT*算法計算路徑代價時把q的父節點（圖中黑色實心點）也考慮在內，在每一次迭代后計算出最小路徑代價，并進行路徑重連（即由紅色實線代替黑色虛線的路徑）。這種基于三角形不等式的定理，理論上會使更多的節點共享同一個父節點，重新連線的結果如圖5 中紅色實線所示。

Fig.2 Path extension mode of the RRT algorithm圖2 RRT算法路徑擴展方式

Fig.3 RRT* algorithm path extension圖3 RRT*算法路徑擴展

Fig.4 T-RRT* reselecting the parent node圖4 T-RRT*重新選擇父節點

Fig.5 T-RRT* path reconnecting after selecting the parent node圖5 T-RRT*選擇父節點后路徑重連

2 改進的NT-RRT*算法在海底環境下的仿真

水下環境有浮動及不平衡的地形兩個障礙。本文對改進的RRT *在有浮動障礙和地形障礙的環境中進行仿真。仿真中利用MATLAB 建模，模型中將環境地圖分為725×648 格，起始點格數設置為（193，305），目標點格數設置為（206，82）。仿真中，4個案例的采樣節點都以紅色圓點表示，擴展樹的分支由紅色直線表示，初始路徑由黑色線表示。NT-RRT*算法如下：

使用MATLAB 形成的算法仿真圖形如圖6—圖11 所示，坐標單位：m。

由圖8 可知，在使用了正態分布的偏置采樣策略后，隨機樹在擴展過程中只朝著目標點進行擴展，而不是像圖6中均勻朝著四周擴展。由圖9 與圖7 對比可知，N-RRT*算法在進行路徑規劃時選擇了一條障礙物更少的路徑，且規劃出的路徑離障礙物更遠，更有利于水下機器人在實際航行中安全前進。

Fig.6 RRT* algorithm simulation圖6 RRT*算法仿真

Fig.7 Specific route formed by RRT*algorithm simulation圖7 RRT*算法仿真形成的具體路線

Fig.8 N-RRT* algorithm simulation圖8 N-RRT*算法仿真

Fig.9 Specific route formed by N-RRT* algorithm simulation圖9 N-RRT*算法仿真形成的具體路線

由圖10 可知，NT-RRT*算法規劃出的路徑很平滑，唯一的一處拐角也離障礙物很遠，水下機器人在實際操作中會更加簡單。由圖11 與圖9 對比可知，NT-RRT*算法規劃出了一條更加安全的路徑，在此路徑中水下機器人離障礙物很遠，且規劃出的路徑幾乎都是直線，不像圖8 所示的彎曲較多。

Fig.10 NT-RRT* algorithm simulation圖10 NT-RRT*算法仿真

Fig.11 Final route formed by NT-RRT* algorithm simulation圖11 NT-RRT*算法仿真形成的最終路線

由表1 可知，在相同步長下，N-RRT*算法所用時間是RRT*算法所用的時間的1∕10，NT-RRT*算法所用時間是RRT*算法所用的時間的1∕20，說明加入了正態分布的采樣策略后提高了算法的收斂速度。N-RRT*算法路徑長度相比于RRT*算法、N-RRT*算法減少了一倍，節點數減少為原來算法的約1∕7，說明加入基于三角不等式的幾何修剪算法后減少了算法規劃的路徑長度和節點數。圖11 說明NT-RRT*算法在3 種算法中路徑光滑度最好，只有一個轉折點，更能滿足水下機器人在實際航行中的運動約束要求。

Table 1 Comparison of time and path length generated by the three algorithms under phase synchronization length表1 相同步長下3 種算法所用時間和形成的路徑長度比較

3 結語

本文從空間采樣策略和幾何修剪算法角度改進RRT*算法，解決RRT*算法在路徑規劃時收斂速度慢、占用內存大、規劃出的路徑曲折性較大等問題。首先介紹了在RRT*算法基礎上加入正態分布采樣策略的N-RRT*算法，解決RRT*收斂速度慢、內存占用大的問題；然后在NRRT*算法基礎上加入基于三角不等式的幾何修剪算法——NT-RRT*算法，減少隨機樹在擴展過程形成的節點數，從而加快算法收斂速度，降低路徑的彎曲度；最后在地形起伏、漂浮障礙物分散的水下環境中進行仿真。仿真結果表明，N-RRT*算法、NT-RRT*算法在收斂速度和路徑長度方面優于RRT*算法，并且NT-RRT*算法收斂速度最快，路徑規劃所用時間最短。其規劃出的路徑離障礙物較遠，安全性較高，路徑非常平滑。

但是此算法還有不足，如沒有加入AUV 在實際情況中所受的運動約束和動力約束影響因素，并且此算法的改進是基于先驗環境信息。后續將利用傳感器得到的信息構造動態環境模型，并將改進后的算法應用其中。