基于彈性繩算法的無人機路徑規劃方法

田景凡 王彥愷

（北京理工大學宇航學院，北京 100081）

1 引言

無人機路徑規劃是無人機任務設計與規劃的重要環節，是實現無人機自主飛行與任務執行的重要階段。路徑規劃問題是指在已知環境信息的任務場景中，預先規劃出從各個無人機起始點到對應目標點，可以繞過途中各種威脅區和障礙物，安全、可靠、無相互碰撞，且同時滿足無人機自身約束條件的可飛行路徑，是任務規劃系統研究的難點之一。

一般情況下，無人機任務路徑規劃需要滿足的原則如下：（1）任務規劃應該得到一條從任務起點到任務終點的路徑，不能存在規劃失敗。（2）無人機在向目標運動的過程中需要保證安全，不碰到障礙物或者經過危險區域。（3）規劃的路徑應盡量優化，滿足規劃階段對路徑長度的要求。

無人機路徑規劃的實現主要包括以下兩個步驟：（1）環境建模。根據無人機所處的實際環境，在仿真過程中建立合理的模型，將實際的物理空間抽象成為能用數學模型求解的虛擬空間。（2）路徑搜索。在完成環境建模之后，根據任務條件規劃出一條從起點到終點的任務路徑，在滿足任務條件約束的同時使路徑長度最短。

無人機路徑規劃問題一直是國內外學者研究的熱點問題。二維空間路徑規劃的研究方法很多，包括A*算法、人工勢場法、進化算法、蟻群算法和粒子群優化等。至于三維空間，情況會變得更加復雜，二維空間中的許多算法不能直接應用于三維空間。三維空間中常用的路徑規劃算法包括稀疏A*算法、快速隨機樹法和彈性繩算法等。

不同文章中對彈性繩的定義和計算方法有所不同，但本質上都是通過彈性繩來優化和選擇路徑的。本文將彈性繩定義為連接起點和目標點的一組節點，通過節點與障礙物的碰撞和滑動，可以得到空間中最短的路徑。利用串聯的彈簧組成彈性帶的方法建立連接車輛和目標點的行駛路徑，彈性帶在彈力平衡的作用下引導車輛的避障和超車行為，就是典型的彈性繩算法。

2 無人機路徑規劃框架

無人機路徑規劃問題需要綜合考慮地形、威脅等因素，結合無人機自身飛行性能，規劃出從起點到終點的最優飛行路徑。假設無人機i在三維空間中的位置為Pi，則無人機路徑的一組節點序列為{Si,Pi1,Pi2,…,Pij,…,Ti}，i∈ [1,N]，其中 Si為起始點，Ti為目標點。

在無人機路徑規劃問題中，應考慮無人機在三維空間中的運動，假設無人機為空間中的一個質點，將無人機的運動學模型簡化為：

式中，(x,y,z)為無人機的空間位置；θ為無人機的航向角；γ為無人機的俯仰角；aN為無人機的法向加速度；V為無人機速度且為定值。

3 無人機路徑規劃算法設計

彈性繩算法是通過對彈性繩收縮運動的簡化模擬，利用算法的收縮特性對三維空間中的路徑進行規劃的方法。彈性繩在空間中的運動過程可以描述為在拉伸狀態下固定一根彈性繩的兩端，彈性繩在彈性力作用下收縮，最后收縮到長度最短的狀態。如果空間中存在障礙物，彈性繩將會沿著障礙物表面滑動并收縮到空間中長度最短的狀態。

在空間中的三維點集S={P1,P2,…,Pn}(n≥3)，該點集S中相鄰兩點相互吸引，且力的大小與兩個節點之間的距離有關，距離越大則吸引作用越強，將該點集S稱為彈性繩系統。彈性繩系統的示意圖如圖1所示。

圖1 彈性繩節點示意圖

彈性繩的運動過程可以分為4種狀態，分別為收縮、碰撞、滑動和平衡。根據彈性繩系統的定義，彈性繩可以看做由n-1段原長為零的彈簧相互連接而成，每段彈簧的能量與彈簧伸長量的平方成正比，彈性繩系統的總能量為每段彈簧的能量之和，即

根據中線定理2(|AB|2+|AC|2)=|AB+AC|2+|AB-AC|2(其中A、B、C是空間中任意不共線的三點)可知，當彈性繩發生收縮、碰撞或滑動時，均會產生總能量的降低。根據能量的定義可知，Es≥0，即彈性繩系統總能量存在下界。根據系統能量單調遞減且存在下界可知，系統能量存在極限Es*，即彈性繩系統可以收斂到空間最短狀態。彈性繩系統中的節點在相互作用下可以收縮到系統勢能最低的狀態，此時彈性繩的長度達到空間最短狀態。

彈性繩S={P1, P2,…, Pn}（n≥3）的各個節點在相互作用下逐點運動，經過一輪運動后變為S'={P'1,P'2,…, P'n}（n≥3）。對于彈性繩上任意一點Pi(i=2,3,…,n-1)，其運動分析如圖2所示。

圖2 運動分析示意圖

假設在節點Pi運動時，其相鄰點P'i-1和Pi+1固定，節點Pi受到相鄰點的吸引作用，合力方向為PiP'i-1+PiPi+1。由此可得彈性繩節點的收縮位置計算公式為：

式中，Si為點Pi的位移矢量(i=2,3,…,n-1)；k∈(0,1)為比例系數。

彈性繩算法和大多數優化算法一樣，在運算過程中也可能會遇到局部最優的問題。通過算法的運算過程可以知道，彈性繩算法通過節點收縮完成最優路徑規劃，是一種局部尋優的算法。組成彈性繩的某一節點的運動和位置更新只由該節點的相鄰兩節點確定，節點更新和位置選擇模式單一，缺少對空間中最優位置的搜索能力。在節點收縮及沿障礙物表面滑動的過程中，如果組成彈性繩的節點偏離了整體最優解的運動方向而朝著局部最優解的方向運動，結果就是彈性繩算法在后續運算中將會朝著局部最優的方向繼續收縮而無法跳出，從而出現陷入局部最優解的情況。

由式（3）可知，組成路徑的彈性繩節點在算法運算過程中的運動方向和更新后的位置僅由與該節點相鄰的兩節點確定。算法在收縮過程中由于缺少對收縮趨勢以外的可行解空間的探索，導致算法對全局的尋優能力較差。因此，需要改進算法在空間中的收縮趨勢，在原有長度收縮的基礎上增加對解空間的探索能力，從而增強算法的全局尋優能力。算法的收縮趨勢由彈性繩節點的更新算法確定，因此需要針對彈性繩節點的更新算法進行改進。

為了提高節點更新算法對可行解空間的搜索能力，引入由m維節點組成的彈性繩路徑組，每一維節點均可表示一條彈性繩路徑，并且均在算法開始時隨機初始化。路徑節點的編碼方式如式（4）所示：

式中，P(i,j)表示第i維彈性繩的第j個節點，i≥ 2,n≥ 3。

因此，改進后的彈性繩節點收縮公式如式（5）所示：

式中，S(i,j)表示節點P(i,j)的運動位移(i=2,3,…,n-1)并且k∈(0,1)；φP(i,j)P(k,j)表示基于不同維度下的節點對可行解空間的搜索位移，φ∈(0,1)為隨機數。

由更新后的節點搜索算法可知，在彈性繩算法運算初期，由于不同維度間同一節點的位置差異和附加隨機數，算法會在搜索位移的作用下在可行解空間擴大搜索范圍，并對節點的位置進行更新，增大算法對可行解空間最優解的搜索能力。在算法運行后期，由于不同維度的彈性繩路徑在收縮作用下均趨于全局空間的最優路徑，不同維度間的節點位置趨于一致。由式（5）可知，這時節點更新公式的搜索位移項將趨于零，路徑節點最終收斂于全局最優路徑。

4 仿真分析

在無人機路徑規劃階段需要的已知信息包括環境中的障礙和威脅、路徑的起點和無人機的目標點等要素。無人機在執行任務過程中的動態避障行為不在本文的考慮范圍內。

本文主要考慮在真實環境中進行無人機路徑規劃的任務，這意味著需要考慮到真實環境中的地形信息，因此，在無人機任務規劃階段利用數字高程模型來描述無人機任務規劃區域的空間信息。數字地圖高程模型是通過數字列陣來地面高程的仿真模型，該數字陣列按一定的空間劃分規則排序。本文將環境中的障礙設置為一系列不同大小的半球，并且將這些障礙與地形信息進行結合，制作了無人機路徑規劃環境的場景地圖。融合后的無人機任務規劃環境地圖如圖3所示。

圖4為彈性繩算法改進前在無人機路徑規劃過程中算法陷入局部最優解的情況。從圖4中可以看出，無人機路徑的節點在場景中的一個障礙處向著錯誤的地方運動，并且在該障礙物處陷入了局部最優解。

利用設計的彈性繩算法對場景中的單架無人機進行路徑規劃，所得結果的如圖5和圖6所示。

圖3 無人機任務規劃環境地圖

圖4 陷入局部最優的情況示意

圖5 彈性繩算法運算過程

圖6 路徑長度隨迭代次數的變化

彈性繩算法是一種基于幾何關系對組成路徑的所有節點進行收縮迭代的路徑規劃算法，通過對組成彈性繩的各個節點位置進行逐次更新迭代，使初始路徑的長度向著長度減少的方向逐漸收縮，并且最終收縮到空間中的最優位置。

無人機在真實地形和存在障礙物的場景中的路徑規劃過程如圖5所示。在仿真初始階段，算法首先在任務空間中隨機初始化出一組彈性繩節點，如圖5（a）所示。隨著彈性繩算法的迭代，在運算過程中的路徑變化如圖5 （b）和圖5（c）所示。最終經過運算得到的最短路徑結果如圖5（d）所示。

任務空間中彈性繩的長度代表無人機任務路徑的長度，由相鄰路徑段長度之和表示。在彈性繩算法運算過程中，無人機路徑的長度隨迭代次數的變化如圖6所示。從圖中可以看出，隨著彈性繩算法的不斷迭代，無人機路徑的長度不斷減少，并且最終達到空間最短距離。從實驗結果可以看出，彈性繩算法可以用在無人機路徑規劃方面。

5 結束語

彈性繩算法是一種基于幾何關系對組成路徑的所有節點進行收縮迭代的路徑規劃算法，通過對組成彈性繩的各個節點位置進行逐次更新迭代，使初始路徑的長度向著長度減少的方向逐漸收縮，并且最終收縮到空間中的最優位置。彈性繩算法在運算初始時隨機初始化了一條連接規劃起點到終點的原始路徑，保證了該路徑是存在的，將路徑規劃問題轉換為路徑的優化問題。另外，該算法的節點更新規則簡單，收斂速度快，可以直接應用到復雜的場景當中。