CN112365743B - 航空器航迹定位数据偏移的修正方法及装置 - Google Patents

Abstract

本发明提供一种航空器航迹定位数据偏移的修正方法及装置，该方法包括在进行航空器当前航迹定位数据偏移的修正操作之前清空历史缓存信息；接收分布式定位***发送的航空器的实时测量点信息；根据实时测量点信息以及当前缓存信息,确定用于作为过滤罩的当前抛物线以及实时偏转角信息；根据实时测量点信息、当前抛物线以及实时偏转角信息,计算生成实时测量点对应的实时过滤点信息；根据实时测量点信息、实时过滤点信息、实时偏转角信息更新当前缓存信息；根据当前航迹的所有过滤点信息，得到航空器当前航迹的修正航迹。本发明以实现定位准确且稳定性高，轨迹更符合运动规律。

Description

航空器航迹定位数据偏移的修正方法及装置

技术领域

本发明涉及航空器定位技术领域，尤其涉及一种航空器航迹定位数据偏移的修正方法及装置。

背景技术

在机场路由引导的项目应用中，需要结合软件***对跑道边灯的控制，对航空器进行路由灯光引导。在引导的过程中，需要航空器按照软件计算出来的优化路径去行驶，点亮相关路径周围的道面灯光，航空器按照灯光指示行进。航空器的实时位置数据，由分布式定位***计算并发送给应用软件端，以此来绘制航空器的航迹信息。软件通过飞机已走过的航迹信息，实时控制熄灭已走过的历史航迹所在路径沿途的灯光，保留未行使路径沿途的灯光。当航空器临时因为某种原因，从岔口离开预设的优化路径时，原来的未行进完毕的优化路径所在的道面沿途灯光全部熄灭，并根据航空器实时位置重新计算优化路径，并点亮新路径所在道面沿途的灯光。

由于定位精度原因，定位数据会存在较大误差，所以在路有引导过程中，因为误差导致定位数据漂移，最主要会引起以下两个方面的问题：

在航空器静止处于滑行道起点时，软件端在接收到定位数据，会错误地发现航空器在向前运动，就会错误地控制前进方向后面的灯光熄灭；

在航空器引导行进到某个岔口临时改变方向偏离预设的优化路径时，软件端在接收到定位数据，会错误地以为航空器还在按预设的优化路径进行，继续保留未进行完毕的路径的灯光，直到航空器走出一定距离后才迟缓地发现已经偏离，并重新给出路由规划建议，熄灭先前路径的灯光，点亮重新规划路径的灯光。

总体来说，就是因为定位数据漂移的原因，会导致灯光控制和航空器运行状态之间没有同步一致，且路径实时重新规划的响应速度较航空器运行计划改变之间有很大延迟。

现有的解决定位数据漂移，进行航迹偏移修正的方法，主要是通过以下三个方面：

提高定位算法的定位精度。缺点：硬件方面，需要更换或添置数量更多、精度更高、性能更强的通讯设备、计算设备、存储设备，成本高昂；软件方面，定位算法的改进难度大，改进空间小，提升效果不明显。

通过道面固有的路径信息去做路径匹配。缺点：需要对道面进行详细的数据建模，传输或存储的数据，其类型和结构会变得复杂，数据吞吐量和计算开销会陡增；对于航空器因为意外原因进行到建模路网以外的区域时，无法进行正确的匹配和定位。

对测量出来带有误差的定位数据在软件算法层面上进行修正。这是目前兼顾开发成本、开发难度、资源消耗、使用和维护便捷等因素，所采用的主流解决方式。但是现有的修正方法，都是去分析统计历史轨迹的运动规律，包括但不限于位置、速度、加速度、分布规律等，通过预测或拟合的方法找到当前航空器的正确位置。这样设计存在以下缺点：

(1)一些修正方法修正后的路径连续光滑，能很好地反映行进趋势，但偏离实际测量数据较远，不能反映真实位置信息。

(2)一些修正方法修正后保持了较高的精度，能很好地反映真实位置信息，但路径连续性不好(如呈锯齿状)，不能与进行的道面吻合；

以上方法都不能很好地过滤掉“由于定位数据漂移导致的轨迹自相交，并形成软状分布，在团状分布中轨迹出现往复运动”的情况，这不符合大型航空器的物理运动规律。

发明内容

有鉴于此，本发明提供一种航空器航迹定位数据偏移的修正方法以及装置，以实现定位准确且稳定性高，轨迹更符合运动规律。

一方面，本发明提供一种航空器航迹定位数据偏移的修正方法，包括：

在进行航空器当前航迹定位数据偏移的修正操作之前清空历史缓存信息；

接收分布式定位***发送的航空器的实时测量点信息；

根据所述实时测量点信息以及当前缓存信息,确定用于作为过滤罩的当前抛物线以及实时偏转角信息；所述当前缓存信息包括：上一测量点缓存信息、上一过滤点缓存信息、上一偏转角缓存信息、以及包括当前航迹的初始过滤点至上一过滤点之间各过滤点信息的缓存队列；所述上一偏转角缓存信息为x轴正方向的单位向量沿着逆时针方向与从所述当前抛物线的焦点到所述当前抛物线的顶点连线形成向量之间的夹角；

根据所述实时测量点信息、当前抛物线以及实时偏转角信息,计算生成所述实时测量点对应的实时过滤点信息；

根据所述实时测量点信息、实时过滤点信息、实时偏转角信息更新当前缓存信息；

根据所述当前航迹的所有过滤点信息，得到航空器当前航迹的修正航迹。

进一步地，根据所述实时测量点信息以及当前缓存信息,确定实时偏转角信息包括：

确定上一过滤点缓存信息到实时测量点信息连接而成的线段与当前抛物线边界的公共点；

轴正方向的单位向量沿着逆时针方向与从所述当前抛物线的焦点到当前抛物线边界的公共点连线形成向量之间的夹角作为实时偏转角缓存信息。

进一步地，根据所述实时测量点信息以及当前缓存信息,确定用于作为过滤罩的当前抛物线包括：

将所述上一过滤点缓存信息为用于作为过滤罩的当前抛物线的焦点,并将所述上一测量点缓存信息与上一过滤点缓存信息的连线为当前抛物线的中线上的线段,以及根据所述上一测量点缓存信息与实时测量点信息之间的连线长度确定所述当前抛物线的大小。

进一步地，根据所述实时测量点信息、当前抛物线以及实时偏转角信息,计算生成所述实时测量点对应的实时过滤点信息的步骤包括:

计算上一偏转角信息和实时偏转角信息之间的夹角，再计算所述当前抛物线边界的公共点与所述当前抛物线的焦点之间的距离，如果公共点存在，则在所述当前抛物线的焦点与所述实时测量点信息形成的线段上确定实时过滤点信息,所述实时过滤点信息满足上一过滤点信息与所述公共点的距离等于所述实时过滤点信息与所述实时测量点之间的距离，若公共点不存在,实时过滤点信息即为上一过滤点信息。

进一步地，根据所述实时测量点信息、当前抛物线以及实时偏转角信息,计算生成所述实时测量点对应的实时过滤点信息的步骤之后还包括:

根据所述缓存队列修正实时过滤点信息，以根据所述修正后的实时过滤点信息更新当前缓存信息以及得到航空器当前航迹的修正航迹。

本发明还提供一种航空器航迹定位数据偏移的修正装置，包括：

预处理单元，用于在进行航空器当前航迹定位数据偏移的修正操作之前清空历史缓存信息；

接收单元，用于接收分布式定位***发送的航空器的实时测量点信息；

过滤单元，用于根据所述实时测量点信息以及当前缓存信息,确定用于作为过滤罩的当前抛物线以及实时偏转角信息；以及根据所述实时测量点信息、当前抛物线以及实时偏转角信息,计算生成所述实时测量点对应的实时过滤点信息；所述当前缓存信息包括：上一测量点缓存信息、上一过滤点缓存信息、上一偏转角缓存信息、以及包括当前航迹的初始过滤点至上一过滤点之间各过滤点信息的缓存队列；所述上一偏转角缓存信息为轴正方向的单位向量沿着逆时针方向与从所述当前抛物线的焦点到所述当前抛物线的顶点连线形成向量之间的夹角；

更新单元，用于根据所述实时测量点信息、实时过滤点信息、实时偏转角信息更新当前缓存信息；

航迹修正单元，用于根据所述当前航迹的所有过滤点信息，得到航空器当前航迹的修正航迹。

进一步地，所述过滤单元具体用于：确定上一过滤点缓存信息到实时测量点信息连接而成的线段与当前抛物线边界的公共点；轴正方向的单位向量沿着逆时针方向与从所述当前抛物线的焦点到当前抛物线边界的公共点连线形成向量之间的夹角作为实时偏转角缓存信息。

进一步地，所述过滤单元具体还用于：将所述上一过滤点缓存信息为用于作为过滤罩的当前抛物线的焦点,并将所述上一测量点缓存信息与上一过滤点缓存信息的连线为当前抛物线的中线上的线段,以及根据所述上一测量点缓存信息与实时测量点信息之间的连线长度确定所述当前抛物线的大小。

进一步地，所述过滤单元具体还用于：计算上一偏转角信息和实时偏转角信息之间的夹角，再计算所述当前抛物线边界的公共点与所述当前抛物线的焦点之间的距离，如果公共点存在，则在所述当前抛物线的焦点与所述实时测量点信息形成的线段上确定实时过滤点信息,所述实时过滤点信息满足上一过滤点信息与所述公共点的距离等于所述实时过滤点信息与所述实时测量点之间的距离，若公共点不存在,实时过滤点信息即为上一过滤点信息。

进一步地，所述的航空器航迹定位数据偏移的修正装置还包括:

过滤点信息修正单元用于：根据所述缓存队列修正实时过滤点信息，以使得所述更新单元根据所述修正后的实时过滤点信息更新当前缓存信息以及使得所述航迹修正单元根据所述修正后的实时过滤点信息得到航空器当前航迹的修正航机。

本发明航空器航迹定位数据偏移的修正方法及装置，位置都是被修正到附近小范围空间领域中，保证不过度偏离测量数据，形成连续光滑的行进轨迹，定位准确且稳定性高，轨迹更符合运动规律。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。

图1为根据本发明示例性第一实施例的航空器航迹定位数据偏移的修正方法的流程图。

图2为根据本发明示例性第二实施例的航空器航迹定位数据偏移的修正方法的框架概略图。

图3为根据本发明示例性第三实施例的航空器航迹定位数据偏移的修正方法的数据流框架图。

图4为根据本发明示例性第四实施例的航空器航迹定位数据偏移的修正方法的计算流程框架图。

图5为Shell结构中相关点之间的空间关系。

图6为一个简单过滤轨迹的生成过程示例。

图7为历史过滤点对最新过滤点的修正示意图。

图8为历史过滤点对最新过滤点的修正程序截图。

图9为长距离往返轨迹在路径上的收敛效果程序截图。

图10为静止状态下漂移的过滤效果程序截图。

图11为直行状态下漂移的过滤效果程序截图。

图12为静止起步状态下漂移的过滤效果程序截图。

图13为减速停止状态下漂移的过滤效果程序截图。

图14为转向状态下漂移的过滤效果程序截图。

图15为根据本发明示例性第五实施例的航空器航迹定位数据偏移的修正装置的结构图。

具体实施方式

下面结合附图对本发明实施例进行详细描述。

需说明的是，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合；并且，基于本公开中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本公开保护的范围。

需要说明的是，下文描述在所附权利要求书的范围内的实施例的各种方面。应显而易见，本文中所描述的方面可体现于广泛多种形式中，且本文中所描述的任何特定结构及/或功能仅为说明性的。基于本公开，所属领域的技术人员应了解，本文中所描述的一个方面可与任何其它方面独立地实施，且可以各种方式组合这些方面中的两者或两者以上。举例来说，可使用本文中所阐述的任何数目个方面来实施设备及/或实践方法。另外，可使用除了本文中所阐述的方面中的一或多者之外的其它结构及/或功能性实施此设备及/或实践此方法。

本发明航空器航迹定位数据偏移的修正方法的思路为：利用空间上的滞后性设计，去抵消因为前方即将出现的测量点的不可预测性。通俗地比喻，就是：现在有A和B两个人，在同一起点同时出发，沿着一条确定的小路从起点走到终点。A走的路径代表定位算法给出测量点绘制的轨迹，会“忽左、忽右、忽前、忽后”地对于确定的小路从起点到终点，A每走一步就会停下来，等着B跟进一步，相互交替，直到终点。B要走的就是修正算法给出的点绘制的轨迹，B会根据A的位置，向前迈出一步，保证自己走的路尽可能是沿着确定的小路，一直连续向前稳稳地走到终点，不会出现“忽左、忽右、忽前、忽后”的情况。现在B的做法是：

①A先走，B始终与A保持一步之差，刚开始时他们都位于同一出发点；

②B根据A的速度，始终与A保持恰当的距离，A速度越快B离A越远，反之B离A越近；

③如果A在B的正前方突然停下来了，或朝着B的方向出现倒退，B站在原地不动，直到A继续向正前方走，超出一定距离，B再跟上去；

④如果A突然改变方向，不再处于B的正前方，那么B会首先调整前进方向为正对A的方向，

再根据此时A和B的距离，决定需要跟进上去的距离或保持原地不动。

以下航空器航迹定位数据偏移的修正方法各实施例，就是将上述游戏思路，形式化将其形式化为数学方法，并算法化后加以实现。

如图1所示，一种航空器航迹定位数据偏移的修正方法包括：

步骤101，在进行航空器当前航迹定位数据偏移的修正操作之前清空历史缓存信息；

步骤102，接收分布式定位***发送的航空器的实时测量点信息；

步骤103，根据所述实时测量点信息以及当前缓存信息,确定用于作为过滤罩的当前抛物线以及实时偏转角信息；所述当前缓存信息包括：上一测量点缓存信息、上一过滤点缓存信息、上一偏转角缓存信息、以及包括当前航迹的初始过滤点至上一过滤点之间各过滤点信息的缓存队列；所述上一偏转角缓存信息为x轴正方向的单位向量沿着逆时针方向与从所述当前抛物线的焦点到所述当前抛物线的顶点连线形成向量之间的夹角；

步骤104，根据所述实时测量点信息、当前抛物线以及实时偏转角信息,计算生成所述实时测量点对应的实时过滤点信息；

步骤105，根据所述实时测量点信息、实时过滤点信息、实时偏转角信息更新当前缓存信息；

步骤106，根据所述当前航迹的所有过滤点信息，得到航空器当前航迹的修正航迹。

本实施例位置都是被修正到附近小范围空间领域中，保证不过度偏离测量数据，形成连续光滑的行进轨迹，定位准确且稳定性高，轨迹更符合运动规律。

具体地，如图2所示，航空器航迹定位数据偏移的修正方法概略框架图包括以下步骤：

①过滤算法模块包含初始化和计算两个功能，模块内部自带缓存机制；

②在使用过滤算法模块生成过滤轨迹之前，需要调用初始化功能清空缓存；

③过滤算法模块整体接收一个最新测量点作为输入；

④通过输入数据和缓存数据来调用计算功能，生成最新过滤点，并更新缓存数据；

⑤过滤算法模块整体返回一个最新测量点作为输出；

⑥返回步骤③，过滤算法模块接收下一个最新测量点，或者结束。

进一步地，如图3所示，航空器航迹定位数据偏移的修正方法数据流框架图(对应细化概略框架图)包括以下步骤：

①过滤算法模块包含两个函数：初始化函数和几何计算函数，包含四个缓存变量：最近测量点缓存、最近过滤点缓存、最近偏转角缓存、最近历史过滤点的缓存队列；

②在使用过滤算法模块生成过滤轨迹之前，需要且仅需要调用一次初始化函数来清空缓存，以清除生成上一条完整过滤轨迹的计算过程中留在缓存变量中的临时数据；

③过滤算法模块整体等待接收一个最新测量点作为输入，最新测量点的数据是来至分布式定位***的即时计算结果，并将该点添加到当前原始轨迹中；

④通过将最新测量点和所有缓存数据输入到几何计算函数，计算生成最新过滤点，分别使用最新测量点和最新过滤点来更新最近测量点缓存和最近过滤点缓存，使用最新偏转角来更新最近偏转角缓存，将最新过滤点添加到最近生成的历史过滤点的环形缓存队列中(若队列已满，覆盖最早的过滤点存储的位置)；

⑤过滤算法模块整体返回一个最新过滤点作为输出，并添加到当前过滤轨迹中；

⑥返回步骤③，过滤算法模块等待接收下一个最新测量点，或者结束。

为更详细的解释航空器航迹定位数据偏移的修正方法的实现思路，现描述基础变量和基本函数定义：

①P是一个轨迹点，用于指定运动目标在某个时刻在地球表面上的位置，P(x,y)是P数据格式表示形式，是由经纬度坐标转换成的局部大地参考系的二维直角坐标，x、y分别是直角坐标系下的横坐标和纵坐标。

②L是一条轨迹，表示为L＝{P₀,P₁,...,P_i-1,P_i}，P₀称为起始点，P_i称为最新点；F和S是数据格式为P的轨迹点；LF＝{F₀,F₁,...,F_i-1,F_i}称为当前原始轨迹，F_i称为最近测量点，F_i+1称为最新测量点；LS＝{S₀,S₁,...,S_i-1,S_i}称为当前过滤轨迹，S_i称为最近过滤点，S_i+1称为最新过滤点；Add是在轨迹中顺序添加新轨迹点的方法，调用方式LF.Add(F_i+1)或者LS.Add(S_i+1)。

③若P点与抛物线焦点连接而成的线段，与抛物线无公共点，则P位于抛物线内部；抛物线及其内部区域这里称为Shell(过滤罩)；抛物线焦点是某个过滤点S，称为Shell的焦点；抛物线的顶点Z称为Shell的顶点；抛物线的对称中线称为Shell的中线；抛物线本身称为Shell的边界；Shell边界上的一点称为C；x轴正方向的单位向量

沿着逆时针方向与向量

之间的夹角α称为Shell的偏转角。

④Shell(s,f,α)是过滤罩Shell数据格式表示形式；其中s表示Shell的焦点，f表示Shell中线段SZ的距离，α表示Shell的偏转角；Shell的位置、大小、方向分别是由s、f、α唯一确定的；f允许的最小值设为Min_f，允许的最大值设为Max_f。

⑤对于轨迹LS＝{S₀,S₁,...,S_i-1,S_i}，Shell_i是以最近过滤点S_i为焦点、点Z_i为顶点、F_iS_i为中线上的线段(如果F_i≠S_i)、用于计算最新过滤点S_i+1的Shell；x轴正方向的单位向量

沿着逆时针方向与向量

之间的夹角α_i称为最近偏转角。

⑥Shell_i的焦点S_i到最新测量点F_i+1连接而成的线段与Shell_i边界的公共点称为C_i(如果公共点存在)；焦点S_i与顶点Z_i形成的向量

与焦点S_i与点C_i形成的向量

之间的、小于平角的夹角称为θ_i(如果C_i存在)；x轴正方向的单位向量

沿着逆时针方向与向量

之间的夹角α_i+1称为最新偏转角(如果C_i存在)。

⑦fliter是过滤算法模块的实例；Init是初始化函数，在使用模块生成完整过滤轨迹之前仅调用一次，调用方式为：fliter.Init()；Generate是计算函数，每接收到一个测量点就要调用一次来生成一个过滤点，调用方式为：S_i+1＝fliter.Generate(F_i+1)。

⑧F^*是fliter中用于存储最近测量点F_i的缓存变量；S^*是fliter中用于存储最近过滤点S_i的缓存变量；α^*是fliter中用于存储最近偏转角α_i的缓存变量；RN是fliter中用于存储LS中最近生成的历史过滤点的环形缓存队列；当LS的长度小于N时有RN＝LS，否则RN＝{S_i-N+1,S_i-N+2,...,S_i-1,S_i}，其中N为队列最大允许的长度。

⑨AddElement是RN中添加元素的方法，调用方式RN.AddElement(S_i+1)，即把元素顺序S_i+1添加到S_i之后；Clear是RN中清空所有元素的方法，调用方式RN.Clear()；IsEmpty是RN中判断自己是否为空列表的方法，调用方式RN.IsEmpty()；Size是判断RN长度的方法，调用方式RN.Size()；中Get是RN中获得某个轨迹点的方法，调用方式

S_i＝RN.Get(RN.Size()-1)。

⑩GeoF是fliter中实现Generate的核心函数，称为几何计算函数，满足关系S_i+1＝GeoF(F_i+1,F^*,S^*,α^*,RN)，用于计算最新过滤点；Create是GeoF的内部函数，用于生成Shell，满足关系Shell_i＝Create(F_i+1,F^*,S^*,α^*)；Update是GeoF的内部函数，用于计算或更新最新偏转角，满足关系α_i+1＝UpdateS(α^*,S^*,F_i+1or S_i+1)；GetRaw是GeoF的内部函数，用于初步计算最新过滤点，满足关系S_i+1＝GetRaw(Shell_i,F_i+1,α_i+1)；Attach是GeoF的内部函数，用于修正最新过滤点，得到最新过滤点作为最终结果，满足关系S_i+1＝Attach(S_i+1,RN)。

Distance是工具函数，用于计算两点间的距离，调用方式为|P₁P₂|＝Distance(P₁,P₂)，其中P₁、P₂为任意两个轨迹点，|P₁P₂|为两点的距离值，距离的预设最小值为Min_d，距离的预设最大值为Max_d，预设值会在Create函数中用到；Atan是工具函数，用于计算x轴正方向的单位向量

沿着逆时针方向到向量

之间的夹角

调用方式为

Bettween是工具函数，计算最近偏转角α_i和最新偏转角α_i+1之间小于平角的夹角θ_i，满足关系θ_i＝Bettween(α_i,α_i+1)；Linear是工具函数，调用方式y＝Linear(X₁,X₂,x)，当X₁≤x≤X₂时，满足0≤y≤1，x和y呈线性关系，X₁＞x时y＝0，x＞X₂时y＝1；CosP是工具函数，调用方式l＝CosP(Shell_i.f,θ_i)，其中θ_i是Shell_i焦点S_i与顶点Z_i形成的向量

与焦点S_i与Shell_i边界上的点C_i形成的向量

之间的夹角，l是线段S_iC_i的距离值。

具体地，如图4所示，航空器航迹定位数据偏移的修正方法的计算流程框架图(主要将数据流框架图中的计算部分流程化、形式化)包括以下步骤：

S1：fliter是过滤算法模块的实例，调用初始化函数fliter.Init()以清空缓存变量F^*、S^*、α^*、RN，它们是和具体实例fliter相关的内部属性，设置接收点顺序号i＝-1。

设置全局接收点顺序索引号i＝-1。

S2：接收一个最新测量点F_i+1，并添加到原始轨迹LF的末尾：LF.Add(F_i+1)。

S3：该步骤为几何计算函数计算生成最新过滤点，并更新缓存数据的过程：

S_i+1＝GeoF(F_i+1,F^*,S^*,α^*,RN)，函数GeoF实现过程后面详述。

步骤S3中对函数GeoF的实现是具体实施的最主要内容，重点讨论子步骤S33(生成Shell_i相关)、S34(计算S_i+1相关)、S35(修正S_i+1相关)的详细实现过程。

S31：判断历史过滤点的环形缓存队列RN是否为空队列：bool＝RN.IsEmpty()；如果bool为真，转至步骤S32，否则转至步骤S33。

S32：此时LS中还没有过滤点生成，这里需要生成第一个过滤点，设置第一个过滤点就是第一个测量点：S_i+1＝F_i+1(即S₀＝F₀)；设置最新偏转角α_i+1＝-∞(即α₀＝-∞)，表示第一个Shell还没有生成。

函数GeoF实现过程如下：

①计算bool＝RN.IsEmpty()，判断bool是否为真，如果不是，转至步骤③；

②最新过滤点直接取值为最新测量点：S_i+1＝F_i+1，并初始化最新偏转角α_i+1＝-∞，转至步骤⑨；

③计算Shell_i＝Create(F_i+1,F^*,S^*,α^*)，用于创建初步计算S_i+1时，所依赖的过滤罩Shell_i；

④计算α_i+1＝Update(α^*,S^*,F_i+1)，用于计算最新偏转角α_i+1；

⑤计算S_i+1＝GetRaw(Shell_i,F_i+1,α_i+1)，用于初步计算最新过滤点S_i+1，其中利用了步骤③中所创建的过滤罩Shell_i；

⑥计算S_i+1＝Attach(S_i+1,RN)，用于修正步骤⑤中初步计算出的最新过滤点S_i+1的值；

⑦判断S^*≠S_i+1是否成立，如果不成立，转至步骤⑨；

⑧计算α_i+1＝Update(α^*,S^*,S_i+1)，用于调整步骤④中计算的最新偏转角α_i+1的值；

⑨更新缓存变量的值：F^*＝F_i+1、S^*＝S_i+1、α^*＝α_i+1，分别将本次计算获得的最新测量点、最新过滤点、最新偏转角缓存起来，成为下一次调用GeoF计算所依赖的最近测量点、最近过滤点、最近偏转角参数；

⑩将最新过滤点S_i+1添加到RN环形缓存队列中：RN.AddElement(S_i+1)；；

返回最新过滤点S_i+1。

S33：调用Shell_i＝Create(F_i+1,F^*,S^*,α^*)生成计算S_i+1所依赖的过滤罩Shell_i(S₀作为第一个过滤点，它是不需要通过过滤罩来计算推导的)，这里S^*是Shell_i的焦点(Shell_i.s＝S^*)，α^*是Shell_i的偏转角(Shell_i.α＝α^*)，线段值|F_i+1F^*|的大小决定Shell_i的大小(Shell_i.f)；调用α_i+1＝Update(α^*,S^*,F_i+1)来计算x轴正方向的单位向量

沿着逆时针方向到线段F_i+1S^*的夹角α_i+1，当线段F_i+1S^*不存在(即F_i+1＝S^*时)，α_i+1的值设置为α^*。

其中步骤S33中的Create函数的实现过程如下：

①创建Shell_i实例：Shell_i＝new Shell()，初始化设置Shell_i的焦点Shell_i.s＝S^*，初始化设置Shell_i的偏转角Shell_i.α＝α^*；

②计算最新测量点F_i+1到最近测量点F^*的距离：|F_i+1F^*|＝Distance(F_i+1,F^*)，计算该距离值在区间[Min_f,Max_f]中的相对比例系数：φ＝Linear(Min_d,Max_d,|F_i+1F^*|)；

③计算与步骤①中生成的Shell_i大小相关的f值：Shell_i.f＝Min_f+(Max_f-Min_f)*φ；

④返回计算最新过滤点S_i+1所依赖的过滤罩Shell_i。

其中工具函数Linear的实现如下，

①判断x≤X₁是否成立，如果成立，返回0；

②判断x≥X₂是否成立，如果成立，返回1；

③返回一个介于(0,1)之间的比例系数：(x-X₁)/(X₂-X₁)。

注：Shell_i.f的取值决定过滤罩的大小，它被设定在[Min_f,Max_f]范围之间，且与值|F_i+1F^*|的大小相关，也就是说原始轨迹中最新测量点F_i+1与原始轨迹的最近测量点F^*之间距离|F_i+1F^*|越大，漂移误差绝对值会认为越大，所需要的过滤罩Shell_i就设计得越大。

S34：调用S_i+1＝GetRaw(Shell_i,F_i+1,α_i+1)初步计算出最新过滤点S_i+1的值，这里首先计算Shell_i的偏转角α_i(α_i＝Shell_i.α)和α_i+1之间的夹角θ_i＝Bettween(α_i,α_i+1)，再计算Shell_i的焦点S_i与F_i+1构成的线段与Shell_i边界的公共点C_i与焦点之间的距离|S_iC_i|＝CosP_i(Shell_i,θ_i)，如果C_i存在，则在线段S_iF_i+1上找到点S_i+1使得S_iC_i|＝|S_i+1F_i+1|，否则(C_i不存在时F_i+1点在Shell_i的边界或内部)S_i+1就取值为S_i。

其中工具函数CosP的实现如下，它是一种关于参数(ρ_i,θ_i)抛物线的极坐标方程：

|S_iC_i|＝ρ_i＝CosP(Shell_i.f,θ_i)＝2*Shell_i.f/(1+cosθ_i)＝2*S_iZ_i|/(1+cosθ_i)。

注：当α_i＜0时出现在i＝0,α_i＝-∞的情况下，说明已经生成的Shell₀是还没有设置偏转角的，但是在步骤S33中已经生成的α₁是下一个过滤罩Shell₁的偏转角(α₁经过步骤S35后不会改变其值)，此时将Shell₀的偏转角α₀预设为α₁，即α₀与α₁夹角θ₀＝0，保持上述两个过滤罩不转向。当|S_iF_i+1|＜＝|S_iC_i|时，F_i+1位于过滤罩Shell_i内部或边界上，此时不管F_i+1是否与S_i重合，最新测量点S_i+1的值都被设定为与S_i重合。

GetRaw函数的过程说明如图6的示例所示，Shell_i是以线段S_i-1S_i所在的直线为对称中轴(如果S_i的生成过程没有被Attach函数修正，那么F_i可能在中轴上)，顶点为z_i，偏转角为

沿着逆时针到

的夹角α_i，θ_i为

沿着逆时针到

的夹角，F_i+1是Shell_i外部的一点，计算出|S_iF_i+1|、|S_iC_i|后初步在计算出线段S_iF_i+1上计算出S_i+1满足|S_iC_i|＝|S_{i+ 1}F_i+1|。

之所以会构造Shell这种几何形状作为过滤罩，是因为它相对于航空器的运动方向呈现对称性，并且便于计算。Shell_i的方向就是最近的运动目标的两个过滤点构成的向量

的方向，目标在运动方向两侧的误差分布理论上应该是对称的，但是前后方向不对称，即F_i+1相对于S_i的位置关系可以用向量

描述。

与

的夹角θ_i，以及最近测量点F_i和F_i+1的距离d_i＝|F_iF_i+1|，是对过滤罩Shell该如何设计极为重要的两个参数。θ_i说明了当前运动的目标在下一刻需要转向的程度(实时方向变化)，d_i说明了当前运动的目标在下一刻需要位移的程度(实时速度)。⑴：θ_i除了反映真实转向外，还反映误差导致的方向偏移，尤其在低速情况下，误差漂移会导致目标频繁改变方向甚至倒退，所以Shell的设计需要尽可能阻止因为误差导致的突然转向。因此原则就是θ_i值越大时越需要被抑制，这样过滤后的轨迹会更加连续光滑。而Shell的形状设计为抛物线区域时，当θ_i值越大，S_iF_i+1所在的直线与Shell边界的交点C_i到焦点S_i的距离就越大，F_i+1落在Shell_i内部的概率也就越高，就能有效地将因为误差导致剧烈转向的F_i+1有效地过滤掉。所以，θ_i这个指标决定了Shell_i的几何形状的设计。⑵：d_i除了反映真实速度外，还反映误差导致的位置偏移量，d_i值越大时因误差导致的位置偏移量会越大。假如所有的Shell_i设为固定小尺寸，仅仅跟随目标的运动改变偏转角，而没有随着实时速度改变尺寸。当目标速度越快时，d_i＝|F_iF_i+1|值越大，由于目标移动具有的惯性，导致速度不会立马减小，所以后续测量的d_i+1＝|F_i+1F_i+2|值也会较大。因为Shell_i.f值较小，所以经过GetRaw函数计算后得到距离F_i+1很近的S_i+1。因为d_i+1较大，所以|S_i+1F_i+2|值也会大概率较大，那么F_i+2落在以S_i+1为焦点的Shell_i+1内部的概率就比较低。在由于误差原因导致的目标突然转向，使得θ_i+1比较大的情况下，需要将F_i+2成功过滤掉(即S_i+2与S_i+1重合)的可能性就会较小，就算不能将F_i+2过滤掉，也需要抑制从S_i+1向F_i+2方向位移的距离(选择离S_i+1近，离F_i+2远的一点作为S_i+2)，但是这样的抑制效果也弱。假如所有的Shell_i设为固定大尺寸，这样会把距离S_i足够远的F_i+1也囊括到Shell_i内部，会导致S_i+1重合到S_i，S_i+1距离真实测量的F_i+1就偏差太远了，使得过滤效果不符合实际测量。因此综上所述，随着d_i的变化，Shell_i.f的值理应成正相关地适度放大或缩小，因此d_i这个指标决定了Shell_i的大小设计。

S35：调用S_i+1＝Attach(S_i+1,RN)，用最近生成的历史过滤点构成的局部轨迹RN来修正最新过滤点S_i+1的值：当S_i+1与RN中除S_i以外的一点S_j，满足S_j是RN中离S_i+1最近的轨迹点，且|S_i+1S_j|小于Shell.f允许的最小值Min_f时，S_i+1取值为S_j，否则当存在RN中除S_i以外相邻的两点S_j、S_j+1构成的线段S_jS_j+1，使得S_jS_j+1是RN中离S_i+1最近的线段，且通过S_i+1作S_jS_j+1所在的直线的垂足S_⊥位于线段S_jS_j+1内，并且|S_i+1S_⊥|＜|S_i+1S_i|，且|S_i+1S_⊥|小于Shell.f允许的最小值Min_f时，S_i+1取值为S_⊥，否则S_i+1的值不变，即不需要修正位置；如果S_i+1不与S^*重合的情况下，调用α_i+1＝Update(α^*,S^*,S_i+1)来更新x轴正方向的单位向量

沿着逆时针方向到线段S_i+1S^*的夹角α_i+1。

在不考虑Attach函数修正影响的前提下，即函数GeoF省去步骤S35，我们来完整地演示一遍函数GeoF是如何工作的。如图6所示，是一个简单过滤轨迹的生成过程示例图：

①获得初始测量点F₀，S₀直接取值为F₀；

②获得F₁，根据S₀确定Shell₀的焦点，根据S₀F₁计算出Shell₀和Shell₁的偏转角(偏转角一致)，根据|F₁F₀|确定Shell₀的大小，再根据S₀F₁计算出C₀(C₀位于Shell₀的顶点)，从而计算出S₁；

③获得F₂，根据S₁确定Shell₁的焦点，已知Shell₁的偏转角已计算出，根据S₁F₂计算出Shell₂的偏转角，根据|F₂F₁|确定Shell₁的大小，再根据S₁F₂计算出C₁，从而计算出S₂；

④获得F₃，根据S₂确定Shell₂的焦点，已知Shell₂的偏转角已计算出，根据S₂F₃计算出Shell₃的偏转角，根据|F₃F₂|确定Shell₂的大小，再根据S₂F₃计算出C₂，从而计算出S₃；

⑤获得F₄，根据S₃确定Shell₃的焦点，已知Shell₃的偏转角已计算出，根据S₃F₄计算出Shell₄的偏转角，根据|F₄F₃|确定Shell₃的大小，再根据S₃F₄计算出C₃，从而计算出S₄；

⑥获得F₅，根据S₄确定Shell₄的焦点，已知Shell₄的偏转角已计算出，根据S₄F₅计算出Shell₅的偏转角，根据|F₅F₄|确定Shell₄的大小，此时因为F₅在Shell₄内部，所以不存在S₄F₅与Shell₄边界的交点C₄，所以S₅与S₄重合；

⑦获得末尾点F₆，根据S₅确定Shell₅的焦点，已知Shell₅的偏转角已计算出，根据S₅F₆计算出Shell₆的偏转角(不再用到)，根据|F₆F₅|确定Shell₅的大小，再根据S₅F₆计算出C₅，从而计算出S₆。

所以将折线LF＝F₀F₁F₂F₃F₄F₅F₆的通过算法转换成了折线LS＝S₀S₁S₂S₃S₄S₅S₆(其中S₄、S₅重合，所以|S₄S₅|＝0)。可以看出原始轨迹LF的相邻线段之间方向变化较为剧烈且轨迹自相交；过滤轨迹LS的相邻线段之间方向变化较为平缓，如∠F_k-1F_kF_k+1≤∠S_k-1S_kS_k+1(当k＝1,2,3)成立，并解除了自相交。

其中步骤S33和步骤S35中的Update函数的实现过程如下：

①给定一个轨迹点p，它可以是最新测量点F_i+1或最新过滤点S_i+1；

②判断S^*＝＝p和α^*≥0是否同时成立，若成立，最新偏转角保持为缓存α^*中的最近偏转角的值：α_i+1＝α^*，转入步骤④；

③通过轨迹点p和缓存在S^*中的最近过滤点的相对位置，计算最新偏转角α_i+1＝Atan(S^*,p)；

④返回最新偏转角α_i+1。

注：最新测量点F_i+1或最新过滤点S_i+1，相对于最近过滤点S^*是没有移动的(重合)情况下，最新偏转角α_i+1要与之前存储的最近偏转角α^*一致，即过滤罩不转向。但是这里有一个前提，就是最近偏转角被成功的设置了，即α^*≠-∞。如果在重合的情况下又没有设置最近偏转角，那么会调用else语句块中的α₁＝Atan(S^*,S^*)＝0来设置。

其中步骤S34中的GetRaw函数的实现过程如下：

①从过滤罩Shell_i中获取最近偏转角α_i＝Shell_i.α；

②判断α_i＜0是否成立，如果是，则初始化夹角θ_i＝0，转入步骤④；

③计算最近偏转角α_i和最新偏转角α_i+1之间的夹角：θ_i＝Bettween(α_i,α_i+1)；

④从过滤罩Shell_i中获取最近过滤点S_i＝Shell_i.s；

⑤计算最近过滤点S_i到最新测量点F_i+1的距离：|S_iF_i+1|＝Distance(S_i,F_i+1)；

⑥计算最近过滤点S_i(焦点)到过滤罩上与过滤罩顶点相对于焦点呈夹角θ_i的点C_i之间的距离：|S_iC_i|＝CosP(Shell_i.f,θ_i)；

⑦判断|S_iF_i+1|≤|S_iC_i|是否成立，如果成立，最新过滤点S_i+1与最近过滤点S_i重合：S_i+1＝S_i，转入步骤⑨；

⑧计算距离比值系数λ＝|S_iC_i|/|S_iF_i+1|，计算位于线段S_iF_i+1上的最新过滤点：S_i+1＝λ*S_i+(1-λ)*F_i+1；

⑨返回最新过滤点S_i+1。

其中步骤S35中的Attach函数的实现过程如下：

①判断RN.Size()＞0是否成立，若不成立，转至步骤⑨；

②获取环形缓存队列RN中的最近轨迹点S_i＝RN.Get(RN.Size()-1)，计算距离它到最新过滤点S_i+1的距离|S_iS_i+1|＝Distance(S_i,S_i+1)；

③计算RN中每一点到S_i+1的距离，找出距离最小的点min_p，以及最小距离值min_d；

④判断min_d＜Min_f和min_d＜|S_iS_i+1|是否同时成立，其中Min_f是允许的最小过滤罩f值，若不成立转至步骤⑥；

⑤最新过滤点S_i+1赋值为min_p：S_i+1＝min_p，转至步骤⑨；

⑥计算RN中每相邻两点构成的线段到S_i+1的垂直距离，找出距离最小的线段的两个端点min_pA和min_pB，以及最小距离值min_l和S_i+1在该线段上的垂足的位置相对于整个线段的比例系数min_λ；

⑦判断min_l＜Min_f和min_l＜|S_iS_i+1|是否同时成立，若不成立转至步骤⑨；

⑧根据步骤⑥中得到的比例系数，min_λ计算最新过滤点位于线段min_pA min_pB上的最新过滤点：S_i+1＝(1-min_λ)*min_pA+min_λ*min_pB；

⑨返回最新过滤点S_i+1。

注：在三种情况下，代入函数的参数S_i+1可以逃避Attach函数的条件筛选，即S_i+1不被修正，而保持原值输出，这里称之为“逃逸Attach函数”：①当RN中没有轨迹点或只有一个轨迹点时，即生成S₀、S₁时可以逃逸Attach函数；②当S_i+1的值与RN中最后一个轨迹点S_i重合时可以逃逸Attach函数；③当|S_iS_i+1|不大于“S_i+1与RN构成的折线包含的所有点集(S_i点除外)”中离S_i+1最近的那个点的距离，且与该点的距离值小于Min_f的时候，则可以逃逸Attach函数。

Attach函数的过程说明如图7的示例所示，分别展示了两种不同的修正S_i+1值的情形，修正之前用S′_i+1表示S_i+1。Situation:A的情形是：当计算出S′_i+1距离RN-S_i中距离最近的点S_u，其距离值|S_uS′_i+1|满足|S_uS′_i+1|＜|S_iS′_i+1|且|S_uS′_i+1|＜Min_f时，S′_i+1被修正到S_u，使得修正后的S_i+1＝S_u；Situation:B的情形是：当S′_i+1与RN-S_i的位置关系不满足左半边图的情形下，找到一条线段S_uS_u+1，满足S′_i+1到S_uS_u+1所在的直线的垂足S_⊥在线段S_uS_u+1内部(不含边界两点)，且S_uS_u+1是RN构成的折线中距离S′_i+1最近的线段，且距离值|S_⊥S′_i+1|满足|S_⊥S′_i+1|＜|S_iS′_i+1|且|S_⊥S′_i+1|＜Min_f时，S′_i+1被修正到S_⊥，使得修正后的S_i+1＝S_⊥。

之所以设计子函数Attach，是因为函数GeoF中经过子函数GetRaw初步生成的最新过滤点S_i+1，如果跳过步骤S35直接加入到LS的话，在运动目标往返经过某一条路径的时候，形成的轨迹会存在这样一个特点：LS中经过该路径的部分，会形成多条大致与路径平行走向，相互不相关的子轨迹，分散宽度有时会超过路径宽度，出现轨迹整体漂移。因为函数GetRaw只与Shell_i、F_i+1、α_i+1相关；Shell_i只与F_i+1、F^*(F_i)、S^*(S_i)、α^*(α_i)相关；α_i+1又与α^*(α_i)、S^*(S_i)、F_i+1相关。所以S_i+1与S_i、α_i、F_i、F_i+1这四个参数直接相关。其中最近测量点F_i、最新测量点F_i+1是独立数据，最近过滤点S_i、最近偏转角α_i是基于历史轨迹迭代计算出来的数据。越是最先出现或生成的历史轨迹点(比如S₀、F₁)，对计算最新过滤点S_i+1的影响就越小，因为在迭代过程中，历史数据影响权重会迅速减弱，最近和最新数据影响会非常大。因此可以认为，不包含步骤S35的函数GeoF只不具备对历史数据的记忆跟踪能力，当在同一条路径上，既生成去程子轨迹(记为ls₁)时，又生成返程子轨迹(记为ls₂)时，通常ls₁与ls₂之间还有较长的子轨迹连接两者，所以ls₁包含的轨迹点对ls₂来说，就是久远的历史数据，对ls₂中每一个过滤点的生成，几乎没有影响，可以认为ls₁与ls₂是几乎不相关的。所以这种情况下，可以保证GeoF生成的LS平缓光滑，但是经过同一路段的子轨迹会发生整体偏移。如图9所示，原始轨迹LF在同一路段出现多次往返的情况下，存在较宽(约10m)的偏移宽度(图中用白色粗线做围界标识出来)；用不包含步骤S35的函数GeoF生成的过滤轨迹LS(标识“Without S35”)，在同一路段出现多次往返的情况下，同样存在较宽的偏移宽度，与LF在同一路段的宽度相当；用包含步骤S35的函数GeoF生成的过滤轨迹LS(标识“Include S35”)，在同一路段出现多次往返的情况下(近期往返数据存储在RN中)，子轨迹间仅存在很窄(不到3m)的偏移宽度，所有往返同一路段的子轨迹高度重合。包含步骤S35的情形下的白色围界区所在的带状区域，几乎处于LF对应的带状区域中间，后者的分布通常是关于路段的中心线呈现某种对称概率分布的，所以函数Attach能大概率将往复在同一路段上的子轨迹收敛到路段所在的区域内，减少LS的某一段漂移出路段边界的情况。

Attach函数发挥效果的实际程序运行截图如图8所示，该图显示的场景涉及了图5和图7这两张示意图说明的集合对象。Situation A和Situation B中，修正轨迹点的情形分别对应图7的Situation:A和Situation:B。图中标记数字的小圆点代表轨迹点及其位置，用竖直连线连接的上方带*的数字表示生成一条完整过滤轨迹后(预先从数据库调取的航迹记录)，在该圆点对应位置重合的轨迹点数目。相邻的轨迹点之间用线段连接构成完整的LS(因为每个圆点可能是重合的多个轨迹点，所以构成折线LS的某一条线段可能是多次重合线段的结果)。Shell_i是抛物线为边界的深色区域，带星状边圈的小圆点对应的轨迹点是Shell_i的焦点S_i，由于这里F_i+1位于Shell_i外部，所以S_iF_i+1交Shell_i的边界于C_i，用函数GetRaw初步计算出未经过修正前的最新过滤点S_i'₊₁。在Situation A中，圆形区域的圆心位置对应的某个历史过滤点即为Attach函数找到的修正后的点S_i+1，S′_i+1处于圆形区域边界上，向量

即为修正量，移动距离就是圆的半径，是RN中离S′_i+1最近的轨迹点，值得注意的是图中存在更近的轨迹点，那是比RN中的点更早的历史轨迹点或之后S_i+1_之后的轨迹点，不参与轨迹过滤算法的计算；在Situation B中，胶囊形区域中心即为连接RN中相邻两点的线段，它是RN中离S′_i+1最近的线段，落在线段之间的垂足即为Attach函数找到的修正后的点S_i+1，它极可能不和任何一个历史轨迹点处于同一位置，S′_i+1处于胶囊形区域边界上，与该线段正交的向量

即为修正量。

其中步骤S35中的Update函数，调用条件是经过Attach函数计算后的S_i+1不与最近过滤点S^*重合，即S^*≠S_i+1。这样设计的逻辑在于，如果经过Attach函数计算后存在S^*与S_i+1重合，那么作为参数输入Attach前的S_i+1，就已经与S^*(即RN中最后一个轨迹点S_i)重合了，在前面已经分析过，S_i+1是逃逸Attach函数了的。所以在GetRaw函数的处理过程中，F_i+1是位于Shell_i内部或边界上的，此时计算的最新偏转角α_i+1是指向F_i+1的，更切合航空器的实际运动方向，如果步骤S35中调用Update函数不设置条件筛选，那么强行调用后会出现α_i+1＝0的情况，下一个过滤罩Shell_i+1相对于Shell_i的方向会突变，不符合运动规律，影响过滤效果。

S36：此时最新过滤点S_i+1已计算出，这里需要对缓存数据进行更新：F^*＝F_i+1(原值为F_i)，S^*＝S_i+1(原值为S_i)，α^*＝α_i+1(原值为α_i)，添加S_i+1到最近生成的历史过滤点的环形缓存队列中：RN.AddElement(S_i+1)。

S4：输出一个最新过滤点S_i+1，并添加到过滤轨迹LS的末尾：LS.Add(S_i+1)，此时可以设置序号i自增(i++)，返回步骤S2(如果还有需要接收的某条轨迹的测量点)，否则算法结束，生成了完整的过滤轨迹LS。

本实施例首先数据复杂性要求低，仅需要基于时间先后的测量点的空间位置信息，不需要每个测量点的时间戳去计算基于时间差的运动规律，也不需要依赖于道路信息去做提高精度的路径匹配；其次，计算资源消耗小，运用简单的空间几何关系计算得出结果，输入最新测量点后迅速计算出最新过滤点，不需要对历史数据做拟合等统计分析来预测运动趋势，大大减少计算开销；再次，定位准确且稳定性高，轨迹更符合运动规律，位置都是被修正到附近小范围空间领域中，保证不过度偏离测量数据，形成连续光滑的行进轨迹、不出现自相交、前后往复、左右锯齿状摆动等情况，能体现稳定静止、匀加减速、平滑转弯等符合物理惯性特征的运动趋势。

本实施例在定位数据的定位精度误差在5m～10m的情况下，Min_f＝5、Max_f＝100、Min_d＝30、Max_d＝150、N＝1000的参数设置下，可以达到以下效果：

如图10所示，运动目标在事实静止的情况下，接受到的红色标注的测量点给出的定位数据出现团状分布，分布范围在约8m跨度的平面空间中，并在点与点之间出现约1m～3m幅度的随机性漂移，形成自相交轨迹(上半图)；过滤算法将团状分布的点集，修正映射到单个蓝色标注的过滤点上，该点所在的位置是团状分布空间内的某一点，随着测量点在空间内漂移，过滤点保持进行不动，符合静止的实际情况(下半图)。

如图11所示，运动目标在事实直行的情况下，接受到的红色标注的测量点给出的定位数据出现沿直行的左右方向漂移，并前后往复运动，形成锯齿状并存在局部自相交的尺寸约4m的小团块的曲折轨迹(上半图)；过滤算法将测量点绘制的轨迹，修正到一条蓝色标注的过滤点构成的不再往复，连续向前行进的光滑轨迹上(下半图)。

如图12所示，运动目标在从事实静止到加速的过程中，接受到的红色标注的测量点给出的定位数据在起步到逐渐加速的时候出现团状分布，形成轨迹头部为自相交的尺寸约6m的小团块而后曲折行进的轨迹(上半图)；过滤算法将测量点绘制的轨迹，修正到一条蓝色标注的过滤点构成的逐渐加速、连续向前行进的光滑轨迹上(下半图)。

如图13所示，运动目标在从事实减速到静止的过程中，接受到的红色标注的测量点给出的定位数据在逐渐减速到静止的时候出现团状分布，形成轨迹曲折行进而后尾部为自相交的尺寸约5m的小团块的轨迹(上半图)；过滤算法将测量点绘制的轨迹，修正到一条蓝色标注的过滤点构成的逐渐减速、连续向前行进的光滑轨迹上(下半图)。

如图14所示，运动目标在事实变速转弯的情况下，接受到的红色标注的测量点给出的定位数据在“减速后转向”与“转向后加速”的转向过程中出现团块分布，分布范围在约12m跨度的平面空间中，团块内部点与点之间出现1m～2m幅度的随机性漂移，形成锯齿状行进、转弯时自相交轨迹(上半图)；过滤算法将测量点绘制的轨迹，修正到一条蓝色标注的过滤点构成的“逐渐减速、果断转弯、逐渐加速”并且过程中不出现往复运动的连续向前行进的光滑轨迹上(下半图)。

如图15所示，本发明示例性第五实施例的航空器航迹定位数据偏移的修正装置的结构图包括：

预处理单元1501，用于在进行航空器当前航迹定位数据偏移的修正操作之前清空历史缓存信息；

接收单元1502，用于接收分布式定位***发送的航空器的实时测量点信息；

过滤单元1503，用于根据所述实时测量点信息以及当前缓存信息,确定用于作为过滤罩的当前抛物线以及实时偏转角信息；以及根据所述实时测量点信息、当前抛物线以及实时偏转角信息,计算生成所述实时测量点对应的实时过滤点信息；所述当前缓存信息包括：上一测量点缓存信息、上一过滤点缓存信息、上一偏转角缓存信息、以及包括当前航迹的初始过滤点至上一过滤点之间各过滤点信息的缓存队列；所述上一偏转角缓存信息为轴正方向的单位向量沿着逆时针方向与从所述当前抛物线的焦点到所述当前抛物线的顶点连线形成向量之间的夹角；

更新单元1504，用于根据所述实时测量点信息、实时过滤点信息、实时偏转角信息更新当前缓存信息；

航迹修正单元1505，用于根据所述当前航迹的所有过滤点信息，得到航空器当前航迹的修正航迹。

优选地：所述过滤单元1503具体用于：确定上一过滤点缓存信息到实时测量点信息连接而成的线段与当前抛物线边界的公共点；轴正方向的单位向量沿着逆时针方向与从所述当前抛物线的焦点到当前抛物线边界的公共点连线形成向量之间的夹角作为实时偏转角缓存信息。

优选地：所述过滤单元1503具体还用于：将所述上一过滤点缓存信息为用于作为过滤罩的当前抛物线的焦点,并将所述上一测量点缓存信息与上一过滤点缓存信息的连线为当前抛物线的中线上的线段,以及根据所述上一测量点缓存信息与实时测量点信息之间的连线长度确定所述当前抛物线的大小。

优选地：所述过滤单元1503具体还用于：计算上一偏转角信息和实时偏转角信息之间的夹角，再计算所述当前抛物线边界的公共点与所述当前抛物线的焦点之间的距离，如果公共点存在，则在所述当前抛物线的焦点与所述实时测量点信息形成的线段上确定实时过滤点信息,所述实时过滤点信息满足上一过滤点信息与所述公共点的距离等于所述实时过滤点信息与所述实时测量点之间的距离，若公共点不存在,实时过滤点信息即为上一过滤点信息。

优选地：所述的航空器航迹定位数据偏移的修正装置还包括:过滤点信息修正单元1506用于：根据所述缓存队列修正实时过滤点信息，以使得所述更新单元根据所述修正后的实时过滤点信息更新当前缓存信息以及使得所述航迹修正单元根据所述修正后的实时过滤点信息得到航空器当前航迹的修正航迹

本实施例修正后的路径连续光滑，不会出现运行方向和位移程度突变的情况，能很好地反映航空器的连续行进趋势；修正后的路径中的每一个点，与对应的实际测量数据之间，不存在太大的偏差，把误差控制在较小范围，并尽可能与进行的道面在空间上吻合；修正后的路径，要过滤掉因定位数据误差漂移造成的轨迹自相交的情况，消除轨迹在较小空间范围内出现的团结分布、高度折叠等情况，阻止出现沿着道面行进方向上的往复运动，使轨迹的形态符合航空器运动的物理规律，如呈现持续前进、缓慢加减速、转弯幅度大、不能轻易改变方向等大惯性物体的运动特征。本修正装置对输入数据的依赖必须足够简单，仅仅需要输入按时间先后顺序生成的、来至定位数据的测量点序列，通过对这些点的空间几何关系的分析，计算得出合理的修正后的定位点，而不需要依赖额外的数据集，避免增加使用、维护、计算难度。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。

Claims (4)

1.一种航空器航迹定位数据偏移的修正方法，其特征在于，包括：

在进行航空器当前航迹定位数据偏移的修正操作之前清空历史缓存信息；

接收分布式定位***发送的航空器的实时测量点信息；

根据所述实时测量点信息以及当前缓存信息,确定用于作为过滤罩的当前抛物线以及实时偏转角信息；所述当前缓存信息包括：上一测量点缓存信息、上一过滤点缓存信息、上一偏转角缓存信息、以及包括当前航迹的初始过滤点至上一过滤点之间各过滤点信息的缓存队列；所述上一偏转角缓存信息为x轴正方向的单位向量沿着逆时针方向与从所述当前抛物线的焦点到所述当前抛物线的顶点连线形成向量之间的夹角；

根据所述实时测量点信息、当前抛物线以及实时偏转角信息,计算生成所述实时测量点信息对应的实时过滤点信息；

根据所述实时测量点信息、实时过滤点信息、实时偏转角信息更新当前缓存信息；

根据所述当前航迹的所有过滤点信息，得到航空器当前航迹的修正航迹；

根据所述实时测量点信息以及当前缓存信息,确定实时偏转角信息包括：

确定上一过滤点缓存信息到实时测量点信息连接而成的线段与当前抛物线边界的公共点；

x轴正方向的单位向量沿着逆时针方向与从所述当前抛物线的焦点到当前抛物线边界的公共点连线形成向量之间的夹角作为实时偏转角信息；

根据所述实时测量点信息以及当前缓存信息,确定用于作为过滤罩的当前抛物线包括：

将所述上一过滤点缓存信息为用于作为过滤罩的当前抛物线的焦点,并将所述上一测量点缓存信息与上一过滤点缓存信息的连线为当前抛物线的中线上的线段,以及根据所述上一测量点缓存信息与实时测量点信息之间的连线长度确定所述当前抛物线的大小；

根据所述实时测量点信息、当前抛物线以及实时偏转角信息,计算生成所述实时测量点信息对应的实时过滤点信息的步骤包括:

根据上一偏转角缓存信息和实时偏转角信息，计算当前抛物线的焦点与当前抛物线的顶点连线形成向量与当前抛物线的焦点与当前抛物线边界的公共点连线形成向量之间的夹角，再计算所述当前抛物线边界的公共点与所述当前抛物线的焦点之间的距离，如果公共点存在，则在所述当前抛物线的焦点与所述实时测量点信息形成的线段上确定实时过滤点信息,所述实时过滤点信息满足上一过滤点缓存信息与所述公共点的距离等于所述实时过滤点信息与所述实时测量点信息之间的距离，若公共点不存在,实时过滤点信息即为上一过滤点缓存信息。

2.如权利要求1所述的航空器航迹定位数据偏移的修正方法，其特征在于：根据所述实时测量点信息、当前抛物线以及实时偏转角信息,计算生成所述实时测量点信息对应的实时过滤点信息的步骤之后还包括:

根据所述缓存队列修正实时过滤点信息，以根据所述修正后的实时过滤点信息更新当前缓存信息以及得到航空器当前航迹的修正航迹。

3.一种航空器航迹定位数据偏移的修正装置，其特征在于，包括：

预处理单元，用于在进行航空器当前航迹定位数据偏移的修正操作之前清空历史缓存信息；

接收单元，用于接收分布式定位***发送的航空器的实时测量点信息；

过滤单元，用于根据所述实时测量点信息以及当前缓存信息,确定用于作为过滤罩的当前抛物线以及实时偏转角信息；以及根据所述实时测量点信息、当前抛物线以及实时偏转角信息,计算生成所述实时测量点信息对应的实时过滤点信息；所述当前缓存信息包括：上一测量点缓存信息、上一过滤点缓存信息、上一偏转角缓存信息、以及包括当前航迹的初始过滤点至上一过滤点之间各过滤点信息的缓存队列；所述上一偏转角缓存信息为x轴正方向的单位向量沿着逆时针方向与从所述当前抛物线的焦点到所述当前抛物线的顶点连线形成向量之间的夹角；

更新单元，用于根据所述实时测量点信息、实时过滤点信息、实时偏转角信息更新当前缓存信息；

航迹修正单元，用于根据所述当前航迹的所有过滤点信息，得到航空器当前航迹的修正航迹；

所述过滤单元具体用于：确定上一过滤点缓存信息到实时测量点信息连接而成的线段与当前抛物线边界的公共点；x轴正方向的单位向量沿着逆时针方向与从所述当前抛物线的焦点到当前抛物线边界的公共点连线形成向量之间的夹角作为实时偏转角信息；

所述过滤单元具体还用于：将所述上一过滤点缓存信息为用于作为过滤罩的当前抛物线的焦点,并将所述上一测量点缓存信息与上一过滤点缓存信息的连线为当前抛物线的中线上的线段,以及根据所述上一测量点缓存信息与实时测量点信息之间的连线长度确定所述当前抛物线的大小；

所述过滤单元具体还用于：根据上一偏转角缓存信息和实时偏转角信息，计算当前抛物线的焦点与当前抛物线的顶点连线形成向量与当前抛物线的焦点与当前抛物线边界的公共点连线形成向量之间的夹角，再计算所述当前抛物线边界的公共点与所述当前抛物线的焦点之间的距离，如果公共点存在，则在所述当前抛物线的焦点与所述实时测量点信息形成的线段上确定实时过滤点信息,所述实时过滤点信息满足上一过滤点缓存信息与所述公共点的距离等于所述实时过滤点信息与所述实时测量点信息之间的距离，若公共点不存在,实时过滤点信息即为上一过滤点缓存信息。

4.如权利要求3所述的航空器航迹定位数据偏移的修正装置，其特征在于：还包括:

过滤点信息修正单元用于：根据所述缓存队列修正实时过滤点信息，以使得所述更新单元根据所述修正后的实时过滤点信息更新当前缓存信息以及使得所述航迹修正单元根据所述修正后的实时过滤点信息得到航空器当前航迹的修正航迹。

Images