基于改進人工勢場法的動態(tài)環(huán)境下無人機路徑規(guī)劃

田亞卓，張勇軍

(北京科技大學工程技術研究院，北京,100083)

基于改進人工勢場法的動態(tài)環(huán)境下無人機路徑規(guī)劃

田亞卓，張勇軍

(北京科技大學工程技術研究院，北京,100083)

基于傳統(tǒng)人工勢場法，在相對位置引力場和斥力場函數(shù)中分別引入目標與障礙物的速度矢量，將其位置動態(tài)變化信息融入在改進后的相對速度引力場和斥力場中，引入相對位置引力、相對速度引力、相對位置斥力、相對速度斥力等比例調(diào)節(jié)因子，建立改進后的人工勢場函數(shù)及多障礙物合力函數(shù)，并在模擬環(huán)境下進行仿真。結果表明，該改進人工勢場法能滿足無人機在目標與障礙物動態(tài)變化條件下的路徑規(guī)劃的安全性、實時性和可達性，并提高了動態(tài)環(huán)境中無人機的跟蹤與避障速度。

無人機; 路徑規(guī)劃; 動態(tài)環(huán)境; 人工勢場法; 仿真分析

無人機的軌跡規(guī)劃是指在滿足無人機性能約束指標，在當前特定的條件下，尋找一條從起始位置到任務目標完成位置的最優(yōu)或次優(yōu)飛行軌跡[1-2]，它不僅能夠保證飛行任務的圓滿完成，還能夠盡可能地節(jié)約飛行時間和能耗，降低飛行器過載[3]，因此無人機的路徑規(guī)劃成為無人機領域中值得深入研究的熱點問題之一。

近年來國內(nèi)外研究者提出了許多航跡規(guī)劃的方法，主要有基于圖論的方法、基于單元分解的方法、基于數(shù)學規(guī)劃的方法以及基于人工勢場的方法等，其中基于圖論的方法是用網(wǎng)絡圖來處理軌跡規(guī)劃問題，主要有Voronoi圖法[4]、通視圖法[5]、快速搜索隨機樹法(RRT)[6]和隨機路線圖法[7]，圖論對于求解最優(yōu)化問題很有實用價值，但大部分基于圖論的方法需要包含該地區(qū)內(nèi)的所有可能的軌跡，否則規(guī)劃可能會不準確；單元分解的方法是用許多單元合成的新空間來替代規(guī)劃區(qū)域，具體有柵格分解法[8]和基于A*算法的軌跡規(guī)劃算法[9]，單元分解法雖然可以較為準確地進行最優(yōu)航跡規(guī)劃，但其算法復雜度較高，難以保證實時性；基于數(shù)學規(guī)劃的方法主要是用函數(shù)來模擬影響路徑規(guī)劃的條件，此類方法在工程實踐中運用比較多，如混合整數(shù)線性規(guī)劃[10]和多約束條件下的飛行器航路規(guī)劃[11],此類方法綜合了軌跡距離、障礙位置和軌跡規(guī)劃中的其他因素，但是算法過于復雜，函數(shù)的選取也需要著重考慮；人工勢場法路徑規(guī)劃是1986年由Khatib提出的一種基于物理力學原理對基本空間進行路徑規(guī)劃的方法[12]，傳統(tǒng)人工勢場法最早是用于移動機器人與機械臂的運動軌跡規(guī)劃，其基本思想是將機器人在周圍環(huán)境中的運動，設計成一種抽象的人造引力場中的運動，目標點對移動機器人產(chǎn)生“引力”，障礙物對移動機器人產(chǎn)生“斥力”，最后通過求合力來控制移動機器人的運動，盡管該方法存在局部最優(yōu)解的難點問題，但以其結構簡潔、運算步驟少、生成路徑平滑等優(yōu)勢在機器人避障中有著廣泛的應用，同時，由于它不需要對全局路徑進行搜索，規(guī)劃時間短，執(zhí)行效率高，魯棒性強，所以該方法便于底層的實時控制，規(guī)劃出來的路徑實時、高效、安全。

隨著無人機路徑規(guī)劃研究的深入，有研究者發(fā)現(xiàn)人工勢場法不僅能夠應用于機械臂與移動機器人的軌跡規(guī)劃，也能夠有效應用于無人機的航跡規(guī)劃，容易針對無人機執(zhí)行任務時的目標點與障礙物來規(guī)劃一條平滑、安全的飛行路徑，實現(xiàn)飛行器在引力與斥力的合力作用下從起始點運動到目標點[13]，但該方法是靜態(tài)路徑規(guī)劃方法。盡管有研究者對威脅分布建立柵格化模型和基于稀疏A*搜索算法預規(guī)劃實現(xiàn)人工勢場法的動態(tài)航跡規(guī)劃[14-15]，并收到一定效果，但算法較為復雜，難以滿足實時控制需求。面向目標及障礙物均發(fā)生變化的三維動態(tài)環(huán)境下無人機路徑規(guī)劃，傳統(tǒng)人工勢場法容易出現(xiàn)規(guī)劃路徑過長、難以收斂、無法準確到達目標點等問題。為此，本文在傳統(tǒng)人工勢場法的基礎上提出一種適用于動態(tài)環(huán)境的人工勢場法改進策略，在相對位置引力場和斥力場函數(shù)中分別引入目標與障礙物的速度矢量，將其位置動態(tài)變化信息融入在改進后的相對速度引力場和斥力場中，引入相對位置引力、相對速度引力、相對位置斥力、相對速度斥力等比例調(diào)節(jié)因子，建立改進后的人工勢場函數(shù)及多障礙物合力函數(shù)，并在模擬環(huán)境下進行仿真分析，為無人機在目標與障礙物動態(tài)變化條件下的路徑規(guī)劃提供參考。

1 傳統(tǒng)人工勢場法

人工勢場包括引力場與斥力場，其中目標點對物體產(chǎn)生引力，障礙物對物體產(chǎn)生斥力，避免物體與之發(fā)生碰撞，可能會存在多個斥力同時作用。物體在路徑上每一點所受的合力等于這一點所有斥力和引力的和。無人機在傳統(tǒng)人工勢場中的受力分析如圖1所示。

圖1 無人機在傳統(tǒng)人工勢場中的受力分析

傳統(tǒng)人工勢場法的引力勢場函數(shù)Uatt(X)和斥力勢場函數(shù)Urepi(X)分別為：

(1)

Urepi(X)=

(2)

式中：k為引力正比例位置增益系數(shù)；ρ(X,Xg)=‖Xg-X‖，表示無人機與目標點的距離，其中X、Xg分別為無人機與目標點的位置；η為斥力正比例位置增益系數(shù)；ρi(X,X0)為無人機與第i個障礙物之間的距離；ρ0為障礙物對無人機產(chǎn)生影響的最大距離。

無人機所受到的引力Fatt(X)和斥力Frepi(X)分別為：

Fatt(X)=-[Uatt(X)]=-kρ(X,Xg)

(3)

Frepi(X)=-[Urepi(X)]=

(4)

總斥力為：

(5)

則無人機所受到的合力為

F(X)=Fatt(X)+Frep(X)

(6)

2 改進人工勢場法

2.1 改進的引力場函數(shù)與引力函數(shù)

為了解決傳統(tǒng)人工勢場法在動態(tài)環(huán)境中能夠合理追蹤目標點的問題，本文對傳統(tǒng)引力勢場函數(shù)進行改進，引入相對引力速度矢量，構造相對速度引力勢場函數(shù)，建立改進人工引力場。首先將式(1)中傳統(tǒng)的引力勢場函數(shù)定義為改進人工勢場法的相對位置引力勢場函數(shù)，即：

(7)

式中：kα為引力正比例位置增益系數(shù)。

對相對位置引力勢場函數(shù)求負梯度得到相對位置引力，即：

Fattp(X)=-kαρ(X,Xg)

(8)

相對速度引力勢場函數(shù)可表示為：

(9)

式中：kβ為引力正比例速度增益系數(shù)，V(X,Xg)為無人機相對于目標點的速度。

對相對速度引力場函數(shù)求負梯度得到相對速度引力，即：

Fattv(X)=-kβV(X,Xg)

(10)

在引入了相對速度引力勢場函數(shù)之后，改進引力場函數(shù)變?yōu)椋?/p>

Uatt(X)=Uattp(X)+Uattv(X)=

(11)

無人機在該勢場函數(shù)下所受的引力為相對位置引力與相對速度引力的合力，即：

Fatt(X)=Fattp(X)+Fattv(X)

=-kαρ(X,Xg)-kβV(X,Xg)

(12)

由引力的表達式可知，相對位置引力的作用范圍是全局的，在整個無人機運動區(qū)域內(nèi)，均會受到相對位置引力的作用。相對速度引力使得無人機的速度與目標的速度保持一致，其大小與相對速度的大小成正比，方向為目標點相對于無人機的運動方向。kα和kβ為權重系數(shù)，調(diào)整其大小可以調(diào)整相對速度引力和相對位置引力在合引力中所占的權重，當kβ=0時，改進的引力勢場函數(shù)退化為傳統(tǒng)的引力勢場函數(shù)。

2.2 改進的斥力場函數(shù)與斥力函數(shù)

相對位置斥力函數(shù)為：

(13)

式中：ηα為斥力正比例位置增益系數(shù)。

由于在動態(tài)環(huán)境下障礙物也是運動的，故同樣需要引入相對速度斥力勢場函數(shù)

(14)

式中：Vi(X,X0)表示無人機在飛行過程中，相對于第i個障礙物的相對速度大小；ηβ為斥力正比例速度增益系數(shù)；θori為無人機飛行過程中，相對于第i個障礙物的相對速度矢量與xOy平面的夾角；ρ1為障礙物對無人機產(chǎn)生相對速度影響的最大距離。本文此處假設障礙物的運動方向為沿z軸正方向，則相對速度斥力場函數(shù)可以表示為:

(15)

式中：Xori、Yori、Zori分別為無人機的X、Y、Z坐標相對于第i個障礙物的相應坐標的標量差。

當θori>0時,相對速度斥力為：

(16)

即：

(17)

當θori<0時，相對速度斥力為：

(18)

即：

(19)

而當cosθori<0時，無人機可視為正在逃離第i個障礙物，但如果在此情況下依然考慮相對速度斥力，則可能會對無人機后續(xù)避障產(chǎn)生相反作用，因此此處設定該障礙物對無人機的相對速度斥力為0。

基于上述討論，將第i個障礙物的相對速度斥力勢場函數(shù)表示為:

(20)

相對速度斥力函數(shù)表示為:

Frepvi(X)=

(21)

在引入了相對速度斥力場之后，無人機在人工勢場中所受到的斥力為：

(22)

式中：N為障礙物的個數(shù)。

斥力場的作用范圍是局部的，引入的相對速度斥力垂直于無人機與障礙物的相對位置方向，當夾角θori較小時，速度斥力幾乎垂直于相對速度的方向，能夠使無人機更快地脫離障礙物威脅。無人機與障礙物之間的相對距離越小，或相對速度越大，則相對速度斥力越大。ηα和ηβ為權重系數(shù)，調(diào)整其大小可以調(diào)整相對位置斥力和相對速度斥力在斥力中的權重。

在改進人工勢場法中無人機受到的力，為其在改進后的人工勢場中所受到的引力與斥力的合力。

3 仿真分析

3.1 引力場的仿真及分析

設無人機的初始位置為P(0,0,0)，初始速度為V(0,0,0)，最大速度為20 m/s，最大轉角為120°，目標初始位置為(4000,4000,0)，以速度為6 m/s沿z軸方向做勻速直線運動，分別在傳統(tǒng)人工引力場和改進人工引力場中以該初始條件做仿真，kα和kβ取值分別為0.4和0.2，無人機在傳統(tǒng)人工引力場和改進人工引力場中的運動軌跡仿真結果如圖2所示，其中，Sgoal為目標點運動軌跡，箭頭所指為運動方向，Stra為無人機在傳統(tǒng)人工引力場中的運動軌跡，Simp為無人機在改進人工引力場中的運動軌跡。從圖2中可以看出，無人機在改進引力勢場中可以更加平滑和高效地達到目標點，由于目標點初始位置為(4000,4000,0)，速度為6 m/s沿z軸正方向做勻速直線運動，故在改進人工勢場法中，無人機受到一個沿z軸正方向的相對速度引力，改進引力勢場中，無人機的整體路徑相對于傳統(tǒng)引力勢場有一個z軸正方向的提升，就是因為相對速度引力的作用。

圖2無人機在傳統(tǒng)人工引力場和改進人工引力場中的運動軌跡

Fig.2TrajectoryofUAVbytraditionalAPFandimprovedAPF

圖3為在傳統(tǒng)引力勢場和改進引力勢場作用下無人機到目標點的距離，其中，Dtra、Dimp分別為無人機在傳統(tǒng)人工引力場和改進人工引力場中運動時到目標點的距離。從圖3中可以看出，在改進引力勢場函數(shù)下，無人機能更快地到達目標點，表明改進后的引力勢場函數(shù)更高效、運動軌跡更平滑。

圖3無人機在傳統(tǒng)人工引力場和改進人工引力場中到目標點的距離

Fig.3ThedistanceofUAVtothetargetpointbytraditionalAPFandimprovedAPF

3.2 斥力場的仿真及分析

(a)無人機在傳統(tǒng)斥力場中的運動軌跡

(b)無人機在改進斥力場中的運動軌跡

圖5 無人機與目標點之間的動態(tài)距離

在無人機、目標點與第一個運動障礙物的運動情況保持不變的情況下，加入了另一運動的障礙物，其初始點的位置為(3000,3000,2500)，以速度為2 m/s沿z軸負方向做勻速直線運動，當無人機脫離第一個障礙物時，該障礙物開始運動，當無人機脫離該障礙物之后，該障礙物停止運動。無人機在兩個運動障礙物下的運動軌跡仿真結果如圖6所示，Sobs1和Sobs2分別表示第一個障礙物和第二個障礙物的運動軌跡，運動方向分別為其對應箭頭所指方向。由圖6可知，無人機在傳統(tǒng)人工斥力場中遭遇多個動態(tài)障礙物時，規(guī)劃的避障路徑非常復雜且路徑過長，但是在改進人工斥力場中，無人機能夠更加高效地避開多個運動的障礙物、高速有效地到達目標點。

(a)無人機在傳統(tǒng)斥力場中的運動軌跡

(b)無人機在改進斥力場中的運動軌跡

上述仿真結果表明，在單個與多個運動障礙物和運動目標點的環(huán)境中，改進人工勢場法能夠較好地解決傳統(tǒng)人工勢場法軌跡規(guī)劃過長的問題，實現(xiàn)了無人機在三維動態(tài)環(huán)境下的路徑規(guī)劃。

4 結論

(1)基于傳統(tǒng)人工勢場法，在相對位置引力場和斥力場函數(shù)中分別引入目標與障礙物的速度矢量，將其位置動態(tài)變化信息融入在改進后的相對速度引力場和斥力場中，引入相對位置引力、相對速度引力、相對位置斥力、相對速度斥力等比例調(diào)節(jié)因子，建立改進后的人工勢場函數(shù)及多障礙物合力函數(shù)，并在模擬環(huán)境下進行仿真。

(2)該改進人工勢場法能滿足無人機在目標與障礙物動態(tài)變化條件下的路徑規(guī)劃的安全性、實時性和可達性，并提高了動態(tài)環(huán)境中無人機的跟蹤與避障速度。

[1] 唐強，張翔倫，左玲. 無人機航跡規(guī)劃算法的初步研究[J]. 航空計算技術，2003，33(1)：125-128，132.

[2] 屈耀紅，潘泉，閆建國，等. 無人機全局航路規(guī)劃技術仿真[J].系統(tǒng)仿真學報, 2006，18(2)：278-281.

[3] 殷春武，侯明善，李明翔. 基于區(qū)間灰度的無人機航跡擇優(yōu)評價方法研究[J].系統(tǒng)工程理論與實踐，2016，36(4)：1025-1032.

[4] 趙文婷，彭俊毅. 基于VORONOI圖的無人機航跡規(guī)劃[J].系統(tǒng)仿真學報，2006，18(S2)：159-162，165.

[5] Asano T, Guibas L, Hershberger J, et al. Visibility-polygon search and Euclidean shortest paths[C]∥Foundations of Computer Science 1985,26th Annual Symposium on IEEE, 1985:155-164.

[6] LavalleSM. Rapidly-exploringrandomtrees:A new tool for path planning[R].Ames:Computer Science Dept., Iowa State University,Oct,1998.

[7] KavrakiL, LatombeJC. Randomizedpreprocessingof configuration space for fast path planning[C]//Proceedings of IEEE International Conference on Robotics and Automation, San Diego: IEEE Computer Society Press, 1994:2138-2145.

[8] Parsons D, Canny J. A motion planner for multiple mobile robots[C]∥Proceedings of IEEE International Conference on Robotics and Automation,Cincinnati,Ohio,USA, 1990:8-13.

[9] 劉新，周成平，俞琪,等. 基于分層策略的三維航跡快速規(guī)劃方法[J].宇航學報，2010,31(11)：2524-2529.

[10] Hauser J, Jadbabaie A. Aggressive maneuvering of a thrust vectored flying wing: A receding horizon approach[C]//Proceedings of the 39th IEEE Conference on Decision and Control,Sydney,Australia,2000:3582-3587.

[11] 熊丹君，蔡滿意，劉宇坤，等. 多約束條件下飛行器航路規(guī)劃[J].彈箭與制導學報，2009，29(2)：289-292.

[12] Khatib O.Real-time obstacle avoidance for manipulators and mobile robots[J].International Journal of Robotics Research,1986,5(1):396-404.

[13] 王強，張安，吳忠杰. 改進人工勢場法與模擬退火算法的無人機航路規(guī)劃[J].火力與指揮控制，2014，39(8):70-73.

[14] 丁家如，杜昌平，趙耀，等. 基于改進人工勢場法的無人機路徑規(guī)劃算法[J].計算機應用，2016，36(1)：287-290.

[15] 姚遠，周興社，張凱龍，等. 基于稀疏A*搜索和改進人工勢場的無人機動態(tài)航跡規(guī)劃[J].控制理論與應用，2010，27(7)：953-959.

[責任編輯張惠芳]

UAVpathplanningbasedonimprovedartificialpotentialfieldindynamicenvironment

TianYazhuo,ZhangYongjun

(Institute of Engineering and Technology, University of Science and Technology Beijing, Beijing 100083, China)

Based on the traditional artificial potential field method, the velocity vector of the targets and obstacles were introduced into the relative position gravitational field and repulsion field function. The information of dynamic changing position was integrated into the improved relative velocity gravitational field and repulsion field. The relative position of gravity, the relative position and speed of repulsion, and other proportional adjustment factors were introduced. The improved artificial potential field function and multi-obstacle force function were established to simulate in the simulation environment. The results show that the improved artificial potential field method can not only meet the requirements of safety, real-time capability and reachability of UAV path planning under the dynamic changes of targets and obstacles, but also improves the tracking and obstacle avoidance speed of the UAV in a dynamic environment.

UAV; path planing; dynamic environment; artifical potential field; simulation analysis

V249

A

1674-3644(2017)06-0451-06

2017-09-09

田亞卓(1991-)，男，北京科技大學碩士生.E-mail:tian19911116@126.com

張勇軍(1973-)，男，北京科技大學研究員，博士生導師.E-mail:zhangyj@ustb.edu.cn

10.3969/j.issn.1674-3644.2017.06.009