CN105717942B - 一种无人飞行器空间避障方法及相关路径在线规划方法 - Google Patents

Abstract

本发明提出了一种无人飞行器空间避障方法及相关路径在线规划方法，该方法是一种基于三维速度障碍锥的动态空间直接避障方法，能够给出避障的插入点及避障方向，并与航迹规划方法相结合，可以实现无人飞行器在线航迹的重新规划。避障方法主要包括以下步骤：（1）构建三维速度障碍锥，（2）运用三维速度障碍锥进行避碰判断，（3）求解避障方向，（4）求解避障点。相关路径在线规划方法是以前述避障方法求解的避障方向和避障点来作为曲线路径在线规划的初始条件进行路径在线规划的方法。

Description

一种无人飞行器空间避障方法及相关路径在线规划方法

技术领域

本发明属于无人飞行器空间避障飞行的控制领域，尤其是涉及一种基于三维速度障碍锥和空间速度障碍球冠的无人飞行器空间避障方法以及相关的路径在线规划方法。

背景技术

当前无人飞行器空间归避障碍的飞行有多种控制方法。主要是利用探头探测周围信息，信息包括无人飞行器与障碍物之间的相对距离、方位、视线角、视线角速度、相对速度及加速度等。控制规避障碍物(以下简称“避障”)的方式主要有两类：一类是在航路规划时，将航路上存在的障碍物以约束条件的形式加在无人飞行器飞行的性能指标中。例如：采用A^*搜索算法、快速随机搜索树、遗传算法等，在进行航路的规划过程中通过数学运算，解算出不符合避障约束条件的解，并将其剔除；第二类是设计避障导引函数，如人工势场法或改进的导航函数法、导引避障法及基于跟踪误差的李雅普诺夫控制律设计法等，将避障的过程结合在规划算法之中。第一类方式主要用于对静态障碍如高山、高层建筑物等的归避；然而对于动态障碍物诸如动态的飞行物时，由于发现障碍物后能够反应的时间极短，而且飞行器一般具有机动快速的特点，这样采用第二类方式才能够更好地解决这类问题。

上述第二类方式中的人工势场法，其实现避障的原则是：首先定义一个势场力函数，在该势场中势场力对障碍物产生排斥力，对无人飞行器产生吸引力，并将此原则转换成路径规划的费用函数，引导无人飞行器使用最小费用，沿安全路径到达目的地。但是，经过反复试验发现：该势场力函数在对静态障碍物进行避障时，控制的实时性比较好；但是当障碍物为运动障碍，将障碍物和无人飞行器的速度加入到势场函数构造中时，侧避障效果不佳，往往在计算运动体路径时产生无效路径；而且，由于运动障碍物的作用，无人飞行器即使根据实际飞行路径来看与障碍物二者之间并不会碰撞，但根据势场函数的计算,仍然会产生势场力,使无人飞行器在飞行途中会偏离原来的运动轨迹；另外，有时还易陷入局部极小，使无人飞行器的飞行路径混乱。

Hyo-Sang Shin等基于微分几何学原理，对无人飞行器飞行过程中的控制方法，应用避障的速度圆锥法进行碰撞检测，采用导引律的思想进行碰撞冲突消解，给出了无人飞行器航向角及速度的变化规律。但是，由于微分几何学中给出的是平面的几何关系，所以解出的控制飞行规律的方程是非连续的导引控制规律，并且引入了符号函数，其规划路径易产生颤振。

F.Belkhouche[16]针对三维静态避障，采用了平面降维思想，利用避障圆锥法实现了避障，并用扩展比例导引律进行避障导引，在导引律设计参数中考虑了飞行器的飞行速度、角速度等约束限制，可产生非直线导引航迹。但是,由于采用的是平面避障思想,所以避障中产生的非直线导引航迹并不是三维空间的非直线导引的最短距离航迹。

综上所述：目前空间避障方法还是基于平面降维避障的思想，而不是三维空间的直接避障，这样就会导致在避障平面上飞行角度改变较大、飞行航迹偏离原航迹较远等问题。

发明内容

针对上述提出的问题，有必要提出一种新的空间直接避障法，该方法可以较好的实现三维空间直接避障的问题。本发明以此为目的提出了一种三维速度障碍锥的动态空间直接避障方法，该方法能够给出避障的插入点及避障方向；并与航迹规划方法相结合，可以实现无人飞行器在线航迹的重新规划。

本发明提出的避障方法具体包括以下步骤：

步骤S1，构建三维速度障碍锥；

根据无人飞行器在三维空间中的当前时刻位置坐标P_u＝(x_u,y_u,z_u)和探测到的障碍物O的位置坐标P_o＝(x_o,y_o,z_o)，以P_u作为三维速度障碍锥的顶点，并将障碍物点膨化为以P_o为球心、半径为R的障碍球PO，然后过P_u点作障碍球的切线，则所有以P_u为顶点障碍球的切线形成的锥面为三维速度障碍锥；

步骤S2，运用构建三维速度障碍锥进行避碰判断；

根据无人飞行器和障碍物的速度矢量大小和方向，求解无人飞行器相对于障碍物O的相对速度矢量v_uo的大小和方向；然后求解相对速度矢量v_uo与无人飞行器和障碍物的位置矢量之间的夹角大小α；判断障碍锥的半顶角α₀与α的关系，若α<α₀，需要对障碍物进行避碰，则继续进行步骤S3；若α>α₀，不需要对障碍物进行避碰，则退出；

步骤S3，求解避障方向；

步骤S4，求解避障点。

优选的，步骤S3中通过建立空间速度障碍球冠求解避障方向，具体包括以下步骤：

步骤S31，建立相对碰撞区RCC和绝对碰撞区ACC；

根据建立相对碰撞区；其中l_uo为v_uo方向上的射线；

将RCC沿障碍物O速度矢量v_o平移||v_o||，得到绝对碰撞区ACC，即ACC＝RCC⊕v_o，其中⊕表示闵科夫斯基矢量和；

步骤S32，建立空间速度障碍球冠；

做以无人飞行器位置点P_u为球心、||v_u||为半径的速度球S_u，然后取速度球S_u与ACC相交且位于ACC内部的球面为空间速度障碍球冠G_o＝(r,ψ_o,θ_o)，其中r为球心P_u和球冠中心M之间所形成向量的长度，ψ_o和θ_o为球心P_u和球冠中心M之间所形成向量的方向角，记此球面为球冠大圆；

步骤S33，根据空间速度矢量几何关系，解出球冠的参数；

步骤S34，确定避障方向；

避障方向为速度矢量v_u根据避障后无人飞行器的飞行方向需要在空间以某一旋转角旋转至对应的期望速度矢量的方向。

优选的，步骤S33具体为：

步骤S331，根据三维空间向量v_uo和作平面P_uP_oT，则平面P_uP_oT与三维速度障碍锥交线为圆锥体的母线l₃、l′₃，母线l₃、l′₃与障碍球的切点为Q、Q′；将相对速度矢量v_uo旋转至母线l₃方向上，得到期望相对速度矢量的方向，对应的旋转角度为最小角度δ，母线l′₃为无人飞行器避障过程中速度矢量偏转角度最大位置；

步骤S332，确定母线l₃上的一个单位向量q，从而可以确定相对速度矢量v_uo旋转到母线l₃方向上转过的偏航角Δψ_uo和俯仰角Δθ_uo；然后由单位向量q求得期望相对速度矢量的大小及无人飞行器期望速度矢量由此可确定无人飞行器在完成对障碍物的避障任务所需转过的最小角度及最大角度；

步骤S333，将无人飞行器对障碍物O进行避障的速度矢量抽象到三维空间中得到矢量关系，其中B、C点为两个临界边l₃和l′₃上的临界点。M为线段BC的二分点，根据矢量三角形的加法原则可得出及矢量A点即为P_u点，由此可将障碍物O的球冠G_o表示为

优选的，步骤S34具体为：

在进行避障时，无人飞行器选择临界曲线上的点进行避障，对于单个障碍，三维避障锥为临界曲线；设在球冠G_o1临界曲线上任意一点K的位置坐标为(x_k,y_k,z_k)，根据几何位置关系和确定任意点K的三个坐标分量x_k，y_k，z_k之间的关系表达式，由此计算出矢量在三维空间中的两个方向角，即无人飞行器期望速度矢量方向角和以及相应的速度矢量v_u旋转角度Δψ_u和Δθ_u。

优选的，步骤S4具体为：

步骤S41，根据无人飞行器的避障速度球冠确定避碰的主体障碍物；

当无人飞行器与多运动障碍有碰撞危险时，计算无人飞行器与各障碍物在速度空间的碰撞时间，将时间最短的障碍物作为主体障碍物，然后依据单个障碍物避障插入点坐标的计算方法确定无人飞行器对主体障碍物避障的插入点，作为多障碍物避障的插入点；

步骤S42，在主体障碍物的速度矢量相反方向确定避碰点；

采用障碍物经过调整时间t后的空间位置坐标p′_o(x′_o,y′_o,z′_o)来确定避碰点，将避障的插入点确定在障碍物速度矢量v_o的相反方向上，同时将障碍物的膨化半径乘上一个比例系数k，然后根据障碍物位置坐标p′_o(x′_o,y′_o,z′_o)，得出避障插入点位置坐标Q_ins(x_ins,y_ins,z_ins)，

x_ins＝x′_o+R′cosθ_o cosψ_o

y_ins＝y′_o+R′cosθ_o sinψ_o

z_ins＝z′_o+R′sinθ_o

其中R′＝kR，k为设定的常数且k≥1。

优选的，当障碍物为多个时，

步骤S2要对多个障碍物进行避障判断，如果无人飞行器的速度矢量v_u与障碍物O_i的速度矢量v_oi之间的相对速度矢量v_uoi均在障碍物O_i形成的障碍锥内，则需要同时对多个障碍物进行避碰，否则不需要同时对多个障碍物进行避碰。

对于多个障碍物同时避障时，则各球冠大圆的并集为避障临界曲线。

使用前述的无人飞行器空间避障方法进行路径在线规划的方法，该方法是在现有的在线规划方法的基础上，修改了曲线路径在线重规划的初始条件，具体包括以下步骤：

步骤S51，将步骤S3和步骤S4确定的无人飞行器避碰速度矢量方向和避障点，以及当前时刻无人飞行器的运动状态以及目标点处要求的状态作为曲线路径在线重规划的初始条件；

步骤S52，根据曲线路径规划原理以及步骤S51确定的初始条件，对无人飞行器进行路径在线重规划。

本发明的提出的基于三维速度障碍锥的动态空间直接避障方法，能够给出避障的插入点及避障方向；将避障方法与航迹规划方法相结合，可以实现无人飞行器在线航迹重规划。本发明的提出避障方法和基于该避障方法的路径在线规划方法，大大增加了避障的灵活性。

附图说明

图1基于三维速度障碍锥的无人飞行器空间避障流程图；

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

图3相对碰撞区RCC和绝对碰撞区ACC示意图；

图4无人飞行器对障碍物O形成的球冠示意图；

图5无人飞行器速度矢量v_u在空间的最大和最小旋转角示意图；

图6无人飞行器和障碍物空间速度矢量关系图；

图7无人飞行器进行避障时速度矢量v_u可行区域求解的示意图；

图8无人飞行器的最优可飞行PH航迹；

图9无人飞行器在空间位置坐标点P_t处探测到障碍物O仿真图；

图10无人飞行器对探测到的障碍物O进行避障判定仿真图；

图11无人飞行器对单个障碍物进行避障仿真图；

图12无人飞行器在空间位置坐标点P_t处探测到障碍物O₁仿真图；

图13无人飞行器对探测到的障碍物O₁和O₂进行避障判定仿真图；

图14无人飞行器对多障碍物进行避障仿真图。

具体实施方式

下面结合附图说明本发明的具体实施方式。

如附图1所示，本发明提出的避障方法主要包括：构建三维速度障碍锥、运用三维速度障碍锥进行避碰判断、求解避障方向和求解避障点四个步骤。

具体包括以下步骤：

步骤S1，构建三维速度障碍锥；

根据无人飞行器(Unmanned Aerial Vehicle，UAV)在三维空间中的当前时刻位置坐标P_u＝(x_u,y_u,z_u)和探测到的障碍物O的位置坐标P_o＝(x_o,y_o,z_o)，以P_u作为三维速度障碍锥的顶点，并将障碍物点膨化为以P_o为球心、半径为R的障碍球PO，然后过P_u点作障碍球PO的切线l₁和l₂，则所有以P_u为顶点障碍球的切线形成的锥面为三维速度障碍锥，如图2所示。所有切点组成了圆周，d₀为无人飞行器到障碍物O的距离。

步骤S2，运用构建的三维速度障碍锥进行避碰判断；

根据无人飞行器速度v_u和障碍物速度v_o的大小和方向，求解无人飞行器相对于障碍物O的相对速度矢量v_uo的大小和方向；然后求解相对速度矢量v_uo与无人飞行器和障碍物的位置矢量之间的夹角大小α，如图2所示；

然后求出障碍锥的半顶角α₀，其中R为障碍球PO的半径，d₀为无人飞行器到障碍物O的距离。

当障碍物为单个的时候，判断障碍锥的半顶角α₀与α的关系，若α<α₀，需要对障碍物进行避碰，则继续进行步骤S3；若α>α₀，不需要对障碍物进行避碰，则退出。

当障碍物为多个的时候，无人飞行器需要同时对多个障碍物进行避碰的条件为：无人飞行器对每一个障碍物的避碰判断条件同时成立。

即：无人飞行器需要同时对多个障碍物进行避碰的判定条件可确定为

即无人飞行器的速度矢量v_u与障碍物O_i的速度矢量v_oi之间的相对速度矢量v_uoi都在障碍物O_i形成的障碍锥内，此时需要对多个障碍物进行避碰，否则不需要同时对多个障碍物进行避碰。

步骤S3，通过建立空间速度障碍球冠求解避障方向，具体包括以下步骤：

步骤S31，建立相对碰撞区RCC和绝对碰撞区ACC；

无人飞行器与障碍物的相对速度：

v_uo＝v_u-v_o (1)

通过相对速度，把O当作静止障碍物，而无人飞行器的速度则看作v_uo，l_uo为v_uo方向上的射线，如图3所示。如果v_uo保持不变，则无人飞行器与障碍物O发生碰撞的条件为：

满足式(2)的相对速度v_uo构成的集合，称为三维空间的相对碰撞区RCC(RelativeCollision Cone)。

若无人飞行器的相对速度v_uo∈RCC，则无人飞行器就会与障碍物发生碰撞。

将RCC沿障碍物O速度矢量v_o平移||v_o||，得到绝对碰撞区ACC(AbsoluteCollision Cone)，如图3中的阴影部分。

ACC＝RCC⊕v_o (4)

其中⊕表示闵科夫斯基矢量和。

可以得出，若无人飞行器的速度矢量位于ACC之内，则v_uo∈RCC，此时无人飞行器就会与障碍物发生碰撞，记相应的无人飞行器速度v_u为速度障碍VO(Velocity Obstacle)。

步骤S32，建立空间速度障碍球冠；

对于图3所示的情况，做以无人飞行器位置点P_u为球心、||v_u||为半径的速度球S_u，如图4所示。然后取速度球S_u与ACC相交且位于ACC内部的球面为空间速度障碍球冠G_o＝(r,ψ_o,θ_o)，其中r为球心P_u和球冠中心M之间所形成向量的长度，ψ_o和θ_o为球心P_u和球冠中心M之间所形成向量的方向角；

步骤S33，根据空间速度矢量几何关系，解出球冠的参数，具体步骤如下：

步骤S331，根据三维空间向量v_uo和作平面P_uP_oT，则平面P_uP_oT与三维速度障碍锥的圆锥曲面P_ul₁l₂的交线为圆锥体的母线l₃、l′₃，母线l₃、l′₃与障碍球的切点为Q、Q′；将相对速度矢量v_uo旋转至母线l₃方向上，得到期望相对速度矢量的方向，对应的旋转角度为最小角度δ。

δ＝α₀-α (5)

母线l′₃为无人飞行器避障过程中速度矢量偏转角度最大位置，如图5所示。

步骤S332，为了确定相对速度矢量v_uo旋转到母线l₃方向上转过的偏航角Δψ_uo和俯仰角Δθ_uo，需要确定母线l₃上的一个单位向量q，在母线l₃上确定一点P(x_P,y_P,z_P)，使得

从而可以确定相对速度矢量v_uo旋转到母线l₃方向上转过的偏航角Δψ_uo和俯仰角Δθ_uo；然后由单位向量q求得期望相对速度矢量的大小及无人飞行器期望速度矢量由此可确定无人飞行器在完成对障碍物的避障任务所需转过的最小角度及最大角度。

根据图5中的几何关系，可以对未知点P进行求解，相应的几何关系表达式可表示为

这样，点P的坐标可解算得到。因此，母线l₃上的单位矢量q可表示为

q＝(x_P-x_u,y_P-y_u,z_P-z_u) (9)

期望相对速度矢量可表示为

其中，λ为期望相对速度矢量的大小。

又障碍物的速度矢量v_o大小和方向已知，那么障碍物速度矢量v_o和期望相对速度矢量之间的夹角为

由图5可得期望相对速度矢量的大小满足下述方程

方程中有两个解，根据示意图5可确定的大小为

根据速度矢量三角形可得，

又

v_o＝(||v_o||cosθ_o cosψ_o,||v_o||cosθ_o sinψ_o,||v_o||sinθ_o) (15)

那么，无人飞行器期望速度矢量的形式为

因此，无人飞行器期望速度矢量的方向角可表示为

从而，根据无人飞行器即时的速度矢量v_u方向角(ψ_u,θ_u)和期望的速度矢量方向角可确定无人飞行器在完成对障碍物的避障任务所需转过的最小角度对应的方向角为

其中，Δψ>0，Δθ>0表示逆时针旋转；Δψ<0，Δθ<0表示顺时针旋转。

根据无人飞行器速度矢量偏转的最小角度对应的方向角(Δψ,Δθ)，可以确定无人飞行器期望速度矢量与即时速度矢量v_u之间的关系为

根据计算母线l₃上无人飞行器期望速度矢量大小和方向的方法，计算出对应于母线l′₃上的相对速度矢量和期望速度矢量的大小和方向，此时，使相对速度矢量v_uo转到方向(即母线l′₃的方向上)。

步骤S333，将图5中无人飞行器对障碍物O进行避障的速度矢量抽象到三维空间中得到矢量关系，如图6所示。在图6中，其中B、C点为两个临界边l₃和l′₃上的临界点。

由已经求解出的期望速度矢量和的大小和方向，图6中的矢量可表示为：

在图6中，M为线段BC的二分点。那么，根据矢量三角形的加法原则可得出及矢量A点即为P_u点。

那么，矢量可表示为

其中，

因此，在A点处的空间坐标系下，矢量的方向角可表示为

矢量的长度可表示为

由此可将障碍物O的球冠G_o表示为

步骤S34，确定避障方向；

避障方向为速度矢量v_u根据避障后无人飞行器的飞行方向需要在空间以某一旋转角旋转至对应的期望速度矢量的方向。

在进行避障时，无人飞行器选择临界曲线上的点进行避障，对于单个障碍，三维避障锥为临界曲线。对于单个障碍，避障方向可选择无人飞行器位置点与避障锥上的任一点的连线矢量作为避障边界方向，其中，若将速度矢量v_u在空间最小旋转角对应的临界速度矢量的方向作为避障方向，无人飞行器为旋转角度最优避碰方向。

对于多个障碍物，则各球冠大圆的并集为避障临界曲线。图7为两个障碍物所形成的球冠相交示意图。其中，无人飞行器的速度矢量v_u位于两个障碍物球冠相交的公共区域上，点S和S′分别为两个球冠上两个最大圆的交点。

为了求解出点S和S′的空间位置坐标，设定点S和S′的位置坐标分别为(x_s,y_s,z_s)，(x_s′,y_s′,z_s′)。首先，点S在无人飞行器速度矢量v_u的球面上，那么

根据图中的几何关系可得

即可求解得出点S的位置坐标；点S′的位置坐标可类似求得。由此，无人飞行器在进行障碍物避障时的速度矢量则可求出，从而得出无人飞行器速度矢量v_u在避障过程中方向角的变化大小。根据各球冠最大圆的交点S、S′的位置坐标，可确定出临界曲线即两个球冠大圆的并集作为避障边界，选择避障边界上的任一点K进行避障。

根据任务需要，在进行避障时，无人飞行器可选择临界曲线上的任一点进行避障。

假设在球冠G_o1临界曲线上任意一点K的位置坐标为(x_k,y_k,z_k)，根据图7中的几何位置关系可得

则可以确定任意点K三个坐标分量x_k，y_k，z_k之间的关系表达式，从而可得出下面的任意角度避障的结论。在无人飞行器可行的避障临界曲线上任意设定所需避障转向点K(x_k,y_k,z_k)的一个坐标分量，其他两个坐标分量可根据三者之间的关系表达式(42)、(43)确定。这样，无人飞行器完成对障碍物避障任务的期望速度矢量可确定为

由此可以计算出矢量在三维空间中的两个方向角，即期望速度矢量方向角和相应的无人飞行器速度矢量v_u旋转角度Δψ_u和Δθ_u也可以求出。

步骤S4，求解避障点，具体包括以下步骤：

步骤S41，根据无人飞行器的避障速度球冠确定避碰的主体障碍物；

当无人飞行器与多运动障碍有碰撞危险时，计算无人飞行器与各障碍物在速度空间的碰撞时间，将时间最短的障碍物作为主体障碍物，然后依据单个障碍物避障插入点坐标的计算方法确定无人飞行器对主体障碍物避障的插入点，作为多障碍物避障的插入点；

步骤S42，在主体障碍物的速度矢量相反方向确定避碰点；

采用障碍物经过调整时间t后的空间位置坐标p′_o(x′_o,y′_o,z′_o)来确定避碰点，将避障的插入点确定在障碍物速度矢量v_o的相反方向上，同时将障碍物的膨化半径乘上一个比例系数k，然后根据障碍物位置坐标p′_o(x′_o,y′_o,z′_o)，得出避障插入点位置坐标Q_ins(x_ins,y_ins,z_ins)，

x_ins＝x′_o+R′cosθ_o cosψ_o

y_ins＝y′_o+R′cosθ_o sinψ_o

z_ins＝z′_o+R′sinθ_o

其中R′＝kR，k为设定的常数且k≥1。

使用前述的无人飞行器空间避障方法进行路径在线规划的方法，该方法是在现有的在线规划方法的基础上，修改了曲线路径在线重规划的初始条件，具体包括以下步骤：

步骤S51，将步骤S3和步骤S4确定的无人飞行器避碰速度矢量方向和避障点，以及当前时刻无人飞行器的运动状态以及目标点处要求的状态作为曲线路径在线重规划的初始条件；

步骤S52，根据曲线路径规划原理以及步骤S51确定的初始条件，对无人飞行器进行路径在线重规划。

为了更加直观的描述本发明的具体步骤和优势，下面结合附图，运用仿真实施例的形式加以叙述。

实施例1：

根据无人飞行器三维速度障碍锥避障原理，以PH(Pythagorean Hodograph)曲线航迹规划方法为例，完成无人飞行器在三维空间中对运动障碍物的避障仿真。仿真的过程分为三个步骤：

(1)根据无人飞行器的PH航迹规划算法，完成无人飞行器从初始点到目标点之间的航迹规划；

(2)无人飞行器沿着规划出的PH航迹飞行，并以一定的探测距离对周围环境中存在的障碍物进行探测，并通过无人飞行器避障判断条件，确定无人飞行器是否需要对探测到的障碍物进行避障；

(3)当无人飞行器需要对探测到的障碍物进行避障时，依据三维速度障碍锥避障算法，计算出无人飞行器完成避障的插入点位置坐标以及插入点处无人飞行器速度矢量方向角。然后，再根据无人飞行器航迹重规划算法，完成对障碍物的避障。

按照上述步骤进行仿真实验，主要验证无人飞行器对单运动障碍及多运动障碍避障时空间速度障碍原理的有效性及相应航迹重规划方法的正确性。首先设定仿真情景，无人飞行器从初始点P_s(0,0,0)到目标点P_f(500,200,100)处执行任务。根据规划最优可飞行PH航迹的算法，规划出一条满足曲率、挠率以及爬升角等相关约束条件的可飞行PH航迹，无人飞行器以飞行速度||v_u||＝50m/s沿此航迹飞行。规划的可飞行PH航迹如图8所示。

单运动障碍情况

在飞行到空间位置坐标P_t(162.11,101.69,64.26)时，探测到运动到的障碍物O，障碍物的位置坐标为O(254.4,63.6,60.7)，障碍物速度大小为||v_o||＝40m/s，障碍物速度矢量v_o的两个方向角为ψ_o＝12.4°，θ_o＝135.3°，障碍物的膨化半径R＝25m；又无人飞行器在探测点P_t处的速度矢量v_u的两个方向角为ψ_u＝24.77°，θ_u＝12°，无人飞行器的探测距离d₀＝100m，仿真结果如图9所示。

那么，依据无人飞行器三维空间避障判断条件，可以判定无人飞行器需要对探测到的障碍物进行避障。仿真结果如图10所示。

依据三维速度障碍锥避障算法，可以计算出PH航迹重规划的插入点位置坐标为Q_ins(226.34,159.56,79.05)，以及插入点Q_ins处的无人飞行器速度矢量偏转最小角度对应的方向角为由此根据PH航迹规划算法对无人飞行器进行航迹重规划，图11中给出了无人飞行器重规划航迹仿真结果。

实施例2：

多运动障碍情况

无人飞行器沿着规划的PH航迹飞行过程中，同时探测到两个运动的障碍物，它们的速度矢量大小和方向分别为||v_o1||＝35m/s，||v_o2||＝32m/s，ψ_o1＝106.3°，θ_o1＝11.4°，ψ_o1＝-50.3°，θ_o2＝15.4°；障碍物O₁和O₂的膨化半径分别为R₁＝25，R₂＝30；位置坐标分别为O₁(254.5,63.8,59.8)，O₂(208.1,186.2,42.5)，仿真结果如图12所示。

根据无人飞行器对多障碍物避障的判定原理，可以分别计算出α₁＝11.1°，α₂＝14.8°；又α₀₁＝14.5°，α₀₂＝17.5°。所以α₁<α₀₁且α₂<α₀₂，则满足无人飞行器对多障碍物同时避障的判定条件，即无人飞行器需要同时对障碍物O₁和O₂进行避障，仿真结果如图13所示。

根据障碍物形成的球冠参数计算方法，可以得到障碍物O₁和O₂的球冠表达分别为G_o1(38.1,15.4°,3.1°)，G_o2(32.2,22.8°,23.5°)。那么，根据空间速度障碍锥避障算法，得出无人飞行器完成对障碍物避障任务时的插入点坐标为Q_ins(229.7,148.5,77.6)，选择K点坐标(206.5,101.0,88.5)，得到插入点处速度矢量的方向角为则根据无人飞行器的PH航迹重规划算法，规划出满足避障约束条件的可飞行PH航迹如图14所示，使无人飞行器完成对障碍物的避障任务。从图14中可以看出，按照三维速度障碍锥避障算法，无人飞行器成功实现了对多运动障碍的避障，并能在线重规划PH航迹。

通过避障仿真计算，可以得出采用不同避障算法以及不同转向点下的无人飞行器速度矢量所需要旋转的角度，旋转角度大小如表1所示。

表1不同避障算法及转向点下的无人飞行器速度矢量偏转角度

通过比较表1中无人飞行器在不同避障算法和转向点下的速度矢量旋转角度大小，可以得出结论：相对于降维避障算法，三维速度障碍锥避障算法在插入点Q_ins处的方向角具有更大的可选择性；另外，采用三维速度障碍锥避障算法，可以根据不同种类无人飞行器的性能指标确定避障转向点，完成对不同性能指标无人飞行器的在线可飞行航迹重规划。

此外，应当理解，以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种无人飞行器空间避障方法，其特征在于，包括以下步骤：

步骤S1，构建三维速度障碍锥；

根据无人飞行器在三维空间中的当前时刻位置坐标P_u＝(x_u,y_u,z_u)和探测到的障碍物O的位置坐标P_o＝(x_o,y_o,z_o)，以P_u作为三维速度障碍锥的顶点，并将障碍物点膨化为以P_o为球心、半径为R的障碍球PO，然后过P_u点作障碍球的切线，则所有以P_u为顶点障碍球的切线形成的锥面为三维速度障碍锥；

步骤S2，运用构建的三维速度障碍锥进行避碰判断；

根据无人飞行器和障碍物的速度矢量大小和方向，求解无人飞行器相对于障碍物O的相对速度矢量v_uo的大小和方向；然后求解相对速度矢量v_uo与无人飞行器和障碍物的位置矢量之间的夹角大小α；判断障碍锥的半顶角α₀与α的关系，若α＜α₀，需要对障碍物进行避碰，则继续进行步骤S3；若α＞α₀，不需要对障碍物进行避碰，则退出；

步骤S3，求解避障方向；

步骤S4，求解避障点。

2.如权利要求1所述的一种无人飞行器空间避障方法，其特征在于，步骤S3中通过建立空间速度障碍球冠求解避障方向，具体包括以下步骤：

步骤S31，建立相对碰撞区RCC和绝对碰撞区ACC；

根据建立相对碰撞区；其中l_uo为v_uo方向上的射线；

将RCC沿障碍物O速度矢量v_o平移||v_o||，得到绝对碰撞区ACC，即其中表示闵科夫斯基矢量和；

步骤S32，建立空间速度障碍球冠；

做以无人飞行器位置点P_u为球心、||v_u||为半径的速度球S_u，然后取速度球S_u与ACC相交且位于ACC内部的球面为空间速度障碍球冠G_o＝(r,ψ_o,θ_o)，其中r为球心P_u和球冠中心M之间所形成向量的长度，ψ_o和θ_o为球心P_u和球冠中心M之间所形成向量的方向角；

步骤S33，根据空间速度矢量几何关系，解出球冠的参数；

步骤S34，确定避障方向；

避障方向为速度矢量v_u根据避障后无人飞行器的飞行方向需要在空间以某一旋转角旋转至对应的期望速度矢量的方向。

3.如权利要求2所述的一种无人飞行器空间避障方法，其特征在于，步骤S33具体为：

步骤S331，根据三维空间向量v_uo和作平面P_uP_oT，则平面P_uP_oT与三维速度障碍锥交线为圆锥体的母线l₃、l′₃，母线l₃、l′₃与障碍球的切点为Q、Q′；将相对速度矢量v_uo旋转至母线l₃方向上，得到期望相对速度矢量的方向，对应的旋转角度为最小角度δ，母线l′₃为无人飞行器避障过程中速度矢量偏转角度最大位置；

步骤S332，确定母线l₃上的一个单位向量q，从而确定相对速度矢量v_uo旋转到母线l₃方向上转过的偏航角Δψ_uo和俯仰角Δθ_uo；然后由单位向量q求得期望相对速度矢量的大小及无人飞行器期望速度矢量由此确定无人飞行器在完成对障碍物的避障任务所需转过的最小角度及最大角度；

步骤S333，将无人飞行器对障碍物O进行避障的速度矢量抽象到三维空间中得到矢量关系，其中B、C点为两个临界边l₃和l′₃上的临界点；M为线段BC的二分点，根据矢量三角形的加法原则得出及矢量A点即为P_u点，由此将障碍物O的球冠G_o表示为

4.如权利要求2所述的一种无人飞行器空间避障方法，其特征在于，步骤S34具体为：

在进行避障时，无人飞行器选择临界曲线上的点进行避障，当对于单个障碍，三维避障锥为临界曲线；设在球冠G_o1临界曲线上任意一点K的位置坐标为(x_k,y_k,z_k)，根据几何位置关系和确定任意点K的三个坐标分量x_k，y_k，z_k之间的关系表达式，由此计算出矢量在三维空间中的两个方向角，即无人飞行器期望速度矢量方向角和以及相应的速度矢量v_u旋转角度Δψ_u和Δθ_u。

5.如权利要求1所述的一种无人飞行器空间避障方法，其特征在于，步骤S4具体为：

步骤S41，根据无人飞行器的避障速度球冠确定避碰的主体障碍物；

当无人飞行器与多运动障碍有碰撞危险时，计算无人飞行器与各障碍物在速度空间的碰撞时间，将时间最短的障碍物作为主体障碍物，然后依据单个障碍物避障插入点坐标的计算方法确定无人飞行器对主体障碍物避障的插入点，作为多障碍物避障的插入点；

步骤S42，在主体障碍物的速度矢量相反方向确定避碰点；

采用障碍物经过调整时间t后的空间位置坐标p′_o(x′_o,y′_o,z′_o)来确定避碰点，将避障的插入点确定在障碍物速度矢量v_o的相反方向上，同时将障碍物的膨化半径乘上一个比例系数k，然后根据坐标p′_o(x′_o,y′_o,z′_o)，得出避障插入点位置坐标Q_ins(x_ins,y_ins,z_ins)，

x_ins＝x′_o+R′cosθ_o cosψ_o

y_ins＝y′_o+R′cosθ_o sinψ_o

z_ins＝z′_o+R′sinθ_o

其中R′＝kR，k为设定的常数且k≥1。

6.如权利要求1至5任一所述的一种无人飞行器空间避障方法，其特征在于，当障碍物为多个时，

步骤S2要对多个障碍物进行避障判断，如果无人飞行器的速度矢量v_u与障碍物O_i的速度矢量v_oi之间的相对速度矢量v_uoi在障碍物O_i形成的障碍锥内，则需要同时对多个障碍物进行避碰，否则不需要同时对多个障碍物进行避碰。

7.如权利要求6所述的一种无人飞行器空间避障方法，其特征在于，对于多个障碍物同时避障时，则各球冠大圆的并集为避障临界曲线。

8.使用如权利要求1至7任一所述的一种无人飞行器空间避障方法进行路径在线规划的方法，其特征在于，包括以下步骤：

步骤S51，将步骤S3和步骤S4确定的无人飞行器避碰速度矢量方向和避障点，以及当前时刻无人飞行器的运动状态以及目标点处要求的状态作为曲线路径在线重规划的初始条件；

步骤S52，根据曲线路径规划原理以及步骤S51确定的初始条件，对无人飞行器进行路径在线重规划。