一种基于GMM辅助的车辆融合定位方法及系统

一种基于gmm辅助的车辆融合定位方法及系统
技术领域
1.本发明涉及自动驾驶车辆定位技术领域，尤其是涉及一种基于gmm辅助的车辆融合定位方法及系统。


背景技术：

2.自动驾驶三大核心技术分别为感知、规划和控制，其中感知层由定位、检测和识别算法组成，用于理解车辆周围的位置和驾驶环境。精确和鲁棒的全局定位对于车辆实现自动驾驶至关重要，也是规划、决策和运动控制的基础。目前，车辆定位大多借助全球卫星导航系统(global navigation satellite system，gnss)、惯性测量单元(inertial measurement unit，imu)和轮速传感器的方式，由于其成本相对较低，因而被广泛用于为车辆提供定位信息。
3.由于imu和轮速的里程计算法，是相对定位，因此，最终的定位精度是由gnss这一绝对定位算法的精度决定的，gnss在开阔场景可获得较好的定位精度，然而，在城市峡谷、隧道、林荫等场景中，由于多径效应和非视距(non line of sight，nlos)接收的影响，导致较多的异常测量。针对这一问题，常见的解决方法有：
4.1、从硬件设计上如扼流环、双极化天线和天线阵列等方法，减轻信号干扰以提高gnss的精度，但是这种方法成本较高；
5.2、利用信号强度、卫星仰角等gnss测量信息或者基于激光雷达、相机等传感器，对卫星能见度进行分类，从而排除nlos接收或者校正nlos接收提高gnss定位性能，但此类方法的性能强烈依赖卫星能见度分类的质量，且常常只能检测到部分nlos接收，精度受限。
6.此外，现有的gnss/imu/轮速的融合定位方法中，大多仅仅基于车辆平面运动假设，忽略了车辆的俯仰和侧倾运动，并不适用于崎岖地形中的车辆定位，无法保证定位的精确性。


技术实现要素：

7.本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于gmm(gaussian mixed model，高斯混合模型)辅助的车辆融合定位方法及系统，能够有效抑制gnss异常测量对定位的影响，低成本地实现高精度定位。
8.本发明的目的可以通过以下技术方案来实现：一种基于gmm辅助的车辆融合定位方法，包括以下步骤：
9.s1、根据imu的角速度和加速度测量信息，计算基于imu的车辆位置、速度和姿态的预积分项，即得到imu预积分项；
10.s2、根据轮速传感器的速度测量信息和imu中陀螺仪的三轴角速度测量信息，并结合车辆二自由度模型，计算基于车辆动力学的车辆位置的预积分项，即得到动力学预积分项；
11.s3、获得gnss测量信号后，利用imu预积分项和动力学预积分项构建imu因子和动
力学因子；
12.并根据gnss接收机的原始观测信息，结合系统状态，构建伪距因子；
13.基于gnss接收机时钟误差，构建时钟漂移因子；
14.s4、联合imu因子、动力学因子、伪距因子和时钟漂移因子，构造因子图，其中，imu因子、动力学因子和时钟漂移因子的噪声均是高斯建模，伪距因子的噪声采用gmm建模；
15.之后通过优化求解因子图，估计得到车辆的定位信息。
16.进一步地，所述步骤s1的具体过程为：
17.获得imu测量时刻t下imu在载体坐标系b系下的加速度测量值a
t
、角速度测量值ω
t
以及下一个imu测量时刻t+1的加速度测量值a
t+1
、角速度测量值ω
t+1
，两个imu测量时刻的时间间隔为δt，基于中值积分的方法，计算imu采样间隔内的预积分项为：
[0018][0019][0020][0021]
其中，为位置预积分项，为速度预积分项，为姿态预积分项，ba、b
ω
分别为加速度计零偏、陀螺仪零偏，下标t、t+1表示imu测量时刻，为t时刻的imu加速度平均值，为t时刻的imu角速度平均值；
[0022]
利用gnss测量信息的采样间隔[tk,t
k+1
]内所有imu测量值进行预积分，即可得到[tk,t
k+1
]内总的imu预积分项。
[0023]
进一步地，所述步骤s2具体包括以下步骤：
[0024]
s21、建立车辆二自由度模型，用于计算得到车辆坐标系中的速度；
[0025]
s22、根据车辆坐标系中的速度，结合imu中陀螺仪的三轴角速度测量信息，基于中值积分的方法，考虑相邻轮速测量时刻[t,t+1]，两个时刻之间的时间间隔为δt，推导得到动力学预积分的离散形式：
[0026][0027]
其中，为位置预积分项，为姿态预积分项，为车辆坐标系中的速度；
[0028]
对gnss测量信息的采样间隔[tk,t
k+1
]内的所有轮速测量值进行预积分，即得到
[tk,t
k+1
]内总的动力学预积分项。
[0029]
进一步地，所述步骤s21中车辆二自由度模型具体为：
[0030][0031]
其中，kf和kr分别是前轴和后轴的侧偏刚度，iz为横摆转动惯量，lf和lr分别表示质心距前轴和后轴的距离，m为整车质量，α为等效前轮转角；
[0032]
考虑车辆稳态转向特性：则侧偏角β
t
和横摆速度ω
r,t
为：
[0033][0034]
其中，k＝m(lfk
f-lrkr)/l2kfkr为稳定因子，v
t
、α
t
分别是t时刻测量的车轮速度和前轮角度，根据前轮与方向盘的传动比i，可得α
t
＝δα/i，其中δα为方向盘转角，由此得到车辆坐标系中的速度和角速度为：
[0035][0036]
进一步地，所述步骤s3中构建的imu因子具体为：
[0037][0038]
动力学因子具体为：
[0039][0040]
其中，x是待估计的状态量，基于滑动窗口的方式进行优化，窗口内的状态x为：
[0041][0042]
n是滑动窗口大小，状态量包括载体坐标系相对于enu坐标系的位置速度姿态以及加速度计零偏陀螺仪零偏接收机时钟误差δti、接收机时钟漂移速率
[0043]
表示[tk,t
k+1
]时间间隔内的观测量，代表位置误差，代表速度误差，代表欧拉角表示的旋转误差，[
·
]
xyz
表示获取四元数虚部的计算操作。
[0044]
进一步地，所述步骤s3中构建伪距因子的具体过程为：
[0045]
gnss接收机获取卫星的星历数据和伪距观测数据，在时刻tk的观测数据中，根据星历数据得到卫星sj位置卫星钟差大气层延迟δρ
kn,k
、电离层延迟δρ
kp,k
、接收机时钟误差δtk和地球自转的sagnac效应带来的测距误差由下式计算：
[0046][0047]
对于tk时刻第j颗卫星的原始观测数据，考虑测距误差的伪距测量建模为：
[0048][0049]
其中，ω
earth
是地球自转角速度，c是光速，是gnss接收机在ecef坐标系中的坐标，与设置的状态量之间的关系为：
[0050][0051]
其中，是ecef坐标系与enu坐标系之间的变换矩阵，是从imu中心到gnss天线相位中心的杆臂，是载体坐标系与enu坐标系之间的旋转矩阵，是状态量；
[0052]
因此对于tk时刻第j颗卫星的原始观测数据，将系统状态和伪距相关测量联系起来的残差为：
[0053][0054]
其中，z
ρk
是伪距测量值，即为构建的伪距因子。
[0055]
进一步地，所述步骤s3中构建时钟漂移因子的具体过程为：
[0056]
应用恒定时钟误差漂移(constant clock error drift，cced)模型对gnss接收机的时钟误差进行建模，构建得到时钟漂移因子为：
[0057][0058]
进一步地，所述步骤s4中伪距因子的噪声采用gmm建模的具体过程为：以滑动窗口内的伪距残差序列作为输入，基于em(expectation-maximization algorithm，最大期望算
法)估计伪距因子的gmm参数，之后将估计的gmm参数作为因子图优化中伪距因子的噪声模型。
[0059]
进一步地，所述步骤s4具体包括以下步骤：
[0060]
s41、gmm参数估计：以滑动窗口内的伪距残差序列做输入，基于em估计伪距因子的gmm参数，得到：
[0061][0062]
其中，o是残差序列，m为残差的总数量，ek为伪距残差，h是隐变量，θ是待估计的gmm参数，n是该gmm中包含的高斯分量的个数，ωj是第j个高斯分量的权重，μj是第j个高斯分量的均值，σj是第j个高斯分量的方差；
[0063]
设计e-步：基于θ的初始猜测估计隐变量h，α
kj
表示ek属于第j个高斯分量组件的概率：
[0064][0065][0066]
设计m-步：根据估计的h计算θ，完成gmm参数的更新：
[0067][0068]
迭代执行e-步和m-步，直到达到最大迭代次数或预设的收敛条件，至此，完成gmm参数的估计；
[0069]
s42、将估计的gmm参数作为因子图优化中伪距因子的噪声模型，得到将gmm应用到因子优化的一般公式为：
[0070]
[0071][0072][0073]
其中，γs是归一化常数，用于保持负对数似然为正；
[0074]
s43、因子图优化求解：将步骤s42中的伪距因子相关项，结合噪声服从高斯分布的imu因子、动力学因子和时钟漂移因子，一起构造因子图，并使用ceres solver中的levenberg-marquardt算法优化进行因子图求解，得到估计车辆的位置、速度、姿态信息：
[0075][0076]
其中，∑imu、∑dynamics、∑cced分别是imu因子、动力学因子和时钟漂移因子的标准差，最后一项是伪距因子相关项。
[0077]
一种基于gmm辅助的车辆融合定位系统，包括输入模块、预积分模块、gmm参数估计模块以及因子图优化模块，所述输入模块分别与预积分模块、gmm参数估计模块相连接，所述预积分模块分别与gmm参数估计模块、因子图优化模块相连接，所述gmm参数估计模块与因子图优化模块相连接，所述输入模块用于分别获取imu、轮速传感器以及gnss接收机输出的测量信息，并将imu测量信息和轮速传感器测量信息传输给预积分模块、将gnss测量信息传输给gmm参数估计模块；
[0078]
所述预积分模块用于计算得到imu预积分项和动力学预积分项，并将imu预积分项和动力学预积分项传输给因子图优化模块、将动力学预积分项传输给gmm参数估计模块；
[0079]
所述gmm参数估计模块采用gmm建模，以滑动窗口内的伪距残差序列作为输入，基于最大期望化算法估计伪距因子的gmm参数；
[0080]
所述因子图优化模块利用imu预积分项和动力学预积分项构建imu因子和动力学因子；
[0081]
针对gnss接收机时钟误差，构建时钟漂移因子；
[0082]
并联合imu因子、动力学因子、伪距因子和时钟漂移因子构造因子图，
[0083]
再通过优化求解因子图，估计得到车辆定位信息。
[0084]
与现有技术相比，本发明具有以下优点：
[0085]
一、本发明首先根据imu的角速度和加速度测量信息，计算得到imu预积分项；之后根据轮速传感器的速度测量信息和imu的角速度测量信息，并基于车辆二自由度模型，计算得到动力学预积分项；当获得gnss测量信号后，一方面利用imu预积分项和动力学预积分项构建imu因子和动力学因子、根据gnss接收机的原始观测信息构建伪距因子、针对接收机时钟误差构建时钟漂移因子，另一方面联合imu因子、动力学因子、伪距因子和时钟漂移因子构造因子图，通过优化求解因子图，以估计得到车辆定位信息。由此实现一种gnss/imu/轮速紧耦合鲁棒定位方案，能够有效抑制gnss异常测量对车辆定位产生的不利影响，同时无
需设置用于减轻信号干扰的硬件设施，即可低成本地提升车辆定位的准确性。
[0086]
二、本发明计算动力学预积分项时，考虑到车辆常常不是在近似平面上运动，因此不能忽略俯仰和侧倾运动带来的影响，而车辆二自由度动力学模型中无法获得这些信息，因此角速度信息需要利用imu中陀螺仪的三轴角速度测量信息，再结合车辆二自由度模型得到的车辆坐标系中的速度，以推导得到动力学预积分项。由此确保了计算得到动力学预积分项能够贴合于车辆实际运动，适用于在崎岖地形中的车辆定位。
[0087]
三、本发明在构造因子图时，其中，imu因子、动力学因子和时钟漂移因子的噪声均是高斯建模，伪距因子的噪声则采用gmm建模，以滑动窗口内的伪距残差序列作为输入，再基于最大期望化算法(em)估计伪距因子的gmm参数，之后将估计的gmm参数作为因子图优化中伪距因子的噪声模型。由此充分保证了因子图的可靠性，能够准确估计出车辆的位置、速度、姿态等定位信息。
附图说明
[0088]
图1为本发明的方法流程示意图；
[0089]
图2为实施例中线性二自由度车辆动力学模型示意图；
[0090]
图3为实施例中构造的因子图结构示意图；
[0091]
图4为实施例中搭建的系统结构示意图。
具体实施方式
[0092]
下面结合附图和具体实施例对本发明进行详细说明。
[0093]
实施例
[0094]
如图1所示，一种基于gmm辅助的车辆融合定位方法，包括以下步骤：
[0095]
s1、根据imu的角速度和加速度测量信息，计算基于imu的车辆位置、速度和姿态的预积分项，即得到imu预积分项；
[0096]
s2、根据轮速传感器的速度测量信息和imu中陀螺仪的三轴角速度测量信息，并结合车辆二自由度模型，计算基于车辆动力学的车辆位置的预积分项，即得到动力学预积分项；
[0097]
s3、获得gnss测量信号后，利用imu预积分项和动力学预积分项构建imu因子和动力学因子；
[0098]
并根据gnss接收机的原始观测信息，结合系统状态，构建伪距因子；
[0099]
基于gnss接收机时钟误差，构建时钟漂移因子；
[0100]
s4、联合imu因子、动力学因子、伪距因子和时钟漂移因子，构造因子图，其中，imu因子、动力学因子和时钟漂移因子的噪声均是高斯建模，伪距因子的噪声采用gmm建模；
[0101]
之后通过优化求解因子图，估计得到车辆的定位信息。
[0102]
本实施例应用上述技术方案，主要内容包括：
[0103]
一、根据imu的角速度和加速度测量信息，计算基于imu的车辆位置、速度和姿态的预积分项；
[0104]
具体的，本实施例首先选定因子图更新周期与gnss的测量周期相同，由于imu的输出频率(100hz)远高于gnss的输出频率(1hz)，因此在gnss测量信息的采样间隔[tk,t
k+1
]
内，会有多个imu的测量值。考虑某个imu测量时刻t，获得imu在载体坐标系b系下的加速度测量值a
t
和角速度测量值ω
t
，下一imu测量时刻t+1的加速度测量值为a
t+1
、角速度测量值为ω
t+1
。两个imu采样的时间间隔为δt，基于中值积分的方法，计算imu采样间隔内的预积分项：
[0105][0106]
其中：
[0107][0108][0109]
利用gnss测量信息的采样间隔[tk,t
k+1
]内的所有imu测量值进行预积分，即可得到[tk,t
k+1
]内总的imu预积分项：位置预积分项速度预积分项姿态预积分项ba、b
ω
分别是加速度计、陀螺仪零偏。
[0110]
二、根据轮速传感器的速度测量信息和imu中陀螺仪的三轴角速度测量信息，基于车辆二自由度模型(如图2所示)，计算基于车辆动力学的车辆位置的预积分项；
[0111]
21)基于如下车辆二自由度模型：
[0112][0113]
其中，kf和kr分别是前轴和后轴的侧偏刚度，iz为横摆转动惯量，lf和lr分别表示质心距前轴和后轴的距离，m为整车质量，α为等效前轮转角。
[0114]
考虑车辆稳态转向特性：则侧偏角β
t
和横摆速度ω
r,t
为：
[0115][0116]
其中，稳定因子k＝m(lfk
f-lrkr)/l2kfkr，v
t
、α
t
分别是t时刻测量的车轮速度和前轮角度，根据前轮与方向盘的传动比i，可得α
t
＝δα/i，其中δα为方向盘转角，得到车辆坐标系中的速度和角速度为：
[0117]
[0118]
22)此外，考虑到实际车辆并不是在近似平面上运动，因此俯仰和侧倾运动带来的影响不能忽略。由于车辆动力学模型中无法获得这些信息，因此角速度信息不采用步骤21)中的计算值，而是利用imu中陀螺仪的三轴角速度测量信息ω
t
，再结合步骤21)中计算得到的车辆坐标系中的速度基于中值积分的方法，考虑相邻轮速测量时刻[t,t+1]，两个时刻之间的时间间隔为δt，推导得到动力学预积分的离散形式：
[0119][0120]
对[tk,t
k+1
]时间间隔内的所有轮速测量值进行预积分，即可得到[tk,t
k+1
]内总的动力学预积分项：位置预积分项姿态预积分项由于不在动力学因子中对角速度零偏构建约束，因此均利用前一时刻tk的角速度零偏估计值对时间段内的角速度测量值进行修正。
[0121]
三、在gnss测量到来时(即获得gnss测量信号时)，利用imu预积分项和动力学预积分项构建imu因子和动力学因子；
[0122]
根据gnss接收机的原始观测信息，结合系统状态，构建伪距因子；
[0123]
对gnss接收机时钟误差应用恒定时钟误差漂移(cced)模型建模，构建时钟漂移因子；
[0124]
具体的：
[0125]
31)本实施例设置的状态量包括：载体坐标系相对于enu坐标系的位置速度姿态加速度计零偏陀螺仪零偏接收机时钟误差δti和接收机时钟漂移速率
[0126]
基于滑动窗口的方式进行优化，窗口内的状态x可以总结为：
[0127][0128]
其中n是滑动窗口大小。
[0129]
32)在t
k+1
时刻gnss测量到来时，根据imu预积分项：位置预积分项速度预积分项姿态预积分项并结合系统状态，构建imu因子：
[0130][0131]
其中，表示[tk,t
k+1
]时间间隔内的观测量，x表示待估计的状态量，代表位置误差，代表速度误差，代表欧拉角表示的旋转误差，[
·
]
xyz
表示获取四元数虚部的计算操作。
[0132]
33)在t
k+1
时刻gnss测量到来时，根据动力学预积分项：位置预积分项姿态预积分项并结合系统状态，构建动力学因子：
[0133][0134]
34)由于各卫星系统的时钟误差均不相同，利用多星座信息需要考虑星座间的时钟误差，因此本实施例仅使用gps单卫星系统，gnss接收机可以获取卫星的星历数据和伪距观测数据。在时刻tk的观测数据中，根据星历数据可以得到卫星sj位置卫星钟差大气层延迟δρ
kn,k
及电离层延迟δρ
kp,k
。另外gnss接收机时钟误差δtk和地球自转的sagnac效应带来的测距误差也不容忽略，其中δtk是最大分量且随时间的推移而变化，必须与gnss接收机的位置一起估计，由下式计算：
[0135][0136]
对于tk时刻第j颗卫星的原始观测数据，考虑上述测距误差的伪距测量建模为：
[0137][0138]
其中，ω
earth
是地球自转角速度，c是光速，是gnss接收机在ecef坐标系中的坐标，其与设置的状态量之间的关系为：
[0139][0140]
式中，是ecef坐标系与enu坐标系之间的变换矩阵，本实施例将其设置为定值，其由起点处的ecef坐标计算得到；是从imu中心到gnss天线相位中心的杆臂；是载体坐标系与enu坐标系之间的旋转矩阵，是状态量。
[0141]
因此对于tk时刻第j颗卫星的原始观测数据，将系统状态和伪距相关测量联系起来的残差可以表述为：
[0142][0143]
式中，是伪距测量值，即为构建的伪距因子。
[0144]
35)由于gnss接收机的时钟误差不是一个定值，而是通常以一定的速度漂移，故本方案应用恒定时钟误差漂移(cced)模型对其进行建模，构建的时钟漂移因子为：
[0145][0146]
四、在gnss测量到来时，联合imu因子、动力学因子、伪距因子和时钟漂移因子构造因子图，如图3所示，其中，imu因子、动力学因子和时钟漂移因子的噪声均是高斯建模；伪距因子的噪声采用高斯混合模型(gmm)建模，以滑动窗口内的伪距残差序列做输入，基于最大期望化算法(em)估计伪距因子的gmm参数，将估计的gmm参数作为因子图优化中伪距因子的噪声模型；最终通过优化求解因子图估计车辆的位置、速度、姿态等信息。
[0147]
具体的：
[0148]
41)gmm参数估计：以滑动窗口内的伪距残差序列做输入，基于em算法估计伪距因子的gmm参数，实现步骤如下：
[0149][0150]
其中，o是残差序列，m表示残差的数量，伪距残差ek的计算方式如步骤34)所述；h是隐变量；θ是待估计的gmm参数，n是该gmm中包含的高斯分量的个数，ωj是第j个高斯分量的权重，μj是第j个高斯分量的均值，σj是第j个高斯分量的方差。
[0151]
a)e-步：基于θ的初始猜测估计隐变量h。α
kj
表示ek属于第j个高斯分量组件的概率：
[0152][0153][0154]
b)m-步：根据估计的h计算θ，完成gmm参数的更新：
[0155][0156]
迭代执行e-步和m-步，直到达到最大迭代次数或设置的收敛条件。至此，完成了gmm参数的估计。
[0157]
42)将估计的gmm参数作为因子图优化中伪距因子的噪声模型，得到将gmm应用到因子优化的一般公式为：
[0158][0159][0160][0161]
43)因子图优化：将步骤42)中的伪距因子相关项，结合噪声服从高斯分布的imu因子、动力学因子和时钟漂移因子一起构造因子图，最终使用ceres solver中的levenberg-marquardt算法优化进行因子图求解，估计车辆的位置、速度、姿态等信息：
[0162][0163]
式中，∑imu、∑dynamics、∑cced分别是imu因子、动力学因子和时钟漂移因子的标准差；最后一项是步骤42)中给出的伪距因子相关项。
[0164]
基于上述方法过程，本实施例搭建如图4所示的系统结构，包括输入模块、预积分模块、gmm参数估计模块以及因子图优化模块，其中，输入模块分别与预积分模块、gmm参数估计模块相连接，预积分模块分别与gmm参数估计模块、因子图优化模块相连接，gmm参数估计模块与因子图优化模块相连接。
[0165]
输入模块用于分别获取imu、轮速传感器以及gnss接收机输出的测量信息，并将imu测量信息和轮速传感器测量信息传输给预积分模块、将gnss测量信息传输给gmm参数估计模块；
[0166]
预积分模块用于计算得到imu预积分项和动力学预积分项，并将imu预积分项和动力学预积分项传输给因子图优化模块、将动力学预积分项传输给gmm参数估计模块；
[0167]
gmm参数估计模块采用gmm建模，以滑动窗口内的伪距残差序列作为输入，基于最大期望化算法估计伪距因子的gmm参数；
[0168]
因子图优化模块则利用imu预积分项和动力学预积分项构建imu因子和动力学因子；
[0169]
针对gnss接收机时钟误差，构建时钟漂移因子；
[0170]
并联合imu因子、动力学因子、伪距因子和时钟漂移因子构造因子图，
[0171]
再通过优化求解因子图，估计得到车辆定位信息。
[0172]
综上可知，本技术方案实现了一种基于gmm辅助的gnss/imu/轮速紧耦合鲁棒定位方案，能够有效抑制gnss异常测量对定位的影响，具有成本低廉、定位鲁棒、计算量小等优点。

技术特征：
1.一种基于gmm辅助的车辆融合定位方法，其特征在于，包括以下步骤：s1、根据imu的角速度和加速度测量信息，计算基于imu的车辆位置、速度和姿态的预积分项，即得到imu预积分项；s2、根据轮速传感器的速度测量信息和imu中陀螺仪的三轴角速度测量信息，并结合车辆二自由度模型，计算基于车辆动力学的车辆位置的预积分项，即得到动力学预积分项；s3、获得gnss测量信号后，利用imu预积分项和动力学预积分项构建imu因子和动力学因子；并根据gnss接收机的原始观测信息，结合系统状态，构建伪距因子；基于gnss接收机时钟误差，构建时钟漂移因子；s4、联合imu因子、动力学因子、伪距因子和时钟漂移因子，构造因子图，其中，imu因子、动力学因子和时钟漂移因子的噪声均是高斯建模，伪距因子的噪声采用gmm建模；之后通过优化求解因子图，估计得到车辆的定位信息。2.根据权利要求1所述的一种基于gmm辅助的车辆融合定位方法，其特征在于，所述步骤s1的具体过程为：获得imu测量时刻t下imu在载体坐标系b系下的加速度测量值a
t
、角速度测量值ω
t
以及下一个imu测量时刻t+1的加速度测量值a
t+1
、角速度测量值ω
t+1
，两个imu测量时刻的时间间隔为δt，基于中值积分的方法，计算imu采样间隔内的预积分项为：间隔为δt，基于中值积分的方法，计算imu采样间隔内的预积分项为：间隔为δt，基于中值积分的方法，计算imu采样间隔内的预积分项为：其中，为位置预积分项，为速度预积分项，为姿态预积分项，b
a
、b
ω
分别为加速度计零偏、陀螺仪零偏，下标t、t+1表示imu测量时刻，为t时刻的imu加速度平均值，为t时刻的imu角速度平均值；利用gnss测量信息的采样间隔[t
k
,t
k+1
]内所有imu测量值进行预积分，即可得到[t
k
,t
k+1
]内总的imu预积分项。3.根据权利要求2所述的一种基于gmm辅助的车辆融合定位方法，其特征在于，所述步骤s2具体包括以下步骤：s21、建立车辆二自由度模型，用于计算得到车辆坐标系中的速度；s22、根据车辆坐标系中的速度，结合imu中陀螺仪的三轴角速度测量信息，基于中值积分的方法，考虑相邻轮速测量时刻[t,t+1]，两个时刻之间的时间间隔为δt，推导得到动力学预积分的离散形式：
其中，为位置预积分项，为姿态预积分项，为车辆坐标系中的速度；对gnss测量信息的采样间隔[t
k
,t
k+1
]内的所有轮速测量值进行预积分，即得到[t
k
,t
k+1
]内总的动力学预积分项。4.根据权利要求3所述的一种基于gmm辅助的车辆融合定位方法，其特征在于，所述步骤s21中车辆二自由度模型具体为：其中，k
f
和k
r
分别是前轴和后轴的侧偏刚度，i
z
为横摆转动惯量，l
f
和l
r
分别表示质心距前轴和后轴的距离，m为整车质量，α为等效前轮转角；考虑车辆稳态转向特性：则侧偏角β
t
和横摆速度ω
r,t
为：其中，k＝m(l
f
k
f-l
r
k
r
)/l2k
f
k
r
为稳定因子，υ
t
、α
t
分别是t时刻测量的车轮速度和前轮角度，根据前轮与方向盘的传动比i，可得α
t
＝δα/i，其中δα为方向盘转角，由此得到车辆坐标系中的速度和角速度为：5.根据权利要求4所述的一种基于gmm辅助的车辆融合定位方法，其特征在于，所述步骤s3中构建的imu因子具体为：
动力学因子具体为：其中，x是待估计的状态量，基于滑动窗口的方式进行优化，窗口内的状态x为：n是滑动窗口大小，状态量包括载体坐标系相对于enu坐标系的位置速度姿态以及加速度计零偏陀螺仪零偏接收机时钟误差δt
i
、接收机时钟漂移速率、接收机时钟漂移速率表示[t
k
,t
k+1
]时间间隔内的观测量，代表位置误差，代表速度误差，代表欧拉角表示的旋转误差，[
·
]
xyz
表示获取四元数虚部的计算操作。6.根据权利要求5所述的一种基于gmm辅助的车辆融合定位方法，其特征在于，所述步骤s3中构建伪距因子的具体过程为：gnss接收机获取卫星的星历数据和伪距观测数据，在时刻t
k
的观测数据中，根据星历数据得到卫星s
j
位置卫星钟差大气层延迟电离层延迟接收机时钟误差δt
k
和地球自转的sagnac效应带来的测距误差和地球自转的sagnac效应带来的测距误差由下式计算：对于t
k
时刻第j颗卫星的原始观测数据，考虑测距误差的伪距测量建模为：其中，ω
earth
是地球自转角速度，c是光速，是gnss接收机在ecef
坐标系中的坐标，与设置的状态量之间的关系为：其中，是ecef坐标系与enu坐标系之间的变换矩阵，是从imu中心到gnss天线相位中心的杆臂，是载体坐标系与enu坐标系之间的旋转矩阵，是状态量；因此对于t
k
时刻第j颗卫星的原始观测数据，将系统状态和伪距相关测量联系起来的残差为：其中，z
ρk
是伪距测量值，即为构建的伪距因子。7.根据权利要求6所述的一种基于gmm辅助的车辆融合定位方法，其特征在于，所述步骤s3中构建时钟漂移因子的具体过程为：应用恒定时钟误差漂移模型对gnss接收机的时钟误差进行建模，构建得到时钟漂移因子为：8.根据权利要求1所述的一种基于gmm辅助的车辆融合定位方法，其特征在于，所述步骤s4中伪距因子的噪声采用gmm建模的具体过程为：以滑动窗口内的伪距残差序列作为输入，基于em估计伪距因子的gmm参数，之后将估计的gmm参数作为因子图优化中伪距因子的噪声模型。9.根据权利要求8所述的一种基于gmm辅助的车辆融合定位方法，其特征在于，所述步骤s4具体包括以下步骤：s41、gmm参数估计：以滑动窗口内的伪距残差序列做输入，基于em估计伪距因子的gmm参数，得到：其中，o是残差序列，m为残差的总数量，e
k
为伪距残差，h是隐变量，θ是待估计的gmm参数，n是该gmm中包含的高斯分量的个数，ω
j
是第j个高斯分量的权重，μ
j
是第j个高斯分量的均值，σ
j
是第j个高斯分量的方差；设计e-步：基于θ的初始猜测估计隐变量h，α
kj
表示e
k
属于第j个高斯分量组件的概率：
设计m-步：根据估计的h计算θ，完成gmm参数的更新：迭代执行e-步和m-步，直到达到最大迭代次数或预设的收敛条件，至此，完成gmm参数的估计；s42、将估计的gmm参数作为因子图优化中伪距因子的噪声模型，得到将gmm应用到因子优化的一般公式为：优化的一般公式为：优化的一般公式为：其中，γ
s
是归一化常数，用于保持负对数似然为正；s43、因子图优化求解：将步骤s42中的伪距因子相关项，结合噪声服从高斯分布的imu因子、动力学因子和时钟漂移因子，一起构造因子图，并使用ceres solver中的levenberg-marquardt算法优化进行因子图求解，得到估计车辆的位置、速度、姿态信息：其中，∑imu、∑dynamics、∑cced分别是imu因子、动力学因子和时钟漂移因子的标准差，最后一项是伪距因子相关项。10.一种应用如权利要求1所述基于gmm辅助的车辆融合定位方法的车辆融合定位系统，其特征在于，包括输入模块、预积分模块、gmm参数估计模块以及因子图优化模块，所述输入模块分别与预积分模块、gmm参数估计模块相连接，所述预积分模块分别与gmm参数估
计模块、因子图优化模块相连接，所述gmm参数估计模块与因子图优化模块相连接，所述输入模块用于分别获取imu、轮速传感器以及gnss接收机输出的测量信息，并将imu测量信息和轮速传感器测量信息传输给预积分模块、将gnss测量信息传输给gmm参数估计模块；所述预积分模块用于计算得到imu预积分项和动力学预积分项，并将imu预积分项和动力学预积分项传输给因子图优化模块、将动力学预积分项传输给gmm参数估计模块；所述gmm参数估计模块采用gmm建模，以滑动窗口内的伪距残差序列作为输入，基于最大期望化算法估计伪距因子的gmm参数；所述因子图优化模块利用imu预积分项和动力学预积分项构建imu因子和动力学因子；针对gnss接收机时钟误差，构建时钟漂移因子；并联合imu因子、动力学因子、伪距因子和时钟漂移因子构造因子图，再通过优化求解因子图，估计得到车辆定位信息。

技术总结
本发明涉及一种基于GMM辅助的车辆融合定位方法及系统，该方法包括：根据IMU的角速度和加速度测量信息，计算IMU预积分项；根据轮速传感器的速度测量信息和IMU的角速度测量信息，结合车辆二自由度模型，计算动力学预积分项；获得GNSS测量信号后，利用IMU预积分项和动力学预积分项构建IMU因子和动力学因子；根据GNSS接收机原始观测信息，结合系统状态，构建伪距因子；基于GNSS接收机时钟误差，构建时钟漂移因子；联合构建的各因子，构造因子图，其中，IMU因子、动力学因子和时钟漂移因子噪声均是高斯建模，伪距因子噪声采用GMM建模；优化求解因子图，估计车辆的定位信息。与现有技术相比，本发明能有效抑制GNSS异常测量对定位的影响，低成本、鲁棒地实现高精度定位。鲁棒地实现高精度定位。鲁棒地实现高精度定位。


技术研发人员：朱佳琪 卓桂荣 熊璐 周桓宇 李子尧
受保护的技术使用者：同济大学
技术研发日：2023.06.19
技术公布日：2023/8/13