基于3D-APF和約束動力學的無人機編隊飛行控制

吳云華, 牛 康, 李 磊, 陳志明

(南京航空航天大學航天學院, 江蘇 南京 210016)

0 引 言

近年來,隨著無人機技術的發(fā)展,無人機已廣泛應用于戰(zhàn)場環(huán)境偵察、地面目標打擊、電力巡線等軍事和民用領域[1-3]。與此同時,多無人機協(xié)同自主編隊飛行也迅速成為近年來的一個前沿領域, 采用多無人機編隊飛行可充分利用有限的單機資源, 共同執(zhí)行大范圍目標搜索、自然災害災情獲取等復雜任務。因此,對多無人機編隊飛行展開深入研究具有重要意義。

與單無人機飛行相比,多無人機編隊飛行不僅要解決編隊無人機的路徑規(guī)劃問題,更重要的是如何讓編隊無人機在飛行過程中保持期望的隊形。在無人機的路徑規(guī)劃方面,文獻[4]采用蟻群算法實現了二維平面內的單無人機路徑規(guī)劃;文獻[5]采用稀疏A*遺傳算法完成三維單機路徑規(guī)劃;文獻[6]采用快速搜索隨機樹算法實現了二維平面內的單機路徑規(guī)劃,但是縱觀這些路徑規(guī)劃算法可以發(fā)現,這些算法都需要不同次數的迭代來完成無人機的路徑規(guī)劃,規(guī)劃的實時性較差;文獻[7]采用遺傳算法完成無人機多目標的航路動態(tài)規(guī)劃算法,但是通過仿真結果可以看出該方法比較依賴于適應度函數和迭代次數的選擇。人工勢函數法于1986提出,但是通過國內外文獻可以看出[8-11],在采用人工勢函數對無人機路徑規(guī)劃時,都將無人機視為質點模型,并沒有建立虛擬力環(huán)境下的無人機運動學模型和無人機的姿態(tài)約束。在無人機的隊形保持方面。文獻[12-13]采用領航-跟隨法,該方法是最常見的編隊控制方法具有計算簡單、易于實現等優(yōu)點。但是該方法魯棒性較差,一旦領航者出現問題就會造成隊形無法保障。文獻[14]采用了虛擬領航者的無人機編隊方法,即在無人機編隊中指定一個虛擬的領航者,通過數據共享來實現無人機的編隊飛行。文獻[15-17]采用虛擬結構法,該方法在一定程度上可以增強隊形控制的魯棒性,但是該方法應用范圍受隊形的影響較大,結構單一且適用性較差。文獻[18]采用基于行為的編隊控制方法,該方法為編隊中每個無人機設定預定行為,例如:障礙規(guī)避,隊形保持,目標搜索等。很明顯該方法非常適用于不確定環(huán)境,但缺乏嚴謹的理論分析。文獻[19]提出基于模型預測控制的無人機編隊方法,雖然結合了無人機運動學模型,但是該模型僅僅是用于跟蹤編隊算法已規(guī)劃完成的軌跡。

因此,考慮到無人機路徑規(guī)劃的實時性,本論文采用改進的三維人工勢函數對編隊無人機進行路徑規(guī)劃;同時建立虛擬力環(huán)境下編隊無人機的約束動力學方程組,來解決編隊無人機的隊形保持問題。

1 三維虛擬勢力場

針對編隊無人機的空間障礙規(guī)避問題,同時避免直接將經典人工勢函數擴展到三維空間帶來的問題,論文建立了改進的三維人工勢能場,具體改進如下:

(1) 在二維引力場的基礎上引入垂直方向虛擬引力場;

(2) 將空間障礙物抽象為半球、橢球、圓柱等;

(3) 斥力場僅對位于當前平面的無人機產生斥力作用。

與直接將二維人工勢能場擴展到三維人工勢能場相比,該方法的優(yōu)點在于:①各方向的引力均可以通過控制因子實現自由控制;②可以避免空間障礙物為圓柱體時垂直方向上斥力突然增大的現象;③將障礙抽象為圓柱、球或者橢球等,可以減少局部極小的存在。

1.1 三維引力場

假設當前無人機的位置為qc=[xc,yc,zc],目標點為qg=[xg,yg,zg]。可得無人機的三維引力勢函數表達式為

(1)

式中,ε為比例因子ε>0;ρxy=‖qc_xy-qg_xy‖表示在無人機所處的當前平面,目標點與無人機的平面距離,其中qc_xy,qg_xy分別為qc_xy=[xc,yc],qg_xy=[xg,yg];m,k分別為大于0的常數;Kp是垂直方向比例因子;ρz=‖qc_z-qg_z‖表示無人機與目標點在垂直方向上距離。

1.2 三維斥力場

空間的抽象障礙對無人機產生斥力作用,假設障礙物i的坐標qoi=[xoi,yoi,zoi],定義第i個障礙產生的斥力函數為

(2)

式中,ρOxy(q)=‖qc_xy-qoi_xy‖在無人機所處的當前平面內,障礙物i與無人機的平面距離;qc_xy,qoi_xy為qc_xy=[xc,yc],qoi_xy=[xoi,yoi];Ks為常值;DL為障礙的最小影響范圍;Eqo為無人機與障礙之間的單位矢量;Ko為斥力常數。

因此,可得三維空間內總勢能場U(q)T和無人偵察機所受合力F(q)T分別為

(3)

F(q)T=?U(q)T/?q

(4)

2 虛擬力下無人機運動學建模

與其他無人機相比,四旋翼無人機具有靈活度高、成本低以及對起降場地幾乎沒有任何要求等優(yōu)點。因此,本文以四旋翼為例建立無人機在虛擬力下的運動學模型。

2.1 坐標系定義

在建模之前,首先定義本文用到的坐標系——地坐標系SE和體坐標系SB,其中機體坐標系的原點與無人機重心重合。參考坐標系如圖1所示。

圖1 無人機參考坐標系Fig.1 Reference frames SE and SB of unmanned aerial vehicle (UAV)

2.2 虛擬力下無人機運動學建模

如圖1所示,無人機在飛行過程中除了受到重力Mg、旋翼拉力Fi、氣動阻力faero外還受到三維虛擬力合力FVirtual的作用。其中,虛擬合外力的表達式為

FVirtual=F(q)T=?U(q)T/?q

(5)

由牛頓運動學定律經過推導可得,在虛擬力環(huán)境下無人機的線運動方程為

(6)

(7)

(8)

uz=cosφcosθ

(9)

3 無人機隊形保持策略

雖然在三維虛擬力可以很好地使無人機完成三維路徑規(guī)劃,但是并不能使多個無人機保持期望的隊形。結合約束動力學理論[20-21],在無人機編隊飛行的隊形保持策略中,本文將多無人機編隊飛行看作一個虛擬力環(huán)境下的完整性約束動力學系統(tǒng)。其中,約束條件為編隊期望隊形,其表達式定義為

ρ(x,y,z,t)=0

(10)

M=2n-3

(11)

式中,M表示約束方程的個數;n表示無人機的個數。以3架飛機編隊為例,其隊形約束方程ρ表示為

(12)

結合式(10),在含有隊形約束的情況下,根據約束動力學理論定義編隊無人機的約束動力學方程組為

(13)

(14)

ρ(x,y,z,t)=0

(15)

式中,q為編隊無人機的慣性坐標系下位置矢量q=[x1,y1,z1,…,xn,yn,zn]T;M為編隊中各無人機的權重矩陣;∑F為對應無人機受到外界的合外力;λ為拉格朗日乘子;ρ為隊形約束方程;JTλ為隊形約束力;JT為約束方程的雅克比矩陣。約束方程的雅克比矩陣JT的表達式為

JT=[?ρ/?x,?ρ/?y,?ρ/?z]T

(16)

由式(14)可以看出,式中只有拉格朗日乘子λ未知。因此要得到編隊無人機運動的約束方程則必須求出拉格朗日乘子的表達式。論文采用Penalty-Formulation法求解λ表達式。

首先,將式(10)分別對時間t求一階和二階微分可得

在課程結束后應用微課，應當起到深化知識，促使學生所理解的內容更為系統(tǒng)的作用，這樣學生才能在數學知識應用階段更加靈活，富有創(chuàng)新性，因此不難發(fā)現，微課在課程結束后的應用需要對學生的創(chuàng)新及數學思維能力進行進一步培養(yǎng)及升華。例如：在學生對角的學習時，教師可以制作與之對應并促使知識點延伸的微課，也就是發(fā)現生活中的角、角的形成、角不同部分的名稱，角怎樣比較等等。然后教師需要為學生演示實際操作方法，促使學生明確實踐方法，并為學生布置課后作業(yè)，引導并鼓勵學生自己去發(fā)現、探索及實踐，這樣他們就能對知識形成更為深刻的理解，其思維能力也能夠在實踐探究過程中得到鍛煉及提升。

(17)

(18)

式(18)可以表示為

(19)

將式(13)和式(14)轉換到正向動力學方程格式下可變?yōu)?/p>

(20)

Penalty-Formulation基本思想為將隊形約束力近似為一個虛擬的柔性原件(彈簧或者阻尼器)來逼近拉格朗日乘子[24-25],從而建立隊形約束力。因此,拉格朗日乘子λ可表示為

(21)

式中,Ks為柔性彈性因子;Kd為柔性阻尼因子。以三機三角形編隊為例,各機之間的約束力采用虛擬的彈簧合阻尼器如圖2所示。

圖2 三機之間通過虛擬彈簧和阻尼器連接Fig.2 Three UAVs interconnected by virtual springs and dampers

將式(16)、式(10)代入式(20)可得λ的表達式為

(22)

因此,將式(22)代入式(20)可得,編隊無人機的約束運動學方程為

(23)

在飛行過程中對于隊形保持的優(yōu)劣,定義Δσ為評價指標,其中Δσ的表達式為

(14)

式中,ρ*為編隊個體之間的實際距離;ρd為編隊個體間的期望距離。

4 仿真實驗與結果分析

為了驗證本文路無人機編隊飛行方法的有效性,算法在Matlab環(huán)境下進行以下仿真實驗。其中,將空間的障礙抽象為半橢球形或半球形。仿真以三機三角固定編隊為例,首先給定目標點位置為Tar(30,30,30);無人機初始位置分別為A(0,4,0),B(4,4,0),C(4,0,0);障礙物抽象為橢球形,其中表達式方程為(x-15)2/52+(y-15)2/52+z2/302=1。

圖3為在無隊形約束的情況下,編隊無人機在改進三維人工勢能場的飛行曲線?？梢钥闯?雖然改進的三維人工勢能場可以很好地指導編隊中各無人機避開障礙,到達預定的目標點;但是顯而易見,各無人機并不能保持期望的隊形繞開三維障礙并到達指定的目標點。

圖3 無隊形約束下無人機編隊飛行仿真Fig.3 UAVs formation flight simulation without formation constraint

圖4為在無隊形約束的情況下,編隊隊形誤差曲線?？梢钥闯?雖然在初始條件下各無人機保持期望隊形,但是隨著時間的進行，無人機在沒有隊形約束的情況下隊形總體誤差開始增大。

圖4 無隊形約束下無人機隊形誤差曲線Fig.4 Formation error of UAVs without formation constraint

圖5～圖7分別為在含有隊形約束的情況下:①無人機編隊飛行仿真曲線;②各無人機姿態(tài)角變化曲線;③隊形總體隊形誤差曲線。

圖5 隊形約束下無人機編隊飛行仿真Fig.5 UAVs formation flight simulation with formation constraint

圖6 無人機姿態(tài)角曲線Fig.6 Attitude angle curve of UAVs

圖7 隊形約束下無人機隊形誤差曲線Fig.7 Formation error of UAVs with formation constraint

對比圖3和圖5可以看出,編隊在隊形約束的情況下不僅可以安全的規(guī)避障礙,同時可以保持期望的隊形到達目標區(qū)域。

圖6為無人機在隊形約束的情況下,各無人機的姿態(tài)角曲線。可以看出,各機的姿態(tài)角均在很小的角度變化。

對比圖4和圖7可以看出:在含有隊形約束的情況下,隊形誤差一直保存在很小的范圍內,可以說明無人機編隊飛行過程中一直保持期望的隊形。

綜上可以看出,本文提出的算法可以有效地規(guī)劃編隊飛行路徑,同時整個過程編隊無人機均保持著期望隊形到達目標點。驗證了本文所提算法的有效性。

5 結 論

本文提出了一種虛擬力環(huán)境下無人機編隊飛行方法。該方法針對編隊無人機的路徑規(guī)劃問題提出的改進的三維勢能場,建立了虛擬力環(huán)境下的無人機運動學模型。對于無人機的隊形保持問題,本文根據約束動力學理論,建立了隊形約束下的編隊無人機運動學方程組并采用Penalty-Formulation方法對約束動力學方程組進行求解。最后,通過詳細的對比仿真實驗,并對仿真實驗進行對比分析,驗證了本文方法的有效性。

參考文獻：

[1] SASKA M, BACA T, THOMAS J, et al. System for deployment of groups of unmanned micro aerial vehicles in GPS-denied environments using onboard visual relative localization[J]. Autonomous Robots, 2017, 41(4): 1-26.

[2] DUAN H. Multiple UAV formation control[M]∥Bio-inspired computation in unmanned aerial vehicles.Berlin:Springer, 2014: 143-181.

[3] 朱旭, 張遜遜, 尤謹語, 等. 基于信息一致性的無人機緊密編隊集結控制[J]. 航空學報, 2015, 36(12): 3919-3929.

ZHU X, ZHANG X X, YOU J Y, et al. Swarm control of UAV close formation based on information consensus[J]. Acta Aeronautica et Astronautica Sinica, 2015, 36(12): 3919- 3929.

[4] HABIB N, SOEPRIJANTO A, PURWANTO D, et al. Mobile robot motion planning to avoid obstacle using modified ant colony optimization[J]. Applied Mechanics & Materials, 2015, 776: 396-402.

[5] 劉莉,于成龍,王祝,等.小型無人機快速三維航跡規(guī)劃方法[J].系統(tǒng)工程與電子技術, 2013, 35(12): 2521-2526.

LIU L, YU C L, WANG Z, et al. Fast 3D route planning method for small UAV[J].Systems Engineering and Electronics, 2013, 35(12): 2521-2526.

[6] QURESHI A H, MUMTAZ S, KHAN W, et al. Augmenting RRT*-planner with local trees for motion planning in complex dynamic environments[C]∥Proc.of the International Conference on Methods and Models in Automation and Robotics, 2014:657-662.

[7] LIN C E, SYU Y M. GA/DP hybrid solution for UAV multi-target path planning[J]. Journal of Aeronautics Astronautics & Aviation, 2016, 48(3): 203-220.

[8] 曲成剛, 曹喜濱, 張澤旭.人工勢場和虛擬領航者結合的多智能體編隊[J]. 哈爾濱工業(yè)大學學報, 2014, 46(5): 1-5.

QU C G, CAO X B, ZHANG Z X. Multi-agent system formation integrating virtual leaders into artificial potentials[J]. Journal of Harbin Institute of Technology, 2014, 46(5): 1-5.

[9] CHEN Y B, LUO G C, MEI Y S, et al. UAV path planning using artificial potential field method updated by optimal control theory[J].International Journal of Systems Science,2016,47(6): 1407-1420.

[10] LIU Y, ZHAO Y. A virtual-waypoint based artificial potential field method for UAV path planning[C]∥Proc.of the Guidance, Navigation and Control Conference, 2017: 949-953.

[11] QU Y, ZHANG Y, ZHANG Y. A UAV solution of regional surveillance based on pheromones and artificial potential field theory[C]∥Proc.of the International Conference on Unmanned Aircraft Systems, 2015: 380-385.

[12] ROLDAO V, CUNHA R, CABECINHAS D, et al. A leader-following trajectory generator with application to quadrotor formation flight[J].Robotics & Autonomous Systems,2014,62(10): 1597-1609.

[13] 付明玉,焦建芳,張愛華.基于虛擬領航者的多艘船舶協(xié)調路徑跟蹤控制[J].華中科技大學學報(自然科學版),2013,41(2): 102-108.

FU M Y, JIAO J F, ZHANG A H. Coordinated path following control for multiple surface vessels by using virtual-leader[J]. Journal of Huazhong University of Science and Technology (Nature Science), 2013, 41(2): 102-108.

[14] GOSIEWSKI Z ?, AMBROZIAK L. UAV autonomous formation flight experiment with virtual leader control structure[J]. Solid State Phenomena, 2013, 198: 254-259.

[15] CHEN Y B, YU J Q, SU X L, et al. Path planning for multi-UAV formation[J]. Journal of Intelligent & Robotic Systems, 2015, 77(1): 229-246.

[16] ASKARI A, MORTAZAVI M, TALEBI H A. UAV formation control via the virtual structure approach[J]. Journal of Aerospace Engineering, 2015, 28(1): 04014047.

[17] SHAO Z, ZHU X, ZHOU Z. A nonlinear control of 2-D UAVs formation keeping via virtual structures[C]∥Proc.of the International Conference on Intelligent Robotics and Applications, 2014: 420- 431.

[18] KIM S, SHIN J, SUK J. Development of robust flocking control law for multiple UAVs using behavioral decentralized method[J]. International Journal of Aeronautical & Space Sciences, 2015, 43(10): 805-813.

[19] MAO S, TAN W K, LOW K H. Autonomous formation flight of indoor UAVs based on model predictive control[C]∥Proc.of the AIAA Infotech @ Aerospace, 2016: 1-9.

[20] SHAH S V, SAHA S K, DUTT J K. Dynamics of robotic systems[M]∥Dynamics of tree-type robotic systems. Netherlands: Springer, 2013: 9-25.

[21] CARRERA E, PAGANI A, BANERJEE J R. Linearized buckling analysis of isotropic and composite beam-columns by carrera unified formulation and dynamic stiffness method[J]. Mechanics of Composite Materials & Structures, 2016, 23(9): 1092-1103.

[22] KELLY A, SEEGMILLER N. Recursive kinematic propagation for wheeled mobile robots[J]. International Journal of Robotics Research, 2015, 34(3): 288-313.

[24] LEE L F, KROVI V. A standardized testing ground for artificial potential-field based motion planning for robot collectives[EB/OL].[2017-06-28].http:∥pdfs.semanticscholar.org/9335/8b006c6b5105fc32c9ac9e9ce79aca487205.pdf.

[25] OROZCO-ROSAS U, MONTIEL O, SEPúLVEDA R. Pseudo-bacterial potential field based path planner for autonomous mobile robot navigation[J]. International Journal of Advanced Robotic Systems, 2015, 81(12): 1-14.