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

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

0 引言

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

1 人工勢(shì)場(chǎng)法

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

圖1 人工勢(shì)場(chǎng)法原理示意圖[2]

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

3 對(duì)比分析

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

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

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

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

表1 人工勢(shì)場(chǎng)法與RRT 算法的性能比較

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

4 結(jié)論

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