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

王 康，郭劍東，桑 標(biāo)

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

1 引言

無人機因具有靈活輕便、機動性強、隱蔽性好等優(yōu)點，已經(jīng)廣泛應(yīng)用于民用和軍用領(lǐng)域[1]。但隨著無人機執(zhí)行任務(wù)和工作環(huán)境錯綜復(fù)雜復(fù)雜，自主避障技術(shù)成為無人機發(fā)展的關(guān)鍵技術(shù)之一。作為常用的無人機自主避障方法，無人機航路規(guī)劃方法受到廣泛關(guān)注。

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

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

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

2 航路規(guī)劃問題

2.1 任務(wù)空間建模

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

(1)

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

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

圖1 復(fù)雜環(huán)境下的任務(wù)空間模型

2.2 問題描述

本文將飛行任務(wù)空間中所有障礙物的表面與內(nèi)部均視為禁飛區(qū)或威脅區(qū)。假設(shè)任務(wù)空間為S，Sobs表示障礙物威脅區(qū)域，即?！?，Sfree表示可飛安全區(qū)域，即Γ>1，三者的關(guān)系為：

(2)

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

2.3 無人機性能約束

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

1)最大航程

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

(3)

2)最短航路段

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

li

(4)

3)最大轉(zhuǎn)彎角

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

(5)

4)最大爬升/俯沖角

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

(6)

5)最大飛行高度

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

Hi≤Hmax

(7)

3 基本RRT算法原理

圖2 基本RRT算法示意圖

4 綜合改進RRT算法

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

4.1 貪婪啟發(fā)式策略

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

p(Prand=Pgoal)=p1

(8)

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

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

4.2 動態(tài)引力步長策略

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

=Pnear+Pnode

(9)

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

(10)

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

(11)

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

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

(12)

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

4.3 平滑度優(yōu)化方法

圖3 航路平滑平面示意圖

當(dāng)?shù)玫揭?guī)劃航路后，從起點Pstart開始，按上述方法逐個節(jié)點遞推直到到達終點Pgoal。該平滑方法去除了航路中的冗余節(jié)點，不僅減小了航路長度，而且提高了航路平滑度。

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

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

5 數(shù)字仿真驗證

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

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

表1 試驗一仿真性能對比

圖5 試驗一對比仿真結(jié)果

從圖5可以看出，相比于啟發(fā)式RRT算法，貪婪啟發(fā)式RRT算法規(guī)劃出的航路直線段更多、更長，且當(dāng)?shù)浇K點的視線內(nèi)沒有障礙物時，不存在隨機節(jié)點導(dǎo)致航路不平滑的情況，因此整體航路更平滑。從表1可以看出，貪婪啟發(fā)式RRT算法在整體性能上都有提升，尤其在平均規(guī)劃時間上提升顯著。

試驗二為動態(tài)引力步長RRT算法與目標(biāo)引力RRT算法比較試驗。目標(biāo)引力RRT算法的終點方向擴展步長固定，引力系數(shù)kp=1。兩種算法的性能對比結(jié)果如表2所示，圖6為其中一次規(guī)劃結(jié)果。

表2 試驗二仿真性能對比

圖6 試驗二對比仿真結(jié)果

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

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

表3 試驗三仿真性能對比

圖7 試驗三對比仿真結(jié)果

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

圖8 航路平滑度優(yōu)化對比仿真結(jié)果

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

6 結(jié)論

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