歐陽子路，王鴻東*，王檢耀，易宏

1 上海交通大學(xué)海洋工程國家重點實驗室，上海200240 2 上海交通大學(xué)海洋智能裝備與系統(tǒng)教育部重點實驗室，上海200240

0 引 言

無人水面艇（USV）具有自主程度高、航速快、隱身性能好及機動性強等特點，被廣泛應(yīng)用在水文探測、海事巡航以及軍事作戰(zhàn)等領(lǐng)域。近年來，隨著人工智能技術(shù)的再次興起及自動駕駛技術(shù)的大規(guī)模應(yīng)用，USV 作為智能技術(shù)與傳統(tǒng)船舶學(xué)科交叉融合的產(chǎn)物，已成為國內(nèi)外智能裝備的研究熱點之一。

自動避碰是USV 實現(xiàn)智能航行的關(guān)鍵技術(shù)之一，一定程度上反映了USV智能化水平的高低［1-2］。國內(nèi)外許多學(xué)者在USV 避碰方面也取得了一定的研究成果。Svec 等［3］提出了3 種考慮風(fēng)浪對USV艇體造成影響的規(guī)避障礙物方法，分別為：考慮障礙物邊界進行路徑規(guī)劃；考慮外界環(huán)境的影響修正規(guī)劃路徑并再規(guī)劃；將局部邊界最優(yōu)規(guī)劃與啟發(fā)式A*算法相結(jié)合并通過博弈樹搜索位置。Kuwata 等［4］采用速度障礙法并結(jié)合國際海上避碰規(guī)則設(shè)計了USV 避碰方法，該方法在避碰過程中根據(jù)USV 操縱運動特性設(shè)置了最小轉(zhuǎn)向角并考慮風(fēng)浪流的影響。Benjamin 等［5］將行為控制框架原理、間隔規(guī)劃等多重思想融入多目標(biāo)區(qū)間優(yōu)化方法中來解決USV 避碰問題。莊佳園等［6］考慮國際海上航行規(guī)則，將USV 碰撞分為追越、交叉相遇與正面相遇3 種局面，設(shè)計了一種符合國際海上航行規(guī)則的相對坐標(biāo)系下動態(tài)規(guī)避方法，并搭建了USV 半實物仿真平臺。吳博等［7］通過分析USV 與障礙物之間的相對速度和相對位置的角度關(guān)系，提出了一種基于速度障礙法并考慮風(fēng)浪流影響的USV 操縱運動特性的自主避碰算法。張洋洋等［8］將速度障礙法和動態(tài)窗口法相結(jié)合，提出了一種無人水面艇動態(tài)避障算法。

從上述國內(nèi)外研究情況來看，解決USV 避碰問題的主要思路可分為兩類：一類是將其轉(zhuǎn)化為局部路徑規(guī)劃問題并與智能算法相結(jié)合；另一類是基于速度障礙法得出避碰路徑的可行范圍。而目前將兩者結(jié)合的研究成果較為少見。

受前人研究啟發(fā)，本文擬將速度障礙法基本思想與已成功運用于Kinodynamic 路徑規(guī)劃問題［9］的雙向搜索樹（Bi-RRT）算法相結(jié)合，首先針對父節(jié)點延伸方向位于錐形碰撞區(qū)內(nèi)的情況提出避碰危險度系數(shù)ω與障礙物排斥向量R，從而使搜索樹向著遠離障礙物的趨勢延伸；然后提出一種兩搜索樹并行延伸的方法及當(dāng)父節(jié)點延伸方向位于錐形碰撞區(qū)外時觸發(fā)的目標(biāo)吸引向量A，以提高算法的實時性并加強兩搜索樹朝目標(biāo)點靠近的趨勢；最后通過仿真實驗證明改進算法的可行性與優(yōu)越性。

1 無人水面艇避碰模型

USV 在自主航行過程中通過智能感知系統(tǒng)實時探測預(yù)定航線及其周邊的障礙物，并自動規(guī)避障礙物。本文設(shè)定的USV 避碰問題如圖1 所示，具體描述如下：t0時刻艇A 以速度V沿預(yù)定航線OS 航行，USV 智能感知系統(tǒng)感知到航線OS 以及周邊出現(xiàn)此前未探測到的障礙物區(qū)域B1，B2，B3；以艇A 總長為直徑作圓，所覆蓋的區(qū)域視作該USV 區(qū)域?；谏鲜銮闆r，本文基于改進Bi-RRT算法設(shè)計一種自動避碰算法，使艇A 盡量保留原定航線，在B2，B3的限制下安全、高效地規(guī)避B1。

圖1 USV 避碰情形Fig.1 The situation of USV collision avoidance

2 速度障礙法和Bi-RRT 算法

2.1 速度障礙法的原理

速度障礙法由Fiorini 等［10］提出，該方法的避碰原理為：首先，基于Featherstone 的理論，將任意多邊形表示為相應(yīng)的一系列圓，同時為了便于建模與計算，將USV 與障礙物分別用不同的二維圓形代替；然后，將USV 尺寸附加到障礙物尺寸上計算兩者之間的速度障礙關(guān)系，從而將USV 簡化為一個質(zhì)點，使得障礙物區(qū)域膨脹為半徑更大的圓形區(qū)域；最后，定義錐形碰撞區(qū)為USV 與障礙物發(fā)生碰撞的相對速度的集合，幾何上表示為以USV為頂點向障礙物圓周作出的兩條切線所夾的二維區(qū)域，而在錐形碰撞區(qū)以外區(qū)域中的相對速度將使USV 安全避碰。

速度障礙法為USV 避碰過程中如何改變和控制航向提供了簡單高效的依據(jù)，也是Bi-RRT 算法在USV 避碰中運用及改進的理論依據(jù)。

2.2 Bi-RRT 算法

Bi-RRT 算法是La Valle 等［11］受雙向搜索技術(shù)［12］的啟發(fā)而提出。相比La Valle 于1998 年提出的經(jīng)典快速擴展隨機樹（RRT）算法［12］，Bi-RRT 算法分別以起點Xinit與終點Xgoal為根節(jié)點，構(gòu)造兩棵快速擴展隨機樹，兩樹同時生長，直到相遇為止。

Bi-RRT 算法由于保持了經(jīng)典RRT 算法在狀態(tài)空間中規(guī)劃路徑的思想，所以也保持了其優(yōu)越性。La Valle［13］對RRT 算法分別進行了完整性約束、非完整性約束、Kinodynamic、閉式運動鏈條件下的路徑規(guī)劃測試，結(jié)果表明，該算法在處理具有微分約束的系統(tǒng)及單查詢非完整性約束系統(tǒng)的路徑規(guī)劃問題上表現(xiàn)得非常高效。Bi-RRT 算法作為RRT 算法的改進，算法終止條件是以路徑規(guī)劃起點為根節(jié)點生長的搜索樹與以路徑規(guī)劃終止點為根節(jié)點生長的搜索樹相遇，理論上這可以提高對位姿空間搜索的效率；同時在該算法中，兩樹擴展節(jié)點的操作有著向?qū)Ψ剿阉鳂淇拷内厔?，理論上可以增強搜索的目的性，縮短路徑規(guī)劃時間。設(shè)兩棵搜索樹分別為Ta與Tb，搜索樹Ta以USV 當(dāng)前位置Xinit進行初始化，搜索樹Tb以原始航線上的航跡恢復(fù)點Xgoal進行初始化。迭代過程中某搜索樹T生成隨機點Pr(Xr，Yr)，T中離該隨機點Pr最近的節(jié)點為Pp(Xp，Yp)，Pp將作為該迭代過程中的父節(jié)點以USV 探索步長S向Pr進行延伸，延伸得到的子節(jié)點為Pn(Xn，Yn)。障礙物坐標(biāo)為Po(Xo，Yo) ，障礙物膨脹半徑為R。Bi-RRT 程序流程如圖2 所示。

Bi-RRT 算法應(yīng)用于USV 避碰時兩棵搜索樹分別以艇A 當(dāng)前位置Xinit(X0，Y0)與預(yù)定航線OS上的航跡恢復(fù)點Xgoal(X1，Y1)為根節(jié)點，然后不斷擴展節(jié)點直到兩樹滿足相遇條件，最后通過兩棵搜索樹相遇點分別進行回溯操作，得到艇A 避碰路徑點及完成路徑規(guī)劃。圖3 所示為算法的構(gòu)建過程。

3 Bi-RRT 算法的改進

基于速度障礙法，綜合考慮USV 避碰的實際情況與算法本身的運行效率及實時性，針對搜索樹延伸方式不利于智能規(guī)避障礙物以及父節(jié)點選取方式影響算法實時性的弊端，本文對Bi-RRT 算法進行了改進。

3.1 障礙物排斥向量R 的設(shè)計

圖2 Bi-RRT 算法的程序流程圖Fig.2 The execution procedure of Bi-RRT algorithm

圖3 Bi-RRT 算法的構(gòu)建過程Fig.3 The build process of Bi-RRT algorithm

Bi-RRT 算法搜索樹延伸方式為：先將一棵搜索樹中距離位姿空間隨機點最近的節(jié)點擴展至新節(jié)點，然后再擴展另一棵搜索樹距離該新節(jié)點最近的節(jié)點。盡管該擴展方法理論上加強了兩棵樹延伸的目的性，但是若該隨機點引導(dǎo)兩棵搜索樹同時向趨近障礙物方向發(fā)展，其表現(xiàn)在USV 避碰模型上則是該隨機點引導(dǎo)USV 航向調(diào)整至錐形碰撞區(qū)，所帶來的影響是輕則加大后續(xù)節(jié)點延伸標(biāo)準(zhǔn)步長后落至障礙物區(qū)域概率增大，加大算法的無謂消耗而重則使算法無法收斂和規(guī)劃出有效的避碰路徑。

鑒此，本文提出一種基于速度障礙法的障礙物排斥向量R，以改進Bi-RRT 算法的延伸步驟，改進后在迭代過程中能自動識別父節(jié)點延伸方向是否位于錐形碰撞區(qū)，并能根據(jù)避碰危險度系數(shù)ω動態(tài)調(diào)整R 的作用強度，使USV 避碰路徑更加合理高效。

根據(jù)Bi-RRT 算法的延伸步驟，延伸子節(jié)點坐標(biāo)Pn(Xn，Yn)為

采用向量表示式（1）為

若該次延伸操作使USV 航向引導(dǎo)至錐形碰撞區(qū)，即幾何上滿足α1＜θ1，其中：

此時，將觸發(fā)本文改進Bi-RRT 算法中R與ω，在R與ω作用下，延伸子節(jié)點Pn及其坐標(biāo)不再滿足式（1）和式（2），由過渡向量V 及向量表達式（8）得到：

式中，Ls為安全避碰臨界距離。式（6）的核心函數(shù)為雙曲正切函數(shù)y=tanh(x)，該函數(shù)圖像如圖4所示。

圖4 雙曲正切函數(shù)圖像Fig.4 Graph of hyperbolic tangent function

式（7）表明，當(dāng)父節(jié)點Pp延伸方向位于錐形碰撞區(qū)以內(nèi)時，USV 航向偏危險，此時在原始延伸基礎(chǔ)上，添加Po到Pp連線方向根據(jù)ω動態(tài)調(diào)節(jié)的R，讓搜索樹朝著遠離障礙物的趨勢延伸。圖5所示為R與ω作用示意圖。

圖5 R 與ω 作用示意圖Fig.5 Schematic diagram of the action between R and ω

由圖4 可知，函數(shù)y=tanh(x)位于x軸正半軸部分有如下特性：1）當(dāng)輸入x∈(0，2)時，y∈[0 ，1]且數(shù)值變化劇烈；2）當(dāng)輸入x∈( 2，+∞ )時，y對x的變化不敏感。當(dāng) |PpPo|＞Ls時，隨著 |PpPo|的增大，根據(jù)特性2），此時ω值保持在1 附近，表現(xiàn)在USV 避障上則是當(dāng)父節(jié)點Pp距離障礙物中心滿足一定安全限度時，R的作用不明顯；當(dāng)|PpPo|＜Ls時，隨著 |PpPo|的減小，根據(jù)特性1），此時ω值將顯著增大，表現(xiàn)在USV 避障上則是父節(jié)點Pp越靠近障礙物中心，R的作用越明顯，使USV 航向向著遠離障礙物的趨勢延伸。

3.2 并行伸展與目標(biāo)點吸引向量A 的設(shè)計

Bi-RRT 算法對兩棵搜索樹的父節(jié)點的定位與伸展規(guī)定了先后順序，Ta產(chǎn)生新節(jié)點Pn后，Tb才能實施延伸步驟。盡管兩棵樹延伸有相互接近的趨勢，但是這種順序結(jié)構(gòu)的執(zhí)行無謂地消耗了運行時間。

為此，本文提出一種兩棵搜索樹并行伸展且在特定條件下具有相互接近趨勢的改進方法。該改進方法通過兩棵搜索樹各自產(chǎn)生隨機點，根據(jù)隨機點各自選取父節(jié)點自行延伸的方式完成搜索樹的擴展。R在特定條件下激發(fā)目標(biāo)點吸引向量A，使搜索樹并行拓展延伸的同時也不失Bi-RRT算法兩搜索樹相互趨近的優(yōu)點。

在迭代過程中，設(shè)某棵搜索樹基于式（1）得到Pn后，若α1＞θ1，此時將觸發(fā)本文改進Bi-RRT 算法中的A，在A作用下，延伸子節(jié)點Pn及其坐標(biāo)不再滿足式（1）和式（2），由過渡向量U及式（19）給出Pn''：

式中，Pg為目標(biāo)點坐標(biāo)。對于搜索樹Ta而言，Pg為原始航線上航跡恢復(fù)點Xgoal；對于搜索樹Tb而言，Pg為USV 當(dāng)前位置Xinit。

式（9）表明，當(dāng)父節(jié)點延伸方向位于錐形碰撞區(qū)外時，USV 航向偏安全，此時在原始延伸基礎(chǔ)上，添加Pp到Pg連線方向上的A，使搜索樹有朝著目標(biāo)點生長的趨勢。圖6 所示為A作用示意圖。

圖6 A 作用示意圖Fig.6 Schematic diagram of the action of A

3.3 改進Bi-RRT 自動避碰算法流程

由于本文兩搜索樹為同時并行延伸，互不干擾，因此以Ta為例，給出利用本文算法實現(xiàn)自動避碰的流程，如圖7 所示。Tb除在第1 步初始化步驟中是以原始航線上航跡恢復(fù)點Xgoal初始化外，其他流程與Ta完全相同。

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

為了驗證改進Bi-RRT 算法的有效性，使用Microsoft Visual Studio 2017 開發(fā)環(huán)境編寫C++程序進行仿真實驗，并將Bi-RRT 算法與本文改進Bi-RRT 算法進行了對比。本文選取文獻［14］所述高速USV 進行仿真實驗，其航速為20 kn，設(shè)定單步探索步長S=10。

仿真實驗中，USV 預(yù)定航線OS 設(shè)為二維坐標(biāo)系第1 象限角平分線上從點（0，0）至點（100，100）線段；USV 當(dāng)前位置Xinit設(shè)為（40，40）；航跡恢復(fù)點Xgoal為（65，65）；OS 上障礙物B1考慮安全區(qū)半徑設(shè)為10，OS 周邊障礙物B2，B3半徑分別設(shè)為10，15；為了測試算法對各種避碰環(huán)境的通用性，B1，B2，B3圓心坐標(biāo)O1，O2，O3均由C++程序隨機數(shù)函數(shù)生成，作為算法輸入，且限定O1在OS 上并位于Xinit與Xgoal之間；O2和O3距離OS 不超過25。

圖7 改進Bi-RRT 自動避碰算法搜索樹Ta 擴展流程Fig.7 The extension procedure of search tree Ta in improved Bi-RRT algorithm

圖8 所示為Bi-RRT 算法與改進Bi-RRT 算法對隨機障礙物避碰路徑點生成情況對比。由圖可見，USV 按照改進Bi-RRT 算法規(guī)劃出的避碰路徑點航行至航跡恢復(fù)點Xgoal的路徑轉(zhuǎn)向點明顯減少，路徑更加平滑。這是由于父節(jié)點延伸方向位于錐形碰撞區(qū)以外時觸發(fā)了目標(biāo)點吸引向量A，使兩棵搜索樹延伸更具有目的性，減少了因算法的隨機性所帶來的路徑震蕩問題。

由于兩種算法均為隨機算法，為了降低結(jié)果的隨機性，提高論證的可信度，以該避碰環(huán)境作為輸入，多次運行此兩種算法，以比較路徑點數(shù)目、父節(jié)點延伸失敗次數(shù)與算法完成避碰路徑點規(guī)劃所消耗的時間，結(jié)果如表1 所示。圖9所示為兩種算法的搜索樹延伸失敗節(jié)點對比。

由于兩種算法探索步長設(shè)置相同，因此路徑點數(shù)目可以直接反映路徑長度。由表1 與圖8 可見，USV 按照改進Bi-RRT 算法規(guī)劃出的避碰路徑點航行至航跡恢復(fù)點Xgoal的路徑長度明顯短于同等條件下Bi-RRT 算法的結(jié)果。當(dāng)航速一定時，使用改進Bi-RRT 算法規(guī)劃出的路徑不僅能使USV 能在更短時間內(nèi)完成避碰動作，而且USV 所消耗的燃料也更少，對于提升USV 執(zhí)行任務(wù)時的續(xù)航力有著重要意義。

圖8 Bi-RRT 算法改進前、后避碰路徑點規(guī)劃對比Fig.8 The comparison of path point planning for automatic collision between basic and improved Bi-RRT algorithm

圖9 Bi-RRT 算法改進前、后碰撞次數(shù)對比Fig.9 The comparison of collision times between basic and improved Bi-RRT algorithm

如表1 和圖9 所示，改進Bi-RRT 算法兩搜索樹Ta與Tb父節(jié)點延伸失敗次數(shù)明顯少于Bi-RRT算法，改進Bi-RRT 算法搜索樹延伸失敗點數(shù)量明顯少于改進前算法，這表明改進Bi-RRT 算法迭代過程中子延伸節(jié)點有更大概率通過“碰撞檢測”，這是由于障礙物排斥向量R 與碰撞危險度系數(shù)ω動態(tài)調(diào)節(jié)父節(jié)點延伸方向，使延伸得到的新子節(jié)點能有更大概率得以保留，一定程度上加速了算法的收斂。由表1 對比，還可以發(fā)現(xiàn)，本文提出的改進Bi-RRT 算法在完成相同避碰路徑規(guī)劃任務(wù)時的耗時比改進前的算法有顯著降低，這是由于兩棵搜索樹并行延伸擴展的方式以及當(dāng)父節(jié)點延伸方向位于錐形碰撞區(qū)外時觸發(fā)了目標(biāo)吸引向量A，提高了算法的實時性。

表1 改進前后算法多次運行結(jié)果比較Table 1 The comparison of running result between basic and improved Bi-RRT algorithm

5 結(jié) 語

本文將Bi-RRT 算法與速度障礙法結(jié)合，針對父節(jié)點延伸方向位于錐形碰撞區(qū)內(nèi)的情況，提出了障礙物排斥向量R與避碰危險度系數(shù)ω來動態(tài)調(diào)節(jié)父節(jié)點延伸方向，從而使搜索樹向著遠離障礙物的趨勢延伸，并使延伸得到的新子節(jié)點能有更大的概率得以保留，從而加速算法的收斂。此外，提出了兩棵搜索樹并行延伸的方法，當(dāng)父節(jié)點延伸方向位于錐形碰撞區(qū)外時觸發(fā)目標(biāo)吸引向量A，使兩棵搜索樹延伸更具有目的性，減少了由于算法的隨機性帶來的路徑震蕩問題。該改進算法未來可應(yīng)用于對實時性要求更高的USV 動態(tài)避障問題，以及持續(xù)大風(fēng)浪流擾動情況下USV 回歸原預(yù)定航線的航跡滾動規(guī)劃問題。