面向水下未知空間探測的改進RRT路徑搜索算法

張季然 陳德山 李廷文 呂潔印 汪 洋*

(武漢理工大學國家水運安全工程技術研究中心1) 武漢 430063) (武漢理工大學交通與物流工程學院2) 武漢 430063) (長江水上交通監測與應急處置中心3) 武漢 430014) (深圳中集智能科技有限公司4) 深圳 518057)

0 引 言

使用水下機器人探測水下未知空間,是輔助工程人員獲取水下未知空間特征的一種重要手段.在事前不掌握全局空間信息的情形下,水下機器人需要通過一定策略的巡航,以便了解水下空間的幾何形態.常用的水下機器人包括ROV(remote operated vehicle)和AUV(autonomous underwater vehicle),均可攜帶聲吶、光學攝像機、超短基線定位系統等設備用于水下探測和導航[1],操作人員能夠實時地控制水下機器人的運動.

相對于陸地上的路徑搜索,水下環境具有更高的空間維度,加大了水下探測復雜度.水下環境中攝像頭能見度低,探測范圍極其有限,無法使用深度攝像頭或激光雷達等光學傳感器.受水中雜質與波流影響,水下三維聲吶的成像會產生許多額外噪點,傳統的GPS定位系統在水中無法使用[2].

針對水下路徑規劃領域的研究,張汝波等[3]針對水下復雜環境中可能存在多個不同威脅等級的障礙物,提出了一種改進蟻群算法合理規劃水下機器人最優航行路徑.朱大奇等[4]提出了一種基于生物啟發的集成自組織映射算法,用于自主水下機器人(AUV)系統在三維水下避障環境下的任務分配和路徑規劃.Vidal等[5]提出一種新的自主水下機器人三維探測和覆蓋方法,并能夠引導機器人,在一次探索中獲得對占用數據和光學數據的完全發現和覆蓋.Shen等[6]提出了一種利用曲率重構的水下機器人在線規劃未知環境覆蓋路徑的算法.Hernández等[7]提出了一種在未知環境下AUV在線規劃無碰撞路徑的框架,該框架允許水下機器人逐步構建周圍環境地圖,并考慮機器人的運動約束來規劃可行的三維路徑.Li等[8]提出一種水下自主航行器最優路徑規劃方法進行海底地形匹配導航,以避開這些區域.

目前的研究大多數是在水下空間結構的先驗信息較充分的條件下進行,從二維路徑搜索逐漸發展至三維路徑搜索,而在水下空間未知的水域中進行路徑探索仍然涉及較少.為此,文中提出一種面向水下未知空間探測的混合RRT搜索算法.在缺乏環境先驗信息的水下空間中,通過水下機器人自身攜帶的三維聲吶系統獲得當前位置的環境信息,在搜索過程中按照局部搜索與全局搜索兩個尺度進行計算,提升水下未知空間中路徑搜索和規劃的效率,并通過真實環境實驗中測試了提出算法的性能.

1 模型感知及問題描述

1.1 聲吶數據模型

水下未知空間的路徑搜索依賴于水下機器人的自主探索和繪制[9].已知水下機器人的初始位置狀態與姿態信息,對整個水下未知空間進行路徑搜索,尋找出未知空間內部起始點與目標點之間滿足約束且無碰撞的可行路徑,并且盡可能高效完成作業任務.在水下機器人進行路徑搜索時必須考慮到水下復雜的空間環境:①水下場景通常具有很大的規模尺寸;②水下可能存在狹窄空間,機器人的可穿越空間由于這些空間從而受到限制;③機器人容易出現在局部環境中反復進行路徑搜索的問題,導致路徑搜索耗時長,效率低[10].

對采集環境信息的聲吶進行簡化,定義dmax為三維聲吶傳感器能探測到的最大有效范圍,掃測范圍為聲吶正前方-45°～45°,聲吶模型示意圖見圖1.

圖1 聲吶模型示意圖

滿足以下條件的目標可被實驗聲吶探測到.

(1)

式中:(x0,y0,z0)假設為水下機器人坐標;(x,y,z)為目標坐標;R為探測范圍,有0≤R≤dmax;θ為水平掃測半徑,φ為垂直掃測半徑,有-π/4≤θ≤π/4,0≤φ≤π/4.

1.2 問題描述

將水下三維未知空間內的搜索區域設置為Lx×Ly×Lz的長方體,對目標區域進行柵格化,將其均分為M×N×K的三維立方網格,工作空間為M.工作空間分別由自由空間Mfree、已占用空間Mocc和未知空間Mun構成.水下機器人經過的柵格區域為已占用空間Mocc,未經過的柵格區域為未知空間Mun,可通行區域為自由空間Mfree.在任務區域隨機設置Ni個靜態子目標點Gi,要求水下機器人在水下未知空間尋找到所有子目標點,并規劃出起始點與最終目標點之間的最優路徑.

水下機器人的初始位置設為Xinit,終點位置設為Xgoal.將水下的路徑搜索問題定義為在水下未知空間中,在起點至終點之間尋找一條可通行路徑σ:[0,T]→M,有σ(0)=Xinit,σ(T)=Xgoal,所有的子目標點σ(t)∈Gi(t),?t∈[0,T].水下機器人從起始點Xinit出發,通過三維聲吶傳感器以Xinit為圓心,R為半徑探測水下機器人周圍的未知空間Mun.最優路徑規劃問題被定義為對給定成本函數c的可行軌跡σ*的最小化搜索,通過子目標點Gi(t)將Xinit至Xgoal聯系起來.

(2)

為解決上述問題,按照全局路徑和局部路徑的最優試探開展同步計算.在局部路徑計算層面基于周邊dmax范圍探測數據,結合前沿點信息進行小尺度路徑搜索尋找子目標點σ(t)∈Gi(t);而在全局路徑計算層面進行粗粒度的路徑分支決策,并將已選分支的邊緣信號反饋給局部路徑的計算,實現水下未知空間的路徑探索.

2 RRT路徑搜索算法

2.1 RRT算法基本原理

快速擴展隨機樹(rapidly exploring random tree)算法在陌生高維環境中能快速的導向空白區域,快速的找出一條可通行的路徑.RRT算法的路徑搜索過程類似于樹枝的生長、擴散過程[11].在未知空間內進行探索時,以狀態空間中的一個初始點作為根節點,通過隨機采樣增加葉節點的方式,生成一個快速擴展隨機樹.

在水下機器人進行環境探測時,以起始點Xstart作為根節點,通過在工作空間M中隨機采樣得到隨機點Xrand,然后從當前的隨機樹中尋找與Xrand點最近的葉節點Xnearest點,若Xnearest點和Xrand點之間無碰撞,則將Xrand增加為新的葉Xnew,如此循環,將搜索導向未知空白區域,直到葉節點中包含目標點Xgoal或者在目標點所在的區域內,則完成搜索,生成隨機搜索樹.RRT算法的擴展過程見圖2.

圖2 RRT算法的擴展過程

2.2 RRT*算法

RRT*算法是在基本RRT算法上進行改進的,與基本RRT算法相比,RRT*算法在RRT基礎上,改進了原有的節點選擇函數,新增了父節點優化的過程[12].該算法是漸近優化的,隨著迭代次數的增加,使得路徑規劃的結果能在一定程度上收斂至當前隨機樹下的最優解[13].RRT*算法和重布線過程見圖3～4.

圖3 RRT*算法示意圖

圖4 重布線過程

3 水下實時路徑探索算法設計

為提高路線搜索效率,水下機器人需要滿足以下要求:①提高水下機器人對整體環境的了解程度;②減少水下機器人對區域重復搜索;③減少水下機器人反向搜索.綜合以上三方面因素,提出了一種混合RRT搜索算法,具體思路為:將探索路徑規劃問題重新劃分為兩個分叉階段,即負責在當前機器人姿態周圍的空間內進行高效探索的局部搜索階段,利用RRT*算法對局部探測區域進行路徑規劃;當機器人局部探索完成之后,將機器人重新定位回全局空間內部的全局搜索階段,利用RRT算法進行全局搜索并作為局部搜索階段的補充,將全局探測前沿點信息傳遞給局部搜索階段,更好的對局部區域進行路徑規劃,全局搜索階段中記錄了機器人當前時刻在水下的位置信息以及已探索空間的區域信息,避免機器人陷入反復進行局部探索的處境中.在探測過程中,將局部探測水下空間感知信息按時間順序逐步添加至全局水下空間感知信息中.

3.1 混合RRT搜索算法

混合RRT搜索算法是基于RRT路徑規劃算法所改進的搜索算法,基本RRT算法對于未知區域有著強烈的傾向.在混合RRT搜索算法中,通過三維聲吶獲取水下空間感知信息,獲取環境邊界區域Mb.在全局路徑計算層面,利用RRT算法進行粗粒度的路徑分支決策,并將已選分支的邊緣信號反饋給局部路徑的計算,再通過RRT*算法進行微局部路徑搜索并更新水下空間感知信息,實現水下空間路徑搜索.混合RRT搜索算法的框架圖見圖5.

3.2 局部搜索算法

局部搜索算法是將傳感器可探測環境作為當前窗口,在該窗口內進行路徑規劃,在其中搜尋最優子目標點Xtemp,探測起始位置到子目標點的之間的可行區域,規劃出當前窗口下的局部路徑,并將所選取的最優子目標點更新為下一窗口的起始點[14].三種不同情形子目標點的選取策略見圖6.

圖6 三種不同情形子目標點的選取策略

局部搜索算法的探測流程如下:初始化起始點Xinit,水下機器人搭載的三維聲吶傳感器的掃描半徑為r,掃描區域為0°～90°,見圖6a).局部搜索算法生成擴展樹的節點在RRT中通過樹的分支(邊)相互連接,樹上的每一個點都是一個頂點,儲存在頂點集合V中.RRT中樹的一個分支為一條邊,每條邊根據兩個連通點的空間坐標存儲在邊集合E中,頂點集合與邊集合共同組成了圖像G=(V,E).在每一次迭代中隨機在初始點Xinit周圍取一個點作為采樣點,記為Xrand,通過NearestVertex公式尋找到樹中距離當前采樣點Xrand最近的節點,記為Xnearest,NearestVertex公式定義為

NearestVertex(x)=argmin‖x-v‖

(3)

式中:x為環境內自由空間內的任意一點,v∈V.以Xnearest節點為中心,η為生長半徑Xrand方向進行生長,在半徑另一端生成的節點記為Xnew.檢驗Xnew節點是否位于未知區域內或者Xnew與以Xnearest之間的線段上的任意一點是否位于未知區域內.若上述任一條件成立,則認為Xnew節點為局部探測的邊界點.如果Xnew或Xnew與Xnearest之間的點都在已知區域內,則此次迭代尚未找到未知空間邊界點,把Xnew作為一個頂點加入到頂點集合中,將他們之間的連接線作為樹枝加入到邊集合中,通過不斷迭代去對未知空間進行路徑搜索.考慮局部搜索算法是在微局部區域進行路徑搜索,單個區域最大迭代次數上限設置為200次.

3.3 全局搜索算法

全局搜索算法在整個路徑搜索期間(直到水下空間感知信息被完全探索完畢)樹的起始點不會重置并一直保持增長.全局搜索算法是局部搜索算法的補充,局部搜索算法只能在機器人當前掃描的區域進行探測,在遠離機器人的位置探測性能不佳,可能會錯失探索水下空間感知信息上的小角落,此時通過全局搜索算法可確保在遠離機器人當前位置的未知區域可以被探測.

RRT算法擴展方式較為平均,按照隨機采樣的方式進行新節點的采樣和生成.在反饋子目標點給局部搜索算法的過程中,為了減少局部搜索過程中子目標點選取的隨機性,采用啟發式搜索策略,使得子目標點的選取具有一定趨勢偏向于優良選取結果,從而提高路徑搜索效率.對于機器人掃測范圍內任意子目標點x,有估計函數:

f(x)=p(x)+q(x)

(4)

式中:f(x)為機器人起始點到子目標點距離的估計函數;p(x)為狀態空間中從起始點到掃測邊界點距離的實際代價值;q(x)為狀態空間中從起始點到子目標點距離的最佳代價估計值.在選擇子目標點的過程中,選取多個子目標點作為候選點,并分別計算候選子目標點Xtemp到起始點的代價估計值.由于在機器人掃測范圍內,p(x)即聲吶掃測半徑r,因此比較不同的候選點的估計函數f(xtemp),即比較q(xtemp).

q(xtemp)=w1·D(xstart,xtemp)-w2·D(hstart,htemp)

(5)

式中:D(xstart,xtemp)為起始點與子目標點間的歐式距離;D(hstart,htemp)為起始點與子目標點在Z軸方向上的距離;w1與w2分別為兩個距離的權重系數,為保證機器人行駛路線在水平方向上的平緩性,w1取值0.7,w2取值0.3.則估計函數為.

(6)

當D(xstart,xtemp)>r時,將此子目標點Xtemp從候選點中排除.

混合RRT搜索算法通過局部搜索與全局搜索相結合,其偽代碼見表1.

表1 混合RRT搜索算法偽代碼

4 計算仿真與實物場景測試

4.1 仿真分析

設計了混合RRT搜索算法與RRT算法的對比仿真實驗,水下未知空間環境見圖7.任務空間大小為600 m×600 m×300 m,起始點坐標為(50,50,150),搜索目標點坐標為(500,500,150),隨機樹的擴展步長為5 m,聲吶探測最大半徑設為80 m.起點為圖7左側圓圈,搜索目標點為圖9右側圓圈.水下障礙物模型用五個圓柱體進行代替,將水下機器人與搜索目標點視為質點,為了防止實際環境下水下機器人與障礙物進行碰撞,將水下機器人與障礙物的最短距離設為4 m.

圖7 水下未知空間

混合RRT搜索算法搜索過程見圖8.

圖8 混合RRT搜索算法搜索圖

由圖8可知:細線為全局搜索算法生成的擴展樹,黑色粗線為局部路徑搜索算法生成的擴展樹,中間線段為水下機器人實際行走的路線,見圖9.

圖9 水下機器人航行路線

混合RRT搜索算法搜索過程的俯視圖與水下機器人航行路線的俯視圖分別見圖10～11.

圖10 混合RRT搜索算法搜索過程俯視圖

圖11 水下機器人航行路線俯視圖

圖12為RRT算法規劃過程的三維視角與二維俯視圖視角,采用相同的起點與終點坐標,灰色線段為最終路徑.

圖12 RRT算法路線圖

圖13為RRT*算法規劃過程的三維視角與二維俯視圖視角,黑色線段為搜索過程,灰色線段為最終路徑.

圖13 RRT*算法路線圖

將RRT算法、RRT*算法與混合RRT搜索算法的路徑長度、尋路時間、迭代次數以及有效頂點個數等數據進行30次實驗對比,見圖14.由于RRT*算法為漸進最優,所以將其迭代次數統一設置為與RRT算法迭代次數平均值相近次數500次.

圖14 各參數對比圖

表2為30次仿真實驗中各算法路徑長度、尋路時間、迭代次數與有效頂點個數的平均值.由各對比圖與數據表可知:在各項指標對比上,混合RRT搜索算法皆有良好的表現.與RRT算法相比,路徑長度降低了16.4%,尋路時間縮短了68.5%,迭代次數降低了84.1%,頂點個數減少了90.8%;與RRT*算法相比,路徑長度降低了11.4%,尋路時間縮短了19.9%,迭代次數降低了84.1%,頂點個數減少了72.4%.由此可見:混合RRT搜索算法在路徑探索上具有明顯優勢,生成路徑最短且符合航行器運動學模型,可應用于水下未知空間的航行器路徑探索.

表2 仿真結果對比圖

4.2 實驗驗證

實驗所用數據由美國某公司研制的BV5000型號[15]三維掃描聲吶系統采集獲取.該系統掃描區間可分為垂直面以及水平面,其中在系統垂直面為單獨掃描扇形區域,系統發射256條波束,每條波束之間的角度間隔為0.18°.在完成依次垂直面掃描后,通過云臺水平面旋轉,即可對三維空間進行掃描并構建,BV5000聲吶系統的水平面探測方向可達360°,因此其探測范圍為一定空間范圍內的柱狀區域.BV5000波束頻率為1.35 MHz,探測空間范圍最大為30 m,點云圖精度最小可達1.2 cm.由于聲波在水下長距離傳播的特性,該系統可在渾濁水域作業.水下機器人無水下環境的先驗信息,通過搭載的BV5000聲吶不斷獲取水下環境信息,實驗水池大小為25 m×25 m×5 m,所有障礙物皆為靜態障礙物,包括水下管道、模擬艙室與翻扣沉船.

設置水下機器人的起始位置(-1,2.5,0),設立搜索目標點位置(13.98,2.24,0.94),根據搭載的BV5000聲吶完成水下未知空間探測實驗.使用混合RRT搜索算法的局部搜索探測路徑圖與路徑搜索圖見圖15,灰色線段為水下機器人探測路線.

圖15 局部路徑搜索圖

全局空間的路徑搜索見圖16,水下機器人在實驗水池內完成了從起點到搜索目標的之間的空間探測與路徑搜索,實驗結果證明,混合RRT搜索算法在水下機器人的路徑規劃上具有可行性,并能根據實際環境信息規劃出無碰撞路徑,使水下機器人能夠快速高效的到達搜索目標點.

圖16 全局路線搜索圖與全局路線圖

5 結 束 語

文中提出一種基于水下未知空間探測的混合RRT搜索算法,利用RRT算法快速性的特點及其趨于未知空間延展的特性對水下未知空間進行路徑搜索.將全局路徑規劃細分為一步步的局部路徑規劃,利用局部搜索階段與全局搜索階段相結合,將所提混合RRT搜索算法應用到水下未知空間.最后通過仿真實驗與真實環境實驗驗證了算法的有效性,后續研究將在實際水域內進行驗證與優化.