CN116558512A - 一种基于因子图的gnss与车载传感器融合定位方法及系统 - Google Patents

一种基于因子图的gnss与车载传感器融合定位方法及系统 Download PDF

Info

Publication number
CN116558512A
CN116558512A CN202310730402.6A CN202310730402A CN116558512A CN 116558512 A CN116558512 A CN 116558512A CN 202310730402 A CN202310730402 A CN 202310730402A CN 116558512 A CN116558512 A CN 116558512A
Authority
CN
China
Prior art keywords
vehicle
factor
gnss
imu
measurement
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.)
Pending
Application number
CN202310730402.6A
Other languages
English (en)
Inventor
朱佳琪
卓桂荣
熊璐
李子尧
周桓宇
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Tongji University
Original Assignee
Tongji University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Tongji University filed Critical Tongji University
Priority to CN202310730402.6A priority Critical patent/CN116558512A/zh
Publication of CN116558512A publication Critical patent/CN116558512A/zh
Pending legal-status Critical Current

Links

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/20—Instruments for performing navigational calculations
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

本发明涉及一种基于因子图的GNSS与车载传感器融合定位方法及系统，该方法包括：根据IMU的角速度和加速度测量信息，得到IMU预积分项；根据轮速传感器的速度测量信息，并结合车辆二自由度模型，计算得到动力学预积分项；获得GNSS测量信号后，根据GNSS接收机的原始观测信息，结合系统状态，构建伪距因子；基于GNSS接收机时钟误差，构建时钟漂移因子；整合IMU预积分项和动力学预积分项，构建车载传感器因子；联合车载传感因子、伪距因子和时钟漂移因子，构造因子图，之后通过优化求解因子图，估计得到车辆的定位信息。与现有技术相比，本发明能够避免由于INS误差累积导致GNSS异常测量时存在的定位不准确问题，能够低成本、鲁棒地实现高精度定位。

Description

一种基于因子图的GNSS与车载传感器融合定位方法及系统
技术领域
本发明涉及自动驾驶车辆定位技术领域，尤其是涉及一种基于因子图的GNSS与车载传感器融合定位方法及系统。
背景技术
对于L3及以上级别的自动驾驶汽车而言，高精度定位是一项必不可少的基础技术，精确的位置、速度和姿态信息对于车辆实现自动驾驶而言至关重要，它影响着运动规划及决策和运动跟踪的性能。
GNSS(Global Navigation Satellite System，全球卫星导航系统)是对GPS(Global Positioning System，全球定位系统)、BDS(Beidou Navigation SatelliteSystem，北斗卫星导航系统)、GLONASS(Global'naya Navigatsionnaya SputnikovayaSistema，格洛纳斯卫星导航系统)、GALILEO(Galileo satellite navigation system，伽利略卫星导航系统)等各国的单个卫星导航定位系统的统一称谓，GNSS定位的算法主要有RTK(载波相位差分技术)和INS(惯性导航系统)，可以同时获取不同卫星系统的数据，从而保证数据的稳定性和精度，正在逐渐成为自动驾驶领域的主流定位方式，相较于传统的单一卫星的定位方式，厘米级的定位精度使得自动驾驶的安全性和准确性得到了极大的保障。
GNSS在开阔场景可获得较好的定位精度，然而，在城市峡谷、隧道、林荫等场景中，由于多径效应和非视距(Non Line of Sight，NLOS)接收的影响，导致定位性能显著降低。针对这一问题，目前常见的解决方法有：
1、利用扼流环、双极化天线和天线阵列等方法，从硬件上减轻信号干扰，但是这种方式需要设计复杂的天线，导致成本较高；
2、利用信号强度、卫星仰角等GNSS测量信息或者基于激光雷达、相机等传感器，对卫星能见度进行分类，从而排除NLOS接收或者校正NLOS接收提高GNSS定位性能，但此类方法的性能依赖于卫星能见度分类的质量，且常常只能检测到部分NLOS接收，精度受限；
3、使用不同传感器的数据进行融合定位，其中GNSS/INS的融合定位被广泛采用，但由于INS的误差累积特性，导致GNSS存在较长时间的异常测量，影响定位性能；为此现有技术进一步融合相机、激光雷达等传感器，以改善由于误差累加导致的GNSS异常测量，但是受环境特点分布影响严重，在多变环境中难以保证连续鲁棒的定位。
发明内容
本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于因子图的GNSS与车载传感器融合定位方法，能够低成本、鲁棒地实现高精度定位。
本发明的目的可以通过以下技术方案来实现：一种基于因子图的GNSS与车载传感器融合定位方法，包括以下步骤：
S1、根据IMU的角速度和加速度测量信息，计算基于IMU的车辆位置、速度和姿态的预积分项，即得到IMU预积分项；
S2、根据轮速传感器的速度测量信息，并结合车辆二自由度模型，计算基于车辆动力学的车辆位置的预积分项，即得到动力学预积分项；
S3、获得GNSS测量信号后，根据GNSS接收机的原始观测信息，结合系统状态，构建伪距因子；
基于GNSS接收机时钟误差，构建时钟漂移因子；
整合IMU预积分项和动力学预积分项，构建车载传感器因子；
S4、联合车载传感因子、伪距因子和时钟漂移因子，构造因子图，之后通过优化求解因子图，估计得到车辆的定位信息。
进一步地，所述步骤S1的具体过程为：
获得IMU测量时刻t下IMU在载体坐标系b系下的加速度测量值at、角速度测量值ωt以及下一个IMU测量时刻t+1的加速度测量值at+1、角速度测量值ωt+1，两个IMU测量时刻的时间间隔为δt，基于中值积分的方法，计算IMU采样间隔内的预积分项为：
其中，为位置预积分项，/>为速度预积分项，/>为姿态预积分项，ba、bω分别为加速度计零偏、陀螺仪零偏，下标t、t+1表示IMU测量时刻，/>为t时刻的IMU加速度平均值，/>为t时刻的IMU角速度平均值；
利用GNSS测量信息的采样间隔[tk，tk+1]内所有IMU测量值进行预积分，即可得到[tk，tk+1]内总的IMU预积分项。
进一步地，所述步骤S2具体包括以下步骤：
S21、建立车辆二自由度模型，用于计算得到车辆坐标系中的速度；
S22、根据车辆坐标系中的速度，获得相邻轮速测量时刻之间的动力学预积分项，对GNSS测量信息的采样间隔[tk，tk+1]内所有轮速测量值的预积分项进行累加，即得到[tk，tk+1]内总的动力学预积分项。
进一步地，所述步骤S21中车辆二自由度模型具体为：
其中，kf和kr分别是前轴和后轴的侧偏刚度，Iz为横摆转动惯量，lf和lr分别表示质心距前轴和后轴的距离，m为整车质量，α为等效前轮转角；
考虑车辆稳态转向特性：则侧偏角βt和横摆速度ωr，t为：
其中，K＝m(lfkf-lrkr)/l2kfkr为稳定因子，vt、αt分别是t时刻测量的车轮速度和前轮角度，根据前轮与方向盘的传动比i，可得αt＝δα/i，其中δα为方向盘转角，由此得到车辆坐标系中的速度和角速度/>为：
进一步地，所述步骤S22的具体过程为：
根据步骤S21计算得到的车辆坐标系中的速度获得相邻轮速测量时刻[t，t+1]之间的动力学预积分项：
其中，δt为相邻轮速测量时刻之间的时间间隔，为相邻轮速测量时刻之间的位置预积分项，
对GNSS测量信息的采样间隔[tk，tk+1]内所有轮速测量值的预积分项进行累加，即可得到[tk，tk+1]内总的动力学预积分项：位置预积分项
进一步地，所述步骤S3中构建伪距因子的具体过程为：
GNSS接收机获取卫星的星历数据和伪距观测数据，在时刻tk的观测数据中，根据星历数据得到卫星sj位置卫星钟差/>大气层延迟δρkn，k、电离层延迟δρkp,k、接收机时钟误差δtk和地球自转的Sagnac效应带来的测距误差/>由下式计算：
对于tk时刻第j颗卫星的原始观测数据，考虑测距误差的伪距测量建模为：
其中，ωearth是地球自转角速度，c是光速，是GNSS接收机在ECEF坐标系中的坐标，与设置的状态量/>之间的关系为：
其中，是ECEF坐标系与ENU坐标系之间的变换矩阵，/>是从IMU中心到GNSS天线相位中心的杆臂，/>是载体坐标系与ENU坐标系之间的旋转矩阵，是状态量；
因此对于tk时刻第j颗卫星的原始观测数据，将系统状态和伪距相关测量联系起来的残差为：
其中，是伪距测量值，/>即为构建的伪距因子，X是待估计的状态量，基于滑动窗口的方式进行优化，窗口内的状态X为：
n是滑动窗口大小，状态量包括载体坐标系相对于ENU坐标系的位置速度/>姿态/>以及加速度计零偏/>陀螺仪零偏/>接收机时钟误差δti、接收机时钟漂移速率
进一步地，所述步骤S3中构建时钟漂移因子的具体过程为：
应用恒定时钟误差漂移(Constant Clock Error Drift，CCED)模型对GNSS接收机的时钟误差进行建模，构建得到时钟漂移因子为：
进一步地，所述步骤S3中构建车载传感器因子的具体过程为：
根据IMU预积分项和动力学预积分项，首先建立位置、速度和姿态的动力学更新方程，之后基于动力学更新方程，构建得到车载传感器因子。
进一步地，所述位置、速度和姿态的动力学更新方程具体为：
所述车载传感器因子具体为：
其中，为从ENU坐标系转换到载体坐标系的旋转矩阵，gn为重力加速度，/>为[tk，tk+1]内的观测量。
进一步地，所述步骤S4具体是使用Ceres Solver中的Levenberg-Marquardt算法优化求解因子图，估计得到车辆的位置、速度、姿态信息为：
其中，∑Vehicle、∑P、∑CCED分别为车载传感器因子、伪距因子和时钟漂移因子对应的标准差。
一种基于因子图的GNSS与车载传感器融合定位系统，包括输入模块、预积分模块以及因子图优化模块，所述输入模块分别与预积分模块、因子图优化模块相连接，所述预积分模块与因子图优化模块相连接，所述输入模块用于分别获取IMU、轮速传感器以及GNSS接收机输出的测量信息，并将IMU测量信息和轮速传感器测量信息传输给预积分模块、将GNSS测量信息传输给因子图优化模块；
所述预积分模块用于计算得到IMU预积分项和动力学预积分项，并将IMU预积分项和动力学预积分项传输给因子图优化模块；
所述因子图优化模块利用IMU预积分项和动力学预积分项构建车载传感器因子；
根据GNSS测量信息，构建伪距因子；
针对GNSS接收机时钟误差，构建时钟漂移因子；
并联合车载传感器因子、伪距因子和时钟漂移因子构造因子图，
再通过优化求解因子图，估计得到车辆定位信息。
与现有技术相比，本发明具有以下优点：
一、本发明首先根据IMU的角速度和加速度测量信息，计算得到IMU预积分项；之后根据轮速传感器的速度测量信息，并基于车辆二自由度模型，计算得到动力学预积分项；当获得GNSS测量信号后，一方面利用IMU预积分项和动力学预积分项联合构建车载传感器因子、根据GNSS接收机的原始观测信息构建伪距因子、针对接收机时钟误差构建时钟漂移因子，另一方面联合车载传感器因子、伪距因子和时钟漂移因子构造因子图，通过优化求解因子图，以估计得到车辆定位信息。由此实现一种GNSS与车载传感器紧耦合鲁棒定位方案，能够避免由于INS误差累积导致GNSS异常测量时存在的定位不准确问题，同时无需设置用于减轻信号干扰的硬件设施，即可低成本地提升车辆定位的准确性。
二、本发明计算动力学预积分项时，充分考虑到车辆稳态转向特性，以构建出车辆二自由度动力学模型，并利用车辆二自由度模型得到车辆坐标系中的速度，从而进一步获得相邻轮速测量时刻之间的动力学预积分项，由此确保了计算得到动力学预积分项的准确性。
三、本发明在构造因子图时，根据GNSS接收机的原始观测信息，结合系统状态，构建伪距因子；并基于GNSS接收机时钟误差，构建时钟漂移因子；还通过整合IMU预积分项和动力学预积分项，构建车载传感器因子；最后联合车载传感因子、伪距因子和时钟漂移因子，构造因子图。其中，在构建伪距因子时，充分考虑GNSS接收机时钟误差和地球自转Sagnac效应带来的测距误差；在构建时钟漂移因子时，则考虑到GNSS接收机误差会以一定速度漂移的特性；在构建车载传感器因子时，通过建立位置、速度和姿态的动力学更新方程以确定出车载传感器因子。由此充分保证了因子图的可靠性，能够准确估计出车辆的位置、速度、姿态等定位信息。
附图说明
图1为本发明的方法流程示意图；
图2为实施例中线性二自由度车辆动力学模型示意图；
图3为实施例中构造的因子图结构示意图；
图4为实施例中搭建的系统结构示意图。
具体实施方式
下面结合附图和具体实施例对本发明进行详细说明。
实施例
如图1所示，一种基于因子图的GNSS与车载传感器融合定位方法，包括以下步骤：
S1、根据IMU的角速度和加速度测量信息，计算基于IMU的车辆位置、速度和姿态的预积分项，即得到IMU预积分项；
S2、根据轮速传感器的速度测量信息，并结合车辆二自由度模型，计算基于车辆动力学的车辆位置的预积分项，即得到动力学预积分项；
S3、获得GNSS测量信号后，根据GNSS接收机的原始观测信息，结合系统状态，构建伪距因子；
基于GNSS接收机时钟误差，构建时钟漂移因子；
整合IMU预积分项和动力学预积分项，构建车载传感器因子；
S4、联合车载传感因子、伪距因子和时钟漂移因子，构造因子图，之后通过优化求解因子图，估计得到车辆的定位信息。
本实施例应用上述技术方案，主要内容包括：
一、根据IMU的角速度和加速度测量信息，计算基于IMU的车辆位置、速度和姿态的预积分项；
具体的，本实施例首先选定因子图更新周期与GNSS的测量周期相同。由于GNSS的输出频率远低于IMU的输出频率，因此在GNSS输出测量值的一个时间间隔[tk，tk+1]内，会有多个IMU的测量值。考虑其中一个IMU测量时刻t+1，获得IMU在载体坐标系b系下的加速度测量值at+1和角速度测量值ωt+1，上一IMU测量时刻t的IMU加速度测量值为at和角速度测量值为ωt，两个IMU采样的时间间隔为δt，基于中值积分的方法，计算IMU采样间隔内的预积分项：
其中：
对[tk，tk+1]时间间隔内所有IMU测量值的预积分项进行累加，即可得到[tk，tk+1]内总的IMU预积分项：位置预积分项速度预积分项/>姿态预积分项/>ba、bω分别是加速度计、陀螺仪零偏。
二、根据轮速传感器的速度测量信息，基于车辆二自由度模型(如图2所示)，计算基于车辆动力学的车辆位置的预积分项；
21)基于如下车辆二自由度模型：
其中，kf和kr分别是前轴和后轴的侧偏刚度，Iz为横摆转动惯量，lf和lr分别表示质心距前轴和后轴的距离，m为整车质量，α为等效前轮转角。
考虑车辆稳态转向特性，则侧偏角βt和横摆速度ωr，t为：
其中，稳定因子K＝m(lfkf-lrkr)/l2kfkr，vt、αt分别是t时刻测量的车轮速度和前轮角度，根据前轮与方向盘的传动比i，可得αt＝δα/i，其中δα为方向盘转角，得到车辆坐标系中的速度和角速度/>为：
22)基于步骤21)中计算得到的车辆坐标系中的速度获得相邻轮速测量时刻[t，t+1]之间的动力学预积分项，其中，两个时刻之间的时间间隔为δt：
对[tk，tk+1]时间间隔内所有轮速测量值的预积分项进行累加，即可得到[tk，tk+1]内总的动力学预积分项：位置预积分项
三、在[tk，tk+1]时间间隔内，根据GNSS接收机的原始观测信息，结合系统状态，计算伪距残差，构建伪距因子；并对接收机时钟误差应用恒定时钟误差漂移(CCED)模型建模，构建时钟漂移因子；
31)本实施例设置的状态量包括：载体坐标系相对于ENU坐标系的位置速度姿态/>加速度计零偏/>陀螺仪零偏/>接收机时钟误差δti和接收机时钟漂移速率/>
基于滑动窗口的方式进行优化，窗口内的状态X可以总结为：
其中n是滑动窗口大小。
32)由于各卫星系统的时钟误差均不相同，多星座需要考虑星座间的时钟误差，因此本实施例仅使用GPS单卫星系统，GNSS接收机可以获取卫星的星历数据和伪距观测数据。在时刻tk的观测数据中，根据星历数据可以得到卫星sj位置 卫星钟差/>大气层延迟δρkn，k及电离层延迟δρkp，k。另外GNSS接收机时钟误差δtk和地球自转的Sagnac效应带来的测距误差/>也不容忽略，由于δtk是最大分量且随着时间的推移而变化，必须与GNSS接收机的位置一起估计，/>由下式计算：
对于tk时刻第j颗卫星的原始观测数据，考虑上述测距误差的伪距测量建模为：
其中，ωearth是地球自转角速度，c是光速，是GNSS接收机在ECEF坐标系中的坐标，其与设置的状态量/>之间的关系为：
式中，是ECEF坐标系与ENU坐标系之间的变换矩阵，本文将其设置为定值，由起点处的ECEF坐标计算得到；/>是从IMU中心到GNSS天线相位中心的杆臂。/>是载体坐标系与ENU坐标系之间的旋转矩阵，是状态量。
因此对于tk时刻第j颗卫星的原始观测数据，将系统状态和伪距相关测量联系起来的残差可以表述为：
式中，是伪距测量值。/>即为构建的伪距因子。
33)由于GNSS接收机的时钟误差不是一个定值，而是通常以一定的速度漂移，故本方案应用恒定时钟误差漂移(CCED)模型对其进行建模，构建的时钟漂移因子为：
四、在GNSS测量到来时，联合[tk，tk+1]内IMU预积分项和动力学预积分项，构建车载传感器因子；再结合车载传感器因子、伪距因子和时钟漂移因子构造因子图(如图3所示)，通过优化求解因子图，估计得到车辆的位置、速度、姿态等车辆定位信息。
41)在tk+1时刻GNSS测量到来时，根据IMU预积分项(位置预积分项速度预积分项/>姿态预积分项/>)和动力学预积分项(位置预积分项/>)，构建的位置、速度和姿态的动力学更新方程为：
因此构建的车载传感器因子为：
42)在tk+1时刻，由车载传感器因子、伪距因子和时钟漂移因子构造因子图，使用Ceres Solver中的Levenberg-Marquardt算法优化求解因子图，估计车辆的位置、速度、姿态等信息：
式中，∑Vehicle、∑P、∑CCED分别是车载传感器因子、伪距因子和时钟漂移因子的标准差。
基于上述方法过程，本实施例搭建如图4所示的系统结构，包括输入模块、预积分模块以及因子图优化模块，其中，输入模块分别与预积分模块、因子图优化模块相连接，预积分模块与因子图优化模块相连接，输入模块用于分别获取IMU、轮速传感器以及GNSS接收机输出的测量信息，并将IMU测量信息和轮速传感器测量信息传输给预积分模块、将GNSS测量信息传输给因子图优化模块；
预积分模块用于计算得到IMU预积分项和动力学预积分项，并将IMU预积分项和动力学预积分项传输给因子图优化模块；
因子图优化模块则利用IMU预积分项和动力学预积分项构建车载传感器因子；
根据GNSS测量信息，构建伪距因子；
针对GNSS接收机时钟误差，构建时钟漂移因子；
并联合车载传感器因子、伪距因子和时钟漂移因子构造因子图，
再通过优化求解因子图，估计得到车辆定位信息。
综上可知，本技术方案实现了一种基于因子图的GNSS与车载传感器紧耦合定位方案，能够避免由于INS误差累积导致GNSS存在较长时间异常测量时存在的定位不准确问题，能够在多变环境中可靠保证连续鲁棒的定位，具有成本低廉、估计精度高、计算量小等优点。

Claims (10)

1.一种基于因子图的GNSS与车载传感器融合定位方法，其特征在于，包括以下步骤：
S1、根据IMU的角速度和加速度测量信息，计算基于IMU的车辆位置、速度和姿态的预积分项，即得到IMU预积分项；
S2、根据轮速传感器的速度测量信息，并结合车辆二自由度模型，计算基于车辆动力学的车辆位置的预积分项，即得到动力学预积分项；
S3、获得GNSS测量信号后，根据GNSS接收机的原始观测信息，结合系统状态，构建伪距因子；
基于GNSS接收机时钟误差，构建时钟漂移因子；
整合IMU预积分项和动力学预积分项，构建车载传感器因子；
S4、联合车载传感因子、伪距因子和时钟漂移因子，构造因子图，之后通过优化求解因子图，估计得到车辆的定位信息。
2.根据权利要求1所述的一种基于因子图的GNSS与车载传感器融合定位方法，其特征在于，所述步骤S1的具体过程为：
获得IMU测量时刻t下IMU在载体坐标系b系下的加速度测量值at、角速度测量值ωt以及下一个IMU测量时刻t+1的加速度测量值at+1、角速度测量值ωt+1，两个IMU测量时刻的时间间隔为δt，基于中值积分的方法，计算IMU采样间隔内的预积分项为：
其中，为位置预积分项，/>为速度预积分项，/>为姿态预积分项，ba、bω分别为加速度计零偏、陀螺仪零偏，下标t、t+1表示IMU测量时刻，/>为t时刻的IMU加速度平均值，/>为t时刻的IMU角速度平均值；
利用GNSS测量信息的采样间隔[tk,tk+1]内所有IMU测量值进行预积分，即可得到[tk,tk+1]内总的IMU预积分项。
3.根据权利要求2所述的一种基于因子图的GNSS与车载传感器融合定位方法，其特征在于，所述步骤S2具体包括以下步骤：
S21、建立车辆二自由度模型，用于计算得到车辆坐标系中的速度；
S22、根据车辆坐标系中的速度，获得相邻轮速测量时刻之间的动力学预积分项，对GNSS测量信息的采样间隔[tk,tk+1]内所有轮速测量值的预积分项进行累加，即得到[tk,tk+1]内总的动力学预积分项。
4.根据权利要求3所述的一种基于因子图的GNSS与车载传感器融合定位方法，其特征在于，所述步骤S21中车辆二自由度模型具体为：
其中，kf和kr分别是前轴和后轴的侧偏刚度，Iz为横摆转动惯量，lf和lr分别表示质心距前轴和后轴的距离，m为整车质量，α为等效前轮转角；
考虑车辆稳态转向特性：则侧偏角βt和横摆速度ωr,t为：
其中，K×m(lfkf-lrkr)/l2kfkr为稳定因子，υt、αt分别是t时刻测量的车轮速度和前轮角度，根据前轮与方向盘的传动比i，可得αt＝δα/i，其中δα为方向盘转角，由此得到车辆坐标系中的速度和角速度/>为：
5.根据权利要求4所述的一种基于因子图的GNSS与车载传感器融合定位方法，其特征在于，所述步骤S22的具体过程为：
根据步骤S21计算得到的车辆坐标系中的速度获得相邻轮速测量时刻[t,t+1]之间的动力学预积分项：
其中，δt为相邻轮速测量时刻之间的时间间隔，为相邻轮速测量时刻之间的位置预积分项，
对GNSS测量信息的采样间隔[tk,tk+1]内所有轮速测量值的预积分项进行累加，即可得到[tk,tk+1]内总的动力学预积分项：位置预积分项
6.根据权利要求5所述的一种基于因子图的GNSS与车载传感器融合定位方法，其特征在于，所述步骤S3中构建伪距因子的具体过程为：
GNSS接收机获取卫星的星历数据和伪距观测数据，在时刻tk的观测数据中，根据星历数据得到卫星sj位置卫星钟差/>大气层延迟δρkn,k、电离层延迟δρkp,k、接收机时钟误差δtk和地球自转的Sagnac效应带来的测距误差/>由下式计算：
对于tk时刻第j颗卫星的原始观测数据，考虑测距误差的伪距测量建模为：
其中，ωearth是地球自转角速度，c是光速，是GNSS接收机在ECEF坐标系中的坐标，与设置的状态量/>之间的关系为：
其中，是ECEF坐标系与ENU坐标系之间的变换矩阵，/>是从IMU中心到GNSS天线相位中心的杆臂，/>是载体坐标系与ENU坐标系之间的旋转矩阵，是状态量；
因此对于tk时刻第j颗卫星的原始观测数据，将系统状态和伪距相关测量联系起来的残差为：
其中，是伪距测量值，/>即为构建的伪距因子，X是待估计的状态量，基于滑动窗口的方式进行优化，窗口内的状态X为：
n是滑动窗口大小，状态量包括载体坐标系相对于ENU坐标系的位置速度/>姿态以及加速度计零偏/>陀螺仪零偏/>接收机时钟误差δti、接收机时钟漂移速率/>
7.根据权利要求6所述的一种基于因子图的GNSS与车载传感器融合定位方法，其特征在于，所述步骤S3中构建时钟漂移因子的具体过程为：
应用恒定时钟误差漂移(Constant Clock Error Drift，CCED)模型对GNSS接收机的时钟误差进行建模，构建得到时钟漂移因子为：
8.根据权利要求7所述的一种基于因子图的GNSS与车载传感器融合定位方法，其特征在于，所述步骤S3中构建车载传感器因子的具体过程为：
根据IMU预积分项和动力学预积分项，首先建立位置、速度和姿态的动力学更新方程，之后基于动力学更新方程，构建得到车载传感器因子；
所述位置、速度和姿态的动力学更新方程具体为：
所述车载传感器因子具体为：
其中，为从ENU坐标系转换到载体坐标系的旋转矩阵，gn为重力加速度，/>为[tk,tk+1]内的观测量。
9.根据权利要求8所述的一种基于因子图的GNSS与车载传感器融合定位方法，其特征在于，所述步骤S4具体是使用Ceres Solver中的Levenberg-Marquardt算法优化求解因子图，估计得到车辆的位置、速度、姿态信息为：
其中，∑Vehicle、∑P、∑CCED分别为车载传感器因子、伪距因子和时钟漂移因子对应的标准差。
10.一种应用如权利要求1所述基于因子图的GNSS与车载传感器融合定位方法的融合定位系统，其特征在于，包括输入模块、预积分模块以及因子图优化模块，所述输入模块分别与预积分模块、因子图优化模块相连接，所述预积分模块与因子图优化模块相连接，所述输入模块用于分别获取IMU、轮速传感器以及GNSS接收机输出的测量信息，并将IMU测量信息和轮速传感器测量信息传输给预积分模块、将GNSS测量信息传输给因子图优化模块；
所述预积分模块用于计算得到IMU预积分项和动力学预积分项，并将IMU预积分项和动力学预积分项传输给因子图优化模块；
所述因子图优化模块利用IMU预积分项和动力学预积分项构建车载传感器因子；
根据GNSS测量信息，构建伪距因子；
针对GNSS接收机时钟误差，构建时钟漂移因子；
并联合车载传感器因子、伪距因子和时钟漂移因子构造因子图，
再通过优化求解因子图，估计得到车辆定位信息。
CN202310730402.6A 2023-06-19 2023-06-19 一种基于因子图的gnss与车载传感器融合定位方法及系统 Pending CN116558512A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310730402.6A CN116558512A (zh) 2023-06-19 2023-06-19 一种基于因子图的gnss与车载传感器融合定位方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310730402.6A CN116558512A (zh) 2023-06-19 2023-06-19 一种基于因子图的gnss与车载传感器融合定位方法及系统

Publications (1)

Publication Number Publication Date
CN116558512A true CN116558512A (zh) 2023-08-08

Family

ID=87494902

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310730402.6A Pending CN116558512A (zh) 2023-06-19 2023-06-19 一种基于因子图的gnss与车载传感器融合定位方法及系统

Country Status (1)

Country Link
CN (1) CN116558512A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116817928A (zh) * 2023-08-28 2023-09-29 北京交通大学 基于因子图优化的卫导/惯导列车多源融合定位的方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116817928A (zh) * 2023-08-28 2023-09-29 北京交通大学 基于因子图优化的卫导/惯导列车多源融合定位的方法
CN116817928B (zh) * 2023-08-28 2023-12-01 北京交通大学 基于因子图优化的卫导/惯导列车多源融合定位的方法

Similar Documents

Publication Publication Date Title
CN108873038B (zh) 自主泊车定位方法及定位系统
CN110501024B (zh) 一种车载ins/激光雷达组合导航系统的量测误差补偿方法
CN108535755B (zh) 基于mems的gnss/imu车载实时组合导航方法
US9488480B2 (en) Method and apparatus for improved navigation of a moving platform
CN111156994B (zh) 一种基于mems惯性组件的ins/dr&gnss松组合导航方法
US7979231B2 (en) Method and system for estimation of inertial sensor errors in remote inertial measurement unit
CN108594283B (zh) Gnss/mems惯性组合导航系统的自由安装方法
Bevly et al. GNSS for vehicle control
Wu Versatile land navigation using inertial sensors and odometry: Self-calibration, in-motion alignment and positioning
CN113203418B (zh) 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及系统
JP2000502802A (ja) Ｇｐｓ速度を利用する改良された車両ナビゲーションシステム及びその方法
JP2000506604A (ja) 改良された車両ナビゲーションシステム及びその方法
CN110988951A (zh) 多源数据融合实时导航定位方法及系统
JP2000502803A (ja) 改良された車両ナビゲーションシステム用ゼロ運動検出システム
CN108051839B (zh) 一种车载三维定位装置及三维定位的方法
CN107765279B (zh) 一种融合惯性、卫星的车载定位定向瞄准系统及瞄准方法
CN112505737A (zh) 一种基于Elman神经网络在线学习辅助的GNSS/INS组合导航方法
JP2000502801A (ja) 多重軸加速度計を使用する改良された車両ナビゲーションシステム及びその方法
CN110133695B (zh) 一种双天线gnss位置延迟时间动态估计系统及方法
CN109579870A (zh) 捷联惯导系统的自动对准方法和组合导航装置
CN116558512A (zh) 一种基于因子图的gnss与车载传感器融合定位方法及系统
Meguro et al. Low-cost lane-level positioning in urban area using optimized long time series GNSS and IMU data
CN115407376A (zh) 车辆定位校准方法、装置、计算机设备、存储介质
CN112731502A (zh) 一种无人机空中加油惯性辅助北斗三频精密相对导航方法
CN115166802A (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