CN112365743B - 航空器航迹定位数据偏移的修正方法及装置 - Google Patents
航空器航迹定位数据偏移的修正方法及装置 Download PDFInfo
- Publication number
- CN112365743B CN112365743B CN202011084881.1A CN202011084881A CN112365743B CN 112365743 B CN112365743 B CN 112365743B CN 202011084881 A CN202011084881 A CN 202011084881A CN 112365743 B CN112365743 B CN 112365743B
- Authority
- CN
- China
- Prior art keywords
- real
- information
- current
- point
- time
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G08—SIGNALLING
- G08G—TRAFFIC CONTROL SYSTEMS
- G08G5/00—Traffic control systems for aircraft, e.g. air-traffic control [ATC]
- G08G5/0073—Surveillance aids
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Traffic Control Systems (AREA)
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={P0,P1,...,Pi-1,Pi},P0称为起始点,Pi称为最新点;F和S是数据格式为P的轨迹点;LF={F0,F1,...,Fi-1,Fi}称为当前原始轨迹,Fi称为最近测量点,Fi+1称为最新测量点;LS={S0,S1,...,Si-1,Si}称为当前过滤轨迹,Si称为最近过滤点,Si+1称为最新过滤点;Add是在轨迹中顺序添加新轨迹点的方法,调用方式LF.Add(Fi+1)或者LS.Add(Si+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={S0,S1,...,Si-1,Si},Shelli是以最近过滤点Si为焦点、点Zi为顶点、FiSi为中线上的线段(如果Fi≠Si)、用于计算最新过滤点Si+1的Shell;x轴正方向的单位向量沿着逆时针方向与向量之间的夹角αi称为最近偏转角。
⑥Shelli的焦点Si到最新测量点Fi+1连接而成的线段与Shelli边界的公共点称为Ci(如果公共点存在);焦点Si与顶点Zi形成的向量与焦点Si与点Ci形成的向量之间的、小于平角的夹角称为θi(如果Ci存在);x轴正方向的单位向量沿着逆时针方向与向量之间的夹角αi+1称为最新偏转角(如果Ci存在)。
⑦fliter是过滤算法模块的实例;Init是初始化函数,在使用模块生成完整过滤轨迹之前仅调用一次,调用方式为:fliter.Init();Generate是计算函数,每接收到一个测量点就要调用一次来生成一个过滤点,调用方式为:Si+1=fliter.Generate(Fi+1)。
⑧F*是fliter中用于存储最近测量点Fi的缓存变量;S*是fliter中用于存储最近过滤点Si的缓存变量;α*是fliter中用于存储最近偏转角αi的缓存变量;RN是fliter中用于存储LS中最近生成的历史过滤点的环形缓存队列;当LS的长度小于N时有RN=LS,否则RN={Si-N+1,Si-N+2,...,Si-1,Si},其中N为队列最大允许的长度。
⑨AddElement是RN中添加元素的方法,调用方式RN.AddElement(Si+1),即把元素顺序Si+1添加到Si之后;Clear是RN中清空所有元素的方法,调用方式RN.Clear();IsEmpty是RN中判断自己是否为空列表的方法,调用方式RN.IsEmpty();Size是判断RN长度的方法,调用方式RN.Size();中Get是RN中获得某个轨迹点的方法,调用方式
Si=RN.Get(RN.Size()-1)。
⑩GeoF是fliter中实现Generate的核心函数,称为几何计算函数,满足关系Si+1=GeoF(Fi+1,F*,S*,α*,RN),用于计算最新过滤点;Create是GeoF的内部函数,用于生成Shell,满足关系Shelli=Create(Fi+1,F*,S*,α*);Update是GeoF的内部函数,用于计算或更新最新偏转角,满足关系αi+1=UpdateS(α*,S*,Fi+1or Si+1);GetRaw是GeoF的内部函数,用于初步计算最新过滤点,满足关系Si+1=GetRaw(Shelli,Fi+1,αi+1);Attach是GeoF的内部函数,用于修正最新过滤点,得到最新过滤点作为最终结果,满足关系Si+1=Attach(Si+1,RN)。
Distance是工具函数,用于计算两点间的距离,调用方式为|P1P2|=Distance(P1,P2),其中P1、P2为任意两个轨迹点,|P1P2|为两点的距离值,距离的预设最小值为Min_d,距离的预设最大值为Max_d,预设值会在Create函数中用到;Atan是工具函数,用于计算x轴正方向的单位向量沿着逆时针方向到向量之间的夹角调用方式为Bettween是工具函数,计算最近偏转角αi和最新偏转角αi+1之间小于平角的夹角θi,满足关系θi=Bettween(αi,αi+1);Linear是工具函数,调用方式y=Linear(X1,X2,x),当X1≤x≤X2时,满足0≤y≤1,x和y呈线性关系,X1>x时y=0,x>X2时y=1;CosP是工具函数,调用方式l=CosP(Shelli.f,θi),其中θi是Shelli焦点Si与顶点Zi形成的向量与焦点Si与Shelli边界上的点Ci形成的向量之间的夹角,l是线段SiCi的距离值。
具体地,如图4所示,航空器航迹定位数据偏移的修正方法的计算流程框架图(主要将数据流框架图中的计算部分流程化、形式化)包括以下步骤:
S1:fliter是过滤算法模块的实例,调用初始化函数fliter.Init()以清空缓存变量F*、S*、α*、RN,它们是和具体实例fliter相关的内部属性,设置接收点顺序号i=-1。
设置全局接收点顺序索引号i=-1。
S2:接收一个最新测量点Fi+1,并添加到原始轨迹LF的末尾:LF.Add(Fi+1)。
S3:该步骤为几何计算函数计算生成最新过滤点,并更新缓存数据的过程:
Si+1=GeoF(Fi+1,F*,S*,α*,RN),函数GeoF实现过程后面详述。
步骤S3中对函数GeoF的实现是具体实施的最主要内容,重点讨论子步骤S33(生成Shelli相关)、S34(计算Si+1相关)、S35(修正Si+1相关)的详细实现过程。
S31:判断历史过滤点的环形缓存队列RN是否为空队列:bool=RN.IsEmpty();如果bool为真,转至步骤S32,否则转至步骤S33。
S32:此时LS中还没有过滤点生成,这里需要生成第一个过滤点,设置第一个过滤点就是第一个测量点:Si+1=Fi+1(即S0=F0);设置最新偏转角αi+1=-∞(即α0=-∞),表示第一个Shell还没有生成。
函数GeoF实现过程如下:
①计算bool=RN.IsEmpty(),判断bool是否为真,如果不是,转至步骤③;
②最新过滤点直接取值为最新测量点:Si+1=Fi+1,并初始化最新偏转角αi+1=-∞,转至步骤⑨;
③计算Shelli=Create(Fi+1,F*,S*,α*),用于创建初步计算Si+1时,所依赖的过滤罩Shelli;
④计算αi+1=Update(α*,S*,Fi+1),用于计算最新偏转角αi+1;
⑤计算Si+1=GetRaw(Shelli,Fi+1,αi+1),用于初步计算最新过滤点Si+1,其中利用了步骤③中所创建的过滤罩Shelli;
⑥计算Si+1=Attach(Si+1,RN),用于修正步骤⑤中初步计算出的最新过滤点Si+1的值;
⑦判断S*≠Si+1是否成立,如果不成立,转至步骤⑨;
⑧计算αi+1=Update(α*,S*,Si+1),用于调整步骤④中计算的最新偏转角αi+1的值;
⑨更新缓存变量的值:F*=Fi+1、S*=Si+1、α*=αi+1,分别将本次计算获得的最新测量点、最新过滤点、最新偏转角缓存起来,成为下一次调用GeoF计算所依赖的最近测量点、最近过滤点、最近偏转角参数;
⑩将最新过滤点Si+1添加到RN环形缓存队列中:RN.AddElement(Si+1);;
S33:调用Shelli=Create(Fi+1,F*,S*,α*)生成计算Si+1所依赖的过滤罩Shelli(S0作为第一个过滤点,它是不需要通过过滤罩来计算推导的),这里S*是Shelli的焦点(Shelli.s=S*),α*是Shelli的偏转角(Shelli.α=α*),线段值|Fi+1F*|的大小决定Shelli的大小(Shelli.f);调用αi+1=Update(α*,S*,Fi+1)来计算x轴正方向的单位向量沿着逆时针方向到线段Fi+1S*的夹角αi+1,当线段Fi+1S*不存在(即Fi+1=S*时),αi+1的值设置为α*。
其中步骤S33中的Create函数的实现过程如下:
①创建Shelli实例:Shelli=new Shell(),初始化设置Shelli的焦点Shelli.s=S*,初始化设置Shelli的偏转角Shelli.α=α*;
②计算最新测量点Fi+1到最近测量点F*的距离:|Fi+1F*|=Distance(Fi+1,F*),计算该距离值在区间[Min_f,Max_f]中的相对比例系数:φ=Linear(Min_d,Max_d,|Fi+1F*|);
③计算与步骤①中生成的Shelli大小相关的f值:Shelli.f=Min_f+(Max_f-Min_f)*φ;
④返回计算最新过滤点Si+1所依赖的过滤罩Shelli。
其中工具函数Linear的实现如下,
①判断x≤X1是否成立,如果成立,返回0;
②判断x≥X2是否成立,如果成立,返回1;
③返回一个介于(0,1)之间的比例系数:(x-X1)/(X2-X1)。
注:Shelli.f的取值决定过滤罩的大小,它被设定在[Min_f,Max_f]范围之间,且与值|Fi+1F*|的大小相关,也就是说原始轨迹中最新测量点Fi+1与原始轨迹的最近测量点F*之间距离|Fi+1F*|越大,漂移误差绝对值会认为越大,所需要的过滤罩Shelli就设计得越大。
S34:调用Si+1=GetRaw(Shelli,Fi+1,αi+1)初步计算出最新过滤点Si+1的值,这里首先计算Shelli的偏转角αi(αi=Shelli.α)和αi+1之间的夹角θi=Bettween(αi,αi+1),再计算Shelli的焦点Si与Fi+1构成的线段与Shelli边界的公共点Ci与焦点之间的距离|SiCi|=CosPi(Shelli,θi),如果Ci存在,则在线段SiFi+1上找到点Si+1使得SiCi|=|Si+1Fi+1|,否则(Ci不存在时Fi+1点在Shelli的边界或内部)Si+1就取值为Si。
其中工具函数CosP的实现如下,它是一种关于参数(ρi,θi)抛物线的极坐标方程:
|SiCi|=ρi=CosP(Shelli.f,θi)=2*Shelli.f/(1+cosθi)=2*SiZi|/(1+cosθi)。
注:当αi<0时出现在i=0,αi=-∞的情况下,说明已经生成的Shell0是还没有设置偏转角的,但是在步骤S33中已经生成的α1是下一个过滤罩Shell1的偏转角(α1经过步骤S35后不会改变其值),此时将Shell0的偏转角α0预设为α1,即α0与α1夹角θ0=0,保持上述两个过滤罩不转向。当|SiFi+1|<=|SiCi|时,Fi+1位于过滤罩Shelli内部或边界上,此时不管Fi+1是否与Si重合,最新测量点Si+1的值都被设定为与Si重合。
GetRaw函数的过程说明如图6的示例所示,Shelli是以线段Si-1Si所在的直线为对称中轴(如果Si的生成过程没有被Attach函数修正,那么Fi可能在中轴上),顶点为zi,偏转角为沿着逆时针到的夹角αi,θi为沿着逆时针到的夹角,Fi+1是Shelli外部的一点,计算出|SiFi+1|、|SiCi|后初步在计算出线段SiFi+1上计算出Si+1满足|SiCi|=|Si+ 1Fi+1|。
之所以会构造Shell这种几何形状作为过滤罩,是因为它相对于航空器的运动方向呈现对称性,并且便于计算。Shelli的方向就是最近的运动目标的两个过滤点构成的向量的方向,目标在运动方向两侧的误差分布理论上应该是对称的,但是前后方向不对称,即Fi+1相对于Si的位置关系可以用向量描述。与的夹角θi,以及最近测量点Fi和Fi+1的距离di=|FiFi+1|,是对过滤罩Shell该如何设计极为重要的两个参数。θi说明了当前运动的目标在下一刻需要转向的程度(实时方向变化),di说明了当前运动的目标在下一刻需要位移的程度(实时速度)。⑴:θi除了反映真实转向外,还反映误差导致的方向偏移,尤其在低速情况下,误差漂移会导致目标频繁改变方向甚至倒退,所以Shell的设计需要尽可能阻止因为误差导致的突然转向。因此原则就是θi值越大时越需要被抑制,这样过滤后的轨迹会更加连续光滑。而Shell的形状设计为抛物线区域时,当θi值越大,SiFi+1所在的直线与Shell边界的交点Ci到焦点Si的距离就越大,Fi+1落在Shelli内部的概率也就越高,就能有效地将因为误差导致剧烈转向的Fi+1有效地过滤掉。所以,θi这个指标决定了Shelli的几何形状的设计。⑵:di除了反映真实速度外,还反映误差导致的位置偏移量,di值越大时因误差导致的位置偏移量会越大。假如所有的Shelli设为固定小尺寸,仅仅跟随目标的运动改变偏转角,而没有随着实时速度改变尺寸。当目标速度越快时,di=|FiFi+1|值越大,由于目标移动具有的惯性,导致速度不会立马减小,所以后续测量的di+1=|Fi+1Fi+2|值也会较大。因为Shelli.f值较小,所以经过GetRaw函数计算后得到距离Fi+1很近的Si+1。因为di+1较大,所以|Si+1Fi+2|值也会大概率较大,那么Fi+2落在以Si+1为焦点的Shelli+1内部的概率就比较低。在由于误差原因导致的目标突然转向,使得θi+1比较大的情况下,需要将Fi+2成功过滤掉(即Si+2与Si+1重合)的可能性就会较小,就算不能将Fi+2过滤掉,也需要抑制从Si+1向Fi+2方向位移的距离(选择离Si+1近,离Fi+2远的一点作为Si+2),但是这样的抑制效果也弱。假如所有的Shelli设为固定大尺寸,这样会把距离Si足够远的Fi+1也囊括到Shelli内部,会导致Si+1重合到Si,Si+1距离真实测量的Fi+1就偏差太远了,使得过滤效果不符合实际测量。因此综上所述,随着di的变化,Shelli.f的值理应成正相关地适度放大或缩小,因此di这个指标决定了Shelli的大小设计。
S35:调用Si+1=Attach(Si+1,RN),用最近生成的历史过滤点构成的局部轨迹RN来修正最新过滤点Si+1的值:当Si+1与RN中除Si以外的一点Sj,满足Sj是RN中离Si+1最近的轨迹点,且|Si+1Sj|小于Shell.f允许的最小值Min_f时,Si+1取值为Sj,否则当存在RN中除Si以外相邻的两点Sj、Sj+1构成的线段SjSj+1,使得SjSj+1是RN中离Si+1最近的线段,且通过Si+1作SjSj+1所在的直线的垂足S⊥位于线段SjSj+1内,并且|Si+1S⊥|<|Si+1Si|,且|Si+1S⊥|小于Shell.f允许的最小值Min_f时,Si+1取值为S⊥,否则Si+1的值不变,即不需要修正位置;如果Si+1不与S*重合的情况下,调用αi+1=Update(α*,S*,Si+1)来更新x轴正方向的单位向量沿着逆时针方向到线段Si+1S*的夹角αi+1。
在不考虑Attach函数修正影响的前提下,即函数GeoF省去步骤S35,我们来完整地演示一遍函数GeoF是如何工作的。如图6所示,是一个简单过滤轨迹的生成过程示例图:
①获得初始测量点F0,S0直接取值为F0;
②获得F1,根据S0确定Shell0的焦点,根据S0F1计算出Shell0和Shell1的偏转角(偏转角一致),根据|F1F0|确定Shell0的大小,再根据S0F1计算出C0(C0位于Shell0的顶点),从而计算出S1;
③获得F2,根据S1确定Shell1的焦点,已知Shell1的偏转角已计算出,根据S1F2计算出Shell2的偏转角,根据|F2F1|确定Shell1的大小,再根据S1F2计算出C1,从而计算出S2;
④获得F3,根据S2确定Shell2的焦点,已知Shell2的偏转角已计算出,根据S2F3计算出Shell3的偏转角,根据|F3F2|确定Shell2的大小,再根据S2F3计算出C2,从而计算出S3;
⑤获得F4,根据S3确定Shell3的焦点,已知Shell3的偏转角已计算出,根据S3F4计算出Shell4的偏转角,根据|F4F3|确定Shell3的大小,再根据S3F4计算出C3,从而计算出S4;
⑥获得F5,根据S4确定Shell4的焦点,已知Shell4的偏转角已计算出,根据S4F5计算出Shell5的偏转角,根据|F5F4|确定Shell4的大小,此时因为F5在Shell4内部,所以不存在S4F5与Shell4边界的交点C4,所以S5与S4重合;
⑦获得末尾点F6,根据S5确定Shell5的焦点,已知Shell5的偏转角已计算出,根据S5F6计算出Shell6的偏转角(不再用到),根据|F6F5|确定Shell5的大小,再根据S5F6计算出C5,从而计算出S6。
所以将折线LF=F0F1F2F3F4F5F6的通过算法转换成了折线LS=S0S1S2S3S4S5S6(其中S4、S5重合,所以|S4S5|=0)。可以看出原始轨迹LF的相邻线段之间方向变化较为剧烈且轨迹自相交;过滤轨迹LS的相邻线段之间方向变化较为平缓,如∠Fk-1FkFk+1≤∠Sk-1SkSk+1(当k=1,2,3)成立,并解除了自相交。
其中步骤S33和步骤S35中的Update函数的实现过程如下:
①给定一个轨迹点p,它可以是最新测量点Fi+1或最新过滤点Si+1;
②判断S*==p和α*≥0是否同时成立,若成立,最新偏转角保持为缓存α*中的最近偏转角的值:αi+1=α*,转入步骤④;
③通过轨迹点p和缓存在S*中的最近过滤点的相对位置,计算最新偏转角αi+1=Atan(S*,p);
④返回最新偏转角αi+1。
注:最新测量点Fi+1或最新过滤点Si+1,相对于最近过滤点S*是没有移动的(重合)情况下,最新偏转角αi+1要与之前存储的最近偏转角α*一致,即过滤罩不转向。但是这里有一个前提,就是最近偏转角被成功的设置了,即α*≠-∞。如果在重合的情况下又没有设置最近偏转角,那么会调用else语句块中的α1=Atan(S*,S*)=0来设置。
其中步骤S34中的GetRaw函数的实现过程如下:
①从过滤罩Shelli中获取最近偏转角αi=Shelli.α;
②判断αi<0是否成立,如果是,则初始化夹角θi=0,转入步骤④;
③计算最近偏转角αi和最新偏转角αi+1之间的夹角:θi=Bettween(αi,αi+1);
④从过滤罩Shelli中获取最近过滤点Si=Shelli.s;
⑤计算最近过滤点Si到最新测量点Fi+1的距离:|SiFi+1|=Distance(Si,Fi+1);
⑥计算最近过滤点Si(焦点)到过滤罩上与过滤罩顶点相对于焦点呈夹角θi的点Ci之间的距离:|SiCi|=CosP(Shelli.f,θi);
⑦判断|SiFi+1|≤|SiCi|是否成立,如果成立,最新过滤点Si+1与最近过滤点Si重合:Si+1=Si,转入步骤⑨;
⑧计算距离比值系数λ=|SiCi|/|SiFi+1|,计算位于线段SiFi+1上的最新过滤点:Si+1=λ*Si+(1-λ)*Fi+1;
⑨返回最新过滤点Si+1。
其中步骤S35中的Attach函数的实现过程如下:
①判断RN.Size()>0是否成立,若不成立,转至步骤⑨;
②获取环形缓存队列RN中的最近轨迹点Si=RN.Get(RN.Size()-1),计算距离它到最新过滤点Si+1的距离|SiSi+1|=Distance(Si,Si+1);
③计算RN中每一点到Si+1的距离,找出距离最小的点min_p,以及最小距离值min_d;
④判断min_d<Min_f和min_d<|SiSi+1|是否同时成立,其中Min_f是允许的最小过滤罩f值,若不成立转至步骤⑥;
⑤最新过滤点Si+1赋值为min_p:Si+1=min_p,转至步骤⑨;
⑥计算RN中每相邻两点构成的线段到Si+1的垂直距离,找出距离最小的线段的两个端点min_pA和min_pB,以及最小距离值min_l和Si+1在该线段上的垂足的位置相对于整个线段的比例系数min_λ;
⑦判断min_l<Min_f和min_l<|SiSi+1|是否同时成立,若不成立转至步骤⑨;
⑧根据步骤⑥中得到的比例系数,min_λ计算最新过滤点位于线段min_pA min_pB上的最新过滤点:Si+1=(1-min_λ)*min_pA+min_λ*min_pB;
⑨返回最新过滤点Si+1。
注:在三种情况下,代入函数的参数Si+1可以逃避Attach函数的条件筛选,即Si+1不被修正,而保持原值输出,这里称之为“逃逸Attach函数”:①当RN中没有轨迹点或只有一个轨迹点时,即生成S0、S1时可以逃逸Attach函数;②当Si+1的值与RN中最后一个轨迹点Si重合时可以逃逸Attach函数;③当|SiSi+1|不大于“Si+1与RN构成的折线包含的所有点集(Si点除外)”中离Si+1最近的那个点的距离,且与该点的距离值小于Min_f的时候,则可以逃逸Attach函数。
Attach函数的过程说明如图7的示例所示,分别展示了两种不同的修正Si+1值的情形,修正之前用S′i+1表示Si+1。Situation:A的情形是:当计算出S′i+1距离RN-Si中距离最近的点Su,其距离值|SuS′i+1|满足|SuS′i+1|<|SiS′i+1|且|SuS′i+1|<Min_f时,S′i+1被修正到Su,使得修正后的Si+1=Su;Situation:B的情形是:当S′i+1与RN-Si的位置关系不满足左半边图的情形下,找到一条线段SuSu+1,满足S′i+1到SuSu+1所在的直线的垂足S⊥在线段SuSu+1内部(不含边界两点),且SuSu+1是RN构成的折线中距离S′i+1最近的线段,且距离值|S⊥S′i+1|满足|S⊥S′i+1|<|SiS′i+1|且|S⊥S′i+1|<Min_f时,S′i+1被修正到S⊥,使得修正后的Si+1=S⊥。
之所以设计子函数Attach,是因为函数GeoF中经过子函数GetRaw初步生成的最新过滤点Si+1,如果跳过步骤S35直接加入到LS的话,在运动目标往返经过某一条路径的时候,形成的轨迹会存在这样一个特点:LS中经过该路径的部分,会形成多条大致与路径平行走向,相互不相关的子轨迹,分散宽度有时会超过路径宽度,出现轨迹整体漂移。因为函数GetRaw只与Shelli、Fi+1、αi+1相关;Shelli只与Fi+1、F*(Fi)、S*(Si)、α*(αi)相关;αi+1又与α*(αi)、S*(Si)、Fi+1相关。所以Si+1与Si、αi、Fi、Fi+1这四个参数直接相关。其中最近测量点Fi、最新测量点Fi+1是独立数据,最近过滤点Si、最近偏转角αi是基于历史轨迹迭代计算出来的数据。越是最先出现或生成的历史轨迹点(比如S0、F1),对计算最新过滤点Si+1的影响就越小,因为在迭代过程中,历史数据影响权重会迅速减弱,最近和最新数据影响会非常大。因此可以认为,不包含步骤S35的函数GeoF只不具备对历史数据的记忆跟踪能力,当在同一条路径上,既生成去程子轨迹(记为ls1)时,又生成返程子轨迹(记为ls2)时,通常ls1与ls2之间还有较长的子轨迹连接两者,所以ls1包含的轨迹点对ls2来说,就是久远的历史数据,对ls2中每一个过滤点的生成,几乎没有影响,可以认为ls1与ls2是几乎不相关的。所以这种情况下,可以保证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的某一条线段可能是多次重合线段的结果)。Shelli是抛物线为边界的深色区域,带星状边圈的小圆点对应的轨迹点是Shelli的焦点Si,由于这里Fi+1位于Shelli外部,所以SiFi+1交Shelli的边界于Ci,用函数GetRaw初步计算出未经过修正前的最新过滤点Si'+1。在Situation A中,圆形区域的圆心位置对应的某个历史过滤点即为Attach函数找到的修正后的点Si+1,S′i+1处于圆形区域边界上,向量即为修正量,移动距离就是圆的半径,是RN中离S′i+1最近的轨迹点,值得注意的是图中存在更近的轨迹点,那是比RN中的点更早的历史轨迹点或之后Si+1之后的轨迹点,不参与轨迹过滤算法的计算;在Situation B中,胶囊形区域中心即为连接RN中相邻两点的线段,它是RN中离S′i+1最近的线段,落在线段之间的垂足即为Attach函数找到的修正后的点Si+1,它极可能不和任何一个历史轨迹点处于同一位置,S′i+1处于胶囊形区域边界上,与该线段正交的向量即为修正量。
其中步骤S35中的Update函数,调用条件是经过Attach函数计算后的Si+1不与最近过滤点S*重合,即S*≠Si+1。这样设计的逻辑在于,如果经过Attach函数计算后存在S*与Si+1重合,那么作为参数输入Attach前的Si+1,就已经与S*(即RN中最后一个轨迹点Si)重合了,在前面已经分析过,Si+1是逃逸Attach函数了的。所以在GetRaw函数的处理过程中,Fi+1是位于Shelli内部或边界上的,此时计算的最新偏转角αi+1是指向Fi+1的,更切合航空器的实际运动方向,如果步骤S35中调用Update函数不设置条件筛选,那么强行调用后会出现αi+1=0的情况,下一个过滤罩Shelli+1相对于Shelli的方向会突变,不符合运动规律,影响过滤效果。
S36:此时最新过滤点Si+1已计算出,这里需要对缓存数据进行更新:F*=Fi+1(原值为Fi),S*=Si+1(原值为Si),α*=αi+1(原值为αi),添加Si+1到最近生成的历史过滤点的环形缓存队列中:RN.AddElement(Si+1)。
S4:输出一个最新过滤点Si+1,并添加到过滤轨迹LS的末尾:LS.Add(Si+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所述的航空器航迹定位数据偏移的修正装置,其特征在于:还包括:
过滤点信息修正单元用于:根据所述缓存队列修正实时过滤点信息,以使得所述更新单元根据所述修正后的实时过滤点信息更新当前缓存信息以及使得所述航迹修正单元根据所述修正后的实时过滤点信息得到航空器当前航迹的修正航迹。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011084881.1A CN112365743B (zh) | 2020-10-12 | 2020-10-12 | 航空器航迹定位数据偏移的修正方法及装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011084881.1A CN112365743B (zh) | 2020-10-12 | 2020-10-12 | 航空器航迹定位数据偏移的修正方法及装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112365743A CN112365743A (zh) | 2021-02-12 |
CN112365743B true CN112365743B (zh) | 2021-11-09 |
Family
ID=74507622
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202011084881.1A Active CN112365743B (zh) | 2020-10-12 | 2020-10-12 | 航空器航迹定位数据偏移的修正方法及装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112365743B (zh) |
Families Citing this family (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113641685B (zh) * | 2021-10-18 | 2022-04-08 | 中国民用航空总局第二研究所 | 一种用于引导航空器的数据处理*** |
CN114001976B (zh) * | 2021-10-19 | 2024-03-12 | 杭州飞步科技有限公司 | 控制误差的确定方法、装置、设备及存储介质 |
CN114627687B (zh) * | 2022-01-19 | 2023-04-07 | 南京航空航天大学 | 一种基于神经网络预测逃逸轨迹的直升机近地告警方法 |
Family Cites Families (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US6345217B1 (en) * | 2000-03-31 | 2002-02-05 | Rapistan Systems Advertising Corp. | Automated guided vehicle (AGV) with bipolar magnet sensing |
EP1489380A1 (en) * | 2002-03-28 | 2004-12-22 | Matsushita Electric Industrial Co., Ltd. | Relative position information correction device, relative position information correction method, relative position information correction program, shape vector generation device, shape vector generation method, and shape vector generation program |
CN103425377B (zh) * | 2012-05-21 | 2016-05-25 | 汉王科技股份有限公司 | 坐标修正方法及装置、电磁触控装置 |
JP6380284B2 (ja) * | 2015-08-05 | 2018-08-29 | 株式会社Soken | 位置検出装置、及び位置検出システム |
JP6793085B2 (ja) * | 2017-04-07 | 2020-12-02 | クラリオン株式会社 | 駐車支援装置 |
JP7047392B2 (ja) * | 2018-01-16 | 2022-04-05 | 愛知製鋼株式会社 | 車両位置管理システム及び車両位置管理方法 |
CN109459772A (zh) * | 2018-11-28 | 2019-03-12 | 江苏理工学院 | 一种飞行器位置信号融合滤波方法 |
CN109725804B (zh) * | 2018-12-21 | 2020-11-17 | 歌尔科技有限公司 | 基于投影的轨迹识别方法、投影设备及存储介质 |
CN111210668B (zh) * | 2019-12-30 | 2022-02-15 | 四川函钛科技有限公司 | 一种基于时序qar参数的着陆阶段飞行轨迹偏移修正方法 |
-
2020
- 2020-10-12 CN CN202011084881.1A patent/CN112365743B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN112365743A (zh) | 2021-02-12 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112365743B (zh) | 航空器航迹定位数据偏移的修正方法及装置 | |
CN109766878B (zh) | 一种车道线检测的方法和设备 | |
US10347139B2 (en) | Autonomous nap-of-the-earth (ANOE) flight path planning for manned and unmanned rotorcraft | |
US11136027B2 (en) | Vehicle control device | |
US10656650B2 (en) | Method for guiding and controlling drone using information for controlling camera of drone | |
US6529821B2 (en) | Route planner with area avoidance capability | |
CN112904842A (zh) | 一种基于代价势场的移动机器人路径规划与优化方法 | |
KR20180100194A (ko) | 경로 이탈 인식 방법, 단말 및 저장 매체 | |
CN109656264B (zh) | 用于为飞行器生成到着陆地点的3d路径的计算机实施的方法和*** | |
US20130325222A1 (en) | Automatic recovery method for an unmanned aerial vehicle | |
US20110288773A1 (en) | Loss of separation avoidance maneuvering | |
EP4130668A1 (en) | Method for determining lane line recognition abnormal event, and lane line recognition apparatus and system | |
WO2021072709A1 (zh) | 目标检测与跟踪方法、***、设备及存储介质 | |
JP2020076745A (ja) | 映像及びマップデータ基盤測位方法及び装置 | |
CN116740667B (zh) | 路口面数据生成方法、装置、电子设备及存储介质 | |
CN107289945A (zh) | 一种无人机导航方法 | |
CN111665508A (zh) | 直升机载地形跟随与回避可视化导航***以及导航方法 | |
Duraisamy et al. | Track level fusion of extended objects from heterogeneous sensors | |
US9410807B2 (en) | Method for determining an offset lateral trajectory for an aircraft | |
JP2021062653A (ja) | 軌道生成装置、軌道生成方法及び軌道生成プログラム | |
JP6713781B2 (ja) | パッシブ測位システム及びマルチセンサシステム | |
CN115016546B (zh) | 无人机三维路径规划方法、装置、电子设备和存储介质 | |
CN112622924A (zh) | 一种驾驶规划方法及装置、车辆 | |
US20140297183A1 (en) | Method and device for calculating predictions on a flight plan of an aircraft | |
CN110525670A (zh) | 管理飞行器的飞行剖面显示的方法和电子设备、电子*** |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |