CN113375672B - 一种无人飞行器的高实时航迹避让方法及系统 - Google Patents

Abstract

本公开的无人飞行器的高实时航迹避让方法及系统，根据改进卡尔曼滤波算法预测所述无人飞行器的高实时航迹，得到所述无人飞行器的高实时预测航迹；根据所述无人飞行器的高实时预测航迹计算目标无人飞行器与入侵无人飞行器的空域间隔，根据所述空域间隔和所述目标无人飞行器与其它无人飞行器的安全间隔判断所述目标无人飞行器的高实时预测航迹是否存在冲突；当所述高实时预测航迹存在冲突时，利用改进的人工势场算法基于所述高实时预测航迹和所述高实时预测航迹的冲突存在结果进行高实时航迹避让规划。可以在超短时间内实现无人飞行器的自主预测其他飞行器的航迹并且判断是否存在威胁从而规划避让航迹，实现无人飞行器动态、实时自主避让。

Description

一种无人飞行器的高实时航迹避让方法及系统

技术领域

本公开属于无人飞行器感知与规避技术领域，特别是涉及到一种无人飞行器的高实时航迹避让方法及系统。

背景技术

低空空域内运行的无人飞行器具有机动能力强、受天气等因素影响大、活动空间范围和类型广泛等特点，融合运行安全问题严峻，其中如何避免无人飞行器间的空中碰撞冲突是一个感知规避领域的核心技术。

航迹预测目前较为成熟的是基于飞行器动力学模型的方法，按照飞行器动力学及运动学模型，利用各类机型的性能参数以及气象信息实现飞行器的飞行轨迹预测。航迹规划方法目前分为势场法、路径搜索法、几何法等。势场法是按照计划航路点对无人机吸引、空域内的其他飞行器或物体对无人飞行器排斥的规则构建人工势场从而实现航迹规划。

但是，现有技术中飞行器动力学模型方法需要依赖过多外部数据，然而低空空域并不能获得管制服务，且中小型无人飞行器由于成本及体积限制，通常并不搭载TCAS系统，因此需要无人飞行器能够独立进行实时预测的方法。无人飞行器路径规划避让问题方面，势场法容易陷入局部极小值，作为实时避让算法不够理想。此外，现有感知规避技术大多上是分模块的静态研究，未能将高实时规避作为目标，修正各个模块并进行系统性地研究，均不能解决无人飞行器动态、实时自主避让的问题。

发明内容

有鉴于此，本公开提出了一种无人飞行器的高实时航迹避让方法及系统，可以在超短时间内实现无人飞行器的自主预测其他飞行器的航迹并且判断是否存在威胁从而避让航迹规划，实现无人飞行器动态、实时自主避让。

根据本公开的一方面，提出了一种无人飞行器的高实时航迹避让方法，所述方法包括：

根据改进卡尔曼滤波算法预测所述无人飞行器的高实时航迹，得到所述无人飞行器的高实时预测航迹；

根据所述无人飞行器的高实时预测航迹计算目标无人飞行器与入侵无人飞行器的空域间隔，根据所述空域间隔和所述目标无人飞行器与其它无人飞行器的安全间隔判断所述目标无人飞行器的高实时预测航迹是否存在冲突；

当所述高实时预测航迹存在冲突时，利用改进的人工势场算法基于所述高实时预测航迹和所述高实时预测航迹的冲突存在结果进行高实时避让航迹规划。

在一种可能的实现方式中，所述根据改进卡尔曼滤波算法预测所述无人飞行器的高实时航迹，得到所述无人飞行器的高实时预测航迹，包括：

P1：在变维卡尔曼滤波算法中增加模型转换变量state，state＝statex*statey，其中，statex与statey分别表示X轴和Y轴的数据是否需要更换模型，0表示不更换模型，1表示更换模型；

P2：初始化变维卡尔曼滤波算法的开始状态为非机动模型；

P3：利用非机动模型对无人飞行器的X轴和Y轴的数据进行同时计算，当无人飞行器在X轴或Y轴上的运动状态发生改变，在非机动模型下对Y轴或X轴的数据进行计算，并记录X轴数据的迭代次数kx或Y轴数据的迭代次数ky，直到无人飞行器在Y轴或X轴上的运动状态发生改变，标记Y轴数据的迭代次数ky或X轴数据的迭代次数kx，将变维卡尔曼滤波算法的开始状态变为机动模型；

P4：利用机动模型对无人飞行器的X轴和Y轴的数据进行同时计算，当无人飞行器在X轴或Y轴上的运动状态发生改变，在机动模型下对Y轴或X轴的数据进行计算，并记录X轴数据的迭代次数kx′或Y轴数据的迭代次数ky′，直到无人飞行器在Y轴或X轴上的运动状态发生改变，标记Y轴数据的迭代次数ky′或X轴数据的迭代次数kx′，将变维卡尔曼滤波算法的开始状态变为非机动模型；

P5：重复步骤P3-P5，当X轴数据的迭代次数或Y轴数据的迭代次数满足变维卡尔曼滤波算法的总迭代次数时，得到无人飞行器在X轴或Y轴的高实时航迹预测状态，根据无人飞行器在X轴或Y轴的高实时航迹预测状态得到无人飞行器的高实时预测航迹。

在一种可能的实现方式中，所述根据所述空域间隔和所述目标无人飞行器与其它无人飞行器的安全间隔判断所述目标无人飞行器的高实时预测航迹是否存在冲突，包括：

当所述空域间隔小于等于所述目标无人飞行器与其它无人飞行器的安全间隔时，所述高实时预测航迹存在冲突。

在一种可能的实现方式中，所述当所述高实时预测航迹存在冲突时，利用改进的人工势场算法基于所述高实时预测航迹和所述高实时预测航迹的冲突存在结果进行高实时航迹避让规划，包括：

当所述高实时预测航迹存在冲突时，选取所述目标无人飞行器的高实时预测航迹的脱离点和再入点，由所述脱离点、再入点、高实时预测航迹组合成局部极小值障碍区域；

当检测到当目标无人飞行器陷入局部极小值区域时，利用目标无人飞行器虚拟目标策略打破目标无人飞行器的人工势场分布，使目标无人飞行器摆脱局部极小值区域实现高实时航迹避让规划。

在一种可能的实现方式中，所述检测到目标无人飞行器陷入局部极小值区域，包括：

根据目标无人飞行器的规划空间环境复杂度设置检测周期tcheck和规划步长L，根据检测周期tcheck和规划步长L设置检测半径Rcheck；

记录检测周期tcheck开始时刻和检测周期tcheLck时刻的目标无人飞行器的坐标(xcheck_start，yt_check_start)和(xcheck_end，yt_check_end)，计算目标无人飞行器在检测周期tcheck内坐标(xcheck_start，yt_check_start)和(xcheck_end，yt_check_end)之间的直线距离Dcheck；

比较所述直线距离Dcheck和检测半径Rcheck，当直线距离Dcheck大于检测半径时，判定目标无人飞行器没有陷入局部最小区域，否则目标无人飞行器陷入局部最小值区域。

在一种可能的实现方式中，所述虚拟目标无人飞行器位于目标无人飞行器与当前障碍物之间的连线的垂直线上。

根据本公开的另一方面，提出了一种无人飞行器的高实时航迹避让系统，所述系统包括：

高实时预测航迹模块，根据改进卡尔曼滤波算法预测所述无人飞行器的高实时航迹，得到所述无人飞行器的高实时预测航迹；

高实时预测航迹冲突判断模块，根据所述无人飞行器的高实时预测航迹计算目标无人飞行器与入侵无人飞行器的空域间隔，根据所述空域间隔和所述目标无人飞行器与其它无人飞行器的安全间隔判断所述目标无人飞行器的高实时预测航迹是否存在冲突；

高实时航迹避让规划模块，当所述高实时预测航迹存在冲突时，利用改进的人工势场算法基于所述高实时预测航迹和所述高实时预测航迹的冲突存在结果进行高实时航迹避让规划。

本公开的无人飞行器的高实时航迹避让方法，根据改进卡尔曼滤波算法预测所述无人飞行器的高实时航迹，得到所述无人飞行器的高实时预测航迹；根据所述无人飞行器的高实时预测航迹计算目标无人飞行器与入侵无人飞行器的空域间隔，根据所述空域间隔和所述目标无人飞行器与其它无人飞行器的安全间隔判断所述目标无人飞行器的高实时预测航迹是否存在冲突；当所述高实时预测航迹存在冲突时，利用改进的人工势场算法基于所述高实时预测航迹和所述高实时预测航迹的冲突存在结果进行高实时避让航迹规划。可以在超短时间内实现无人飞行器的自主预测其他飞行器的航迹并且判断是否存在威胁从而进行避让航迹规划，实现无人飞行器动态、实时自主避让。

根据下面参考附图对示例性实施例的详细说明，本公开的其它特征及方面将变得清楚。

附图说明

包含在说明书中并且构成说明书的一部分的附图与说明书一起示出了本公开的示例性实施例、特征和方面，并且用于解释本公开的原理。

图1示出了根据本公开一实施例的无人飞行器的高实时航迹避让方法流程图；

图2示出了根据本公开一实施例的无人飞行器的高实时预测航迹规划的威胁区示意图；

图3示出了根据本公开一实施例的步骤S3的进一步限定方法流程图；

图4示出了根据本公开一实施例的无人飞行器的高实时航迹避让系统原理示意图。

具体实施方式

以下将参考附图详细说明本公开的各种示例性实施例、特征和方面。附图中相同的附图标记表示功能相同或相似的元件。尽管在附图中示出了实施例的各种方面，但是除非特别指出，不必按比例绘制附图。

在这里专用的词“示例性”意为“用作例子、实施例或说明性”。这里作为“示例性”所说明的任何实施例不必解释为优于或好于其它实施例。

另外，为了更好的说明本公开，在下文的具体实施方式中给出了众多的具体细节。本领域技术人员应当理解，没有某些具体细节，本公开同样可以实施。在一些实例中，对于本领域技术人员熟知的方法、手段、元件和电路未作详细描述，以便于凸显本公开的主旨。

图1示出了根据本公开一实例的无人飞行器的高实时航迹避让方法流程图。

如图1所示，该方法可以包括：

步骤S1：根据改进卡尔曼滤波算法预测所述无人飞行器的高实时航迹，得到无人飞行器的高实时预测航迹。

其中，改进卡尔曼滤波算法可以为在变维卡尔曼滤波方法中增加模型转换变量，以控制无人飞行器在机动模型和非机动模型之间转换，具体过程如下：

P1：在变维卡尔曼滤波算法中增加模型转换变量state，state＝statex*statey，其中，statex与statey分别表示X轴和Y轴的数据是否需要更换模型，0表示不更换模型，1表示更换模型。

P2：初始化变维卡尔曼滤波算法的开始状态为非机动模型，即statex＝statey＝0，state＝0。

P3：利用非机动模型对无人飞行器的X轴和Y轴的数据进行同时计算，当无人飞行器在X轴或Y轴上的运动状态发生改变，在非机动模型下对Y轴或X轴的数据进行计算，并记录X轴数据的迭代次数kx或Y轴数据的迭代次数ky，直到无人飞行器在Y轴或X轴上的运动状态发生改变，标记Y轴数据的迭代次数ky或X轴数据的迭代次数kx，将变维卡尔曼滤波算法的开始状态变为机动模型。

举例说明，当某X轴上的运动状态改变为例，即Ux(k)≥Th，Uy(k)＜Th时，statex＝1，statey＝0，state＝0。同时，仍以非机动模型继续对Y轴的数据进行计算，并记录X轴数据的迭代次数kx，用来标记此时X轴数据的计算进度。利用非机动模型对Y轴上的数据进行ky次迭代后，若Y轴上的运动状态发生改变，即Uy(k)≥Th，则statex＝statey＝1，state＝1。此时变换模型，即将非机动模型转换为机动模型。然后，利用机动模型对X轴和Y轴的数据进行同时计算，X轴上的预测运动状态为X^(kx+1|kx)，kx＝k-ky，其中，k为此时变维卡尔曼滤波总迭代次数。

步骤S14：利用机动模型对无人飞行器的X轴和Y轴的数据进行同时计算，当无人飞行器在X轴或Y轴上的运动状态发生改变，在机动模型下对Y轴或X轴的数据进行计算，并记录X轴数据的迭代次数kx′或Y轴数据的迭代次数ky′，直到无人飞行器在Y轴或X轴上的运动状态发生改变，标记Y轴数据的迭代次数ky′或X轴数据的迭代次数kx′，将变维卡尔曼滤波算法的开始状态变为非机动模型。

例如，利用机动模型对X轴和Y轴的数据同时进行数次迭代之后，若当某一轴上的运动状态改变，仍以X轴为例，即δax(k)≤Ta，δay(k)＞Ta时，statex＝1，statey＝0，state＝0。同时，仍在机动模型继续对Y轴的数据进行计算，记录X轴数据的迭代次数kx′用来标记此时X轴数据的计算进度。当利用机动模型对Y轴上的数据进行ky′次迭代后，Y轴上数据的运动状态发生改变，即δay(k)≤Ta，则statex＝statey＝1，state＝1。此时变换模型，即将机动模型转换为非机动模型，然后利用非机动模型对X轴和Y轴的数据同时计算，X轴上的状态预测为X^(kx′+1|kx′)，kx′＝k-ky′。

P5：重复步骤P3-P5，当X轴数据的迭代次数或Y轴数据的迭代次数满足变维卡尔曼滤波算法的总迭代次数时，得到无人飞行器在X轴或Y轴的高实时航迹预测状态，根据无人飞行器在X轴或Y轴的高实时航迹预测状态得到无人飞行器的高实时预测航迹。

步骤S2：根据所述无人飞行器的高实时预测航迹计算目标无人飞行器与入侵无人飞行器的空域间隔，根据所述空域间隔和所述目标无人飞行器与其它无人飞行器的安全间隔判断所述目标无人飞行器的高实时预测航迹是否存在冲突。

其中，当空域间隔小于等于所述目标无人飞行器与其它无人飞行器的安全间隔时，说明高实时预测航迹存在冲突。

例如，根据无人飞行器高实时预测航迹，实时进行我方飞行器(目标无人飞行器)与其它飞行器的间隔的计算，设定其它飞行器与我方飞行器的飞行安全间隔为lsafe。

图2示出了根据本公开一实例的无人飞行器的高实时预测航迹规划的威胁区示意图。

如图2所示，Pathoriginal为我方飞行器原始航迹，t0时刻我方飞行器探测到入侵飞行器(它方飞行器)，tend时刻结束探测(tend取决于探测器性能，一般需获得至少100组探测数据)，Pathintrude为根据入侵飞行器t0至tend时间段内探测数据进行长时间航迹预测得到的入侵飞行器预测航迹，从tend时刻开始，计算预测航迹各航点与我方飞行器原始航迹Pathoriginal的欧氏距离ρ，当ρ≤lsafe时，记录此刻航点的时间tp，将其作为威胁区域起始点，判断冲突存在。

步骤S3：当所述高实时预测航迹存在冲突时，利用改进的人工势场算法基于所述高实时预测航迹和所述高实时预测航迹的冲突存在结果进行高实时避让航迹规划。

图3示出了根据本公开一实例的步骤S3的进一步限定方法流程图。如图3所示，步骤S3具体可以包括：

步骤S31：当所述高实时预测航迹存在冲突时，选取所述目标无人飞行器的高实时预测航迹的脱离点和再入点，由所述脱离点、再入点、高实时预测航迹组合成障碍区域。

步骤S32：当检测到当目标无人飞行器陷入局部极小值区域时，利用目标无人飞行器虚拟目标策略打破目标无人飞行器的人工势场分布，使目标无人飞行器摆脱局部极小值区域实现高实时航迹避让规划。

其中，检测目标无人飞行器是否陷入局部极小值区域的具体过程为：

L1：根据目标无人飞行器的规划空间环境复杂度设置检测周期tcheck和规划步长L，根据检测周期tcheck和规划步长L设置检测半径Rcheck。检测半径Rcheck可以为运动飞行器在一个检测周期内的最小直线运动距离。

其中，若当前目标无人飞行器的规划空间环境中障碍物或威胁区域个数较少，规划航迹相对平滑，物体在固定时间内移动的直线距离较远，因此，检测周期tcheck可设置较低数值，即设置较短周期来对其进行检测；若障碍物或威胁区域个数较多，或部分区域分布比较密集时，因躲避障碍物的原因，其在固定时间内移动的直线距离较小，故设置较长周期来对其进行检测检测，即检测周期tcheck可设置为较高数值。

L2：记录检测周期tcheck开始时刻和检测周期tcheck时刻的目标无人飞行器的坐标(xcheck_start，yt_check_start)和(xcheck_end，yt_check_end)，根据几何学中两点之间的距离公式，计算目标无人飞行器在检测周期tcheck内坐标(xcheck_start，yt_check_start)和(xcheck_end，yt_check_end)之间的直线距离Dcheck。

步骤P3：比较直线距离Dcheck和检测半径Rcheck的大小，当直线距离Dcheck大于检测半径时，判定目标无人飞行器没有陷入局部最小区域，否则目标无人飞行器陷入局部最小值区域。

当检测到当前目标点(当前目标无人飞行器)陷入局部极小区域时，在当前目标点周围再次设置一个虚拟目标点(虚拟它方无人飞行器)，以打破当前目标点的受力平衡状态或打破局部徘徊区域的人工势场分布，使当前目标点摆脱局部极小值或局部徘徊情况。由于虚拟目标点位于实际目标点周围，目标无人飞行器规划航迹并不会为逃离局部极小点或局部徘徊区域而过度远离实际最优航迹。若通过在实际目标点周围设置虚拟目标点并没有使当前规划点摆脱局部极小值或局部徘徊情况，则在实际目标点与当前障碍物的连线垂直线上，选距离实际目标点较远点作为虚拟目标点，以摆脱局部极小值或局部徘徊情况。

然后，依据将目标无人飞行器安全间隔分为冲突告警区、避让区、危险接近区构建目标无人飞行器的约束模型，针对目标无人飞行器自身性能，可以构建例如最大偏航角/最小转弯半径约束、最大航程约束、飞行高度约束和最大爬升/俯冲角约束等模型，重新建立危险接近区，无人飞行器目标到达危险接近前感知其他飞行器信息并加以预测，进行避让机动，并最终恢复到原飞行轨迹并与其余飞行器重新保持安全间隔，实现目标无人飞行器高实时航迹规划和避让规划。

本公开的无人飞行器的高实时航迹避让方法，由于改进了卡尔曼滤波算法，采用了机动模型和非机动模型，提出各轴同步变维滤波算法，与原有二维算法相比，可以有效解决三维实时航迹预测的问题。还可以在超短时间内实现无人飞行器的自主预测其他飞行器的航迹并且判断是否存在威胁从而避让航迹规划，实现无人飞行器动态、实时自主避让。

根据本公开的另一方面，还提出了一种无人飞行器的高实时航迹避让系统，将航迹预测和避让策略整合成一个系统，将无人飞行器安全间隔分为冲突告警区、避让区、危险接近区，以解决无人飞行器动态、实时自主避让问题。

图4示出了根据本公开一实施例的无人飞行器的高实时航迹避让系统原理示意图。

如图4所示，该系统40包括高实时预测航迹模块41，高实时预测航迹冲突判断模块42和高实时航迹避让规划模块43。

其中，高实时预测航迹模块41，根据改进卡尔曼滤波算法预测所述无人飞行器的高实时航迹，得到所述无人飞行器的高实时预测航迹；

高实时预测航迹冲突判断模块42，根据所述无人飞行器的高实时预测航迹计算目标无人飞行器与入侵无人飞行器的空域间隔，根据所述空域间隔和所述目标无人飞行器与其它无人飞行器的安全间隔判断所述目标无人飞行器的高实时预测航迹是否存在冲突；

高实时航迹避让规划模块43，当所述高实时预测航迹存在冲突时，利用改进的人工势场算法基于所述高实时预测航迹和所述高实时预测航迹的冲突存在结果进行高实时航迹避让规划。

本公开的无人飞行器的高实时航迹避让系统，将航迹预测和避让策略整合成一个系统，将无人飞行器安全间隔分为冲突告警区、避让区、危险接近区，无人飞行器目标到达危险接近前感知其他飞行器信息并加以预测，进行避让机动，并最终恢复到原飞行轨迹并与其余飞行器重新保持安全间隔，解决无人飞行器动态、实时自主避让问题。

以上已经描述了本公开的各实施例，上述说明是示例性的，并非穷尽性的，并且也不限于所披露的各实施例。在不偏离所说明的各实施例的范围和精神的情况下，对于本技术领域的普通技术人员来说许多修改和变更都是显而易见的。本文中所用术语的选择，旨在最好地解释各实施例的原理、实际应用或对市场中的技术改进，或者使本技术领域的其它普通技术人员能理解本文披露的各实施例。

Claims (6)

1.一种无人飞行器的高实时航迹避让方法，其特征在于，所述方法包括：

根据改进卡尔曼滤波算法预测所述无人飞行器的高实时航迹，得到所述无人飞行器的高实时预测航迹；

根据所述无人飞行器的高实时预测航迹计算目标无人飞行器与入侵无人飞行器的空域间隔，根据所述空域间隔和所述目标无人飞行器与其它无人飞行器的安全间隔判断所述目标无人飞行器的高实时预测航迹是否存在冲突；

当所述高实时预测航迹存在冲突时，利用改进的人工势场算法基于所述高实时预测航迹和所述高实时预测航迹的冲突存在结果进行高实时避让航迹规划；

所述根据改进卡尔曼滤波算法预测所述无人飞行器的高实时航迹，得到所述无人飞行器的高实时预测航迹，包括：

P1：在变维卡尔曼滤波算法中增加模型转换变量state，state＝statex*statey，其中，statex与statey分别表示X轴和Y轴的数据是否需要更换模型，0表示不更换模型，1表示更换模型；

P2：初始化变维卡尔曼滤波算法的开始状态为非机动模型；

P3：利用非机动模型对无人飞行器的X轴和Y轴的数据进行同时计算，当无人飞行器在X轴或Y轴上的运动状态发生改变，在非机动模型下对Y轴或X轴的数据进行计算，并记录X轴数据的迭代次数kx或Y轴数据的迭代次数ky，直到无人飞行器在Y轴或X轴上的运动状态发生改变，标记Y轴数据的迭代次数ky或X轴数据的迭代次数kx，将变维卡尔曼滤波算法的开始状态变为机动模型；

P4：利用机动模型对无人飞行器的X轴和Y轴的数据进行同时计算，当无人飞行器在X轴或Y轴上的运动状态发生改变，在机动模型下对Y轴或X轴的数据进行计算，并记录X轴数据的迭代次数kx′或Y轴数据的迭代次数ky′，直到无人飞行器在Y轴或X轴上的运动状态发生改变，标记Y轴数据的迭代次数ky′或X轴数据的迭代次数kx′，将变维卡尔曼滤波算法的开始状态变为非机动模型；

P5：重复步骤P3-P5，当X轴数据的迭代次数或Y轴数据的迭代次数满足变维卡尔曼滤波算法的总迭代次数时，得到无人飞行器在X轴或Y轴的高实时航迹预测状态，根据无人飞行器在X轴或Y轴的高实时航迹预测状态得到无人飞行器的高实时预测航迹。

2.根据权利要求1所述的高实时航迹避让方法，其特征在于，所述根据所述空域间隔和所述目标无人飞行器与其它无人飞行器的安全间隔判断所述目标无人飞行器的高实时预测航迹是否存在冲突，包括：

当所述空域间隔小于等于所述目标无人飞行器与其它无人飞行器的安全间隔时，所述高实时预测航迹存在冲突。

3.根据权利要求1所述的高实时航迹避让方法，其特征在于，所述当所述高实时预测航迹存在冲突时，利用改进的人工势场算法基于所述高实时预测航迹和所述高实时预测航迹的冲突存在结果进行高实时避让航迹规划，包括：

当所述高实时预测航迹存在冲突时，选取所述目标无人飞行器的高实时预测航迹的脱离点和再入点，由所述脱离点、再入点、高实时预测航迹组合成局部极小值区域；

当检测到当前目标无人飞行器陷入局部极小值区域时，利用无人飞行器虚拟目标策略打破目标无人飞行器的人工势场分布，使目标无人飞行器摆脱局部极小值区域实现高实时航迹避让规划。

4.根据权利要求3所述的高实时航迹避让方法，其特征在于，所述检测到目标无人飞行器陷入局部极小值区域，包括：

根据目标无人飞行器的规划空间环境复杂度设置检测周期tcheck和规划步长L，根据检测周期tcheck和规划步长L设置检测半径Rcheck；

记录检测周期tcheck开始时刻和检测周期tcheck时刻的目标无人飞行器的坐标(xcheck_start,yt_check_start)和(xcheck_end,yt_check_end),计算目标无人飞行器在检测周期tcheck内坐标(xcheck_start,yt_check_start)和(xcheck_end,yt_check_end)之间的直线距离Dcheck；

比较所述直线距离Dcheck和检测半径Rcheck，当直线距离Dcheck大于检测半径时，判定目标无人飞行器没有陷入局部最小区域，否则目标无人飞行器陷入局部最小值区域。

5.根据权利要求3所述的高实时航迹避让方法，其特征在于，所述无人飞行器虚拟目标位于目标无人飞行器与当前障碍物之间的连线的垂直线上。

6.一种无人飞行器的高实时航迹避让系统，其特征在于，所述系统包括：

高实时预测航迹模块，根据改进卡尔曼滤波算法预测所述无人飞行器的高实时航迹，得到所述无人飞行器的高实时预测航迹；

高实时预测航迹冲突判断模块，根据所述无人飞行器的高实时预测航迹计算目标无人飞行器与入侵无人飞行器的空域间隔，根据所述空域间隔和所述目标无人飞行器与其它无人飞行器的安全间隔判断所述目标无人飞行器的高实时预测航迹是否存在冲突；

高实时航迹避让规划模块，当所述高实时预测航迹存在冲突时，利用改进的人工势场算法基于所述高实时预测航迹和所述高实时预测航迹的冲突存在结果进行高实时航迹避让规划；

所述根据改进卡尔曼滤波算法预测所述无人飞行器的高实时航迹，得到所述无人飞行器的高实时预测航迹，包括：

P1：在变维卡尔曼滤波算法中增加模型转换变量state，state＝stat ex*statey，其中，statex与statey分别表示X轴和Y轴的数据是否需要更换模型，0表示不更换模型，1表示更换模型；

P2：初始化变维卡尔曼滤波算法的开始状态为非机动模型；

P3：利用非机动模型对无人飞行器的X轴和Y轴的数据进行同时计算，当无人飞行器在X轴或Y轴上的运动状态发生改变，在非机动模型下对Y轴或X轴的数据进行计算，并记录X轴数据的迭代次数kx或Y轴数据的迭代次数ky，直到无人飞行器在Y轴或X轴上的运动状态发生改变，标记Y轴数据的迭代次数ky或X轴数据的迭代次数kx，将变维卡尔曼滤波算法的开始状态变为机动模型；

P4：利用机动模型对无人飞行器的X轴和Y轴的数据进行同时计算，当无人飞行器在X轴或Y轴上的运动状态发生改变，在机动模型下对Y轴或X轴的数据进行计算，并记录X轴数据的迭代次数kx′或Y轴数据的迭代次数ky′，直到无人飞行器在Y轴或X轴上的运动状态发生改变，标记Y轴数据的迭代次数ky′或X轴数据的迭代次数kx′，将变维卡尔曼滤波算法的开始状态变为非机动模型；

P5：重复步骤P3-P5，当X轴数据的迭代次数或Y轴数据的迭代次数满足变维卡尔曼滤波算法的总迭代次数时，得到无人飞行器在X轴或Y轴的高实时航迹预测状态，根据无人飞行器在X轴或Y轴的高实时航迹预测状态得到无人飞行器的高实时预测航迹。

Images