CN110849349A - 一种基于磁传感器与轮式里程计融合定位方法 - Google Patents

Abstract

本发明公开了一种基于磁传感器与轮式里程计融合定位方法，通过对车辆CAN报文解析模块和磁场传感器进行同步采样匹配，构建同步匹配成功的车辆数据(车速数据、方向盘转角数据和地球磁场数据)，然后基于该车辆数据，采用将车辆轮式里程计的定位估计值与磁场传感器的灰色预测值相融合方法，抑制了轮式里程计的累积误差，提高了航向角估计的精度，通过灰色预测方法的预测值建立扩展卡尔曼滤波的观测模型，有效抑制磁场传感器的磁场数据波动；优点是采用低成本的磁场传感器和车辆现有的速度传感器、转角传感器实现长时间的定位，在保证低成本的基础上，有效提高定位精度和鲁棒性。

Description

一种基于磁传感器与轮式里程计融合定位方法

技术领域

本发明涉及一种定位方法，尤其是涉及一种基于磁传感器与轮式里程计融合定位方法。

背景技术

实现车辆厘米级定位要求是无人驾驶车辆自主导航的关键，现有车辆定位方法主要有基于轮式里程计定位方法、基于视觉SLAM的定位方法、基于激光SLAM的定位方法以及基于高精度差分GPS的定位方法。基于视觉SLAM的定位方法和基于激光SLAM的定位方法受环境动态目标的影响较大，鲁棒性较差，成本较高。高精度差分GPS的定位方法成本较高，主要用于无人驾驶车辆的离线地图制作。基于轮式里程计定位方法通过车辆上现有的速度传感器和转角传感器即可实现车辆的位姿推算，价格低廉，但随时间的累积误差较大，不发长时间有效工作。

多传感器融合是有效提高定位精度和鲁棒性的有效方法。磁传感器成本低廉，可获得全局航向信息，不受光照条件和周围动态目标影响，具有广泛的应用前景，但是磁传感器易受周围磁场干扰，获取的磁场数据存在噪声波动。

鉴此，设计一种基于磁传感器与轮式里程计融合定位方法，在保证低成本的基础上，有效提高定位精度和鲁棒性，对于车辆定位技术的发展具有重要意义。

发明内容

本发明所要解决的技术问题是提供一种基于磁传感器与轮式里程计融合定位方法，该定位方法将车辆轮式里程计的定位估计值与磁场传感器的灰色预测值相融合方法，抑制了轮式里程计的累积误差，提高了航向估计的精度，通过灰色预测方法的预测值建立扩展卡尔曼滤波的观测模型，有效抑制磁场传感器的磁场数据波动，采用低成本的磁场传感器和车辆现有的速度传感器、转角传感器实现长时间的定位，在保证低成本的基础上，有效提高定位精度和鲁棒性。

本发明解决上述技术问题所采用的技术方案为：一种基于磁传感器与轮式里程计融合定位方法，包括以下步骤：

(1)将车辆CAN报文解析模块的相邻两次采样之间的时间间隔记为△t₁，△t₁＝0.01s，将磁场传感器相邻两次采样之间的时间间隔记为△t₂，△t₂＝0.05s；创建一个用于存放地球磁场数据的数组，数组容量为10，当超出其容量时，该数组中已存地球磁场数据将按照存储时间从早到晚的顺序被覆盖掉，且该数组中地球磁场数据按照存入先后顺序从前向后排列，后存入的地球磁场数据排在先存入的地球磁场数据之后，将数组中存放的地球磁场数据的数量记为变量n，初始状态时，所述的数组中不存在地球磁场数据，此时所述的数组中存储的地球磁场数据为0，n的取值为0；设计一个用于存储所述的车辆CAN报文解析模块每次采样获取的采样数据的时间戳的缓存，该缓存的容量为100，当超出其容量时，该缓存中已存时间戳将按照存储时间从早到晚的顺序被覆盖掉，且该缓存中时间戳按照存入先后顺序从前向后排列，后存入的时间戳排在先存入的地球磁场数据之后，所述的车辆CAN报文解析模块每次采样获取的采样数据包括车速数据和方向盘转角数据，所述的车辆CAN报文解析模块每次采样时的时刻采用UTC时间表示，将所述的车辆CAN报文解析模块每次采样时的时刻作为该次采样时获取的采样数据的时间戳保存到所述的缓存中，所述的磁场传感器每次采样时获取地球磁场数据，所述的磁场传感器每次采样的时刻采用UTC时间表示；

(2)设定车辆CAN报文解析模块和磁场传感器的同步采样成功匹配次数变量，将其记为t，对t进行初始化赋值，令t＝0；

(3)同时开启所述的车辆CAN报文解析模块和所述的磁场传感器，所述的车辆CAN报文解析模块和所述的磁场传感器同时开始进行第1次采样，所述的车辆CAN报文解析模块和所述的磁场传感器后续每采样一次其采样次数增加1；

(4)将所述的磁场传感器的当前采样次数记为第l次，此时对所述的车辆CAN报文解析模块和所述的车载单目相机进行第l次同步采样匹配，具体匹配过程为：

4.1将所述的磁场传感器第l次采样得到的地球磁场数据记为m_l，将所述的磁场传感器第l次采样的时刻记为t_l；

4.2将t_l与缓存中存储的所有时间戳进行匹配，查找与t_l相差小于5毫秒且相差最小的时间戳，如果找到，则第l次同步采样匹配成功，获取查找到的时间戳对应的车速数据和方向盘转角数据，进入步骤(5)，如果没有找到，则采用l的当前值加1的和更新l的值，重复步骤(4)，直到满足同步采样匹配成功条件；

(5)先采用t的当前值加1的和更新t的值，然后构建第t次采样同步匹配成功的车辆数据，具体过程为：

5.1将第t次采样同步匹配成功的的车速数据记为v_t，方向盘转角数据记为δ_ft，地球磁场数据记为m^t；

5.2将当前采样同步匹配成功后查找到的时间戳对应的车速数据赋值给v_t，方向盘转角数据赋值给δ_ft，m_l赋值给m^t，第t次采样同步匹配成功的车辆数据构建完成；

(6)将m^t作为数组中最后一个数据保存到数组中，重新统计数组中地球磁场数据的数量，采用统计的数量更新n的值，将当前数组记为

其中

为当前数组中第j个地球磁场数据，j＝1,2,…,n；

(7)判断t的当前值是否大于等于2，如果大于等于2，则进入步骤(8)，如果小于2，则采用l的当前值加1的和更新l的值，重复步骤(4)～(6)，直到满足进入步骤(8)的条件；

(8)将当前数组

中n个地球磁场数据分别通过地磁坐标变换方法进行转换，得到车辆的航向角数据序列

其中

为地球磁场数据

转换得到的航向角数据，

为航向角数据序列中第j个航向角数据；

(9)采用一次累加生成算法(1-AGO)对航向角数据序列

进行处理，得到灰色序列，具体步骤为：

9.1设定一个用于存放n个数据的原始灰度序列，将其记为θ⁽⁰⁾，将θ⁽⁰⁾中的第i个数据记为θ⁽⁰⁾(i)，i＝1,2,…,n，将

的值赋给θ⁽⁰⁾(i)，得到原始灰度序列：

9.2将θ⁽⁰⁾一次累加生成序列记为θ⁽¹⁾，θ⁽¹⁾采用式(2)表示为：

θ⁽¹⁾＝(θ⁽¹⁾(1),θ⁽¹⁾(2),…,θ⁽¹⁾(n)) (2)

其中，θ⁽¹⁾(i)为θ⁽¹⁾中的第i个数据；

9.3采用式(3)为θ⁽¹⁾赋值：

9.4将θ⁽¹⁾的邻值生成序列记为z⁽¹⁾，z⁽¹⁾采用式(4)表示为：

z⁽¹⁾＝(z⁽¹⁾(1),z⁽¹⁾(2),…,z⁽¹⁾(n)) (4)

其中，z⁽¹⁾(i)为z⁽¹⁾中的第i个数据；

9.5采用式(5)对z⁽¹⁾进行赋值：

其中，K＝2,…,n；β为邻值生成权值系数，β＝0.5；

9.6建立灰微分方程模型，灰微分方程模型采用式(6)表示为：

θ⁽⁰⁾(K)+az⁽¹⁾(K)＝b (6)

其中，a称为灰色发展系数，b称为灰色作用量，a和b为待求参数；

9.7将式(6)展开并整理成矩阵向量形式，采用式(7)表示为：

Y＝Bu (7)

其中，u、B和Y分别采用式(8)、(9)和(10)表示，具体为：

9.8通过最小二乘法求解式(7)，计算得到参数a和b：

式(11)中，上标T表示矩阵的转置，上标-1表示矩阵的逆运算；

9.9建立GM(1,1)的白化模型，白化模型采用式(12)表示为：

9.10将第t-1次采样同步匹配成功的一次累加生成序列的预测值记为

采用式(13)表示为：

式(13)中，e表示自然对数的底；

9.11将第t次采样同步匹配成功的一次累加生成序列的预测值记为

采用式(14)表示为：

9.12将第t次采样同步匹配成功的原始灰度序列的的预测值记为采用式(15)表示为：

9.13将第t次采样同步匹配成功的车辆的航向角数据的预测值记为θ_t，令

(10)将当前定位次数记为t'，令t'＝t-1，采用扩展卡尔曼滤波算法先进行数据融合后再进行车辆第t'次定位，具体过程为：

a、通过车辆运动学航迹推测算法得到车辆运动学生成的运动轨迹，即当前定位时车辆位置估计坐标和航向角估计值

表示当前定位时车辆坐标系下的车辆位置的横坐标估计值，表示当前定位时车辆坐标系下的车辆位置的纵坐标估计值；其中车辆运动学航迹推测算法公式如下所示：

ds_t'＝v_t'-1·△t₁ (17)

δ_ft'＝ω_t'·η (19)

其中，

表示第t'-1次定位时车辆坐标系下的车辆位置的横坐标估计值，

表示第t'-1次定位时车辆坐标系下的车辆位置的纵坐标估计值，

表示第t'-1次定位时车辆航向角估计值，ds_t'表示从第t'-1次定位时到第t'次定位时车辆行驶的距离，dθ_t'表示第t'次定位时相对于第t'-1次定位时车辆航向角变化量，v_t'-1为第t'-1次定位时车辆的车速，L为车辆轴距，δ_ft'为当前定位时车辆前轮转角，η为车辆的角传动比，η预先根据车辆型号通过现有成熟的实验方法标定得到；当t'＝1时，

v_t'-1＝0，sin表示正弦函数，cos表示余弦函数，tan表示正切函数；

b、采用

和

构建的状态向量，将其记为A_t'，采用式(20)对A_t'进行初始化赋值：

其中，在t'＝1时，

c、采用v_t'-1和δ_ft'构建当前定位时的控制输入向量，将其记为B_t'：

d、建立当前定位时带噪声的车辆运动学模型，将其向量表达式记为f(A_t',B_t')：

其中，N(·)为高斯白噪声生成函数，N(0,Q)表示采用高斯白噪声生成函数生成的维度为3×1的高斯白噪声向量，其中，0为高斯白噪声生成函数的均值，Q为高斯白噪声生成函数的状态传播过程协方差矩阵，状态传播过程协方差矩阵Q为采用随机函数生成的维度为3x3的矩阵，生成后为固定值；

e、将当前定位时f(A_t',B_t')关于状态向量A_t'的雅克比矩阵记为F_t'，F_t'采用式(23)表示为：

f、将状态传播后的协方差矩阵记为

采用公式(24)对状态传播后的协方差矩阵

进行更新：

其中P表示当前定位之前状态协方差矩阵的最新值，上标T表示矩阵的转置；当t'＝1时，即初始时刻，P初始化为维度为3×3的单位矩阵，即：

g、建立当前定位时的GPS观测模型：

其中Z_t'为当前定位时的GPS观测模型的观测向量，

为当前定位时的GPS观测模型的观测函数；N(·)为高斯白噪声生成函数，N(0,R)表示采用高斯白噪声生成函数生成的高斯白噪声向量，N(0,R)的维度为3×1，其中，0为高斯白噪声生成函数的均值，R为观测协方差矩阵，观测协方差矩阵R的维度为3×3，观测协方差矩阵R为：

h、将当前定位时观测函数

关于状态向量A_t'的雅克比矩阵记为H_t'，H_t'采用式(29)表示为：

i、将当前定位时的GPS观测模型的观测残差记为y_t'，计算当前定位时的GPS观测模型的观测残差y_t'：

j、将当前定位时的卡尔曼增益记为K_t'，计算当前定位时的卡尔曼增益K_t'：

上式中，的取值为其当前最新值；上标“-1”表示矩阵逆运算；

k、对状态向量A_t'和状态协方差矩阵P进行更新：

其中，I为维度为3×3的单位矩阵，

的取值为其当前最新值；

l、将(x′_t，y′_t)作为当前定位时车辆最终位置坐标，将θ′_t作为当前定位时车辆最终航向角，第t'次定位完成；

(11)采用l的当前值加1的和更新l的值，返回步骤(4)进行下一次定位。

与现有技术相比，本发明的优点在于通过对车辆CAN报文解析模块和磁场传感器进行同步采样匹配，构建同步匹配成功的车辆数据(车速数据、方向盘转角数据和地球磁场数据)，然后基于该车辆数据，采用将车辆轮式里程计的定位估计值与磁场传感器的灰色预测值相融合方法，抑制了轮式里程计的累积误差，提高了航向角估计的精度，通过灰色预测方法的预测值建立扩展卡尔曼滤波的观测模型，有效抑制磁场传感器的磁场数据波动，采用低成本的磁场传感器和车辆现有的速度传感器、转角传感器实现长时间的定位，在保证低成本的基础上，有效提高定位精度和鲁棒性。

具体实施方式

以下结合实施例对本发明作进一步详细描述。

实施例：一种基于磁传感器与轮式里程计融合定位方法，包括以下步骤：

(1)将车辆CAN报文解析模块的相邻两次采样之间的时间间隔记为△t₁，△t₁＝0.01s，将磁场传感器相邻两次采样之间的时间间隔记为△t₂，△t₂＝0.05s；创建一个用于存放地球磁场数据的数组，数组容量为10，当超出其容量时，该数组中已存地球磁场数据将按照存储时间从早到晚的顺序被覆盖掉，且该数组中地球磁场数据按照存入先后顺序从前向后排列，后存入的地球磁场数据排在先存入的地球磁场数据之后，将数组中存放的地球磁场数据的数量记为变量n，初始状态时，数组中不存在地球磁场数据，此时数组中存储的地球磁场数据为0，n的取值为0；设计一个用于存储车辆CAN报文解析模块每次采样获取的采样数据的时间戳的缓存，该缓存的容量为100，当超出其容量时，该缓存中已存时间戳将按照存储时间从早到晚的顺序被覆盖掉，且该缓存中时间戳按照存入先后顺序从前向后排列，后存入的时间戳排在先存入的地球磁场数据之后，车辆CAN报文解析模块每次采样获取的采样数据包括车速数据和方向盘转角数据，车辆CAN报文解析模块每次采样时的时刻采用UTC时间表示，将车辆CAN报文解析模块每次采样时的时刻作为该次采样时获取的采样数据的时间戳保存到缓存中，磁场传感器每次采样时获取地球磁场数据，磁场传感器每次采样的时刻采用UTC时间表示；

(2)设定车辆CAN报文解析模块和磁场传感器的同步采样成功匹配次数变量，将其记为t，对t进行初始化赋值，令t＝0；

(3)同时开启车辆CAN报文解析模块和磁场传感器，车辆CAN报文解析模块和磁场传感器同时开始进行第1次采样，车辆CAN报文解析模块和磁场传感器后续每采样一次其采样次数增加1；

(4)将磁场传感器的当前采样次数记为第l次，此时对车辆CAN报文解析模块和车载单目相机进行第l次同步采样匹配，具体匹配过程为：

4.1将磁场传感器第l次采样得到的地球磁场数据记为m_l，将磁场传感器第l次采样的时刻记为t_l；

4.2将t_l与缓存中存储的所有时间戳进行匹配，查找与t_l相差小于5毫秒且相差最小的时间戳，如果找到，则第l次同步采样匹配成功，获取查找到的时间戳对应的车速数据和方向盘转角数据，进入步骤(5)，如果没有找到，则采用l的当前值加1的和更新l的值，重复步骤(4)，直到满足同步采样匹配成功条件；

(5)先采用t的当前值加1的和更新t的值，然后构建第t次采样同步匹配成功的车辆数据，具体过程为：

5.1将第t次采样同步匹配成功的的车速数据记为v_t，方向盘转角数据记为δ_ft，地球磁场数据记为m^t；

5.2将当前采样同步匹配成功后查找到的时间戳对应的车速数据赋值给v_t，方向盘转角数据赋值给δ_ft，m_l赋值给m^t，第t次采样同步匹配成功的车辆数据构建完成；

(6)将m^t作为数组中最后一个数据保存到数组中，重新统计数组中地球磁场数据的数量，采用统计的数量更新n的值，将当前数组记为

其中

为当前数组中第j个地球磁场数据，j＝1,2,…,n；

(7)判断t的当前值是否大于等于2，如果大于等于2，则进入步骤(8)，如果小于2，则采用l的当前值加1的和更新l的值，重复步骤(4)～(6)，直到满足进入步骤(8)的条件；

(8)将当前数组

中n个地球磁场数据分别通过地磁坐标变换方法进行转换，得到车辆的航向角数据序列

其中

为地球磁场数据

转换得到的航向角数据，

为航向角数据序列中第j个航向角数据；

(9)采用一次累加生成算法(1-AGO)对航向角数据序列

进行处理，得到灰色序列，具体步骤为：

9.1设定一个用于存放n个数据的原始灰度序列，将其记为θ⁽⁰⁾，将θ⁽⁰⁾中的第i个数据记为θ⁽⁰⁾(i)，i＝1,2,…,n，将

的值赋给θ⁽⁰⁾(i)，得到原始灰度序列：

9.2将θ⁽⁰⁾一次累加生成序列记为θ⁽¹⁾，θ⁽¹⁾采用式(2)表示为：

θ⁽¹⁾＝(θ⁽¹⁾(1),θ⁽¹⁾(2),…,θ⁽¹⁾(n)) (2)

其中，θ⁽¹⁾(i)为θ⁽¹⁾中的第i个数据；

9.3采用式(3)为θ⁽¹⁾赋值：

9.4将θ⁽¹⁾的邻值生成序列记为z⁽¹⁾，z⁽¹⁾采用式(4)表示为：

z⁽¹⁾＝(z⁽¹⁾(1),z⁽¹⁾(2),…,z⁽¹⁾(n)) (4)

其中，z⁽¹⁾(i)为z⁽¹⁾中的第i个数据；

9.5采用式(5)对z⁽¹⁾进行赋值：

其中，K＝2,…,n；β为邻值生成权值系数，β＝0.5；

9.6建立灰微分方程模型，灰微分方程模型采用式(6)表示为：

θ⁽⁰⁾(K)+az⁽¹⁾(K)＝b (6)

其中，a称为灰色发展系数，b称为灰色作用量，a和b为待求参数；

9.7将式(6)展开并整理成矩阵向量形式，采用式(7)表示为：

Y＝Bu (7)

其中，u、B和Y分别采用式(8)、(9)和(10)表示，具体为：

9.8通过最小二乘法求解式(7)，计算得到参数a和b：

式(11)中，上标T表示矩阵的转置，上标-1表示矩阵的逆运算；

9.9建立GM(1,1)的白化模型，白化模型采用式(12)表示为：

9.10将第t-1次采样同步匹配成功的一次累加生成序列的预测值记为

采用式(13)表示为：

式(13)中，e表示自然对数的底；

9.11将第t次采样同步匹配成功的一次累加生成序列的预测值记为

采用式(14)表示为：

9.12将第t次采样同步匹配成功的原始灰度序列的的预测值记为

采用式(15)表示为：

9.13将第t次采样同步匹配成功的车辆的航向角数据的预测值记为θ_t，令

(10)将当前定位次数记为t'，令t'＝t-1，采用扩展卡尔曼滤波算法先进行数据融合后再进行车辆第t'次定位，具体过程为：

a、通过车辆运动学航迹推测算法得到车辆运动学生成的运动轨迹，即当前定位时车辆位置估计坐标

和航向角估计值

表示当前定位时车辆坐标系下的车辆位置的横坐标估计值，

表示当前定位时车辆坐标系下的车辆位置的纵坐标估计值；其中车辆运动学航迹推测算法公式如下所示：

ds_t'＝v_t'-1·△t₁ (17)

δ_ft'＝ω_t'·η (19)

其中，

表示第t'-1次定位时车辆坐标系下的车辆位置的横坐标估计值，

表示第t'-1次定位时车辆坐标系下的车辆位置的纵坐标估计值，表示第t'-1次定位时车辆航向角估计值，ds_t'表示从第t'-1次定位时到第t'次定位时车辆行驶的距离，dθ_t'表示第t'次定位时相对于第t'-1次定位时车辆航向角变化量，v_t'-1为第t'-1次定位时车辆的车速，L为车辆轴距，δ_ft'为当前定位时车辆前轮转角，η为车辆的角传动比，η预先根据车辆型号通过现有成熟的实验方法标定得到；当t'＝1时，

v_t'-1＝0，sin表示正弦函数，cos表示余弦函数，tan表示正切函数；

b、采用

构建的状态向量，将其记为A_t'，采用式(20)对A_t'进行初始化赋值：

其中，在t'＝1时，

c、采用v_t'-1和δ_ft'构建当前定位时的控制输入向量，将其记为B_t'：

d、建立当前定位时带噪声的车辆运动学模型，将其向量表达式记为f(A_t',B_t')：

其中，N(·)为高斯白噪声生成函数，N(0,Q)表示采用高斯白噪声生成函数生成的维度为3×1的高斯白噪声向量，其中，0为高斯白噪声生成函数的均值，Q为高斯白噪声生成函数的状态传播过程协方差矩阵，状态传播过程协方差矩阵Q为采用随机函数生成的维度为3x3的矩阵，生成后为固定值；

e、将当前定位时f(A_t',B_t')关于状态向量A_t'的雅克比矩阵记为F_t'，F_t'采用式(23)表示为：

f、将状态传播后的协方差矩阵记为

采用公式(24)对状态传播后的协方差矩阵

进行更新：

其中P表示当前定位之前状态协方差矩阵的最新值，上标T表示矩阵的转置；当t'＝1时，即初始时刻，P初始化为维度为3×3的单位矩阵，即：

g、建立当前定位时的GPS观测模型：

其中Z_t'为当前定位时的GPS观测模型的观测向量，

为当前定位时的GPS观测模型的观测函数；N(·)为高斯白噪声生成函数，N(0,R)表示采用高斯白噪声生成函数生成的高斯白噪声向量，N(0,R)的维度为3×1，其中，0为高斯白噪声生成函数的均值，R为观测协方差矩阵，观测协方差矩阵R的维度为3×3，观测协方差矩阵R为：

h、将当前定位时观测函数

关于状态向量A_t'的雅克比矩阵记为H_t'，H_t'采用式(29)表示为：

i、将当前定位时的GPS观测模型的观测残差记为y_t'，计算当前定位时的GPS观测模型的观测残差y_t'：

j、将当前定位时的卡尔曼增益记为K_t'，计算当前定位时的卡尔曼增益K_t'：

上式中，

的取值为其当前最新值；上标“-1”表示矩阵逆运算；

k、对状态向量A_t'和状态协方差矩阵P进行更新：

其中，I为维度为3×3的单位矩阵，

的取值为其当前最新值；

l、将(x′_t，y′_t)作为当前定位时车辆最终位置坐标，将θ′_t作为当前定位时车辆最终航向角，第t'次定位完成；

(11)采用l的当前值加1的和更新l的值，返回步骤(4)进行下一次定位。

Claims (1)

1.一种基于磁传感器与轮式里程计融合定位方法，其特征在于包括以下步骤：

(1)将车辆CAN报文解析模块的相邻两次采样之间的时间间隔记为△t₁，△t₁＝0.01s，将磁场传感器相邻两次采样之间的时间间隔记为△t₂，△t₂＝0.05s；创建一个用于存放地球磁场数据的数组，数组容量为10，当超出其容量时，该数组中已存地球磁场数据将按照存储时间从早到晚的顺序被覆盖掉，且该数组中地球磁场数据按照存入先后顺序从前向后排列，后存入的地球磁场数据排在先存入的地球磁场数据之后，将数组中存放的地球磁场数据的数量记为变量n，初始状态时，所述的数组中不存在地球磁场数据，此时所述的数组中存储的地球磁场数据为0，n的取值为0；设计一个用于存储所述的车辆CAN报文解析模块每次采样获取的采样数据的时间戳的缓存，该缓存的容量为100，当超出其容量时，该缓存中已存时间戳将按照存储时间从早到晚的顺序被覆盖掉，且该缓存中时间戳按照存入先后顺序从前向后排列，后存入的时间戳排在先存入的地球磁场数据之后，所述的车辆CAN报文解析模块每次采样获取的采样数据包括车速数据和方向盘转角数据，所述的车辆CAN报文解析模块每次采样时的时刻采用UTC时间表示，将所述的车辆CAN报文解析模块每次采样时的时刻作为该次采样时获取的采样数据的时间戳保存到所述的缓存中，所述的磁场传感器每次采样时获取地球磁场数据，所述的磁场传感器每次采样的时刻采用UTC时间表示；

(2)设定车辆CAN报文解析模块和磁场传感器的同步采样成功匹配次数变量，将其记为t，对t进行初始化赋值，令t＝0；

(3)同时开启所述的车辆CAN报文解析模块和所述的磁场传感器，所述的车辆CAN报文解析模块和所述的磁场传感器同时开始进行第1次采样，所述的车辆CAN报文解析模块和所述的磁场传感器后续每采样一次其采样次数增加1；

(4)将所述的磁场传感器的当前采样次数记为第l次，此时对所述的车辆CAN报文解析模块和所述的车载单目相机进行第l次同步采样匹配，具体匹配过程为：

4.1将所述的磁场传感器第l次采样得到的地球磁场数据记为m_l，将所述的磁场传感器第l次采样的时刻记为t_l；

4.2将t_l与缓存中存储的所有时间戳进行匹配，查找与t_l相差小于5毫秒且相差最小的时间戳，如果找到，则第l次同步采样匹配成功，获取查找到的时间戳对应的车速数据和方向盘转角数据，进入步骤(5)，如果没有找到，则采用l的当前值加1的和更新l的值，重复步骤(4)，直到满足同步采样匹配成功条件；

(5)先采用t的当前值加1的和更新t的值，然后构建第t次采样同步匹配成功的车辆数据，具体过程为：

5.1将第t次采样同步匹配成功的的车速数据记为v_t，方向盘转角数据记为δ_ft，地球磁场数据记为m^t；

5.2将当前采样同步匹配成功后查找到的时间戳对应的车速数据赋值给v_t，方向盘转角数据赋值给δ_ft，m_l赋值给m^t，第t次采样同步匹配成功的车辆数据构建完成；

(6)将m^t作为数组中最后一个数据保存到数组中，重新统计数组中地球磁场数据的数量，采用统计的数量更新n的值，将当前数组记为

其中

为当前数组中第j个地球磁场数据，j＝1,2,…,n；

(7)判断t的当前值是否大于等于2，如果大于等于2，则进入步骤(8)，如果小于2，则采用l的当前值加1的和更新l的值，重复步骤(4)～(6)，直到满足进入步骤(8)的条件；

(8)将当前数组

中n个地球磁场数据分别通过地磁坐标变换方法进行转换，得到车辆的航向角数据序列

其中

为地球磁场数据转换得到的航向角数据，

为航向角数据序列中第j个航向角数据；

(9)采用一次累加生成算法(1-AGO)对航向角数据序列

进行处理，得到灰色序列，具体步骤为：

9.1设定一个用于存放n个数据的原始灰度序列，将其记为θ⁽⁰⁾，将θ⁽⁰⁾中的第i个数据记为θ⁽⁰⁾(i)，i＝1,2,…,n，将

的值赋给θ⁽⁰⁾(i)，得到原始灰度序列：

9.2将θ⁽⁰⁾一次累加生成序列记为θ⁽¹⁾，θ⁽¹⁾采用式(2)表示为：

θ⁽¹⁾＝(θ⁽¹⁾(1),θ⁽¹⁾(2),…,θ⁽¹⁾(n)) (2)

其中，θ⁽¹⁾(i)为θ⁽¹⁾中的第i个数据；

9.3采用式(3)为θ⁽¹⁾赋值：

9.4将θ⁽¹⁾的邻值生成序列记为z⁽¹⁾，z⁽¹⁾采用式(4)表示为：

z⁽¹⁾＝(z⁽¹⁾(1),z⁽¹⁾(2),…,z⁽¹⁾(n)) (4)

其中，z⁽¹⁾(i)为z⁽¹⁾中的第i个数据；

9.5采用式(5)对z⁽¹⁾进行赋值：

其中，K＝2,…,n；β为邻值生成权值系数，β＝0.5；

9.6建立灰微分方程模型，灰微分方程模型采用式(6)表示为：

θ⁽⁰⁾(K)+az⁽¹⁾(K)＝b (6)

其中，a称为灰色发展系数，b称为灰色作用量，a和b为待求参数；

9.7将式(6)展开并整理成矩阵向量形式，采用式(7)表示为：

Y＝Bu (7)

其中，u、B和Y分别采用式(8)、(9)和(10)表示，具体为：

9.8通过最小二乘法求解式(7)，计算得到参数a和b：

式(11)中，上标T表示矩阵的转置，上标-1表示矩阵的逆运算；

9.9建立GM(1,1)的白化模型，白化模型采用式(12)表示为：

9.10将第t-1次采样同步匹配成功的一次累加生成序列的预测值记为

采用式(13)表示为：

式(13)中，e表示自然对数的底；

9.11将第t次采样同步匹配成功的一次累加生成序列的预测值记为

采用式(14)表示为：

9.12将第t次采样同步匹配成功的原始灰度序列的的预测值记为

采用式(15)表示为：

9.13将第t次采样同步匹配成功的车辆的航向角数据的预测值记为θ_t，令

(10)将当前定位次数记为t'，令t'＝t-1，采用扩展卡尔曼滤波算法先进行数据融合后再进行车辆第t'次定位，具体过程为：

a、通过车辆运动学航迹推测算法得到车辆运动学生成的运动轨迹，即当前定位时车辆位置估计坐标

和航向角估计值

表示当前定位时车辆坐标系下的车辆位置的横坐标估计值，

表示当前定位时车辆坐标系下的车辆位置的纵坐标估计值；其中车辆运动学航迹推测算法公式如下所示：

ds_t'＝v_t'-1·△t₁ (17)

δ_ft'＝ω_t'·η (19)

其中，表示第t'-1次定位时车辆坐标系下的车辆位置的横坐标估计值，

表示第t'-1次定位时车辆坐标系下的车辆位置的纵坐标估计值，表示第t'-1次定位时车辆航向角估计值，ds_t'表示从第t'-1次定位时到第t'次定位时车辆行驶的距离，dθ_t'表示第t'次定位时相对于第t'-1次定位时车辆航向角变化量，v_t'-1为第t'-1次定位时车辆的车速，L为车辆轴距，δ_ft'为当前定位时车辆前轮转角，η为车辆的角传动比，η预先根据车辆型号通过现有成熟的实验方法标定得到；当t'＝1时，

v_t'-1＝0，sin表示正弦函数，cos表示余弦函数，tan表示正切函数；

b、采用

和构建的状态向量，将其记为A_t'，采用式(20)对A_t'进行初始化赋值：

其中，在t'＝1时，

c、采用v_t'-1和δ_ft'构建当前定位时的控制输入向量，将其记为B_t'：

d、建立当前定位时带噪声的车辆运动学模型，将其向量表达式记为f(A_t',B_t')：

其中，N(·)为高斯白噪声生成函数，N(0,Q)表示采用高斯白噪声生成函数生成的维度为3×1的高斯白噪声向量，其中，0为高斯白噪声生成函数的均值，Q为高斯白噪声生成函数的状态传播过程协方差矩阵，状态传播过程协方差矩阵Q为采用随机函数生成的维度为3x3的矩阵，生成后为固定值；

e、将当前定位时f(A_t',B_t')关于状态向量A_t'的雅克比矩阵记为F_t'，F_t'采用式(23)表示为：

f、将状态传播后的协方差矩阵记为采用公式(24)对状态传播后的协方差矩阵进行更新：

其中P表示当前定位之前状态协方差矩阵的最新值，上标T表示矩阵的转置；当t'＝1时，即初始时刻，P初始化为维度为3×3的单位矩阵，即：

g、建立当前定位时的GPS观测模型：

其中Z_t'为当前定位时的GPS观测模型的观测向量，

为当前定位时的GPS观测模型的观测函数；N(·)为高斯白噪声生成函数，N(0,R)表示采用高斯白噪声生成函数生成的高斯白噪声向量，N(0,R)的维度为3×1，其中，0为高斯白噪声生成函数的均值，R为观测协方差矩阵，观测协方差矩阵R的维度为3×3，观测协方差矩阵R为：

h、将当前定位时观测函数

关于状态向量A_t'的雅克比矩阵记为H_t'，H_t'采用式(29)表示为：

i、将当前定位时的GPS观测模型的观测残差记为y_t'，计算当前定位时的GPS观测模型的观测残差y_t'：

j、将当前定位时的卡尔曼增益记为K_t'，计算当前定位时的卡尔曼增益K_t'：

上式中，的取值为其当前最新值；上标“-1”表示矩阵逆运算；

k、对状态向量A_t'和状态协方差矩阵P进行更新：

其中，I为维度为3×3的单位矩阵，的取值为其当前最新值；

l、将(x′_t，y′_t)作为当前定位时车辆最终位置坐标，将θ′_t作为当前定位时车辆最终航向角，第t'次定位完成；

(11)采用l的当前值加1的和更新l的值，返回步骤(4)进行下一次定位。