楊秀霞 周硙硙 張毅 華偉 孟啟源

（1.海軍航空工程學院控制工程系煙臺264001）（2.海軍航空工程學院研究生管理大隊煙臺264001）

一種三維空間UAV自主避障算法研究?

楊秀霞1周硙硙2張毅1華偉2孟啟源2

（1.海軍航空工程學院控制工程系煙臺264001）（2.海軍航空工程學院研究生管理大隊煙臺264001）

在復雜環(huán)境下，為提升UAV空間執(zhí)行任務的安全性和可靠性，提出一種三維空間UAV自主避障方法，建立三維空間碰撞錐和速度障礙錐模型，確定UAV空間自主避障的最小速度矢量偏轉角，并求解了UAV的避障點。同時，將確定的避障點和避障方向作為UAV三維五次Pythagorean Hodograph（PH）避障重規(guī)劃路徑的初始化條件進行仿真驗證，仿真結果驗證了自主避障方法的有效性和可行性。

UAV；自主避障；空間速度障礙；路徑重規(guī)劃

Class NumberTP301

1 引言

近年來，許多學者對UAV在動靜態(tài)環(huán)境中的自主避障算法進行了研究，提高UAV在復雜環(huán)境中對威脅障礙物的“感知和避碰”能力［1］，如何提高UAV在三維空間復雜環(huán)境中執(zhí)行艱巨任務的生存能力成為UAV技術研究中亟待解決的關鍵問題之一［2］。為UAV配備高性能的自主避障系統(tǒng)可實現(xiàn)UAV在復雜環(huán)境下對動靜態(tài)障礙物的避碰，即要求自主避障系統(tǒng)配備的自主避障算法能實現(xiàn)對動靜態(tài)威脅障礙物的避碰；另外，提升UAV的自主避障系統(tǒng)性能，可提高UAV在復雜動態(tài)環(huán)境中執(zhí)行任務的生存能力。

目前，針對三維空間自主避障的研究文獻相對較少。學者J.Ellerbroek［3～4］對三維空間中飛行器之間的相互避碰分離進行了研究，通過空間障礙錐進行建模，并采用降維方式實現(xiàn)避碰分離。F.Belk?houche［5］針對三維空間靜態(tài)威脅障礙物避碰，采用了平面降維的思想，利用避障圓錐法實現(xiàn)了避障，并采用擴展比例導引律進行避障導引，在導引律設計參數(shù)中考慮了飛行器的飛行速度、角速度等約束限制，可產生非直線導引航跡，但文中沒考慮對動態(tài)威脅障礙物的避碰。文獻［6］同樣采用降維避障方法，利用避碰障礙錐，對三維避碰采用比例導引方法實現(xiàn)。文獻［7］采用三維勢場法避障，也存在跟二維勢場法相似地缺陷，會陷入局部最優(yōu)，并產生無效的運動。文獻［8］基于流水避石原理實現(xiàn)對三維空間靜態(tài)障礙物的避碰，并給出相應的避障路徑規(guī)劃方法。

本文通過建立三維空間碰撞錐和三維空間速度障礙模型，在此基礎上確定UAV對威脅障礙物避碰的空間最小速度矢量偏轉角。同時，也對UAV的空間避障點進行了求解，并將其作為三維五次PH避障重規(guī)劃路徑的初始條件，實現(xiàn)UAV對威脅障礙物的避障路徑重規(guī)劃，提升UAV在復雜環(huán)境中執(zhí)行任務的安全性和可靠性。

2 三維空間碰撞錐的建立

UAV在執(zhí)行任務的過程中，利用自身攜帶的傳感器裝置，可探測到周圍環(huán)境中存在的動靜態(tài)障礙物。并可獲取動靜態(tài)障礙物的位姿信息Pose3obs()

PO，vo；同時，UAV的飛行傳感器也可實時地獲取自身的位姿信息Pose3uav() PU，vu。動靜態(tài)障礙物的位姿信息Pose3obs() PO，vo和UAV自身的位姿信息Pose3uav() PU，vu可表示為

式中：vo和vu分別為障礙物和UAV的速度矢量的大??；θo、φo和θu、φu分別為障礙物和UAV的速度矢量方向角。

另外，根據(jù)UAV攜帶的傳感器性能，設定其探測距離為d0；為簡化UAV對威脅障礙物的避碰，將UAV簡化為一點，障礙物相應的“膨脹”為一個球體，障礙球的半徑記作R，且半徑R由UAV自身的尺寸和障礙物的威脅輻射范圍確定。過UAV的位置坐標點PU作障礙球PO的切線，所有障礙球PO的切線形成三維空間錐面，而障礙球上所有的切點形成切線圓（Tangent Circle，TC），相應的UAV與障礙球之間的空間碰撞錐（3-Dimensional Collision Cone，3-DCC）如圖1所示。

圖1三維空間碰撞錐示意圖

當UAV感知到周圍環(huán)境中存在的動靜態(tài)障礙物時，需要判定障礙物的威脅性。為簡化對UAV感知到的障礙物威脅性判定，依據(jù)示意圖1中的矢量幾何關系，確定矢量之間的夾角，從而給出UAV對障礙物威脅性判定的結論。

結論1：當α≥αo時，感知到的障礙物不具有威脅性；當α＜αo時，感知到的障礙物具有威脅性。其中，α為相對速度矢量vuo與3-DCC的軸線矢量之間的夾角；αo為空間障礙錐的半頂角。

而α與αo可由下面的關系式進行求解，

為實現(xiàn)UAV對威脅障礙物的避碰，需要將位于3-DCC內的相對速度矢量vuo偏轉出3-DCC；而UAV在實際避碰過程中控制UAV的速度矢量vu，那么需要對3-DCC進行轉化。為簡化UAV空間避障模型，將3-DCC平移障礙物速度矢量vo，得到三維空間速度障礙（3-D Velocity Obstacle，3-DVO），示意圖如圖2所示。

3 UAV空間自主避障方向的確定

作平面P3-DVOK1K2，其中，平面P3-DVOK1K2由矢量vuo和確定；K1和K2為平面P3-DVOK1K2與TC的交點，如圖3所示。另外，平面P3-DVOK1K2與3-DVO錐面的交線為P3-DVOK1、P3-DVOK2，交線P3-DVOK1、P3-DVOK2也為3-DVO錐面的母線。當UAV的速度矢量vu偏轉至母線P3-DVOK1、P3-DVOK2上時，該UAV速度矢量vu為臨界速度矢量，對應的相對速度矢量vuo位于母線P3-DVOK1、P3-DVOK2上，因此UAV可實現(xiàn)對威脅障礙物的避碰。UAV的速度矢量vu與母線P3-DVOK1、P3-DVOK2相交的臨界狀態(tài)點A1、A2，且臨界狀態(tài)點A1、A2可由下列關系式進行求解：

圖2三維空間速度障礙示意圖

圖3UAV避障臨界狀態(tài)點示意圖

根據(jù)式（8）～（11）可確定臨界狀態(tài)點A1(x1，y1，z1)的坐標，臨界狀態(tài)點A2(x2，y2，z2)的坐標可類似確定。那么，對應臨界狀態(tài)點A1、A2的避障速度矢量也可確定。此時，臨界狀態(tài)點A1對應的UAV速度矢量因避障而偏轉空間角度最?。慌R界狀態(tài)點A2對應的速度矢量因避障而偏轉空間角度最大。

4 UAV自主避障算法仿真驗證

將在第三節(jié)確定的UAV空間自主避障速度矢量方向應用到三維五次PH曲線［9］路徑規(guī)劃，并進行仿真驗證。為實現(xiàn)UAV對威脅障礙物的避碰PH路徑重規(guī)劃，需要確定避障點Pavo及避障方向，并以此作為三維五次PH避障路徑重規(guī)劃的初始化條件。文獻［10］研究了三維空間UAV可飛行PH路徑規(guī)劃方法，考慮了三維空間UAV路徑規(guī)劃的曲率、撓率和爬升角約束條件，并給出了規(guī)劃三維可飛行PH路徑參數(shù)選取方法。在此基礎上，對UAV空間自主避障算法進行仿真驗證。由于UAV自主避障方向在第3節(jié)已經確定，下面對UAV的避障點Pavo進行求解。

首先，UAV對威脅障礙物的避碰時間可確定為

則避碰點Pavo可表示為

在復雜環(huán)境下，UAV執(zhí)行從起飛點Ps(0，0，0)至目標點Pf(600，400，125)的飛行任務。UAV在飛行過程中利用自身攜帶的傳感器裝置探測到周圍環(huán)境中存在的障礙物，UAV需要對威脅障礙物進行避碰。表1給出了UAV對威脅障礙物O1避碰的初始化條件。

表1 威脅障礙物避碰的初始化條件

根據(jù)表1中UAV和威脅障礙物O1的初始化條件，可確定UAV對威脅障礙物O1的最小空間避碰速度矢量方向為Qavo(28.8，11.9)，避碰點為Pavo(216，180，41.7)。將其應用于PH曲線路線規(guī)劃的仿真結果如圖4所示，仿真結果表明提出的三維空間UAV自主避障算法能夠實現(xiàn)對威脅障礙物的避碰。

圖4UAV對威脅障礙物避碰仿真結果

5 結語

本文通過建立三維空間碰撞錐和速度障礙模型，確定了UAV對空間威脅障礙物避碰的最小速度矢量偏轉角，并求解了避障點的坐標。同時，將得到的避障點和避障方向作為三維五次PH避障路徑重規(guī)劃的初始化條件，進行了相應的仿真驗證。仿真結果表明：提出的UAV空間自主避障方法能夠實現(xiàn)對威脅障礙物的避碰，對UAV在復雜環(huán)境下執(zhí)行艱巨任務的安全性和生存能力具有重要理論意義。

［1］齊曉慧，田慶民，甄紅濤.無人飛行器系統(tǒng)的感知與規(guī)避——研究與應用［M］.北京：國防工業(yè)出版社，2014，154-177. QI Xiaohui，TIAN Qingmin，ZHEN Hongtao.Sense and Avoid in UAS：Research and Applications［M］.Beijing：National Defense Industry Press，2014，154-177.

［2］周歡，魏瑞軒，崔軍輝，等.面向不確定性環(huán)境的多無人機協(xié)同防撞［J］.電光與控制，2014，21（1）：91-96. ZHOU Huan，WEI Ruixuan，CUI Jiahui，et al.Multi-UAV Cooperative Collision Avoidance Against Uncertain Envi?ronment，Electronics Optics and Control，2014，21（1）：91-96.

［3］Ellerbroek J，Brantegem K C R，van Paassen M M，et al. Design of a coplanar airborne separation display［J］.IEEE TransactionsonHuman-MachineSystems，2013，43（3）：277-289.

［4］Ellerbroek J，Visser M，Van Dam S B J，et al.Design of an airborne three-dimensional separation assistance dis?play［J］.IEEE Transactions on Systems，Man，and Cyber?netics-Part A：Systems and Humans，2011，41（5）：863-875.

［5］F.Belkhouche，B.Bendjilali.Reactive Path Planning for 3-D Autonomous Vehicles［J］.IEEE Transactions on Con?trol Systems Technology，2012，20（1）：249-256.

［6］Su-Cheol Han，Hyochoong Banz，Chang-sun Yoo.Pro?portional Navigation-Based Collision Avoidance［J］.Inter?national Journal of Control，Automation，and Systems，2009，7（4）：553-565.

［7］洪曄，邊信黔.基于三維速度勢場的AUV局部避碰研究［J］.機器人，2007，29（1）：88-91. HONG Ye，BIAN Xinqian.An AUV Obstacle-Avoidan?ceM ethod Based on Three-di mensional［J］.Robot，2007，29（1）：88-91.

［8］梁宵，王紅倫，李大偉，等.基于流水避石原理的無人機三維航路規(guī)劃方法［J］.航空學報，2013，34（7）：1670-1681. LIANG Xiao，WANG Honglun，LI Dawei，et al.Three-di?mensional path planning for unmanned aerial vehicles based on principles of stream avoiding obstacles［J］.Acta Aeronauticaet AstronauticaSinica，2013，34（7）：1670-1681.

［9］Farouki R T，Sakkalis T.Pythagorean hodographs［J］. IBM Journal of Research and Development，1990，34（5）：736-752.

［10］Zhang Yi，Yang Xiuxia，Zhou Weiwei.Flyable path plan?ning for multiple UAVs in complicated threat environ?ment［C］//Multisensor Fusion and Information Integra?tion for Intelligent Systems（MFI），2014 International Conference on.IEEE，2014：1-5.

A Three-dimensional Autonomous Obstacle Avoidance Algorithm for UAV

YANG Xiuxia1ZHOU Weiwei2ZHANG Yi1HUA Wei2MENG Qiyuan2
（1.Department of Control Engineering，Naval Aeronautical and Astronautical University，Yantai264001）（2.Graduate Students'Brigade，Naval Aeronautical and Astronautical University，Yantai264001）

In order to improve the security and reliability of UAV's space mission in complex environment，a 3-D UAV auton?omous obstacle avoidance algorithm is proposed in the paper.3-D models of collision cone and velocity obstacle are established，the minimum velocity vector deflection angle of spatial autonomous obstacle avoidance is determined for UAV，and the obstacle avoid?ance point of UAV is solved.At the same time，the UAV's 3-D quintic Pythagorean Hodograph（PH）obstacle avoidance planning path is simulated and verified with the determined obstacle avoidance point and the direction of obstacle avoidance as the initial con?ditions，the simulation results verify the effectiveness and feasibility of the autonomous obstacle avoidance method.

UAV，autonomous obstacle avoidance，spatial velocity obstacle，path re-planning

TP301

10.3969/j.issn.1672-9722.2017.07.015

2017年1月14日，

2017年2月17日

楊秀霞，女，博士，副教授，研究方向：飛行器智能控制及制導。周硙硙，男，碩士，研究方向：飛行器導航、制導與控制。張毅，男，博士，副教授，研究方向：飛行器導航、制導與控制。華偉，男，碩士，研究方向：飛行器導航、制導與控制。孟啟源，男，碩士，研究方向：飛行器導航、制導與控制。