CN106134434B - 一种用于车载导航定位系统位置误差修正的轨迹匹配方法 - Google Patents

Abstract

本发明实施例公开了一种用于车载导航定位系统位置误差修正的轨迹匹配方法，包括提取轨迹特征段；对提取的轨迹特征段进行特征段匹配；根据匹配结果对车辆轨迹修正。本发明利用发射车行驶过程中获取的轨迹点数据和预先获得的路网数据对发射车的行驶轨迹进行匹配，利用获得的发射车轨迹与路网数据的差异值对发射车的轨迹进行修正，从而实现发射车的自主导航。

Description

一种用于车载导航定位系统位置误差修正的轨迹匹配方法

技术领域

本发明涉及一种用于车载导航定位系统位置误差修正的轨迹匹配方法。

背景技术

导弹机动发射是提高导弹生存能力的重要途径。导弹武器实施机动发射时，发射车运动过程中对导弹进行实时自主地定位是导弹命中目标的关键所在。

随着我国导弹研制进程的不断推进，机动发射成为我国导弹技术发展的重要方向，因此能够进行实时、自主定位的发射平台成为导弹发射过程中的关键点。惯性导航系统能够完全依靠其自身设备自主地完成导航任务，不与外界发生任何光、电联系，并且工作条件不受环境因素限制，是一种自主性强、全天候、全参数的导航系统，惯性导航系统的采用对导弹完成机动发射具有重要意义。但是惯性导航系统的导航误差随时间积累而发散，因此必须将惯性导航系统与其它辅助导航系统进行组合，从而补偿惯性导航系统的导航误差，实现导弹机动发射平台的实时的自主定位。

目前常用的组合导航系统包括惯性/卫星、惯性/地磁、惯性/地形匹配等组合导航系统。车辆运动过程中卫星信息解算可能出现延迟，在不同时间段卫星辅助导航精度也存在差异，而地磁、地形本身的测量精度就不高，因此组合导航的精度有限。

随着测量技术的发展，目前对道路进行测量的精度可以达到厘米级，因此通过行使轨迹与道路进行匹配的组合导航在精度上具有先天的优势。

发明内容

本发明的技术解决问题是：本发明提供了一种用于车载导航定位系统位置误差修正的轨迹匹配方法，本发明利用发射车行驶过程中获取的轨迹点数据和预先获得的路网数据对发射车的行驶轨迹进行匹配，利用获得的发射车轨迹与路网数据的差异值对发射车的轨迹进行修正，从而实现发射车的自主导航。

本发明的技术解决方案是：

一种用于车载导航定位系统位置误差修正的轨迹匹配方法，包括以下步骤：

步骤1：提取轨迹特征段，所述轨迹特征段为车辆行驶过程中满足特征提取条件的连续轨迹点的集合；

步骤2：轨迹特征段匹配，确定轨迹特征段覆盖范围内的道路特征段；利用轨迹特征段中轨迹点与所述轨迹点在道路特征段上的投影点的距离均值对轨迹特征段进行匹配；

步骤3：车辆轨迹修正，确定步骤2中匹配完成的轨迹特征段相对于步骤1中轨迹特征段的差异值，将所述差异值作为位置误差对车辆轨迹进行修正。

所述步骤1中提取轨迹特征段步骤如下：

1a：按周期获取一个轨迹点，将该轨迹点对应的行驶状态作为车辆的跟踪状态，添加到跟踪状态列表；所述行驶状态可分为：直行状态、顺时针转弯状态和逆时针转弯状态；

1b：若下一个轨迹点的行驶状态不为直行状态且不同于跟踪状态，则获得对车辆的跟踪子状态，若跟踪子状态中轨迹点满足特征提取条件，提取跟踪状态列表中的轨迹点作为一个轨迹特征段；否则将下一个轨迹点添加到跟踪状态列表；所述特征提取条件为：判断每个周期获取的轨迹点的转弯角度是否大于特征提取角度；

1c：若当前跟踪状态为直行状态，则提取跟踪状态列表中的轨迹点作为一个轨迹特征段；否则返回步骤1b。

所述步骤2中对轨迹特征段进行匹配的方法为：

计算所有轨迹点与对应投影点间距离的均值；

根据均值将轨迹特征段向道路特征段进行平移；

若平移后的轨迹特征段中轨迹点与对应投影点间距离的均方差满足匹配精度，则完成对轨迹特征段的匹配；否则，利用平移后的轨迹特征段中轨迹点与在平移后轨迹特征段所覆盖内的道路特征段上的投影点间距离的均值，对轨迹特征段再次进行平移。

本发明与现有技术相比具有如下优点：

本发明通过将车载的导航的轨迹特征同道路路网数据进行匹配，从而可获取当前自主导航的定位误差，能够有效抑制导弹发射平台在机动过程中定位误差的发散，为导弹机动发射平台提供高精度的位置信息。提高了机动导弹发射准确度，有效缩短了导弹发射的准备时间。同时本发明提出的匹配方法只依赖于导弹发射平台自身存储的路网数据和其自身导航系统所产生的轨迹数据，而不依赖于其它外部系统和设备，因此不会改变导航发射平台导航的自主性。

附图说明

图1为本发明流程图；

图2为轨迹特征段外接矩形示意图；

图3为轨迹特征段匹配流程图。

具体实施方式

下面就结合附图对本发明做进一步介绍。

本发明实施例首先根据车载自主导航定位系统提供的定位数据提取车辆运动轨迹的特征段，所述特征段是指车辆行驶过程中行驶方向变化明显的一段轨迹。主要同车辆直线行驶和车辆缓慢转弯两种状态相区别，然后利用路径匹配算法将车辆运动轨迹的特征段与路网数据进行匹配，最后计算车辆运动轨迹与路网数据之间的位置误差，进而对发射车的轨迹进行修正。具体步骤如图1所示。

1、提取轨迹特征段

车辆行驶过程中，可以通过车载惯性导航平台或捷联惯组等设备获得行驶过程中的轨迹点数据。所述轨迹点数据包括经度、纬度和高程以及用于实现车辆自主导航的航向角信息。接收轨迹点数据的具体方法可以通过串口通信，直接集成等方法实现。连续的轨迹点数据可以构成用于对车辆行驶轨迹进行匹配的特征段。特征提取条件可通过航向角变化值和航向角变化率进行确定。

用于获取轨迹点数据的惯性导航设备的误差可以分解为北向误差和东向误差，也可以分解为沿道路行进方向的径向误差和垂直道路行进方向的法向误差。如果对直线特征进行匹配，只能发现垂直于道路行进方向的法向误差，而无法得到沿道路行进方向的径向误差，因此需要在航向具有一定程度变化的路段进行匹配。另一方面为了提高匹配的计算效率，只考虑对车载自主导航系统的定位误差进行平移变换，忽略旋转变换及累积误差，因此上述特征提取条件应该满足：

(1)特征段内轨迹点的航向角变化必须大于预设的航向角变化值，即从特征段开始到特征段结束，航向角变化必须大于一定的角度(角度的大小需要根据实际的自主导航系统进行确定，在本实施例中依经验取值45°)。

(2)特征段转弯的角度不能过于平缓。即从特征段开始到特征段结束，航向特征段转弯过程的航向角速率速度必须大于一定的阈值。(航向角变化率根据实际的自主导航系统进行确定，本实施例中依经验定为4°/s)。主要是因为当特征段航向角速度太小时，转弯过于平缓，为了满足条件(1)，提取的特征段就会过长。

上述条件对导航轨迹特征段进行了限定。但是在实际的自主导航轨迹中还会出现满足上述条件但不属于轨迹特征的情况需要进行剔除。主要包括两种情况：(1)车辆紧急避让。车辆在行驶过程中紧急避让障碍物产生的轨迹变化。(2)自主导航平台自身的随机误差产生的轨迹变化。

根据当前轨迹点的航向角信息，将车辆的行驶状态分为三类：直行状态、顺时针转弯状态和逆时针转弯。如果当前的行驶状态发生改变，则表明发现了一个特征；对直线特征不进行处理，对顺时针转弯和逆时针转弯的特征进行提取。在特征提取过程中为避免自主导航数据的随机误差或者紧急避让产生的行驶状态改变对特征提取的影响，引入跟踪子状态的概念。即当跟踪状态发生改变时并不立即进行特征提取，而是将这种改变作为当前跟踪状态的一种子状态。

为了避免提取的特征段过长而无法满足算法的要求，在提取过程中根据时间间隔对特征提取过程进行控制，在规定的时间段内行驶航向角的变化量小于一定值时结束当前特征的跟踪，开始新的跟踪。

下述为轨迹特征段的提取过程：

步骤1a：每个周期接收一个轨迹点的信息，根据航向角的变化方向记录当前跟踪状态，并将与当前跟踪状态相反的状态设置为跟踪子状态，同时将该轨迹点作为跟踪状态列表中的一个轨迹点进行记录，即如果当前跟踪状态是逆时针转弯，子状态则为顺时针转弯，反之亦然。

步骤1b：接收下一个轨迹点信息，如果该轨迹点的行进状态与当前跟踪状态的子状态相同，计算当前子状态的转弯角度，如果跟踪子状态中轨迹点的转弯角度大于特征提取角度(本实施例选15°或者当前跟踪子状态的转弯角度大于当前跟踪状态转弯角度的1/4)，则将跟踪状态列表中的轨迹点作为一个轨迹特征进行提取。如果该轨迹点的行进状态与当前跟踪状态的子状态不同，则将该轨迹点添加到跟踪状态列表，转入步骤1c作进一步判断。

步骤1c，判断当前跟踪状态是否为直行状态，在本实施例中，将执行状态定义为持续时间大于3秒并且角度变化量不超过3°的一端轨迹点集合，则将当前跟踪状态作为提取到的一个轨迹特征；若当前跟踪状态非直行状态，则返回步骤1b。

2、轨迹特征段匹配

如图3为轨迹特征段匹配流程图。利用提取出的特征段，首先确定轨迹特征段覆盖的空间范围。即如图2所示计算轨迹特征段的最小外接矩形(最小外接矩形即能够包含该特征段的最小矩形，其提取方法为公知常识)，在该矩形的基础上向外各扩充1/4作为轨迹特征段覆盖的空间范围。并将上述轨迹特征段覆盖范围内的路网数据，作为用于对轨迹特征段进行匹配的道路特征段。

计算轨迹特征段上轨迹点沿前进方向的法向量在备选特征路段上的投影点，并计算轨迹点与投影点之间的距离，如果无法投影到备选特征路段上，认为该轨迹点与投影点间的距离为其它点距离的均值。计算所有轨迹点与对应投影点间距离的均值，并将该均值作为平移量对轨迹特征段进行整体平移。

平移完成后重新计算轨迹特征段中轨迹点到对应投影点的均方差，将本次均方差与上次均方差进行比较，如果两者之差满足匹配精度，则完成对轨迹特征段的匹配。否则，利用重新确定轨迹特征段覆盖范围内的道路特征段，进一步重复上述步骤对轨迹特征段进行平移，直到均方差的差值满足匹配精度。

对均方差的计算实例如下：

假设当前轨迹点的坐标为(x₀，y₀)，行驶轨迹以二次曲线y＝ax²+bx+c为例，其中c为常数)

设F(x，y)＝ax²+bx+c-y，则有

法向量：即与该点切向量垂直的向量

$\stackrel{\→}{n}=\left(F_X\right(x),F_Y(y\left)\right)$

即

投影点：即该点法线与备选道路曲线的交点，设为(x_i，y_i)

$\left\{\begin{array}{c}y={\mathrm{ax}}^2+bx+c......\left(1\right)\\ y=-\frac{1}{2{\mathrm{ax}}_0+b}(x-x_0)+y_0\end{array}\right.\mathrm{...}\left(2\right)$

其中，上述方程组的(2)式代表投影点的法线；

投影点距离：即投影点与当前轨迹点的距离

$d=\sqrt{{(x_0-x_t)}^2+{(y_0-y_t)}^2}$

距离均值：即该范围内所有投影点距离(d_i，i＝1，2，…n)的平均值

$\stackrel{\‾}{d}=\underset{i=1}{\overset{n}{\Σ}}\frac{d_1+d_2+d_3+...+d_n}{n}$

均方差：即该范围内所有投影点距离偏离距离均值的距离平均数的方根

$\mathrm{\σ}=\sqrt{\frac{{(d_1-\stackrel{\‾}{d})}^2+{(d_2-\stackrel{\‾}{d})}^2+{(d_3-\stackrel{\‾}{d})}^2+\mathrm{...}+{(d_n-\stackrel{\‾}{d})}^2}{n}}$

3、车辆轨迹修正

完成对轨迹特征段的匹配后，将对轨迹特征段的各次平移量作为匹配后轨迹特征段与确定的轨迹特征段的差异值，将该差异值作为位置误差，对车辆轨迹进行修正。

本发明未详细说明部分属本领域技术人员公知常识。

Claims (1)

1.一种用于车载导航定位系统位置误差修正的轨迹匹配方法，其特征在于包括以下步骤：

步骤1：提取轨迹特征段，所述轨迹特征段为车辆行驶过程中满足特征提取条件的连续轨迹点的集合；

步骤2：轨迹特征段匹配，确定轨迹特征段覆盖范围内的道路特征段；利用轨迹特征段中轨迹点与所述轨迹点在道路特征段上的投影点的距离均值对轨迹特征段进行匹配；

步骤3：车辆轨迹修正，确定步骤2中匹配完成的轨迹特征段相对于步骤1中轨迹特征段的差异值，将所述差异值作为位置误差对车辆轨迹进行修正；

所述步骤1中提取轨迹特征段步骤如下：

1a：按周期获取一个轨迹点，将该轨迹点对应的行驶状态作为车辆的跟踪状态，添加到跟踪状态列表；所述行驶状态可分为：直行状态、顺时针转弯状态和逆时针转弯状态；

1b：若下一个轨迹点的行驶状态不为直行状态且不同于跟踪状态，则获得对车辆的跟踪子状态，若跟踪子状态中轨迹点满足特征提取条件，提取跟踪状态列表中的轨迹点作为一个轨迹特征段；否则将下一个轨迹点添加到跟踪状态列表；所述特征提取条件为：判断每个周期获取的轨迹点的转弯角度是否大于特征提取角度；

1c：若当前跟踪状态为直行状态，则提取跟踪状态列表中的轨迹点作为一个轨迹特征段；否则返回步骤1b；

所述步骤2中对轨迹特征段进行匹配的方法为：

计算所有轨迹点与对应投影点间距离的均值；

根据均值将轨迹特征段向道路特征段进行平移；

若平移后的轨迹特征段中轨迹点与对应投影点间距离的均方差满足匹配精度，则完成对轨迹特征段的匹配；否则，利用平移后的轨迹特征段中轨迹点与在平移后轨迹特征段所覆盖内的道路特征段上的投影点间距离的均值，对轨迹特征段再次进行平移。