李克玉，陸永耕，鮑世通，徐培真

(上海電機(jī)學(xué)院電氣學(xué)院上海 200240)

1 引言

無(wú)人機(jī)的避障規(guī)劃是指在未知障礙物的環(huán)境中，無(wú)人機(jī)能夠借助信息的反饋，在一些約束條件(如時(shí)間最短、距離最短和能耗最低等)下，自主分析環(huán)境信息并規(guī)劃出一條從起始狀態(tài)到目標(biāo)狀態(tài)的無(wú)碰撞路徑[1，2]。國(guó)內(nèi)外很多學(xué)者對(duì)無(wú)人機(jī)的路徑規(guī)劃避障算法進(jìn)行了研究，而且提出了很多優(yōu)化的算法。其中啟發(fā)式算法有遺傳模擬退火算法[3]、蟻群算法[4]、遺傳算法[5]、A*算法[6，7]等。數(shù)學(xué)優(yōu)化算法主要有人工勢(shì)場(chǎng)法[8]、快速擴(kuò)展隨機(jī)樹(shù)(RRT)法[9]等。

RRT算法由于其無(wú)需對(duì)系統(tǒng)進(jìn)行建模，無(wú)需對(duì)搜索區(qū)域進(jìn)行幾何劃分，搜索的范圍廣等眾多優(yōu)勢(shì)[10]，其在避障規(guī)劃上的運(yùn)用受到了很多學(xué)者的青睞，改進(jìn)方法也層出不窮。RRT*算法以新增節(jié)點(diǎn)半徑r的鄰域內(nèi)隨機(jī)樹(shù)上所有的節(jié)點(diǎn)為備選節(jié)點(diǎn)，很好的解決RRT算法生成避障路徑非最優(yōu)問(wèn)題[11]；RRT-Connect算法通過(guò)在起始點(diǎn)和目標(biāo)點(diǎn)并行生成兩棵隨機(jī)樹(shù)的方式來(lái)提高尋找避障路徑的速度[12]，將其與橋梁檢測(cè)(Bride Test)算法融合，很好的解決了RRT-connect算法在窄道內(nèi)采樣難的問(wèn)題[13]；在RRT算法中引入人工勢(shì)場(chǎng)法，很好的利用了勢(shì)場(chǎng)函數(shù)法能夠攜帶障礙物和目標(biāo)點(diǎn)等優(yōu)點(diǎn)，使擴(kuò)展樹(shù)搜索更具有方向性的同時(shí)還提高了障礙物搜索效率[14]。

由此可以看出，在二維空間內(nèi)，目前國(guó)內(nèi)外對(duì)RRT算法的避障路徑快速生成[15]和優(yōu)化窄道局部采樣[16]等方面做了很好的改進(jìn)，使得改進(jìn)后的RRT算法在避障能力和搜索效率等方面都有很大的提高。但是對(duì)于三維空間，由于維度的增加，相關(guān)建模和數(shù)據(jù)處理難度也相應(yīng)的增加[17]，如果依舊使用二維空間的RRT算法以及相關(guān)的改進(jìn)方法進(jìn)行無(wú)人機(jī)路徑規(guī)劃，其效率將會(huì)降低甚至無(wú)法完成完成路徑規(guī)劃[18]。因此本文提出了一種基于預(yù)規(guī)劃路徑優(yōu)化RRT算法的無(wú)人機(jī)三維避障規(guī)劃算法，該算法首先在障礙物膨脹規(guī)則和起點(diǎn)到終點(diǎn)連線與障礙物相交規(guī)則下生成預(yù)規(guī)劃路徑，此預(yù)規(guī)劃路徑可強(qiáng)化RRT擴(kuò)展樹(shù)搜索的連貫性，減少障礙物碰撞檢測(cè)的時(shí)間；將預(yù)規(guī)劃路徑看做由連續(xù)的質(zhì)點(diǎn)構(gòu)成，連續(xù)質(zhì)點(diǎn)可作為搜索樹(shù)在擴(kuò)展中的隨機(jī)狀態(tài)點(diǎn)，可使搜索樹(shù)的擴(kuò)展具有方向性，進(jìn)而減少避障搜索的時(shí)間，提高無(wú)人機(jī)避障規(guī)劃的效率，使得生成避障路徑的時(shí)間更優(yōu)。

2 改進(jìn)算法的基本原理

2.1 RRT算法原理

在度量空間X(包括障礙區(qū)Xobs和自由區(qū)Xfree)中，首先將起始狀態(tài)點(diǎn)Xstart作為隨機(jī)樹(shù)的根節(jié)點(diǎn)，向每個(gè)方向擴(kuò)展一定距離，在自由區(qū)Xfree取一個(gè)狀態(tài)點(diǎn)Xrand作為根節(jié)點(diǎn)的目標(biāo)點(diǎn)，然后選擇距離此狀態(tài)點(diǎn)最近的樹(shù)擴(kuò)展節(jié)點(diǎn)Xnearest作為新的生長(zhǎng)基點(diǎn)，向此狀態(tài)點(diǎn)方向擴(kuò)展一個(gè)定步長(zhǎng)L，得到該方向的新節(jié)點(diǎn)Xnew，若在步長(zhǎng)L擴(kuò)展過(guò)程中與障礙物相撞，則重新選擇狀態(tài)點(diǎn)按上述規(guī)則重新擴(kuò)展，重復(fù)上述過(guò)程，直到有樹(shù)擴(kuò)展節(jié)點(diǎn)到達(dá)目標(biāo)狀態(tài)點(diǎn)Xgoal附近為止，最后路徑回溯，生成的回溯路徑即為RRT算法生成的路徑[19，20]。圖1為RRT搜索樹(shù)節(jié)點(diǎn)擴(kuò)展過(guò)程。

圖1 RRT搜索樹(shù)節(jié)點(diǎn)擴(kuò)展過(guò)程

2.2 障礙物的膨脹規(guī)則

為了保證預(yù)規(guī)劃路徑的生成和無(wú)人機(jī)遇到障礙物時(shí)的安全，便于計(jì)算機(jī)識(shí)別和處理，本文基于文獻(xiàn)[21，22]，對(duì)障礙物膨脹過(guò)程的方式提出改進(jìn)。具體改進(jìn)方法是利用最小的長(zhǎng)方體完全包圍障礙物，以長(zhǎng)方體的對(duì)角線長(zhǎng)度的1.2倍為直徑做障礙物的外接球。

圖2 障礙物的膨脹

2.3 相交規(guī)則

2.3.1 具體步驟

設(shè)膨脹化的障礙物的球心Xoi(xoi，yoi，zoi)及球的半徑為Roi，無(wú)人機(jī)的起點(diǎn)為X0(x0，y0，z0)，終點(diǎn)為Xg(xg，yg，zg)。

Step1：連接起點(diǎn)和終點(diǎn)。

Step2：計(jì)算球心到連線的垂足坐標(biāo)。

(xni-xoi)(xg-x0)+(yni-yoi)(yg-y0)

+(zni-zoi)(zg-z0)=0

(1)

垂足Xni在連線上，根據(jù)向量共線可知：

(2)

將(2)式代入(1)式，式中只有一個(gè)未知數(shù)k，可化解k為

(3)

將(3)式代入(2)式即可求出垂足的坐標(biāo)Xni，設(shè)各球心到連線的距離為L(zhǎng)i。

Step3：檢查連線是否與障礙物相交，只要判斷球心到連線的距離與球的半徑的關(guān)系即可。

1)當(dāng)Li>Roi時(shí)，直線與球不相交，則可判斷此連線為預(yù)規(guī)劃路線；

2)當(dāng)Li=Roi時(shí)，直線與球相切，則可判斷此連線為預(yù)規(guī)劃路線，且切點(diǎn)為預(yù)規(guī)劃路線與球面的交點(diǎn)；

3)當(dāng)Li

Step4：確定預(yù)規(guī)劃路徑新起點(diǎn)

延長(zhǎng)垂線交球面于點(diǎn)Xi，可求得交點(diǎn)坐標(biāo)為

(4)

Step5：以交點(diǎn)為新起點(diǎn)，跳轉(zhuǎn)至step 1。

2.3.2 預(yù)規(guī)劃路徑生成

在障礙物膨脹規(guī)則和相交規(guī)則下，預(yù)規(guī)劃路徑的生成有以下倆種情況：

1)當(dāng)Li>=Roi時(shí)，起點(diǎn)與終點(diǎn)的連線與障礙物的膨脹球體不相交或與球面相切，可判斷此連線為求得的預(yù)規(guī)劃路徑。此預(yù)規(guī)劃路徑可看作是由連續(xù)的質(zhì)點(diǎn)組成，而RRT算法的隨機(jī)狀態(tài)點(diǎn)Xrand可從連續(xù)的質(zhì)點(diǎn)中通過(guò)每單位長(zhǎng)度提取來(lái)得到，具體實(shí)現(xiàn)方法如下：

①根據(jù)已知的起點(diǎn)X0和終點(diǎn)Xg，由三維空間倆點(diǎn)式求出預(yù)規(guī)劃路徑的直線方程如下式

(5)

②通過(guò)控制X或Y或Z軸按一定的擴(kuò)展樹(shù)步長(zhǎng)的比例取值來(lái)確定隨機(jī)狀態(tài)點(diǎn)Xrand(i)(xrand(i)，yrand(i)，zrand(i))的選取及坐標(biāo)。設(shè)xrand(i)=x0+i×AL，根據(jù)上式(5)可得

這樣在三維空間X中，RRT算法在預(yù)規(guī)劃路徑上等量劃分的隨機(jī)狀態(tài)點(diǎn)的指引下，使搜索樹(shù)更具有方向性，減免了很多不必要的擴(kuò)展節(jié)點(diǎn)，進(jìn)而提高避障規(guī)劃的效率。

2)當(dāng)Li

圖3 球體切割面

以交點(diǎn)為新起點(diǎn)連接終點(diǎn)，通過(guò)相交規(guī)則判斷新連線是否可以作為預(yù)規(guī)劃路徑，循環(huán)往復(fù)，直至最后連接終點(diǎn)時(shí)的連線與障礙物沒(méi)有相交(Li

此種情況生成的預(yù)規(guī)劃路徑可分成一段一段來(lái)處理，可根據(jù)每條線段的倆個(gè)端點(diǎn)坐標(biāo)來(lái)確定分段函數(shù)，每個(gè)分段函數(shù)可根據(jù)(1)中的處理方式來(lái)確定RRT搜索樹(shù)的隨機(jī)狀態(tài)點(diǎn)，RRT搜索樹(shù)可沿著這些隨機(jī)狀態(tài)點(diǎn)進(jìn)行擴(kuò)展。其示意圖如圖4。

圖4 預(yù)規(guī)劃路徑分段處理示意圖

3 仿真分析

3.1 仿真環(huán)境

為了驗(yàn)證上述改進(jìn)算法的有效性，本文基于MATLAB2018a在ThinkPad Intel(R)Core(TM)i3-5005主頻2.00GHz的計(jì)算機(jī)上進(jìn)行仿真。仿真的環(huán)境設(shè)置為100km*100 km*100km的區(qū)域，在空間中隨機(jī)分布6個(gè)障礙物，障礙物經(jīng)過(guò)膨脹化處理后為球體。這6個(gè)球體的相關(guān)信息見(jiàn)表1所示。

表1 障礙物相關(guān)信息表

在上述環(huán)境下，無(wú)人機(jī)從起點(diǎn)X0(0，0，0)勻速飛行到終點(diǎn)Xg(100，100，100)。

3.2 實(shí)驗(yàn)圖例

分別采用原RRT算法、文獻(xiàn)[22]中基于切點(diǎn)優(yōu)化人工勢(shì)場(chǎng)法和本文改進(jìn)的RRT算法在相同環(huán)境下對(duì)無(wú)人機(jī)進(jìn)行三維避障規(guī)劃，通過(guò)生成避障路徑的相關(guān)數(shù)據(jù)對(duì)比，來(lái)證明本文改進(jìn)算法的優(yōu)勢(shì)。

3.2.1 預(yù)規(guī)劃路徑及隨機(jī)狀態(tài)點(diǎn)的生成

在上述障礙物建模中運(yùn)用本文提出的預(yù)規(guī)劃路徑生成策略，設(shè)置步長(zhǎng)L=5，比例系數(shù)A=2，在分段的預(yù)規(guī)劃路徑上分別采樣的RRT的隨機(jī)狀態(tài)點(diǎn)，在環(huán)境約束下，本文總共采樣11個(gè)隨機(jī)狀態(tài)點(diǎn)，如圖5。

圖5 隨機(jī)狀態(tài)點(diǎn)采樣

4 各路徑規(guī)劃算法仿真對(duì)比圖

4.1 原RRT算法仿真圖

圖6 正面圖

圖7 Y軸視圖

4.2 文獻(xiàn)[22]改進(jìn)的算法仿真圖

圖8 正視圖

4.3 本文改進(jìn)的RRT算法仿真圖

圖9 正視圖

圖10 Y軸視圖

4.4 三種算法的相關(guān)數(shù)據(jù)對(duì)比

本文基于原RRT算法和本文改進(jìn)RRT算法分別采樣了算法的運(yùn)行時(shí)間、搜索樹(shù)的擴(kuò)展節(jié)點(diǎn)總數(shù)、生成路徑節(jié)點(diǎn)數(shù)和生成路徑的長(zhǎng)度四種數(shù)據(jù)，基于文獻(xiàn)[22]介紹的算法值采樣了算法的運(yùn)行時(shí)間和生成避障路徑的長(zhǎng)度倆種數(shù)據(jù)，并在相同環(huán)境下各取10次仿真結(jié)果的平均值來(lái)比較三種算法的特性。表2中法一代表原RRT算法，法二代表文獻(xiàn)[22]介紹的算法，法三本文改進(jìn)的RRT算法。從表2可以很清楚的看出，在設(shè)定的相同靜態(tài)障礙物的三維環(huán)境下，本文改進(jìn)的算法較原RRT算法的平均節(jié)點(diǎn)個(gè)數(shù)生成總量減少將近一半，生成的避障路徑節(jié)點(diǎn)占比明顯增大，無(wú)效節(jié)點(diǎn)明顯減少，避障路徑趨于平緩，算法復(fù)雜度降低，效率提高；較原RRT算法和文獻(xiàn)[22]介紹的算法的避障路徑搜索時(shí)間要短，生成的避障路徑平均長(zhǎng)度也要短；由此可見(jiàn)，基于預(yù)規(guī)劃路徑優(yōu)化RRT算法有效的減少了無(wú)人機(jī)避障規(guī)劃時(shí)間，并提高了避障路徑搜索的效率。

表2 兩種路徑規(guī)劃法的特性數(shù)據(jù)對(duì)比

5 結(jié)論

本文針對(duì)經(jīng)典RRT算法在三維避障規(guī)劃中效率較低與算法復(fù)雜度高的問(wèn)題，在RRT搜索前先根據(jù)膨脹規(guī)則對(duì)障礙物進(jìn)行處理，然后根據(jù)相交規(guī)則生成預(yù)規(guī)劃路徑，最后基于預(yù)規(guī)劃路徑生成隨機(jī)狀態(tài)點(diǎn)引導(dǎo)RRT算法在三維空間進(jìn)行搜索，進(jìn)而減少無(wú)人機(jī)的避障搜索時(shí)間。

三種算法在相同三維環(huán)境的實(shí)驗(yàn)下得出的仿真結(jié)果顯示，本文改進(jìn)的基于預(yù)規(guī)劃路徑優(yōu)化RRT算法在無(wú)人機(jī)三維避障規(guī)劃中具有較強(qiáng)的避障能力強(qiáng)、較少的避障規(guī)劃時(shí)間，并且生成的避障路徑新生節(jié)點(diǎn)少、算法復(fù)雜度低和搜索效率高的優(yōu)點(diǎn)，最終生成避障路徑的時(shí)間更優(yōu)，在現(xiàn)實(shí)中具有一定的實(shí)用價(jià)值。