基于APF-RRT融合算法的無人機航跡規(guī)劃研究

黃肖文，任 彥

(內(nèi)蒙古科技大學(xué) 信息工程學(xué)院 ，內(nèi)蒙古 包頭 014000)

1 引言

無人機航跡規(guī)劃是指在給定起始點和目標(biāo)點位置，并綜合考慮無人機的飛行約束條件下，尋找一條從起始點到目標(biāo)點的最優(yōu)或可行航跡，以保證無人機能夠完成飛行任務(wù)[1]。目前航跡規(guī)劃的方法有主要是RRT算法、A*算法、模糊邏輯算法、遺傳算法、人工勢場法和蟻群算法等。RRT算法是基于采樣的單一的查詢路徑規(guī)劃算法[2]。其算法的本質(zhì)是一種隨機生成的數(shù)據(jù)結(jié)構(gòu)。航跡規(guī)劃中對狀態(tài)空間的建模是一大難題，既能用于完整系統(tǒng)的機械臂路徑規(guī)劃，也能添加約束條件用于非完整系統(tǒng)的輪式機器人或無人機尋路等優(yōu)點，為高維復(fù)雜環(huán)境的機器人路徑規(guī)劃開辟了一種新的解決方案[3]。由于RRT算法的隨機性，一方面它使得RRT算法具有較強的探索能力，但另一方面它將會導(dǎo)致航跡規(guī)劃的盲目性。同時規(guī)劃出的路徑會出現(xiàn)較多的冗余點，導(dǎo)致路徑長度增大，轉(zhuǎn)角增多。目前針對這些問題，學(xué)者們提出了一些關(guān)于改進RRT算法的策略。Kuffner JJ等提出了RRT-Connect算法，基于該方面的改進算法一般包括兩個想法：Connect啟發(fā)式算法和雙向搜索技術(shù)[6],在一定程度上提高了收斂速度，但未考慮運動學(xué)約束；劉曉倩等提出了目標(biāo)偏向策略，減少了葉結(jié)點的樹木，提高了搜索效率，但增大了算法運行的時間[8]。王全等人提出了平滑的拼接算法，通過優(yōu)化隨機間的連接方式，在滿足運動系統(tǒng)的約束條件下，生成了相應(yīng)的圓弧路徑，但其僅適用于輪式機器人的路徑規(guī)劃中[10].上式算法都是在增大算法的復(fù)雜度和運行時間的基礎(chǔ)上，規(guī)劃出相對較優(yōu)的路徑。

本文提出一種基于人工勢場(APF)和RRT算法上的融合算法，在勢場法中，勢函數(shù)同時具有障礙物和目標(biāo)雙方的信息，如果將這種信息傳達給RRT，將隨機樹在擴展的過程傾向沿著勢場的方向進行擴展，減小了搜索的盲目性，同時彌補了勢場法容易陷入局部極小值的缺陷。采用貪心策略結(jié)合剪枝思想，對規(guī)劃出的初始航跡進行平滑處理。同時解決了搜索效率低和航跡冗余度大的問題。

2 RRT基本原理

RRT的基本思想是以隨機采樣的方式，不間斷地搜索系統(tǒng)的狀態(tài)空間，首先建立出擴展搜索樹，其次根據(jù)擴展搜索樹反向查詢出一條包含有初始點到目標(biāo)點的有效路徑。算法以路徑初始起點Kinit作為隨機樹T的根節(jié)點，kgoal作為路徑規(guī)劃的目標(biāo)點，同時也是航跡規(guī)劃的終點。擴展樹中從初始狀態(tài)點到目標(biāo)狀態(tài)點的樹枝路徑就是規(guī)劃出的路徑。核心思想是以隨機采樣的方式，不間斷地對未知的系統(tǒng)狀態(tài)空間進行搜索[7]。

2.1 隨機樹T的構(gòu)建階段

由于無人機在飛行過程中高度一般保持不變，為了簡化環(huán)境建模的難度，本文假設(shè)無人機所處的運動狀態(tài)空間為二維空間，并用D表示，D∈R3。將無人機的運動狀態(tài)空間分為兩部分，約束部分和自由部分。將自由部分定義為Dfree，包含所有無人機不與任何障礙物發(fā)生碰撞的自由狀態(tài)空間?？焖贁U展隨機樹只能在自由狀態(tài)空間內(nèi)進行擴展，以此來保證無人機的避障性。無人機的碰撞算法是通過檢測狀態(tài)點k是否滿足q∈Dfree，以此來判定所選取的狀態(tài)點是否可添加至快速擴展隨機樹上。

將所擴建的隨機樹記為Sk，為保證所規(guī)劃出的路徑均為無碰撞路徑，滿足Sk∈Dfree。其中是擴展隨機樹的節(jié)點，kinit為初始點，kgoal為目標(biāo)點，knew為新增節(jié)點。Dgoal?D為目標(biāo)區(qū)域。

新增節(jié)點的隨機增長函數(shù)為：

(1)

式(1)中，ε為擴展隨機樹的步長。

2.2 查詢階段

路徑查詢階段就是從目標(biāo)節(jié)點kgoal出發(fā)，依次尋找父節(jié)點，直至到達起始節(jié)點kinit，即生成了一條從起始節(jié)點到目標(biāo)節(jié)點的無碰撞路徑，則最終生成了一條包含目標(biāo)節(jié)點的搜索樹[8]。

2.3 碰撞檢測

在隨機樹的構(gòu)建當(dāng)中，為了檢測擴展點是否位于自由狀態(tài)空間Dfree中，引入增量檢測策略，即在knew和krand連線上，插值出多個等距離的點，運用迭代的計算方式對這些點作碰撞檢測，若全部點均不存在障礙物，則可將qnew加入到擴展樹上，否則，則說明兩點間有障礙物存在，放棄qnew。本文中插入8個等距離點。

3 RRT算法與APF算法融合策略

RRT算法的最大優(yōu)點是搜索能力強，但由于搜索隨機性大導(dǎo)致效率低。本文主要利用APF的目標(biāo)導(dǎo)向作用，以此來提高RRT算法的搜索效率。在RRT算法的搜索樹的擴展中加入勢場法的目標(biāo)引力思想，針對第m個擴展節(jié)點knew，構(gòu)造目標(biāo)引力函數(shù)H(n)，使得隨機樹以目標(biāo)勢場的引力作用下，以一定的趨勢朝著目標(biāo)點進行生長。從而達到降低搜索無效性，提高搜索效率，加快了規(guī)劃的收斂速度。

構(gòu)造出的RRT算法的目標(biāo)引力函數(shù)H(n)：

(2)

式(2)中，C為引力因子，可調(diào)節(jié)C的取值來改變隨機樹的目標(biāo)偏向性。

擴展節(jié)點knew的生長引導(dǎo)函數(shù)G(n)為：

G(n)=F(n)+H(n)

(3)

則，新增節(jié)點的計算公式為：

knew=knear+G(n)=knew+

(4)

由式(4),可以看出knew的計算包含兩個部分，即隨機采樣點krand的影響，目標(biāo)點kgoal的引力吸引。

3.1 路徑平滑處理

由于RRT算法本身的隨機性，很容易在選取節(jié)點時選出很多沒有必要的節(jié)點[8]。即使引入了目標(biāo)引力作用，規(guī)劃出的路徑也會發(fā)生震蕩的情況，存在大量的冗余節(jié)點。對規(guī)劃出的初始路徑進行剪枝處理，可以減少無人機不必要的轉(zhuǎn)向和機械損耗。本文采用貪心策略進行路徑剪枝，貪心策略是指在問題的求解過程中，僅僅考慮在某種狀態(tài)下的最優(yōu)解。具體步驟是在規(guī)劃出基礎(chǔ)路徑所形成的隊形序列中basepath(kinit,k1,…,kgoal)，運用貪心策略，即用k0連接kinit,k1直到kgoal，直到碰到障礙物停止，此時假設(shè)連到kn，碰到障礙物，將kn存入數(shù)組T中；從kn開始連接kn+1,kn+2直到kgoal，直到kn=kgoal停止。根據(jù)對數(shù)組存入的節(jié)點依次進行連接，生成了新的規(guī)劃路徑。

3.2 算法步驟

其過程如下：

第一步：初始化步驟，對起始點kinit、目標(biāo)點kgoal、步長ε、步長引導(dǎo)因子c進行初始化；

第二步：隨機采樣，在自由空間Dfree中通過隨機采樣，選取出krand，krand∈Dfree,krand?Sk；

第三步：計算最近點knear，在隨機樹Sk中選取出與krand度量最近的點knear。滿足幾何關(guān)系：Dis(knear,krand)≤Dis(k,krand)；

第四步：計算新擴展節(jié)點knew，在方向，截取選出knew，滿足Dis(knear,knew)=ε；

第五步：碰撞檢測，通過增量方法，確保knew∈Dfree。即判斷krand和knear的連線上，是否有障礙物的存在。若滿足，則繼續(xù)，否則，返回第二步；

第六步：判斷是否到達目標(biāo)點qgoal或目標(biāo)區(qū)域Dgoal，若到達則繼續(xù)，否則，返回第二步;

第七步：使用貪心策略，進行路徑平滑處理。

4 仿真與實驗驗證

仿真平臺為MATLAB R2012a.仿真環(huán)境的空間尺寸為800×600(mm×mm),障礙帶為黑色填充實線條。設(shè)定無人機的起始點kinit坐標(biāo)為(210,190)，目標(biāo)點坐標(biāo)為(420,610)。實驗中選取步長ε為30，最大迭代次數(shù)l=10000，步長引導(dǎo)因子c=0.2。如圖1為基本RRT算法生成的路徑，圖2為APF與RRT融合算法生成的路徑，圖3為平滑后的融合算法在實驗環(huán)境中的路徑規(guī)劃效果圖。用藍色*表示起始點，黃色*表示目標(biāo)點，紅色曲線表示規(guī)劃出的路徑。圖3中的綠色細曲線表示平滑完的規(guī)劃航跡。由圖3可以看出，平滑過的路徑節(jié)點數(shù)明顯減少，長度明顯縮短。由表1可以看出采用ARF-RRT融合算法明顯提高了規(guī)劃速度，加入航跡平滑后，路徑長度明顯得到了縮短。

表1 各方法生成航跡長度與規(guī)劃時間對比

圖1 基本RRT算法

圖2 APF與RRT融合算法

圖3 路徑平滑后融合算法

5 結(jié)論

本文提出人工勢場算法與隨機擴展樹法的融合策略，既解決了RRT算法無目標(biāo)導(dǎo)向的問題，又彌補了APF算法容易陷入局部最小值的缺陷。同時引入貪心策略和剪枝思想，對規(guī)劃的初始路徑進行平滑。仿真實驗表明：航跡規(guī)劃的時間和長度明顯縮短，航跡多余的冗余節(jié)點也得到了去除。