基于人工勢場和RRT的四旋翼無人機航跡規劃算法研究

楊登紅 宋宏銳 文 淵

貴州理工學院航空航天工程學院 貴州 貴陽 550003

0 引言

隨著科學技術的不斷發展，四旋翼無人機的功能和性能得以不斷提升。根據載荷不同，四旋翼無人機可以實現不同功能，現已在航拍、測繪、巡線、植保、播種等領域不斷取得實質性應用。然而，因四旋翼無人機的低空飛行特性，四旋翼無人機在植保、播種等領域應用時易受現場環境影響，如山地、樹木、房屋等障礙物，都直接威脅四旋翼無人機飛行安全，需要四旋翼無人機具備探測障礙物并進行實時航跡規劃的能力，避免撞擊障礙物出現墜機事故。目前，已有針對無人機航跡規劃的研究，但對四旋翼無人機航跡規劃算法研究較少。因此，本文對傳統人工勢場法和RRT快速拓展隨機樹算法應用于四旋翼無人機航跡規劃進行研究，通過模擬實際校園環境作為航跡規劃及避障的仿真環境，對兩種算法進行仿真分析，得出兩種算法應用于四旋翼無人機航跡規劃和避障的特性及性能，對后續研究具有一定的參考價值。

1 人工勢場法

1.1 人工勢場法原理 人工勢場法是由Khatib提出的一種虛擬力法并成功用這種方法實現了機械臂進行避障[1]，其原理如圖1所示。

圖1 人工勢場法原理示意圖[2]

由圖1(a)可知，人工勢場法原理是將四旋翼無人機的飛行環境視為一種虛擬的人工受力場[3]，然后將四旋翼無人機P的運動視為場內運動。由圖1(b)可知，障礙物Po對四旋翼產生斥力Fc，目標點Pwp對四旋翼無人機產生引力Fy，引力和斥力的合力Fh作為四旋翼無人機的加速力，進而控制四旋翼無人機的運動方向從而實現避障。由于人工勢場法有良好的實時性，所以很適合四旋翼無人機在動態或者靜態的障礙物環境中進行航跡規劃。

1.2 人工勢場法航跡規劃仿真 傳統人工勢場法的算法模型相對簡單，在四旋翼無人機的飛行環境中無需過多地考慮環境因素或者相對復雜的碰撞算法，可直接在規劃空間中進行航跡規劃，具有算法運算量少、計算效率高，實時性強等優點[4]。但同時，傳統人工勢場法的缺點的產生正是因其算法設計簡單，沒有考慮到飛行環境中的復雜情況，因此在復雜的飛行環境中，算法很可能會陷入到其缺陷中，從而導致航跡規劃失敗。

根據傳統人工勢場法的數學模型[5]，利用Matlab進行仿真，得到傳統人工勢場法航跡規劃結果如圖2所示，其中藍色的軌跡是算法輸出的路徑，白色區域代表障礙物，黑色區域為可行域，起點為點，目標航路點為Pwp。

圖2 傳統人工勢場法局部最小值仿真

由圖可知，當目標航路點與障礙物相隔太近時，四旋翼無人機受障礙物周圍的斥力場影響而有很大可能導致無法達到目標航路點的情況。另外，當四旋翼無人機在復雜的飛行環境中執行任務時，遇到多個障礙物則也會導致局部最小值的情況等[6]。

2 RRT快速拓展隨機樹

2.1 RRT的算法模型 RRT快速擴展隨機樹算法是由La Valle教授在1998年提出[7]的，其思想是效仿樹枝生長的隨機性，快速拓展至復雜空間的所有區域，伺機找到可行的路徑，具有概率的完備性。RRT算法主要以避障策略為主，其原理并不復雜，因此算法輸出實時性高，適合四旋翼無人機避障應用。RRT隨機樹節點拓展過程如圖3所示，其主要流程如下：

第一步：以起始點xroot作為隨機樹的初始根節點；

第二步：根節點xroot在狀態空間中的隨機方向中搜索采樣點xrand，搜索并連接離xroot最近的隨機采樣點，以規定的步長Lengt h進行截取并生成子節點xnode；

第三步：將根節點與子節點相連就得到一條無碰撞路徑段。在隨機節點的生長方向中，若分布在障礙物內，就不會在此方向連接并刪除該節點；

第四步：此時返回到第二步，直到將四旋翼無人機到目標航路點xgoal被隨機樹尋到。此時由xgoal回溯尋路，逐步尋到根節點后并標記即可獲取一條無碰撞的航跡。

圖3 RRT快速隨機拓展樹節點拓展

2.2 RRT航跡規劃算法仿真分析 根據RRT快速拓展隨機樹的算法流程以及實現算法的數學模型[8]，利用Matlab模擬具有不同障礙物的四旋翼無人機飛行環境，通過Matlab進行仿真，得到RRT 航跡規劃仿真結果如圖4所示。

圖4 RRT航跡規劃仿真結果

圖4(a)與圖4(b)所示航跡的終點坐標均為(486，468，488)，前者隨機生成的節點為N=987個，搜索時間為t=34s，后者隨機生成N=46個節點，搜索時間為t=6s。圖4(c)所示航跡的終點坐標為(50，581，310)，隨機生成的節點為N=103個，搜索時間為t=10s。圖4(d)所示航跡的終點坐標為(0，400，400)，即在Y和Z軸組成的平面內，隨機生成的節點為N=1603個，搜索時間為t=54s。由此看出，本文所述RRT 算法能快速找到可行航跡，生成航跡的隨機性較大，但從概率的角度看，RRT 算法具有概率完備性，不會陷入類似人工勢場法等其他航跡規劃算法的局部最小值問題。

2.3 主要參數對RRT算法的影響

2.3.1 搜索步長對算法的影響 搜索步長是RRT算法的重要參數之一，若搜索步長過大則導致航跡折線角度過小，會使航跡與障礙物的安全距離縮小。設定搜索步長Length=100 m 和Length=1 m，通過Matlab進行仿真，得到仿真結果如圖5所示。由圖可以看出，當Length=100 m 時，雖然搜索時間僅為t=0.8s，但航跡規劃質量不佳，四旋翼無人機進行巡航會有較大的概率避障失敗。反之，當Length=1 m 時，其搜索時間為t=323s。因此，若RRT搜索步長過小，則搜索時間增加，搜索效率大大降低，不適用于四旋翼無人機實時避障，即使航跡相對平滑，發生避障失敗的概率也會增大。

綜上所述，經多次試驗，在(500 m×500 m×500 m)的三維環境中，RRT快速隨機拓展樹的最佳搜索步長設置范圍為30-40m。

2.3.2 碰撞檢測算法中的分段值 碰撞檢測算法是實現RRT算法的重要避障數學模型，其主要采用分段法進行障礙物規避，分段數量Rp直接影響RRT檢測障礙物和隨機樹的節點生成情況，分段數量Rp對隨機樹拓展的影響如圖6所示。如果Rp增大，則計算量增大，算法效率會降低。反之，如果Rp減小，當遇到小型障礙物時，如果步長Length大于障礙物尺寸，則有一定概率會出現新節點與鄰近點生成的樹枝穿過障礙物，四旋翼無人機按照此航跡飛行避障失敗的概率必然增大。

圖5 搜索步長變化仿真結果

圖6 分段數量R p 對隨機樹拓展的影響

3 對比分析

在航跡規劃仿真過程中，因二維環境比三維環境可以更直觀地驗證算法的航跡規劃效率，在仿真時可側重考慮四旋翼無人機在水平方向的航跡規劃效果。為了能夠直觀地比較兩個算法的特性及性能，本文將人工勢場法和RRT算法在相同二維飛行環境中進行對比仿真，便于觀察不同算法的航跡規劃效果。

本文主要通過算法的航跡規劃效果、航跡規劃時間和航跡規劃效率來進行對比分析。先根據學校教學樓分布情況，通過Matlab模擬四旋翼無人機的飛行環境，在該模擬環境中分別基于人工勢場法和RRT算法進行航跡規劃仿真，得到航跡規劃結果如圖7所示。根據教學樓分布構造的飛行環境大小為(500 m×500 m)，四旋翼無人機起飛點坐標均為xroot(388，120)，飛行終點坐標均為xgoal(133，434)。

圖7 同一實際環境中的航跡規劃算法仿真

人工勢場法航跡規劃效果如圖7(a)所示，由圖可知，在四旋翼無人機接近目標航路點時，由于附近的障礙物比較多并且可行空間比較小，可能導致吸引和排斥勢場力大小相等，從而使得算法規劃的航跡陷入局部最小值狀態，導致四旋翼無人機在飛行中無法找到可行航跡。RRT 快速拓展隨機樹航跡規劃如圖7(b)所示，綠色折線是RRT 算法標記的航跡，后經過貝塞爾曲線優化后所得到的紅色曲線是可行航跡，該航跡平滑比較平滑，避障效果較好。

表1 人工勢場法與RRT算法的性能比較

RRT算法/3 404 501 34 626 10 RRT算法/4 404 508 56 1086 10 RRT算法/5 404 499 43 991 10

人工勢場法與RRT算法的航跡規劃性能如表1所示。由表1可知，在同一飛行環境中，人工勢場法陷入局部循環無法搜索到可行航跡，但RRT算法5次運行均能搜索到可行航跡，經過貝塞爾曲線優化后，能夠與四旋翼無人機的飛行性能配合實時避開障礙物。因此，在相對復雜的環境中，如果使用人工勢場法，則會導致飛行環境引力與斥力場相對紊亂，從而增加導致算法規劃航跡失敗的概率，但由于RRT 算法具有隨機性，在復雜環境中體現出了全概率規劃出可行航跡的特性，具有更好的航跡規劃性能。

4結論

本文針對四旋翼無人機航跡規劃與避障問題，對人工勢場法和RRT快速拓展隨機樹算法進行研究，并根據學校教學樓分布模擬實際飛行環境對航跡規劃效果進行仿真驗證。結果表明，四旋翼無人機在障礙物較多、可行空間較小等情況下，人工勢場法可能因吸引和排斥勢場力大小相等，從而使得算法陷入局部最小值狀態，導致四旋翼無人機在飛行中無法找到可行航跡，但由于RRT算法具有隨機性，在復雜環境中體現出了全概率規劃出可行航跡的特性，具有更好的航跡規劃性能，在實際航跡規劃中具有更大的應用前景。