基于改進(jìn)Bi-RRT 的無(wú)人水面艇自動(dòng)避碰算法

歐陽(yáng)子路，王鴻東*，王檢耀，易宏

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

0 引 言

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

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

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

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

1 無(wú)人水面艇避碰模型

USV 在自主航行過程中通過智能感知系統(tǒng)實(shí)時(shí)探測(cè)預(yù)定航線及其周邊的障礙物，并自動(dòng)規(guī)避障礙物。本文設(shè)定的USV 避碰問題如圖1 所示，具體描述如下：t0時(shí)刻艇A 以速度V沿預(yù)定航線OS 航行，USV 智能感知系統(tǒng)感知到航線OS 以及周邊出現(xiàn)此前未探測(cè)到的障礙物區(qū)域B1，B2，B3；以艇A 總長(zhǎng)為直徑作圓，所覆蓋的區(qū)域視作該USV 區(qū)域?；谏鲜銮闆r，本文基于改進(jìn)Bi-RRT算法設(shè)計(jì)一種自動(dòng)避碰算法，使艇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)的一系列圓，同時(shí)為了便于建模與計(jì)算，將USV 與障礙物分別用不同的二維圓形代替；然后，將USV 尺寸附加到障礙物尺寸上計(jì)算兩者之間的速度障礙關(guān)系，從而將USV 簡(jiǎn)化為一個(gè)質(zhì)點(diǎn)，使得障礙物區(qū)域膨脹為半徑更大的圓形區(qū)域；最后，定義錐形碰撞區(qū)為USV 與障礙物發(fā)生碰撞的相對(duì)速度的集合，幾何上表示為以USV為頂點(diǎn)向障礙物圓周作出的兩條切線所夾的二維區(qū)域，而在錐形碰撞區(qū)以外區(qū)域中的相對(duì)速度將使USV 安全避碰。

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

2.2 Bi-RRT 算法

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

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

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

3 Bi-RRT 算法的改進(jìn)

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

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

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

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

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

采用向量表示式（1）為

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

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

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

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

式（7）表明，當(dāng)父節(jié)點(diǎn)Pp延伸方向位于錐形碰撞區(qū)以內(nèi)時(shí)，USV 航向偏危險(xiǎn)，此時(shí)在原始延伸基礎(chǔ)上，添加Po到Pp連線方向根據(jù)ω動(dòng)態(tài)調(diào)節(jié)的R，讓搜索樹朝著遠(yuǎn)離障礙物的趨勢(shì)延伸。圖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)時(shí)，y∈[0 ，1]且數(shù)值變化劇烈；2）當(dāng)輸入x∈( 2，+∞ )時(shí)，y對(duì)x的變化不敏感。當(dāng) |PpPo|＞Ls時(shí)，隨著 |PpPo|的增大，根據(jù)特性2），此時(shí)ω值保持在1 附近，表現(xiàn)在USV 避障上則是當(dāng)父節(jié)點(diǎn)Pp距離障礙物中心滿足一定安全限度時(shí)，R的作用不明顯；當(dāng)|PpPo|＜Ls時(shí)，隨著 |PpPo|的減小，根據(jù)特性1），此時(shí)ω值將顯著增大，表現(xiàn)在USV 避障上則是父節(jié)點(diǎn)Pp越靠近障礙物中心，R的作用越明顯，使USV 航向向著遠(yuǎn)離障礙物的趨勢(shì)延伸。

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

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

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

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

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

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

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

3.3 改進(jìn)Bi-RRT 自動(dòng)避碰算法流程

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

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

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

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

圖7 改進(jìn)Bi-RRT 自動(dòng)避碰算法搜索樹Ta 擴(kuò)展流程Fig.7 The extension procedure of search tree Ta in improved Bi-RRT algorithm

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

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

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

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

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

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

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

5 結(jié) 語(yǔ)

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