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

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

(1.南京航空航天大學(xué)自動(dòng)化學(xué)院，江蘇 南京 210016;2.南京航空航天大學(xué)中小型無人機(jī)先進(jìn)技術(shù)工信部重點(diǎn)實(shí)驗(yàn)室，江蘇 南京 210016)

1 引言

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

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

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

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

2 航路規(guī)劃問題

2.1 任務(wù)空間建模

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

(1)

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

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

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

2.2 問題描述

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

(2)

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

2.3 無人機(jī)性能約束

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

1)最大航程

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

(3)

2)最短航路段

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

li

(4)

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

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

(5)

4)最大爬升/俯沖角

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

(6)

5)最大飛行高度

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

Hi≤Hmax

(7)

3 基本RRT算法原理

圖2 基本RRT算法示意圖

4 綜合改進(jìn)RRT算法

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

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

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

p(Prand=Pgoal)=p1

(8)

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

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

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

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

=Pnear+Pnode

(9)

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

(10)

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

(11)

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

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

(12)

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

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

圖3 航路平滑平面示意圖

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

綜合改進(jìn)RRT算法流程如圖4所示。

圖4 綜合RRT改進(jìn)算法流程圖

5 數(shù)字仿真驗(yàn)證

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

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

表1 試驗(yàn)一仿真性能對比

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

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

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

表2 試驗(yàn)二仿真性能對比

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

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

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

表3 試驗(yàn)三仿真性能對比

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

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

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

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

6 結(jié)論

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