融合危险度模糊分级的UAV 动态避障方法

邹振东，贾云飞，张佳庆

（1.南京理工大学机械工程学院，江苏南京 210094；2.晋西工业集团有限责任公司，山西太原 030000）

随着UAV 应用领域的日益拓宽，对UAV 自主避障的能力有了更高的要求。针对动态障碍空间的反应式避障问题，常见避障算法如人工势场[1]、RRT 算法[2]、DWA[3]、VFH[4]等算法虽然在规划和避障领域得到了广泛应用，但应对运动障碍时的及时性往往欠佳。

速度障碍法[5-7](Velocity Obstacles，VO)是一种几何类的碰撞预测方法。其本质是一种二维冲突求解方法，研究人员将其拓展到三维情况以适应UAV 的运动需求。基于速度障碍法，张毅等对UAV 的航迹优化生成进行了研究[8]。吴学礼等提出了UAV 冲突解脱方法[9]。毕可心等则对飞行冲突网络进行了建模分析[10]。

在以上基础上，文中提出了一种融合危险度模糊分级的UAV 动态避障方法，考虑其他静态障碍的影响，在避免飞行冲突的过程中选择更优的避障方向，提高了避障性能可靠性。

1 坐标变换

UAV 在实际避障飞行过程中，须知道障碍物与机身的实时位置关系，考虑到UAV 飞行姿态对位置信息的影响，需建立机体坐标系和世界坐标系的几何关系模型。UAV 运动姿态如图1 所示。

世界坐标系为Xw，机体坐标系为Xu，从机体坐标系到世界坐标系的转换如下[11]：

Tu-w为机体在世界坐标系的位置，矩阵R为由UAV 俯仰、偏航、滚转等运动所对应的三轴旋转矩阵。在此基础上，只以世界坐标衡量UAV 和周围障碍状态信息，可将UAV 进行质点化处理。

2 运动空间求解

2.1 速度障碍法原理

速度障碍法为一种几何类的避障方法，主要基于UAV 与障碍物的相对位置、速度等信息来计算碰撞可能性。如图2 所示，将UAV 视作质点，以UAV半径对障碍进行膨胀处理，障碍总半径即R=r1+r2，对碰撞区域进行圆形包含。在二维平面上，障碍两侧切线与Pu点形成的锥形区域即为障碍威胁区域，也称碰撞锥，将碰撞锥沿着障碍速度矢量平移所得的区域即为速度障碍区域。两者可视作同一原理的不同表征形式。当相对速度矢量位于碰撞锥内或者UAV速度矢量位于速度障碍区域内时，视作将要碰撞。在此基础上，将预期将要碰撞的障碍称为速度障碍。

图2 二维速度障碍区域表征

为了充分利用三维空间的机动能力，将速度障碍扩展到三维区域，圆形障碍区需进行三维重建，采用圆球形式进行障碍区域表征，碰撞球保护区如图3所示。

图3 三维空间表征

已知UAV 的位置信息为Pu(xu,yu,zu)，速度矢量为vu(vux,vuy,vuz)，障碍物的位置和速度矢量分别为Pi(xi,yi,zi)、vi(vix,viy,viz)。可求得UAV 和障碍物的相对位置矢量Pui和相对速度矢量vui等信息，根据相对位置矢量和相对速度矢量的夹角α以及障碍锥的半顶角γ可以对碰撞危险进行预测计算。假定后续两者都进行匀速直线运动，当α≥γ时，障碍物不会对当前的UAV 运动状态产生威胁，当α＜γ时，则判定UAV 需进行避障反应。各变量计算如下：

2.2 加速度方向求解

在三维避障空间中，UAV 所需的避障速度范围可由UAV 避障平面与速度障碍区域的相交曲线获得，避障平面分为水平和竖直平面，如图4 所示。

图4 避障平面

由水平避障平面和相交曲线可计算得到UAV速度脱离障碍锥面区域所需的Δvx-、Δvx+和Δvy-、Δvy+，称为速度偏量，如图5 所示。

图5 速度偏量

机动能力限制也是影响避障能力的主要因素之一[14]，设[-αm,αm]为加速度范围限制，[-vm,vm]为速度限制。以1 代表加速度为正，-1 代表加速度为负，0代表加速度无论正负都无法在该方向上避开障碍。单向加速度方向值域为Ui=[1,-1,0]。以x轴为例，速度限制的作用规则如下：

根据速度限制确定了各方向上的非可用加速度之后，在可行值域中剔除不可用的项，即可对各Ui进行更新。

加速度大小范围αv可由预期碰撞时间tp与所需速度偏量计算得出：

3 模糊危险度分析

3.1 模糊算法

模糊算法[12-14]是一种智能算法，它是由模糊指令组成的有序集根据执行指令的结果，是产生出指定问题近似解的一种算法，在无人设备运动领域得到了广泛应用[15-17]。通过模糊算法可很好地求解UAV 运动时的静止障碍威胁度，进而对避障规划作出指导。

3.2 输入与输出的模糊化

分析障碍空间，一般来说，容易对UAV 飞行过程产生威胁的障碍具有以下特征：

①障碍距离较近；

②障碍尺寸较大；

③障碍相对速度方向的夹角较小。

因此，输入量设定为障碍物的半径R、距离DS，以及UAV 与其连线和速度矢量的夹角θ。输入量的论域分别为[0,d]、[0,d]、[0,pi/2]。d为探测域大小，为了适应不同参数下的具体情况，将输入论域进行归一化处理。即将输入量除以总的原论域区间大小，可得模糊论域的范围大小为[0,1]。模糊语言变量规定如下：三输入皆划分为五个模糊论域，分别为TB（极大）、B（大）、N（一般）、S（小）、TS（极小）五个范围。

输出量为静止障碍的危险度dng，设定危险度最大为1，则其模糊论域范围为[0,1]，模糊语言变量也划为TB、B、N、S、TS。

在划分模糊论域之后，距离变量的模糊过程的隶属度函数如图6 所示。

图6 距离输入隶属度

障碍半径及速度相对夹角的隶属度形式与图6类似，基于三输入的隶属度函数便可完成输入量的模糊化处理。

3.3 模糊规则

采用if-then 形式构建模糊规则，如表1 所示。

表1 模糊规则表

上述模糊规则主要是基于距离越小，障碍半径越大，夹角越小则该障碍越危险的思想建立的，其中在矩阵运算时，输出量新增的TB+，B+等符号是为了贴合规则设立的相应的中间结果量。举例说明：TB 输出表示为[0,0,0,0,1]，TB-为[0,0,0,0.3,0.7]，B+为[0,0,0,0.7,0.3]，B 表示为[0,0,0,1,0]，矩阵中的每个值为对应模糊论域的隶属度大小，同理可由隶属度（图6）获得其他符号的矩阵值。

在确定模糊规则后，将模糊规则语句用If DS=A，R=B，θ=C，Then dng=D 来表示，其中A、B、C、D 分别为相应模糊论域上的元素。根据Mamdani推理法[18]，即模糊蕴含关系采用最小运算规则。由表1 的13 条模糊规则可生成13 个模糊关系矩阵Ru，可用公式表示为：

其中，×为叉乘运算算子，◦为合成运算算子，i为第i条规则。叉乘运算基于模糊最小蕴含过程为求交运算过程，◦过程为各因素求交再求或的过程。在求出每个模糊规则后，总的模糊规则为各分规则矩阵各元素求或后得出：

任意条件下的危险度输出为：

3.4 解模糊

由模糊规则计算得到的模糊推理结果仍然具有模糊性，需要进行解模糊化获取精确数字量。采用重心法进行解模糊化，可得：

其中，δi为求解出的各等级危险度权重，Ei为相应数值论域中的值。

4 融合策略

由以上分析，针对环境中的预期碰撞速度障碍和静止障碍，可分别求得加速度可用控制集和危险度。当UAV 飞行过程中遇到单个或多个速度障碍时，首先需确定UAV 可用的避障方向。由于三轴避障方向具备独立性，以单方向运动指令为例，当该方向上加减速指令都可使UAV 脱离速度障碍时，再考虑周围静止障碍的影响，静止障碍贡献方向为相对位置的相反方向。为了综合考虑各因素，提出了一种优化评价函数，用于计算最优指令，函数需考虑的因素如下。

1）UAV 在该方向加速，减速指令下脱离所有不同速度障碍区域的最短预期脱离时间to+和to-；

2）预期碰撞为真时的最短预期碰撞时间tpmi；

3）静止障碍的危险度综合度量，其值越高影响越强。

将各因素影响进行归一化处理，用加减速方向上的脱离时间与最短预期碰撞时间作商，函数如下：

其中，U+为该方向正加速度的权重，U-为负向加速度的权重。n1为加速度贡献为正的静止障碍个数，n2为加速度贡献为负的静止障碍个数，a为调节静止障碍影响因素大小的系数，dngi为第i个静止障碍的危险度。后一项的值域范围为[0,1]，函数可很好地将障碍信息综合考虑。

接着比较正加速度和负加速度权重大小，取加速度方向为其中较小者方向。

5 仿真分析

5.1 环境测试

为验证算法的有效性，分别在多动态障碍环境和复杂障碍环境进行了验证分析。仿真条件设置如下:UAV 初始位置[0,50,50]，初始速度3 m/s，障碍半径为2 m，计算步长为Δt=0.2 s，可用加速度最大1 m/s2，初始航向0 度，障碍空间为[[0,100],[0,100],[0,100]]，目标点为[100,50,50]，单位为m。

速度障碍采用随机生成机制，避障次数为2，每次随机生成2～4 个可能碰撞的速度障碍，在多障碍空间的避障路线如图7 所示。

图7 动态障碍避障轨迹

如图7 所示，实线为飞行轨迹，虚线为动态障碍轨迹，UAV 在同时面临多个动态障碍威胁时，通过改变速度方向，对两个碰撞点都完成了规避，避免了碰撞的发生。

在飞行空间内添加静态障碍团，使其分布不均匀，同样设定了两次预期的碰撞，通过设定较高的加速度阈值保证了多方向的避障可行性。在此条件下的避障路线如图8 所示。

图8 复杂障碍环境避障路线

图8 中，UAV 在两次预期碰撞点前方都通过改变自身运动轨迹完成了避碰，且在避碰时UAV 完成了偏向障碍较稀疏的一侧的运动。

5.2 对比分析

为了验证加入危险度模糊分级策略的有效性，通过仿真分析将该改进速度障碍法与不加入危险分级策略的速度障碍法进行了对比，分析其在1 000 次随机环境中的性能表现。障碍空间大小、初始点、目标点及UAV 性能不变，随机静态障碍采用集群分布式产生，随机初始化多个中心位置，以该位置为中心扩展障碍，形成多障碍集群的分布。每次随机环境产生30 个静态障碍和30 个动态障碍，测试结果如表2 所示。

表2 改进效果对比

由表2 可知，改进后的VOM 在避碰成功率上提高了6%左右，成功避障时平均路径长度变化不大，主要由于反应避碰过程较快，偏移过程对于路径长度的贡献较小。平均避碰次数相比之下减小了11%左右。

6 结束语

针对三维空间中多速度障碍及复杂障碍的反应式避障问题，文中提出了一种基于危险度模糊分级的改进速度障碍法，通过求解可用加速度控制，在其基础上采用模糊算法输出各静止障碍危险度分级，并提出一种优化评价函数综合考虑障碍空间各影响来输出避障指令，使得避障时能朝向障碍较少的方向完成避障。相比原算法，改进算法在避障成功率和平均避障次数等性能上表现出更优异的性能。