基于改進RRT-connect的四旋翼無人機避障軌跡規劃算法*

李成雷， 賀繼林， 鄧 宇,2， 敖小樂

(1.中南大學 高性能復雜制造國家重點實驗室,湖南 長沙 410083；2.山河智能裝備股份有限公司,湖南 長沙 410100)

0 引 言

如何規劃出一條無碰撞、滿足連續性約束且能保證輸出穩定性的航跡，實現無人機的自主巡航[1]，已成為無人機智能化的研究熱點。早期針對地面移動機器人開發的人工勢場、諧函數等矢量場方法近年來被不少研究人員應用于無人機規劃[2,3]，但難以避免陷入局部極小點以及曲線振蕩等問題。A*，D*等圖搜索算法在三維空間中計算復雜度會劇增。快速搜索隨機樹[4](rapidly-exploring random tree,RRT)算法通過隨機抽取一些采樣點來進行搜索，可以大大降低算法的時間和空間復雜度，在高維度空間中有著更明顯的優勢。為進一步提高RRT算法的效率，國內外學者在此基礎上有不同程度的改進，目標偏向RRT，RRT-connect等算法都具有不錯的效果[5～7]。

而RRT算法得到的路徑是由直線段依次連接而成的路徑，在轉折節點處不光滑，不利于后續的跟蹤控制。因而需要對折線路徑進行擬合平滑處理。常用的Dubin曲線不能保證加速度的連續性，螺旋曲線(clothoid curves)由于沒有解析解不適用于實時處理[8]，貝塞爾曲線構造簡單但受控制點的限制，靈活性差。B樣條曲線與控制點的數量獨立，可對各分段的曲線作局部修正，并保證各分段區間之間的連續性。

本文對于四旋翼無人機在分布大量障礙物的低空復雜環境下的航跡規劃問題，提出了一種對RRT-connect算法的改進方法，并采用B樣條函數進行后處理。最后，通過實驗驗證了該算法的有效性和穩定性。

1 路徑規劃

1.1 問題描述

定義C?Rd為規劃問題的d維位形空間(configuration space)，Cobs?C表示障礙物空間，Cfree=CCobs為自由空間。對于給定的初始位形qinit∈Cfree及目標位形qgoal∈Cfree，規劃問題可以描述為：找到一條連續無碰撞路徑σ:[0,1]→Cfree滿足σ(0)=qinit和σ(1)=qgoal。

四旋翼無人機是欠驅動系統，如果在其12維狀態空間中進行路徑規劃會造成問題求解的代價非常高。利用四旋翼飛行器[9]的微分平坦特性降低規劃空間的維數，可以大大降低規劃問題的復雜度。微分平坦系統是指系統的狀態和輸入都可以表示成平坦輸出及其有限階導數的函數[10]，本文選取平坦輸出為

ξ=[xyzψ]

式中x,y,z為飛行器的質心在慣性坐標系的坐標；ψ為偏航角。通過飛行器的平坦輸出可以唯一確定全部狀態和輸入[11]。為進一步簡化問題，本文選取[x,y,z]作為規劃空間，平坦輸出ψ則保持為arctan(y/x)。

1.2 RRT-connect算法步驟

1)分別以初始位形qinit和目標位形qgoal為根節點，初始化兩棵搜索樹Tinit和Tgoal。

2)在自由空間Cfree中抽取采樣點qrand，并在樹Tinit中搜索與qrand歐氏距離最小的節點qnear；在qnear與qrand的連線上，從qnear處以步長r截取節點qnew,1。若qnear～qnew,1之間均在空間Cfree中，則將節點qnew,1添加到樹Tinit中，否則重復本步驟。

3)在樹Tgoal中搜索與步驟(2)中qnew,1歐氏距離最小節點qnear，在qnear與qnew,1的連線上，從qnear處以步長r截取節點qnew,2，若qnear到qnew,2之間均在空間Cfree中，則將節點qnew,2添加到樹Tgoal中。重復本步驟，當兩棵樹相遇時終止程序并輸出路徑，遇到障礙物時進入下一步。

4)交換兩棵樹Tinit,Tgoal，返回執行步驟(2)。

2 RRT-connect算法的改進

2.1 規劃策略

從RRT-connect算法步驟可知：由于采樣點qrand的隨機分布，導致輸出路徑節點過于發散，路徑中包含有大量鋸齒狀的折線，路徑長度偏離最優值較遠。為提高路徑質量，有必要對算法加以改進:

1)轉角約束

在RRT-connect算法步驟(2)和步驟(3)中，通過查找與搜索樹中歐氏距離最小的節點qnear就可以進行后續的擴展。為了避免生成路徑中存在“打結”的現象，本文提出在擴展前施加一個轉角約束，即

(1)

2)路徑修剪

設RRT-connect算法輸出的路徑節點序列為P=[P1,P2,…,Pn]。如果對于節點對(Pi,Pi+2)是無碰撞的，則節點Pi+1就是冗余路徑節點，將其從節點序列P中刪除。在遍歷了整個路徑節點序列后，就可以去除冗余的節點使輸出路徑的質量得到大大改善。如圖1所示。

圖1 轉角約束和路徑修剪示意

2.2 平滑策略

改進后RRT-connect算法生成的路徑仍是折線，四旋翼飛行器在跟蹤所生成的路徑時需要在轉折點附近減速并完成轉向來減小軌跡跟蹤誤差[13]，這導致飛行效率大幅下降。因此，需要對路徑作平滑處理，以獲得幾何連續的軌跡。

2.2.1 B樣條曲線

在規劃空間[x,y,z]中，飛行器軌跡的各分量可以用一個含有n+1個控制點的k階B樣條曲線來表示，即

(2)

式中Bi,k(u)為基函數，Pi為第i個控制點向量。基函數表達式由de Boor-Cox遞推公式[14]給出

(3)

(4)

定義節點向量為U=[u0u1…um]，其中各元素保持單調不減(ui≤ui+1)，且m=k+n+1。本文取u∈[0,1]作為參數區間。

由上述遞推公式可知，基函數與節點向量各元素的分布以及曲線的階數有關，而與控制點獨立，這使得曲線具有良好的局部可修改性。

2.2.2 控制點調整策略

與文獻[11,15]中通過施加硬約束來保證分段多項式連續的平滑策略不同，本文將利用全部路徑節點作為控制點生成單條連續的參數化軌跡，可以預先求得解析解。為了使曲線經過指定的起點和終點，令節點向量U中兩端元素滿足k+1次重合度，并使得內部節點元素均勻分布，即

(5)

雖然B樣條曲線在控制點的作用下對路徑有較好的跟隨性，但仍然存在軌跡偏離路徑節點較遠的情況，特別是在控制邊較長及相鄰控制邊轉角較大時尤其明顯[16]，這大大增加了平滑后軌跡發生碰撞的概率。為此，本文在每條控制邊等分點處插入若干中間控制點，降低軌跡偏離原路徑的程度。在每次生成平滑曲線后都進行碰撞檢測，直到插入的控制點使曲線無碰撞為止。

如圖2所示，通過在每條控制邊中點處各插入一個控制點，使平滑后的軌跡更加貼合原始路徑，避免了生成的軌跡與障礙物間的干涉。

圖2 插入中間控制點前、后軌跡

3 算法驗證與分析

3.1 算法驗證

實驗中，將規劃空間限定為100 m×100 m×30 m的三維區域中，障礙物區域由若干個半徑為8 m的隨機分布的球形區域組成。設定規劃任務的起點坐標為(5,5,10)m，目標點坐標為(95,95,15)m，規劃步長r為3，B樣條曲線階數k為3。

圖3所示為某次規劃的結果。圖中兩個圓形標記分別表示起點和目標點，虛線路徑表示修剪前的路徑，點劃線為修剪后的路徑，實線為平滑后的最終軌跡。從圖3可以看出，規劃算法能找到一條連續路徑避開所有障礙區域，連接起點和目標點。

圖3 規劃結果

圖4(a)為插入中間控制節點后生成的平滑軌跡，與圖3(b)中實線路徑相對應，“+”標記的為新插入控制節點的位置。從圖中可以看出生成的B樣條軌跡貼近原始路徑，能避免與障礙物發生碰撞。圖4(b)為圖4(a)中軌跡對應的曲率隨參數u變化的曲線，可以看出最終軌跡的曲率是連續有界變化的，且在起始、目標位置曲率為0，這說明三階B樣條曲線能保證加速度的連續性，且是有界的。

圖4 B樣條曲線軌跡與對應參數曲率變化

3.2 性能分析

由于本文算法包含隨機性，為了分析其綜合性能及穩定性，以圖3(a)所示環境模型為基礎在MATLAB 2017上進行1 000次重復規劃實驗，并與標準RRT算法,RRT-connect算法及目標偏向RRT(RRT-biasing)算法規劃結果進行對比。4種規劃算法的運行時間、路徑長度分布如圖5所示，詳細統計數據見表1。

圖5 各算法性能對比

算法時間/s平均值標準差路徑長度/m平均值標準差本文算法0.02960.0115140.466.94RRT-connect0.02400.0116173.6813.36標準RRT0.50110.6367204.7227.67Biasing-RRT0.02950.0095167.6111.81

從圖5(a)中可以看出，本文算法的運行時間相較于標準RRT算法，分布更集中且均在0.1 s以內，可以滿足實時性要求。結合表1的數據，本文算法平均運行時間與RRT-connect算法相比慢0.005 6 s，與RRT-biasing算法相比大致持平，這是由于本文算法在RRT-connect算法基礎上增加了路徑修剪及平滑處理過程；從運行時間的標準差來看，本文算法與RRT-connect算法幾乎一致，這說明本文算法的附加處理過程對算法的穩定性影響十分微小。

如表1所示，本文算法路徑長度的平均值為140.46 m(十分接近直線距離127.37 m)，與RRT-connect算法、標準RRT算法及RRT-biasing算法相比分別縮短了19 %,31 %和16 %；在圖5(b)中可以看出，本文算法輸出路徑長度主要分布在150 m以內，少數離群異常點也都能維持在160 m左右。這說明本文的路徑修剪策略是有效的，與其他三種算法的結果相比有較大優勢。同時表1的標準差數據表明本文算法輸出路徑長度的穩定性也要優于其他三種算法。

4 結 論

在進行1 000次重復規劃實驗后，結果表明：本文算法運行時間較短具有實時性，路徑長度接近理論最優值，規劃質量得到提高。