基于采樣空間約束的改進(jìn)RRT算法

李瑋煒，張文波，張林叢

(沈陽(yáng)理工大學(xué) 信息科學(xué)與工程學(xué)院，沈陽(yáng) 110159)

水下機(jī)器人(Autonomous Underwater Vehicle，AUV)路徑規(guī)劃問(wèn)題是進(jìn)行水下網(wǎng)絡(luò)部署、目標(biāo)追蹤以及多機(jī)器人協(xié)同等工作的基礎(chǔ)，可以提高水下機(jī)器人檢測(cè)效率[1]，完成指定區(qū)域的覆蓋[2]和固定目標(biāo)的追蹤[3]。多機(jī)器人路徑規(guī)劃可以實(shí)現(xiàn)對(duì)目標(biāo)的水下圍捕[4]以及協(xié)同探測(cè)[5]。由于無(wú)纜AUV具有活動(dòng)范圍大、機(jī)動(dòng)性好、不受電纜限制、隱蔽性好、安全性高等優(yōu)點(diǎn)，被廣泛應(yīng)用于水下作業(yè)。

水下與陸地不同:水下不僅環(huán)境復(fù)雜，且由于水下更加空曠，需要進(jìn)行大面積路徑規(guī)劃的可能性較大；水下機(jī)器人獲取能源手段單一，算力和工作時(shí)長(zhǎng)有限；采用水聲通信，信息傳播慢、損耗大。國(guó)內(nèi)外常用路徑規(guī)劃算法有A*算法[6]、人工勢(shì)場(chǎng)算法[7]、基于仿生學(xué)算法[8-9]、基于深度學(xué)習(xí)算法[10]、快速擴(kuò)展隨機(jī)樹(shù)(RRT)算法[11]、RRT*算法[12]及其改進(jìn)算法[13-15]等。

RRT算法原理簡(jiǎn)單、算力占用小，具有高隨機(jī)性和強(qiáng)適用性，具有出色的探索能力，適用于水下路徑規(guī)劃，但存在一定的缺陷。一方面，由于采用隨機(jī)采樣的方法，隨機(jī)點(diǎn)沒(méi)有方向性，探索時(shí)間會(huì)隨著解空間的增大而急速增加，給AUV帶來(lái)算力上的極大挑戰(zhàn)；另一方面，RRT算法生成的路徑不光滑，規(guī)劃出的路徑長(zhǎng)度相較最優(yōu)路徑更長(zhǎng)，增加了AUV的工作時(shí)長(zhǎng)，給AUV帶來(lái)能源上的挑戰(zhàn)。

針對(duì)RRT算法的改進(jìn)算法中，RRT*算法是較為經(jīng)典的一種。該算法通過(guò)為新節(jié)點(diǎn)重新選擇父節(jié)點(diǎn)和根據(jù)代價(jià)有條件地將其他節(jié)點(diǎn)的父節(jié)點(diǎn)替換為新節(jié)點(diǎn)兩個(gè)步驟，保證所有節(jié)點(diǎn)到起始點(diǎn)的代價(jià)和最小。其優(yōu)點(diǎn)是規(guī)劃出的路徑相對(duì)平滑，路徑長(zhǎng)度比RRT算法生成的路徑長(zhǎng)度短；其缺點(diǎn)是路徑的優(yōu)劣非常依賴(lài)生成節(jié)點(diǎn)的數(shù)量和時(shí)間，每生成一個(gè)新節(jié)點(diǎn)，都需要對(duì)已經(jīng)存在的路徑的代價(jià)重新計(jì)算。因此，RRT*算法對(duì)AUV的算力有一定要求。

基于上述RRT算法存在問(wèn)題的分析，本文重點(diǎn)解決隨機(jī)性過(guò)高和路徑質(zhì)量低兩個(gè)問(wèn)題，提出一種基于采樣空間約束的改進(jìn)RRT算法。首先通過(guò)約束RRT算法采樣點(diǎn)范圍改善隨機(jī)性過(guò)大的問(wèn)題，實(shí)現(xiàn)當(dāng)規(guī)劃面積較大時(shí)，該算法能夠更快收斂；然后通過(guò)隨機(jī)節(jié)點(diǎn)的特征完成概率計(jì)算，利用節(jié)點(diǎn)概率與既定閾值的對(duì)比，完成隨機(jī)節(jié)點(diǎn)概率選取，從而實(shí)現(xiàn)路徑的優(yōu)化，提升路徑質(zhì)量。

1 相關(guān)算法描述

1.1 RRT算法

RRT算法是一種傳統(tǒng)的基于采樣的路徑規(guī)劃算法，具有算法簡(jiǎn)單、高效等優(yōu)勢(shì)。其路徑規(guī)劃過(guò)程示意圖如圖1所示。圖1中:Xstart為初始節(jié)點(diǎn)；Xgoal為目標(biāo)節(jié)點(diǎn)；Xrand為隨機(jī)采樣節(jié)點(diǎn)；Xnear為距離隨機(jī)采樣節(jié)點(diǎn)最近的節(jié)點(diǎn)；Xnew為新生成節(jié)點(diǎn)；L為步長(zhǎng)。

圖1 RRT算法路徑規(guī)劃過(guò)程示意圖

RRT算法以Xstart節(jié)點(diǎn)作為根節(jié)點(diǎn)，在解空間內(nèi)隨機(jī)生成Xrand節(jié)點(diǎn)，尋找與Xrand節(jié)點(diǎn)最近的Xnear節(jié)點(diǎn)，連接兩節(jié)點(diǎn)，在連線(xiàn)上距離Xnear節(jié)點(diǎn)距離為步長(zhǎng)L的位置，生成新節(jié)點(diǎn)Xnew，不斷重復(fù)上述過(guò)程生成隨機(jī)擴(kuò)展樹(shù)，當(dāng)隨機(jī)樹(shù)中的Xnew節(jié)點(diǎn)包含了目標(biāo)點(diǎn)或進(jìn)入了目標(biāo)區(qū)域，隨機(jī)擴(kuò)展樹(shù)停止擴(kuò)展。通過(guò)回溯節(jié)點(diǎn)的方式在隨機(jī)樹(shù)中生成一條從Xstart節(jié)點(diǎn)到Xgoal節(jié)點(diǎn)的路徑。

1.2 凸包算法

凸包在圖形學(xué)中指在一個(gè)實(shí)數(shù)向量空間V中，對(duì)于給定集合M，所有包含M的凸集的交集N稱(chēng)為M的凸包。

1.3 膨脹算法

膨脹算法以數(shù)學(xué)形態(tài)為基礎(chǔ)對(duì)圖像進(jìn)行分析，用具有一定形態(tài)的結(jié)構(gòu)元素，度量和提取圖像中的對(duì)應(yīng)形狀，從而達(dá)到對(duì)圖像分析和識(shí)別的目的。其定義為

式中:A⊕B表示使用B對(duì)A進(jìn)行膨脹；()z表示B平移z后得到的結(jié)果。所有滿(mǎn)足上式的點(diǎn)z組成的集合即是A被B膨脹后的結(jié)果。

2 RRT算法改進(jìn)

本文的改進(jìn)方向是通過(guò)對(duì)采樣空間進(jìn)行條件約束，縮小采樣區(qū)域的范圍，進(jìn)而提高隨機(jī)節(jié)點(diǎn)的有效性，再通過(guò)對(duì)隨機(jī)產(chǎn)生的節(jié)點(diǎn)特征進(jìn)行概率評(píng)價(jià)以選取節(jié)點(diǎn)，進(jìn)而達(dá)到優(yōu)化路徑的目的。

2.1 采樣空間約束

RRT算法的采樣區(qū)域通常為整個(gè)空間，本文通過(guò)兩個(gè)算法對(duì)采樣空間進(jìn)行限制。首先對(duì)障礙區(qū)域以及目標(biāo)節(jié)點(diǎn)進(jìn)行節(jié)點(diǎn)提取，建立集合X，在集合X中提取凸節(jié)點(diǎn)組成集合S，然后利用凸包算法劃分出不規(guī)則區(qū)域。假設(shè)不規(guī)則障礙區(qū)域與凸包形成的區(qū)域如圖2與圖3所示。

圖2 不規(guī)則障礙區(qū)域

圖3 凸包形成區(qū)域

對(duì)劃分出的障礙區(qū)域和目標(biāo)區(qū)域做膨脹處理，其算法執(zhí)行過(guò)程如下。

首先定義一個(gè)結(jié)構(gòu)元素，其主要由尺寸、中心點(diǎn)和結(jié)構(gòu)形狀三部分決定，用該結(jié)構(gòu)元素掃描集合X的每一個(gè)節(jié)點(diǎn)；再將結(jié)構(gòu)元素的中心點(diǎn)與集合X中的節(jié)點(diǎn)重合；最后提取結(jié)構(gòu)元素的外圍節(jié)點(diǎn)形成新的區(qū)域。膨脹處理效果如圖4所示。圖4中方形節(jié)點(diǎn)是障礙區(qū)域提取的外圍節(jié)點(diǎn)，圓形節(jié)點(diǎn)是以方形節(jié)點(diǎn)為中心做出的膨脹范圍。

圖4 膨脹處理效果圖

膨脹區(qū)域的大小是由結(jié)構(gòu)元素的尺寸、結(jié)構(gòu)形狀以及障礙區(qū)域決定。如果膨脹區(qū)域過(guò)小，則規(guī)劃出的路徑距離障礙區(qū)域過(guò)近，由于AUV自身存在一定的體積，在按規(guī)劃路徑運(yùn)行時(shí)容易與障礙區(qū)域發(fā)生碰撞；如果膨脹區(qū)域過(guò)大，則對(duì)采樣區(qū)域的限制減小。故需要根據(jù)障礙區(qū)域的形態(tài)、AUV的體積、RRT算法的步長(zhǎng)不斷調(diào)試結(jié)構(gòu)元素。

經(jīng)過(guò)凸包算法的區(qū)域劃分以及膨脹算法的處理，將終點(diǎn)膨脹區(qū)域設(shè)定為A區(qū)，將障礙物膨脹區(qū)域設(shè)定為B區(qū)，將起始節(jié)點(diǎn)與終止節(jié)點(diǎn)形成的矩形區(qū)域設(shè)定為C區(qū)，將其他區(qū)域設(shè)定為D區(qū)，如圖5所示。

圖5 空間劃分圖

2.2 節(jié)點(diǎn)概率計(jì)算

本文通過(guò)對(duì)隨機(jī)生成節(jié)點(diǎn)的特征做綜合評(píng)價(jià)，生成該節(jié)點(diǎn)的概率數(shù)據(jù)，通過(guò)概率判定是否采用該節(jié)點(diǎn)作為下一節(jié)點(diǎn)，進(jìn)而優(yōu)化路徑，提高路徑質(zhì)量。

為方便后續(xù)描述和表達(dá)，首先給出坐標(biāo)系確定方式。由圖6所示，以目標(biāo)節(jié)點(diǎn)Xgoal與Xnear節(jié)點(diǎn)的連線(xiàn)為x軸且目標(biāo)節(jié)點(diǎn)方向?yàn)檎较?、Xnear節(jié)點(diǎn)為中心點(diǎn)建立坐標(biāo)系。

圖6 坐標(biāo)軸及其夾角示意圖

其次確定Xrand的四個(gè)特征，即空間區(qū)域貢獻(xiàn)度、與Xgoal的斜率差值D1、與Xnear的斜率差值D2、與Xnear的角度θ。

由圖5可知，路徑空間劃分為四個(gè)區(qū)域，不同區(qū)域內(nèi)Xrand對(duì)結(jié)果的貢獻(xiàn)度大小不同。A區(qū)域隨機(jī)生成的節(jié)點(diǎn)對(duì)結(jié)果的貢獻(xiàn)度最大，其次依序是B、C、D區(qū)域，以此設(shè)定節(jié)點(diǎn)特征概率P1為

Xrand與Xnear之間直線(xiàn)的斜率和Xgoal與Xnear之間直線(xiàn)的斜率差值D1計(jì)算表達(dá)式為

式中:x_Xrand和y_Xrand分別為Xrand節(jié)點(diǎn)的橫、縱坐標(biāo)值；x_Xnear和y_Xnear分別表示為Xnear節(jié)點(diǎn)的橫、縱坐標(biāo)值；x_Xgoal和y_Xgoal分別為Xgoal節(jié)點(diǎn)的橫、縱坐標(biāo)值。以此設(shè)定節(jié)點(diǎn)特征概率P2為

D1越小，代表Xrand與Xgoal之間方向角越小，二者方向更加一致，該隨機(jī)生成節(jié)點(diǎn)對(duì)結(jié)果的貢獻(xiàn)度越大。

Xrand與Xnear之間直線(xiàn)的斜率和Xnear與Xnear-p之間直線(xiàn)的斜率差值D2計(jì)算表達(dá)式為

式中x_Xnear-p和y_Xnear-p分別為Xnear父節(jié)點(diǎn)的橫、縱坐標(biāo)值。

令節(jié)點(diǎn)特征概率P3為

D2越小，代表在該Xnear處的路徑越平滑。

由于以某一點(diǎn)為中心、呈中心對(duì)稱(chēng)的兩個(gè)點(diǎn)與該點(diǎn)連線(xiàn)所得到直線(xiàn)的斜率相等，因此為增加隨機(jī)生成節(jié)點(diǎn)的導(dǎo)向性，減少與目標(biāo)節(jié)點(diǎn)相反方向的隨機(jī)節(jié)點(diǎn)生成，增加X(jué)rand與Xnear構(gòu)成的直線(xiàn)和x軸正方向的夾角變量θ。

令節(jié)點(diǎn)特征概率P4為

根據(jù)隨機(jī)生成節(jié)點(diǎn)的四個(gè)特征概率的重要性分別賦予權(quán)重，并滿(mǎn)足權(quán)重和為1。根據(jù)公式(8)計(jì)算該隨機(jī)生成節(jié)點(diǎn)的概率P。

式中:ωk為節(jié)點(diǎn)的第k個(gè)特征的權(quán)重；n表示節(jié)點(diǎn)特征總數(shù)。

為判斷是否采用該隨機(jī)節(jié)點(diǎn)為Xnew，設(shè)定節(jié)點(diǎn)閾值。節(jié)點(diǎn)閾值影響路徑生成速度和路徑質(zhì)量。當(dāng)閾值設(shè)定過(guò)高時(shí)，對(duì)隨機(jī)采樣節(jié)點(diǎn)要求高，耗費(fèi)時(shí)間多，路徑規(guī)劃時(shí)間變長(zhǎng)；當(dāng)閾值設(shè)定過(guò)低時(shí)，隨機(jī)采樣節(jié)點(diǎn)對(duì)最終路徑的貢獻(xiàn)變小，路徑的平滑程度變差，路徑變長(zhǎng)。因此，當(dāng)對(duì)路徑要求高、節(jié)點(diǎn)算力強(qiáng)時(shí)可以將閾值提高，當(dāng)需要快速規(guī)劃路徑且對(duì)路徑要求不高時(shí)，可以選擇降低閾值。本實(shí)驗(yàn)中閾值確定為0.35。

當(dāng)該點(diǎn)的概率大于設(shè)定閾值時(shí)，則采用該隨機(jī)節(jié)點(diǎn)，若小于設(shè)定閾值棄用該節(jié)點(diǎn)，重新生成隨機(jī)節(jié)點(diǎn)。

2.3 算法執(zhí)行流程

改進(jìn)的RRT算法流程如圖7所示。

圖7 改進(jìn)的RRT算法流程圖

首先對(duì)采樣空間進(jìn)行劃分，通過(guò)限制采樣區(qū)域改善隨機(jī)性過(guò)大的問(wèn)題。再對(duì)隨機(jī)生成的節(jié)點(diǎn)根據(jù)公式(2)~(8)計(jì)算其概率，最后將概率與設(shè)定的閾值進(jìn)行比較，判定是否采用該隨機(jī)節(jié)點(diǎn)。

3 仿真實(shí)驗(yàn)

3.1 仿真實(shí)驗(yàn)及結(jié)果

為驗(yàn)證改進(jìn)RRT算法的有效性，在python3.7環(huán)境下對(duì)改進(jìn)RRT算法、RRT*算法以及RRT算法進(jìn)行仿真實(shí)驗(yàn)。

實(shí)驗(yàn)條件為:仿真環(huán)境大小為60×60 m2、起始節(jié)點(diǎn)為(-25，-25)、終止節(jié)點(diǎn)為(40，40)、步長(zhǎng)為1、最大輪數(shù)為500。

改進(jìn)的RRT算法、RRT*算法以及RRT算法的執(zhí)行結(jié)果如圖8所示。三角點(diǎn)表示起始節(jié)點(diǎn)、圓點(diǎn)表示終止節(jié)點(diǎn)、實(shí)心區(qū)域?yàn)檎系K區(qū)域、加粗線(xiàn)為規(guī)劃后的最終路徑、常規(guī)線(xiàn)為尋路時(shí)隨機(jī)擴(kuò)展樹(shù)的樹(shù)枝。

圖8 不同算法運(yùn)行結(jié)果(單位:m)

通過(guò)仿真實(shí)驗(yàn)運(yùn)行結(jié)果可知，RRT算法和RRT*算法的隨機(jī)擴(kuò)展樹(shù)比較分散，擴(kuò)展結(jié)點(diǎn)較多，擴(kuò)展范圍較大，無(wú)效擴(kuò)展區(qū)域多，改進(jìn)的RRT算法由于限定了采點(diǎn)空間的范圍，因此擴(kuò)展樹(shù)生長(zhǎng)范圍集中，多分布于障礙物周?chē)?，且路徑相?duì)光滑。

3.2 性能對(duì)比分析

三種算法的性能對(duì)比如表1所示。

表1 三種算法性能對(duì)比

由表1可得，在相同的路徑規(guī)劃面積下，改進(jìn)RRT算法的路徑規(guī)劃時(shí)間和組成路徑的節(jié)點(diǎn)數(shù)量均遠(yuǎn)小于RRT*算法和RRT算法，有效解決因隨機(jī)采點(diǎn)機(jī)制沒(méi)有方向性產(chǎn)生隨機(jī)點(diǎn)的問(wèn)題。

圖9和圖10是尋路面積從40×40 m2逐漸遞增至180×180 m2時(shí)，對(duì)三種算法分別進(jìn)行100次實(shí)驗(yàn)，并對(duì)各項(xiàng)數(shù)據(jù)求平均值所得到的結(jié)果。

圖9 路徑規(guī)劃時(shí)間隨尋路面積變化圖

圖10 路徑節(jié)點(diǎn)數(shù)量隨尋路面積變化圖

由圖9、圖10可知，當(dāng)尋路面積增加時(shí)，改進(jìn)的RRT算法在時(shí)間方面的增長(zhǎng)幅度遠(yuǎn)小于RRT*算法和RRT算法，同時(shí)在組成路徑節(jié)點(diǎn)數(shù)量方面也小于RRT*算法和RRT算法并且數(shù)量更加穩(wěn)定，意味著在采樣空間更大時(shí)，改進(jìn)的RRT算法適用性更強(qiáng)、路徑更加穩(wěn)定，對(duì)AUV的算力和能量要求更小，在水下大面積水域的路徑規(guī)劃中效果更好。

4 結(jié)論

提出了一種基于采樣空間約束的改進(jìn)RRT算法，首先針對(duì)RRT算法具有的隨機(jī)性，利用凸包算法和膨脹算法思想，約束采點(diǎn)空間的范圍；針對(duì)生成路徑質(zhì)量低的問(wèn)題，通過(guò)計(jì)算隨機(jī)采樣點(diǎn)的概率并與閾值比較的方式，決定是否采用該隨機(jī)點(diǎn)為產(chǎn)生下一新節(jié)點(diǎn)的基礎(chǔ)。該方法使采樣工作更具有方向性，減少了無(wú)效點(diǎn)的數(shù)量，使得計(jì)算出的路徑相較RRT算法，路徑長(zhǎng)度與路徑規(guī)劃時(shí)間均更短，路徑更平滑。

通過(guò)PyCharm進(jìn)行仿真實(shí)驗(yàn)與數(shù)據(jù)分析，實(shí)驗(yàn)結(jié)果表明，改進(jìn)的RRT算法在采樣空間面積較大的情況下，具有更好的路徑規(guī)劃能力，并且路徑規(guī)劃時(shí)間與路徑長(zhǎng)度均有所下降，可以極大地減少AUV能耗，提高AUV的續(xù)航，適用于水下大面積路徑規(guī)劃。