楊秀霞，劉小偉，張 毅

（海軍航空工程學(xué)院控制工程系，山東煙臺(tái)264001）

基于比例導(dǎo)引角度線性化的無(wú)人機(jī)避障研究

楊秀霞，劉小偉，張 毅

（海軍航空工程學(xué)院控制工程系，山東煙臺(tái)264001）

基于無(wú)人機(jī)與障礙物之間的幾何關(guān)系，在極坐標(biāo)系下建立了無(wú)人機(jī)與障礙物之間的運(yùn)動(dòng)學(xué)方程，通過(guò)引入基于角度線性化的比例導(dǎo)引律，使無(wú)人機(jī)能夠順利避開(kāi)障礙物。引入的基于角度線性化的比例導(dǎo)引律是關(guān)于無(wú)人機(jī)相對(duì)速度航向角與避障點(diǎn)視線角的函數(shù)，由比例函數(shù)及偏差函數(shù)組成。為解決初始時(shí)刻相對(duì)速度航向角不滿足引入導(dǎo)引律的問(wèn)題，在調(diào)節(jié)函數(shù)中加入了指數(shù)項(xiàng)。通過(guò)穩(wěn)定性證明，得到了滿足要求的導(dǎo)引律參數(shù)取值范圍。最后對(duì)設(shè)計(jì)的避障算法進(jìn)行了仿真，仿真結(jié)果驗(yàn)證了算法的有效性。

角度線性化；比例導(dǎo)引；偏差函數(shù)；避障

0 引言

目前，無(wú)人機(jī)在軍事及民用領(lǐng)域已得到越來(lái)越廣泛的應(yīng)用。如何使無(wú)人機(jī)能夠安全導(dǎo)引到目標(biāo)并順利避開(kāi)動(dòng)態(tài)障礙物是眾多學(xué)者研究的重點(diǎn)課題。

最早對(duì)這一問(wèn)題開(kāi)始研究的是O.Khatib[1]、Erdmann和Lozano-Perez[2]等人。后來(lái)，學(xué)者們把在靜態(tài)環(huán)境下得到成功應(yīng)用的路徑規(guī)劃及避障方法推廣到動(dòng)態(tài)環(huán)境中去，如在文獻(xiàn)[3]中提到了確定及隨機(jī)的路徑地圖方法，在文獻(xiàn)[4]中通過(guò)考慮機(jī)器人及障礙物的速度，并加入到勢(shì)能函數(shù)的構(gòu)造中，得到適用于動(dòng)態(tài)環(huán)境的勢(shì)場(chǎng)法，在文獻(xiàn)[5]中通過(guò)動(dòng)態(tài)窗口方法進(jìn)行路徑規(guī)劃及避障。這些方法在靜態(tài)環(huán)境下得到較好的結(jié)果，但是在動(dòng)態(tài)環(huán)境下，尤其是應(yīng)用于無(wú)人機(jī)高速運(yùn)動(dòng)這一特殊情況下，避障效果并不理想。其原因主要是在動(dòng)態(tài)環(huán)境下，碰撞判斷及避障情況下對(duì)速度的控制與靜態(tài)環(huán)境相比存在較大區(qū)別。

為解決動(dòng)態(tài)環(huán)境下的碰撞判斷問(wèn)題，文獻(xiàn)[6]、[7]提出了基于運(yùn)動(dòng)學(xué)方程的碰撞判斷方法。文獻(xiàn)[6]定義了碰撞錐的概念，依據(jù)視線的距離及角速度來(lái)確定碰撞條件。在文獻(xiàn)[7]中基于視線角速度方程，對(duì)直接碰撞和間接碰撞進(jìn)行了比較。

同時(shí)考慮到動(dòng)態(tài)環(huán)境下障礙物速度變化的問(wèn)題，學(xué)者們提出了速度障礙的概念。例如文獻(xiàn)[8]在考慮運(yùn)動(dòng)障礙物速度的基礎(chǔ)上進(jìn)行碰撞錐的構(gòu)建，在此基礎(chǔ)上，通過(guò)使機(jī)器人的速度轉(zhuǎn)移到碰撞錐之外達(dá)到避障目的，但是對(duì)障礙物的速度要求必須是線性的。為解決障礙物速度為不定值的避障問(wèn)題，文獻(xiàn)[9]通過(guò)對(duì)某一時(shí)刻，在考慮無(wú)人機(jī)及障礙物相對(duì)位置情況下，對(duì)可能的碰撞速度進(jìn)行時(shí)間積分，得到非線性速度障礙集進(jìn)行避障。同時(shí)考慮到多無(wú)人機(jī)之間的避碰問(wèn)題，文獻(xiàn)[10]提出了互惠速度障礙的概念，即兩個(gè)即將相碰的障礙物之間互相主動(dòng)避碰，達(dá)到避碰目的。

在對(duì)上述方法分析和總結(jié)的基礎(chǔ)上，本文提出了基于角度線性化的比例導(dǎo)引無(wú)人機(jī)避障算法。首先在極坐標(biāo)系下建立無(wú)人機(jī)與障礙物的相對(duì)運(yùn)動(dòng)學(xué)方程，通過(guò)引入基于角度線性化的導(dǎo)引律進(jìn)行避障算法設(shè)計(jì)?？紤]到一般情況下無(wú)人機(jī)相對(duì)速度初始航向角不滿足給定導(dǎo)引律的問(wèn)題，通過(guò)引入指數(shù)偏差函數(shù)對(duì)航向角進(jìn)行調(diào)節(jié)，使無(wú)人機(jī)的最終穩(wěn)定航向角達(dá)到避障要求。通過(guò)對(duì)設(shè)計(jì)算法穩(wěn)定性證明，得到了滿足要求的各個(gè)參數(shù)取值范圍。最后本文利用Matlab對(duì)設(shè)計(jì)的避障算法進(jìn)行了仿真，結(jié)果驗(yàn)證了算法的有效性。

1 幾何關(guān)系及運(yùn)動(dòng)學(xué)方程

1.1 問(wèn)題描述

當(dāng)無(wú)人機(jī)與障礙物在慣性空間運(yùn)動(dòng)時(shí)，建立無(wú)人機(jī)與障礙物的幾何關(guān)系示意圖，如圖1所示。無(wú)人機(jī)的速度為vuav，方向?yàn)棣譽(yù)av，障礙物的速度為vobs，方向?yàn)棣護(hù)bs。無(wú)人機(jī)相對(duì)障礙物的速度為vrel，方向?yàn)棣譺el。由速度障礙原理，通過(guò)擴(kuò)大障礙圓半徑，把無(wú)人機(jī)簡(jiǎn)化為質(zhì)點(diǎn)U，基于質(zhì)點(diǎn)U及障礙圓切線UB、UA建立碰撞區(qū)域UAB。若相對(duì)速度vrel位于碰撞區(qū)域UAB內(nèi)，則存在碰撞危險(xiǎn)，因此需要調(diào)整無(wú)人機(jī)的速度的大小及方向，使無(wú)人機(jī)的相對(duì)速度vrel位于碰撞區(qū)域UAB之外。調(diào)整時(shí)，一般考慮相對(duì)速度vrel角度變化最小即避障完成時(shí)間短、偏離原目標(biāo)軌跡短原則，結(jié)合圖1，選取避障切線為UB，即避障點(diǎn)為B。

本文提出的避障算法是設(shè)計(jì)關(guān)于無(wú)人機(jī)相對(duì)速度的航向角ψrel的導(dǎo)引律，使無(wú)人機(jī)能夠到達(dá)避障點(diǎn)B，且相對(duì)速度方向ψrel與切線UB方向θBu相等，確保避障完成。

圖1 無(wú)人機(jī)與障礙物幾何關(guān)系示意圖Fig.1 The scheme of geometric relation of UAV and obstacle

1.2 建立運(yùn)動(dòng)學(xué)方程

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

為研究方便，我們建立無(wú)人機(jī)與障礙物的相對(duì)運(yùn)動(dòng)方程，此時(shí)無(wú)人機(jī)以相對(duì)速度vrel飛向避障點(diǎn)B：

式（3）給出了無(wú)人機(jī)與避障點(diǎn)B之間的距離變化關(guān)系，式（4）給出了無(wú)人機(jī)與避障點(diǎn)B之間的視線角的變化方程。

2 角度線性化比例導(dǎo)引律

本文基于角度線性化比例導(dǎo)引律對(duì)避障算法進(jìn)行設(shè)計(jì)。選擇比例導(dǎo)引律最基本的定義，即相對(duì)速度vrel的航向角速度與目標(biāo)視線UB的角速度成比例，即。

其中N為比例導(dǎo)引系數(shù)，滿足N＞1?？紤]初始狀態(tài)有：

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

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

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

c對(duì)無(wú)人機(jī)的飛行曲線有較大的影響，用來(lái)控制曲線曲度。

基于導(dǎo)引律（5），則式（3）、式（4）可表示為：

當(dāng)引入比例導(dǎo)引律時(shí)，要求在初始時(shí)刻ψrel(t0)滿足：

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

為解決這個(gè)問(wèn)題，本文引入前置調(diào)節(jié)過(guò)程。在調(diào)節(jié)過(guò)程中，通過(guò)調(diào)節(jié)相對(duì)速度的航向角使其滿足式（8）。

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

引入指數(shù)函數(shù)的目的是利用指數(shù)函數(shù)隨時(shí)間的衰減特性來(lái)調(diào)節(jié)相對(duì)速度的航向角，這樣在相對(duì)速度下，無(wú)人機(jī)的相對(duì)飛行軌跡就經(jīng)過(guò)兩個(gè)過(guò)程：

1）基于時(shí)變偏差函數(shù)的飛行過(guò)程，也就是前置調(diào)節(jié)過(guò)程，相對(duì)飛行軌跡以給定的曲率飛行，同時(shí)曲率半徑是時(shí)變的。

2）基于常值偏差函數(shù)的飛行過(guò)程，當(dāng)調(diào)節(jié)函數(shù)等于零時(shí)，比例導(dǎo)引中偏差值為常值，隨著導(dǎo)引律的作用，相對(duì)速度航向角趨向于定值，并且到達(dá)目標(biāo)點(diǎn)。此結(jié)論的證明見(jiàn)下節(jié)。

3 穩(wěn)定性證明

基于導(dǎo)引律（9），則式（3）、式（4）可表示為：

由分析知，隨時(shí)間t→∝，指數(shù)項(xiàng)b0e-b1t→0。

因此由式 （12） 得， θBu的平衡點(diǎn)為：或者

基于線性理論，有：

由vrel＞0，rBu＞0，N＞1，知 A＜0,B＞0,因此可得為全局穩(wěn)定點(diǎn)，即隨時(shí)間 t→∝，。

因此，由上面分析可知，當(dāng)無(wú)人機(jī)到達(dá)避障點(diǎn)B時(shí)，相對(duì)速度航向角，若等于避障點(diǎn)B對(duì)應(yīng)的切線角θtan，則避障完成。

因此，避障算法設(shè)計(jì)如下：

1）當(dāng)無(wú)人機(jī)發(fā)現(xiàn)障礙物時(shí)，若相對(duì)速度位于避障錐內(nèi)，需進(jìn)行避障?？紤]原路徑是基于最終到達(dá)目標(biāo)設(shè)計(jì)的，因此對(duì)路徑的改變?cè)叫≡胶?。因此，基于相?duì)速度角度變化最小原則，選擇相對(duì)速度調(diào)整方向。針對(duì)圖1，選擇相對(duì)速度調(diào)整方向?yàn)榍芯€UB，因此避障點(diǎn)為B。

2）計(jì)算避障點(diǎn)B對(duì)應(yīng)的障礙圓切線角度θtan。

3）設(shè)計(jì)線性比例導(dǎo)引律如式（9）所示，確定參數(shù)N、c、b0、b1，使導(dǎo)引律滿足初始條件式（10）及使相對(duì)速度穩(wěn)定航向角ψrel() tf與避障點(diǎn)B對(duì)應(yīng)的切線角相等，即。

4 無(wú)人機(jī)航向角控制

避障算法設(shè)計(jì)的是關(guān)于相對(duì)速度航向角ψrel(t)的導(dǎo)引律，需要轉(zhuǎn)換成與無(wú)人機(jī)航向角ψu(yù)av的關(guān)系。假設(shè)無(wú)人機(jī)速度大小不變，障礙物勻速運(yùn)動(dòng)。建立速度關(guān)系示意圖如圖2所示。

圖2 無(wú)人機(jī)與障礙物速度關(guān)系示意圖Fig.2 The scheme of velocity relation of UAV and obstacle

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

又知 β=ψu(yù)av+ψ0bs，代入式（13）得：

即：

由vuav、vobs、ψo(hù)bs為常值，可知ψu(yù)av為：

ψu(yù)av、ψo(hù)bs、ψrel均為標(biāo)量正值。

因此，若知道了ψrel的變化規(guī)律，就知道了ψu(yù)av的變化規(guī)律。

5 仿真分析

為驗(yàn)證上述算法的有效性，對(duì)算法進(jìn)行仿真。

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

取障礙物坐標(biāo)為xobs0=200km，yobs0=200km，無(wú)人機(jī)坐標(biāo)為xuav0=0km，yuav0=0km，無(wú)人機(jī)初始航向角為 ψu(yù)av0=0.8/rad ，速度為vuav0=200m8/s，為顯化避障效果，假設(shè)擴(kuò)大后的障礙物圓半徑RP=100km。比例導(dǎo)引系數(shù)N=3，無(wú)人機(jī)避障路徑如圖3所示。

圖3 無(wú)人機(jī)避開(kāi)靜態(tài)障礙物示意圖Fig.3 The scheme of UAV avoiding static obstacle

條件2，假設(shè)障礙物為運(yùn)動(dòng)障礙物，vobs=80m/s ，ψo(hù)bs=0/rad 其余條件不變，則無(wú)人機(jī)避障示意圖如圖4、圖5所示。

圖4 無(wú)人機(jī)避開(kāi)動(dòng)態(tài)障礙物示意圖（一）Fig.4 The scheme of UAV avoiding dynamic obstacle(1)

圖5 無(wú)人機(jī)避開(kāi)動(dòng)態(tài)障礙物示意圖（二）Fig.5 The scheme of UAV avoiding dynamic obstacle(2)

圖4所示為無(wú)人機(jī)與障礙物相切示意圖，圖5所示為無(wú)人機(jī)與障礙物相切后繼續(xù)運(yùn)動(dòng)，即障礙物繼續(xù)沿X軸水平向右運(yùn)動(dòng)，無(wú)人機(jī)以ψu(yù)av(tf)角度勻速運(yùn)動(dòng)一段時(shí)間后示意圖，由圖5知下一刻無(wú)人機(jī)與障礙物分離，知無(wú)人機(jī)成功避開(kāi)運(yùn)動(dòng)障礙物。

由仿真結(jié)果圖3、圖4、圖5可知，設(shè)計(jì)的避障算法可成功避開(kāi)靜止及運(yùn)動(dòng)障礙物，證明了設(shè)計(jì)算法的有效性。

6 結(jié)論

本文基于無(wú)人機(jī)與障礙物之間的幾何關(guān)系，得到無(wú)人機(jī)與障礙物在極坐標(biāo)系下的運(yùn)動(dòng)方程，通過(guò)引入線性比例導(dǎo)引律，設(shè)計(jì)避障算法，仿真結(jié)果顯示無(wú)人機(jī)能夠順利避開(kāi)靜態(tài)及動(dòng)態(tài)障礙物，證明了算法的有效性。

本算法的實(shí)現(xiàn)僅需要知道障礙物及無(wú)人機(jī)的位置及速度這4個(gè)參數(shù)，因此，算法操作起來(lái)比較方便。下一步的工作是該算法擴(kuò)展到三維空間，并對(duì)障礙物是變加速的情況進(jìn)行研究。

[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] 朱齊丹,鐘訓(xùn)昱,張智.基于速度變化空間的移動(dòng)機(jī)器人動(dòng)態(tài)避碰規(guī)[J].機(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。

航空科學(xué)基金（20135584010）

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