CN113758483B - 一种自适应fkf地图匹配方法及系统 - Google Patents
一种自适应fkf地图匹配方法及系统 Download PDFInfo
- Publication number
- CN113758483B CN113758483B CN202111088145.8A CN202111088145A CN113758483B CN 113758483 B CN113758483 B CN 113758483B CN 202111088145 A CN202111088145 A CN 202111088145A CN 113758483 B CN113758483 B CN 113758483B
- Authority
- CN
- China
- Prior art keywords
- train
- ins
- information
- sub
- filter
- 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
- 238000000034 method Methods 0.000 title claims abstract description 31
- 230000004927 fusion Effects 0.000 claims abstract description 78
- 238000007405 data analysis Methods 0.000 claims abstract description 6
- 238000013500 data storage Methods 0.000 claims abstract description 4
- 238000009434 installation Methods 0.000 claims abstract description 4
- 238000005259 measurement Methods 0.000 claims description 37
- 230000001133 acceleration Effects 0.000 claims description 30
- 239000013598 vector Substances 0.000 claims description 27
- 230000007704 transition Effects 0.000 claims description 24
- 239000011159 matrix material Substances 0.000 claims description 18
- 238000006073 displacement reaction Methods 0.000 claims description 17
- 238000004364 calculation method Methods 0.000 claims description 16
- 230000003044 adaptive effect Effects 0.000 claims description 14
- 238000011156 evaluation Methods 0.000 claims description 10
- 238000012937 correction Methods 0.000 claims description 9
- 238000001914 filtration Methods 0.000 claims description 6
- 238000004458 analytical method Methods 0.000 claims description 3
- 238000005070 sampling Methods 0.000 claims description 3
- 230000003068 static effect Effects 0.000 claims description 2
- 230000009286 beneficial effect Effects 0.000 abstract description 2
- 238000005516 engineering process Methods 0.000 description 8
- 238000013461 design Methods 0.000 description 5
- 238000010586 diagram Methods 0.000 description 5
- 230000006870 function Effects 0.000 description 5
- 238000010276 construction Methods 0.000 description 2
- 206010063385 Intellectualisation Diseases 0.000 description 1
- 230000006978 adaptation Effects 0.000 description 1
- 238000012512 characterization method Methods 0.000 description 1
- 238000006243 chemical reaction 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
- 238000012544 monitoring process Methods 0.000 description 1
- 238000007500 overflow downdraw method Methods 0.000 description 1
- 238000002601 radiography Methods 0.000 description 1
- 238000004148 unit process Methods 0.000 description 1
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 1：记列车k时刻在匹配路段内的初始位移为Sk，所在路段的长度记为L，采样时间Δt内的位移为
Step 2：计算k时刻列车在匹配路段内的剩余里程；
Sres＝L-Sk
Step 3：根据剩余里程判断跟踪匹配状态：若k+1时刻车辆在下一路段内，/>则仍在当前路段内；
Step 4：根据k+1时刻所在路段类型，计算下一时刻在轨道电子地图中的相对平面坐标；
轨道平面线形是直线的，记所在路段起始坐标为(X0，Y0)，方位角为θz，则k+1时刻列车运行距离后的相对平面坐标为：
轨道平面线形是圆曲线的，记圆曲线半径为R，圆心为(Xc，Xc)，起始点极坐标为θc，则列车运行后的极坐标为：
若圆曲线沿列车行驶方向顺时针旋转，对应的极坐标减少，圆曲线沿列车行驶方向逆时针旋转，对应的极坐标增加，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)
其中F、G和W分别为系统状态转移矩阵、系统噪声矩阵和噪声向量；φ、δv、δp中的三维对应“东-北-天”向的误差，ε和中的三维对应“横-纵-天”向的误差；
针对列车定位多源信息融合系统的两个子滤波器，分别计量测方程；
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，通过信息分配系数β将其分配到各个滤波器中，包括主滤波器和子滤波器，信息分配公式如下：
上式中i＝1，2为子滤波器上标，和/>分别为主滤波器与子滤波器的协方差矩阵，/>和/>为对应的噪声矩阵，/>和/>为滤波器的最优估计，信息分配满足以下信息守恒方程：
P、Q、β相关参数含义与上文相同
因此，信息分配系数的约束条件如下，为主滤波器信息分配系数：
②子滤波器滤波
在完成基于标准KF的子滤波器局部滤波的基础上，得到GNSS/INS和MM/INS局部最优估计，并根据信息分配策略对自滤波器的误差参数进行修生，主要针对一步预测协方差矩阵的修正，G为标准KF中的噪声矩阵：
符号含义上文相同
③多源信息融合
在得到子滤波器最优估计的基础上，进一步计算主滤波器的最优估计，包括状态估计和状态估计均方误差实现列车定位多源信息融合，具体方程如下，为标准KF中的一步预测状态：
符号含义与上文相同。
引入卫星信号评价因子τ，τ取值如下：
其中HDOP0和HDOP1分别为卫星信号好和卫星信号可用状态的经验阈值；
另一方面，列车加速状态对MM/DR轨道电子地图航位推移的精度影响较大，计系统加速度计零偏系数和列车当前加速度系数ak为：
其中，为加速度计的零偏系数，ax、ay、az为加速度观测值，为提高对列车加速状态的适应性，针对列车静止、匀速场景、低加速场景和高加速场景，引入列车加速状态评价因子/>如下：
其中aλ为列车加速状态阈值，和aλ根据列车运行情况取经验值；
设计多源信息融合的信息匹配系数如下：
。
一种自适应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两个子滤波器的量测方程。
6.设计了基于自适应信息分配策略的主滤波器，包括6维状态向量、对应的状态方程和量测方程，并引入卫星信号评价因子τ和加速状态评价因子实现信息匹配系数β的自适应调节，提高列车定位系统对环境的适应性。
附图说明
图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 1：记列车k时刻在匹配路段内的初始位移为Sk，所在路段的长度记为L，采样时间Δt内的位移为
Step 2：计算k时刻列车在匹配路段内的剩余里程。
Sres＝L-Sk
Step 3：根据剩余里程判断跟踪匹配状态：若k+1时刻车辆在下一路段内，/>则仍在当前路段内。
Step 4：根据k+1时刻所在路段类型，计算下一时刻在轨道电子地图中的相对平面坐标。
(1)直线
记所在路段起始坐标为(X0，Y0)，方位角为θz，则k+1时刻列车运行后的相对平面坐标为：
(2)圆曲线
记圆曲线半径为R，圆心为(Xc，Xc)，起始点极坐标为θc，则列车运行后的极坐标为：
若圆曲线沿列车行驶方向顺时针旋转，对应的极坐标减少，否则增加。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中所述列车定位多源信息融合系统的各个滤波器中，包括主滤波器和子滤波器，信息分配公式如下：
/>
上式中i＝1，2为子滤波器上标，和/>分别为主滤波器与子滤波器的协方差矩阵，/>和/>为对应的噪声矩阵，/>和/>为滤波器的最优估计，信息分配满足以下信息守恒方程：
因此，信息分配系数的约束条件如下，为主滤波器信息分配系数：
②子滤波器滤波
在完成基于标准KF的子滤波器局部滤波的基础上，得到GNSS/INS和MM/INS局部最优估计，并根据信息分配策略对自滤波器的误差参数进行修生，主要针对一步预测协方差矩阵的修正，G为标准KF中的噪声矩阵：
③多源信息融合
在得到子滤波器最优估计的基础上，进一步计算主滤波器的最优估计，包括状态估计和状态估计均方误差实现列车定位多源信息融合，具体方程如下为标准KF中的一步预测状态：
在主滤波器设计中，信息分配策略是影响最优估计效果的关键，针对传统平均分配等策略下的定位系统不能实现对行车环境的自适应等问题，设计自适应分配算法，根据列车行驶环境下不同信号的特性进行，对信息分配系数β进行自适应调整，提高系统对行车环境的适应性。
在上述的列车定位多源信息融合系统中，两个子滤波器均包含SINS信息，其差异主要在于GNSS卫星信号和MM轨道电子地图航位推移信息。GNSS卫星信号受可信度受卫星信号质量影响较大，其表征指标主要为HDOP参数，故引入卫星信号评价因子τ，根据卫星信号失锁、干扰、可用等状态，τ取值如下：
其中HDOP0和HDOP1分别为卫星信号好和卫星信号可用的经验阈值。
另一方面，列车加速状态对MM/DR轨道电子地图航位推移的精度影响较大，计系统加速度计零偏系数和列车当前加速度系数ak为：/>
其中，为加速度计的零偏系数，ax、ay、az为加速度观测值，为提高对列车加速状态的适应性，针对列车静止、匀速场景、低加速场景和高加速场景，引入列车加速状态评价因了/>如下：
其中aλ为列车加速状态阈值，和aλ根据列车运行情况取经验值。
综合考虑GNSS信号评价因子τ和加速状态评价因子设计列车定位多源信息融合系统的信息匹配系数如下：
图1为组合定位平台框图，包括GNSS、IMU、里程计三个数据采集模块，以及卫星姿态解算单元、INS姿态解算单元、MM/DR航位推移单元和数据融合单元四个主要单元。该平台将轨道电子地图信息与MM/DR地图航位推移单元相结合，实现地图与组合定位系统的结合，实时修正位置与速度解算结果。
图2为基于多源信息融合的跟踪匹配流程图，该流程图根据组合定位平台设计，包括系统初始化、数据采集、数据解析、数据融合、数据输出与存储5个步骤。
图3为基于自适应FKF的列车多源信息融合算法结构示意图，该融合算法的输入包括GNSS卫星信号、INS姿态信息、轨道电子地图三组信息，航位推移MM/DR三组信息，以SINS惯性导航系统为参考系统，含GNSS/INS、INS/MM两个子系统，分别对应两个局部滤波器，并包含一个主滤波器，附图中虚线为反馈参数。
Claims (4)
1.一种自适应FKF地图匹配方法，其特征在于：
Step 1：设备安装、系统初始化，惯导初始对准、卫星信号稳定性跟踪；
Step 2：数据采集，包括GNSS卫星信号、惯性导航姿态数据、里程计数据和轨道电子地图数据，GNSS卫星信号包括导航电文解算需要的星历数据、经纬度和高程位置数据、东-北-天向速度数据、卫星信号质量数据，惯性导航姿态数据包括加速度计和陀螺仪信息；
Step 3：数据解析阶段，包括基于GNSS卫星信号的导航电文解析，惯性导航单元的姿态解算和基于里程计速度、轨道电子地图的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地图一维里程推移的起始坐标，根据轨道电子地图拓扑数据和一维里程增量，推算列车在k+1时刻的相对平面坐标(Xk+1,Yk+1)；结合轨道线路特征，需分别针对轨道平面线形中的直线、圆曲线、缓和曲线三种线形进行推导；MM/DR地图一维里程推移流程如下：
Step 31：记列车k时刻在匹配路段内的初始位移为Sk，所在路段的长度记为L，采样时间Δt内的位移为
Step 32：计算k时刻列车在匹配路段内的剩余里程；
Sres＝L-Sk
Step 33：根据剩余里程判断跟踪匹配状态：若k+1时刻车辆在下一路段内，/>则仍在当前路段内；
Step 34：根据k+1时刻所在路段类型，计算k+1时刻列车在轨道电子地图中的相对平面坐标；
轨道平面线形是直线的，记所在路段起始坐标为(X0,Y0)，方位角为θz，则k+1时刻列车运行距离后的相对平面坐标为：
轨道平面线形是圆曲线的，记圆曲线半径为R，圆心为(Xc,Yc)，起始点极坐标为θc，则列车运行后的极坐标为：
若圆曲线沿列车行驶方向顺时针旋转，对应的极坐标减少，圆曲线沿列车行驶方向逆时针旋转，对应的极坐标增加，k+1时刻列车在轨道电子地图中的相对平面坐标为：
轨道平面线形是缓和曲线的，若列车位于缓和曲线中，则对缓和曲线建立子坐标系，其中原点为直缓点，第一象限为缓圆点一侧，k时刻列车距离子坐标系原点的位移为Sk，计算k+1时刻列车在子坐标系缓和曲线中的剩余位移量：
求解下列方程的唯一实数解得到列车k+1时刻在子坐标系下的坐标(xk+1,yk+1)：
将子坐标系下的(xk+1,yk+1)从子坐标系转换到轨道电子地图的相对平面坐标系即可得到k+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算法进行数据融合；在各个子滤波器完成局部最优估计的基础上，多源信息融合系统将子滤波器结果传递给主滤波器，并根据信息分配策略实现子滤波器间的参数分配与信息反馈；
引入卫星信号评价因子τ，τ取值如下：
其中HDOP0和HDOP1分别为卫星信号好和卫星信号可用状态的经验阈值；
另一方面，列车加速状态对MM/DR轨道电子地图一维里程推移的精度影响较大，系统加速度计零偏系数和列车当前加速度系数ak为：
其中，为加速度计的零偏系数，ax、ay、az为加速度观测值，为提高对列车加速状态的适应性，针对列车静止、匀速场景、低加速场景和高加速场景，引入列车加速状态评价因子/>如下：
其中aλ为列车加速状态阈值，和aλ根据列车运行情况取经验值；
设计多源信息融合的信息匹配系数如下：
2.根据权利要求1所述的一种自适应FKF地图匹配方法，其特征在于：多源信息融合包括局部滤波器和主滤波器两部分，以全自主导航系统INS惯性导航系统作为参考系统，并包含两个子系统GNSS/INS和INS/MM，并分别对两个子滤波器采用标准KF算法进行数据融合；多源信息融合包括系统状态方程和量测方程建模，系统状态方程以系统的15维误差参数作为系统的状态向量X，包括导航坐标系“东-北-天”向下的3维失准角误差φ、3维速度误差δv、3维位置误差δp、载体坐标系“横-纵-天”向下的3维陀螺零漂ε和3维加计零偏系统量测方程以系统的6位误差参数作为系统的量测向量Z,包括3维速度误差δv和三维位置误差δp；
φ＝[φ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)
其中F、G和W分别为系统状态转移矩阵、系统噪声矩阵和噪声向量；φ、δv、δp中的三维对应“东-北-天”向的误差，ε和中的三维对应“横-纵-天”向的误差；
针对列车定位多源信息融合系统的两个子滤波器，分别设计量测方程；
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子滤波器的观测矩阵与量测噪声。
3.根据权利要求1所述的一种自适应FKF地图匹配方法，其特征在于：滤波器之间的自适应信息分配策略，具体如下：
在各个子滤波器完成局部最优估计的基础上，多源信息融合系统将子滤波器结果传递给主滤波器，并根据信息分配策略实现子滤波器间的参数分配与信息反馈；主滤波器的状态向量为以下6维误差向量：
Xg＝[δLINS δλINS δhINS δvINSe δvINSn δvINSu]T
主滤波器的状态方程和量测方程如下：
4.根据权利要求3所述的一种自适应FKF地图匹配方法，其特征在于：
①信息分配
针对系统噪声Q，通过信息分配系数β将其分配到各个滤波器中，包括主滤波器和子滤波器，信息分配公式如下：
上式中i＝1,2为子滤波器上标，和/>分别为子滤波器与主滤波器的协方差矩阵，和/>为对应的噪声矩阵，/>和/>为滤波器的最优估计，信息分配满足以下信息守恒方程：
因此，信息分配系数的约束条件如下，为主滤波器信息分配系数：
②子滤波器滤波
在完成基于标准KF的子滤波器局部滤波的基础上，得到GNSS/INS和MM/INS局部最优估计，并根据信息分配策略对子滤波器的误差参数进行修正，针对一步预测协方差矩阵的修正：
上式中，βi为分配系数，为标准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 CN113758483A (zh) | 2021-12-07 |
| CN113758483B true 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) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114234969B (zh) * | 2021-12-17 | 2023-09-15 | 上海华测导航技术股份有限公司 | 一种导航定位方法、装置和电子设备 |
| CN114624754B (zh) * | 2022-03-28 | 2024-05-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 | Ｇｐｓ測位による列車走行情報検出装置及びその列車走行情報検出方法 |
| 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 | Ｇｐｓ測位による列車走行情報検出装置及びその列車走行情報検出方法 |
| 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》.2018,第67卷(第11期),10425-10437页. * |
| 刘尚兰."基于线路特征的电子地图匹配技术研究".《中国优秀硕士学位论文全文数据库 工程科技II辑》.2022,(第02期),35-52页. * |
| 徐田来 等."路网辅助的车载INS/GPS组合导航方法".《控制与决策》.2009,第24卷(第3期),398-402页. * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113758483A (zh) | 2021-12-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113758483B (zh) | 一种自适应fkf地图匹配方法及系统 | |
| CN111045068B (zh) | 一种基于非导航卫星信号的低轨卫星自主定轨定姿方法 | |
| CN111156994B (zh) | 一种基于mems惯性组件的ins/dr&gnss松组合导航方法 | |
| CN108594283B (zh) | Gnss/mems惯性组合导航系统的自由安装方法 | |
| CN109471144B (zh) | 基于伪距/伪距率的多传感器紧组合列车组合定位方法 | |
| CN110631574B (zh) | 一种惯性/里程计/rtk多信息融合方法 | |
| CN108931244B (zh) | 基于列车运动约束的惯导误差抑制方法及系统 | |
| CN111947681B (zh) | 一种gnss与惯导组合导航位置输出的滤波校正方法 | |
| CN111024074B (zh) | 一种基于递推最小二乘参数辨识的惯导速度误差确定方法 | |
| CN110006421B (zh) | 一种基于mems和gps的车载导航方法及系统 | |
| CN107860399A (zh) | 一种基于地图匹配的车载捷联惯导行进间精对准方法 | |
| Yang et al. | SINS/odometer/Doppler radar high-precision integrated navigation method for land vehicle | |
| Gao et al. | Development of precise GPS/INS/wheel speed sensor/yaw rate sensor integrated vehicular positioning system | |
| CN111721250B (zh) | 一种铁路轨道平顺性实时检测装置及检测方法 | |
| CN113008229B (zh) | 一种基于低成本车载传感器的分布式自主组合导航方法 | |
| CN112051598B (zh) | 一种基于双重校正的车载gnss/ins组合导航方法 | |
| CN111024128B (zh) | 一种机载光电吊舱光轴稳定状态传递对准方法 | |
| CN111123381B (zh) | 一种用于平台式重力仪减小水平加速度影响的方法 | |
| CN117053802A (zh) | 一种基于旋转mems imu的车载导航系统定位误差减小的方法 | |
| CN115031728A (zh) | 基于ins与gps紧组合的无人机组合导航方法 | |
| US20230349699A1 (en) | Absolute heading estimation with constrained motion | |
| Fang et al. | Integrating SINS sensors with odometer measurements for land vehicle navigation system | |
| Bonnabel et al. | A simple nonlinear filter for low-cost ground vehicle localization system | |
| CN115900701B (zh) | 一种卫导拒止环境下的轨道车组合导航方法及装置 | |
| CN111238530B (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 |