一种实时机载自动防撞地告警及回避决策方法和系统与流程

1.本发明涉及智能飞行器技术领域，更具体的说是涉及一种实时机载自动防撞地告警及回避决策方法和系统。


背景技术：

2.自动防撞地系统(automatic ground collision avoidance system，agcas)，主要用于飞机，通过自动回避机制来防止发生可控飞行撞地事故，在飞行员任务饱和、迷失方向或者失去能力的情况下保护飞行员和飞机。agcas的工作原理是通过建立预测回避轨迹，与下方地形剖面进行比较，判断是否发生撞地危险，及时向飞行员告警并自动执行安全的回避机动。自动防撞地系统的关键在于回避机动的建立以及下方地形剖面的提取。传统回避轨迹采用标准的垂直上拉机动，先将机翼滚转至水平，在做极限负载因子下的上拉机动。然而，飞机在面临威胁地形时，极限上拉机动并不总是最有效的回避策略，例如面对平缓的威胁地形，较小的拉力即可满足回避需求，过大的拉力可能损耗飞机的性能。面对陡峭的威胁地形，即使采用极限拉力也可能无法实现前方地形的安全回避，在此情况下，左右侧向回避可能更有效。此外，地形剖面的提取依赖于飞机位置，对于每一种回避轨迹，由于导航定位误差、传感器误差以及地形数据误差等，实际预测位置可能存在一定的不确定性，这将严重影响回避策略的评估与判断。如何评估每一种回避策略的性能(包括与地形碰撞的概率、回避的安全程度以及飞机的实际性能等)，并给出回避策略的优先级顺序，将极大的提高载体防撞地性能，这也将有利于自动防撞地系统的推广和应用。
3.因此，如何提供一种实时机载自动防撞地告警及回避决策方法和系统是本领域技术人员亟需解决的问题。


技术实现要素：

4.有鉴于此，本发明提供了一种实时机载自动防撞地告警及回避决策方法和系统，以解决目前自动防撞地系统采用极限回避机动、圆形外接包络以及“最后一人站立”回避策略，使得载体安全回避威胁地形的能力受限问题。
5.为了实现上述目的，本发明采用如下技术方案：
6.一种实时机载自动防撞地告警及回避决策方法，包括以下步骤：
7.s1.根据载体的实时位置和姿态信息，通过飞机运动学模型，获取预测回避轨迹；
8.s2.根据所述预测回避轨迹类别构建地形扫描区域形状，通过载体的位置误差信息以及预设不确定轨迹增长角计算各采样轨迹点不确定宽度，通过相邻两帧不确定宽度构造外接矩形包络，提取包络内最大地形高程，形成预测回避轨迹下方地形剖面；
9.s3.根据扫描区域中威胁地形情况、飞机当前位置到威胁地形的时间以及飞行预测参数，构建用于评估所述预测回避轨迹风险的风险等级，对所述预测回避轨迹进行优先级排序；
10.s4.根据所述预测回避轨迹与地形剖面的撞地情况来判断是否产生告警信号，若
产生告警，则执行优先级最高的所述预测回避轨迹；否则，不执行任何所述预测回避轨迹。
11.优选的，所述预测回避轨迹包括：水平左转ll、左转爬升cl、前拉爬升fc、右转爬升cr和水平右转lr；通过三自由度近似法得到简化的状态运动方程为：
[0012][0013][0014][0015][0016][0017]
其中，x为载体即时的水平位置，y为侧向位置，z为垂向位置，为水平速度，为侧向速度和垂向速度，v为地速，χ为航向角，γ为俯仰角，φ为滚转角，nz为负载因子；
[0018]
通过负载因子nz和滚转角φ控制回避轨迹类别，根据预测时间和所述状态运动学模型，通过状态递推获得所述预测回避轨迹。
[0019]
优选的，s2的具体内容包括：
[0020]
1)根据所述预测回避轨迹类别选择地形扫描区域形状，其中，在直线水平飞行情况下，地形扫描区域为梯形；在转弯飞行情况下，地形扫描区域为管道状；
[0021]
2)根据导航系统输出的所述位置误差信息以及所述预设不确定轨迹增长角，计算各采样轨迹点的不确定宽度unwidth；
[0022]
unwidth＝σ
xy
d
tpa
*sin(α)
[0023]
式中，σ
xy
为导航解在水平方向的误差值；d
tpa
为当前轨迹点至初始位置的距离，为各轨迹点间的累积值；α为预测回避轨迹的不确定度增长角；
[0024]
3)根据相邻两帧的不确定宽度构造外接矩形包络，提取包络内最大地形高程，形成预测回避轨迹下方地形剖面；
[0025]
预测回避轨迹下方地形剖面除了所提取的地形高程外，根据导航系统提供的垂向定位误差，添加地形缓冲高度，形成最终用于碰撞检测的地形剖面。
[0026]
优选的，s3的具体内容包括：
[0027]
1)根据扫描区域中威胁地形情况、飞机当前位置到威胁地形的时间以及飞行预测参数，构建用于评估所述预测回避轨迹风险情况的发生度o、危险度s和改出度d，其中：
[0028]
发生度o：根据所述预测回避轨迹扫描区域中超出当前预测飞行高度的地形高程数nj与区域内总地形数之比，衡量回避轨迹发生撞地事故的风险程度；
[0029]
危险度s：根据所述预测回避轨迹与威胁地形发生碰撞时的预测时间与完成预测回避轨迹的总时间之比，作为衡量所述预测回避轨迹发生撞地的危险程度s；
[0030]
改出度d：根据当前位置的不确定度，构成切出不确定区域，记录切出不确定区域无威胁地形时所对应的轨迹点的航向角或俯仰角，计算对应角度变化量与当前轨迹整体角度变化量的比值衡量所述预测回避轨迹安全改出程度；
[0031]
2)以改出度o、危险度s和改出度d为自变量x，计算不同负载因子下各所述预测回避轨迹的效用值v、后悔值r和感知效用值u；
[0032]
3)根据各所述预测回避轨迹的感知效用值u以及飞机性能参数，计算所述预测回避轨迹的相对重要性q；
[0033]
4)根据所述相对重要性q计算风险等级，对所述预测回避轨迹进行优先级排序；q值越小对应风险等级越低，对应的所述预测回避轨迹的优先级越高；反之，q值越大对应风险等级越高，对应的所述预测回避轨迹的优先级越低。
[0034]
优选的，s4中根据所述预测回避轨迹与地形剖面的撞地情况来判断是否产生告警信号的具体内容包括：
[0035]
所述预测回避轨迹与地形剖面的撞地情况由各轨迹点的预测飞行高度与添加了地形缓冲高度的剖面高程比较判断；若轨迹点的预测飞行高度小于剖面高程则该地形剖面高程为威胁地形，存在撞地风险；否则不存在撞地风险。
[0036]
一种实时机载自动防撞地告警及回避决策系统，包括：回避决策模块、通知模块和执行回避模块；
[0037]
所述回避决策模块，用于根据载体的实时位置和姿态信息，通过飞机运动学模型，获取预测回避轨迹；根据所述预测回避轨迹类别构建地形扫描区域形状，通过载体的位置误差信息以及预设不确定轨迹增长角计算各采样轨迹点不确定宽度，通过相邻两帧不确定宽度构造外接矩形包络，提取包络内最大地形高程，形成预测回避轨迹下方地形剖面；根据扫描区域中威胁地形情况、飞机当前位置到威胁地形的时间以及飞行预测参数，构建用于评估所述预测回避轨迹风险的风险等级，对所述预测回避轨迹进行优先级排序；
[0038]
所述通知模块，用于根据所述预测回避轨迹与地形剖面的撞地情况来判断是否产生告警信号；
[0039]
所述执行回避模块，用于根据是否产生所述告警信号来判断是否执行预测回避轨迹，若产生告警，则执行优先级最高的所述预测回避轨迹；否则，不执行任何所述预测回避轨迹。
[0040]
优选的，所述回避决策模块包括：飞机回避轨迹预测单元、威胁地形识别单元、碰撞检测单元和多轨迹决策及风险评估单元；
[0041]
飞机回避轨迹预测单元，用于载体飞行过程中根据实时位置和姿态信息进行飞行轨迹预测，输出参数提供至所述威胁地形识别单元、所述碰撞检测单元和所述多轨迹决策及风险评估单元；
[0042]
威胁地形识别单元，用于接受导航误差信息，以飞机回避轨迹预测单元提供的所述预测回避轨迹为中心，构造包含导航不确定度和轨迹不确定度的扫描区域，输出参数提供给碰撞检测单元和多轨迹决策及风险评估单元；
[0043]
碰撞检测单元，用于将所述预测回避轨迹与地形剖面比较，判断是否存在撞地风险，如果存在，则记录碰撞发生的预测撞地时间；
[0044]
多轨迹决策及风险评估单元，用于接受来自各模块的输出数据，计算风险评估参数及感知效用值，为飞机提供安全有效的回避决策。
[0045]
优选的，还包括输入模块，所述输入模块包括飞机状态单元和地形单元，所述飞机状态单元向所述回避决策模块提供实时飞机状态参数，所述地形单元向所述回避决策模块提供飞行周围地形态势，包括地形剖面。
[0046]
经由上述的技术方案可知，与现有技术相比，本发明公开提供了一种实时机载自
动防撞地告警及回避决策方法和系统，该方法在传统自动防撞地系统的基础上引入矩形外接包络对地形剖面进行提取，并根据扫描区域中威胁地形情况、飞机当前位置到威胁地形的时间以及飞行预测参数，构建评估回避轨迹的风险等级，对回避决策进行优先级排序，在存在威胁地形时，执行优先级最高的回避决策；本发明可以提高回避机动的安全性能，从而解决目前自动防撞地系统采用极限回避机动、圆形外接包络以及“最后一人站立”回避策略，使得载体安全回避威胁地形的能力受限问题。该系统将实时机载自动防撞地告警及回避决策系统通过编程进行实现，通过计算机处理器实现回避飞行轨迹预测、地形扫描区构建和回避轨迹优先级排序。
附图说明
[0047]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。
[0048]
图1为本发明实施例提供的一种实时机载自动防撞地告警及回避决策系统结构示意图；
[0049]
图2为本发明实施例提供的一种实时机载自动防撞地告警及回避决策方法的流程示意图；
[0050]
图3为本发明实时例提供的一种实时机载自动防撞地告警及回避决策方法中地形扫描区域示意图；
[0051]
图4为本发明实施例提供的一种实时机载自动防撞地告警及回避决策方法中的多轨迹决策及风险评估流程图；
[0052]
图5为本发明实施例提供的一种实时机载自动防撞地告警及回避决策方法中的风险评估参数计算示意图。
具体实施方式
[0053]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0054]
本发明实施例提供了一种实时机载自动防撞地告警及回避决策系统，如图1所示，主要包括输入模块1、回避决策模块2、通知模块3和执行回避模块4。其中输入模块1作为自动地形防撞系统的输入来源，主要包括能够提供飞机状态参数的飞机状态单元11和提供飞行周围地形态势的地形单元12；回避决策模块2作为自动防撞地告警系统的主体部分，主要包括飞行回避轨迹预测单元21、威胁地形识别单元22、碰撞检测单元23、多轨迹决策及风险评估单元24；通知模块3为飞行员提供并记录告警信息；执行回避模块4为自动地形防撞系统的执行模块，自动执行系统选择的回避轨迹，并在飞越威胁地形后将控制权重新交换给飞行员。
[0055]
飞机回避轨迹预测单元21，用于载体飞行过程中实时对5种回避决策进行飞行轨
迹预测，其输入参数由输入模块1中的飞机状态单元11提供，输出参数提供给威胁地形识别单元22、碰撞检测单元23和多轨迹决策及风险评估单元24。
[0056]
威胁地形识别单元22，接受来自输入模块1中的导航误差信息，以飞机回避轨迹预测单元21提供的预测轨迹为中心，构造包含导航不确定度和轨迹不确定度的扫描区域，并从输入模块1中的地形单元12提取预测轨迹下方地形剖面，输出参数提供给碰撞检测单元23和多轨迹决策及风险评估单元24。
[0057]
碰撞检测单元23，将提供的预测轨迹与地形剖面比较，判断是否存在撞地风险，如果存在，则记录碰撞发生的预测撞地时间。
[0058]
多轨迹决策及风险评估单元24，接受来自各模块的输出数据，计算风险评估参数及感知效用值，为飞机提供安全有效的回避决策。
[0059]
本发明实施例提供了一种实时机载自动防撞地告警及回避决策算法，如图2所示，包括以下步骤：
[0060]
s1：根据导航系统输出的位置、姿态信息，通过飞机运动学模型，对水平左转(level left,ll)，左转爬升(climbing left,cl)，前拉爬升(forward climb,fc)，右转爬升(climbing right,cr)和水平右转(level right,lr)这5种回避轨迹进行预测；
[0061]
具体的，利用输入模块1中的飞机状态模块11提供的载体即时的水平位置x、侧向位置y和垂向位置z、水平速度侧向速度和垂向速度地速v、航向角χ、俯仰角γ和滚转角φ。引入负载因子nz，用三自由度近似法得到简化的五状态运动方程为：
[0062][0063][0064][0065][0066][0067]
通过负载因子nz和滚转角φ控制回避轨迹类别，根据预测时间和简化的五状态运动学模型，通过状态递推获得5种预测回避轨迹。其中，状态递推获得预测回避轨迹，就是根据当前的速度及角速度乘以采样时间，获得一定预测时间内的位置和角度进而获取预测回避轨迹。
[0068]
s2：根据回避轨迹类别构建地形扫描区域形状，通过导航系统输出的位置误差信息以及预设不确定轨迹增长角计算各轨迹点不确定宽度，由相邻两帧不确定宽度构造外接矩形包络，提取包络内最大地形高程，形成预测轨迹下方地形剖面；
[0069]
具体的，轨迹下方地形剖面的提取包括以下步骤：
[0070]
步骤1)：根据回避轨迹类别选择地形扫描形状，在直线水平飞行情况下，地形扫描区域为梯形；在转弯飞行情况下，地形扫描区域为管道状；
[0071]
步骤2)：根据导航系统输出的位置误差信息以及预设不确定轨迹增长角，计算各采样轨迹点的不确定宽度unwidth；
[0072]
unwidth＝σ
xy
d
tpa
*sin(α)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(6)
[0073]
式中，σ
xy
为导航解在水平方向的误差值；d
tpa
为当前轨迹点至初始位置的距离，为各轨迹点间的累积值；α为预测轨迹的不确定度增长角；
[0074]
步骤3)：根据相邻两帧的不确定宽度构造外接矩形包络，提取包络内最大地形高程，形成预测轨迹下方地形剖面；
[0075]
预测轨迹下方地形剖面除了所提取的地形高程外，根据导航系统提供的垂向定位误差，添加地形缓冲高度，形成最终用于碰撞检测的地形剖面。
[0076]
为了更好的说明地形扫描区域的建立过程，给出如图3所示的地形扫描区域建立示意图。预测载体将进行左转回避决策，根据飞行回避轨迹预测21提供的预测轨迹221，以预测轨迹点ai为中心，由相邻两帧ai和a
i-1
的不确定宽度unwidth构造外接矩形包络222，同输入模块1中的地形数据12提取外接矩形包络222中的最高地形高程值作为轨迹点ai下方的地形剖面高程，对所有轨迹点执行上述操作，即可获得预测轨迹下方的地形剖面曲线。
[0077]
s3：根据扫描区域中威胁地形情况、飞机当前位置到威胁地形的时间以及飞行预测参数，构建评估回避轨迹的风险等级，对回避决策进行优先级排序；
[0078]
所述步骤3中，构建评估回避轨迹风险等级的步骤流程如图4所示，具体实现过程如下：
[0079]
步骤1)：根据扫描区域中威胁地形情况、飞机当前位置到威胁地形的时间以及飞行预测参数，构建用于评估回避轨迹风险情况的发生度o、危险度s和改出度d，如图5所示为风险评估参数计算示意图，具体为
[0080]
发生度o：根据预测轨迹扫描区域中超出当前预测飞行高度的地形高程数nj(h
t
＞h)与区域内总地形数之比，衡量回避轨迹发生撞地事故的风险程度,发生度越大代表预测轨迹发生撞地的风险越高；
[0081][0082]
其中，j代表回避决策序号；h
t
代表地形高度；h代表预测轨迹高度。
[0083]
危险度s：根据预测轨迹与威胁地形发生碰撞时的预测时间与完成预测回避轨迹的总时间之比，作为衡量回避轨迹发生撞地的危险程度s，危险度越大代表威胁地形距离飞机越近，撞地风险越高；
[0084][0085]
其中，j代表回避决策序号，c
time
代表预测轨迹与威胁地形发生碰撞时的预测时间，a
time
为轨迹预测总时间。
[0086]
改出度d：根据当前位置的不确定度，构成如图2所示的切出不确定区域223，记录切出不确定区域无威胁地形时所对应的轨迹点的航向角或俯仰角，计算对应角度变化量与该轨迹整体角度变化量的比值衡量回避轨迹安全改出程度，改出度越大，则代表撞地风险越小，对应的预测轨迹越安全。
[0087][0088]
其中，j代表回避决策序号；k代表预测时刻；first代表预测起始时刻；last代表预
测终止时刻；针对拉起爬升决策θ代表俯仰角，针对左右水平和左右爬升决策θ代表航向角。
[0089]
步骤2)：以改出度o、危险度s和改出度d为自变量x，计算不同负载因子下各回避决策的效用值v、后悔值r和感知效用值u，具体计算公式如下
[0090][0091]
其中，j表示回避决策序号；i表示当前负载因子的采样序列数；x*为理想决策所获得的效用值(理想情况下，我们希望回避决策的发生度o＝0，危险度s＝0，改出度d＝1)；v(
·
)为效用函数，是一个单调递增的凹函数，一般采用幂函数v(x)＝x
θ
作为效用函数，θ为决策的风险规避系数，反映决策时的风险态度(0＜θ＜1)，θ越小，风险规避程度越高；r(
·
)为后悔-欣喜函数r(x)＝1-e
(-δ
·
x)
，也是一个单调递增的凹函数，当时，表示选择决策j时的后悔值。
[0092]
步骤3)：根据各回避轨迹的感知效用值以及飞机性能参数，计算回避决策的相对重要性q，具体计算公式如下所示
[0093][0094]
其中，j代表回避决策序号；u代表感知效用值，下标o、s、d分别代表发生度、危险度和改出度。
[0095]
步骤4)：根据相对重要性q计算风险等级，对回避决策进行优先级排序。q值越小对应风险等级越低，回避决策的优先级越高；反之，q值越大对应风险等级越高，回避决策的优先级越低。为了表示方便，采用百分比定义各回避轨迹的风险等级，具体计算公式如下
[0096][0097]
其中，j为回避决策序号；qj为第j种回避决策的相对重要性；为所有回避决策相对重要性的最大值。
[0098]
s4：根据预测飞行轨迹与地形剖面的撞地情况来判断是否产生告警信号，若产生告警，则执行优先级最高的回避机动；否则，不执行回避机动。
[0099]
本发明的优点在于：对于现有的自动防撞地系统不需要增加额外的硬件开销，只需要对算法进行升级，引入多轨迹决策及风险评估模块，即可实现对预测回避轨迹的风险评估，并根据回避决策的优先级顺序为载体提供安全有效的回避建议，从而提高载体安全回避威胁地形的能力。
[0100]
本领域普通技术人员可以理解：实现上述方法实施例的全部或部分步骤可以通过程序指令相关的硬件来完成，前述的程序可以存储于一计算机可读取存储介质中，该程序在执行时，执行包括上述方法实施例的步骤；而前述的存储介质包括：移动存储设备、只读存储器(read-only memory，rom)、随机存取存储器(random access memory，ram)、磁碟或者光盘等各种可以存储程序代码的介质。
[0101]
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
[0102]
对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。