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

黃肖文，任 彥

(內蒙古科技大學 信息工程學院 ，內蒙古 包頭 014000)

1 引言

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

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

2 RRT基本原理

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

2.1 隨機樹T的構建階段

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

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

新增節點的隨機增長函數為：

(1)

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

2.2 查詢階段

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

2.3 碰撞檢測

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

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

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

構造出的RRT算法的目標引力函數H(n)：

(2)

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

擴展節點knew的生長引導函數G(n)為：

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

(3)

則，新增節點的計算公式為：

knew=knear+G(n)=knew+

(4)

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

3.1 路徑平滑處理

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

3.2 算法步驟

其過程如下：

第一步：初始化步驟，對起始點kinit、目標點kgoal、步長ε、步長引導因子c進行初始化；

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

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

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

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

第六步：判斷是否到達目標點qgoal或目標區域Dgoal，若到達則繼續，否則，返回第二步;

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

4 仿真與實驗驗證

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

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

圖1 基本RRT算法

圖2 APF與RRT融合算法

圖3 路徑平滑后融合算法

5 結論

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