楊左華，王玉龍,，戚愛春

(1.江蘇科技大學(xué) 電子信息學(xué)院，江蘇 鎮(zhèn)江 212003；2.上海大學(xué) 機(jī)電工程與自動化學(xué)院，上海 200444)

0 引 言

水面無人艇（Unmanned Surface Vehicle，USV）是一種可以自主感知環(huán)境、探測目標(biāo)、智能避障，完成特定海洋任務(wù)如偵察、巡邏、監(jiān)視等的小型船舶[1]。為提高無人艇執(zhí)行任務(wù)的自主權(quán)，減少人工監(jiān)督以降低運(yùn)營成本，水面無人艇的智能避障一直是研究熱點(diǎn)。USV 的智能避障分為已知環(huán)境的全局避障和動態(tài)未知環(huán)境的局部避障。全局避障是指在已知水面環(huán)境地圖上為無人艇規(guī)劃出一條從起始點(diǎn)到終點(diǎn)的無障礙路徑，局部避障規(guī)劃是指根據(jù)無人艇的周圍實(shí)時動態(tài)環(huán)境進(jìn)行有效避障[2-3]。

基于隨機(jī)采樣的快速搜索樹算法（Rapid-Exploring Random Tree，RRT）無需對環(huán)境空間進(jìn)行預(yù)處理，已被證明可以用于各種復(fù)雜環(huán)境的路徑探索，完成無人艇的全局避障規(guī)劃[4-5]。RRT 算法首先由Lavalle[6]提出用于路徑規(guī)劃。然而基本的RRT 算法迭代次數(shù)過多，搜索節(jié)點(diǎn)遍布整個環(huán)境地圖，浪費(fèi)了大量內(nèi)存資源，同時收斂速度慢，實(shí)時效果差。針對基本RRT 算法存在的不足，眾多學(xué)者提出了改進(jìn)算法，如RRT-Connect 算法、RRT*算法、B-RRT*算法、IB-RRT*算法。RRT*算法通過改變新節(jié)點(diǎn)連接方式以獲取最低路徑成本，加快了收斂速度，生成的路徑漸近最優(yōu)[7-8]。RRT-Connect 算法提出起始點(diǎn)終點(diǎn)并行生成搜索樹的方法，提高了路徑搜索的速度[9-10]。B-RRT*算法則是RRT-Connect 算法和RRT*算法的結(jié)合，生成一條最優(yōu)路徑[11]。文獻(xiàn)[12]提出了一種基于動態(tài)步長的路徑規(guī)劃自適應(yīng)RRT 算法，并利用三次多項(xiàng)式曲線擬合來平滑規(guī)劃路徑。文獻(xiàn)[13]采用自適應(yīng)步長RRT 方法進(jìn)行雙機(jī)器人路徑規(guī)劃，實(shí)現(xiàn)了雙機(jī)器人在各自構(gòu)型空間中的協(xié)同路徑規(guī)劃。文獻(xiàn)[14]提出一種基于路標(biāo)引導(dǎo)和增長采樣區(qū)域的混合策略來引導(dǎo)RRT 算法向目標(biāo)搜索。文獻(xiàn)[15]改進(jìn)了RRT-Connect 算法，以便設(shè)計管線最優(yōu)路徑，采用貪心算法去除多余節(jié)點(diǎn)，并采用Catmull-Rom 曲線擬合關(guān)鍵節(jié)點(diǎn)處曲線。

對于無人艇的全局避障規(guī)劃，這些算法仍存在一些問題：

1）在避障規(guī)劃時未考慮與障礙物的安全距離。

2）隨機(jī)采樣帶來搜索路徑的不穩(wěn)定性；

3）路徑長度仍需優(yōu)化；

4）規(guī)劃的路徑折點(diǎn)過多，仍需后續(xù)光滑處理。

為解決這些問題，本文提出一種改進(jìn)的無人艇全局避障規(guī)劃方法，即（Line Segment Theorem-RRT*）LT-RRT*算法。考慮到USV 實(shí)際航行時應(yīng)與障礙物保持一定安全距離，首先運(yùn)用柵格法對環(huán)境地圖進(jìn)行預(yù)處理。然后再根據(jù)終點(diǎn)采樣概率選取采樣點(diǎn)，改善全局采樣引起的路徑不穩(wěn)定性，節(jié)約內(nèi)存資源，提高收斂速度。根據(jù)線段定理重新選取新節(jié)點(diǎn)和附近節(jié)點(diǎn)的父節(jié)點(diǎn)，跳過中間節(jié)點(diǎn)直接連接樹節(jié)點(diǎn)，減少路徑折點(diǎn)及路徑成本，使算法最終生成相對平滑的規(guī)劃路徑。最后在相同水面環(huán)境下，將本文算法與現(xiàn)有算法進(jìn)行比較，結(jié)果表明LT-RRT*算法可以為USV 規(guī)劃出更短的避障路徑，其轉(zhuǎn)折點(diǎn)數(shù)量大大減少，同時距離障礙物有一定安全距離，更有利于USV 的安全行駛。

1 現(xiàn)有算法研究

1.1 問題定義

1.2 基本RRT 算法

基本RRT 算法首先在無障礙空間 Dfree中進(jìn)行隨機(jī)采樣，生成隨機(jī)采樣點(diǎn) xsample，尋找樹上距離隨機(jī)采樣點(diǎn)最近的節(jié)點(diǎn) xnearest，從該點(diǎn)向采樣點(diǎn)擴(kuò)展一個固定步長生成新的節(jié)點(diǎn) xnew， 連接最近點(diǎn) xnearest與新節(jié)點(diǎn)xnew，成為樹的新分支。重復(fù)上述過程進(jìn)行多次迭代采樣，直到新節(jié)點(diǎn) xnew距離終點(diǎn) xgoal一定距離，最終找到起始點(diǎn)到終點(diǎn)的最優(yōu)路徑?；綬RT 算法用于無人艇的全局避障規(guī)劃，在添加新節(jié)點(diǎn)時進(jìn)行避障檢測，如果 xnearest與新節(jié)點(diǎn) xnew的連線與障礙物產(chǎn)生碰撞則放棄當(dāng)次采樣，重新進(jìn)行迭代采樣。RRT 算法示意圖及具體算法流程如圖1 所示。

圖 1 RRT 算法示意圖Fig.1 The diagram for RRT algorithm

1.3 RRT*算法

由于基本RRT 算法收斂過慢，路徑過長，RRT*算法改變了新節(jié)點(diǎn)的連接方式，以新節(jié)點(diǎn) xnew為中心，找出一定距離內(nèi)的附近節(jié)點(diǎn)，以最小路徑成本在附近節(jié)點(diǎn)和最近節(jié)點(diǎn) xnearest中選擇新節(jié)點(diǎn)的父節(jié)點(diǎn)，從而修正了路徑。

2 基于LT-RRT*的路徑規(guī)劃算法

2.1 環(huán)境建模

由于障礙物邊緣區(qū)域?yàn)椴焕跓o人艇安全航行的危險區(qū)域，規(guī)劃的避障路徑應(yīng)避開障礙物一定距離?？紤]無人艇航行的安全性，在算法執(zhí)行過程中對路徑段進(jìn)行避障距離檢測，雖然可以使最終生成的路徑避開障礙物一定距離，但會大大增加算法計算時間，影響算法收斂速度。因此在進(jìn)行無人艇全局避障規(guī)劃之前，應(yīng)先對環(huán)境地圖進(jìn)行預(yù)處理，根據(jù)障礙物位置標(biāo)示出其周圍不可航行區(qū)域。

運(yùn)用柵格法[16-17]，將環(huán)境地圖按合適比例劃分成M*N 格。每個障礙物單元的8 個方向上的柵格部分處理為其他顏色，定義為無人艇行駛的危險區(qū)域，避障規(guī)劃時同時檢測與危險區(qū)域的碰撞，可以使無人艇與障礙物保持安全距離。

2.2 改進(jìn)采樣點(diǎn)選取

快速隨機(jī)搜索樹算法RRT 在整個環(huán)境空間生成隨機(jī)采樣點(diǎn)，擴(kuò)展了樹節(jié)點(diǎn)，很好地探索了未知空間。但生成大量偏離終點(diǎn)的無用節(jié)點(diǎn)，導(dǎo)致迭代次數(shù)過多，占用內(nèi)存過多，生成路徑過長，搜索路徑過慢，不能很好地滿足無人艇避障規(guī)劃的實(shí)時性要求。

針對此問題，引入終點(diǎn)采樣概率p（0 ≤ p≤1），一次迭代以p 概率直接選擇終點(diǎn)為采樣點(diǎn)，以1-p 的概率隨機(jī)選擇環(huán)境空間中的點(diǎn)。隨機(jī)采樣點(diǎn)不再過于隨機(jī)，p 的值越大，避障路徑的搜索更加偏向終點(diǎn)，大大減少無用空間的探索，有利于樹節(jié)點(diǎn)更加快速逼近終點(diǎn)，提高收斂速度。

2.3 改進(jìn)父節(jié)點(diǎn)選取

2.4 基于線段定理的路徑重連

RRT*算法在添加新節(jié)點(diǎn)時同時考慮了新節(jié)點(diǎn)的周圍節(jié)點(diǎn)，通過周圍節(jié)點(diǎn)到達(dá)新節(jié)點(diǎn)的成本若低于通過最近節(jié)點(diǎn)到達(dá)新節(jié)點(diǎn)的成本，以周圍節(jié)點(diǎn)作為新節(jié)點(diǎn)的父節(jié)點(diǎn)，從而修正航跡，提高收斂率，減少路徑成本。但上述方法仍不可避免節(jié)點(diǎn)過多，路徑過于曲折。為此，基于線段定理（兩點(diǎn)之間直線最短），改進(jìn)父節(jié)點(diǎn)的選擇，拉直路徑。每一次迭代中，選取的父節(jié)點(diǎn)為改進(jìn)后的的父節(jié)點(diǎn)，路徑的成本就會低于經(jīng)過再連接到?；趦牲c(diǎn)之間線段最短，跳過中間節(jié)點(diǎn)直接連接，可以大大降低路徑成本。同時，LT-RRT*算法刪除 xnew的周圍節(jié)點(diǎn)的上一次連接線段，重新和 xnearnest的父節(jié)點(diǎn)連接，降低了到達(dá)附近節(jié)點(diǎn)的成本。圖2 和圖3 分別顯示了RRT*算法與LT-RRT*算法的顯著差異。RRT*算法中附近的節(jié)點(diǎn)未得到改進(jìn)，而在LT-RRT*算法中不僅拉直了到達(dá)xnew的路徑，也拉直了到達(dá) xnew的周圍節(jié)點(diǎn)的路徑。

圖 2 RRT*算法父節(jié)點(diǎn)選擇和路徑重連Fig.2 RRT* algorithm′s parent node selection and path reconnection

圖 3 LT-RRT*算法父節(jié)點(diǎn)選擇和路徑重連Fig.3 LT-RRT* algorithm′s parent node selection and path reconnection

2.5 基于線段定理的路徑重連

本文提出的LT-RRT*算法規(guī)劃無人艇避障路徑的步驟如下：

1）運(yùn)用柵格法對環(huán)境地圖進(jìn)行處理，以其他顏色標(biāo)示出障礙物周圍的危險區(qū)域，使無人艇距離障礙物一定距離，保證其行駛的安全性。

2）以p 概率（0 ≤ p ≤ 1）直接選擇終點(diǎn)為采樣點(diǎn)xsample，1-p 的概率隨機(jī)選擇環(huán)境空間中的點(diǎn)作為采樣點(diǎn) xsample。如果新節(jié)點(diǎn) xnew距離終點(diǎn)固定步長，則找到路徑，轉(zhuǎn)到步驟6，否則進(jìn)行步驟3。

3）在整個樹T 上尋找距離采樣點(diǎn) xsample最近的節(jié)點(diǎn)，從節(jié)點(diǎn)向采樣點(diǎn)擴(kuò)展固定步長得到新節(jié)點(diǎn)。

4）以新節(jié)點(diǎn) xnew為中心尋找樹T 上距離其一定距離的周圍幾個節(jié)點(diǎn) xnear1、 xnear2……。

5）刪除周圍節(jié)點(diǎn)的上一次連接，連接 xnearnest的父節(jié) 點(diǎn)和 周 圍 節(jié) 點(diǎn)，連 接和 新 節(jié) 點(diǎn)。轉(zhuǎn)入下一次迭代，進(jìn)入步驟2。

6）找到路徑，計算路徑長度和搜索時間。

節(jié)點(diǎn)之間的每一次連接，都要進(jìn)行避障檢測，避開障礙物以及障礙物周圍危險區(qū)域。

3 算法性能分析

在簡單障礙物環(huán)境和復(fù)雜障礙物環(huán)境下，對比現(xiàn)有算法和LT-RRT*算法的避障規(guī)劃效果。

3.1 簡單環(huán)境

避開單個障礙物的水面環(huán)境是無人艇避障規(guī)劃中比較簡單的一種情況。圖4 和圖5 為某湖泊水面地圖，地圖大小為1 384 m*1 216 m，以地圖左上角為原點(diǎn)，向右為x 軸正方向，向下為y 軸正方向，起始點(diǎn)為（600 m，600 m），終點(diǎn)為（720 m，260 m），無人艇起始點(diǎn)與終點(diǎn)的直線連接只有單個障礙物。為保證幾種算法性能對比的有效性，本文考慮在相同環(huán)境地圖上進(jìn)行避障規(guī)劃，且保證規(guī)劃參數(shù)一致。

圖 4 RRT 規(guī)劃路徑和RRT*規(guī)劃路徑Fig.4 RRT algorithm′s planning path result and RRT*algorithm′s planning path result

圖 5 RRT-Connect 規(guī)劃路徑和LT-RRT*規(guī)劃路徑Fig.5 RRT-Connect algorithm′s planning path result and LT-RRT* algorithm′s planning path result

結(jié)合表1 列出的幾種算法規(guī)劃的避障路徑結(jié)果，如圖4（a），可以看出經(jīng)典RRT 算法在整個環(huán)境空間進(jìn)行采樣，因而樹節(jié)點(diǎn)遍布整個環(huán)境空間，規(guī)劃時間較長，不能很好滿足無人艇行駛的實(shí)時性要求，探索到的避障路徑由多個樹節(jié)點(diǎn)組成，路徑比較曲折，路徑長度較長。RRT*算法由于改進(jìn)了新節(jié)點(diǎn)的連接方式，考慮了路徑成本，縮短了規(guī)劃時間和路徑長度，但路徑節(jié)點(diǎn)數(shù)沒有大的改善。由圖5（a）可知，雙向搜索樹算法RRT-Connect 由于從起始點(diǎn)和終點(diǎn)同時向?qū)Ψ教剿鞅苷下窂?，方向性較強(qiáng)，減少了對環(huán)境空間中無用區(qū)域的探索，大大節(jié)約了避障規(guī)劃時間，但最終搜索到的避障路徑仍然較長，節(jié)點(diǎn)數(shù)過多，路徑較曲折，不利于無人艇的行駛。

表 1 簡單環(huán)境Tab.1 Simple environment

圖5（b）中LT-RRT*算法的規(guī)劃時間短，且其規(guī)劃的路徑被拉直，大大縮短了路徑長度。經(jīng)過重新選取父節(jié)點(diǎn)的規(guī)劃路徑長度縮短為RRT 算法規(guī)劃路徑長度的64.3%，RRT*算法規(guī)劃路徑長度的71.4%，RRTConnect 算法規(guī)劃路徑長度的80.3%；路徑節(jié)點(diǎn)數(shù)包括起始點(diǎn)終點(diǎn)只有3 個，分別比RRT 算法規(guī)劃路徑少28 個，比RRT*算法規(guī)劃路徑少25 個，比RRT-Connect 算法規(guī)劃路徑少22 個；同時與障礙物之間留有安全距離。因此可以說在簡單環(huán)境下LT-RRT*算法表現(xiàn)優(yōu)于其他算法。

3.2 復(fù)雜環(huán)境

如圖6 和圖7 所示，湖泊水面地圖不變，坐標(biāo)軸不變，起始點(diǎn)為（470 m，1 000 m），終點(diǎn)為（1 200 m，250 m）。無人艇起始點(diǎn)與終點(diǎn)的直線連接存在多個障礙物，在這樣的復(fù)雜環(huán)境中，算法在探索新節(jié)點(diǎn)時增加了對避障檢測程序CollisionFree 的調(diào)用次數(shù)，計算時間也增長。表2 列出了幾種算法在該復(fù)雜環(huán)境下的避障規(guī)劃結(jié)果。對比實(shí)驗(yàn)結(jié)果，LT-RRT*算法在時間上比RRT 算法和RRT*算法短，略長于RRT-Connect 算法，對全局避障規(guī)劃的影響較小，但路徑長度和節(jié)點(diǎn)數(shù)大大減小，同時與障礙物之間留有安全距離。LTRRT*算法規(guī)劃的避障路徑長度為1 175.6 m，分別縮短為RRT 算法規(guī)劃路徑長度的67.8%，RRT*算法規(guī)劃路徑長度的73.2%，RRT-Connect 規(guī)劃路徑長度的77.7%，路徑節(jié)點(diǎn)數(shù)只有10 個，分別比RRT 算法規(guī)劃路徑減少77 個，比RRT*算法規(guī)劃路徑減少71 個，比RRT-Connect 規(guī)劃路徑減少67 個。上述結(jié)果表明在復(fù)雜環(huán)境下，LT-RRT*算法規(guī)劃的避障路徑效果優(yōu)于其他算法。

圖 6 RRT 規(guī)劃路徑和RRT*規(guī)劃路徑Fig.6 RRT algorithm′s planning path result and RRT*algorithm′s planning path result

圖 7 RRT-Connect 規(guī)劃路徑和LT-RRT*規(guī)劃路徑Fig.7 RRT-Connect algorithm′s planning path result and LT-RRT* algorithm′s planning path result

表 2 復(fù)雜環(huán)境Tab.2 Complex environment

4 結(jié) 語

本文在RRT*算法的基礎(chǔ)上提出一種改進(jìn)的無人艇全局避障規(guī)劃方法LT-RRT*（Line Segment Theorem-RRT*）。考慮無人艇航行的安全性，首先對環(huán)境地圖預(yù)先進(jìn)行處理，使算法最終生成的路徑避開障礙物一定距離。其次根據(jù)終點(diǎn)采樣概率選取隨機(jī)采樣點(diǎn)，改善了全局采樣引起的路徑不穩(wěn)定性，提高了算法收斂速度。依據(jù)線段定理重新選取新節(jié)點(diǎn)和附近節(jié)點(diǎn)的父節(jié)點(diǎn)，降低了規(guī)劃路徑節(jié)點(diǎn)，減少了規(guī)劃路徑成本，使算法最終生成相對平滑的避障路徑。后續(xù)考慮加入無人艇的動力學(xué)約束，并探索不同算法規(guī)劃路徑對后續(xù)軌跡跟蹤的影響。