基于綜合改進RRT算法的無人機三維航路規劃

王 康，郭劍東，桑 標

(1.南京航空航天大學自動化學院，江蘇 南京 210016;2.南京航空航天大學中小型無人機先進技術工信部重點實驗室，江蘇 南京 210016)

1 引言

無人機因具有靈活輕便、機動性強、隱蔽性好等優點，已經廣泛應用于民用和軍用領域[1]。但隨著無人機執行任務和工作環境錯綜復雜復雜，自主避障技術成為無人機發展的關鍵技術之一。作為常用的無人機自主避障方法，無人機航路規劃方法受到廣泛關注。

無人機的航路規劃問題是指在一定區域范圍內、一定約束條件下，無人機具有自主搜索一條無障礙可行航路連接給定起始點和終點[2]。無人機航路規劃算法主要可以分為啟發式算法和非啟發式算法，啟發式算法有人工勢場法[3]、遺傳算法[4]、A*算法[5]等，非啟發式算法有維諾圖法[6]、快速探索隨機樹法[7-8](Rapidly-exploring Random Tree， RRT)等。文獻[9]綜合比較了以上算法，對比結果表明，人工勢場法收斂速度較快，但容易陷入局部最優；遺傳算法雖然從群體出發同時進行多次迭代，但隨著細胞數量的增加對時間的敏感度降低；A*算法主要通過犧牲收斂速度來提高最優性；維諾圖法一般需結合智能算法搜索最優路徑，收斂速度較慢。航路規劃算法通常要在收斂速度和最優性之間存在矛盾，因此，要根據實際情況選擇合適的算法。

RRT算法[7]由于采用增量方式增長、隨機采樣等特點，能夠有效解決復雜環境中障礙物帶來的航路搜索困難問題，其優勢在于無需對飛行任務環境進行幾何劃分，對任務空間的搜索范圍廣、覆蓋率高。但基本RRT算法節點搜索隨機性較強，導致航路搜索效率低，且搜索到的航路往往整體上較差。針對基本RRT算法的缺點，文獻[10]在基本RRT的基礎上提出RRT-Connect算法，將單向隨機搜索樹改進為雙向隨機搜索樹，提高了全局搜索能力和效率，但規劃航路平滑度仍然較差；文獻[11]提出RRT*算法，通過最優準則動態調整擴展隨機樹的父節點，提高了航路的漸進最優性，但是降低了收斂速度；文獻[12]將RRT-Connect與RRT*相結合有效地彌補了RRT*算法收斂速度較慢的問題。為提高RRT算法的收斂速度，改善規劃航路質量，也提出一些優化策略，如啟發式策略[13]和目標引力策略[14]等。

本文針對復雜飛行環境下基本RRT算法節點擴展隨機性強、航路搜索效率低、規劃航路曲折等問題提出了綜合改進方法。首先在啟發式引導策略的基礎上，加入貪婪策略，提高了節點向終點方向擴展的收斂速度；然后在目標引力策略的基礎上，提出動態引力步長策略，保證了節點搜索概率完備性的同時，降低了節點擴展方向的隨機性；最后設計了一種航路平滑度優化策略，提高了規劃航路的平滑度。仿真結果表明，本文提出的綜合改進RRT算法在復雜飛行環境下有較好的避障效果。

2 航路規劃問題

2.1 任務空間建模

無人機飛行任務環境中常常存在各種類型障礙物，如建筑物、山峰、樹林等，這些障礙物大多情況下都是不規則的，直接處理起來比較困難，過分關注障礙物的細節問題，也將大大增加了任務空間建模的困難程度，增加了航路規劃算法計算量。因此，本文根據障礙物信息特征簡化障礙物模型，選擇半球、圓柱、圓錐、圓臺等標準的凸多面體替代各類障礙物，以確保能覆蓋障礙物的整體或關鍵部分。它們的近似表達式可以統一為

(1)

其中，(x，y，z)為障礙物模型上任意一點，(x0，y0，z0)為障礙物模型的中心坐標，參數a，b，c和p，q，r決定障礙物模型的形狀和大小。Γ<1、Γ=1、Γ>1分別表示在障礙物模型內、表面、外。

通過對障礙物建模，可以將復雜環境中的障礙物用合適的一個或多個凸面體等效代替。復雜環境下任務空間模型如圖1所示，任務空間大小為10km×10km×3km，起點為(0，0，0)km，終點為(10，10，0.5)km。

圖1 復雜環境下的任務空間模型

2.2 問題描述

本文將飛行任務空間中所有障礙物的表面與內部均視為禁飛區或威脅區。假設任務空間為S，Sobs表示障礙物威脅區域，即Γ≤1，Sfree表示可飛安全區域，即Γ>1，三者的關系為：

(2)

起點Pstart∈Sfree，終點Pgoal∈Sfree，則航路規劃問題可以表示在任務空間S中搜索出從起點Pstart到終點Pgoal的可行航路。

2.3 無人機性能約束

無人機在飛行過程中除了受到環境約束外，還受到自身飛行性能約束，在進行航路規劃時，需充分考慮各類約束條件，主要有：

1)最大航程

假設一條完整航路由n條長度為li的航路段組成，最大飛行航程用Lmax表示，則最大航程約束可以表示為

(3)

2)最短航路段

為確保無人機在飛行姿態調整時的安全性，則需要一定的直飛距離，最短直飛航路段用lmin表示，則最短航路段約束表示為

li

(4)

3)最大轉彎角

ai和ai+1分別表示無人機第i段和i+1段航路的水平投影向量，最大轉彎角用φmax表示，則最大轉彎角約束表示為

(5)

4)最大爬升/俯沖角

(xi，yi，zi)和(xi+1，yi+1，zi+1)分別表示無人機航路上的第i和i+1個航點坐標，最大爬升/俯沖角用?max表示，則最大爬升/俯沖角約束表示為

(6)

5)最大飛行高度

假設最大飛行高度為Hmax，則無人機飛行過程中，實時高度Hi不能超過其最大飛行高度約束，即

Hi≤Hmax

(7)

3 基本RRT算法原理

圖2 基本RRT算法示意圖

4 綜合改進RRT算法

基本RRT算法在隨機樹構造過程中，節點選擇隨機性太強，會產生很多冗余節點，增加了航路的搜索時間，且航路整體上平滑度較差。因此RRT算法在隨機樹構造中既要滿足節點有一定的隨機性，保證了復雜任務空間中的搜索能力；也要盡可能滿足隨機樹朝終點方向生長，提高收斂速度和航路平滑度。故本文提出以下綜合改進措施。

4.1 貪婪啟發式策略

為降低基本RRT算法節點搜索的隨機性，文獻[13]提出了一種基于概率的啟發式引導策略，即通過選擇一個合適的概率p1∈(0，1)，令隨機目標點Prand等于終點Pgoal的概率為p1，即

p(Prand=Pgoal)=p1

(8)

當隨機目標點Prand以概率p1等于終點Pgoal時，相當于目標最優搜索，此時為了盡可能讓節點朝著終點方向擴展，本文在終點方向的節點擴展上加入貪婪策略，即一直沿著隨機目標點Prand與終點Pgoal連線方向上擴展，直至遇到障礙物，再進行下一隨機選取目標點。

通過在啟發式引導策略上引入貪婪策略，減少了目標最優搜索時節點的選擇，不僅提高了局部收斂速度，也讓搜索航路盡可能的為直線，提高了航路平滑度。

4.2 動態引力步長策略

貪婪啟發式策略提高了節點朝終點方向擴展速度，但并沒有考慮隨機節點的選擇。文獻[14]在隨機節點的選擇上提出目標引力策略，該策略的思想是通過將隨機目標點Prand與終點Pgoal經過一定的組合得到新的目標點Pnode，則新的擴展節點Pnew的計算公式為

=Pnear+Pnode

(9)

其中，ρ1為隨機目標點方向擴展步長，ρ2為終點方向擴展步長。ρ1、ρ2的大小選擇直接決定了新節點的擴展方向和步長，故ρ1、ρ2的選擇尤為重要。但文獻[14]并沒有給出不同任務空間下步長ρ1、ρ2的選擇方法。本文根據節點Pnear到障礙物的距離來確定ρ1、ρ2的大小，得到ρ1、ρ2的表達式為

(10)

其中，ρ為固定步長，kp為引力系數，其表達式為

(11)

其中，(xi，yi)表示第i個障礙物圓心坐標，N表示障礙物個數，Ri，z表示節點Pnear高度下障礙物的半徑，W為調節因子，其大小由無人機飛行環境復雜程度決定。當節點Pnear與障礙物距離較遠時，kp>1，ρ2>ρ1，新節點Pnew會盡可能地沿著終點Pgoal方向擴展；反之，當節點Pnear與障礙物距離較近時，kp<1，ρ1>ρ2，新節點Pnew會盡可能地沿著隨機點Prand方向擴展。通過根據障礙物模型設計動態引力步長策略，不僅保證了復雜環境下航路搜索概率的完備性，而且盡可能降低了節點的隨機性，提高了航路的整體品質。

由于擴展步長過長，在復雜環境下很難通過障礙物威脅檢測，影響收斂速度；擴展步長過小，則會降低隨機樹生長速度和航路平滑度。本文對新節點的擴展步長大小做出如下約束

(12)

其中，ρmin、ρmax為根據無人機性能特性和飛行環境復雜程度確定的最小、最大步長。

4.3 平滑度優化方法

圖3 航路平滑平面示意圖

當得到規劃航路后，從起點Pstart開始，按上述方法逐個節點遞推直到到達終點Pgoal。該平滑方法去除了航路中的冗余節點，不僅減小了航路長度，而且提高了航路平滑度。

綜合改進RRT算法流程如圖4所示。

圖4 綜合RRT改進算法流程圖

5 數字仿真驗證

為驗證綜合改進RRT算法在復雜環境下三維航路規劃性能，在MATLAB R2017a中進行仿真驗證，計算機處理器為Inter Core i5，主頻2.5GHz。仿真中，節點固定搜索步長ρ=1km，最大搜索步長ρmax=1.5km，最小搜索步長ρmin=0.5km。通過仿真100次，比較平均規劃時間、平均航路長度和平均節點個數三個參數驗證算法性能。

試驗一為改進貪婪啟發式RRT算法與啟發式RRT算法比較試驗。兩種算法的性能對比結果如表1所示，圖5為其中一次規劃結果，其中“·”表示節點。

表1 試驗一仿真性能對比

圖5 試驗一對比仿真結果

從圖5可以看出，相比于啟發式RRT算法，貪婪啟發式RRT算法規劃出的航路直線段更多、更長，且當到終點的視線內沒有障礙物時，不存在隨機節點導致航路不平滑的情況，因此整體航路更平滑。從表1可以看出，貪婪啟發式RRT算法在整體性能上都有提升，尤其在平均規劃時間上提升顯著。

試驗二為動態引力步長RRT算法與目標引力RRT算法比較試驗。目標引力RRT算法的終點方向擴展步長固定，引力系數kp=1。兩種算法的性能對比結果如表2所示，圖6為其中一次規劃結果。

表2 試驗二仿真性能對比

圖6 試驗二對比仿真結果

從圖6可以看出，相比于目標引力RRT算法，動態引力步長RRT算法在節點附近障礙物較少時，節點傾向于朝著終點方向擴展，整體航路更優。從表2可以看出，動態引力步長RRT算法整體性能也都有提升，尤其在平均節點個數和平均規劃時間上提升顯著算法

試驗三為綜合改進RRT與貪婪啟發式RRT算法和動態引力步長RRT算法比較試驗。三種算法的性能對比結果如表3所示，圖7為其中一次規劃結果。

表3 試驗三仿真性能對比

圖7 試驗三對比仿真結果

從圖7可以看出，貪婪啟發式RRT算法規劃出的航路雖然存在較長的直線段，但可能由于隨機點選擇問題導致出現較大的偏差和不合理的折線，整體航路較差；動態引力步長RRT算法雖然約束了隨機點的選擇，航路朝著終點方向擴展，但多次隨機點的選擇導致航路出現不必要的轉向，降低了航路平滑度。相比與以上兩種算法，綜合改進RRT算法結合了兩者的優勢，規劃出的航路不僅朝著終點方向擴展，且航路直線段更多，沒有較大的轉彎角度，航路整體更平滑。從表3可以看出，綜合改進RRT算法結合了貪婪啟發式RRT算法規劃時間快、節點個數少和動態引力步長RRT算法航路長度短的優點，收斂速度更快、規劃航路更短，也更平滑。

圖8 航路平滑度優化對比仿真結果

圖8為航路平滑度優化仿真結果對比圖，從圖中可以看出，平滑度優化方法有效地去除原規劃航路中的冗余節點。且平滑后的航路節點個數從原本的14個降低到4個，航路長度更短，縮短了17.98%，航路轉彎角度更小，整體上更平滑，有利于無人機避障與跟蹤飛行。

6 結論

本文提出了一種綜合改進RRT航路規劃算法，針對復雜飛行環境下無人機的三維航路規劃問題開展研究。提出貪婪啟發式策略，以一定概率引導節點朝終點方向擴展，并提高終點擴展方向的收斂速度；提出動態引力步長策略，保證節點搜索概率完備性的同時，降低節點搜索的隨機性；設計平滑度優化方法，出去航路冗余節點，提高航路平滑度。仿真結果表明，本文提出的綜合改進RRT算法在復雜環境下能有效提高航路規劃的收斂速度、縮短航路長度和提高航路平滑度。本文還考慮了無人機自身的限制和運動學特性，在實際工程中有較好的實用性。