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

師穎慧，張 冰，趙 強

（江蘇科技大學電子信息學院，江蘇鎮(zhèn)江 212003）

0 引言

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

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

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

1 改進的RRT*算法

1.1 環(huán)境建模

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

1.2 正態(tài)分布空間采樣策略

假設(shè)環(huán)境空間為D，Dfree為環(huán)境空間中的無障礙區(qū)域，D為環(huán)境空間的障礙區(qū)域，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 的二維正態(tài)分布圖像

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

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

N-RRT*算法步驟如下：

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

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

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

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

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

2 改進的NT-RRT*算法在海底環(huán)境下的仿真

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

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

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

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*算法規(guī)劃出的路徑很平滑，唯一的一處拐角也離障礙物很遠，水下機器人在實際操作中會更加簡單。由圖11 與圖9 對比可知，NT-RRT*算法規(guī)劃出了一條更加安全的路徑，在此路徑中水下機器人離障礙物很遠，且規(guī)劃出的路徑幾乎都是直線，不像圖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，說明加入了正態(tài)分布的采樣策略后提高了算法的收斂速度。N-RRT*算法路徑長度相比于RRT*算法、N-RRT*算法減少了一倍，節(jié)點數(shù)減少為原來算法的約1∕7，說明加入基于三角不等式的幾何修剪算法后減少了算法規(guī)劃的路徑長度和節(jié)點數(shù)。圖11 說明NT-RRT*算法在3 種算法中路徑光滑度最好，只有一個轉(zhuǎn)折點，更能滿足水下機器人在實際航行中的運動約束要求。

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

3 結(jié)語

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

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