李娟，張韻，陳濤

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

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

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

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

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

1 模型及感知地圖建立

1.1 問題描述

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

1.2 目標(biāo)模型

靜態(tài)目標(biāo)任意時刻都保持不變，故靜態(tài)目標(biāo)的模型可以描述為

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

1.3 聲吶模型

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

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

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

1.4 目標(biāo)存在概率地圖

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

AUV 的目標(biāo)存在概率地圖可以定義為

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

k+1 時刻網(wǎng)格a的目標(biāo)存在概率更新公式如下：

其中式（4）表示k時刻網(wǎng)格a存在目標(biāo)的情況；式（5）表示k時刻網(wǎng)格a不存在目標(biāo)的情況。式中：Pd是檢測概率，表示傳感器在真實存在目標(biāo)的網(wǎng)格中發(fā)現(xiàn)目標(biāo)的概率；Pf是虛警概率，表示傳感器在不存在真實目標(biāo)的網(wǎng)格中檢測到目標(biāo)的概率。

為方便計算，本文通過非線性變換將目標(biāo)存在概率公式轉(zhuǎn)變?yōu)榫€性形式：

1.5 不確定度地圖

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

具體計算公式：

式中：μa,k代表k時刻網(wǎng)格a的不確定度；Qa,k代表k時刻網(wǎng)格a的目標(biāo)存在概率。

1.6 區(qū)域遍歷度地圖

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

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

2 搜索決策函數(shù)

為提高搜索效率，AUV 目標(biāo)搜索需滿足以下要求：

1）提高AUV 對整體環(huán)境的了解程度；

2）減少AUV 對任務(wù)區(qū)域的重復(fù)搜索；

3）減少AUV 的反向搜索。

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

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

重復(fù)搜索收益IB（a,k）為在k?1 時刻，網(wǎng)格a傳感器范圍內(nèi)的遍歷度和。遍歷度越高的區(qū)域，說明該區(qū)域大體搜索情況已知，應(yīng)減少對該區(qū)域的重復(fù)搜索。

轉(zhuǎn)向代價IC（a,k）是取決于AUV 下一步動作執(zhí)行時的航向與當(dāng)前的航向是否相同。

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

式中：w1、w2、w3分別為不確定度、重復(fù)搜索、轉(zhuǎn)向收益三部分的權(quán)重系數(shù)，根據(jù)整個搜索狀態(tài)的變化進行調(diào)整。

3 三維空間下目標(biāo)搜索算法

3.1 搜索算法流程

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

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

3.2 基本的三維RRT 算法

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

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

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

RRT 算法的偽代碼如下：

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

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

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

3.3 改進的三維RRT 算法

3.3.1 滾動規(guī)劃

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

本文滾動窗口以探測設(shè)備模型為基礎(chǔ)建立，將探測環(huán)境窗口定義為滾動窗口，滾動窗口模型如圖2 所示。

3.3.2 子目標(biāo)點選取策略

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

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

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

圖3 目標(biāo)在窗口內(nèi)Fig.3 Target in window

3.3.3 反向節(jié)點裁剪規(guī)則

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

圖4 窗口內(nèi)不存在障礙物Fig.4 No obstacles in the window

圖5 窗口內(nèi)存在障礙物Fig.5 Obstacles in the window

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

4 仿真結(jié)果及分析

4.1 改進滾動RRT 算法

為驗證引入滾動思想的三維RRT 算法的有效性，設(shè)計了 RRT 算法與滾動 RRT 算法的對比仿真實驗。仿真環(huán)境800 m×800 m×800 m，AUV的探測半徑為 100 m，起始點坐標(biāo)為（0,0,0），目標(biāo)點坐標(biāo)為（750,750,750），隨機樹的擴展步長為80。其中隨機節(jié)點以及擴展樹枝均為藍色，起點用紅色標(biāo)記，目標(biāo)點以及子目標(biāo)點用綠色表示?；疑该髑蝮w分別表示滾動窗口以及障礙物。如圖8 為 RRT 算法規(guī)劃的過程，紅色實線為最終路徑。圖9 和圖10 分別為改進滾動 RRT 算法規(guī)劃過程的兩個視角，紅色實線為最終路徑。

圖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 組數(shù)據(jù)，對比兩種算法的路徑規(guī)劃結(jié)果，從表1 可以看到相對于傳統(tǒng)RRT，引入滾動思想的RRT 算法通過限制節(jié)點的選取范圍以及樹枝的長度大大減少了節(jié)點數(shù)量，縮短了規(guī)劃時間，路徑規(guī)劃效率有了很大的提升。

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

4.2 目標(biāo)搜索仿真案例

本節(jié)仿真設(shè)置搜索區(qū)域?qū)嶋H大小為800 m×800 m×200 m，聲吶傳感器的探測半徑R為 100 m，檢測概率Pd=0.9，虛警概率Pf=0.1。將搜索區(qū)域柵格化。仿真過程中將按障礙物的最大輪廓線膨化為球形，從而實現(xiàn)最大程度避碰，并將 AUV 和目標(biāo)看作質(zhì)點。

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

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

圖11 初始狀態(tài)Fig.11 Initial state

圖12 RRT 目標(biāo)搜索算法Fig.12 Target search based on RRT algorithm

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

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

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

5 結(jié)束語

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