改進RRT 算法在未知三維環境下AUV 目標搜索中的應用

李娟，張韻，陳濤

（1.哈爾濱工程大學 海洋裝置與控制技術研究所，黑龍江 哈爾濱 150001;2.哈爾濱工程大學 智能科學與工程學院，黑龍江 哈爾濱 150001）

自主水下航行器（autonomous underwater vehicle，AUV）目標搜索是水下搜救、水下圍捕等作業任務的重要組成部分。在未知的水下環境中，AUV 無法預先獲得目標和障礙物的信息。為了更好地完成水下任務，AUV 需具備自適應搜索與決策以及局部規劃能力。

在搜索方面，Ma 等[1]提出一種針對未知環境目標搜索的組合選項和強化學習算法，該算法通過在未知環境下的強化學習來處理實時任務，提高了動態特性。Dadgar 等[2]為克服機器人工作空間的局限性，提出一種基于PSO 的分布式算法，使基于PSO 算法在全局機制下達到整體最優的目的。李娟等[3]針對水下搜索效率低，定位精度不穩定等問題，提出了一種基于動態預測的多AUV目標搜索方法，并將該方法分成了3 種模式，最終通過仿真驗證了其具有搜索高效性。倪建軍等[4]針對未知的三維水下環境中的多AUV 協同目標搜索問題，提出了一種改進的基于海豚群算法的方法，將搜索問題分為隨機巡航、動態聯盟和團隊搜索3個階段。綜合已有研究成果，發現各個搜索算法都存在不足。文獻[1]提出的學習算法很難適應完全未知環境，不能保證搜索的實時性和高效性；文獻[2-3]所提出的算法不能很好地應用在三維環境中；文獻[4]雖然提出了對三維環境的搜索方法，但整體搜索時間過長；文獻[5]提出了水面航行器與水下航行器合作的目標搜索與打擊方法，作者提出的方法更側重在兩種不同設備的合作搜索，針對搜索方法的創新沒有詳細描述；文獻[6-7]分別從仿生算法角度對全局的路徑規劃提出優化方法。

與傳統的路徑規劃方法相比，RRT (rapid-exploration random tree)算法結構簡單，具有隨機性、環境建模簡單等優勢。文獻[8-9]將雙向快速隨機搜索樹算法與粒子群算法相結合，結合粒子群算法后，利用雙向RRT 算法得到的路徑坐標作為粒子群算法的初始粒子，以不與障礙物發生碰撞為約束條件，對初始粒子進行優化，將處理后的路徑作為最終路徑，不僅具備了對未知空間的搜索能力且得到了較為平滑的路徑。但基于此類算法研究，大都局限于二維平面內。

本文實現的目標是：在未知的三維環境中，保證AUV 以可能短的時間搜索到盡可能多的目標。搜索過程中，對RRT 算法進行改進，并結合滾動規劃算法應用到三維目標搜索過程中。一方面，為提高搜索效率，建立搜索實時地圖以及決策函數；另一方面，為解決RRT 算法固有的盲目搜索以及不適用未知環境問題,提出反向節點裁剪規則同滾動規劃相結合的算法。

1 模型及感知地圖建立

1.1 問題描述

將三維任務搜索區域膨化為Lx×Ly×Lz的長方體。對目標區域進行柵格化，將其均分為M×N×K個大小為100 m×100 m×100 m 的三維實際網格。將AUV 的工作空間記為Wn，其中n為空間維數，把AUV 已經過的柵格記為1，未經過的柵格記為0。在任務區域隨機分布著Ni個靜態目標、數量隨機的球形障礙物，要求AUV 在規定時間內，以較短時間找到區域中的所有目標。

1.2 目標模型

靜態目標任意時刻都保持不變，故靜態目標的模型可以描述為

式中：x(k)表示k時刻靜態目標橫坐標值，x(k+1)表示k+1時刻靜態目標橫坐標值；y(k)表示k時刻靜態目標縱坐標值，y(k+1)表示k+1時刻靜態目標縱坐標值；z(k)表示k時刻靜態目標豎坐標值，z(k+1)表示k+1時刻靜態目標豎坐標值。

1.3 聲吶模型

本文引入文獻[10]中的聲吶模型，采用英國Tritech 公司的Micron DST 緊湊型數字成像聲吶用于地圖構建和信息探測。它能通過不斷旋轉換能器，發送一個狹窄的扇形聲束來掃描周圍的環境。該聲吶的工作頻率為700 kHz，水平波束角和垂直波束角分別為30°和3°，能夠實現一個完整的360°扇區掃描，最大范圍可達100 m。

在本文中，對聲吶進行簡化。滿足以下條件的目標可被AUV 聲吶探測到：

式中：（x0,y0,z0）假設為AUV 的坐標；（x,y,z）是目標坐標；R為探測距離。

1.4 目標存在概率地圖

目標存在概率地圖[11]表示目標在實際網格圖中存在的概率情況。由于AUV 在未知環境中進行搜索，因此在搜索開始之前，AUV 對整個任務區域內的目標位置、障礙物位置等信息一無所知，即Pa(0)=0.5。

AUV 的目標存在概率地圖可以定義為

式中：Pa(k)表示k時刻網格a中實際存在目標的概率，0≤Pa(k)≤1。Pa(k)=1表示k時刻AUV 認為網格a中存在目標、Pa(k)=0表示在k時刻AUV認為網格a中不存在目標；Ω是全局環境。

k+1 時刻網格a的目標存在概率更新公式如下：

其中式（4）表示k時刻網格a存在目標的情況；式（5）表示k時刻網格a不存在目標的情況。式中：Pd是檢測概率，表示傳感器在真實存在目標的網格中發現目標的概率；Pf是虛警概率，表示傳感器在不存在真實目標的網格中檢測到目標的概率。

為方便計算，本文通過非線性變換將目標存在概率公式轉變為線性形式：

1.5 不確定度地圖

不確定度地圖[11]反映AUV 對目標搜索區域中網格的了解程度。網格的不確定度與目標存在概率相關。當網格的目標存在概率為0.5 時，不確定度為1，表示AUV 對網格a的目標存在情況完全不了解。網格的目標存在概率與 0.5 相差越多，網格的不確定度越低，隨著AUV 對網格進行探訪，該網格的目標存在概率會增加或減少，AUV 對該網格中是否存在目標的判斷也會隨著探訪次數的增加而越來越準確。

具體計算公式：

式中：μa,k代表k時刻網格a的不確定度；Qa,k代表k時刻網格a的目標存在概率。

1.6 區域遍歷度地圖

為了了解整體搜索狀況，避免部分區域重復搜索，建立遍歷地圖。k時刻網格a遍歷狀態可用Ba(k)表示：

式中：d(k)為當前AUV 距離預測決策點所在區域中心的距離；r表示區域的半徑。

2 搜索決策函數

為提高搜索效率，AUV 目標搜索需滿足以下要求：

1）提高AUV 對整體環境的了解程度；

2）減少AUV 對任務區域的重復搜索；

3）減少AUV 的反向搜索。

綜合以上3個方面，設計了搜索決策函數，此描述了AUV 執行搜索后的綜合收益。該函數綜合考慮了不確定度收益IA（a,k）、重復搜索收益IB（a,k）以及轉向收益IC（a,k）。AUV 通過搜索決策函數對 AUV 在下一時刻的位置進行搜索收益評估，選取綜合收益最大的網格作為下一步的搜索。

不確定度收益IA（a,k）表示在k?1 時刻，以網格a為中心，傳感器探測范圍為半徑的探測區域內的所有網格的不確定度的和。

重復搜索收益IB（a,k）為在k?1 時刻，網格a傳感器范圍內的遍歷度和。遍歷度越高的區域，說明該區域大體搜索情況已知，應減少對該區域的重復搜索。

轉向代價IC（a,k）是取決于AUV 下一步動作執行時的航向與當前的航向是否相同。

假設 AUV 在k?1 時刻處于網格a中，則AUV 在k時刻可以向網格a的相鄰網格移動。假設AUV 在k時刻計劃從網格a前往網格b(網格b是AUV 目前所在網格a的相鄰網格)，則搜索性能收益函數的具體公式如式(9)所示：

式中：w1、w2、w3分別為不確定度、重復搜索、轉向收益三部分的權重系數，根據整個搜索狀態的變化進行調整。

3 三維空間下目標搜索算法

3.1 搜索算法流程

在目標搜索過程中，感知地圖通過聲吶探測情況進行更新，并根據決策函數求出下一時刻搜索區域，最后利用改進滾動RRT 算法對搜索路徑進行規劃，決策及規劃過程反復進行，直到發現所有目標。搜索算法偽代碼如下：

各函數解釋如下：Target probability()為目標存在概率更新；Regional uncertainty()為區域不確定度更新；Regional traversal()為區域遍歷度更新；Decision function()為目標決策函數，決策出搜索點；RRT algorithm()為規劃局部路徑；All_goal()為判斷是否所有目標均被找到。

3.2 基本的三維RRT 算法

三維環境下隨機樹擴展過程如圖1 所示。在二維的基礎上，增加了Z平面的搜索，增加了搜索維度和計算的復雜度。

圖1 三維RRT 擴展過程Fig.1 3D RRT extend process

設O是AUV 所處的水下狀態空間，O∈Wn。選擇AUV 的初始位置作為隨機樹的根節點p_init，根節點作為父節點在空間中隨機采樣得到p_rand，利用歐式距離在隨機樹上找距離p_rand 最近的節點p_near，然后p_near 以r步長向p_rand 擴展得到新節點p_new，若p_near 與p_new 的連線通過了避碰檢測，則將新節點p_new 以及連線加入到隨機樹上。對上述過程重復進行，直到發現目標點。將隨機樹上的從根節點到目標點之間的樹節點連接即為所規劃的路徑。

RRT 算法的偽代碼如下：

各函數解釋如下：Random()為區域內隨機選取一節點p_rand；Nearest()為在隨機樹上尋找距離p_rand 最近的節點p_near；Extend() 為從p_near 向p_rand 擴展步長r，得到新節點p_new。擴展關系式如下：

Collision_free()為若p_new 與p_near 之間存在障礙物，重新選取節點。

從隨機樹的生長過程來看，一方面節點的選取具有盲目性，造成時間的延長，搜索效率降低；另一方面，RRT 算法通常作為全局規劃使用，不能直接應用到未知環境。為減少未知環境下的搜索時間以及提高搜索效率，本文將對以上不足做出改進。

3.3 改進的三維RRT 算法

3.3.1 滾動規劃

可以利用滾動規劃解決三維水下環境復雜多變，很難獲取完整的全局信息的問題。滾動規劃主要分為預測、優化、反饋3 步。滾動過程中，以探測到的環境信息為基礎，建立以AUV 為中心的局部環境模型；將環境模型看作規劃窗口，根據優化決策得到下一步的最優子目標點；在前往子目標點的過程中，根據探測到的新的環境信息，補充校正原來的模型滾動后的局部路徑規劃[10]。

本文滾動窗口以探測設備模型為基礎建立，將探測環境窗口定義為滾動窗口，滾動窗口模型如圖2 所示。

3.3.2 子目標點選取策略

記錄t時刻航行器的所處位置為p_init(t)，將航行器探測設備能夠探測到的環境窗口定義為當前滾動規劃窗口[12]。

不同情形下的子目標選取策略不同，分別對以下3 種情形進行討論：1）目標在窗口內；2）窗口內不存在障礙物；3）窗口內存在障礙物。分別如圖3～5。

分別根據情況采用式（11）～（13）確定子目標點p_temp(t)[13]，圖3～5 分別對應式（11）～（13）。

圖3 目標在窗口內Fig.3 Target in window

3.3.3 反向節點裁剪規則

為降低p_rand 的隨機性，引入反向節點裁剪規則，即控制(p_init,p_new) 向量同(p_init,p_goal)向量間夾角為銳角，避免反向選取節點。三維環境下，為控制向量夾角，分別從聲吶水平探測開角和縱深探測開角考慮，故將向量分別投影到xoy軸和yoz軸，得到投影角μ1和μ2，如圖6和圖7 所示。

圖4 窗口內不存在障礙物Fig.4 No obstacles in the window

圖5 窗口內存在障礙物Fig.5 Obstacles in the window

圖7 YOZ 平面投影Fig.7 Projected on theYOZplane

4 仿真結果及分析

4.1 改進滾動RRT 算法

為驗證引入滾動思想的三維RRT 算法的有效性，設計了 RRT 算法與滾動 RRT 算法的對比仿真實驗。仿真環境800 m×800 m×800 m，AUV的探測半徑為 100 m，起始點坐標為（0,0,0），目標點坐標為（750,750,750），隨機樹的擴展步長為80。其中隨機節點以及擴展樹枝均為藍色，起點用紅色標記，目標點以及子目標點用綠色表示。灰色透明球體分別表示滾動窗口以及障礙物。如圖8 為 RRT 算法規劃的過程，紅色實線為最終路徑。圖9 和圖10 分別為改進滾動 RRT 算法規劃過程的兩個視角，紅色實線為最終路徑。

圖8 RRT 算法Fig.8 RRT algorithm

圖9 改進滾動RRT 算法（角度1）Fig.9 Improved rolling RRT algorithm(point 1)

圖10 改進滾動RRT 算法（角度2）Fig.10 Improved rolling RRT algorithm(point 2)

選取20 次實驗中具有代表性的5 組數據，對比兩種算法的路徑規劃結果，從表1 可以看到相對于傳統RRT，引入滾動思想的RRT 算法通過限制節點的選取范圍以及樹枝的長度大大減少了節點數量，縮短了規劃時間，路徑規劃效率有了很大的提升。

表1 RRT 同改進滾動RRT 對比Table1 Comparison of RRT and Improved rolling RRT

4.2 目標搜索仿真案例

本節仿真設置搜索區域實際大小為800 m×800 m×200 m，聲吶傳感器的探測半徑R為 100 m，檢測概率Pd=0.9，虛警概率Pf=0.1。將搜索區域柵格化。仿真過程中將按障礙物的最大輪廓線膨化為球形，從而實現最大程度避碰，并將 AUV 和目標看作質點。

在 AUV 航行過程中，AUV 航速設置為2 m/s。本文采用步數為算法計量單位，考慮 AUV 的速度，設定 AUV 每航行2 m 為一步，每步所用時間為1 s。

AUV 的初始坐標為(0,0,0)，設置靜目標11個，目標用五角星表示，球體障礙物7個，用黑色球體表示，如圖11 為搜索的初始狀態。紅色實線代表AUV 的實時搜索路徑。如圖12 為RRT 算法目標搜索算法，紅色實線為搜索路徑。圖13 為改進滾動 RRT 目標搜索算法，紅色實線為搜索路徑。

圖11 初始狀態Fig.11 Initial state

圖12 RRT 目標搜索算法Fig.12 Target search based on RRT algorithm

圖13 改進滾動RRT 目標搜索算法Fig.13 Target search based on improved rolling RRT algorithm

因為搜索決策函數一致，從圖12 和圖13 中看出，RRT 目標搜索算法與滾動RRT 搜索算法的搜索路徑大致相同，但從表2 中可以看出，應用滾動RRT 算法的搜索時間明顯少于RRT 算法，搜索速度有了很大的提升。

表2 RRT 搜索同改進滾動RRT 搜索對比Table2 Comparison of RRT and improved rolling RRT

5 結束語

本文提出基于滾動RRT 算法的三維目標搜索算法，算法從決策點規劃角度入手，利用RRT算法快速性的特點提高整體環境的搜索速度。滾動規劃結合子目標點選取策略，將全局路徑規劃細分為一步步的局部路徑規劃，使得 RRT 算法應用到未知環境；其次，利用搜索決策函數求出決策點后，應用改進滾動RRT 算法實時規劃路徑；最后通過仿真驗證了算法的高效性。另外，多次仿真過程中，出現了AUV 對同一區域多次搜索并陷入當前搜索環境的情況，避免區域重復搜索方面并沒有很好的實現；在決策點路徑規劃時，多次出現規劃時間過長的情況；實際海底環境存在非常多的障礙物或是形狀復雜的障礙物，單純利用文章中提出的改進算法，可能會導致失敗的情況發生，因此要通過實際的水下試驗來驗證以及優化；針對這三類問題仍有必要繼續深入研究。