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

楊登紅 宋宏銳 文 淵

貴州理工學(xué)院航空航天工程學(xué)院 貴州 貴陽 550003

0 引言

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

1 人工勢場法

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

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

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

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

根據(jù)傳統(tǒng)人工勢場法的數(shù)學(xué)模型[5]，利用Matlab進(jìn)行仿真，得到傳統(tǒng)人工勢場法航跡規(guī)劃結(jié)果如圖2所示，其中藍(lán)色的軌跡是算法輸出的路徑，白色區(qū)域代表障礙物，黑色區(qū)域為可行域，起點為點，目標(biāo)航路點為Pwp。

圖2 傳統(tǒng)人工勢場法局部最小值仿真

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

2 RRT快速拓展隨機(jī)樹

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

第一步：以起始點xroot作為隨機(jī)樹的初始根節(jié)點；

第二步：根節(jié)點xroot在狀態(tài)空間中的隨機(jī)方向中搜索采樣點xrand，搜索并連接離xroot最近的隨機(jī)采樣點，以規(guī)定的步長Lengt h進(jìn)行截取并生成子節(jié)點xnode；

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

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

圖3 RRT快速隨機(jī)拓展樹節(jié)點拓展

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

圖4 RRT航跡規(guī)劃仿真結(jié)果

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

2.3 主要參數(shù)對RRT算法的影響

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

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

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

圖5 搜索步長變化仿真結(jié)果

圖6 分段數(shù)量R p 對隨機(jī)樹拓展的影響

3 對比分析

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

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

圖7 同一實際環(huán)境中的航跡規(guī)劃算法仿真

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

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

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

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

4結(jié)論

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