CN110865403B - 一种基于神经网络预学习和轮式里程计融合的定位方法 - Google Patents

Abstract

本发明公开了一种基于神经网络预学习和轮式里程计融合的定位方法，该方法首先对神经网络模型进行训练，然后采用训练后的神经网络模型对车辆位置进行预测，最后通过无迹卡尔曼滤波算法得到车辆最终位置数据；优点是采用神经网络预学习和轮式里程计相融合实现定位，在具有较低成本的基础上，使神经网络具有较强的泛化能力，克服轮式里程计误差较大的缺陷，最终实现较高的定位精度和较高的鲁棒性。

Description

一种基于神经网络预学习和轮式里程计融合的定位方法

技术领域

本发明涉及一种定位方法，尤其是涉及一种基于神经网络预学习和轮式里程计融合的定位方法。

背景技术

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

深度神经网络是目前在机器学习领域应用最为广泛的算法之一，可以求解非线性问题，目前有使用神经网络进行车辆参数估计的应用，但完全依赖神经网络进行车辆定位还存在较大问题，泛化能力较差。

发明内容

本发明所要解决的技术问题是提供一种基于神经网络预学习和轮式里程计融合的定位方法。该定位方法采用神经网络预学习和轮式里程计相融合实现定位，在具有较低成本的基础上，使神经网络具有较强的泛化能力，克服轮式里程计误差较大的缺陷，最终实现较高的定位精度和较高的鲁棒性。

本发明解决上述技术问题所采用的技术方案为：一种基于神经网络预学习和轮式里程计融合的定位方法，包括以下步骤：

(1)获取一个标准的由输入层、激活层和全连接层构成的神经网络模型，其中激活层采用RELU激活函数实现，将该神经网络模型记为M_train，将全连接层的连接权重记为W，对该神经网络模型M_train进行训练确定全连接层的连接权重W，具体训练过程为：

A.通过差分GPS模块采集车辆移动过程中某个时刻所处位置的GPS经纬度数据后经过坐标变换，得到该时刻车辆坐标系下的车辆位置坐标和航向角，通过车辆CAN报文解析模块同步获取该时刻的车速和方向盘转角；将该时刻作为第1个时刻，采集连续的U个时刻车辆所处位置的GPS经纬度数据、车速和方向盘转角，U的取值为大于等于10000整数，将U个时刻车辆所处位置的GPS经纬度数据分别转换为车辆位置坐标和航向角，采用每个时刻的车速、方向盘转角、车辆位置坐标和航向角构成该时刻的数据组，得到U个数据组，将U个数据组中，第δ次得到的数据组中车辆坐标系下的车辆位置坐标记为(x^gps-δ，y^gps-δ)，航向角记为θ^gps-δ，速度记为v^δ，方向盘转角记为α^δ，δ＝1,2,…,U，其中x^gps-δ表示第δ次得到的车辆坐标系下的车辆位置的横坐标，y^gps-δ表示第δ次得到的车辆坐标系下的车辆位置的纵坐标；

B.将第δ次得到的车辆坐标系下的车辆位置的横坐标的变化量记为△x^gps-δ，车辆位置的纵坐标的变化量记为△y^gps-δ，航向角的变化量记为△θ^gps-δ，△x^gps-δ、△y^gps-δ和△y^gps-δ分别采用式(1)、式(2)和式(3)表示为：

△x^gps-δ＝x^gps-δ-x^gps-(δ-1) (1)

△y^gps-δ＝y^gps-δ-y^gps-(δ-1) (2)

△θ^gps-δ＝θ^gps-δ-θ^gps-(δ-1) (3)

其中，△x^gps-0＝0，△y^gps-0＝0，△θ^gps-0＝0；

C.采用获取的α^δ、v^δ以及车辆CAN报文解析模块采样周期△t构建输入训练数据向量，将构建的输入训练数据向量记为^dataL^δ，^dataL^δ采用式(4)表示为：

采用△x^gps-δ、△y^gps-δ和△θ^gps-δ构建输入训练数据向量^dataL^δ对应的输出训练数据向量，将输出训练数据向量记为^labelL^δ，^labelL^δ采用式(5)表示为：

由此得到U组训练数据向量对(^dataL¹，^labelL¹)～(^dataL^U，^labelL^U)；

D.采用U组训练数据向量对(^dataL¹，^labelL¹)～(^dataL^U，^labelL^U)对神经网络模型M_train进行训练，得到全连接层的连接权重W，将得到全连接层的连接权重W代入神经网络模型M_train中，得到训练后的神经网络模型M_train；

(2)设定当前采样时刻变量t；对t进行初始化赋值，令t＝1；

(3)对当前采样时刻t车辆的位置进行确定，具体过程为：

3.1通过车辆CAN报文解析模块获取当前采样时刻t的车速v_t和方向盘转角α_t；

3.2将车辆坐标系下，当前采样时刻t车辆位置的预测坐标记为(ⁿⁿx_t，ⁿⁿy_t)，车辆位置的航向角预测值记为ⁿⁿθ_t，其中ⁿⁿx_t为当前采样时刻t车辆位置的横坐标预测值，ⁿⁿy_t为当前采样时刻t车辆位置的纵坐标预测值，按照以下方法确定ⁿⁿx_t、ⁿⁿy_t和ⁿⁿθ_t：

3.2.1采用当前采样时刻t车速数据v_t、方向盘转角数据α_t和△t构建三维向量，该三维向量表示为将三维向量/>作为训练后的神经网络模型M_train的输入向量数据进行神经网络推理后得到输出向量数据，将该输出向量数据记为/>其中，△x_t表示车辆位置的横坐标预测值在当前采样时刻t相对于前一采样时刻t-1的变化量，△y_t表示车辆位置的纵坐标预测值在当前采样时刻t相对于前一采样时刻t-1的变化量，△θ_t表示车辆位置的航向角预测值在当前采样时刻t相对于前一采样时刻t-1的变化量；

3.2.2采用公式(6)分别计算得到ⁿⁿx_t、ⁿⁿy_t和ⁿⁿθ_t：

式(6)中，ⁿⁿx_t-1表示前一采样时刻t-1车辆位置的横坐标预测值，ⁿⁿy_t-1表示前一采样时刻t-1车辆位置的纵坐标预测值，ⁿⁿθ_t-1表示前一采样时刻t-1车辆位置的航向角预测值，在t＝1时的初始采样时刻，ⁿⁿx₀＝0，ⁿⁿy₀＝,0，ⁿⁿθ₀＝0；

3.3通过车辆运动学航迹推测算法得到车辆运动学生成的运动轨迹，即当前采样时刻t车辆位置的估计坐标和航向角估计值/>表示当前采样时刻t车辆坐标系下的车辆位置的横坐标估计值，/>表示当前采样时刻t车辆坐标系下的车辆位置的纵坐标估计值；其中车辆运动学航迹推测算法公式如下所示：

ds_t＝v_t-1·dt (8)

δ_ft＝α_t·η (10)

其中，表示车辆坐标系下前一采样时刻t-1车辆位置的横坐标估计值，/>表示车辆坐标系下前一采样时刻t-1车辆位置的纵坐标估计值，/>表示前一采样时刻t-1车辆位置的航向角估计值，ds_t表示从前一采样时刻t-1到当前采样时刻t车辆行驶的距离，dθ_t表示当前采样时刻t相对于前一采样时刻t-1车辆位置的航向角变化量，v_t-1为前一采样时刻t-1车辆的车速，dt为整车CAN解析模块的采样周期，L为车辆轴距，δ_ft为当前采样时刻t车辆前轮转角，η为车辆的角传动比，“·”为乘运算符号，当t＝1时，/> v_t-1＝0，sin表示正弦函数，cos表示余弦函数，tan表示正切函数。

3.4通过无迹卡尔曼滤波算法对当前采样时刻t的数据：车速v_t、方向盘转角α_t、

ⁿⁿx_t、ⁿⁿy_t和ⁿⁿθ_t进行数据融合，得到车辆当前采样时刻t的位置，具体过程如下：

a.采用和/>构建当前采样时刻t的状态向量，将该状态向量记为A_t，采用以下公式对A_t进行初始化赋值：

其中，在t＝1时的初始时刻，

b.采用v_t-1和δ_ft构建当前采样时刻t的控制输入向量，将该控制输入向量记为B_t：

其中，v_t-1＝0；

c.生成7个当前采样时刻t的无迹卡尔曼滤波的sigma点向量，将当前采样时刻t的无迹卡尔曼滤波的第i个sigma点向量记为采用公式(13)计算得到

式(13)中，C_t表示当前采样时刻t的参考矩阵，[C_t]_i表示选取参考矩阵C_t的第i列，[C_t]_i-3表示选取参考矩阵C_t的第i-3列，C_t采用公式(14)计算得到：

式(14)中，λ＝3α²-3，α＝0.1，P为当前采样时刻t状态传播前的协方差矩阵，其取值为当前最新值，当t＝1时，即初始时刻，P初始化为维度为3×3的单位矩阵，即：

d.将当前采样时刻t的无迹卡尔曼滤波的第i个sigma点向量的均值权重记为协方差权重记为/>分别采用公式(16)和公式(17)生成当前采样时刻t的无迹卡尔曼滤波的每个sigma点向量的均值权重和协方差权重：

式(17)中，β＝2；

e.通过带噪声的车辆运动学状态传播方程对sigma点进行状态传播，将当前采样时刻t的无迹卡尔曼滤波的第i个sigma点向量进行状态传播后得到的向量记为F_i ^t，F_i ^t采用式(18)表示为：

其中，表示/>第1行的元素，/>表示/>第2行的元素，/>表示/>第3行的元素，N(·)为高斯白噪声生成函数，N(0,Q)表示当前采用时刻t采用高斯白噪声生成函数生成的维度为3×1的高斯白噪声向量，其中，0为高斯白噪声生成函数的均值，Q为高斯白噪声生成函数的状态传播过程协方差矩阵，状态传播过程协方差矩阵Q采用随机函数生成，生成后为固定值，其维度为3×3；

f.对状态向量A_t进行无迹变换，得到状态向量A_t的预测值以及协方差矩阵，将状态向量A_t的预测值记为状态向量A_t的协方差矩阵记为/> 采用式(19)表示为：

采用式(20)表示为：

式(20)中，上标T表示矩阵的转置；

g.建立当前采样时刻t的观测模型，将该观测模型记为z_t，z_t采用式(21)表示为：

式(21)中，N(·)为高斯白噪声生成函数，N(0,R)表示采用高斯白噪声生成函数生成的高斯白噪声向量，N(0,R)的维度为3×1，其中，0为高斯白噪声生成函数的均值，R为观测协方差矩阵，观测协方差矩阵R的维度为3×3，观测协方差矩阵R为固定值，取值为：

h.构建当前采样时刻t的观测矩阵H_t，采用观测矩阵H_t对F_i ^t进行观测变换，得到观测变换后的sigma点向量H_t和/>分别采用式(23)和式(14)表示为：

j.对进行无迹变换，得到观测变换后的sigma点的均值/>及协方差矩阵/> 和/>分别采用式(25)和式(26)表示为：

k.将当前采样时刻t的观测模型的观测残差记为y_t，y_t采用式(27)表示为：

l.将当前采样时刻t的卡尔曼增益记为K_t，采用式(28)计算得到K_t：

m.采用式(29)和(30)分别对状态向量A_t和状态协方差矩阵P进行更新：

n、将(x_t'，y_t')作为当前采样时刻t车辆最终位置坐标，将θ_t'作为当前时刻t车辆最终航向角，采用当前时刻t车辆最终位置坐标(x_t'，y_t')和当前采样时刻t车辆最终航向角θ_t'构成当前采样时刻t车辆最终的位置，实现当前采样时刻t车辆的定位；

(4)采用t的当前值加1的值更新t，返回步骤(3)进行下一时刻的车辆定位。

与现有技术相比，本发明的优点在于通过采用神经网络预学习和轮式里程计相融合实现定位，可以采用车辆现有的速度传感器、转角传感器，实时性好，在具有较低成本的基础上，使神经网络具有较强的泛化能力，克服轮式里程计误差较大的缺陷，最终实现较高的定位精度和较高的鲁棒性。

具体实施方式

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

实施例：一种基于神经网络预学习和轮式里程计融合的定位方法，包括以下步骤：

(1)获取一个标准的由输入层、激活层和全连接层构成的神经网络模型，其中激活层采用RELU激活函数实现，将该神经网络模型记为M_train，将全连接层的连接权重记为W，对该神经网络模型M_train进行训练确定全连接层的连接权重W，具体训练过程为：

A.通过差分GPS模块采集车辆移动过程中某个时刻所处位置的GPS经纬度数据后经过坐标变换，得到该时刻车辆坐标系下的车辆位置坐标和航向角，通过车辆CAN报文解析模块同步获取该时刻的车速和方向盘转角；将该时刻作为第1个时刻，采集连续的U个时刻车辆所处位置的GPS经纬度数据、车速和方向盘转角，U的取值为大于等于10000整数，将U个时刻车辆所处位置的GPS经纬度数据分别转换为车辆位置坐标和航向角，采用每个时刻的车速、方向盘转角、车辆位置坐标和航向角构成该时刻的数据组，得到U个数据组，将U个数据组中，第δ次得到的数据组中车辆坐标系下的车辆位置坐标记为(x^gps-δ，y^gps-δ)，航向角记为θ^gps-δ，速度记为v^δ，方向盘转角记为α^δ，δ＝1,2,…,U，其中x^gps-δ表示第δ次得到的车辆坐标系下的车辆位置的横坐标，y^gps-δ表示第δ次得到的车辆坐标系下的车辆位置的纵坐标；

B.将第δ次得到的车辆坐标系下的车辆位置的横坐标的变化量记为△x^gps-δ，车辆位置的纵坐标的变化量记为△y^gps-δ，航向角的变化量记为△θ^gps-δ，△x^gps-δ、△y^gps-δ和△y^gps-δ分别采用式(1)、式(2)和式(3)表示为：

△x^gps-δ＝x^gps-δ-x^gps-(δ-1) (1)

△y^gps-δ＝y^gps-δ-y^gps-(δ-1) (2)

△θ^gps-δ＝θ^gps-δ-θ^gps-(δ-1) (3)

其中，△x^gps-0＝0，△y^gps-0＝0，△θ^gps-0＝0；

C.采用获取的α^δ、v^δ以及车辆CAN报文解析模块采样周期△t构建输入训练数据向量，将构建的输入训练数据向量记为^dataL^δ，^dataL^δ采用式(4)表示为：

采用△x^gps-δ、△y^gps-δ和△θ^gps-δ构建输入训练数据向量^dataL^δ对应的输出训练数据向量，将输出训练数据向量记为^labelL^δ，^labelL^δ采用式(5)表示为：

由此得到U组训练数据向量对(^dataL¹，^labelL¹)～(^dataL^U，^labelL^U)；

D.采用U组训练数据向量对(^dataL¹，^labelL¹)～(^dataL^U，^labelL^U)对神经网络模型M_train进行训练，得到全连接层的连接权重W，将得到全连接层的连接权重W代入神经网络模型M_train中，得到训练后的神经网络模型M_train；

(2)设定当前采样时刻变量t；对t进行初始化赋值，令t＝1；

(3)对当前采样时刻t车辆的位置进行确定，具体过程为：

3.1通过车辆CAN报文解析模块获取当前采样时刻t的车速v_t和方向盘转角α_t；

3.2将车辆坐标系下，当前采样时刻t车辆位置的预测坐标记为(ⁿⁿx_t，ⁿⁿy_t)，车辆位置的航向角预测值记为ⁿⁿθ_t，其中ⁿⁿx_t为当前采样时刻t车辆位置的横坐标预测值，ⁿⁿy_t为当前采样时刻t车辆位置的纵坐标预测值，按照以下方法确定ⁿⁿx_t、ⁿⁿy_t和ⁿⁿθ_t：

3.2.1采用当前采样时刻t车速数据v_t、方向盘转角数据α_t和△t构建三维向量，该三维向量表示为将三维向量/>作为训练后的神经网络模型M_train的输入向量数据进行神经网络推理后得到输出向量数据，将该输出向量数据记为/>其中，△x_t表示车辆位置的横坐标预测值在当前采样时刻t相对于前一采样时刻t-1的变化量，△y_t表示车辆位置的纵坐标预测值在当前采样时刻t相对于前一采样时刻t-1的变化量，△θ_t表示车辆位置的航向角预测值在当前采样时刻t相对于前一采样时刻t-1的变化量；

3.2.2采用公式(6)分别计算得到ⁿⁿx_t、ⁿⁿy_t和ⁿⁿθ_t：

式(6)中，ⁿⁿx_t-1表示前一采样时刻t-1车辆位置的横坐标预测值，ⁿⁿy_t-1表示前一采样时刻t-1车辆位置的纵坐标预测值，ⁿⁿθ_t-1表示前一采样时刻t-1车辆位置的航向角预测值，在t＝1时的初始采样时刻，ⁿⁿx₀＝0，ⁿⁿy₀＝,0，ⁿⁿθ₀＝0；

3.3通过车辆运动学航迹推测算法得到车辆运动学生成的运动轨迹，即当前采样时刻t车辆位置的估计坐标和航向角估计值/>表示当前采样时刻t车辆坐标系下的车辆位置的横坐标估计值，/>表示当前采样时刻t车辆坐标系下的车辆位置的纵坐标估计值；其中车辆运动学航迹推测算法公式如下所示：

ds_t＝v_t-1·dt (8)

δ_ft＝α_t·η (10)

其中，表示车辆坐标系下前一采样时刻t-1车辆位置的横坐标估计值，/>表示车辆坐标系下前一采样时刻t-1车辆位置的纵坐标估计值，/>表示前一采样时刻t-1车辆位置的航向角估计值，ds_t表示从前一采样时刻t-1到当前采样时刻t车辆行驶的距离，dθ_t表示当前采样时刻t相对于前一采样时刻t-1车辆位置的航向角变化量，v_t-1为前一采样时刻t-1车辆的车速，dt为整车CAN解析模块的采样周期，L为车辆轴距，δ_ft为当前采样时刻t车辆前轮转角，η为车辆的角传动比，“·”为乘运算符号，当t＝1时，/> v_t-1＝0，sin表示正弦函数，cos表示余弦函数，tan表示正切函数。

3.4通过无迹卡尔曼滤波算法对当前采样时刻t的数据：车速v_t、方向盘转角α_t、

ⁿⁿx_t、ⁿⁿy_t和ⁿⁿθ_t进行数据融合，得到车辆当前采样时刻t的位置，具体过程如下：

a.采用和/>构建当前采样时刻t的状态向量，将该状态向量记为A_t，采用以下公式对A_t进行初始化赋值：

其中，在t＝1时的初始时刻，

b.采用v_t-1和δ_ft构建当前采样时刻t的控制输入向量，将该控制输入向量记为B_t：

其中，v_t-1＝0；

c.生成7个当前采样时刻t的无迹卡尔曼滤波的sigma点向量，将当前采样时刻t的无迹卡尔曼滤波的第i个sigma点向量记为采用公式(13)计算得到

式(13)中，C_t表示当前采样时刻t的参考矩阵，[C_t]_i表示选取参考矩阵C_t的第i列，[C_t]_i-3表示选取参考矩阵C_t的第i-3列，C_t采用公式(14)计算得到：

式(14)中，λ＝3α²-3，α＝0.1，P为当前采样时刻t状态传播前的协方差矩阵，其取值为当前最新值，当t＝1时，即初始时刻，P初始化为维度为3×3的单位矩阵，即：

d.将当前采样时刻t的无迹卡尔曼滤波的第i个sigma点向量的均值权重记为协方差权重记为/>分别采用公式(16)和公式(17)生成当前采样时刻t的无迹卡尔曼滤波的每个sigma点向量的均值权重和协方差权重：

式(17)中，β＝2；

e.通过带噪声的车辆运动学状态传播方程对sigma点进行状态传播，将当前采样时刻t的无迹卡尔曼滤波的第i个sigma点向量进行状态传播后得到的向量记为F_i ^t，F_i ^t采用式(18)表示为：

其中，表示/>第1行的元素，/>表示/>第2行的元素，/>表示/>第3行的元素，N(·)为高斯白噪声生成函数，N(0,Q)表示当前采用时刻t采用高斯白噪声生成函数生成的维度为3×1的高斯白噪声向量，其中，0为高斯白噪声生成函数的均值，Q为高斯白噪声生成函数的状态传播过程协方差矩阵，状态传播过程协方差矩阵Q采用随机函数生成，生成后即为固定值，其维度为3×3；

f.对状态向量A_t进行无迹变换，得到状态向量A_t的预测值以及协方差矩阵，将状态向量A_t的预测值记为状态向量A_t的协方差矩阵记为/> 采用式(19)表示为：

采用式(20)表示为：

式(20)中，上标T表示矩阵的转置；

g.建立当前采样时刻t的观测模型，将该观测模型记为z_t，z_t采用式(21)表示为：

式(21)中，N(·)为高斯白噪声生成函数，N(0,R)表示采用高斯白噪声生成函数生成的高斯白噪声向量，N(0,R)的维度为3×1，其中，0为高斯白噪声生成函数的均值，R为观测协方差矩阵，观测协方差矩阵R的维度为3×3，观测协方差矩阵R为固定值，取值为：

h.构建当前采样时刻t的观测矩阵H_t，采用观测矩阵H_t对F_i ^t进行观测变换，得到观测变换后的sigma点向量H_t和/>分别采用式(23)和式(14)表示为：/>

j.对进行无迹变换，得到观测变换后的sigma点的均值/>及协方差矩阵/> 和/>分别采用式(25)和式(26)表示为：

k.将当前采样时刻t的观测模型的观测残差记为y_t，y_t采用式(27)表示为：

l.将当前采样时刻t的卡尔曼增益记为K_t，采用式(28)计算得到K_t：

m.采用式(29)和(30)分别对状态向量A_t和状态协方差矩阵P进行更新：

n、将(x_t'，y_t')作为当前采样时刻t车辆最终位置坐标，将θ_t'作为当前时刻t车辆最终航向角，采用当前时刻t车辆最终位置坐标(x_t'，y_t')和当前采样时刻t车辆最终航向角θ_t'构成当前采样时刻t车辆最终的位置，实现当前采样时刻t车辆的定位；

(4)采用t的当前值加1的值更新t，返回步骤(3)进行下一时刻的车辆定位。

Claims (1)

1.一种基于神经网络预学习和轮式里程计融合的定位方法，其特征在于包括以下步骤：

(1)获取一个标准的由输入层、激活层和全连接层构成的神经网络模型，其中激活层采用RELU激活函数实现，将该神经网络模型记为M_train，将全连接层的连接权重记为W，对该神经网络模型M_train进行训练确定全连接层的连接权重W，具体训练过程为：

A.通过差分GPS模块采集车辆移动过程中某个时刻所处位置的GPS经纬度数据后经过坐标变换，得到该时刻车辆坐标系下的车辆位置坐标和航向角，通过车辆CAN报文解析模块同步获取该时刻的车速和方向盘转角；将该时刻作为第1个时刻，采集连续的U个时刻车辆所处位置的GPS经纬度数据、车速和方向盘转角，U的取值为大于等于10000整数，将U个时刻车辆所处位置的GPS经纬度数据分别转换为车辆位置坐标和航向角，采用每个时刻的车速、方向盘转角、车辆位置坐标和航向角构成该时刻的数据组，得到U个数据组，将U个数据组中，第δ次得到的数据组中车辆坐标系下的车辆位置坐标记为(x^gps-δ，y^gps-δ)，航向角记为θ^gps-δ，速度记为v^δ，方向盘转角记为α^δ，δ＝1,2,…,U，其中x^gps-δ表示第δ次得到的车辆坐标系下的车辆位置的横坐标，y^gps-δ表示第δ次得到的车辆坐标系下的车辆位置的纵坐标；

B.将第δ次得到的车辆坐标系下的车辆位置的横坐标的变化量记为△x^gps-δ，车辆位置的纵坐标的变化量记为△y^gps-δ，航向角的变化量记为△θ^gps-δ，△x^gps-δ、△y^gps-δ和△y^gps-δ分别采用式(1)、式(2)和式(3)表示为：

△x^gps-δ＝x^gps-δ-x^gps-(δ-1) (1)

△y^gps-δ＝y^gps-δ-y^gps-(δ-1) (2)

△θ^gps-δ＝θ^gps-δ-θ^gps-(δ-1) (3)

其中，△x^gps-0＝0，△y^gps-0＝0，△θ^gps-0＝0；

C.采用获取的α^δ、v^δ以及车辆CAN报文解析模块采样周期△t构建输入训练数据向量，将构建的输入训练数据向量记为^dataL^δ，^dataL^δ采用式(4)表示为：

采用△x^gps-δ、△y^gps-δ和△θ^gps-δ构建输入训练数据向量^dataL^δ对应的输出训练数据向量，将输出训练数据向量记为^labelL^δ，^labelL^δ采用式(5)表示为：

由此得到U组训练数据向量对(^dataL¹，^labelL¹)～(^dataL^U，^labelL^U)；

D.采用U组训练数据向量对(^dataL¹，^labelL¹)～(^dataL^U，^labelL^U)对神经网络模型M_train进行训练，得到全连接层的连接权重W，将得到全连接层的连接权重W代入神经网络模型M_train中，得到训练后的神经网络模型M_train；

(2)设定当前采样时刻变量t；对t进行初始化赋值，令t＝1；

(3)对当前采样时刻t车辆的位置进行确定，具体过程为：

3.1通过车辆CAN报文解析模块获取当前采样时刻t的车速v_t和方向盘转角α_t；

3.2将车辆坐标系下，当前采样时刻t车辆位置的预测坐标记为(ⁿⁿx_t，ⁿⁿy_t)，车辆位置的航向角预测值记为ⁿⁿθ_t，其中ⁿⁿx_t为当前采样时刻t车辆位置的横坐标预测值，ⁿⁿy_t为当前采样时刻t车辆位置的纵坐标预测值，按照以下方法确定ⁿⁿx_t、ⁿⁿy_t和ⁿⁿθ_t：

3.2.1采用当前采样时刻t车速数据v_t、方向盘转角数据α_t和△t构建三维向量，该三维向量表示为将三维向量/>作为训练后的神经网络模型M_train的输入向量数据进行神经网络推理后得到输出向量数据，将该输出向量数据记为/>其中，△x_t表示车辆位置的横坐标预测值在当前采样时刻t相对于前一采样时刻t-1的变化量，△y_t表示车辆位置的纵坐标预测值在当前采样时刻t相对于前一采样时刻t-1的变化量，△θ_t表示车辆位置的航向角预测值在当前采样时刻t相对于前一采样时刻t-1的变化量；

3.2.2采用公式(6)分别计算得到ⁿⁿx_t、ⁿⁿy_t和ⁿⁿθ_t：

式(6)中，ⁿⁿx_t-1表示前一采样时刻t-1车辆位置的横坐标预测值，ⁿⁿy_t-1表示前一采样时刻t-1车辆位置的纵坐标预测值，ⁿⁿθ_t-1表示前一采样时刻t-1车辆位置的航向角预测值，在t＝1时的初始采样时刻，ⁿⁿx₀＝0，ⁿⁿy₀＝,0，ⁿⁿθ₀＝0；

3.3通过车辆运动学航迹推测算法得到车辆运动学生成的运动轨迹，即当前采样时刻t车辆位置的估计坐标和航向角估计值/> 表示当前采样时刻t车辆坐标系下的车辆位置的横坐标估计值，/>表示当前采样时刻t车辆坐标系下的车辆位置的纵坐标估计值；其中车辆运动学航迹推测算法公式如下所示：

ds_t＝v_t-1·dt (8)

δ_ft＝α_t·η (10)

其中，表示车辆坐标系下前一采样时刻t-1车辆位置的横坐标估计值，/>表示车辆坐标系下前一采样时刻t-1车辆位置的纵坐标估计值，/>表示前一采样时刻t-1车辆位置的航向角估计值，ds_t表示从前一采样时刻t-1到当前采样时刻t车辆行驶的距离，dθ_t表示当前采样时刻t相对于前一采样时刻t-1车辆位置的航向角变化量，v_t-1为前一采样时刻t-1车辆的车速，dt为整车CAN解析模块的采样周期，L为车辆轴距，δ_ft为当前采样时刻t车辆前轮转角，η为车辆的角传动比，“·”为乘运算符号，当t＝1时，/> v_t-1＝0，sin表示正弦函数，cos表示余弦函数，tan表示正切函数；

3.4通过无迹卡尔曼滤波算法对当前采样时刻t的数据：车速v_t、方向盘转角α_t、ⁿⁿx_t、ⁿⁿy_t和ⁿⁿθ_t进行数据融合，得到车辆当前采样时刻t的位置，具体过程如下：

a.采用和/>构建当前采样时刻t的状态向量，将该状态向量记为A_t，采用以下公式对A_t进行初始化赋值：

其中，在t＝1时的初始时刻，

b.采用v_t-1和δ_ft构建当前采样时刻t的控制输入向量，将该控制输入向量记为B_t：

其中，v_t-1＝0；

c.生成7个当前采样时刻t的无迹卡尔曼滤波的sigma点向量，将当前采样时刻t的无迹卡尔曼滤波的第i个sigma点向量记为采用公式(13)计算得到/>

式(13)中，C_t表示当前采样时刻t的参考矩阵，[C_t]_i表示选取参考矩阵C_t的第i列，[C_t]_i-3表示选取参考矩阵C_t的第i-3列，C_t采用公式(14)计算得到：

式(14)中，λ＝3α²-3，α＝0.1，P为当前采样时刻t状态传播前的协方差矩阵，其取值为当前最新值，当t＝1时，即初始时刻，P初始化为维度为3×3的单位矩阵，即：

d.将当前采样时刻t的无迹卡尔曼滤波的第i个sigma点向量的均值权重记为/>协方差权重记为/>分别采用公式(16)和公式(17)生成当前采样时刻t的无迹卡尔曼滤波的每个sigma点向量的均值权重和协方差权重：

式(17)中，β＝2；

e.通过带噪声的车辆运动学状态传播方程对sigma点进行状态传播，将当前采样时刻t的无迹卡尔曼滤波的第i个sigma点向量进行状态传播后得到的向量记为F_i ^t，F_i ^t采用式(18)表示为：

其中，表示/>第1行的元素，/>表示/>第2行的元素，/>表示/>第3行的元素，N(·)为高斯白噪声生成函数，N(0,Q)表示当前采用时刻t采用高斯白噪声生成函数生成的维度为3×1的高斯白噪声向量，其中，0为高斯白噪声生成函数的均值，Q为高斯白噪声生成函数的状态传播过程协方差矩阵，状态传播过程协方差矩阵Q采用随机函数生成，生成后为固定值，其维度为3×3；

f.对状态向量A_t进行无迹变换，得到状态向量A_t的预测值以及协方差矩阵，将状态向量A_t的预测值记为状态向量A_t的协方差矩阵记为/> 采用式(19)表示为：

采用式(20)表示为：

式(20)中，上标T表示矩阵的转置；

g.建立当前采样时刻t的观测模型，将该观测模型记为z_t，z_t采用式(21)表示为：

式(21)中，N(·)为高斯白噪声生成函数，N(0,R)表示采用高斯白噪声生成函数生成的高斯白噪声向量，N(0,R)的维度为3×1，其中，0为高斯白噪声生成函数的均值，R为观测协方差矩阵，观测协方差矩阵R的维度为3×3，观测协方差矩阵R为固定值，取值为：

h.构建当前采样时刻t的观测矩阵H_t，采用观测矩阵H_t对F_i ^t进行观测变换，得到观测变换后的sigma点向量H_t和/>分别采用式(23)和式(14)表示为：

j.对进行无迹变换，得到观测变换后的sigma点的均值/>及协方差矩阵/> 和分别采用式(25)和式(26)表示为：

k.将当前采样时刻t的观测模型的观测残差记为y_t，y_t采用式(27)表示为：

l.将当前采样时刻t的卡尔曼增益记为K_t，采用式(28)计算得到K_t：

m.采用式(29)和(30)分别对状态向量A_t和状态协方差矩阵P进行更新：

n、将(x′_t，y′_t)作为当前采样时刻t车辆最终位置坐标，将θ′_t作为当前时刻t车辆最终航向角，采用当前时刻t车辆最终位置坐标(x′_t，y′_t)和当前采样时刻t车辆最终航向角θ′_t构成当前采样时刻t车辆最终的位置，实现当前采样时刻t车辆的定位；

(4)采用t的当前值加1的值更新t，返回步骤(3)进行下一时刻的车辆定位。