CN116817928A - 基于因子图优化的卫导/惯导列车多源融合定位的方法 - Google Patents
基于因子图优化的卫导/惯导列车多源融合定位的方法 Download PDFInfo
- Publication number
- CN116817928A CN116817928A CN202311083417.4A CN202311083417A CN116817928A CN 116817928 A CN116817928 A CN 116817928A CN 202311083417 A CN202311083417 A CN 202311083417A CN 116817928 A CN116817928 A CN 116817928A
- Authority
- CN
- China
- Prior art keywords
- factor
- model
- factor graph
- imu
- graph model
- 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 59
- 238000005457 optimization Methods 0.000 title claims abstract description 28
- 230000004927 fusion Effects 0.000 title claims abstract description 18
- 230000010354 integration Effects 0.000 claims abstract description 39
- 238000004364 calculation method Methods 0.000 claims abstract description 14
- 238000012217 deletion Methods 0.000 claims abstract description 7
- 230000037430 deletion Effects 0.000 claims abstract description 7
- 230000006870 function Effects 0.000 claims description 37
- 238000005259 measurement Methods 0.000 claims description 31
- 108010074506 Transfer Factor Proteins 0.000 claims description 17
- 238000004590 computer program Methods 0.000 claims description 14
- 239000004973 liquid crystal related substance Substances 0.000 claims description 9
- 230000003068 static effect Effects 0.000 abstract description 2
- 238000010586 diagram Methods 0.000 description 10
- 230000008569 process Effects 0.000 description 10
- 238000012545 processing Methods 0.000 description 10
- 238000004891 communication Methods 0.000 description 8
- 238000004422 calculation algorithm Methods 0.000 description 7
- 238000012546 transfer Methods 0.000 description 6
- 238000001914 filtration Methods 0.000 description 5
- 238000006243 chemical reaction Methods 0.000 description 4
- 239000011159 matrix material Substances 0.000 description 4
- 238000012549 training Methods 0.000 description 4
- 230000015556 catabolic process Effects 0.000 description 3
- 238000006731 degradation reaction Methods 0.000 description 3
- 230000003287 optical effect Effects 0.000 description 3
- 230000001133 acceleration Effects 0.000 description 2
- 230000008901 benefit Effects 0.000 description 2
- 238000010276 construction Methods 0.000 description 2
- 230000003993 interaction Effects 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 230000007704 transition Effects 0.000 description 2
- 238000012897 Levenberg–Marquardt algorithm Methods 0.000 description 1
- 230000002159 abnormal effect Effects 0.000 description 1
- 238000003491 array Methods 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000001413 cellular effect Effects 0.000 description 1
- 230000007812 deficiency Effects 0.000 description 1
- 230000007774 longterm Effects 0.000 description 1
- 238000010801 machine learning Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 230000001953 sensory effect Effects 0.000 description 1
- 230000009897 systematic effect Effects 0.000 description 1
- 230000000007 visual effect Effects 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/20—Instruments for performing navigational calculations
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B61—RAILWAYS
- B61L—GUIDING RAILWAY TRAFFIC; ENSURING THE SAFETY OF RAILWAY TRAFFIC
- B61L25/00—Recording or indicating positions or identities of vehicles or trains or setting of track apparatus
- B61L25/02—Indicating or recording positions or identities of vehicles or trains
- B61L25/025—Absolute localisation, e.g. providing geodetic coordinates
-
- 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- 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
- 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/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
- G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
- G06N7/00—Computing arrangements based on specific mathematical models
- G06N7/01—Probabilistic graphical models, e.g. probabilistic networks
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Theoretical Computer Science (AREA)
- Artificial Intelligence (AREA)
- Mathematical Analysis (AREA)
- Algebra (AREA)
- Mechanical Engineering (AREA)
- Computational Mathematics (AREA)
- Data Mining & Analysis (AREA)
- Evolutionary Computation (AREA)
- Probability & Statistics with Applications (AREA)
- Mathematical Optimization (AREA)
- Pure & Applied Mathematics (AREA)
- Computing Systems (AREA)
- General Engineering & Computer Science (AREA)
- Mathematical Physics (AREA)
- Software Systems (AREA)
- Computer Networks & Wireless Communication (AREA)
- Navigation (AREA)
Abstract
本公开提供了基于因子图优化的卫导/惯导列车多源融合定位的方法，涉及基于卫星导航的列车定位领域。具体地，获取待估测列车对应的多维状态变量、系统初始状态先验信息和先验因子，生成因子图模型；当检测到惯性传感器IMU数据时，将IMU预积分因子添加至因子图模型；当检测到全球卫星导航系统GNSS数据时，将对应的GNSS因子添加至因子图模型；对滑动窗口长度外的所述因子图模型中时间最早的状态变量进行边缘化删除，并构建滑窗因子图模型；基于滑窗因子图模型进行图优化解算，计算待估测列车的定位状态信息。以上方法可以实时得到列车定位估计结果,可适用于长距离、环境复杂多变的铁路列车定位中，对列车静态及动态定位具有通用性，工程应用价值显著。
Description
技术领域
本公开涉及基于卫星导航的列车定位领域，尤其涉及基于因子图优化的卫导/惯导列车多源融合定位的方法、电子设备、存储介质和计算机程序产品。
背景技术
近年来，全球卫星导航系统（Global Navigation Satellite System，GNSS）逐渐发展成熟，能够为全球用户提供全方位、全天候的高精度三维位置、速度和时间等导航信息。其凭借全天候、连续、实时的高精度定位特性被引入铁路领域，有效提升了列车定位的自主性、灵活性和成本效益。但铁路环境复杂，高山、桥梁、隧道等受限环境往往会影响卫星信号的接收，导致性能降级，单一的卫星导航系统难以保证列车定位的连续性及安全性。惯性导航系统（Inertial Navigation System，INS）具有短期精度高、独立性好、不受外界环境干扰等优势，但惯导随着工作时间的增加会产生累积误差，长期精度低。
发明内容
本公开提供了一种基于因子图优化的卫导/惯导列车多源融合定位的方法、电子设备、存储介质和计算机程序产品。
根据本公开的一方面，提供了一种基于因子图优化的卫导/惯导列车多源融合定位的方法，包括：
获取待估测列车对应的多维状态变量、系统初始状态先验信息和先验因子，生成因子图模型；
当检测到惯性传感器IMU数据时，将对应的IMU预积分因子添加至所述因子图模型；
当检测到全球卫星导航系统GNSS数据时，将对应的GNSS因子添加至所述因子图模型；
对滑动窗口长度外的所述因子图模型中时间最早的状态变量进行边缘化删除，并构建滑窗因子图模型；
基于所述滑窗因子图模型进行图优化解算，计算所述待估测列车的定位状态信息。
可选地，所述获取待估测列车对应的多维状态变量、系统初始状态先验信息和先验因子，生成因子图模型，包括：
选取地心地固ECEF为参考坐标系，获取所述多维状态变量，其中，所述多维状态变量包括组成所述因子图模型的至少一个状态变量对应的所述待估测列车的三维位置信息、三维速度信息、姿态信息、惯导零偏、GNSS接收机时钟偏差的等效伪距测量误差和时钟漂移的等效伪距测量误差；
基于传感器测量结果和/或预设值，确定所述系统初始状态先验信息；
基于先验信息对所述系统初始状态先验信息进行对准，获取先验因子及所述先验因子对应的误差函数；
基于所述多维状态变量、所述先验因子、构建所述因子图模型。
可选地，所述基于先验信息对所述系统初始状态先验信息进行对准，获取先验因子及所述先验因子对应的误差函数，包括：
基于所述先验因子的误差函数对所述先验模型进行模型校准，并基于校准后的所述先验模型和当前的状态变量更新所述先验因子。
可选地，所述当检测到惯性传感器IMU数据时，将对应的IMU预积分因子添加至所述因子图模型，包括：
获取预设周期内的所述IMU数据，确定导航状态量输出时刻的所述IMU等效观测量；其中，所述IMU等效观测量包括位置增量、速度增量、姿态增量、下一时刻的加速度计零偏和陀螺仪零偏；
进行IMU预积分计算，将对应的所述IMU预积分因子添加至所述因子图模型。
可选地，所述进行IMU预积分计算，将对应的所述IMU预积分因子添加至所述因子图模型，包括：
基于相邻的所述状态变量的所述IMU等效观测量和所述IMU预积分因子，构建IMU预积分因子模型；
基于IMU预积分因子模型，获取IMU预积分因子对应的误差函数，并将对应所述IMU预积分因子添加至所述因子图模型。
可选地，所述当检测到全球卫星导航系统GNSS数据时，将对应的GNSS因子添加至所述因子图模型，包括：
基于当前的所述状态变量的所述GNSS数、伪距、伪距率，构建GNSS因子模型；
基于GNSS因子模型，获取GNSS因子对应的误差函数，并将对应所述GNSS因子添加至所述因子图模型。
可选地，所述方法还包括：
基于连续的至少两个所述状态变量的接收机时钟偏差和时钟漂移，构建接收机时钟转移因子模型；
基于钟差状态转移因子的误差函数对所述接收机时钟转移模型进行更新，计算对应的所述钟差状态转移因子并添加至所述因子图模型。
根据本公开的又一方面，提供了一种电子设备，包括：
至少一个处理器；以及
与所述至少一个处理器通信连接的存储器；其中，
所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行如上所述的方面和任一可能的实现方式的方法。
根据本公开的又一方面，提供了一种存储有计算机指令的非瞬时计算机可读存储介质，所述计算机指令用于使所述计算机执行如上所述的方面和任一可能的实现方式的方法。
根据本公开的又一方面，提供了一种计算机程序产品，包括计算机程序，所述计算机程序在被处理器执行时实现如上所述的方面和任一可能的实现方式的方法。
由上述技术方案可知，本公开实施例通过利用因子图优化框架进行GNSS和惯性传感器（Inertial Measurement Unit，IMU）数据的多源融合，在解算过程中多次迭代、重复线性化、充分考虑历史量测信息，相较于传统滤波类方式实现更高精度的列车位置实时估计。本质上，该方法基于状态转移系统方程、多传感器量测方程构建滑窗因子图模型，根据系统、量测误差分布模型构造各因子的误差函数，引入贝叶斯公式转化得到因子图的目标函数，最终采用Gauss-Newton（GN）或者Levenberg-Marquardt（LM）最优化算法求解，实时得到列车定位估计结果。
应当理解，本部分所描述的内容并非旨在标识本公开的实施例的关键或重要特征，也不用于限制本公开的范围。本公开的其它特征将通过以下的说明书而变得容易理解。
附图说明
附图用于更好地理解本方案，不构成对本公开的限定。其中：
图1是根据本公开第一实施例的示意图；
图2是根据本公开第一实施例中一个可能的实现方式的示意图；
图3是根据本公开第一实施例中的滑窗因子图模型的示意图；
图4是用来实现本公开实施例的基于因子图优化的卫导/惯导列车多源融合定位的方法的电子设备的框图。
具体实施方式
以下结合附图对本公开的示范性实施例做出说明，其中包括本公开实施例的各种细节以助于理解，应当将它们认为仅仅是示范性的。因此，本领域普通技术人员应当认识到，可以对这里描述的实施例做出各种改变和修改，而不会背离本公开的范围和精神。同样，为了清楚和简明，以下的描述中省略了对公知功能和结构的描述。
显然，所描述的实施例是本公开一部分实施例，而不是全部的实施例。基于本公开中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的全部其他实施例，都属于本公开保护的范围。
需要说明的是，本公开实施例中所涉及的终端设备可以包括但不限于手机、个人数字助理（Personal Digital Assistant，PDA）、无线手持设备、平板电脑（TabletComputer）等智能设备；显示设备可以包括但不限于个人电脑、电视等具有显示功能的设备。
另外，本文中术语“和/或”，仅仅是一种描述关联对象的关联关系，表示可以存在三种关系，例如，A和/或B，可以表示：单独存在A，同时存在A和B，单独存在B这三种情况。另外，本文中字符“/”，一般表示前后关联对象是一种“或”的关系。
现有列车多源融合定位算法以扩展卡尔曼滤波器（Extended Kalman filter,EKF）为代表的贝叶斯滤波器为主，由于其基于马尔可夫假设、单步单次迭代、仅考虑当前历元量测及前一历元状态，在求解非线性估计问题、存在异常量测值时常表现出不稳定性，加剧了铁路沿线山体、建筑物、森林等形成的遮挡环境场景下定位性能降级。而因子图优化（Factor Graph Optimization, FGO）框架通过非线性优化融合各种传感器量测，具有多次迭代，重新线性化和充分考虑历史量测时间关联性的特点，可从理论上弥补滤波方式的不足，也逐渐成为多源融合定位算法的新兴研究重点。因此，引入因子图优化框架至铁路定位领域、设计GNSS/INS列车多源融合定位算法可提升列车定位精度，缓解受限环境下的定位性能降级情况，从而有助于基于GNSS的列车运行控制系统进一步满足列车安全控制的要求。
基于以上问题，本公开提供一种基于因子图优化的卫导/惯导列车多源融合定位的方法。利用因子图优化框架进行GNSS和惯性传感器（Inertial Measurement Unit，IMU）数据的多源融合，在解算过程中多次迭代、重复线性化、充分考虑历史量测信息，相较于传统滤波类方式实现更高精度的列车位置实时估计。本质上，该方法基于状态转移系统方程、多传感器量测方程构建滑窗因子图模型，根据系统、量测误差分布模型构造各因子的误差函数，引入贝叶斯公式转化得到因子图的目标函数，最终采用Gauss-Newton（GN）或者Levenberg-Marquardt（LM）最优化算法求解，实时得到列车定位估计结果。
图1是根据本公开第一实施例的示意图，如图1所示。
步骤S101，获取待估测列车对应的多维状态变量、系统初始状态先验信息和先验因子，生成因子图模型。
步骤S102，当检测到惯性传感器IMU数据时，将对应的IMU预积分因子添加至所述因子图模型。
步骤S103，当检测到全球卫星导航系统GNSS数据时，将对应的GNSS因子添加至所述因子图模型。
步骤S104，对滑动窗口长度外的因子图模型中时间最早的状态变量进行边缘化删除，并构建滑窗因子图模型。
步骤S105，基于滑窗因子图模型进行图优化解算，计算待估测列车的定位状态信息。
在步骤S101中，获取待估测列车对应的多维状态变量、系统初始状态先验信息和先验因子，生成因子图模型。
具体地，选取地心地固ECEF为参考坐标系，获取待估测列车对应的多维状态变量，其中，多维状态变量包括组成因子图模型的至少一个状态变量对应的待估测列车的三维位置信息、三维速度信息、姿态信息、惯导零偏、GNSS接收机时钟偏差的等效伪距测量误差和时钟漂移的等效伪距测量误差。构建的多维状态变量集合为：
公式1；
其中，针对GNSS/INS紧多源融合定位系统，GNSS与INS的状态互不相关可独立分析。INS子系统中的列车的姿态、速度、位置是估计核心，且加速度计和陀螺仪零偏对定位性能影响较大，将其引入状态变量能够有效提高INS性能。GNSS子系统的接收机钟差和钟漂需要进行合理估计，校正伪距、伪距率误差。为便于INS与GNSS的数据融合，选取地心地固（Earth-Centered Earth-Fixed，ECEF）为参考坐标系，构造18维状态变量如下：
公式2；
其中，指代从滑窗中第1个历元到当前历元所包含的状态变量集合，即因子图总优化变量；/>表示第k历元下的状态变量，其包含了ECEF系下列车的3维位置，3维速度/>，4元数形式的载体系相对于ECEF系的姿态，载体系中加速度计和陀螺仪的零偏/>、，以及GNSS接收机时钟偏差的等效伪距测量误差/>和时钟漂移的等效伪距测量误差/>。
进一步地，基于传感器测量结果和/或预设值，确定系统初始状态先验信息；基于先验信息对系统初始状态进行对准，获取先验因子及先验因子对应的误差函数；基于多维状态变量、先验模型、系统初始状态先验信息和先验因子构建因子图模型。
具体地，计算系统初始状态先验信息，添加初始状态节点/>和先验因子至因子图。为确保状态转移过程（INS递推、钟差和钟漂传递）正常进行，需利用高精度的传感器测量结果或预设值给定可靠准确的初始状态的先验信息，该信息与多维状态变量一一对应，表示如下：
公式3；
先验因子用于定位系统初始状态对准，假设所给定先验信息的误差服从高斯分布，则先验因子模型可表示为：
公式4；
转换得到先验因子的误差函数为：
公式5；
式中，是先验因子的协方差矩阵，表征所采用的18维先验信息的不确定度，表示为：
公式6；
其中，其中，为3x1单位矩阵，/>为位置误差标准差，/>为速度误差标准差，为姿态误差标准差，/>为加速度计零偏误差标准差，/>为陀螺仪零偏误差标准差，/>为接收机时钟偏差误差标准差，/>为接收机时钟漂移误差标准差。
基于先验因子的误差函数对先验模型进行模型校准，并基于校准后的先验模型和当前的状态变量更新误差函数。
在步骤S102中，当检测到惯性传感器IMU数据时，将对应的IMU预积分因子添加至因子图模型。
具体地，获取预设周期内的IMU数据，确定导航状态量输出时刻的IMU等效观测量；其中，IMU等效观测量包括位置增量、速度增量、姿态增量、下一时刻的加速度计零偏和陀螺仪零偏；进行IMU预积分计算，将对应的IMU预积分因子添加至因子图模型。
基于相邻的状态变量的IMU等效观测量和IMU预积分因子，构建IMU预积分因子模型；基于IMU预积分因子的误差函数对IMU预积分因子模型进行更新，计算对应的IMU预积分因子并添加至因子图模型。
其中，当检测到IMU加速度、角速度量测值等IMU数据时，则将其添加进IMU预积分计算，得到载体坐标系下的IMU等效观测量。设导航状态量输出时刻为(k=0,1,…)，则IMU预积分周期为/>，综合一个周期内的IMU数据，得到在/>时刻载体系下IMU等效观测量（位置增量/>、速度/>、姿态增量/>及/>时刻加速度计零偏/>、陀螺仪零偏/>）如下：
公式7；
其中，表示相对于t时刻载体系下t时刻的3维加速度量测，/>表示相对于t时刻载体系下t时刻的3维角速度量测，/>是t时刻载体系相对于/>时刻载体系的姿态变换四元数，/>和/>是加速度计和陀螺仪零偏对应的高斯白噪声。
进一步地，IMU预积分因子用于构建相邻两历元的前16维状态变量的转移关系。基于INS航位推算的原理反推IMU预积分的等效观测量，假设IMU等效观测量的误差服从高斯分布，则IMU预积分因子模型可表示如下：
公式8；
转换得到IMU预积分因子的误差函数：
公式9；
式中，是IMU预积分因子的协方差矩阵，其可通过惯导器件的参数指标和状态误差卡尔曼滤波（ESKF）原理计算得到。
在步骤S103中，当检测到全球卫星导航系统GNSS数据时，将对应的GNSS因子添加至因子图模型。
具体地，基于当前的状态变量的GNSS数据、伪距和伪距率，构建GNSS因子模型；
基于GNSS数据的误差函数对GNSS因子模型进行更新，计算对应的GNSS因子并添加至因子图模型。
GNSS因子用于约束当前历元的状态变量，校正INS递推误差。针对当前历元的每一个卫星信号均添加一个GNSS因子，假设GNSS量测误差服从高斯分布，则GNSS因子模型可表示为：
公式10；
转换得到GNSS因子的误差函数：
公式11；
式中，是GNSS因子的协方差矩阵，表示为：
公式12；
进一步地，基于连续的至少两个状态变量的接收机时钟偏差和时钟漂移，构建接收机时钟转移因子模型；基于接收机时钟转移因子模型，获取接收机时钟转移因子对应的误差函数，并将对应接收机时钟转移因子添加至因子图模型。
具体地，接收机时钟转移因子用于约束两个连续历元下的接收机钟差、钟漂状态。采用恒定漂移模型建模，并假设该模型系统误差服从高斯分布，接收机时钟转移因子模型可表示为：
公式13；
转换得到接收机时钟转移因子的误差函数：
公式14；
式中，是与历元k-1和历元k之间的接收机时钟转移因子的协方差矩阵，表示为：
公式15；
在步骤S104中，对滑动窗口长度外的因子图模型中时间最早的状态变量进行边缘化删除，并构建滑窗因子图模型。
具体地，判断建滑窗因子图模型状态变量个数是否超过滑动窗口长度，如果状态变量个数超过滑动窗口长度，则将滑动窗口中最早的状态变量进行边缘化删除。
当检测到状态变量个数已超过滑动窗口长度，则删除滑动窗口长度中最早的状态变量，并基于最后一次图优化得到的边缘化的状态变量的状态值构造先验因子，添加至滑窗因子图模型，从而控制优化窗口的大小，并减少信息损失。
在步骤S105中，基于滑窗因子图模型进行图优化解算，计算待估测列车的定位状态信息。
具体地，对滑窗因子图模型进行图优化解算，得到滑窗因子图中待估测列车的状态估计结果。
根据滑窗因子图中的4类因子构造图优化目标函数，转化为最大后验概率问题估计，然后采用C++非线性求解器GTSAM，利用Levenberg-Marquardt算法求解方程，得到状态变量估计值，从而获取待估测列车的定位状态信息。如下公式17：
本发明各个实施例均采用递进的方式描述，各实施例之间相同或相似的部分相互关联。本公开利用因子图优化框架进行GNSS和IMU数据的多源融合，在解算过程中多次迭代、重复线性化、充分考虑历史量测信息，相较于传统滤波类方式实现更高精度的列车位置实时估计。
将GNSS与INS融合进行列车定位能够实现优势互补，一方面，卫星导航系统的定位信息可实时校正惯导的累积误差，实现惯导的在线标定；另一方面，当卫星信号受到环境遮挡，可见卫星数少于四颗时，GNSS仍可提供观测信息辅助惯导实现列车定位。因此，基于GNSS和INS的列车多源融合定位是新型列控系统的研究趋势。
基于状态转移系统方程、多传感器量测方程构建滑窗因子图模型，根据系统、量测误差分布模型构造各因子的误差函数，引入贝叶斯公式转化得到因子图的目标函数，最终采用Gauss-Newton（GN）或者Levenberg-Marquardt（LM）最优化算法求解，实时得到列车定位估计结果,可适用于长距离、环境复杂多变的铁路列车定位中，对列车静态及动态定位具有通用性，工程应用价值显著。
图2是根据本公开第一实施例中一个可能的实现方式的示意图。 其中，步骤之间的执行顺序如下：
S201，构造多维状态变量；
S202，确定系统初始状态先验信息，添加系统初始状态先验信息和先验因子；
S203，判断是否检测到IMU数据，若检测到则进行步骤S204；若未检测到，则返回执行步骤S203；
S204，基于检测到的IMU数据，进行IMU预积分计算；
S205，判断是否检测到GNSS数据，若检测到GNSS数据，则进行步骤S206；若未检测到，则返回执行步骤S203；
S206，添加当前状态变量至因子图模型；
S207，添加GNSS因子；
S208，添加IMU预积分因子至因子图模型；
S209，添加接收机时钟转移因子至因子图模型；
S210，判断滑动窗口内因子图模型的状态变量个数是否超过滑动窗口长度；若是则进行步骤S211，若不是则执行步骤S212；
S211，进行边缘化删除；
S212，进行图优化解算；
S213，判断是否停止定位，若是则流程结束；否则返回执行步骤S203。
如图3所示，为根据本公开第一实施例中的滑窗因子图模型的示意图，其中，状态变量之间通过先验因子、IMU预积分因子、GNSS因子和钟差状态转移因子构建因子图模型。如滑动窗口长度为L，进行边缘化删除，构建滑窗因子图模型。
需要说明的是，本公开中前述步骤的执行主体的部分或全部可以为位于本地终端的应用，或者还可以为设置在位于本地终端的应用中的插件或软件开发工具包（SoftwareDevelopment Kit，SDK）等功能单元，或者还可以为位于网络侧服务器中的处理引擎，或者还可以为位于网络侧的分布式系统，例如，网络侧的视频处理平台中的处理引擎或者分布式系统等，本实施例对此不进行特别限定。
可以理解的是，所述应用可以是安装在本地终端上的本地程序（native App），或者还可以是本地终端上的浏览器的一个网页程序（web App），本实施例对此不进行限定。
需要说明的是，对于前述的各方法实施例，为了简单描述，故将其都表述为一系列的动作组合，但是本领域技术人员应该知悉，本公开并不受所描述的动作顺序的限制，因为依据本公开，某些步骤可以采用其他顺序或者同时进行。其次，本领域技术人员也应该知悉，说明书中所描述的实施例均属于优选实施例，所涉及的动作和模块并不一定是本公开所必须的。
在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其他实施例的相关描述。
可以理解的是，所述应用可以是安装在本地终端上的本地程序（native App），或者还可以是本地终端上的浏览器的一个网页程序（web App），本实施例对此不进行限定。
本公开的技术方案中，所涉及的用户个人信息的收集、存储、使用、加工、传输、提供和公开等处理，均符合相关法律法规的规定，且不违背公序良俗。
根据本公开的实施例，本公开还提供了一种电子设备、一种可读存储介质和一种计算机程序产品。
图4示出了可以用来实施本公开的实施例的示例电子设备400的示意性框图。电子设备旨在表示各种形式的数字计算机，诸如，膝上型计算机、台式计算机、工作台、个人数字助理、服务器、刀片式服务器、大型计算机、和其它适合的计算机。电子设备还可以表示各种形式的移动装置，诸如，个人数字处理、蜂窝电话、智能电话、可穿戴设备和其它类似的计算装置。本文所示的部件、它们的连接和关系、以及它们的功能仅仅作为示例，并且不意在限制本文中描述的和/或者要求的本公开的实现。
如图4所示，电子设备400包括计算单元401，其可以根据存储在只读存储器（ROM）402中的计算机程序或者从存储单元408加载到随机访问存储器（RAM）403中的计算机程序，来执行各种适当的动作和处理。在RAM 403中，还可存储电子设备400操作所需的各种程序和数据。计算单元401、ROM 402以及RAM 403通过总线404彼此相连。输入/输出（I/O）接口405也连接至总线404。
电子设备400中的多个部件连接至I/O接口405，包括：输入单元406，例如键盘、鼠标等；输出单元407，例如各种类型的显示器、扬声器等；存储单元408，例如磁盘、光盘等；以及通信单元409，例如网卡、调制解调器、无线通信收发机等。通信单元409允许电子设备400通过诸如因特网的计算机网络和/或各种电信网络与其他设备交换信息/数据。
计算单元401可以是各种具有处理和计算能力的通用和/或专用处理组件。计算单元401的一些示例包括但不限于中央处理单元（CPU）、图形处理单元（GPU）、各种专用的人工智能（AI）计算芯片、各种运行机器学习模型算法的计算单元、数字信号处理器（DSP）、以及任何适当的处理器、控制器、微控制器等。计算单元401执行上文所描述的各个方法和处理，例如网页相似度模型的训练方法、网页相似度的预测方法。例如，在一些实施例中，网页相似度模型的训练方法、网页相似度的预测方法可被实现为计算机软件程序，其被有形地包含于机器可读介质，例如存储单元408。在一些实施例中，计算机程序的部分或者全部可以经由ROM402和/或通信单元409而被载入和/或安装到电子设备400上。当计算机程序加载到RAM 403并由计算单元401执行时，可以执行上文描述的网页相似度模型的训练方法、网页相似度的预测方法的一个或多个步骤。备选地，在其他实施例中，计算单元401可以通过其他任何适当的方式（例如，借助于固件）而被配置为执行网页相似度模型的训练方法、网页相似度的预测方法。
本文中以上描述的系统和技术的各种实施方式可以在数字电子电路系统、集成电路系统、场可编程门阵列（FPGA）、专用集成电路（ASIC）、专用标准产品（ASSP）、芯片上系统的系统（SOC）、复杂可编程逻辑设备（CPLD）、计算机硬件、固件、软件、和/或它们的组合中实现。这些各种实施方式可以包括：实施在一个或者多个计算机程序中，该一个或者多个计算机程序可在包括至少一个可编程处理器的可编程系统上执行和/或解释，该可编程处理器可以是专用或者通用可编程处理器，可以从存储系统、至少一个输入装置、和至少一个输出装置接收数据和指令，并且将数据和指令传输至该存储系统、该至少一个输入装置、和该至少一个输出装置。
用于实施本公开的方法的程序代码可以采用一个或多个编程语言的任何组合来编写。这些程序代码可以提供给通用计算机、专用计算机或其他可编程数据处理装置的处理器或控制器，使得程序代码当由处理器或控制器执行时使流程图和/或框图中所规定的功能/操作被实施。程序代码可以完全在机器上执行、部分地在机器上执行，作为独立软件包部分地在机器上执行且部分地在远程机器上执行或完全在远程机器或服务器上执行。
在本公开的上下文中，机器可读介质可以是有形的介质，其可以包含或存储以供指令执行系统、装置或设备使用或与指令执行系统、装置或设备结合地使用的程序。机器可读介质可以是机器可读信号介质或机器可读储存介质。机器可读介质可以包括但不限于电子的、磁性的、光学的、电磁的、红外的、或半导体系统、装置或设备，或者上述内容的任何合适组合。机器可读存储介质的更具体示例会包括基于一个或多个线的电气连接、便携式计算机盘、硬盘、随机存取存储器（RAM）、只读存储器（ROM）、可擦除可编程只读存储器（EPROM或快闪存储器）、光纤、便捷式紧凑盘只读存储器（CD-ROM）、光学储存设备、磁储存设备、或上述内容的任何合适组合。
为了提供与用户的交互，可以在计算机上实施此处描述的系统和技术，该计算机具有：用于向用户显示信息的显示装置（例如，CRT（阴极射线管）或者LCD（液晶显示器）监视器）；以及键盘和指向装置（例如，鼠标或者轨迹球），用户可以通过该键盘和该指向装置来将输入提供给计算机。其它种类的装置还可以用于提供与用户的交互；例如，提供给用户的反馈可以是任何形式的传感反馈（例如，视觉反馈、听觉反馈、或者触觉反馈）；并且可以用任何形式（包括声输入、语音输入或者、触觉输入）来接收来自用户的输入。
可以将此处描述的系统和技术实施在包括后台部件的计算系统（例如，作为数据服务器）、或者包括中间件部件的计算系统（例如，应用服务器）、或者包括前端部件的计算系统（例如，具有图形用户界面或者网络浏览器的用户计算机，用户可以通过该图形用户界面或者该网络浏览器来与此处描述的系统和技术的实施方式交互）、或者包括这种后台部件、中间件部件、或者前端部件的任何组合的计算系统中。可以通过任何形式或者介质的数字数据通信（例如，通信网络）来将系统的部件相互连接。通信网络的示例包括：局域网（LAN）、广域网（WAN）和互联网。
计算机系统可以包括客户端和服务器。客户端和服务器一般远离彼此并且通常通过通信网络进行交互。通过在相应的计算机上运行并且彼此具有客户端-服务器关系的计算机程序来产生客户端和服务器的关系。服务器可以是云服务器，也可以为分布式系统的服务器，或者是结合了区块链的服务器。
Claims (10)
1.一种基于因子图优化的卫导/惯导列车多源融合定位的方法，包括：
获取待估测列车对应的多维状态变量、系统初始状态先验信息和先验因子，生成因子图模型；
当检测到惯性传感器IMU数据时，将对应的IMU预积分因子添加至所述因子图模型；
当检测到全球卫星导航系统GNSS数据时，将对应的GNSS因子添加至所述因子图模型；
对滑动窗口长度外的所述因子图模型中时间最早的状态变量进行边缘化删除，并构建滑窗因子图模型；
基于所述滑窗因子图模型进行图优化解算，计算所述待估测列车的定位状态信息。
2.根据权利要求1所述的方法，其中，所述获取待估测列车对应的多维状态变量、系统初始状态先验信息和先验因子，生成因子图模型，包括：
选取地心地固ECEF为参考坐标系，获取所述多维状态变量，其中，所述多维状态变量包括组成所述因子图模型的至少一个状态变量对应的所述待估测列车的三维位置信息、三维速度信息、姿态信息、惯导零偏、GNSS接收机时钟偏差的等效伪距测量误差和时钟漂移的等效伪距测量误差；
基于传感器测量结果和/或预设值，确定所述系统初始状态先验信息；
基于先验信息对所述系统初始状态先验信息进行对准，获取先验因子及所述先验因子对应的误差函数；
基于所述多维状态变量、先验模型、所述系统初始状态先验信息和所述先验因子构建所述因子图模型。
3.根据权利要求2所述的方法，其中，所述基于先验信息对所述系统初始状态先验信息进行对准，获取先验因子及所述先验因子对应的误差函数，包括：
基于所述先验信息和系统初始状态之间的对应关系，构建先验因子模型，并基于先验因子模型获取先验因子对应的误差函数。
4.根据权利要求1所述的方法，其中，所述当检测到惯性传感器IMU数据时，将对应的IMU预积分因子添加至所述因子图模型，包括：
获取预设周期内的所述IMU数据，确定导航状态量输出时刻的IMU等效观测量；其中，所述IMU等效观测量包括位置增量、速度增量、姿态增量、下一时刻的加速度计零偏和陀螺仪零偏；
进行IMU预积分计算，将对应的所述IMU预积分因子添加至所述因子图模型。
5.根据权利要求4所述的方法，其中，所述进行IMU预积分计算，将对应的所述IMU预积分因子添加至所述因子图模型，包括：
基于相邻的所述状态变量的所述IMU等效观测量和所述IMU预积分因子，构建IMU预积分因子模型；
基于所述IMU预积分因子的误差函数对所述IMU预积分因子模型进行更新，计算对应的所述IMU预积分因子并添加至所述因子图模型。
6.根据权利要求1所述的方法，其中，所述当检测到全球卫星导航系统GNSS数据时，将对应的GNSS因子添加至所述因子图模型，包括：
基于当前的所述状态变量的所述GNSS数据、伪距和距率观测值，构建GNSS因子模型；
基于所述GNSS因子模型的误差函数对所述GNSS因子模型进行更新，计算对应的所述GNSS因子并添加至所述因子图模型。
7.根据权利要求1所述的方法，其中，所述方法还包括：
基于连续的至少两个所述状态变量的接收机时钟偏差和时钟漂移，构建接收机时钟转移因子模型；
基于接收机时钟转移因子模型，获取接收机时钟转移因子对应的误差函数，并将对应所述接收机时钟转移因子添加至所述因子图模型。
8.一种电子设备，包括：
至少一个处理器；以及
与所述至少一个处理器通信连接的存储器；其中，
所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行根据权利要求1-7中任一项所述的方法。
9.一种存储有计算机指令的非瞬时计算机可读存储介质，其中，所述计算机指令用于使所述计算机执行根据权利要求1-7中任一项所述的方法。
10.一种计算机程序产品，包括计算机程序，所述计算机程序在被处理器执行时实现根据权利要求1-7中任一项所述的方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311083417.4A CN116817928B (zh) | 2023-08-28 | 2023-08-28 | 基于因子图优化的卫导/惯导列车多源融合定位的方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311083417.4A CN116817928B (zh) | 2023-08-28 | 2023-08-28 | 基于因子图优化的卫导/惯导列车多源融合定位的方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116817928A true CN116817928A (zh) | 2023-09-29 |
| CN116817928B CN116817928B (zh) | 2023-12-01 |
Family
ID=88120674
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311083417.4A Active CN116817928B (zh) | 2023-08-28 | 2023-08-28 | 基于因子图优化的卫导/惯导列车多源融合定位的方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116817928B (zh) |
Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108196289A (zh) * | 2017-12-25 | 2018-06-22 | 北京交通大学 | 一种卫星信号受限条件下的列车组合定位方法 |
| CN111795686A (zh) * | 2020-06-08 | 2020-10-20 | 南京大学 | 一种移动机器人定位与建图的方法 |
| CN113945206A (zh) * | 2020-07-16 | 2022-01-18 | 北京图森未来科技有限公司 | 一种基于多传感器融合的定位方法及装置 |
| CN114018274A (zh) * | 2021-11-18 | 2022-02-08 | 阿波罗智能技术(北京)有限公司 | 车辆定位方法、装置及电子设备 |
| WO2022257801A1 (zh) * | 2021-06-09 | 2022-12-15 | 山东大学 | 基于slam的移动机器人矿山场景重建方法及系统 |
| CN115685292A (zh) * | 2023-01-05 | 2023-02-03 | 中国人民解放军国防科技大学 | 一种多源融合导航系统的导航方法和装置 |
| CN116086446A (zh) * | 2023-02-28 | 2023-05-09 | 东南大学 | 一种基于柔性卡方检测的自适应因子图优化组合导航方法 |
| CN116086445A (zh) * | 2023-02-28 | 2023-05-09 | 东南大学 | 一种基于因子图优化的多源信息时延融合导航方法 |
| CN116105729A (zh) * | 2023-02-24 | 2023-05-12 | 西北工业大学 | 一种针对野外洞穴森林环境侦察的多传感器融合定位方法 |
| CN116558512A (zh) * | 2023-06-19 | 2023-08-08 | 同济大学 | 一种基于因子图的gnss与车载传感器融合定位方法及系统 |
| CN116576849A (zh) * | 2023-06-19 | 2023-08-11 | 同济大学 | 一种基于gmm辅助的车辆融合定位方法及系统 |
-
2023
- 2023-08-28 CN CN202311083417.4A patent/CN116817928B/zh active Active
Patent Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108196289A (zh) * | 2017-12-25 | 2018-06-22 | 北京交通大学 | 一种卫星信号受限条件下的列车组合定位方法 |
| CN111795686A (zh) * | 2020-06-08 | 2020-10-20 | 南京大学 | 一种移动机器人定位与建图的方法 |
| CN113945206A (zh) * | 2020-07-16 | 2022-01-18 | 北京图森未来科技有限公司 | 一种基于多传感器融合的定位方法及装置 |
| WO2022257801A1 (zh) * | 2021-06-09 | 2022-12-15 | 山东大学 | 基于slam的移动机器人矿山场景重建方法及系统 |
| CN114018274A (zh) * | 2021-11-18 | 2022-02-08 | 阿波罗智能技术(北京)有限公司 | 车辆定位方法、装置及电子设备 |
| CN115685292A (zh) * | 2023-01-05 | 2023-02-03 | 中国人民解放军国防科技大学 | 一种多源融合导航系统的导航方法和装置 |
| CN116105729A (zh) * | 2023-02-24 | 2023-05-12 | 西北工业大学 | 一种针对野外洞穴森林环境侦察的多传感器融合定位方法 |
| CN116086446A (zh) * | 2023-02-28 | 2023-05-09 | 东南大学 | 一种基于柔性卡方检测的自适应因子图优化组合导航方法 |
| CN116086445A (zh) * | 2023-02-28 | 2023-05-09 | 东南大学 | 一种基于因子图优化的多源信息时延融合导航方法 |
| CN116558512A (zh) * | 2023-06-19 | 2023-08-08 | 同济大学 | 一种基于因子图的gnss与车载传感器融合定位方法及系统 |
| CN116576849A (zh) * | 2023-06-19 | 2023-08-11 | 同济大学 | 一种基于gmm辅助的车辆融合定位方法及系统 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116817928B (zh) | 2023-12-01 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP7299261B2 (ja) | 車両のデッドレコニング方法、装置、デバイス、記憶媒体、及びプログラム | |
| CN111721289B (zh) | 自动驾驶中车辆定位方法、装置、设备、存储介质及车辆 | |
| CN114018274B (zh) | 车辆定位方法、装置及电子设备 | |
| CN115265523B (zh) | 机器人同时定位与建图方法、装置及可读介质 | |
| CN114179825B (zh) | 多传感器融合获取量测值置信度方法及自动驾驶车辆 | |
| CN113933818A (zh) | 激光雷达外参的标定的方法、设备、存储介质及程序产品 | |
| CN112800159B (zh) | 地图数据处理方法及装置 | |
| CN115457152A (zh) | 外参标定方法、装置、电子设备及存储介质 | |
| CN111678513A (zh) | 一种超宽带/惯导紧耦合的室内定位装置与系统 | |
| CN113219505B (zh) | 用于车路协同隧道场景的采集gps坐标的方法、装置和设备 | |
| CN112782732B (zh) | 一种基于粒子群算法的导航信号解析方法及计算机存可读介质 | |
| CN113218389A (zh) | 一种车辆定位方法、装置、存储介质及计算机程序产品 | |
| CN116817928B (zh) | 基于因子图优化的卫导/惯导列车多源融合定位的方法 | |
| CN116929343A (zh) | 位姿估计方法、相关设备及存储介质 | |
| CN117387604A (zh) | 基于4d毫米波雷达和imu融合的定位与建图方法及系统 | |
| CN115900697B (zh) | 对象运动轨迹信息处理方法、电子设备以及自动驾驶车辆 | |
| CN111521187A (zh) | 组合导航方法、装置、设备及存储介质 | |
| CN114088104B (zh) | 一种自动驾驶场景下的地图生成方法 | |
| CN115727871A (zh) | 一种轨迹质量检测方法、装置、电子设备和存储介质 | |
| CN116380070A (zh) | 一种基于时间戳优化的视觉惯性定位方法 | |
| CN115032672A (zh) | 一种基于定位子系统的融合定位方法及系统 | |
| CN115147561A (zh) | 位姿图生成方法、高精地图生成方法和装置 | |
| CN114488237A (zh) | 定位方法、装置、电子设备及智能驾驶方法 | |
| CN116380056B (zh) | 惯性定位方法、装置、电子设备及存储介质 | |
| CN116448105B (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 |