CN106134434B - 一种用于车载导航定位***位置误差修正的轨迹匹配方法 - Google Patents
一种用于车载导航定位***位置误差修正的轨迹匹配方法Info
- Publication number
- CN106134434B CN106134434B CN201218000665.4A CN201218000665A CN106134434B CN 106134434 B CN106134434 B CN 106134434B CN 201218000665 A CN201218000665 A CN 201218000665A CN 106134434 B CN106134434 B CN 106134434B
- Authority
- CN
- China
- Prior art keywords
- track
- characteristic section
- track characteristic
- tracing point
- section
- 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.)
- Expired - Fee Related
Links
Landscapes
- Navigation (AREA)
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作为轨迹特征段覆盖的空间范围。并将上述轨迹特征段覆盖范围内的路网数据,作为用于对轨迹特征段进行匹配的道路特征段。
计算轨迹特征段上轨迹点沿前进方向的法向量在备选特征路段上的投影点,并计算轨迹点与投影点之间的距离,如果无法投影到备选特征路段上,认为该轨迹点与投影点间的距离为其它点距离的均值。计算所有轨迹点与对应投影点间距离的均值,并将该均值作为平移量对轨迹特征段进行整体平移。
平移完成后重新计算轨迹特征段中轨迹点到对应投影点的均方差,将本次均方差与上次均方差进行比较,如果两者之差满足匹配精度,则完成对轨迹特征段的匹配。否则,利用重新确定轨迹特征段覆盖范围内的道路特征段,进一步重复上述步骤对轨迹特征段进行平移,直到均方差的差值满足匹配精度。
对均方差的计算实例如下:
假设当前轨迹点的坐标为(x0,y0),行驶轨迹以二次曲线y=ax2+bx+c为例,其中c为常数)
设F(x,y)=ax2+bx+c-y,则有
法向量:即与该点切向量垂直的向量
即
投影点:即该点法线与备选道路曲线的交点,设为(xi,yi)
其中,上述方程组的(2)式代表投影点的法线;
投影点距离:即投影点与当前轨迹点的距离
距离均值:即该范围内所有投影点距离(di,i=1,2,…n)的平均值
均方差:即该范围内所有投影点距离偏离距离均值的距离平均数的方根
3、车辆轨迹修正
完成对轨迹特征段的匹配后,将对轨迹特征段的各次平移量作为匹配后轨迹特征段与确定的轨迹特征段的差异值,将该差异值作为位置误差,对车辆轨迹进行修正。
本发明未详细说明部分属本领域技术人员公知常识。
Claims (1)
1.一种用于车载导航定位***位置误差修正的轨迹匹配方法,其特征在于包括以下步骤:
步骤1:提取轨迹特征段,所述轨迹特征段为车辆行驶过程中满足特征提取条件的连续轨迹点的集合;
步骤2:轨迹特征段匹配,确定轨迹特征段覆盖范围内的道路特征段;利用轨迹特征段中轨迹点与所述轨迹点在道路特征段上的投影点的距离均值对轨迹特征段进行匹配;
步骤3:车辆轨迹修正,确定步骤2中匹配完成的轨迹特征段相对于步骤1中轨迹特征段的差异值,将所述差异值作为位置误差对车辆轨迹进行修正;
所述步骤1中提取轨迹特征段步骤如下:
1a:按周期获取一个轨迹点,将该轨迹点对应的行驶状态作为车辆的跟踪状态,添加到跟踪状态列表;所述行驶状态可分为:直行状态、顺时针转弯状态和逆时针转弯状态;
1b:若下一个轨迹点的行驶状态不为直行状态且不同于跟踪状态,则获得对车辆的跟踪子状态,若跟踪子状态中轨迹点满足特征提取条件,提取跟踪状态列表中的轨迹点作为一个轨迹特征段;否则将下一个轨迹点添加到跟踪状态列表;所述特征提取条件为:判断每个周期获取的轨迹点的转弯角度是否大于特征提取角度;
1c:若当前跟踪状态为直行状态,则提取跟踪状态列表中的轨迹点作为一个轨迹特征段;否则返回步骤1b;
所述步骤2中对轨迹特征段进行匹配的方法为:
计算所有轨迹点与对应投影点间距离的均值;
根据均值将轨迹特征段向道路特征段进行平移;
若平移后的轨迹特征段中轨迹点与对应投影点间距离的均方差满足匹配精度,则完成对轨迹特征段的匹配;否则,利用平移后的轨迹特征段中轨迹点与在平移后轨迹特征段所覆盖内的道路特征段上的投影点间距离的均值,对轨迹特征段再次进行平移。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201218000665.4A CN106134434B (zh) | 2012-03-31 | 2012-03-31 | 一种用于车载导航定位***位置误差修正的轨迹匹配方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201218000665.4A CN106134434B (zh) | 2012-03-31 | 2012-03-31 | 一种用于车载导航定位***位置误差修正的轨迹匹配方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN106134434B true CN106134434B (zh) | 2014-10-22 |
Family
ID=57249956
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201218000665.4A Expired - Fee Related CN106134434B (zh) | 2012-03-31 | 2012-03-31 | 一种用于车载导航定位***位置误差修正的轨迹匹配方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN106134434B (zh) |
Cited By (13)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105841708A (zh) * | 2016-03-16 | 2016-08-10 | 佛山科学技术学院 | 一种基于路径追踪的车载导航定位轨迹匹配方法 |
CN106597496A (zh) * | 2016-12-09 | 2017-04-26 | 东软集团股份有限公司 | 位置修正方法及相关设备 |
CN108303075A (zh) * | 2017-12-29 | 2018-07-20 | 广州斯马特信息科技有限公司 | 轨迹生成方法和*** |
CN109798898A (zh) * | 2019-01-23 | 2019-05-24 | 衢州学院 | 一种基于人工智能的移动定位方法及移动装置 |
CN110058279A (zh) * | 2019-04-15 | 2019-07-26 | 北京三快在线科技有限公司 | 一种确定已行驶路径的方法、装置、设备及存储介质 |
CN110567463A (zh) * | 2019-08-16 | 2019-12-13 | 中国人民解放军63921部队 | 电子地图的匹配方法和设备 |
CN111102991A (zh) * | 2019-11-28 | 2020-05-05 | 湖南率为控制科技有限公司 | 一种基于航迹匹配的初始对准方法 |
CN111323031A (zh) * | 2019-11-25 | 2020-06-23 | 北京嘀嘀无限科技发展有限公司 | 一种偏航识别方法、装置、电子设备及存储介质 |
CN111397604A (zh) * | 2020-06-03 | 2020-07-10 | 腾讯科技(深圳)有限公司 | 一种分析轨迹数据的方法、相关设备及存储介质 |
CN111857113A (zh) * | 2019-04-12 | 2020-10-30 | 北京地平线机器人技术研发有限公司 | 可移动设备的定位方法及定位装置 |
CN113177665A (zh) * | 2021-05-21 | 2021-07-27 | 福建盛海智能科技有限公司 | 一种提高循迹路线精度的方法及终端 |
CN113184423A (zh) * | 2019-04-01 | 2021-07-30 | 上海快仓智能科技有限公司 | 自动引导车的控制方法、自动引导车及货物搬运*** |
CN113447040A (zh) * | 2021-08-27 | 2021-09-28 | 腾讯科技(深圳)有限公司 | 行驶轨迹确定方法、装置、设备以及存储介质 |
-
2012
- 2012-03-31 CN CN201218000665.4A patent/CN106134434B/zh not_active Expired - Fee Related
Cited By (17)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105841708A (zh) * | 2016-03-16 | 2016-08-10 | 佛山科学技术学院 | 一种基于路径追踪的车载导航定位轨迹匹配方法 |
CN106597496A (zh) * | 2016-12-09 | 2017-04-26 | 东软集团股份有限公司 | 位置修正方法及相关设备 |
CN106597496B (zh) * | 2016-12-09 | 2020-02-11 | 东软集团股份有限公司 | 位置修正方法及相关设备 |
CN108303075A (zh) * | 2017-12-29 | 2018-07-20 | 广州斯马特信息科技有限公司 | 轨迹生成方法和*** |
CN109798898A (zh) * | 2019-01-23 | 2019-05-24 | 衢州学院 | 一种基于人工智能的移动定位方法及移动装置 |
CN113184423A (zh) * | 2019-04-01 | 2021-07-30 | 上海快仓智能科技有限公司 | 自动引导车的控制方法、自动引导车及货物搬运*** |
CN113184423B (zh) * | 2019-04-01 | 2023-06-20 | 上海快仓智能科技有限公司 | 自动引导车的控制方法、自动引导车及货物搬运*** |
CN111857113B (zh) * | 2019-04-12 | 2023-11-03 | 北京地平线机器人技术研发有限公司 | 可移动设备的定位方法及定位装置 |
CN111857113A (zh) * | 2019-04-12 | 2020-10-30 | 北京地平线机器人技术研发有限公司 | 可移动设备的定位方法及定位装置 |
CN110058279A (zh) * | 2019-04-15 | 2019-07-26 | 北京三快在线科技有限公司 | 一种确定已行驶路径的方法、装置、设备及存储介质 |
CN110567463A (zh) * | 2019-08-16 | 2019-12-13 | 中国人民解放军63921部队 | 电子地图的匹配方法和设备 |
CN111323031A (zh) * | 2019-11-25 | 2020-06-23 | 北京嘀嘀无限科技发展有限公司 | 一种偏航识别方法、装置、电子设备及存储介质 |
CN111102991A (zh) * | 2019-11-28 | 2020-05-05 | 湖南率为控制科技有限公司 | 一种基于航迹匹配的初始对准方法 |
CN111397604B (zh) * | 2020-06-03 | 2021-04-27 | 腾讯科技(深圳)有限公司 | 一种分析轨迹数据的方法、相关设备及存储介质 |
CN111397604A (zh) * | 2020-06-03 | 2020-07-10 | 腾讯科技(深圳)有限公司 | 一种分析轨迹数据的方法、相关设备及存储介质 |
CN113177665A (zh) * | 2021-05-21 | 2021-07-27 | 福建盛海智能科技有限公司 | 一种提高循迹路线精度的方法及终端 |
CN113447040A (zh) * | 2021-08-27 | 2021-09-28 | 腾讯科技(深圳)有限公司 | 行驶轨迹确定方法、装置、设备以及存储介质 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN106134434B (zh) | 一种用于车载导航定位***位置误差修正的轨迹匹配方法 | |
Wang et al. | Map-based localization method for autonomous vehicles using 3D-LIDAR | |
CN105841708A (zh) | 一种基于路径追踪的车载导航定位轨迹匹配方法 | |
CN103777220B (zh) | 基于光纤陀螺、速度传感器和gps的实时精确位姿估计方法 | |
Milanés et al. | Autonomous vehicle based in cooperative GPS and inertial systems | |
CN110702091B (zh) | 一种沿地铁轨道移动机器人的高精度定位方法 | |
CN108303103A (zh) | 目标车道的确定方法和装置 | |
CN107389064A (zh) | 一种基于惯性导航的无人车变道控制方法 | |
CN105270410A (zh) | 用于自主驾驶车辆的路径规划的精确曲率估计算法 | |
CN108020229B (zh) | 用于定位车辆的方法 | |
CN103453913A (zh) | 停车场中车辆的组合定位方法 | |
Vu et al. | Centimeter-accuracy smoothed vehicle trajectory estimation | |
WO2018072350A1 (zh) | 车辆轨迹预测方法及装置 | |
CN104535061A (zh) | 一种基于多传感器数据融合的导航*** | |
US11703344B2 (en) | Landmark location estimation apparatus and method, and computer-readable recording medium storing computer program programmed to perform method | |
CN109900273B (zh) | 一种室外移动机器人导引方法及导引*** | |
CN102494685B (zh) | 用于wsn/ins高精度实时组合导航的h无穷信息融合方法 | |
CN102788580A (zh) | 无人机视觉导航中的飞行路线合成方法 | |
CN103149580A (zh) | 一种基于stkf和wnn的gps/ins组合导航方法 | |
CN103759733B (zh) | 基于联邦滤波的ins/vkm/vdm车载导航*** | |
CN108627864A (zh) | 基于汽车钥匙的定位方法及***、无人驾驶汽车*** | |
CN106093992A (zh) | 一种基于cors的亚米级组合定位导航***及导航方法 | |
CN112805766A (zh) | 用于更新详细地图的装置和方法 | |
Stoll et al. | GPS-independent localization for off-road vehicles using ultra-wideband (UWB) | |
CN104316058A (zh) | 一种采用交互多模型的移动机器人wsn/ins组合导航方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
GR03 | Grant of secret patent right | ||
GRSP | Grant of secret patent right | ||
DC01 | Secret patent status has been lifted | ||
DCSP | Declassification of secret patent | ||
CF01 | Termination of patent right due to non-payment of annual fee | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20141022 Termination date: 20180331 |