一种动态不确定环境下无人飞行器平面自主避障方法

摘要

本发明公开了一种动态不确定环境下无人飞行器平面自主避障方法，主要包括以下步骤：探测无人飞行器和动态障碍物的位置和速度；构建二维速度障碍锥；选择动态不确定性表示方法，表示速度障碍区；求解避障方向。为了简化计算，进一步将考虑动态不确定性的速度障碍区进行规则化处理，然后以此为基础，采用速度障碍圆弧法求解避障方向。该方法能够有效避免现有避障算法中因忽略“动态障碍物的机动性能”和“传感器的探测误差”而带来的影响，实现动态不确定环境下的无人飞行器平面自主避障。

说明书

技术领域

本发明属于无人飞行器空间避障飞行的控制领域，尤其是涉及一种动态不确定环境下无人飞行器平面自主避障方法。

背景技术

无人飞行器在任务飞行过程中可利用自身携带的传感器装置探测到周围存在的动静态障碍物，这些动静态障碍物可能会影响无人飞行器的安全飞行。对于静态障碍物，无人飞行器可以通过现有的方法较为轻松地实现避障；但对于动态障碍物，尤其是对于具有一定机动性能的动态障碍物，现有技术中还缺少简单实用的无人飞行器平面自主避障方法。

目前，无人飞行器在避障过程中，主要依靠无人飞行器自身携带的传感器对无人飞行器的当前时刻位置P_U(x_u,y_u)、速度v_u(v_u>u,v_u>u)，其中v_u为速度矢量v_u的大小，θ_u为速度矢量v_u的方向角进行探测；并依靠无人飞行器自身携带的传感器探测动态障碍物O的位置P_O(x_o,y_o)、速度v_o(v_o>o,v_o>o)，其中v_o为速度矢量v_o的大小，θ_o为速度矢量v_o的方向角。传感器的探测误差会影响后续的无人飞行器避障，但是现有的无人飞行器自主避障方法很少考虑传感器的探测误差带来的影响。

综上所述，现有的无人飞行器自主避障方法中，很少考虑动态障碍物的运动不确定性(即动态障碍物的机动性)和传感器的探测误差带来的避障影响，上述问题的忽略势必给后续的无人飞行器自主避障带来安全隐患，并使得避障的可靠性大幅度降低。

发明内容

本发明的目的在于提供一种无人飞行器避障方法，其能够有效避免现有避障算法中因忽略“动态障碍物的机动性能”和“传感器的探测误差”而带来的影响，能够实现动态不确定环境下的无人飞行器平面自主避障

本发明提出的一种动态不确定环境下无人飞行器平面自主避障方法，包括以下步骤：

步骤S1，探测无人飞行器和动态障碍物的位置和速度；

通过无人飞行器携带的传感器探测无人飞行器的当前时刻位置P_U(x_u,y_u)、速度v_u(v_u>u,v_u>u)，其中v_u为速度矢量v_u的大小，θ_u为速度矢量v_u的方向角；并探测动态障碍物O的位置P_O(x_o,y_o)、速度v_o(v_o>o,v_o>o)，其中v_o为速度矢量v_o的大小，θ_o为速度矢量v_o的方向角；

步骤S2，构建二维速度障碍锥；

将动态障碍物O膨化为以P_O为圆心、半径为R_o的障碍圆S_c，然后过点P_U作障碍圆S_c的两条切线，则两条切线形成障碍锥CC；然后将障碍锥CC沿v_o的速度矢量方向平移v_o，得到二维速度障碍锥VO；

步骤S3，选择动态不确定性表示方法，表示速度障碍区；

将动态障碍物O的机动性和传感器的探测误差通过障碍物的速度矢量v_o方向角偏差进行表示，并将最大不确定性表示为±Δθ_o，其中逆时针为正，顺时针为负；将障碍物速度矢量v_o对应的两个最大不确定性的速度矢量表示为v_o′和v_o"，并将v_o、v_o′和v_o"形成的速度圆弧表示为S_arcs；则速度圆弧S_arcs上任意一点对应的障碍物速度矢量都将产生一个VO，将其最大不确定性对应的速度障碍锥分别表示为VO′和VO"，VO′的顶点为P_vo′，VO"的顶点为P_vo"；则考虑动态不确定性的速度障碍区∪VO为：∪VO＝VO′∪…∪VO∪…∪VO"；

步骤S4，根据步骤S3得到速度障碍区∪VO，求解避障方向。

由于考虑动态不确定性的速度障碍区∪VO是一个不规则的图形，为了减小在避障过程中数据计算量，需将上述的考虑动态不确定性的速度障碍区∪VO进行规则化的处理。优选的采用如下方法，步骤S4具体为：

步骤S41，将考虑动态不确定性的速度障碍区规则化；

以VO的顶点P_vo为圆心，线段P_voP_vo′为半径，作⊙P_vo；然后分别作VO两条边界线l_vo1，l_vo2的平行线l_vo1^*，l_vo2^*并与⊙P_vo相切，并取l_vo1^*，l_vo2^*与VO轴线P_voQ_o的交点P_vo^*；将以P_vo^*为顶点，以l_vo1^*和l_vo2^*为两条边界线的二维速度障碍锥VO^*作为规则化后的考虑动态不确定性的速度障碍区；

步骤S42，根据步骤S41得到的VO^*，进而求解避障方向。

为了便于求解规则化后的考虑动态不确定性的速度障碍区VO^*，采用速度障碍圆弧法求解避障方向，优选的采用如下方法，步骤S42具体为：

步骤S421，建立速度障碍圆弧G^*；

以P_U为圆心，速度矢量大小v_u为半径做速度圆⊙P_U，然后取速度圆⊙P_U与VO^*相交且位于VO^*内部的圆弧为速度障碍圆弧G^*，即G^*＝⊙P_U∩VO^*；⊙P_U与VO^*的两条边界线l_vo1^*和l_vo2^*的交点为和

步骤S422，求解速度障碍圆弧参数；

步骤S423，求解避障方向。

在求解速度障碍圆弧参数的过程中，优选的采用如下方法，步骤S422具体为：

步骤S4221，将速度障碍圆弧表示为其中为矢量的方向角；β^*对应于速度障碍圆弧的1/2圆心角；

步骤S4222，根据速度矢量关系，求解矢量后，进而求解速度障碍圆弧参数。

现有的避障方法主要是对“危险”障碍物进行避碰，但却忽略了暂时不具威胁障碍的“潜在危险”。有些情况下，当无人飞行器沿避碰重规划的航迹飞行时，之前不具威胁的障碍物会对无人飞行器飞行产生威胁。为了确保无人飞行器避碰重规划航迹的安全性，需要综合考虑障碍物的“危险”和“潜在危险”，优选的采用以下方法，步骤S423具体为：

步骤S4231，定义动态障碍物O的威胁等级，并进行避碰判断；

当无人飞行器的速度矢量v_u∈VO^*时，将动态障碍物O定义为一级威胁障碍物；当且时，将动态障碍物O定义为二级威胁障碍物；当且时，将动态障碍物O定义为三级威胁障碍物；

对于一级威胁障碍物，需要进行避碰；对于二级威胁障碍物，需要分析二级威胁障碍物在无人飞行器避碰过程中产生的影响；对于三级威胁障碍物，不需要对障碍物进行避碰，则退出；

步骤S4232，求解避障方向。

当动态障碍物O存在多个时，且动态障碍物O的威胁等级存在多个时，优选的采用如下避碰判断方法，步骤S4231中避碰判断的方法具体为：

对于探测到的动态障碍物O，当其中至少存在1个一级威胁障碍物时，需要进行避碰；否则不需要对障碍物进行避碰，则退出。

优选的，步骤S4231中的分析二级威胁障碍物在无人飞行器避碰过程中产生影响的具体方法为：

当某个二级威胁障碍物产生的速度障碍圆弧与一级威胁障碍物的速度障碍圆弧相交时，则将该二级威胁障碍物看作一级威胁障碍物；

否则，在求解避障方向后，增加步骤S5：步骤S5，在求解的避障方向中剔除该二级威胁障碍物速度障碍圆弧对应的不可行速度矢量方向范围，其中不可行速度矢量方向范围定义为：若无人飞行器的速度矢量方向保持在某一范围内时，将与已知威胁障碍物发生碰撞，则称该矢量方向范围为不可行速度矢量范围。

优选的，步骤S5具体为：

步骤S51，建立动坐标系；

避碰时，以无人飞行器的速度矢量v_u方向为纵轴y，速度矢量v_u顺时针旋转90°方向为横轴x，建立动坐标系P_Uxy；

步骤S52，设定避碰规则；

假设无人飞行器对已知障碍物避碰是通过法向加速度a_u进行机动，不改变无人飞行器速度矢量v_u的大小，即通过改变无人飞行器的速度矢量v_u方向进行避碰；假设避碰过程中无人飞行器速度矢量v_u的方向角β_u取值范围为[-π,π]；

步骤S53，求解避碰方向。

优选的，步骤S53具体为：

对于单个动态障碍物，依据求解的速度障碍圆弧参数，确定避碰不可行速度矢量方向范围为则相应的避碰方向角范围为

对于多个动态障碍物，依据求解的速度障碍圆弧参数，确定多威胁障碍下避碰不可行速度矢量方向范围。当动态障碍物的数量为i个时，则多威胁障碍下避碰不可行速度矢量方向范围为则相应的避碰方向角范围为

附图说明

图1为动态不确定环境下无人飞行器平面自主避障方法流程图；

图2为碰撞锥CC示意图；

图3为二维速度障碍锥VO示意图；

图4为二维速度障碍锥VO动态不确定性表示示意图；

图5为规则化后的考虑动态不确定性的速度障碍区示意图；

图6为速度障碍圆弧法示意图；

图7为规则化后的考虑动态不确定性的速度障碍区VO^*的速度障碍圆弧法示意图；

图8为单个速度障碍圆弧示意图；

图9为多威胁障碍物速度障碍圆弧示意图。

具体实施方式

下面结合附图1至附图9，介绍本发明的具体实施方式。

如图1所示，本发明的一种动态不确定环境下无人飞行器平面自主避障方法主要包括4个步骤：探测无人飞行器和动态障碍物的位置和速度；构建二维速度障碍锥；选择动态不确定性表示方法，表示速度障碍区；求解避障方向。

在未知动态环境下，无人飞行器执行任务的过程中，利用自身携带的传感器装置，可以探测到周围环境中存在的动态障碍物O，并可以获取自身和障碍物O的位置和速度信息。无人飞行器的位置坐标为P_U(x_u,y_u)，速度矢量为v_u(v_u>u,v_u>u)，其中v_u为速度矢量v_u的大小，θ_u为速度矢量v_u的方向角；障碍物的位置坐标为P_O(x_o,y_o)，速度矢量为v_o(v_ocosθ_o,v_o>o)，其中v_o为速度矢量v_o的大小，θ_o为速度矢量v_o的方向角。为便于建立无人飞行器的避障模型，将无人飞行器简化为一点，并根据无人飞行器和障碍物之间的相对大小关系将障碍物“膨胀”为半径为R_o的威胁圆S_c。同时，设定无人飞行器传感器的探测范围为d_uo。无人飞行器和动态障碍物O之间形成的碰撞锥CC如图2所示。

碰撞锥CC可表示为其中，l_uo为以P_U为顶点并与相对速度矢量v_uo共线的射线，M为射线l_uo与威胁圆S_c的交点。

二维速度障碍锥VO的定义基于碰撞锥CC，VO的示意图如图3所示。根据图3可对VO进行定义，即将CC内所有的点平移速度矢量v_o得到的点集合。

本发明将障碍物的机动性和传感器的探测误差通过障碍物的速度矢量v_o方向角偏差进行表示，并将最大不确定性表示为±Δθ_o，其中逆时针为正，顺时针为负，示意图如图4所示。障碍物速度矢量v_o对应于两个最大不确定性的速度矢量为v_o′和v_o"，并满足||v_o′||＝||v_o"|＝v_o，且形成的速度圆弧表示为S_arcs。那么，速度圆弧S_arcs上任意一点对应的障碍物速度矢量都将产生一个VO，最大不确定性对应的速度障碍锥分别为VO′和VO"。因此，考虑动态不确定性的速度障碍区∪VO可表示为：∪VO＝VO′∪…∪VO∪…∪VO"。

由VO的定义可对无人飞行器探测到的未知障碍物威胁性进行判断，当P_U+v_u∈VO时，未知障碍物具有威胁，无人飞行器需要对障碍物进行避碰；当时，未知障碍物不具有威胁，此时无人飞行器不会与障碍物相碰。为简化未知障碍物威胁性判断，可根据示意图2中的角度大小关系得出未知障碍物威胁性判断的结论。

当α＜γ时，未知障碍物具有威胁，无人飞行器需要对障碍物进行避碰；当α≥γ时，未知障碍物不具有威胁，无人飞行器不需要对障碍物进行避碰。其中，α为相对速度矢量v_uo与CC轴线P_UP_O之间的夹角，γ为CC顶角的一半。

α和γ的大小满足下面关系式，

同理，可逐一判断无人飞行器是否需要对考虑动态不确定性的速度障碍区∪VO中的VO′和VO″等其它速度障碍锥进行避碰，进而求解避障方向。由于速度障碍区∪VO是一个不规则图形，直接采用速度障碍区∪VO进行避碰判断，并求解避障方向的计算量大。

为此，本发明在求解避障方向前，先对∪VO进行几何处理，将考虑动态不确定性的速度障碍区∪VO进行规则化。

动态不确定性可表示为障碍物的速度圆弧S_arcs，相应所有速度障碍顶点的集合也为圆弧，且曲率半径等于速度圆弧S_arcs半径v_o。考虑到∪VO的几何形状属于不规则图形，为了简化计算量，对∪VO进行几何处理，将考虑动态不确定性的速度障碍区∪VO进行规则化，具体如图5所示。

以VO的顶点P_vo为圆心，线段P_voP_vo′为半径，作⊙P_vo。则顶点位于⊙P_vo内的速度障碍包含了顶点在不确定性圆弧上的速度障碍，而不确定性圆弧可通过障碍物的速度圆弧S_arcs进行定义，

不确定性圆弧由障碍物的速度圆弧S_arcs转化得到，不确定性圆弧上的每一点对应于速度圆弧S_arcs上相应的VO顶点，且和S_arcs之间具有相同的圆心角和曲率半径。

分别作VO两条边界线l_vo1，l_vo2的平行线l_vo1^*，l_vo2^*并与⊙P_vo相切，同时l_vo1^*，l_vo2^*与VO的轴线P_voQ_o交于点P_vo^*。则对∪VO进行几何扩展后形成的规则化后的考虑动态不确定性的速度障碍区VO^*具有与VO类似的几何形状，VO^*的顶点为P_vo^*，两条边界线为l_vo1^*、l_vo2^*，且VO^*与VO之间有如下结论，

VO^*与VO共轴线、顶角相等，且产生的VO^*是考虑动态不确定性的最小速度障碍区。

⊙P_vo的半径P_voP_vo′，VO^*的半顶角γ^*，顶点P_vo^*和两条边界线l_vo1^*，l_vo2^*的斜率可由下列关系式确定。

γ^*＝γ(4)

则VO^*的所有参数能够由式(3)～(7)进行求解，那么规则化后的考虑动态不确定性的速度障碍区可以被建立并表示。

以无人飞行器的位置坐标点P_U为圆心，无人飞行器的速度矢量大小v_u为半径，作⊙P_U，⊙P_U与VO相交，示意图如图6所示。这种通过位于VO内的圆弧大小G来量化威胁障碍物影响的方法称为速度障碍圆弧法。

在示意图6中，⊙P_U与VO的临界边界线l_vo1相交于点A₁和A₂，对应于点A₁和A₂为无人飞行器避障的两个临界状态点，即无人飞行器的期望速度矢量满足，

为准确表达位于VO内的圆弧大小G，采用三个参数r，β对其进行描述，即其中，P_mid＝(A₁+A₂)/2，为矢量与ox轴之间的夹角，β为速度圆弧对应的圆心角一半。下面对速度圆弧三个参数进行求解，

根据式(9)～(11)的几何关系，可确定临界点A₁的坐标。同理，A₂的坐标也可以确定。那么，线段A₁A₂的中点P_mid坐标也可以确定。因此，位于VO内的圆弧大小G的三个参数可确定为，

当考虑威胁障碍的机动性和传感器探测误差时，需要使用规则化后的考虑动态不确定性的速度障碍区VO^*。根据速度障碍圆弧法，可对规则化后的考虑动态不确定性的速度障碍区VO^*进行避碰，示意图如图7所示。

从图7中可以得出，当考虑动态不确定性时，产生的速度障碍圆弧变大。参照前述实施例中VO参数的求解方法，只要将VO参数替换成VO^*的参数，即可实现对VO^*速度障碍圆弧参数的求解。

下面通过VO^*的速度障碍圆弧法对单个威胁障碍物避碰和多威胁障碍物避碰方向角的求解进行分析。

(1)单个威胁障碍物避碰

①对于单个一级威胁障碍物而言，需要进行避碰，单个速度障碍圆弧示意图如图8所示，该速度障碍圆弧大小则该速度障碍圆弧对应的圆心角范围为即确定的避碰不可行速度矢量方向范围为则相应的避碰方向角范围为对于一级威胁障碍物而言，无人飞行器的速度矢量v_u方向角θ_u位于p内。因此，将无人飞行器的速度矢量v_u方向角θ_u偏转出p，即可实现无人飞行器对威胁障碍物的避碰。

②对于单个二级或三级威胁障碍物而言，暂不需要进行避碰。

(2)多威胁障碍物避碰

当无人飞行器同时需要对多威胁障碍物避碰时，可通过两个威胁障碍物的避碰进行分析，三个及三个以上威胁障碍物的避碰可类似进行分析。示意图9给出了两个威胁障碍圆弧示意图。

①两个威胁障碍中均为一级威胁障碍物。威胁障碍物O₁对应的速度障碍圆弧为威胁障碍物O₂对应的速度障碍圆弧为且无人飞行器的速度矢量v_u方向角满足

θ_u∈p₁∩p₂(19)

而两个威胁障碍圆弧对应的圆心角范围p可表示为

p＝p₁∪p₂(21)

那么，当无人飞行器的速度矢量v_u方向角偏θ_u转出p时，即可实现对两个威胁障碍物的避碰。

②两个威胁障碍中一个一级威胁障碍物，一个二级威胁障碍物。此时，无人飞行器需要对一级威胁障碍物进行避碰，同时考虑二级威胁障碍物的影响。根据速度障碍圆弧法求解的一级和二级威胁障碍物的圆弧参数，按照两个一级威胁障碍物避碰的最优方向角分析方法，可确定无人飞行器的最优避碰方向角。此时，无人飞行器的最优避碰方向角不仅考虑了“危险”障碍的影响，也考虑了“潜在危险”障碍的影响，提高了无人飞行器避碰的可靠性和安全性。

③两个威胁障碍中一个一级威胁障碍物，一个三级威胁障碍物。则不需考虑三级威胁障碍物的影响，对一级和三级威胁障碍物的避碰可简化为对单个威胁障碍物的避碰。

④其它威胁障碍的避碰

当已知障碍中没有一级威胁障碍物时，无人飞行器则不需要对已知障碍进行避碰。

实施例1：

在动态不确定环境下，设定无人飞行器从起飞点P_s(0,0)到目标点P_t(500,1000)处执行任务。在无人飞行器飞行过程中，通过自身携带的传感器装置，探测到周围环境中的威胁障碍物，下面基于动态不确定环境下的无人飞行器自主避障算法对无人飞行器进行避碰仿真，仿真考虑动态环境中的不确定性。无人飞行器和障碍物的初始化条件如表1所示。其中，无人飞行器探测到的障碍物中只有一个具有一级威胁，其它都为三级威胁障碍物。

表1单个威胁障碍避碰初始化条件

根据障碍圆弧大小的计算方法，可以得到表2中在不同动态不确定性下的威胁障碍圆弧参数。其中，Δθ₀为0°时表示不考虑复杂环境下的动态不确定性。对比表2中不同动态不确定下的威胁障碍圆弧参数，可以得出：障碍圆弧范围的大小与动态不确定性成正相关。

表2不同动态不确定性下的威胁障碍圆弧参数

那么，无人飞行器对威胁障碍物的避碰方向角可确定为障碍圆弧范围以外的值。根据初始化条件和文献“张毅,杨秀霞,周硙硙.基于速度障碍法的多无人飞行器可飞行航迹优化生成[J].系统工程与电子技术,2015,37(2):323‐330.”中的计算方法，可以得出在动态不确定性Δθ₀＝5°时的避障时间t＝2.63、避障点为(207,548)。相应的仿真结果表明：无人飞行器在考虑动态不确定性下，能够实现对单个一级威胁障碍物的避碰。

实施例2：

当无人飞行器同时探测到多威胁障碍物时，相应的初始化条件如表3所示。从表3中可以得出：障碍物O₁，O₂具有一级威胁；而O₃，O₄为三级威胁障碍物。

表3多威胁障碍避碰初始化条件

表4给出了两个威胁障碍在不同动态不确定性下的圆弧参数，而整个障碍圆弧范围为两个威胁障碍圆弧范围的并集，且随动态不确定性的增加而变大。

表4不同动态不确定性下的威胁障碍圆弧参数

同样，无人飞行器对多威胁障碍物的避碰方向角可确定为整个障碍圆弧范围以外的值，再根据相应的初始化条件可确定避障时间t＝2.38、避障点为(291,516)。相应的仿真结果表明：无人飞行器能够实现在动态不确定性下的多威胁障碍避碰。

最后应说明的是，以上所述仅为本发明的优选实施例，并不用于限制本发明，尽管参照前述实施例对发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。