基于比例導引角度線性化的無人機避障研究

楊秀霞，劉小偉，張 毅

（海軍航空工程學院控制工程系，山東煙臺264001）

基于比例導引角度線性化的無人機避障研究

楊秀霞，劉小偉，張 毅

（海軍航空工程學院控制工程系，山東煙臺264001）

基于無人機與障礙物之間的幾何關系，在極坐標系下建立了無人機與障礙物之間的運動學方程，通過引入基于角度線性化的比例導引律，使無人機能夠順利避開障礙物。引入的基于角度線性化的比例導引律是關于無人機相對速度航向角與避障點視線角的函數，由比例函數及偏差函數組成。為解決初始時刻相對速度航向角不滿足引入導引律的問題，在調節函數中加入了指數項。通過穩定性證明，得到了滿足要求的導引律參數取值范圍。最后對設計的避障算法進行了仿真，仿真結果驗證了算法的有效性。

角度線性化；比例導引；偏差函數；避障

0 引言

目前，無人機在軍事及民用領域已得到越來越廣泛的應用。如何使無人機能夠安全導引到目標并順利避開動態障礙物是眾多學者研究的重點課題。

最早對這一問題開始研究的是O.Khatib[1]、Erdmann和Lozano-Perez[2]等人。后來，學者們把在靜態環境下得到成功應用的路徑規劃及避障方法推廣到動態環境中去，如在文獻[3]中提到了確定及隨機的路徑地圖方法，在文獻[4]中通過考慮機器人及障礙物的速度，并加入到勢能函數的構造中，得到適用于動態環境的勢場法，在文獻[5]中通過動態窗口方法進行路徑規劃及避障。這些方法在靜態環境下得到較好的結果，但是在動態環境下，尤其是應用于無人機高速運動這一特殊情況下，避障效果并不理想。其原因主要是在動態環境下，碰撞判斷及避障情況下對速度的控制與靜態環境相比存在較大區別。

為解決動態環境下的碰撞判斷問題，文獻[6]、[7]提出了基于運動學方程的碰撞判斷方法。文獻[6]定義了碰撞錐的概念，依據視線的距離及角速度來確定碰撞條件。在文獻[7]中基于視線角速度方程，對直接碰撞和間接碰撞進行了比較。

同時考慮到動態環境下障礙物速度變化的問題，學者們提出了速度障礙的概念。例如文獻[8]在考慮運動障礙物速度的基礎上進行碰撞錐的構建，在此基礎上，通過使機器人的速度轉移到碰撞錐之外達到避障目的，但是對障礙物的速度要求必須是線性的。為解決障礙物速度為不定值的避障問題，文獻[9]通過對某一時刻，在考慮無人機及障礙物相對位置情況下，對可能的碰撞速度進行時間積分，得到非線性速度障礙集進行避障。同時考慮到多無人機之間的避碰問題，文獻[10]提出了互惠速度障礙的概念，即兩個即將相碰的障礙物之間互相主動避碰，達到避碰目的。

在對上述方法分析和總結的基礎上，本文提出了基于角度線性化的比例導引無人機避障算法。首先在極坐標系下建立無人機與障礙物的相對運動學方程，通過引入基于角度線性化的導引律進行避障算法設計。考慮到一般情況下無人機相對速度初始航向角不滿足給定導引律的問題，通過引入指數偏差函數對航向角進行調節，使無人機的最終穩定航向角達到避障要求。通過對設計算法穩定性證明，得到了滿足要求的各個參數取值范圍。最后本文利用Matlab對設計的避障算法進行了仿真，結果驗證了算法的有效性。

1 幾何關系及運動學方程

1.1 問題描述

當無人機與障礙物在慣性空間運動時，建立無人機與障礙物的幾何關系示意圖，如圖1所示。無人機的速度為vuav，方向為ψuav，障礙物的速度為vobs，方向為ψobs。無人機相對障礙物的速度為vrel，方向為ψrel。由速度障礙原理，通過擴大障礙圓半徑，把無人機簡化為質點U，基于質點U及障礙圓切線UB、UA建立碰撞區域UAB。若相對速度vrel位于碰撞區域UAB內，則存在碰撞危險，因此需要調整無人機的速度的大小及方向，使無人機的相對速度vrel位于碰撞區域UAB之外。調整時，一般考慮相對速度vrel角度變化最小即避障完成時間短、偏離原目標軌跡短原則，結合圖1，選取避障切線為UB，即避障點為B。

本文提出的避障算法是設計關于無人機相對速度的航向角ψrel的導引律，使無人機能夠到達避障點B，且相對速度方向ψrel與切線UB方向θBu相等，確保避障完成。

圖1 無人機與障礙物幾何關系示意圖Fig.1 The scheme of geometric relation of UAV and obstacle

1.2 建立運動學方程

由于障礙圓是剛性圓，因此B點的速度大小及航向角與圓心一致，因此可得無人機U與B點之間的距離及視線UB的角度θBu的變化率為：

為研究方便，我們建立無人機與障礙物的相對運動方程，此時無人機以相對速度vrel飛向避障點B：

式（3）給出了無人機與避障點B之間的距離變化關系，式（4）給出了無人機與避障點B之間的視線角的變化方程。

2 角度線性化比例導引律

本文基于角度線性化比例導引律對避障算法進行設計。選擇比例導引律最基本的定義，即相對速度vrel的航向角速度與目標視線UB的角速度成比例，即。

其中N為比例導引系數，滿足N＞1。考慮初始狀態有：

1）如果ψrel(t0)=NθBu(t0)，則比例導引律可表示為：ψrel(t)=NθBu(t)；

2）如果 ψrel(t0)≠NθBu(t0)，則比例導引律可表示為：

設c=ψrel(t0)-NθBu(t0)，則相對速度航向角可表示為：

c對無人機的飛行曲線有較大的影響，用來控制曲線曲度。

基于導引律（5），則式（3）、式（4）可表示為：

當引入比例導引律時，要求在初始時刻ψrel(t0)滿足：

但是，在大多數情況下，當選定參數N及c后， ψrel(t0)并不滿足式（8），因此引入的比例導引律并不能直接應用。

為解決這個問題，本文引入前置調節過程。在調節過程中，通過調節相對速度的航向角使其滿足式（8）。

通過選擇合適的參數b0，使ψrel(t0)滿足：

引入指數函數的目的是利用指數函數隨時間的衰減特性來調節相對速度的航向角，這樣在相對速度下，無人機的相對飛行軌跡就經過兩個過程：

1）基于時變偏差函數的飛行過程，也就是前置調節過程，相對飛行軌跡以給定的曲率飛行，同時曲率半徑是時變的。

2）基于常值偏差函數的飛行過程，當調節函數等于零時，比例導引中偏差值為常值，隨著導引律的作用，相對速度航向角趨向于定值，并且到達目標點。此結論的證明見下節。

3 穩定性證明

基于導引律（9），則式（3）、式（4）可表示為：

由分析知，隨時間t→∝，指數項b0e-b1t→0。

因此由式 （12） 得， θBu的平衡點為：或者

基于線性理論，有：

由vrel＞0，rBu＞0，N＞1，知 A＜0,B＞0,因此可得為全局穩定點，即隨時間 t→∝，。

因此，由上面分析可知，當無人機到達避障點B時，相對速度航向角，若等于避障點B對應的切線角θtan，則避障完成。

因此，避障算法設計如下：

1）當無人機發現障礙物時，若相對速度位于避障錐內，需進行避障。考慮原路徑是基于最終到達目標設計的，因此對路徑的改變越小越好。因此，基于相對速度角度變化最小原則，選擇相對速度調整方向。針對圖1，選擇相對速度調整方向為切線UB，因此避障點為B。

2）計算避障點B對應的障礙圓切線角度θtan。

3）設計線性比例導引律如式（9）所示，確定參數N、c、b0、b1，使導引律滿足初始條件式（10）及使相對速度穩定航向角ψrel() tf與避障點B對應的切線角相等，即。

4 無人機航向角控制

避障算法設計的是關于相對速度航向角ψrel(t)的導引律，需要轉換成與無人機航向角ψuav的關系。假設無人機速度大小不變，障礙物勻速運動。建立速度關系示意圖如圖2所示。

圖2 無人機與障礙物速度關系示意圖Fig.2 The scheme of velocity relation of UAV and obstacle

設vrel與vuav之間的夾角為α，vuav與vobs之間的夾角為β，則由正弦定理得：

又知 β=ψuav+ψ0bs，代入式（13）得：

即：

由vuav、vobs、ψobs為常值，可知ψuav為：

ψuav、ψobs、ψrel均為標量正值。

因此，若知道了ψrel的變化規律，就知道了ψuav的變化規律。

5 仿真分析

為驗證上述算法的有效性，對算法進行仿真。

條件1，假設障礙物為靜止，vobs=0m/s。

取障礙物坐標為xobs0=200km，yobs0=200km，無人機坐標為xuav0=0km，yuav0=0km，無人機初始航向角為 ψuav0=0.8/rad ，速度為vuav0=200m8/s，為顯化避障效果，假設擴大后的障礙物圓半徑RP=100km。比例導引系數N=3，無人機避障路徑如圖3所示。

圖3 無人機避開靜態障礙物示意圖Fig.3 The scheme of UAV avoiding static obstacle

條件2，假設障礙物為運動障礙物，vobs=80m/s ，ψobs=0/rad 其余條件不變，則無人機避障示意圖如圖4、圖5所示。

圖4 無人機避開動態障礙物示意圖（一）Fig.4 The scheme of UAV avoiding dynamic obstacle(1)

圖5 無人機避開動態障礙物示意圖（二）Fig.5 The scheme of UAV avoiding dynamic obstacle(2)

圖4所示為無人機與障礙物相切示意圖，圖5所示為無人機與障礙物相切后繼續運動，即障礙物繼續沿X軸水平向右運動，無人機以ψuav(tf)角度勻速運動一段時間后示意圖，由圖5知下一刻無人機與障礙物分離，知無人機成功避開運動障礙物。

由仿真結果圖3、圖4、圖5可知，設計的避障算法可成功避開靜止及運動障礙物，證明了設計算法的有效性。

6 結論

本文基于無人機與障礙物之間的幾何關系，得到無人機與障礙物在極坐標系下的運動方程，通過引入線性比例導引律，設計避障算法，仿真結果顯示無人機能夠順利避開靜態及動態障礙物，證明了算法的有效性。

本算法的實現僅需要知道障礙物及無人機的位置及速度這4個參數，因此，算法操作起來比較方便。下一步的工作是該算法擴展到三維空間，并對障礙物是變加速的情況進行研究。

[1] O Khatib.Real time collision avoidance for manipulatorsand mobile robots[J].The International Journal of Robotics Research,1986,5(1):90–98.

[2] M Erdmann,T.Lozano-Perez.On multiple moving objects[J].Algorithmica,1987,2(1):477–521.

[3] K Fujimura,H Samet.Roadmap-based motion planning in dynamic environments[J].IEEE Trans.Robot, 2005,21(5):885–897.

[4] S Ge,Y Cui.Dynamic motion planning for mobile robotsusing potential field method[J].Auton Robots,2002, 13(3):207–233.

[5] M Seder,I Petrovic.Dynamic window based approach to mobile robot motion control in the presence of moving ob stacles[C].Conf.Robot.Autom,Roma,Italy, 2007:1986–1992.

[6] A Chakravarthy,D Ghose.Obstacle avoidance in dynamic environment:a collision cone approach[J].IEEE Trans.Syst.,Man Cybern.A,Syst.,Humans,1998,28 (5):562–574.

[7] F Belkhouche,B Belkhouche.K inematics based charac terization of the collision course[J].Robot.Autom,2008, 23(2):127–136.

[8] 朱齊丹,鐘訓昱,張智.基于速度變化空間的移動機器人動態避碰規[J].機器人,2009,31(6):539–547.

[9] F Large,S Sckhavat,Z Shiller,C Laugier.Using nonlinear velocity obstacles to plan motions in a dynamicenvironment[C].Conf.Control,Autom,Robot.Vis, 2002:734–739.

[10] van den Berg J,Lin M,Manocha D.Reciprocal velocity obstacles for real-time multi-agent navigation[C].2008 IEEE International Conference on Robotics and Automation,2008:1928–1935.

Study onAngle Linearization of Proportional Navigation-based CollisionAvoidance for UAV

YANG Xiu-xia，LIU Xiao-wei，ZHANG Yi
（NavalAeronautical andAstronautical University Department of Control Engineering，Shandong Yantai 264001，China）

Based on the geometric relationship between the UAV and the obstacles,we estublish the kinematics equations between the UAV and the obstacles in polar coordinates.By introducing a navigation law of angle linearization of proportional navigation,we design an algorithm of collision avoidance to make the UAV successfully avoid the obstacles.The angle linearization of proportional navigation law is the function of the angle of the relative velocity of the UAV with the angle of the line of sight of the obstacle avoidance point,which is composed of the proportional function and the deviation function.To solve the problem that the angle of the relative velocity does not meet the law at the initial time,the paper adds exponential function in the deviation function.By proving the stability of the algorithm,we obtained the ranges of the parameters which meet the requirements.Finally,we make the simulation of the algorithm,and the simulation results show the effectiveness of the algorithm.

Angle linearization;Proportional navigation;Deviation function;Collision avoidance

V448.16

A

2095-8110（2014）03-0001-05

2014–05–06；

2014–06–27。

航空科學基金（20135584010）

楊秀霞（1975–），女，博士，碩導，主要從事飛行器導航制導與控制方面的研究。E-mail:yangxiuxia@126.com