[发明专利]一种基于三维动态碰撞区的无人机自主避撞决策方法

权利要求书

1.一种基于三维动态碰撞区的无人机自主避撞决策方法，其特征在于，具体包括如下步骤：

步骤一：在当前无人机飞往目标的航路上，获取当前无人机所在空域的综合态势信息；

步骤二：针对当前采样时刻，判断无人机是否处于紧急转向段，如果是，则继续进行紧急转向，直至无人机进入平稳飞行段；否则，无人机继续平稳飞行段；

步骤三：当无人机处于平稳飞行段时，计算当前采样时刻探测到的每个障碍物的威胁指数；

假设目标视线由无人机质心指向障碍物质心；

第i个障碍物的总威胁指数T_i由距离威胁指数T_ri、角度威胁指数T_ai和速度威胁指数T_vi三部分组成；

各威胁指数的定义如下所示：

(1)距离威胁指数T_ri:

其中，r_a为警戒距离，r_d为危险距离；R_Li为无人机与第i个障碍物的相对距离；当R_Li大于r_a时，障碍物的距离威胁指数为零；当R_Li介于r_a与r_d时，R_Li越小，距离威胁指数越大；R_Li小于r_d时，无人机与障碍物的相对距离过近，距离威胁指数达到最大；

(2)角度威胁指数T_ai:

q_r为无人机相对于障碍物的速度方向与目标方向之间的夹角，相对速度方向左偏时为正；当时，无人机与障碍物之间的相对距离出现减小的趋势，表明无人机正在接近障碍物，存在碰撞风险；|q_r|越接近于零，则碰撞风险越大，对应的角度威胁指数越大；时，相对距离呈不变或增大趋势，表明无人机正在远离障碍物，此时角度威胁指数为零；

(3)速度威胁指数T_vi:

当障碍物为动态入侵机时：

式中，V_A为无人机的地速大小，V_i为入侵机的地速大小；V_i∈[0,0.6V_A)时入侵机速度明显小于无人机速度，此时相对速度主要由无人机决定，无人机通过调整自身速度较为容易地改变相对速度，避撞难度较低，入侵机的速度威胁指数较小；V_i∈[0.6V_A,1.5V_A)时，V_i接近甚至超过V_A，无人机通过调整自身速度而改变相对速度的能力显著减弱，避撞难度提升，入侵机的速度威胁指数随V_i与V_A比值的增大而增大；V_i∈(1.5V_A,+∞)时，入侵机的速度威胁指数达到最大；

当障碍物为静态障碍物时，威胁评估对象为静态障碍物表面上与无人机距离最近的一点，且V_i＝0；

步骤四、将当前采样时刻下，各障碍物对应的威胁指数分别采用直接线性加权求和，得到各障碍物的总威胁指数；

第i个障碍物的总威胁指数计算如下：

T_i＝ω_rT_ri+ω_aT_ai+ω_vT_vi (4)

其中，ω_r为距离威胁指数T_ri对应的权重，ω_a为角度威胁指数T_ai对应的权重，ω_v为速度威胁指数T_vi对应的权重，ω_r+ω_a+ω_v＝1，且ω_r＞0，ω_a＞0，ω_v＞0；

步骤五、按总威胁指数从大到小的顺序对所有障碍物进行排序，判断是否有至少一个障碍物的总威胁指数大于警戒阈值，如果是，则说明该障碍物为威胁源，执行步骤六；否则，无人机继续按原航路飞行；

步骤六、判断各威胁源中是否存在警戒目标，如果是，计算无人机对于警戒目标的无机动碰撞区的边界条件；进入步骤七；否则，进行航路重规划；

警戒目标是指威胁源中的入侵机；

无机动碰撞区的边界条件是指：当无人机和警戒目标最接近时恰好发生碰撞，即最近点的距离恰好等于最小安全距离；

具体计算过程如下：

步骤601、定义基本的坐标系和运动变量；

定义地面系O_gx_gy_gz_g(S_g)的原点O_g为地面上某一固定点，x_g轴指向地平面的北向，y_g轴指向地平面的西向，z_g轴按右手定则铅垂向上；

步骤602、假设无人机A与入侵机B均匀速直线飞行，通过综合态势信息查找二者质心的位置向量及地速向量；

无人机A质心的位置向量为P_A，入侵机B质心的位置向量为P_B，无人机A的地速向量为V_A，入侵机B的地速向量为V_B；

地速向量V_A在水平面O_gx_gy_g的投影与x_g轴之间的夹角为χ_A，地速向量V_B在水平面O_gx_gy_g的投影与x_g轴之间的夹角为χ_B，根据右手定则，投影相对于x_g轴向左旋转时夹角为正；地速向量V_A与水平面O_gx_gy_g之间的夹角为γ_A，地速向量V_B与水平面O_gx_gy_g之间的夹角为γ_B，速度向量指向水平面上方时夹角为正；

无人机A与入侵机B之间的相对距离为R_L，相对速度为V_r；

步骤603、以无人机A质心为原点，建立与无人机A质心固连的空间直角坐标系O_rx_ry_rz_r；

定义该坐标系的x_r轴与地速向量V_A在水平面上的投影重合，z_r轴在铅垂面内垂直于x_r轴指向上方，y_r轴垂直于O_rx_rz_r平面，其方向由右手定则确定；

地速向量V_B的水平面投影与x_r轴之间的夹角为ψ_B，相对距离R_L的水平面投影与x_r轴之间的夹角为ψ_L，当投影相对于x_r轴向左旋转时为正；地速向量V_A与O_rx_ry_r平面的夹角等同于地速向量V_A与水平面O_gx_gy_g之间的夹角γ_A；地速向量V_B与O_rx_ry_r平面的夹角等同于地速向量V_B与水平面O_gx_gy_g之间的夹角γ_B；相对距离R_L与O_rx_ry_r平面的夹角为γ_L；当向量在平面上方时夹角为正；

步骤604、无人机A不改变当前地速，建立t时刻无人机与入侵机的相对距离R_L(t)表达式；

t时刻无人机与入侵机的相对距离R_L(t)为：

Δx、Δy、Δz分别为相对距离R_L沿x_r轴、y_r轴和z_r轴的分量；

步骤605、根据相对距离R_L(t)表达式计算无机动碰撞区的边界条件；

边界条件解析式为：

其中，R₀为无人机的最小安全距离；

步骤七，判断警戒目标是否位于相应的无机动碰撞区边界条件内，如果是，进入步骤八，则，进行航路重规划；

步骤八、在无机动碰撞区内预测无人机避开警戒目标可用的逃逸时间和逃逸距离；

首先，建立最大机动碰撞区和不可规避区；

(1)对最大机动碰撞区进行建模，建立以t和R_L为未知量的二元方程组：如下

R_L(t)≠0时，R_L′(t)＝0等价于将展开整理得：

a_h为紧急转向期间，无人机水平方向上的平均加速度；a_v为紧急转向期间，无人机铅垂方向上的平均加速度；且a_h＞0表示平均加速度沿O_ry_r轴正方向，a_v＞0表示平均加速度沿O_rz_r轴正方向；

(2)对不可规避区进行建模，具体如下：

不可规避区分为两类：单向不可规避区和完全不可规避区；

单向不可规避区为无机动碰撞区与某一种紧急转向对应的最大机动碰撞区的交集；完全不可规避区为所有单向不可规避区的交集；采用三种紧急转向对应的最大机动碰撞区与无机动碰撞区的交集来表示完全不可规避区；

然后，从最大机动碰撞区和不可规避区建模过程中，提取碰撞区安全信息，并存储逃逸时间和逃逸距离；

首先，针对无人机-入侵机对，危险目标j和应急机动方式i的单向不可规避区定义为E_ij；应急机动方式的集合为{l,r,u}，包括向左滚转并拉升、向右滚转并拉升和拉升；

安全信息的定义基于危险目标，即位于相应无机动碰撞区内的警戒目标；包括：单向逃逸时间/距离，以及完全逃逸时间；

单向逃逸时间/距离：分为局部和全局两类；

其中，局部单向逃逸时间/距离是指一个危险目标j到达单向不可规避区E_ij的剩余时间/距离，对应于某一特定的应急机动方式和无人机-入侵机对的安全信息，表示为msg_ij，i∈{l,r,u}，j∈{1,2,…,n}，n为无人机探测到的入侵机数量；

全局单向逃逸时间/距离是指对应于同一种应急机动方式i，全局最小的局部单向逃逸时间/距离，表示为

完全逃逸时间：分为局部和全局两类；

其中，局部完全逃逸时间是指一个危险目标j到达完全不可规避区E_com,j的剩余时间，表示为

全局完全逃逸时间是指全局最小的局部完全逃逸时间，表示为

“单向”表示安全信息的行下标为集合{l,r,u}中的某一个给定值，“完全”则表示行下标将遍历地从{l,r,u}中取值；“局部”表示安全信息的列下标为集合{1,…,n}中的某一个给定值，“全局”则表示列下标将遍历地从{1,…,n}中取值；

步骤九：判断局部完全逃逸时间是否大于决断时间，如果是，则警戒目标中存在紧急目标，无人机根据应急避撞策略，进行应急机动；否则，进行航路重规划；

当全局完全逃逸时间不大于决断时间时，认为碰撞即将发生，无人机应采取应急机动；

应急机动包括:向上应急机动,左上应急机动和右上应急机动；

根据局部单向逃逸距离，采取对应于最大的全局单向逃逸距离的应急机动，并启动对应的应急机动，具体如下：

1)向上应急机动：在紧急转向段，无人机以最大过载拉升，有n_x＝sinγ，n_y＝0，n_z＝n_nmax；当航迹倾斜角达到最大上升角γ_max时，无人机进入平稳飞行段，跟踪χ＝χ₀，γ＝γ_max的定常直线飞行轨迹；

n_nmax为无人机最大法向过载；

2)左上应急机动：在紧急转向段，无人机以最大过载向左滚转并拉升，有n_x＝sinγ，n_y＝n_nmaxsinμ_max，n_z＝n_nmaxcosμ_max；当航迹倾斜角达到最大上升角γ_max时，无人机进入平稳飞行段，跟踪χ＝χ₀-Δχ_max，γ＝γ_max的定常直线飞行轨迹；

μ_max为允许的最大速度滚转角，Δχ_max为预设的最大航迹方位角改变量(Δχ_max＞0)；

3)右上应急机动：在紧急转向段，无人机以最大过载向右滚转并拉升，有n_x＝sinγ，n_y＝-n_nmaxsinμ_max，n_z＝n_nmaxcosμ_max；当航迹倾斜角达到最大上升角γ_max时，无人机进入平稳飞行段，跟踪χ＝χ₀+Δχ_max，γ＝γ_max的定常直线飞行轨迹；

χ₀为采取应急机动前的航迹方位角；

当仅存在单个紧急目标时，局部单向逃逸距离越大，则无人机在采取相应的应急机动方式后，与紧急目标的最近点距离越大，飞行的安全性越高，则该机动方式越有优势；反之，则该机动方式越有劣势，越不利于避免碰撞发生；

步骤十：当前采样时刻无人机完成自主避撞后，返回步骤二，重复进行下一采样时刻的无人机自主避撞决策。