一种三维空间UAV自主避障算法研究∗

杨秀霞 周硙硙 张毅 华伟 孟启源

（1.海军航空工程学院控制工程系烟台264001）（2.海军航空工程学院研究生管理大队烟台264001）

一种三维空间UAV自主避障算法研究∗

杨秀霞1周硙硙2张毅1华伟2孟启源2

（1.海军航空工程学院控制工程系烟台264001）（2.海军航空工程学院研究生管理大队烟台264001）

在复杂环境下，为提升UAV空间执行任务的安全性和可靠性，提出一种三维空间UAV自主避障方法，建立三维空间碰撞锥和速度障碍锥模型，确定UAV空间自主避障的最小速度矢量偏转角，并求解了UAV的避障点。同时，将确定的避障点和避障方向作为UAV三维五次Pythagorean Hodograph（PH）避障重规划路径的初始化条件进行仿真验证，仿真结果验证了自主避障方法的有效性和可行性。

UAV；自主避障；空间速度障碍；路径重规划

Class NumberTP301

1 引言

近年来，许多学者对UAV在动静态环境中的自主避障算法进行了研究，提高UAV在复杂环境中对威胁障碍物的“感知和避碰”能力［1］，如何提高UAV在三维空间复杂环境中执行艰巨任务的生存能力成为UAV技术研究中亟待解决的关键问题之一［2］。为UAV配备高性能的自主避障系统可实现UAV在复杂环境下对动静态障碍物的避碰，即要求自主避障系统配备的自主避障算法能实现对动静态威胁障碍物的避碰；另外，提升UAV的自主避障系统性能，可提高UAV在复杂动态环境中执行任务的生存能力。

目前，针对三维空间自主避障的研究文献相对较少。学者J.Ellerbroek［3～4］对三维空间中飞行器之间的相互避碰分离进行了研究，通过空间障碍锥进行建模，并采用降维方式实现避碰分离。F.Belk⁃houche［5］针对三维空间静态威胁障碍物避碰，采用了平面降维的思想，利用避障圆锥法实现了避障，并采用扩展比例导引律进行避障导引，在导引律设计参数中考虑了飞行器的飞行速度、角速度等约束限制，可产生非直线导引航迹，但文中没考虑对动态威胁障碍物的避碰。文献［6］同样采用降维避障方法，利用避碰障碍锥，对三维避碰采用比例导引方法实现。文献［7］采用三维势场法避障，也存在跟二维势场法相似地缺陷，会陷入局部最优，并产生无效的运动。文献［8］基于流水避石原理实现对三维空间静态障碍物的避碰，并给出相应的避障路径规划方法。

本文通过建立三维空间碰撞锥和三维空间速度障碍模型，在此基础上确定UAV对威胁障碍物避碰的空间最小速度矢量偏转角。同时，也对UAV的空间避障点进行了求解，并将其作为三维五次PH避障重规划路径的初始条件，实现UAV对威胁障碍物的避障路径重规划，提升UAV在复杂环境中执行任务的安全性和可靠性。

2 三维空间碰撞锥的建立

UAV在执行任务的过程中，利用自身携带的传感器装置，可探测到周围环境中存在的动静态障碍物。并可获取动静态障碍物的位姿信息Pose3obs()

PO，vo；同时，UAV的飞行传感器也可实时地获取自身的位姿信息Pose3uav() PU，vu。动静态障碍物的位姿信息Pose3obs() PO，vo和UAV自身的位姿信息Pose3uav() PU，vu可表示为

式中：vo和vu分别为障碍物和UAV的速度矢量的大小；θo、φo和θu、φu分别为障碍物和UAV的速度矢量方向角。

另外，根据UAV携带的传感器性能，设定其探测距离为d0；为简化UAV对威胁障碍物的避碰，将UAV简化为一点，障碍物相应的“膨胀”为一个球体，障碍球的半径记作R，且半径R由UAV自身的尺寸和障碍物的威胁辐射范围确定。过UAV的位置坐标点PU作障碍球PO的切线，所有障碍球PO的切线形成三维空间锥面，而障碍球上所有的切点形成切线圆（Tangent Circle，TC），相应的UAV与障碍球之间的空间碰撞锥（3-Dimensional Collision Cone，3-DCC）如图1所示。

图1三维空间碰撞锥示意图

当UAV感知到周围环境中存在的动静态障碍物时，需要判定障碍物的威胁性。为简化对UAV感知到的障碍物威胁性判定，依据示意图1中的矢量几何关系，确定矢量之间的夹角，从而给出UAV对障碍物威胁性判定的结论。

结论1：当α≥αo时，感知到的障碍物不具有威胁性；当α＜αo时，感知到的障碍物具有威胁性。其中，α为相对速度矢量vuo与3-DCC的轴线矢量之间的夹角；αo为空间障碍锥的半顶角。

而α与αo可由下面的关系式进行求解，

为实现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可实现对威胁障碍物的避碰。UAV的速度矢量vu与母线P3-DVOK1、P3-DVOK2相交的临界状态点A1、A2，且临界状态点A1、A2可由下列关系式进行求解：

图2三维空间速度障碍示意图

图3UAV避障临界状态点示意图

根据式（8）～（11）可确定临界状态点A1(x1，y1，z1)的坐标，临界状态点A2(x2，y2，z2)的坐标可类似确定。那么，对应临界状态点A1、A2的避障速度矢量也可确定。此时，临界状态点A1对应的UAV速度矢量因避障而偏转空间角度最小；临界状态点A2对应的速度矢量因避障而偏转空间角度最大。

4 UAV自主避障算法仿真验证

将在第三节确定的UAV空间自主避障速度矢量方向应用到三维五次PH曲线［9］路径规划，并进行仿真验证。为实现UAV对威胁障碍物的避碰PH路径重规划，需要确定避障点Pavo及避障方向，并以此作为三维五次PH避障路径重规划的初始化条件。文献［10］研究了三维空间UAV可飞行PH路径规划方法，考虑了三维空间UAV路径规划的曲率、挠率和爬升角约束条件，并给出了规划三维可飞行PH路径参数选取方法。在此基础上，对UAV空间自主避障算法进行仿真验证。由于UAV自主避障方向在第3节已经确定，下面对UAV的避障点Pavo进行求解。

首先，UAV对威胁障碍物的避碰时间可确定为

则避碰点Pavo可表示为

在复杂环境下，UAV执行从起飞点Ps(0，0，0)至目标点Pf(600，400，125)的飞行任务。UAV在飞行过程中利用自身携带的传感器装置探测到周围环境中存在的障碍物，UAV需要对威胁障碍物进行避碰。表1给出了UAV对威胁障碍物O1避碰的初始化条件。

表1 威胁障碍物避碰的初始化条件

根据表1中UAV和威胁障碍物O1的初始化条件，可确定UAV对威胁障碍物O1的最小空间避碰速度矢量方向为Qavo(28.8，11.9)，避碰点为Pavo(216，180，41.7)。将其应用于PH曲线路线规划的仿真结果如图4所示，仿真结果表明提出的三维空间UAV自主避障算法能够实现对威胁障碍物的避碰。

图4UAV对威胁障碍物避碰仿真结果

5 结语

本文通过建立三维空间碰撞锥和速度障碍模型，确定了UAV对空间威胁障碍物避碰的最小速度矢量偏转角，并求解了避障点的坐标。同时，将得到的避障点和避障方向作为三维五次PH避障路径重规划的初始化条件，进行了相应的仿真验证。仿真结果表明：提出的UAV空间自主避障方法能够实现对威胁障碍物的避碰，对UAV在复杂环境下执行艰巨任务的安全性和生存能力具有重要理论意义。

［1］齐晓慧，田庆民，甄红涛.无人飞行器系统的感知与规避——研究与应用［M］.北京：国防工业出版社，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］周欢，魏瑞轩，崔军辉，等.面向不确定性环境的多无人机协同防撞［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］梁宵，王红伦，李大伟，等.基于流水避石原理的无人机三维航路规划方法［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日

杨秀霞，女，博士，副教授，研究方向：飞行器智能控制及制导。周硙硙，男，硕士，研究方向：飞行器导航、制导与控制。张毅，男，博士，副教授，研究方向：飞行器导航、制导与控制。华伟，男，硕士，研究方向：飞行器导航、制导与控制。孟启源，男，硕士，研究方向：飞行器导航、制导与控制。