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

本发明属于无人飞行器空间避障飞行的控制领域，尤其是涉及一种基于速度障碍圆弧法的无人飞行器避障方法，该方法针对的是二维平面内的无人飞行器避障问题。

背景技术：

无人飞行器在任务飞行过程中可利用自身携带的传感器装置探测到周围存在的动静态障碍物(后文将无人飞行器探测到的障碍物称为已知障碍物)，这些动静态障碍物可能会影响无人飞行器的安全飞行。通常，为了简化对障碍物威胁的判定，将无人飞行器假设为一质点，其当前时刻位置坐标为P_u(x_u,y_u)，同时根据无人飞行器和障碍物之间的相对尺寸大小将已知障碍物点O_i膨化为以P_oi为圆心、半径为R_i(i＝1,2,…)的障碍圆，然后过点P_u作障碍圆的切线l_i1和l_i2，则l_i1和l₂形成二维速度障碍锥。

目前，主要通过无人飞行器和障碍物之间的相对速度矢量v_uoi和位置矢量之间的夹角α_i与障碍锥的半顶角α_0i大小关系进行障碍威胁判断。当α_i≥α_0i时，已知障碍物O_i对无人飞行器沿当前航迹飞行不具有威胁；当α_i＜α_0i时，已知障碍物O_i对无人飞行器安全飞行产生威胁，无人飞行器需要对已知障碍物O_i进行避碰。

但是，根据上述理论确定无人飞行器需要进行避碰的障碍物时，却忽略了暂时不具威胁障碍的“潜在危险”。有些情况下，当无人飞行器沿避碰重规划的航迹飞行时，之前不具威胁障碍会对无人飞行器飞行产生威胁。为了确保无人飞行器避碰重规划航迹的安全性，需要提供一种无人飞行器避碰方法，可综合考虑已知障碍的“危险”和“潜在危险”。

而且，现有技术中的避碰算法很难解决多威胁障碍避碰的问题。诸如文献Mujumdar A,Padhi R.Reactive collision avoidance using nonlinear geometric and differential geometric guidance[J].Journal of guidance,Control,and Dynamics,2011,34(01):303-310.等根据最短接近点提出一种反应式避障算法，通过单视觉传感器获取威胁障碍信息，但算法针对单威胁障碍避碰问题，不能解决多威胁障碍避碰复杂问题。而文献Van den Berg J,Lin M,Manocha D.Reciprocal velocity obstacles for real-time multi-agent navigation[C]//IEEE InternationalConference on Robotics and Automation,2008:1928-1935.在多智能体飞行避碰中提出了一种互惠速度障碍法，用于解决多智能体相互之间的避碰，但该方法很难适应复杂环境下的动态避碰情况。

综上所述，现有技术中缺乏合适的无人飞行器避障方法，来解决因忽略“潜在危险”障碍而带来的影响，以及解决多障碍避碰的复杂问题。

技术实现要素：

本发明的目的在于提供一种无人飞行器避障方法，其能够有效避免现有避障算法中因忽略“潜在危险”障碍而带来的影响，并能解决多障碍避碰的复杂问题。

本发明提出的一种基于速度障碍圆弧法的无人飞行器避障方法，包括以下步骤：

步骤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，求解避障方向。

优选的，步骤S3中避碰判断具体采用以下方法：

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

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

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

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

优选的，步骤S4具体为：

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

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

优选的，步骤S5具体为：

步骤S51，建立动坐标系；

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

步骤S52，设定避碰规则；

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

步骤S53，求解避碰方向。

优选的，步骤S53具体为：

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

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

优选的，在求解避碰方向时，为了直观地确定无人飞行器的避障方向，将所有已知障碍物速度障碍圆弧对应的不可行速度矢量方向绘制在同一个一维坐标轴上。

本发明提出的一种基于速度障碍圆弧法的无人飞行器避障方法，通过速度障碍圆弧参数量化了威胁障碍的影响，综合考虑了“潜在危险”障碍的影响，并且简化了复杂环境下无人飞行器对多障碍的避碰问题，该方法大幅度提高了无人飞行器在复杂环境下的自主避障能力。

附图说明

图1为基于速度障碍圆弧法的无人飞行器避障方法流程图；

图2为速度障碍锥示意图；

图3为二维平面速度障碍法原理图；

图4为速度障碍圆弧示意图；

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

图6为UAV速度矢量旋转临界状态图；

图7为速度矢量几何关系图；

图8为单个一级威胁障碍避碰角度的坐标轴表示示意图；

图9为两个一级威胁障碍的避碰圆弧示意图；

图10为两个一级威胁障碍避碰角度的坐标轴表示示意图；

图11为一级和二级威胁障碍的避碰圆弧示意图；

图12为一级和二级威胁障碍的避碰圆弧相隔较远时的示意图；

图13为一级和二级威胁障碍的避碰圆弧相隔较近时的示意图。

具体实施方式

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

如图1所示，本发明的基于速度障碍圆弧法的无人飞行器避障方法主要包括5个步骤：构建二维速度障碍锥；建立速度障碍圆弧；定义已知障碍物的威胁等级，并进行避碰判断；求解速度障碍圆弧参数；求解避障方向。

无人飞行器(以下简称UAV)在任务飞行过程中可利用自身携带的传感器装置探测到周围存在的动静态障碍物(下称UAV探测到的障碍物为已知障碍物)，这些动静态障碍物可能会影响UAV的安全飞行。因此，需要设计障碍威胁判断准则，确定障碍物是‘危险’还是‘潜在危险’。通常，为了简化对障碍物威胁判定，将UAV假设为一质点，同时根据UAV和障碍物之间的相对尺寸大小对已知障碍物O_i(i＝1,2,…)进行‘膨化’处理，障碍物‘膨化’后的圆半径记作R_i(i＝1,2,…)。图2为已知障碍物对UAV所产生的速度障碍锥示意图，射线l_i1和l_i2过UAV质点且为障碍圆的切线。

将图2中速度障碍锥沿已知障碍物的速度矢量v_oi方向平移||v_oi||，得到如图3所示的二维平面速度障碍法原理图。在图3中，障碍物对UAV形成的障碍锥称为相对碰撞区(RelativeCollision Cone,RCC)，平移后形成的阴影障碍锥区域称为绝对碰撞区(Absolute Collision Cone,ACC)。那么，根据示意图3可对已知障碍威胁进行判断。

当UAV的速度矢量v_u位于ACC内时，已知障碍O_i对UAV的飞行具有威胁；当UAV的速度矢量v_u在ACC之外时，已知障碍O_i对UAV的当前航迹不会产生威胁。

假设UAV对已知威胁障碍避碰是通过法向加速度a_u进行机动，不改变UAV速度矢量v_u的大小，即通过改变UAV的速度矢量v_u方向进行避碰。那么，以UAV的位置点P_u为圆心、速度矢量大小||v_u||为半径的速度圆⊙P_u与已知威胁或具有‘潜在危险’障碍物的ACC产生交集G＝⊙P_u∩ACC，交集G为速度圆⊙P_u在ACC内的圆弧，称为速度障碍圆弧。其中，速度圆⊙P_u与ACC边界线l′_i1和l′_i2交点为避碰的两个临界状态点P_i1和P_i2，示意图如图4所示。

为了准确分析已知障碍的影响，定义已知障碍物的威胁等级。当速度矢量v_u∈ACC时，定义已知障碍O_i为一级威胁障碍物；当速度矢量且时，定义已知障碍O_i为二级威胁障碍物；当速度矢量且时，定义已知障碍O_i为三级威胁障碍物。

UAV沿航迹飞行过程中，必须对一级威胁障碍物进行避碰，且需要分析二级威胁障碍物在避碰中产生的影响，但可忽略三级威胁障碍物的影响。为了准确分析一级和二级威胁障碍物的影响，需要对速度障碍圆弧G进行定量表达。下面给出一级和二级威胁障碍物的速度障碍圆弧G定义。

记已知障碍O_i的速度障碍圆弧为G_i＝(r_i，β_i，γ_i)。其中，参数9i＝(P_i1+P_i2)/2；β_i为矢量的方向角；γ_i对应于速度障碍圆弧的1/2圆心角。

将图4中已知障碍O_i的速度障碍圆弧表示在UAV速度圆上的示意图如图5所示。对速度障碍圆弧三个参数r_i、β_i和γ_i的求解关键在求出矢量而矢量的求解需要求出ACC边界线l′_i1和l′_i2上的两个临界状态点P_i1和P_i2。下面通过示意图6中的速度矢量几何关系，确定临界状态点P_i1和P_i2所对应的UAV两个临界速度矢量和图6中的P_u′为平移后速度障碍锥的顶点，P_u′＝P_u+v_o，即UAV的位置坐标P_u沿障碍速度矢量v_o方向平移速度矢量大小v_o得到的位置坐标。

在示意图7中，UAV的两个临界速度矢量分别为和由图6中的速度矢量几何关系可得，

${\hat{v}}_u=v_{oi}+{\hat{v}}_{uoi}---\left(1\right)$

${\hat{v}}_u^{\′}=v_{oi}+{\hat{v}}_{uoi}^{\′}---\left(2\right)$

临界相对速度矢量和分别位于ACC边界线l₁和l₂上，且可表示为

其中，P_u′＝P_u+v_oi，q_i1和q_i2分别为ACC边界线l₁和l₂上的单位方向矢量。

q_i1和q_i2可通过障碍锥内的角度几何关系进行求解。下面给出q_i1的求解方法，q_i2同理可求。假设边界线l₁上存在一点Q_i1(x_i1,y_i1)，则点Q_i1可由式(5)和(6)确定。

其中，δ_i＝α_0i-α_i。

则单位方向矢量q_i1可表示为

那么，临界相对速度矢量与障碍物的速度矢量v_oi的夹角可表示为

$cos(\∠({\hat{v}}_{uoi},v_{oi}\left)\right)=cos(\∠(q_{i1},v_{oi}\left)\right)=\frac{q_{i1}\·v_{oi}}{\left|\right|v_{oi}\left|\right|}---\left(8\right)$

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

$cos(\mathrm{\π}-\∠({\hat{v}}_{uoi},v_{oi}\left)\right)=\frac{\left|\right|{\hat{v}}_{uoi}|{|}^2+|\left|v_{oi}\right|{|}^2-\left|\right|v_u|{|}^2}{2\left|\right|{\hat{v}}_{uoi}\left|\right|\left|\right|v_{oi}\left|\right|}---\left(9\right)$

则由式(7)和(9)可确定临界相对速度矢量另一个临界相对速度矢量可同理求得。那么，再由式(1)和(2)可求得UAV的两个临界速度矢量和

为了便于求解已知障碍O_i的速度障碍圆弧参数，将图6中的速度矢量几何关系抽象到如图7所示的矢量关系图上。其中，Q_oi为线段P_i1P_i2的中点。则根据图7的矢量关系图可对矢量进行求解。图7中的矢量关系可表示为，

其中，

由式(10)和(11)可确定矢量

式(12)确定了已知障碍O_i的速度障碍圆弧矢量则相应的速度障碍圆弧参数r_i、β_i和γ_i可以求解。

其中，X为惯性坐标系的水平坐标轴。

(一)单个一级威胁障碍物的避碰

根据求解的速度障碍圆弧参数β和γ，可以确定障碍圆弧所对应的圆心角范围为[β-γ,β+γ]。为直观地确定UAV最优避障方向，将UAV速度矢量v_u的方向角β_u及障碍圆弧所对应的圆心角范围[β-γ,β+γ]表示在一维坐标轴上，示意图如图8所示。那么，从图8可直观地确定UAV的最优避障方向。

(二)多威胁障碍的避碰

在未知动态环境下，UAV可能同时探测到多个不同威胁等级的障碍物，下面以两个障碍物的情况进行分析，三个及三个以上的情况可根据速度障碍圆弧法类似分析。

(1)两个一级威胁障碍物的避碰情况

两个一级威胁障碍物的避碰圆弧示意图如图9所示。此时，UAV的速度矢量v_u位于两个一级威胁障碍物的相交避碰圆弧内。根据速度障碍圆弧法，可确定两个一级威胁障碍物的障碍圆弧参数G₁＝(r₁,β₁,γ₁)和G₂＝(r₂,β₂,γ₂)。

根据两个一级威胁障碍圆弧参数G₁和G₂，可确定障碍圆弧所对应的圆心角范围为[β₁-γ₁,β₁+γ₁]∪[β₂-γ₂,β₂+γ₂]，UAV速度矢量v_u的方向角β_u和障碍圆弧圆心角范围一维坐标表示如图10所示。则从图10中可直观地确定UAV同时对两个一级威胁障碍物避碰的最优方向角。

(2)一级和二级威胁障碍物的避碰情况

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

图12为一级和二级威胁障碍物的避碰圆弧相交情况，其它一级和二级威胁障碍物的避碰圆弧情况如图12和图13所示，UAV的避碰分析同图11。

(3)其它威胁障碍的避碰情况

由于UAV避碰过程中可不考虑三级威胁障碍物的影响，则对一级和三级威胁障碍物的避碰可简化为对单个威胁障碍物的避碰；当已知障碍中没有一级威胁障碍物时，UAV不需要对已知障碍进行避碰。

下面以Pythagorean Hodograph曲线路径规划为例对基于速度障碍圆弧法的避碰算法进行验证。

假设UAV从起飞点P_s(0,0)到目标点P_f(500,1000)处执行任务，在未知动态环境下UAV探测到动态障碍物，并基于速度障碍圆弧法对UAV在线避障路径重规划进行仿真验证。

实施例1：UAV对单个一级威胁障碍物的避碰

ⅰ)不考虑二级和三级威胁障碍物的影响

UAV在飞行到P_u(200,400)处时，探测到障碍物O。通过携带的传感器装置获取的障碍物信息为：位置坐标P_o(113.9,522.8)、速度矢量大小||v_o||＝40、速度方向角β_o＝15°、膨化半径R＝45；另外，UAV自身的信息为：速度矢量大小||v_u||＝55、速度方向角β_u＝63.4°。那么，根据速度障碍圆弧法可计算得到：已知障碍为一级威胁障碍物，UAV需要对威胁障碍进行避碰。

通过速度障碍圆弧参数计算方法，可得到已知障碍的圆弧参数G＝(50.6,84.1°,23.1°)。那么，由障碍圆弧参数可确定避障点处的避障方向范围为[-180°,61°]∪[107.2°,180°]。

ⅱ)考虑二级和三级威胁障碍物的影响

当UAV对单个一级威胁障碍避碰过程中，同时也探测到二级和三级威胁障碍物，根据威胁圆弧参数计算方法，可计算出一级和二级威胁障碍物的圆弧参数G，如表1所示。

表1单个一级威胁障碍避碰情况下的圆弧参数

根据表1中的威胁障碍圆弧参数G可确定单个一级威胁障碍物避碰情况下的UAV避障方向角范围，如表2所示。

表2单个一级障碍避碰情况下的UAV避障方向角范围

对比表2中不同情况下的避障方向角范围，可直观地得出二级威胁障碍物对UAV自主避障的影响，使可行的避障方向角范围变小。

实施例2：UAV对多个一级威胁障碍物的避碰

ⅰ)不考虑二级和三级威胁障碍物的影响

当UAV在飞行到P_u(200,400)处同时探测到两个威胁障碍物情况下。首先，通过传感器装置确定障碍物的信息为：位置坐标P_o1(114.0,522.9)、P_o2(322.9,486.0)；速度矢量大小||v_o1||＝40、||v_o2||＝35；速度方向角β_o1＝15°、β_o2＝125°；膨化半径R₁＝45、R₂＝35。根据速度障碍圆弧法可计算得到：两个已知障碍都为一级威胁障碍物，UAV需要对两个威胁障碍进行避碰。

依据速度障碍圆弧参数的计算方法，相应的两个已知一级威胁障碍物的圆弧参数为G₁＝(50.6,84.1°,23.1°)，G₂＝(53.4,72.2°,13.5°)。那么，由障碍圆弧参数可确定避障点处的避障方向范围为[-180°,59.8°]∪[107.2°,180°]。

ⅱ)考虑二级和三级威胁障碍物的影响

依据速度障碍圆弧参数计算方法，得出的一级和二级威胁障碍圆弧参数G如表3所示。

表3多个一级威胁障碍避碰情况下的圆弧参数

由表3中威胁障碍的圆弧参数可确定考虑二级和三级威胁障碍物前后的UAV避障方向角范围，如表4所示。

表4多个一级障碍避碰情况下的UAV避障方向角范围

同样，对比表4中不同情况下的UAV避障方向角范围可得：考虑二级和三级威胁障碍物影响后，使得UAV避障方向角范围变小。因此，UAV在二级威胁障碍影响下的避障方向角需要重新确定，上述仿真结果验证了避碰算法的有效性和实用性。

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