CN113758483A - 一种自适应fkf地图匹配方法及系统 - Google Patents
一种自适应fkf地图匹配方法及系统 Download PDFInfo
- Publication number
- CN113758483A CN113758483A CN202111088145.8A CN202111088145A CN113758483A CN 113758483 A CN113758483 A CN 113758483A CN 202111088145 A CN202111088145 A CN 202111088145A CN 113758483 A CN113758483 A CN 113758483A
- Authority
- CN
- China
- Prior art keywords
- ins
- train
- data
- information
- sub
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 34
- 230000004927 fusion Effects 0.000 claims abstract description 77
- 238000004364 calculation method Methods 0.000 claims abstract description 24
- 238000007405 data analysis Methods 0.000 claims abstract description 6
- 238000013500 data storage Methods 0.000 claims abstract description 5
- 238000009434 installation Methods 0.000 claims abstract description 4
- 238000005259 measurement Methods 0.000 claims description 36
- 230000001133 acceleration Effects 0.000 claims description 31
- 230000007704 transition Effects 0.000 claims description 26
- 239000013598 vector Substances 0.000 claims description 26
- 239000011159 matrix material Substances 0.000 claims description 24
- 238000006073 displacement reaction Methods 0.000 claims description 18
- 238000004422 calculation algorithm Methods 0.000 claims description 15
- 230000003044 adaptive effect Effects 0.000 claims description 14
- 238000012937 correction Methods 0.000 claims description 10
- 238000011156 evaluation Methods 0.000 claims description 10
- 230000006870 function Effects 0.000 claims description 8
- 238000006243 chemical reaction Methods 0.000 claims description 4
- 238000004458 analytical method Methods 0.000 claims description 3
- 238000009795 derivation Methods 0.000 claims description 3
- 238000001914 filtration Methods 0.000 claims description 3
- 238000005070 sampling Methods 0.000 claims description 3
- 230000003068 static effect Effects 0.000 claims description 3
- 238000012546 transfer Methods 0.000 claims description 3
- 238000007500 overflow downdraw method Methods 0.000 claims description 2
- 238000005516 engineering process Methods 0.000 description 8
- 238000010586 diagram Methods 0.000 description 4
- 238000013461 design Methods 0.000 description 3
- 238000010276 construction Methods 0.000 description 2
- 206010063385 Intellectualisation Diseases 0.000 description 1
- 238000009825 accumulation Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000012512 characterization method Methods 0.000 description 1
- 230000000295 complement effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 238000004148 unit process Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
- G01C21/30—Map- or contour-matching
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/01—Satellite radio beacon positioning systems transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/13—Receivers
- G01S19/14—Receivers specially adapted for specific applications
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Navigation (AREA)
Abstract
本发明提出一种自适应FKF地图匹配方法及系统,1设备安装、系统初始化,惯导初始对准、卫星信号稳定性跟踪;2数据采集;3数据解析;4数据融合;5数据存储与输出;系统包括GNSS、IMU、里程计三个数据采集模块,以及卫星姿态解算单元、INS姿态解算单元、MM/DR航位推移单元和数据融合单元四个主要单元。有益效果:该系统将轨道电子地图信息与MM/DR地图航位推移单元相结合,实现轨道地图与传统组合定位系统的融合,实时修正位置与速度解算结果。
Description
技术领域
本发明属于列车导航定位领域,具体涉及到一种基于轨道电子地图跟踪匹配的列车定位方法。
背景技术
地图匹配即从基准图中根据特征匹配和识别,将存在误差的定位信息修正到相对可信的电子地图中,随着高精度轨道电子地图的构建,地图匹配技术在列车定位中的应用越来越广泛。由于列车运行在既定线路上,轨道电子地图对列车形成强约束,可以作为列车定位的辅助条件,参与车辆位置和速度解算,提高定位精度,在列车定位中有重要意义。
随着GNSS(Global Navigation Satellite System,全球卫星定位系统)技术的不断发展,卫星定位在铁路行业得到了广泛的应用,同时对列车定位产生了重要的影响,其连续、实时的特点可以为列车提供较为精确的速度位置信息,辅助列车运行控制系统,提高列车运行安全性和智能化。然而卫星信号受环境干扰严重,在卫星失锁等情况下无法提供准确的位置服务。
除了GNSS卫星定位和应答器等列车定位技术外,INS(Inertia NavigationSystem,惯性导航系统)、多普雷达等技术也可用于列车定位。惯导系统通过测量列车的加速度、角速度等参量,利用参考位置进行计算最终得到列车的速度位置信息,然而随着时间积累势必会引入误差,无法满足长时间精确定位的要求,对于列车定位而言,定位的精度和可靠性是两个重点要考虑的方面。
轨道电子地图将全球卫星定位系统、地理信息系统等先进技术进行融合,将含地理要素的空间信息与轨道信息相结合并应用于列车定位技术。这使得列车运行控制系统不仅能实时获取列车精确的位置、速度和时间等信息,还能正确判断列车所处状态并作为列车定位的重要补充手段。结合GIS技术的高精度数字轨道地图可以辅助实现准确定位,车载设备可以根据数字轨道地图的线路信息进行列车运行控制。利用地图匹配技术,不仅能够修正列车运行时的定位误差还能够提高列车运行控制系统的可靠性及稳定性。轨道电子地图中还能保存应答器、信号机、桥梁隧道等线路信息,使列车在运行中可以实时掌握线路数据信息。同时在辅助列车定位、铁路既有线路复测、扩能改造工程中对新建和改造轨道的地图更新,等方面有重要意义。
在列车定位系统中,GNSS卫信号星在开阔路段能提供高精度、实时的位置速度估计,但是其信号容易受到环境的干扰,行车过程中的遮挡区域其导航定位功能基本不可用;IMU惯性姿态信息能提供全自主的运动状态监测而无需任何外部信息,对姿态信息进行积分运算能实现位置速度和姿态的估计,但其误差随时间累积,且车辆震动等扰动情况对姿态信息干扰严重;电子地图能提供车辆运行的道路特征和行车轨迹信息,但无法独立完成定位功能,需提供外部位置、速度和航向等信息,用于实现航位推算。
发明内容
本发明目的是提供一种自适应FKF地图匹配方法。
本发明的技术方案:一种自适应FKF地图匹配方法:
Step 1:设备安装、系统初始化,惯导初始对准、卫星信号稳定性跟踪;
Step 2:数据采集,包括GNSS卫星信号、惯性导航姿态数据、里程计数据和轨道电子地图数据,GNSS卫星信号包括导航电文解算需要的星历数据、经纬度和高程位置数据、东-北-天向速度数据、卫星信号质量数据,惯性导航姿态数据主要包括加速度计和陀螺仪信息;
Step 3:数据解析阶段包括基于GNSS卫星信号的导航电文解析、惯性导航单元的INU姿态解算和基于里程计速度、轨道电子地图的MM/DR地图一维里程推移;
Step 4:数据融合,基于多源信息融合架构,将Step 3中所解析的卫星定位、惯性导航姿态数据、和地图一维里程推移数据进行融合与误差校正;
Step 5:数据存储与输出;
轨道电子地图的MM/DR地图一维里程推移,根据k时刻列车的行车速度vodo/k、在地图中的位移Sodo/k和平面坐标(xk,yk),在轨道电子地图中推算k+1时刻的位移Sodo/k+1和平面坐标(xk+1,yk+1),并转换到导航坐标系得到经纬度数据(λ,L),作为MM/DR导航信息参与GNSS/INS/MM多源信息融合解算;
多源信息融合方法:在轨道电子地图中,若已知列车的行车速度和加速度,则通过地图中的一维里程进行航位推移和位置、速度解算;车辆正常行驶时,其速度即车辆在车体坐标系下沿车体正前方向的速度,记k时刻速度为加速度为列车在时间段Δt=(tk-1-tk)内的里程增量为:
将k时刻的位置(Pe/k,Pv/k)转换成相对平面坐标(Xk,Yk),并作为MM/DR航位推移的起始坐标(Xk+1,Yk+1),根据轨道电子地图拓扑数据和一维里程增量,推算列车在k+1时刻的相对平面坐标(Xk+1,Yk+1);结合轨道线路特征,需分别针对轨道平面线形中的直线、圆曲线、缓和曲线三种线形进行推导;MM/DR航位推移流程如下:
Step 2:计算k时刻列车在匹配路段内的剩余里程;
Sres=L-Sk
Step 4:根据k+1时刻所在路段类型,计算下一时刻在轨道电子地图中的相对平面坐标;
若圆曲线沿列车行驶方向顺时针旋转,对应的极坐标减少,圆曲线沿列车行驶方向逆时针旋转,对应的极坐标增加,k+1时刻列车在轨道电子地图中的相对平面坐标为:
轨道平面线形是缓和曲线的,若列车位于缓和曲线中,则对缓和曲线建立子坐标系,其中原点为直缓点,第一象限为缓圆点一侧,k时刻列车距离子坐标系原点的位移为Sk,计算k+1时刻列车在子坐标系缓和曲线中的剩余位移量:
求解下列方程的唯一实数解得到列车k+1时刻在子坐标系下的坐标(xk+1,yk+1):
将子坐标系下的(xk+1,yk+1)从子坐标系转换到轨道电子地图的相对平面坐标系即可得到k+1时刻位置(Xk+1,Yk+1),进一步转换到导航坐标系,得到k+1时刻的导航坐标(Pe/k+1,Pv/k+1),完成轨道电子中的航位推移MM/DR;
为了计算k+1时刻列车的行车速度,首先将k时刻和k+1时刻位置信息转换到载体坐标系,分别记为(X′k,Y′k)和(X′k+1,Y′k+1),则列车速度在载体坐标系下为:
多源信息融合主要包括局部滤波器和主滤波器两部分,以全自主导航系统INS惯性导航系统作为参考系统,并包含两个子系统GNSS/INS和INS/MM,并分别对两个子滤波器采用标准KF算法进行数据融合;多源信息融合包括系统状态方程和量测方程建模,系统状态方程以INS系统的15维误差参数作为系统的状态向量X,包括导航坐标系“东-北-天”向下的3维失准角误差φ、3维速度误差δv、3维位置误差δp、载体坐标系“横-纵-天”向下的3维陀螺零漂ε和3维加计零偏
φ=[φE φN φU]
δv=[δvE δvN δvU]
δp=[δL δλ δh]
ε=[εx εy εz]
Z=[δvE δvN δvU δL δλ δh]
15维向量为融合系统局部滤波器的公共状态向量,系统状态方程如下:
X(k)=FX(k-1)+GW(k-1)
针对列车定位多源信息融合系统的两个子滤波器,分别计量测方程;
GNSS/INS子滤波器的量测方程以GNSS和INS的残差为量测向量,对应的量测方程如下:
其中,LINS、λINS、hINS、vINSe、vINSn、vINSu分别为INS惯性导航系统解算的纬度、经度、高程等位置信息和导航坐标系下的“东-北-天”向速度信息,LGNSS、λGNSS、hGNSS、vGNSSe、vGNSSn、vGNSSu分别为GNSS卫星定位系统的纬度、经度、高程三维位置信息与东北天向速度信息,HGNSS/INS和VGNSS/INSk分别为子滤波器的观测矩阵与量测噪声;
INS/MM子滤波器的量测方程以地图航位推移信息和INS的残差为量测向量,对应的量测方程如下:
上式中,LMM、λMM、hMM、vMMe、vMMn、vMMu为轨道电子地图航位推移的导航坐标系位置和速度数据,HINS/MM和VINS/MM分别为INS/MM子滤波器的观测矩阵与量测噪声。
滤波器之间的自适应信息分配策略,具体如下:
在各个子滤波器完成局部最优估计的基础上,多源信息融合系统将子滤波器结果传递给主滤波器,并根据信息分配策略实现子滤波器间的参数分配与信息反馈;主滤波器的状态向量为以下6维误差向量:
Xg=[δLINS δλINS δhINS δvINSe δvINSn δvINSu]T
主滤波器的状态方程和量测方程如下:
X、F、W、G、Z、H、V与前文含义相同,上标g代表主滤波器,k、k-1等代表迭代次数。
①信息分配
针对系统噪声Q,通过信息分配系数β将其分配到各个滤波器中,包括主滤波器和子滤波器,信息分配公式如下:
P、Q、β相关参数含义与上文相同
②子滤波器滤波
在完成基于标准KF的子滤波器局部滤波的基础上,得到GNSS/INS和MM/INS局部最优估计,并根据信息分配策略对自滤波器的误差参数进行修生,主要针对一步预测协方差矩阵的修正,G为标准KF中的噪声矩阵:
符号含义上文相同
③多源信息融合
符号含义与上文相同。
引入卫星信号评价因子τ,τ取值如下:
其中HDOP0和HDOP1分别为卫星信号好和卫星信号可用状态的经验阈值;
设计多源信息融合的信息匹配系数如下:
一种自适应FKF地图匹配系统,含有处理器和存储器,包括GNSS、IMU、里程计三个数据采集模块,包括以下四个单元,(1)卫星姿态解算单元:作用是在卫星信号可用的情况下为系统提供WG84坐标系下的经纬度位置数据、东北天速度数据、卫星信号质量数据等,是为系统提供绝对坐标的重要单元;
(2)INS姿态解算单元:作用是通过对加速度计、陀螺仪采集的车辆运动姿态信息进行处理,并结合INS/DR航位推移进行位置和速度解算;
(3)MM/DR航位推移单元:作用是根据里程计采集的列车行驶速度、里程信息,结合轨道电子地图,在轨道线路和路网拓扑中进行MM/DR地图航位推移,得到地图辅助信息;
(4)数据融合单元:用于实现基于GNSS/INS/MM多源信息的误差校正和位姿最优估计。
系统还包括局部滤波器和主滤波器两部分,以全自主导航系统INS惯性导航系统作为参考系统,并包含两个子系统GNSS/INS和INS/MM,并分别对两个子滤波器采用标准KF算法进行数据融合,主滤波器通过信息分配系数的调节实现误差校正和反馈,提高列车定位系统对行车环境的适应性。
本发明的有益效果:1.相对于传统的投影匹配等地图匹配方法而言,设计了一种GNSS/INS/MM列车组合定位平台,将轨道电子地图与GNSS、INS信息相结合,基于多源信息融合的基本思想实现列车运行场景下的高精度定位,具体包括卫星姿态解算、INS姿态解算、MM/DR航位推移、数据融合四个单元。
2.针对GNSS/INS/MM列车组合定位平台,设计了一种基于多源信息融合的跟踪匹配方法,在列车定位过程中,将轨道电子地图信息融合到实时的位置结算过程中,包括系统初始化、数据采集、数据解析、数据融合、数据存储与输出几个主要阶段。
3.提出一种适用于GNSS/INS/MM列车组合定位平台的地图航位推移模型,以列车里程计速度信息为基础,根据轨道电子地图中的直线、缓和曲线、圆曲线等平面线型与平面坐标系转换策略,实现MM/DR地图航位推算并解算得到导航坐标系下的位置速度信息。
4.提出一种基于自适应FKF的数据融合模型,包括局部滤波器和主滤波器两部分,其中局部滤波器包括GNSS/INS和INS/MM两个子滤波器。
5.对GNSS/INS/MM列车定位多源信息融合平台建模,设计了15维系统状态方程和GNSS/INS、INS/MM两个子滤波器的量测方程。
附图说明
图1 GNSS/INS/MM组合平台框图;
图2基于多源信息融合的跟踪匹配流程图;
图3基于自适应FKF的GNSS/INS/MM多源信息融合算法结构示意图。
具体实施方式
1、一种GNSS/INS/MM组合定位平台,
与传统的投影匹配不同,本发明所提出的GNSS/INS/MM组合定位平台基于多源信息融合的基本思想,将轨道电子地图与GNSS卫星定位信息、INS运动姿态信息相结合,实现基于地图匹配辅助的列车定位于轨迹跟踪方法,通过信息互补提高列车定位精度和系统对环境的适应性。组合定位平台主要包括以下四个单元,如附图1所示:
(1)卫星姿态解算单元:在卫星信号可用的情况下为系统提供WG84坐标系下的经纬度位置数据、东北天速度数据、卫星信号质量数据等,是为系统提供绝对坐标的重要单元。
(2)INS姿态解算单元:该单元通过对加速度计、陀螺仪采集的车辆运动姿态信息(主要包括三轴加速度信息和三轴角速度信息)进行处理,并结合INS/DR航位推移进行位置和速度解算。
(3)MM/DR航位推移单元:根据里程计采集的列车行驶速度、里程信息,结合轨道电子地图,在轨道线路和路网拓扑中进行MM/DR地图航位推移,得到地图辅助信息。
(4)数据融合单元:该单元是多源组合定位系统的核心单元,用于实现基于GNSS/INS/MM多源信息的误差校正和位姿最优估计,设计合适的多源数据融合算法是融合单元的关键。
2、一种基于多源信息融合的跟踪匹配模型,
设计附图2所示基于多源信息融合的跟踪匹配流程,主要包括以下几个步骤:
Step 1:系统初始化阶段,包括设备安装、惯导初始对准、卫星信号稳定性跟踪。
Step 2:数据采集,包括GNSS卫星信号、IMU惯性姿态信息、里程计数据和轨道电子地图数据,GNSS卫星信号包括导航电文解算需要的星历数据、经纬度和高程等位置数据、东-北-天向速度数据、卫星信号质量数据,IMU惯性导航单元姿态数据主要包括加速度计和陀螺仪信息,轨道电子地图根据专用地图构建方法进行处理,并分析地图结构。
Step 3:数据解析阶段包括基于GNSS卫星信号的导航电文解析、惯性导航单元的INU姿态解算和基于里程计速度和轨道电子地图的MM/DR地图一维里程推移。
Step 4:数据融合基于多源信息融合架构,将Step 3中所解析的卫星定位、惯导导航、和地图航位推移数据进行融合与误差校正。
Step 5:数据存储与输出。
基于该GNSS/INS/MM组合定位方法从以下两个方面进行分析:
(1)轨道电子地图中的一维里程推移算法MM/DR,根据k时刻列车的行车速度vodo/k、在地图中的位移Sodo/k和平面坐标(xk,yk),,在轨道电子地图中推算k+1时刻的位移Sodo/k+1和平面坐标(xk+1,yk+1),并转换到导航坐标系得到经纬度数据(λ,L),作为MM/DR导航信息参与GNSS/INS/MM多源信息融合解算;
(2)根据三种主要导航信息在不同场景下的特征,设计融合算法进行互补融合,提高列车定位精度。
3、一种轨道电子地图中的地图航位推移模型,
在轨道电子地图中,若已知列车的行车速度和加速度,则可以通过地图中的一维里程进行航位推移和位置、速度解算。车辆正常行驶时,其速度即车辆在车体坐标系下沿车体正前方向的速度,记k时刻速度为加速度为列车在时间段Δt=(tk-1-tk)内的里程增量为:
将k时刻的位置(Pe/k,Pv/k)转换成相对平面坐标(Xk,Yk),并作为航位推移(MM/DR)的起始坐标(Xk+1,Yk+1),根据轨道电子地图拓扑数据和一维里程增量,推算列车在k+1时刻的相对平面坐标(Xk+1,Yk+1)。结合轨道线路特征,需分别针对轨道平面线形中的直线、圆曲线、缓和曲线三种线形进行推导。MM/DR航位推移流程如下:
Step 2:计算k时刻列车在匹配路段内的剩余里程。
Sres=L-Sk
Step 4:根据k+1时刻所在路段类型,计算下一时刻在轨道电子地图中的相对平面坐标。
(1)直线
(2)圆曲线
若圆曲线沿列车行驶方向顺时针旋转,对应的极坐标减少,否则增加。k+1时刻列车在轨道电子地图中的相对平面坐标为:
(3)缓和曲线
若列车位于缓和曲线中,则对缓和曲线建立子坐标系,其中原点为直缓点,第一象限为缓圆点一侧,k时刻列车距离子坐标系原点的位移为Sk,计算k+1时刻列车在子坐标系缓和曲线中的剩余位移量:
求解下列方程的唯一实数解得到列车k+1时刻在子坐标系下的坐标(xk+1,yk+1):
将子坐标系下的(xk+1,yk+1)从子坐标系转换到轨道电子地图的相对平面坐标系即可得到k+1时刻位置(Xk+1,Xk+1),进一步转换到导航坐标系,得到k+1时刻的导航坐标(Pe/k+1,Pv/k+1),完成轨道电子中的航位推移MM/DR。
为了计算k+1时刻列车的行车速度,首先将k时刻和k+1时刻位置信息转换到载体坐标系,分别记为(X′k,Y′k)和(X′k+1,Y′k+1),则列车速度在载体坐标系下为:
4、一种基于自适应FKF的列车定位多源信息融合系统,
根据融合滤波器的基本原理,设计基于FKF的GNSS/INS/MM列车定位多源信息融合系统架构,如附图3所示。
该融合系统主要包括局部滤波器和主滤波器两部分,以全自主导航系统INS惯性导航系统作为参考系统,并包含两个子系统GNSS/INS和INS/MM,并分别对两个子滤波器采用标准KF算法进行数据融合,主滤波器通过信息分配系数的调节实现误差校正和反馈,提高列车定位系统对行车环境的适应性。
5、GNSS/INS/MM列车定位多源信息融合系统建模,
包括系统状态方程和量测方程建模,其特征在于:
(1)系统状态方程
因基于GNSS/INS/MM的列车定位多源信息融合系统中,参考系统为INS惯性导航系统,故以INS系统的15维误差参数作为系统的状态向量X,包括导航坐标系“东-北-天”向下的3维失准角误差φ、3维速度误差δv、3维位置误差δp、载体坐标系“横-纵-天”向下的3维陀螺零漂ε和3维加计零偏
φ=[φE φN φU]
δv=[δvE δvN δvU]
δp=[δL δλ δh]
ε=[εx εy εz]
Z=[δvE δvN δvU δL δλ δh]
因列车定位多源信息融合系统中INS为GNSS/INS与INS/MM两个子系统的公共子系统,故上述15维向量为融合系统局部滤波器的公共状态向量,系统状态方程如下:
X(k)=FX(k-1)+GW(k-1)
其中F和Wk-1分别为系统状态转移矩阵和噪声向量。
(2)系统量测方程
针对列车定位多源信息融合系统的两个子滤波器,分别计量测方程。
GNSS/INS子滤波器的量测方程以GNSS和INS的残差为量测向量,对应的量测方程如下:
其中,LINS、λINS、hINS、vINSe、vINSn、vINSu分别为INS惯性导航系统解算的纬度、经度、高程等位置信息和导航坐标系下的东北天向速度信息,LGNSS、λGNSS、hGNSS、vGNSSe、vGNSSn、vGNSSu分别为GNSS卫星定位系统的位置与速度信息,HGNSS/INS和VGNSS/INSk分别为子滤波器的观测矩阵与量测噪声。
INS/MM子滤波器的量测方程以地图航位推移信息和INS的残差为量测向量,对应的量测方程如下:
上式中,LMM、λMM、hMM、vMMe、vMMn、vMMu为轨道电子地图航位推移的导航坐标系位置和速度数据,HINS/MM和VINS/MM分别为INS/MM子滤波器的观测矩阵与量测噪声。
6、自适应信息分配的主滤波器设计
列车定位多源信息融合系统进行主滤波器设计,滤波器之间的自适应信息分配策略,具体如下:
在各个子滤波器完成局部最优估计的基础上,多源信息融合平台将子滤波器结果传递给主滤波器,并根据信息分配策略实现子滤波器间的参数分配与信息反馈。主滤波器的状态向量为以下6维向量:
Xg=[δLINS δλINS δhINS δvINSe δvINSn δvINSu]T
主滤波器的状态方程和量测方程如下:
基于上述子滤波器和主滤波器,进一步设计针对列车定位多源信息融合系统的FKF多源信息融合算法,包括以下3个方面:
①信息分配
针对系统噪声Q,通过信息分配系数β将其分配到4中所述列车定位多源信息融合系统的各个滤波器中,包括主滤波器和子滤波器,信息分配公式如下:
②子滤波器滤波
在完成基于标准KF的子滤波器局部滤波的基础上,得到GNSS/INS和MM/INS局部最优估计,并根据信息分配策略对自滤波器的误差参数进行修生,主要针对一步预测协方差矩阵的修正,G为标准KF中的噪声矩阵:
③多源信息融合
在主滤波器设计中,信息分配策略是影响最优估计效果的关键,针对传统平均分配等策略下的定位系统不能实现对行车环境的自适应等问题,设计自适应分配算法,根据列车行驶环境下不同信号的特性进行,对信息分配系数β进行自适应调整,提高系统对行车环境的适应性。
在上述的列车定位多源信息融合系统中,两个子滤波器均包含SINS信息,其差异主要在于GNSS卫星信号和MM轨道电子地图航位推移信息。GNSS卫星信号受可信度受卫星信号质量影响较大,其表征指标主要为HDOP参数,故引入卫星信号评价因子τ,根据卫星信号失锁、干扰、可用等状态,τ取值如下:
其中HDOP0和HDOP1分别为卫星信号好和卫星信号可用的经验阈值。
图1为组合定位平台框图,包括GNSS、IMU、里程计三个数据采集模块,以及卫星姿态解算单元、INS姿态解算单元、MM/DR航位推移单元和数据融合单元四个主要单元。该平台将轨道电子地图信息与MM/DR地图航位推移单元相结合,实现地图与组合定位系统的结合,实时修正位置与速度解算结果。
图2为基于多源信息融合的跟踪匹配流程图,该流程图根据组合定位平台设计,包括系统初始化、数据采集、数据解析、数据融合、数据输出与存储5个步骤。
图3为基于自适应FKF的列车多源信息融合算法结构示意图,该融合算法的输入包括GNSS卫星信号、INS姿态信息、轨道电子地图三组信息,航位推移MM/DR三组信息,以SINS惯性导航系统为参考系统,含GNSS/INS、INS/MM两个子系统,分别对应两个局部滤波器,并包含一个主滤波器,附图中虚线为反馈参数。
Claims (7)
1.一种自适应FKF地图匹配方法,其特征在于:
Step 1:设备安装、系统初始化,惯导初始对准、卫星信号稳定性跟踪;
Step 2:数据采集,包括GNSS卫星信号、惯性导航姿态数据、里程计数据和轨道电子地图数据,GNSS卫星信号包括导航电文解算需要的星历数据、经纬度和高程位置数据、东-北-天向速度数据、卫星信号质量数据,惯性导航姿态数据主要包括加速度计和陀螺仪信息;
Step 3:数据解析阶段包括基于GNSS卫星信号的导航电文解析、惯性导航单元的INU姿态解算和基于里程计速度、轨道电子地图的MM/DR地图一维里程推移;
Step 4:数据融合,基于多源信息融合架构,将Step 3中所解析的卫星定位、惯性导航姿态数据、和地图一维里程推移数据进行融合与误差校正;
Step 5:数据存储与输出;
轨道电子地图的MM/DR地图一维里程推移,根据k时刻列车的行车速度vodo/k、在地图中的位移Sodo/k和平面坐标(xk,yk),在轨道电子地图中推算k+1时刻的位移Sodo/k+1和平面坐标(xk+1,yk+1),并转换到导航坐标系得到经纬度数据(λ,L),作为MM/DR导航信息参与GNSS/INS/MM多源信息融合解算;
多源信息融合方法:在轨道电子地图中,若已知列车的行车速度和加速度,则通过地图中的一维里程进行航位推移和位置、速度解算;车辆正常行驶时,其速度即车辆在车体坐标系下沿车体正前方向的速度,记k时刻速度为加速度为列车在时间段Δt=(tk-1-tk)内的里程增量为:
将k时刻的位置(Pe/k,Pv/k)转换成相对平面坐标(Xk,Yk),并作为MM/DR航位推移的起始坐标(Xk+1,Yk+1),根据轨道电子地图拓扑数据和一维里程增量,推算列车在k+1时刻的相对平面坐标(Xk+1,Yk+1);结合轨道线路特征,需分别针对轨道平面线形中的直线、圆曲线、缓和曲线三种线形进行推导;MM/DR航位推移流程如下:
Step 2:计算k时刻列车在匹配路段内的剩余里程;
Sres=L-Sk
Step 4:根据k+1时刻所在路段类型,计算下一时刻在轨道电子地图中的相对平面坐标;
若圆曲线沿列车行驶方向顺时针旋转,对应的极坐标减少,圆曲线沿列车行驶方向逆时针旋转,对应的极坐标增加,k+1时刻列车在轨道电子地图中的相对平面坐标为:
轨道平面线形是缓和曲线的,若列车位于缓和曲线中,则对缓和曲线建立子坐标系,其中原点为直缓点,第一象限为缓圆点一侧,k时刻列车距离子坐标系原点的位移为Sk,计算k+1时刻列车在子坐标系缓和曲线中的剩余位移量:
求解下列方程的唯一实数解得到列车k+1时刻在子坐标系下的坐标(xk+1,yk+1):
将子坐标系下的(xk+1,yk+1)从子坐标系转换到轨道电子地图的相对平面坐标系即可得到k+1时刻位置(Xk+1,Yk+1),进一步转换到导航坐标系,得到k+1时刻的导航坐标(Pe/k+1,Pv/k+1),完成轨道电子中的航位推移MM/DR;
为了计算k+1时刻列车的行车速度,首先将k时刻和k+1时刻位置信息转换到载体坐标系,分别记为(X′k,Y′k)和(X′k+1,Y′k+1),则列车速度在载体坐标系下为:
2.根据权利要求1所述的一种自适应FKF地图匹配方法,其特征在于:多源信息融合主要包括局部滤波器和主滤波器两部分,以全自主导航系统INS惯性导航系统作为参考系统,并包含两个子系统GNSS/INS和INS/MM,并分别对两个子滤波器采用标准KF算法进行数据融合;多源信息融合包括系统状态方程和量测方程建模,系统状态方程以INS系统的15维误差参数作为系统的状态向量X,包括导航坐标系“东-北-天”向下的3维失准角误差φ、3维速度误差δv、3维位置误差δp、载体坐标系“横-纵-天”向下的3维陀螺零漂ε和3维加计零偏
φ=[φE φN φU]
δv=[δvE δvN δvU]
δp=[δL δλ δh]
ε=[εx εy εz]
Z=[δvE δvN δvU δL δλ δh]
15维向量为融合系统局部滤波器的公共状态向量,系统状态方程如下:
X(k)=FX(k-1)+GW(k-1)
针对列车定位多源信息融合系统的两个子滤波器,分别计量测方程;
GNSS/INS子滤波器的量测方程以GNSS和INS的残差为量测向量,对应的量测方程如下:
其中,LINS、λINS、hINS、vINSe、vINSn、vINSu分别为INS惯性导航系统解算的纬度、经度、高程等位置信息和导航坐标系下的“东-北-天”向速度信息,LGNSS、λGNSS、hGNSS、vGNSSe、vGNSSn、vGNSSu分别为GNSS卫星定位系统的纬度、经度、高程三维位置信息与东北天向速度信息,HGNSS/INS和VGNSS/INSk分别为子滤波器的观测矩阵与量测噪声;
INS/MM子滤波器的量测方程以地图航位推移信息和INS的残差为量测向量,对应的量测方程如下:
上式中,LMM、λMM、hMM、vMMe、vMMn、vMMu为轨道电子地图航位推移的导航坐标系位置和速度数据,HINS/MM和VINS/MM分别为INS/MM子滤波器的观测矩阵与量测噪声。
4.根据权利要求3所述的一种自适应FKF地图匹配方法,其特征在于:
①信息分配
针对系统噪声Q,通过信息分配系数β将其分配到各个滤波器中,包括主滤波器和子滤波器,信息分配公式如下:
②子滤波器滤波
在完成基于标准KF的子滤波器局部滤波的基础上,得到GNSS/INS和MM/INS局部最优估计,并根据信息分配策略对自滤波器的误差参数进行修生,主要针对一步预测协方差矩阵的修正,G为标准KF中的噪声矩阵:
③多源信息融合
6.一种自适应FKF地图匹配系统,含有处理器和存储器,其特征在于:包括GNSS、IMU、里程计三个数据采集模块,包括以下四个单元,(1)卫星姿态解算单元:作用是在卫星信号可用的情况下为系统提供WG84坐标系下的经纬度位置数据、东北天速度数据、卫星信号质量数据等,是为系统提供绝对坐标的重要单元;
(2)INS姿态解算单元:作用是通过对加速度计、陀螺仪采集的车辆运动姿态信息进行处理,并结合INS/DR航位推移进行位置和速度解算;
(3)MM/DR航位推移单元:作用是根据里程计采集的列车行驶速度、里程信息,结合轨道电子地图,在轨道线路和路网拓扑中进行MM/DR地图航位推移,得到地图辅助信息;
(4)数据融合单元:用于实现基于GNSS/INS/MM多源信息的误差校正和位姿最优估计。
7.根据权利要求6所述的一种自适应FKF地图匹配系统,其特征在于:包括局部滤波器和主滤波器两部分,以全自主导航系统INS惯性导航系统作为参考系统,并包含两个子系统GNSS/INS和INS/MM,并分别对两个子滤波器采用标准KF算法进行数据融合,主滤波器通过信息分配系数的调节实现误差校正和反馈,提高列车定位系统对行车环境的适应性。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111088145.8A CN113758483B (zh) | 2021-09-16 | 2021-09-16 | 一种自适应fkf地图匹配方法及系统 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111088145.8A CN113758483B (zh) | 2021-09-16 | 2021-09-16 | 一种自适应fkf地图匹配方法及系统 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113758483A true CN113758483A (zh) | 2021-12-07 |
CN113758483B CN113758483B (zh) | 2023-11-24 |
Family
ID=78796075
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111088145.8A Active CN113758483B (zh) | 2021-09-16 | 2021-09-16 | 一种自适应fkf地图匹配方法及系统 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113758483B (zh) |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114234969A (zh) * | 2021-12-17 | 2022-03-25 | 上海华测导航技术股份有限公司 | 一种导航定位方法、装置和电子设备 |
CN114609657A (zh) * | 2022-03-07 | 2022-06-10 | 江西理工大学 | 一种基于信息融合的永磁磁悬浮列车交通定位系统及方法 |
CN114624754A (zh) * | 2022-03-28 | 2022-06-14 | 智己汽车科技有限公司 | 一种时空定位与近场补偿的自动驾驶定位装置与方法 |
CN116908893A (zh) * | 2023-07-17 | 2023-10-20 | 中国铁路广州局集团有限公司株洲机务段 | 一种基于多源融合的列车定位系统及方法 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2004168216A (ja) * | 2002-11-21 | 2004-06-17 | Railway Technical Res Inst | Gps測位による列車走行情報検出装置及びその列車走行情報検出方法 |
CN107656301A (zh) * | 2017-09-20 | 2018-02-02 | 北京航天发射技术研究所 | 一种基于多源信息融合的车载定位方法 |
CN108196289A (zh) * | 2017-12-25 | 2018-06-22 | 北京交通大学 | 一种卫星信号受限条件下的列车组合定位方法 |
CN110823217A (zh) * | 2019-11-21 | 2020-02-21 | 山东大学 | 一种基于自适应联邦强跟踪滤波的组合导航容错方法 |
-
2021
- 2021-09-16 CN CN202111088145.8A patent/CN113758483B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2004168216A (ja) * | 2002-11-21 | 2004-06-17 | Railway Technical Res Inst | Gps測位による列車走行情報検出装置及びその列車走行情報検出方法 |
CN107656301A (zh) * | 2017-09-20 | 2018-02-02 | 北京航天发射技术研究所 | 一种基于多源信息融合的车载定位方法 |
CN108196289A (zh) * | 2017-12-25 | 2018-06-22 | 北京交通大学 | 一种卫星信号受限条件下的列车组合定位方法 |
CN110823217A (zh) * | 2019-11-21 | 2020-02-21 | 山东大学 | 一种基于自适应联邦强跟踪滤波的组合导航容错方法 |
Non-Patent Citations (3)
Title |
---|
WEI JIANG 等: ""A Multi-Sensor Positioning Method-Based Train Localization System for Low Density Line"", 《IEEE TRANSACTIONS ON VEHICULAR TECHNOLOGY》, vol. 67, no. 11, pages 10425 - 10437, XP011702871, DOI: 10.1109/TVT.2018.2869157 * |
刘尚兰: ""基于线路特征的电子地图匹配技术研究"", 《中国优秀硕士学位论文全文数据库 工程科技II辑》, no. 02, pages 35 - 52 * |
徐田来 等: ""路网辅助的车载INS/GPS组合导航方法"", 《控制与决策》, vol. 24, no. 3, pages 398 - 402 * |
Cited By (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114234969A (zh) * | 2021-12-17 | 2022-03-25 | 上海华测导航技术股份有限公司 | 一种导航定位方法、装置和电子设备 |
CN114234969B (zh) * | 2021-12-17 | 2023-09-15 | 上海华测导航技术股份有限公司 | 一种导航定位方法、装置和电子设备 |
CN114609657A (zh) * | 2022-03-07 | 2022-06-10 | 江西理工大学 | 一种基于信息融合的永磁磁悬浮列车交通定位系统及方法 |
CN114624754A (zh) * | 2022-03-28 | 2022-06-14 | 智己汽车科技有限公司 | 一种时空定位与近场补偿的自动驾驶定位装置与方法 |
CN114624754B (zh) * | 2022-03-28 | 2024-05-14 | 智己汽车科技有限公司 | 一种时空定位与近场补偿的自动驾驶定位装置与方法 |
CN116908893A (zh) * | 2023-07-17 | 2023-10-20 | 中国铁路广州局集团有限公司株洲机务段 | 一种基于多源融合的列车定位系统及方法 |
Also Published As
Publication number | Publication date |
---|---|
CN113758483B (zh) | 2023-11-24 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111045068B (zh) | 一种基于非导航卫星信号的低轨卫星自主定轨定姿方法 | |
CN113758483B (zh) | 一种自适应fkf地图匹配方法及系统 | |
CN111156994B (zh) | 一种基于mems惯性组件的ins/dr&gnss松组合导航方法 | |
CN101357643B (zh) | 数字轨道地图辅助gps实现精确列车定位方法及系统 | |
AU663840B2 (en) | Traffic control system utilizing on-board vehicle information measurement apparatus | |
Yang et al. | Magnetometer and differential carrier phase GPS-aided INS for advanced vehicle control | |
CN110779521A (zh) | 一种多源融合的高精度定位方法与装置 | |
CN109471144B (zh) | 基于伪距/伪距率的多传感器紧组合列车组合定位方法 | |
CN109000640B (zh) | 基于离散灰色神经网络模型的车辆gnss/ins组合导航方法 | |
CN107132563B (zh) | 一种里程计结合双天线差分gnss的组合导航方法 | |
CN104729506A (zh) | 一种视觉信息辅助的无人机自主导航定位方法 | |
CN109059909A (zh) | 基于神经网络辅助的卫星/惯导列车定位方法与系统 | |
CN111947681B (zh) | 一种gnss与惯导组合导航位置输出的滤波校正方法 | |
WO2005048000A2 (en) | Locomotive location system and method | |
CN109781098B (zh) | 一种列车定位的方法和系统 | |
CN102621570B (zh) | 基于双全球定位和惯性测量的汽车动力学参数测量方法 | |
CN110285804B (zh) | 基于相对运动模型约束的车辆协同导航方法 | |
Zhou et al. | Kinematic measurement of the railway track centerline position by GNSS/INS/odometer integration | |
Elsheikh et al. | Integration of GNSS precise point positioning and reduced inertial sensor system for lane-level car navigation | |
CN107247275A (zh) | 基于公交车的城市gnss脆弱性监测系统及其方法 | |
Gao et al. | Development of precise GPS/INS/wheel speed sensor/yaw rate sensor integrated vehicular positioning system | |
CN112051598B (zh) | 一种基于双重校正的车载gnss/ins组合导航方法 | |
CN113074757A (zh) | 车载惯导安装误差角的标定方法 | |
Zhou et al. | Onboard train localization based on railway track irregularity matching | |
CN114136275A (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 |