一种基于速度障碍圆弧法的无人飞行器避障方法与流程

技术特征：

1.一种基于速度障碍圆弧法的无人飞行器避障方法，其特征在于，包括以下步骤：

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

根据无人飞行器在二维空间中的当前时刻位置坐标P_u(x_u,y_u)和探测到的已知障碍物O_i(i＝1,2,…)的位置坐标P_oi(x_oi,y_oi)，将已知障碍物点O_i膨化为以P_oi为圆心、半径为R_i(i＝1,2,…)的障碍圆，然后过点P_u作障碍圆的切线l_i1和l_i2，则l_i1和l_i2形成二维速度障碍锥，将该二维速度障碍锥内部空间称为相对碰撞区RCC；

步骤S2，建立速度障碍圆弧G；

将RCC沿已知障碍物O_i的速度矢量v_oi方向平移||v_oi||，得到绝对碰撞区ACC，切线l_i1和l_i2平移后记为l′_i1和l′_i2；

以无人飞行器位置点P_u为圆心，速度矢量大小||v_u||为半径的速度圆⊙P_u，然后取速度圆⊙P_u与ACC相交且位于ACC内部的圆弧为速度障碍圆弧G，即G＝⊙P_u∩ACC；速度圆⊙P_u与ACC边界线l′_i1和l′_i2的交点为避障的两个临界状态点P_i1和P_i2；

步骤S3，定义已知障碍物的威胁等级，并进行避碰判断；

当无人飞行器的速度矢量v_u∈ACC时，将已知障碍物O_i定义为一级威胁障碍物；当且时，将已知障碍物O_i定义为二级威胁障碍物；当且时，将已知障碍物O_i定义为三级威胁障碍物；

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

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

步骤S5，求解避障方向。

2.如权利要求1所述的一种基于速度障碍圆弧法的无人飞行器避障方法，其特征在于，步骤S3中避碰判断具体采用以下方法：

对于探测到的i(i＝1,2,…)个已知障碍物O_i，当其中至少存在1个一级威胁障碍物时，需要进行避碰；否则不需要对障碍物进行避碰，则退出。

3.如权利要求1或2所述的一种基于速度障碍圆弧法的无人飞行器避障方法，其特征在于，步骤S3中的分析二级威胁障碍物在无人飞行器避碰过程中产生影响的具体方法为：

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

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

4.如权利要求1所述的一种基于速度障碍圆弧法的无人飞行器避障方法，其特征在于，步骤S4具体为：

步骤S41，将已知障碍物O_i的速度障碍圆弧表示为G_i＝(r_i,β_i,γ_i)，其中Q_oi＝(P_i1+P_i2)/2；β_i为矢量的方向角；γ_i对应于速度障碍圆弧的1/2圆心角；

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

5.如权利要求3所述的一种基于速度障碍圆弧法的无人飞行器避障方法，其特征在于，步骤S5具体为：

步骤S51，建立动坐标系；

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

步骤S52，设定避碰规则；

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

步骤S53，求解避碰方向。

6.如权利要求5所述的一种基于速度障碍圆弧法的无人飞行器避障方法，其特征在于，步骤S53具体为：

对于单个已知障碍物，依据求解的速度障碍圆弧参数，确定避碰不可行速度矢量方向范围为[β-γ,β+γ]，则相应的避碰方向角范围为[-π,β-γ]∪[β+γ,π]；

对于多个已知障碍物，依据求解的速度障碍圆弧参数，确定多威胁障碍下避碰不可行速度矢量方向范围为[min{β_i-γ_i},max{β_i+γ_i}]，则相应的避碰方向角范围为[-π,min{β_i-γ_i}]∪[max{β_i+γ_i},π]。

7.如权利要求5或6所述的一种基于速度障碍圆弧法的无人飞行器避障方法，其特征在于，求解避碰方向时，为了直观地确定无人飞行器的避障方向，将所有已知障碍物速度障碍圆弧对应的不可行速度矢量方向绘制在同一个一维坐标轴上。