一种基于三维动态碰撞区的无人机自主避撞决策方法与流程
本发明涉及一种基于三维动态碰撞区的无人机自主避撞决策方法，属于无人机导航制导与控制技术领域。
背景技术：
无人机具有使用灵活、适应性强和性价比高等优点，在实际需求与技术发展的双重推动下，其应用范围不断拓展。无人机自主避撞决策是指无人机根据对飞行态势的认识和理解，决定是否重新规划航路以规避危险。随着无人机产业的迅猛发展，空域资源的稀缺性日益凸显。同时，由于自主规避防撞能力的缺失，目前国内外的无人机还难以与有人机等飞行器共享空域，无人机的未来发展受到严重限制。实现复杂环境下无人机自主避撞决策是使无人机达到与有人机相当的安全水平、融入共享空域的前提之一，具有重要的意义。
避撞决策通常以碰撞检测的结果为依据。基于来自各种机载传感器和通信链路的数据，碰撞检测通过对本机和环境的状态估计与空间遭遇建模，为避撞决策提供了当前态势下对碰撞冲突的评估信息。在目前的碰撞检测研究中，相对于传统的基于固定阈值的碰撞区，三维动态碰撞区由于综合考虑了无人机与入侵机的实时状态信息而能够更好地适应复杂环境的快速变化。虽然三维动态碰撞区在避撞决策领域有较大的应用潜力，但目前相关研究尚有较大的空白。由于缺乏对模型中包含的时间安全信息和距离安全信息的充分的挖掘和利用，目前尚未有研究提出基于三维动态碰撞区的避撞决策方法。
基于上述分析，无人机自主避撞决策方法应解决的技术问题如下：
(1)能够使无人机根据当前飞行态势灵活地切换机动方式，为其在复杂环境下的飞行安全提供保证；
(2)避撞决策所依据的飞行安全信息物理意义明确，形式简洁，具有合理性和有效性；
(3)决策方法具有较低的计算成本和较好的实时性，易于工程实现。
技术实现要素：
本发明为了解决上述问题，使无人机在避撞决策方面具有充分的灵活性和有效性，提出一种基于三维动态碰撞区的无人机自主避撞决策方法，充分利用环境中障碍物的威胁评估结果和三维动态碰撞区中包含的飞行安全信息，为无人机提供了从当前态势到机动方式的合理映射，使无人机在适当的时机采取适当的规避机动，实现了复杂环境下无人机的航路平滑性与飞行安全性之间的平衡。
所述基于三维动态碰撞区的无人机自主避撞决策方法，具体包括如下步骤：
步骤一：在当前无人机飞往目标的航路上，获取当前无人机所在空域的综合态势信息；
所述的综合态势信息具体包括：无人机的位置和地速向量，所探测到的各个障碍物的位置、形状、尺寸和地速向量等。
步骤二：针对当前采样时刻，判断无人机是否处于紧急转向段，如果是，则继续进行紧急转向，直至无人机进入平稳飞行段；否则，无人机继续平稳飞行段；
在紧急转向段，无人机通过施加最大的控制力，实现航迹倾斜角和/或航迹方位角的快速改变；当地速方向的改变量达到航迹倾斜角的最大上升角时，无人机结束紧急转向，进入到平稳飞行段。
步骤三：当无人机处于平稳飞行段时，计算当前采样时刻探测到的每个障碍物的威胁指数；
假设目标视线由无人机质心指向障碍物质心；第i个障碍物的总威胁指数ti由距离威胁指数tri、角度威胁指数tai和速度威胁指数tvi三部分组成。
各威胁指数的定义如下所示：
(1)距离威胁指数tri:
其中，ra为警戒距离，rd为危险距离。rli为无人机与第i个障碍物的相对距离。当rli大于ra时，障碍物的距离威胁指数为零；当rli介于ra与rd时，rli越小，距离威胁指数越大；rli小于rd时，无人机与障碍物的相对距离过近，距离威胁指数达到最大。
(2)角度威胁指数tai:
qr为无人机相对于障碍物的速度方向与目标方向之间的夹角，相对速度方向左偏时为正。当
(3)速度威胁指数tvi:
当障碍物为动态入侵机时：
式中，va为无人机的地速大小，vi为入侵机的地速大小。vi∈[0,0.6va)时入侵机速度明显小于无人机速度，此时相对速度主要由无人机决定，无人机通过调整自身速度较为容易地改变相对速度，避撞难度较低，入侵机的速度威胁指数较小；vi∈[0.6va,1.5va)时，vi接近甚至超过va，无人机通过调整自身速度而改变相对速度的能力显著减弱，避撞难度提升，入侵机的速度威胁指数随vi与va比值的增大而增大；vi∈(1.5va,+∞)时，入侵机的速度威胁指数达到最大。
当障碍物为静态障碍物时，威胁评估对象为静态障碍物表面上与无人机距离最近的一点，且vi＝0。
步骤四、将当前采样时刻下，各障碍物对应的威胁指数分别采用直接线性加权求和，得到各障碍物的总威胁指数；
第i个障碍物的总威胁指数计算如下：
ti＝ωrtri+ωatai+ωvtvi(4)
其中，ωr为距离威胁指数tri对应的权重，ωa为角度威胁指数tai对应的权重，ωv为速度威胁指数tvi对应的权重，ωr+ωa+ωv＝1，且ωr＞0，ωa＞0，ωv＞0。
步骤五、按总威胁指数从大到小的顺序对所有障碍物进行排序，判断是否有至少一个障碍物的总威胁指数大于警戒阈值，如果是，则说明该障碍物为威胁源，执行步骤六；否则，无人机继续按原航路飞行；
步骤六、判断各威胁源中是否存在警戒目标，如果是，计算无人机对于警戒目标的无机动碰撞区的边界条件；进入步骤七；否则，进行航路重规划；
警戒目标是指威胁源中的入侵机。
无机动碰撞区域的边界条件是指：当无人机和警戒目标最接近时恰好发生碰撞，即最近点的距离恰好等于最小安全距离；
具体计算过程如下：
步骤601、定义基本的坐标系和运动变量。
定义地面系ogxgygzg(sg)的原点og为地面上某一固定点，xg轴指向地平面的北向，yg轴指向地平面的西向，zg轴按右手定则铅垂向上；
步骤602、假设无人机a与入侵机b均匀速直线飞行，通过综合态势信息查找二者质心的位置向量及地速向量；
无人机a质心的位置向量为pa，入侵机b质心的位置向量为pb，无人机a的地速向量为va，入侵机b的地速向量为vb；
地速向量va在水平面ogxgyg的投影与xg轴之间的夹角为χa，地速向量vb在水平面ogxgyg的投影与xg轴之间的夹角为χb，根据右手定则，投影相对于xg轴向左旋转时夹角为正；地速向量va与水平面ogxgyg之间的夹角为γa，地速向量vb与水平面ogxgyg之间的夹角为γb，速度向量指向水平面上方时夹角为正；
无人机a与无人机b之间的相对距离为rl，相对速度为vr。
步骤603、以无人机a质心为原点，建立与无人机a质心固连的空间直角坐标系orxryrzr。
定义该坐标系的xr轴与地速向量va在水平面上的投影重合，zr轴在铅垂面内垂直于xr轴指向上方，yr轴垂直于orxrzr平面，其方向由右手定则确定；
地速向量vb的水平面投影与xr轴之间的夹角为ψb，相对距离rl的水平面投影与xr轴之间的夹角为ψl，当投影相对于xr轴向左旋转时为正；地速向量va与orxryr平面的夹角等同于地速向量va与水平面ogxgyg之间的夹角γa；地速向量vb与orxryr平面的夹角等同于地速向量vb与水平面ogxgyg之间的夹角γb；相对距离rl与orxryr平面的夹角为γl；当向量在平面上方时夹角为正。
建立空间直角坐标系orxryrzr的另一种方法为：将地面坐标系sg的原点平移到无人机质心位置后，绕z轴转过角χa。因此，成立以下变换关系：
其中，χl为相对距离rl在水平面的投影与xg轴的夹角。
步骤604、无人机a不改变当前地速，建立t时刻无人机与入侵机的相对距离rl(t)表达式；
t时刻无人机与入侵机的相对距离rl(t)为：
δx、δy、δz分别为相对距离rl沿xr轴、yr轴和zr轴的分量。
步骤605、根据相对距离rl(t)表达式计算无机动碰撞区的边界条件；
边界条件解析式为：
其中，
步骤七，判断警戒目标是否位于相应的无机动碰撞区边界条件内，如果是，进入步骤八，否则，进行航路重规划；
步骤八、在无机动碰撞区内预测无人机避开警戒目标可用的逃逸时间和逃逸距离。
首先，建立最大机动碰撞区和不可规避区；
(1)对最大机动碰撞区进行建模，建立以t和rl为未知量的二元方程组：如下
rl(t)≠0时，rl′(t)＝0等价于
ah为紧急转向期间，无人机水平方向和上的平均加速度；av为紧急转向期间，无人机铅垂方向上的平均加速度；且ah＞0表示平均加速度沿oryr轴正方向，av＞0表示平均加速度沿orzr轴正方向。
对应于不同的紧急转向方式，在上式中ah和av的取值分别为：
1)无人机以最大过载拉升时，ah＝0，av＝a2；
a2为无人机在水平方向上产生的最大平均加速度的大小；
2)无人机以最大过载向左滚转并拉升时，ah＝a1，av＝a2；
3)无人机以最大过载向右滚转并拉升时，ah＝-a1，av＝a2。
a1为无人机在铅垂方向上产生的最大平均加速度的大小。
采取迭代法进行求解，得到最接近时刻t与对应于当前ψl和γl的碰撞区边界值rl。遍历地取ψl∈[0,2π]，
若求解得到的rl＜0，则表示两机不会发生碰撞，此时的边界值取rl＝r0。
(2)对不可规避区进行建模，具体如下：
不可规避区分为两类：单向不可规避区和完全不可规避区。
单向不可规避区为无机动碰撞区与某一种紧急转向对应的最大机动碰撞区的交集；完全不可规避区为所有单向不可规避区的交集。采用三种紧急转向对应的最大机动碰撞区与无机动碰撞区的交集来表示完全不可规避区。
然后，从最大机动碰撞区和不可规避区建模过程中，提取碰撞区安全信息，并存储逃逸时间和逃逸距离；
首先，针对无人机-入侵机对，危险目标j和应急机动方式i的单向不可规避区定义为eij；应急机动方式的集合为{l,r,u}，包括向左滚转并拉升、向右滚转并拉升和拉升；
安全信息的定义基于危险目标，即位于相应无机动碰撞区内的警戒目标。包括：单向逃逸时间/距离，以及完全逃逸时间。
单向逃逸时间/距离：分为局部和全局两类。其中，局部单向逃逸时间/距离是指一个危险目标j到达单向不可规避区eij的剩余时间/距离，对应于某一特定的应急机动方式和无人机-入侵机对的安全信息，表示为msgij，i∈{l,r,u}，j∈{1,2,…,n}，n为无人机探测到的入侵机数量；全局单向逃逸时间/距离是指对应于同一种应急机动方式i，全局最小的局部单向逃逸时间/距离，表示为
完全逃逸时间：分为局部和全局两类。其中，局部完全逃逸时间是指一个危险目标j到达完全不可规避区ecom,j的剩余时间，表示为
“单向”表示安全信息的行下标为集合{l,r,u}中的某一个给定值，“完全”则表示行下标将遍历地从{l,r,u}中取值；“局部”表示安全信息的列下标为集合{1,…,n}中的某一个给定值，“全局”则表示列下标将遍历地从{1,…,n}中取值。
步骤九：判断局部完全逃逸时间是否大于决断时间，如果是，则警戒目标中存在紧急目标，无人机根据应急避撞策略，进行应急机动。否则，进行航路重规划；
决断时间根据无人机的机动能力、感知设备的测量误差，以及两机最小安全距离等因素给出。
当全局完全逃逸时间不大于决断时间时，认为碰撞即将发生，无人机应采取应急机动。
应急机动包括:向上应急机动，左上应急机动和右上应急机动；
根据局部单向逃逸距离，采取对应于最大的全局单向逃逸距离的应急机动，并启动对应的应急机动，具体如下：
1)向上应急机动：在紧急转向段，无人机以最大过载拉升，有nx＝sinγ，ny＝0，nz＝nnmax；当航迹倾斜角达到最大上升角γmax时，无人机进入平稳飞行段，跟踪χ＝χ0，γ＝γmax的定常直线飞行轨迹。
nnmax为无人机最大法向过载；
2)左上应急机动：在紧急转向段，无人机以最大过载向左滚转并拉升，有nx＝sinγ，ny＝nnmaxsinμmax，nz＝nnmaxcosμmax；当航迹倾斜角达到最大上升角γmax时，无人机进入平稳飞行段，跟踪χ＝χ0-δχmax，γ＝γmax的定常直线飞行轨迹。
μmax为允许的最大速度滚转角，δχmax为预设的最大航迹方位角改变量(δχmax＞0)。
3)右上应急机动：在紧急转向段，无人机以最大过载向右滚转并拉升，有nx＝sinγ，ny＝-nnmaxsinμmax，nz＝nnmaxcosμmax；当航迹倾斜角达到最大上升角γmax时，无人机进入平稳飞行段，跟踪χ＝χ0+δχmax，γ＝γmax的定常直线飞行轨迹。
χ0为采取应急机动前的航迹方位角；
当仅存在单个紧急目标时，局部单向逃逸距离越大，则无人机在采取相应的应急机动方式后，与紧急目标的最近点距离越大，飞行的安全性越高，则该机动方式越有优势；反之，则该机动方式越有劣势，越不利于避免碰撞发生。
步骤十：当前采样时刻无人机完成自主避撞后，返回步骤二，重复进行下一采样时刻的无人机自主避撞决策。
本发明的优点在于：
(1)本发明一种基于三维动态碰撞区的无人机自主避撞决策方法，能使无人机采取与碰撞冲突的紧急性相匹配的规避机动，从而在保证了无人机在复杂环境下飞行安全的同时尽可能地减小了其相对于原航路的偏离；
(2)本发明一种基于三维动态碰撞区的无人机自主避撞决策方法，充分提炼并利用了三维动态碰撞区中包含的飞行安全信息，该信息具有明确直观的物理意义，为避撞决策提供了合理的依据；
(3)本发明一种基于三维动态碰撞区的无人机自主避撞决策方法，具有较低的计算成本和较好的实时性，易于工程实现。
附图说明
图1是本发明基于三维动态碰撞区的无人机自主避撞决策方法的整体流程图；
图2是本发明基于三维动态碰撞区的无人机自主避撞决策方法中采集的无人机所在空域的综合态势示意图；
图3是本发明基于三维动态碰撞区的无人机自主避撞决策方法中无人机与入侵机相对运动的空间几何关系；
图4是本发明基于三维动态碰撞区的无人机自主避撞决策方法中无机动碰撞区示意图；
图5是本发明基于三维动态碰撞区的无人机自主避撞决策方法中最大机动碰撞区示意图；
图6是本发明基于三维动态碰撞区的无人机自主避撞决策方法中不可规避区示意图；
图7是本发明基于三维动态碰撞区的无人机自主避撞决策方法中局部单向逃逸距离的示意图；
图8是本发明基于三维动态碰撞区的无人机自主避撞决策方法中无人机与单入侵机发生紧急遭遇的示意图；其中，图8(a)表示入侵机在所有单向不可规避区外；图8(b)表示入侵机在某一单向不可规避区内；
图9是本发明基于三维动态碰撞区的无人机自主避撞决策方法中无人机与多入侵机紧急遭遇的示意图；
图10是本发明基于三维动态碰撞区的无人机自主避撞决策方法中无人机采取应急机动的示意图；
图11是本发明基于三维动态碰撞区的无人机自主避撞决策方法中无人机采取航路重规划的示意图；
图12是本发明实施例的无人机航路图；其中，图12(a)是三维航路；图12(b)是航路在水平面投影；图12(c)是无人机飞行高度；
图13是本发明实施例中无人机在紧急情况下的应急避撞图；其中，图13(a)表示应急避撞前；图13(b)表示应急避撞后；
图14是本发明实施例中无人机沿航迹系各轴过载图；其中，图14(a)表示沿航迹系x轴过载；图14(b)表示沿航迹系y轴过载；图14(c)表示沿航迹系z轴过载；
图15是本发明实施例中无人机与各动态障碍物距离图；
图16是本发明实施例中动态障碍物的最大威胁指数图。
具体实施方式
下面将结合附图和实施例对本发明作进一步的详细说明。
本发明提出一种基于三维动态碰撞区的无人机自主避撞决策方法，通过充分利用对环境中障碍物的威胁评估结果和三维动态碰撞区中包含的飞行安全信息，为无人机提供了从当前态势到机动方式的合理映射，使无人机在适当的时机采取适当的规避机动，实现了复杂环境下无人机的航路平滑性与飞行安全性之间的平衡。
一种基于三维动态碰撞区的无人机自主避撞决策方法，整体流程如图1所示，具体包括如下步骤：
步骤一：在当前无人机飞往目标的航路上，获取当前无人机所在空域的综合态势信息。
所述的综合态势信息如图2所示，具体包括：无人机的位置和地速向量，所探测到的各个障碍物的位置、形状、尺寸和地速向量等。
步骤二：针对当前采样时刻，结合此时的综合态势信息，判断无人机当前是否处于紧急转向段，若是，则继续进行紧急转向，直至无人机进入平稳飞行段；否则，无人机继续平稳飞行段。
本发明中主要考虑了无人机可能采取的两种规避动作：航路重规划和应急机动。航路重规划使无人根据动态环境的变化在向目标点移动的同时规避障碍，而应急机动则更为剧烈，通常被用于消解即将发生的碰撞冲突。
应急机动的设计需要综合考虑多方面因素。首先，从飞行安全的角度看，无人机在速度方向改变到一定程度后，保持当前速度即可避免碰撞，而不需要一直转向；其次，从无人机实际飞行性能约束的角度看，无人机不能无限制地进行紧急转向。例如，受到发动机最大剩余推力δtmax的限制，无人机进行定常直线上升运动时的最大上升角为γmax＝arcsin(δtmax/w),其中w为无人机重力。若无人机无限制地进行拉升而使得航迹倾斜角过大，则很可能引起失控等严重问题；最后，从航路规划的角度看，在保证无人机可以安全避开障碍的情况下，应该尽可能地减小采取应急机动所带来的负面影响，避免无人机偏离原始航路过远。
综合以上分析，本发明将应急机动分为两个阶段：紧急转向段以及平稳飞行段。在紧急转向段，无人机通过施加最大的控制力，实现航迹倾斜角和/或航迹方位角的快速改变；当地速方向的改变量达到一定值时，无人机结束紧急转向，进入到平稳飞行段。需要注意的是，考虑到所规划航路的可飞性和无人机的飞行安全，无人机在退出当前紧急转向段前不会执行新的动作。
步骤三：当无人机处于平稳飞行段时，计算当前采样时刻探测到的每个障碍物的威胁指数。
通过对障碍物威胁水平的量化和排序，威胁评估为避撞决策奠定了初步的基础。在低空复杂环境下，无人机可能同时遭遇多个障碍物。此时，为更好地保证无人机自身安全，无人机在获得空域的综合态势后，需在线进行威胁评估并形成对当前态势的危险性的量化认识，从而为碰撞冲突的消解提供依据。为实现对各个障碍物的威胁程度的衡量，本发明采用威胁指数法，基于分治法思想，通过提取引起碰撞冲突的关键因素，将威胁指数的求解问题分解为多个较小的子问题，最终综合考虑各个子问题的求解结果，得到各个障碍物的威胁指数并进行排序，从而为后续的避撞决策建立基础。
首先将入侵机作为威胁评估的对象，在碰撞冲突问题中，决定入侵机威胁程度的三个关键因素包括无人机与入侵机的相对距离rl、双方相对速度向量vr的方向以及大小。无人机在障碍环境中飞行的综合态势如图2所示，其中目标视线由无人机质心指向入侵机质心；qr为无人机相对于入侵机速度方向与目标视线方向夹角，相对速度左偏时为正。在该态势下，定义第i个入侵机的总威胁指数ti由距离威胁指数tri、角度威胁指数tai和速度威胁指数tvi三部分组成。
各威胁指数的定义如下所示：
(1)距离威胁指数tri:
其中，ra与rd分别为警戒距离与危险距离。rli为无人机与第i个入侵机的相对距离。当rli大于ra时，入侵机的距离威胁指数为零；rli介于ra与rd时，rli越小，距离威胁指数越大；rli小于rd时，无人机与入侵机的相对距离过近，距离威胁指数达到最大。
(2)角度威胁指数tai:
当
(3)速度威胁指数tvi:
当障碍物为动态入侵机时：
式中，va与vi分别为无人机与入侵机的地速大小。vi∈[0,0.6va)时入侵机速度明显小于无人机速度，此时相对速度主要由无人机决定，无人机可通过调整自身速度较为容易地改变相对速度，避撞难度较低，入侵机的速度威胁指数较小；vi∈[0.6va,1.5va)时，vi接近甚至超过va，无人机通过调整自身速度而改变相对速度的能力显著减弱，避撞难度提升，入侵机的速度威胁指数随vi与va比值的增大而增大；vi∈(1.5va,+∞)时，入侵机的速度威胁指数达到最大。
当障碍物为静态障碍物时，威胁评估对象为静态障碍物表面上与无人机距离最近的一点，且vi＝0。
步骤四、将当前采样时刻下，在根据上述方法得到各关键因素对应的威胁指数后，采用直接线性加权法，将各项威胁指数分别乘以权重后求和，得到各障碍物的总威胁指数；
第i个障碍物的总威胁指数计算如下：
ti＝ωrtri+ωatai+ωvtvi(4)
其中，ωr为距离威胁指数tri对应的权重，ωa为角度威胁指数tai对应的权重，ωv为速度威胁指数tvi对应的权重，ωr+ωa+ωv＝1，且ωr＞0，ωa＞0，ωv＞0。
步骤五、按照总威胁指数从大到小的顺序对所有障碍物进行排序，判断是否有至少一个障碍物的总威胁指数大于警戒阈值，如果是，则说明该障碍物为威胁源，执行步骤六；否则，无人机继续按原航路飞行；
对障碍物进行排序能迅速确定当前态势的危险程度和需要优先规避的目标，从而为无人机避撞决策的合理性和有效性提供保障。
步骤六、判断各威胁源中是否存在警戒目标，如果是，计算无人机对于警戒目标的无机动碰撞区的边界条件；进入步骤七；否则，进行航路重规划；
警戒目标是指威胁源中的入侵机。
无机动碰撞区用于判断当前态势下是否存在发生碰撞冲突的可能，其定义为无人机不采取规避机动时，必然导致碰撞发生的入侵机当前位置的集合。该区域的临界情况为：两机最接近时恰好发生碰撞，即最近点的距离恰好等于最小安全距离。防撞飞行中的最近点是指，对于两个相对运动的物体，如果他们的运动路径确定，则在整个运动过程中，必存在两点，当两个物体同时分别到达这两点时，它们的距离最近。
计算各个警戒目标的无机动碰撞区，具体过程如下：
步骤601、为建立无机动碰撞区，首先需要定义基本的坐标系和运动变量。
定义地面系ogxgygzg(sg)的原点og为地面上某一固定点，轴xg、yg分别指向地平面的北向和西向，轴zg按右手定则铅垂向上；
步骤602、假设无人机a与入侵机b均匀速直线飞行，通过综合态势信息查找二者质心的位置向量及地速向量；
无人机a质心的位置向量为pa，入侵机b质心的位置向量为pb，无人机a的地速向量为va，入侵机b的地速向量为vb；
地速向量va在水平面ogxgyg的投影与轴xg之间的夹角为χa，地速向量vb在水平面ogxgyg的投影与轴xg之间的夹角为χb，根据右手定则，投影相对于轴xg向左旋转时夹角为正；地速向量va与ogxgyg之间的夹角为γa，地速向量vb与ogxgyg之间的夹角为γb，速度向量指向水平面上方时夹角为正；
无人机a与无人机b之间的相对距离为rl，相对速度为vr。
步骤603、以无人机a质心为中心，最小安全距离r0为半径的圆球为禁入区，当入侵机位于禁入区内时，则视为碰撞已经发生。
为研究无人机与入侵机的相对运动关系，再次以无人机a质心为原点，建立与无人机a质心固连的空间直角坐标系orxryrzr，如图3所示。定义该坐标系的xr轴与地速向量va在水平面上的投影重合，zr轴在铅垂面内垂直于xr轴指向上方，yr轴垂直于orxrzr平面，其方向由右手定则确定；
地速向量vb的水平面投影与xr轴之间的夹角为ψb，相对距离rl的水平面投影与xr轴之间的夹角为ψl，当投影相对于xr轴向左旋转时为正；地速向量va与orxryr平面的夹角等同于地速向量va与水平面ogxgyg之间的夹角γa；地速向量vb与orxryr平面的夹角等同于地速向量vb与水平面ogxgyg之间的夹角γb；相对距离rl与orxryr平面的夹角为γl；当向量在平面上方时夹角为正。
建立空间直角坐标系orxryrzr的另一种方法为：将地面坐标系sg的原点平移到无人机质心位置后，绕轴z转过角χa。因此，成立以下变换关系：
其中，χl为相对距离rl在水平面的投影与xg轴的夹角。
步骤604、无人机a不改变当前地速，建立t时刻无人机与入侵机的相对距离rl(t)表达式；
无机动碰撞区的示意图如图4所示。可见，无机动碰撞区的边界线是与无人机的禁入区相切的、平行于当前相对速度vr的射线。假设我方无人机不改变当前地速，若入侵机位于无机动碰撞区外，则碰撞不会发生；否则，碰撞必然会发生。设开始计算碰撞区的时刻为初始时刻，并将其记为0，则初始时刻无人机与入侵机的相对距离为rl(0)，记为rl；已知va，vb，γa，ψb，γb，r0，忽略飞行时可能存在的侧滑角等影响，t时刻无人机与入侵机的相对距离rl(t)为：
δx、δy、δz分别为相对距离rl沿xr轴、yr轴和zr轴的分量。
步骤605、根据相对距离rl(t)表达式计算无机动碰撞区的边界条件；
无机动碰撞区的临界条件为：无人机与入侵机的最近点距离等于无人机的最小安全距离r0。无人机与入侵机的相对距离最近时，有rl′(t)＝0成立。由于rl(t)≠0时，又有rl′(t)＝0等价于
其中
对于式(8)中的h，需要分如下情况进行讨论：
(1)当h≥0时，令
根据临界条件rl(t)＝r0，得到临界相对距离：
(2)当h＜0时，
综上，遍历地取ψl∈[0,2π]，
步骤七，判断警戒目标是否位于相应的无机动碰撞区边界条件内，如果是，进入步骤八，否则，进行航路重规划；
步骤八：在无机动碰撞区内预测无人机避开警戒目标可用的逃逸时间和逃逸距离。
在定义逃逸时间和逃逸距离前，需要建立最大机动碰撞区和不可规避区。
最大机动碰撞区用于预测无人机进行相应机动后潜在的碰撞冲突情况；
不可规避区用于预测无论无人机是否机动或采取何种机动，都无法避免碰撞发生的入侵机当前位置的集合。
最大机动碰撞区用于预测无人机进行相应机动后潜在的碰撞冲突情况，不可规避区则用于预测无论无人机是否机动或采取何种机动，都无法避免碰撞发生的入侵机当前位置的集合。
首先，建立最大机动碰撞区和不可规避区；
(1)对最大机动碰撞区进行建模；
在三维空间，当无人机遭遇需要应急规避的威胁时，通常采用的机动包括水平方向转弯机动、垂直方向升降机动以及两者的结合。本发明中，最大机动碰撞区的计算基于无人机可能采取的规避机动方式，对无人机的向左滚转并拉升、向右滚转并拉升和拉升这三种紧急转向方式进行分析。
定义在紧急转向期间，无人机水平方向的平均加速度为ah，铅垂方向上的平均加速度为av，且ah＞0表示平均加速度沿oryr轴正方向，av＞0表示平均加速度沿orzr轴正方向。
假设无人机在水平可产生的最大平均加速度为a1，在铅垂方向上可产生的最大平均加速度a2。对应于不同的紧急转向方式，在上式中ah和av的取值分别为：
1)无人机以最大过载拉升时，ah＝0，av＝a2；
2)无人机以最大过载向左滚转并拉升时，ah＝a1，av＝a2；
3)无人机以最大过载向右滚转并拉升时，ah＝-a1，av＝a2。
最大机动碰撞区是指从当前时刻起，无人机以最大过载采取某种紧急转向方式，恰好不能避免碰撞发生的入侵机的当前位置集合。与无机动碰撞区相似，该区域的临界情况仍为：无人机与入侵机最接近时，二者的相对距离恰好等于无人机的最小安全距离r0。又根据rl(t)≠0时rl′(t)＝0与
rl(t)≠0时，rl′(t)＝0等价于
针对以上二元方程组，采取迭代法进行求解，得到最接近时刻t与对应于当前ψl和γl的碰撞区边界值rl。遍历地取ψl∈[0,2π]，
若求解得到的rl＜0，则表示两机不会发生碰撞，此时的边界值取rl＝r0。
以无人机在水平面中向左机动为例，最大机动碰撞区的示意图如图5所示。p0为初始时刻无人机禁入区边界上一点，经过δt时间后，p0到达p1点。p1′由p1沿初始时刻的入侵机速度vb方向平移vb·δt得到。若初始时刻入侵机位于p1′，经过δt时间后入侵机刚好到达p1点，即碰撞刚好发生。可见，p1′为最大机动碰撞区边界线上一点。同理，p2′也在该区域边界线上。以此类推，可得到边界线上各点，从而得到最大机动碰撞区。
(2)对不可规避区进行建模；
不可规避区可分为两类：单向不可规避区和完全不可规避区。
定义单向不可规避区为无机动碰撞区与某一种紧急转向对应的最大机动碰撞区的交集，完全不可规避区为所有单向不可规避区的交集。二者的区别在于，单向不可规避区仅对应于单个可采取的紧急转向方式，而完全不可规避区对应于所有可采取的紧急转向方式。显然，当入侵机位于完全不可规避区内时，无论无人机是否机动、采取何种可能的机动，都无法避免两机发生碰撞。采用三种紧急转向对应的最大机动碰撞区与无机动碰撞区的交集来表示完全不可规避区。
以二维平面情况为例，不可规避区的示意图如图6所示。图中向左与向右机动碰撞区和无机动碰撞区的交集分别构成了向左机动不可规避区与向右机动不可规避区，两个单向不可规避区的交集即为完全不可规避区。
然后，从最大机动碰撞区和不可规避区建模过程中，提取碰撞区安全信息，并存储逃逸时间和逃逸距离；
首先，针对无人机-入侵机对，危险目标j和应急机动方式i的单向不可规避区定义为eij；应急机动方式的集合为{l,r,u}，包括向左滚转并拉升、向右滚转并拉升和拉升；
安全信息的定义基于危险目标，即位于相应无机动碰撞区内的警戒目标。包括：单向逃逸时间/距离，以及完全逃逸时间。
单向逃逸时间/距离：分为局部和全局两类。其中，局部单向逃逸时间/距离是指一个危险目标j到达单向不可规避区eij的剩余时间/距离，按照表1的形式，对应于某一特定的应急机动方式和无人机-入侵机对的安全信息，表示为msgij，i∈{l,r,u}，j∈{1,2,…,n}，n为无人机探测到的入侵机数量；
表1碰撞区安全信息的存储形式
局部单向逃逸距离越大，则无人机在采取相应的应急机动方式后，与入侵机的最近点距离越大。对该命题的证明过程如下：如图7所示，假设禁入区的半径为r0，入侵机到单向不可规避区的局部单向逃逸距离为de。因为空间中的任意一点都可以表示为一个自由的初始相对速度向量和与其相对应的、根据不同的最小安全距离而建立的单向不可规避区的边界线的交点，所以存在δr，以r0+δr为最小安全距离，可建立相应的虚拟禁入区、虚拟无机动碰撞区和虚拟单向不可规避区，且该虚拟单向不可规避区的边界与初始相对速度的交点恰好为入侵机当前位置。根据等式(6)和(13)，入侵机与无人机的最近点距离为r0+δr，且δr与de成正相关。因此，de越大，入侵机与无人机的最近点距离越大。
当仅存在单个紧急目标时，局部单向逃逸距离可作为评价相应的应急机动方式优劣的指标:局部单向逃逸距离越大，则无人机在采取相应的应急机动方式后，与紧急目标的最近点距离越大，飞行的安全性越高，则该机动方式越有优势；反之，则该机动方式越有劣势，越不利于避免碰撞发生。
根据不可规避区的定义，当入侵机已位于完全不可规避区内时，视为已发生碰撞。当入侵机位于完全不可规避区外，即至少位于一个单向不可规避区外时，无人机才有可能避免碰撞发生。在此前提下，无人机与单入侵机发生紧急遭遇的示意图如图8所示。
在图8(a)中，入侵机位于所有单向不可规避区外。以入侵机位置为起点、沿初始时刻相对速度方向的射线先后与向左机动不可规避区和向右机动不可规避区相交。这表明入侵机沿当前方向飞行，将先到达向左机动不可规避区，再到达向右机动不可规避区。设局部单向逃逸距离分别为d1和d2(0＜d1＜d2)，则最大的局部单向逃逸距离dmax＝max(d1,d2)＝d2。此时，无人机无论是向左还是向右机动，均可避免碰撞，但前者的局部单向逃逸距离较小，因此向右机动更有利于保证无人机的安全。
在图8(b)中，入侵机位于完全不可规避区外，但已进入到向左机动不可规避区内。此时，有d1＜0＜d2，即二者的局部单向逃逸距离分别为负数和正数，仍成立dmax＝max(d1,d2)＝d2。无人机若向左机动，必然与入侵机发生碰撞，而向右机动尚能避免碰撞发生。因此，向右机动仍为此时的最优选择。
结合上述分析，可得到以下结论：对于应急规避单入侵机的情况，无人机应采取对应于最大的局部单向逃逸距离的应急机动方式。
全局单向逃逸时间/距离是指对应于同一种应急机动方式i，全局最小的局部单向逃逸时间/距离，表示为
假设当前场景中同时存在着多个紧急目标，与单机情况相比，应急规避多机策略设计的难点在于如何处理不同“最优”应急机动方式间的冲突。无人机紧急遭遇多入侵机的示意图如图9所示。按照单机的应急规避策略，无人机规避入侵机1、2的最优机动方式分别为向右机动和向左机动，而无人机不能同时采取两种不同的机动方式。为得到全局最优的机动方式，需进一步对多机情景进行分析。
在当前情况下，仅使用局部单向逃逸距离不能够充分地对机动方式的优劣进行评价。全局单向逃逸距离是局部单向逃逸距离在多紧急目标情景下的推广，该距离越小，则无人机与所有紧急目标的最近点距离的最小值越小，发生碰撞的危险性越大，采取该机动方式的安全性越低。因此，将全局单向逃逸距离作为多机情况下评价机动方式优劣的指标。当仅存在单个紧急目标时，全局单向逃逸距离即为单机的局部单向逃逸距离。
假设图9所示的情况中各个局部单向逃逸距离如表2所示。对应于无人机向左和向右机动的全局单向逃逸距离分别为90米与150米，则向右机动为此时的最优机动方式。
表2多机情况下的各个局部单向逃逸距离
综上所述，尽管紧急遭遇的入侵机数目不同，应急规避多入侵机或单入侵机的策略仍具有统一的形式。
最终，可得到应急避撞机动策略为：采取对应于最大的全局单向逃逸距离的应急机动。该策略既适用于单入侵机情景，也适用于多入侵机情景。
完全逃逸时间：分为局部和全局两类。其中，局部完全逃逸时间是指一个危险目标j到达完全不可规避区ecom,j的剩余时间，表示为
“单向”表示安全信息的行下标为集合{l,r,u}中的某一个给定值，“完全”则表示行下标将遍历地从{l,r,u}中取值；“局部”表示安全信息的列下标为集合{1,…,n}中的某一个给定值，“全局”则表示列下标将遍历地从{1,…,n}中取值。
无人机过早、过晚或者采取不适当的方式进行避撞机动，将会导致不必要的机动飞行或无法避开障碍。因此，利用所建立的三维动态碰撞区来帮助无人机选择避撞机动方式，以及引入时间阈值来决定避撞时机是非常重要的。此外，为避免不必要的计算开支，提高避撞决策方法的实时性，在计算碰撞区之前，首先对各架入侵机进行威胁评估。若评估的结果均小于警戒阈值，则说明此时无人机与各架入侵机发生碰撞的风险均较小，不必进行应急机动，因而计算碰撞区的步骤可以省去。
上述的每一个三维动态碰撞区都对应于一个特定的无人机-入侵机对。为对由碰撞区得到的安全信息进行区分，称对应于一个无人机-入侵机对的信息是局部的，而对应于所有无人机-入侵机对的信息是全局的。结合单向不可规避区与完全不可规避区的定义，可以看出，动态碰撞区中的安全信息可以二维数组的形式进行存储。假设当前无人机共探测到n架入侵机，则碰撞区安全信息的存储形式如表1所示。由于无人机需避免与任何一架入侵机发生碰撞，因此在考虑全局安全信息时应取最保守的值。
步骤九：判断局部完全逃逸时间是否大于决断时间，如果是，则警戒目标中存在紧急目标，无人机根据应急避撞策略，进行应急机动。否则，进行航路重规划；
紧急目标是指局部完全逃逸时间不大于决断时间的危险目标。决断时间根据无人机的机动能力、感知设备的测量误差，以及两机最小安全距离等因素给出。
当全局完全逃逸时间不大于决断时间时，认为碰撞即将发生，无人机应采取应急机动以快速改变自身的航迹速度方向，进而改变飞行轨迹，消解碰撞冲突。在设计应急避撞机动策略前，需要首先确定基本的应急机动方式。结合三自由度质点模型，假设无人机最大法向过载为nnmax，允许的最大速度滚转角为μmax，无人机绕速度向量向右滚转为正；理想情况下，无人机在机动时保持当前速度大小不变，在采取应急机动前的航迹方位角为χ0，预设的最大航迹方位角改变量为δχmax(δχmax＞0)。
应急机动包括:向上应急机动,左上应急机动和右上应急机动；
根据局部单向逃逸距离，采取对应于最大的全局单向逃逸距离的应急机动，并启动对应的应急机动，如图10所示，具体如下：
1)向上应急机动：在紧急转向段，无人机以最大过载拉升，有nx＝sinγ，ny＝0，nz＝nnmax；当航迹倾斜角达到最大上升角γmax时，无人机进入平稳飞行段，跟踪χ＝χ0，γ＝γmax的定常直线飞行轨迹。
2)左上应急机动：在紧急转向段，无人机以最大过载向左滚转并拉升，有nx＝sinγ，ny＝nnmaxsinμmax，nz＝nnmaxcosμmax；当航迹倾斜角达到最大上升角γmax时，无人机进入平稳飞行段，跟踪χ＝χ0-δχmax，γ＝γmax的定常直线飞行轨迹。
3)右上应急机动：在紧急转向段，无人机以最大过载向右滚转并拉升，有nx＝sinγ，ny＝-nnmaxsinμmax，nz＝nnmaxcosμmax；当航迹倾斜角达到最大上升角γmax时，无人机进入平稳飞行段，跟踪χ＝χ0+δχmax，γ＝γmax的定常直线飞行轨迹。
当仅存在单个紧急目标时，由于所建立的碰撞区模型同时包括了动态的距离安全信息和时间安全信息，因此可为应急避撞决策提供明确、有效的判断依据。以下将基于碰撞区模型，对无人机与紧急目标发生紧急遭遇的场景进行分析，从而设计应急避撞策略，增强无人机在极端情况下的应变能力，为无人机在共享空域的安全飞行提供进一步的保障。
局部单向逃逸距离可作为评价相应的应急机动方式优劣的指标:局部单向逃逸距离越大，则无人机在采取相应的应急机动方式后，与紧急目标的最近点距离越大，飞行的安全性越高，则该机动方式越有优势；反之，则该机动方式越有劣势，越不利于避免碰撞发生。
步骤十：当前采样时刻无人机完成自主避撞后，返回步骤二，重复进行下一采样时刻的无人机自主避撞决策。
所述的航路重规划如图11所示，是指无人机根据动态环境的变化，利用人工势场法加滚动时域优化法，在向目标点移动的同时规避障碍。
实施例
采用本发明方法的复杂环境下无人机自主避撞决策效果如图12-16。其中无人机的飞行轨迹为实线，动态障碍物分别沿相应的虚线方向进行匀速直线运动。无人机航路如图12所示，三维航路如图12(a)所示，航路在水平面投影如图12(b)所示，无人机的飞行高度如图12(c)所示，显然，采用所提出的避撞机动策略后，通过及时采取应急机动，无人机可躲避静态和动态障碍物并最终安全到达目标点。
无人机在紧急情况下的应急避撞如图13所示。在发生紧急遭遇前，如图13(a)所示，无人机通过航路重规划，在向目标点飞行的同时规避了静态和动态障碍物；当仿真进行到126s时，无人机与动态障碍物发生紧急遭遇，无人机根据应急避撞策略，采取左上应急机动。紧急转向过程持续了5s，在此期间无人机的航迹方位角由42.95°变化到92.72°，航迹倾斜角由-5.02°变化到26.03°；在紧急转向段结束后，如图13(b)所示，无人机与动态障碍物的碰撞冲突消解，继续采取航路重规划方式飞行，直至到达目标点。
无人机在飞行过程中沿航迹系的过载如图14所示。图14(a)表示沿航迹系x轴过载，图14(b)表示沿航迹系y轴过载，图14(c)表示沿航迹系z轴过载，可见，各轴过载均处于约束的范围内。此外，在126s到131s，由于采取左上应急机动，无人机沿y轴与z轴的过载均达到最大。
无人机与各动态障碍物距离如图15所示，最近点距离分别为328.5m、484.5m和247.3m，均大于最小安全距离150m。
飞行过程中各个时刻对无人机最具威胁的动态障碍物的威胁指数如图16所示，该指数高于警戒阈值的情况分别出现于49s至51s与123s至127s。在第一个时间段，虽然1号动态障碍物与无人机的相对距离较小，但此时尚未达到需要采取应急避撞机动的条件，无人机通过采取航路重规划策略即可避免碰撞发生，并将最大威胁指数降低到警戒阈值以下；在第二个时间段，由于紧急遭遇情况的发生，无人机需要采取应急避撞机动以保证飞行安全，并降低最大威胁指数。
- 还没有人留言评论。精彩留言会获得点赞！