基于改進RRT算法的無人機三維避障規劃

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

(上海電機學院電氣學院上海 200240)

1 引言

無人機的避障規劃是指在未知障礙物的環境中，無人機能夠借助信息的反饋，在一些約束條件(如時間最短、距離最短和能耗最低等)下，自主分析環境信息并規劃出一條從起始狀態到目標狀態的無碰撞路徑[1，2]。國內外很多學者對無人機的路徑規劃避障算法進行了研究，而且提出了很多優化的算法。其中啟發式算法有遺傳模擬退火算法[3]、蟻群算法[4]、遺傳算法[5]、A*算法[6，7]等。數學優化算法主要有人工勢場法[8]、快速擴展隨機樹(RRT)法[9]等。

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

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

2 改進算法的基本原理

2.1 RRT算法原理

在度量空間X(包括障礙區Xobs和自由區Xfree)中，首先將起始狀態點Xstart作為隨機樹的根節點，向每個方向擴展一定距離，在自由區Xfree取一個狀態點Xrand作為根節點的目標點，然后選擇距離此狀態點最近的樹擴展節點Xnearest作為新的生長基點，向此狀態點方向擴展一個定步長L，得到該方向的新節點Xnew，若在步長L擴展過程中與障礙物相撞，則重新選擇狀態點按上述規則重新擴展，重復上述過程，直到有樹擴展節點到達目標狀態點Xgoal附近為止，最后路徑回溯，生成的回溯路徑即為RRT算法生成的路徑[19，20]。圖1為RRT搜索樹節點擴展過程。

圖1 RRT搜索樹節點擴展過程

2.2 障礙物的膨脹規則

為了保證預規劃路徑的生成和無人機遇到障礙物時的安全，便于計算機識別和處理，本文基于文獻[21，22]，對障礙物膨脹過程的方式提出改進。具體改進方法是利用最小的長方體完全包圍障礙物，以長方體的對角線長度的1.2倍為直徑做障礙物的外接球。

圖2 障礙物的膨脹

2.3 相交規則

2.3.1 具體步驟

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

Step1：連接起點和終點。

Step2：計算球心到連線的垂足坐標。

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

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

(1)

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

(2)

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

(3)

將(3)式代入(2)式即可求出垂足的坐標Xni，設各球心到連線的距離為Li。

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

1)當Li>Roi時，直線與球不相交，則可判斷此連線為預規劃路線；

2)當Li=Roi時，直線與球相切，則可判斷此連線為預規劃路線，且切點為預規劃路線與球面的交點；

3)當Li

Step4：確定預規劃路徑新起點

延長垂線交球面于點Xi，可求得交點坐標為

(4)

Step5：以交點為新起點，跳轉至step 1。

2.3.2 預規劃路徑生成

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

1)當Li>=Roi時，起點與終點的連線與障礙物的膨脹球體不相交或與球面相切，可判斷此連線為求得的預規劃路徑。此預規劃路徑可看作是由連續的質點組成，而RRT算法的隨機狀態點Xrand可從連續的質點中通過每單位長度提取來得到，具體實現方法如下：

①根據已知的起點X0和終點Xg，由三維空間倆點式求出預規劃路徑的直線方程如下式

(5)

②通過控制X或Y或Z軸按一定的擴展樹步長的比例取值來確定隨機狀態點Xrand(i)(xrand(i)，yrand(i)，zrand(i))的選取及坐標。設xrand(i)=x0+i×AL，根據上式(5)可得

這樣在三維空間X中，RRT算法在預規劃路徑上等量劃分的隨機狀態點的指引下，使搜索樹更具有方向性，減免了很多不必要的擴展節點，進而提高避障規劃的效率。

2)當Li

圖3 球體切割面

以交點為新起點連接終點，通過相交規則判斷新連線是否可以作為預規劃路徑，循環往復，直至最后連接終點時的連線與障礙物沒有相交(Li

此種情況生成的預規劃路徑可分成一段一段來處理，可根據每條線段的倆個端點坐標來確定分段函數，每個分段函數可根據(1)中的處理方式來確定RRT搜索樹的隨機狀態點，RRT搜索樹可沿著這些隨機狀態點進行擴展。其示意圖如圖4。

圖4 預規劃路徑分段處理示意圖

3 仿真分析

3.1 仿真環境

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

表1 障礙物相關信息表

在上述環境下，無人機從起點X0(0，0，0)勻速飛行到終點Xg(100，100，100)。

3.2 實驗圖例

分別采用原RRT算法、文獻[22]中基于切點優化人工勢場法和本文改進的RRT算法在相同環境下對無人機進行三維避障規劃，通過生成避障路徑的相關數據對比，來證明本文改進算法的優勢。

3.2.1 預規劃路徑及隨機狀態點的生成

在上述障礙物建模中運用本文提出的預規劃路徑生成策略，設置步長L=5，比例系數A=2，在分段的預規劃路徑上分別采樣的RRT的隨機狀態點，在環境約束下，本文總共采樣11個隨機狀態點，如圖5。

圖5 隨機狀態點采樣

4 各路徑規劃算法仿真對比圖

4.1 原RRT算法仿真圖

圖6 正面圖

圖7 Y軸視圖

4.2 文獻[22]改進的算法仿真圖

圖8 正視圖

4.3 本文改進的RRT算法仿真圖

圖9 正視圖

圖10 Y軸視圖

4.4 三種算法的相關數據對比

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

表2 兩種路徑規劃法的特性數據對比

5 結論

本文針對經典RRT算法在三維避障規劃中效率較低與算法復雜度高的問題，在RRT搜索前先根據膨脹規則對障礙物進行處理，然后根據相交規則生成預規劃路徑，最后基于預規劃路徑生成隨機狀態點引導RRT算法在三維空間進行搜索，進而減少無人機的避障搜索時間。

三種算法在相同三維環境的實驗下得出的仿真結果顯示，本文改進的基于預規劃路徑優化RRT算法在無人機三維避障規劃中具有較強的避障能力強、較少的避障規劃時間，并且生成的避障路徑新生節點少、算法復雜度低和搜索效率高的優點，最終生成避障路徑的時間更優，在現實中具有一定的實用價值。