一种基于惯性测量单元的融合定位方法与流程

1.本发明涉及车辆定位技术领域，尤其涉及一种基于惯性测量单元的融合定位方法。


背景技术：

2.目前常用的车辆定位技术主要是gps/gnss定位，利用gps信号/gnss信号的空间交汇测量及环境特征的匹配定位。但是，对于实际场景中，例如室内、地下、建筑物密集区域以及其他复杂环境，gps/gnss定位精度差甚至可能会出现无法正常工作的情况。因此，如何在各种实际场景中对车辆进行高精度定位，成为本领域亟需解决的问题。


技术实现要素：

3.本发明实施例提供一种基于惯性测量单元的融合定位方法，其能实现车辆的高精度定位。
4.本发明一实施例提供一种基于惯性测量单元的融合定位方法，其包括：
5.采集车辆的历史imu数据、历史轮速脉冲及历史定位基准信息，并将历史imu数据及历史轮速脉冲馈入车辆的卡尔曼滤波模型获得历史定位信息；
6.基于所述历史定位信息和历史定位基准信息，利用卷积神经网络训练卡尔曼滤波模型的第一影响参数，以使得卡尔曼滤波模型输出的历史定位信息和历史定位基准信息的残差最小；
7.根据训练得到的第一影响参数，对所述卡尔曼滤波模型中的第二影响参数进行参数调节；
8.根据所述车辆的当前imu数据和当前轮速脉冲，利用参数调节后的卡尔曼滤波模型进行定位预测，得到所述车辆当前的定位位姿。
9.作为上述方案的改进，所述方法还包括：
10.配置所述卷积神经网络的输入为所述历史定位信息和所述历史定位基准信息，输出为imu的历史线性加速度和历史角速度随机噪声、历史轮速脉冲随机噪声。
11.作为上述方案的改进，所述利用卷积神经网络训练卡尔曼滤波模型的第一影响参数，以使得卡尔曼滤波模型输出的历史定位信息和历史定位基准信息的残差最小，包括：
12.获得残差最小时对应的imu的历史线性加速度和历史角速度随机噪声、历史轮速脉冲随机噪声；
13.根据残差最小时对应的imu的历史线性加速度和历史角速度随机噪声、历史轮速脉冲随机噪声以及对应时刻的历史定位信息和历史定位基准信息，进行卡尔曼滤波模型的反向迭代计算，得到所述第一影响参数；
14.其中，所述第一影响参数包括：线性加速度的测量误差的第一系数、角速度的测量误差的第二系数和轮速脉冲的测量误差的第三系数。
15.作为上述方案的改进，所述历史线性加速度的随机噪声为：
[0016][0017]
所述历史角速度的随机噪声为：
[0018][0019]
所述历史轮速脉冲的随机噪声为：
[0020][0021]
其中，a表示历史线性加速度，ω表示历史角速度，vcmd表示历史轮速脉冲，表示历史线性加速度的过程协方差，表示历史角速度的过程协方差，表示历史轮速脉冲的过程协方差，μ表示固定系数，sa表示历史线性加速度的测量误差的第一系数，s
ω
表示历史角速度的测量误差的第二系数，s
vcmd
表示历史轮速脉冲的测量误差的第三系数。
[0022]
作为上述方案的改进，所述根据训练得到的第一影响参数，对所述卡尔曼滤波模型中的第二影响参数进行参数调节，包括：
[0023]
所述根据训练得到的第一影响参数，对所述卡尔曼滤波模型中的第二影响参数进行参数调节，包括：
[0024]
根据所述第一影响参数中的线性加速度的测量误差的第一系数、角速度的测量误差的第二系数，计算所述卡尔曼滤波模型的过程误差；
[0025]
根据所述影响参数中的和轮速脉冲的测量误差的第三系数，计算所述卡尔曼滤波模型的测量误差；
[0026]
根据所述过程误差和所述测量误差，对所述卡尔曼滤波模型进行参数调节。
[0027]
作为上述方案的改进，所述根据所述第一影响参数中的线性加速度的测量误差的第一系数、角速度的测量误差的第二系数，计算所述卡尔曼滤波模型的过程误差，包括：
[0028]
根据所述影响参数中的线性加速度的测量误差的第一系数，计算所述影响参数中线性加速度的随机噪声；
[0029]
根据所述影响参数中的角速度的测量误差的第二系数，计算所述影响参数中角速度的随机噪声；
[0030]
计算所述影响参数的线性加速度的随机噪声和角速度的随机噪声的加和，得到所述卡尔曼滤波模型的过程误差。
[0031]
作为上述方案的改进，所述根据所述影响参数中的轮速脉冲的测量误差的第三系数，计算所述卡尔曼滤波模型的测量误差，包括：
[0032]
根据所述影响参数中的轮速脉冲的测量误差的第三系数，计算所述影响参数中轮速脉冲的随机噪声，作为所述卡尔曼滤波模型的测量误差。
[0033]
作为上述方案的改进，所述根据所述车辆的当前imu数据和当前轮速脉冲，利用参数调节后的卡尔曼滤波模型进行定位预测，得到所述车辆当前的定位位姿，包括：
[0034]
将当前imu数据中作为所述卡尔曼滤波模型的输入，得到所述车辆的预测位姿；
[0035]
根据当前轮速脉冲，利用预设的两轮差速运动模型，计算所述车辆的当前车速；
[0036]
将所述当前车速作为所述卡尔曼滤波模型的测量输入，得到所述车辆的测量位姿；
[0037]
通过所述卡尔曼滤波模型对所述预测位姿和所述测量位姿进行数据融合，得到所
述车辆当前的定位位姿。
[0038]
作为上述方案的改进，所述历史定位基准信息是对所述车辆进行slam定位后得到。
[0039]
作为上述方案的改进，所述卡尔曼滤波模型包括多个依次相连的ekf层；
[0040]
则，所述将历史imu数据及历史轮速脉冲馈入车辆的卡尔曼滤波模型获得历史定位信息，包括：
[0041]
第i个ekf层融合k+j时刻的历史imu数据、历史轮速脉冲以及上一个ekf层输出的历史定位信息，得到k+j时刻的历史定位信息；
[0042]
其中，第1个ekf层以k-1时刻的历史定位基准信息作为其上一个所述ekf层输出的历史定位信息；i＝1，2，...，n+1；j＝0，1，2，...，n；k表示所述卡尔曼滤波模型馈入数据的初始时刻。
[0043]
相对于现有技术，本发明实施例的有益效果在于：通过采集车辆的历史imu数据、历史轮速脉冲及历史定位基准信息，将历史imu数据及历史轮速脉冲馈入车辆的卡尔曼滤波模型获得历史定位信息；基于所述历史定位信息和历史定位基准信息，利用卷积神经网络训练卡尔曼滤波模型的第一影响参数，以使得卡尔曼滤波模型输出的历史定位信息和历史定位基准信息的残差最小；根据训练得到的第一影响参数，对所述卡尔曼滤波模型中的第二影响参数进行参数调节；根据所述车辆的当前imu数据和当前轮速脉冲，利用参数调节后的卡尔曼滤波模型进行定位预测，得到所述车辆当前的定位位姿；本发明实施例基于采集到的训练集，通过卷积神经网络cnn，得到影响参数，然后在应用阶段，将训练得到的影响参数导入到卡尔曼滤波模型中，并结合车辆的当前imu数据和当前轮速脉冲进行融合定位，对于gnss信号或gps信号较差的区域，仍然可以实现车辆的高精度定位。
附图说明
[0044]
为了更清楚地说明本发明的技术方案，下面将对实施方式中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0045]
图1是本发明实施例提供的一种基于惯性测量单元的融合定位方法的流程图；
[0046]
图2是本发明实施例提供的卡尔曼滤波过程的流程示意图；
[0047]
图3是本发明实施例提供的融合定位流程示意图。
具体实施方式
[0048]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0049]
请参见图1，是本发明实施例提供的一种基于惯性测量单元的融合定位方法的流程示意图，所述基于惯性测量单元的融合定位方法具体包括：
[0050]
s11：采集车辆的历史imu数据、历史轮速脉冲及历史定位基准信息，并将历史imu数据及历史轮速脉冲馈入车辆的卡尔曼滤波模型获得历史定位信息；
[0051]
其中，所述历史定位基准信息可以是对所述车辆进行slam(simultaneous localization and mapping，同步定位与建图)定位后得到，也可是通过gnss或gps定位得到。
[0052]
s12：基于所述历史定位信息和历史定位基准信息，利用卷积神经网络训练卡尔曼滤波模型的第一影响参数，以使得卡尔曼滤波模型输出的历史定位信息和历史定位基准信息的残差最小；
[0053]
其中，配置所述卷积神经网络的输入为所述历史定位信息和所述历史定位基准信息，输出为imu的历史线性加速度和历史角速度随机噪声、历史轮速脉冲随机噪声。
[0054]
进一步，所述利用卷积神经网络训练卡尔曼滤波模型的第一影响参数，以使得卡尔曼滤波模型输出的历史定位信息和历史定位基准信息的残差最小，包括：
[0055]
获得残差最小时对应的imu的历史线性加速度和历史角速度随机噪声、历史轮速脉冲随机噪声；
[0056]
根据残差最小时对应的imu的历史线性加速度和历史角速度随机噪声、历史轮速脉冲随机噪声以及对应时刻的历史定位信息和历史定位基准信息，进行卡尔曼滤波模型的反向迭代计算，得到所述第一影响参数；
[0057]
其中，所述第一影响参数包括：线性加速度的测量误差的第一系数、角速度的测量误差的第二系数和轮速脉冲的测量误差的第三系数。
[0058]
s13：根据训练得到的第一影响参数，对所述卡尔曼滤波模型中的第二影响参数进行参数调节；
[0059]
s14：根据所述车辆的当前imu数据和当前轮速脉冲，利用参数调节后的卡尔曼滤波模型进行定位预测，得到所述车辆当前的定位位姿。
[0060]
在发明实施例基于采集到的历史imu数据、历史轮速脉冲，通过卷积神经网络cnn，得到卡尔曼滤波模型相关的第一影响参数，然后在应用阶段，将训练得到的第一影响参数导入到卡尔曼滤波模型的第二影响参数中，并结合车辆的当前imu数据和当前轮速脉冲进行融合定位，对于gnss信号或gps信号较差的区域，仍然可以实现车辆的高精度定位；同时通过将足量的训练集导入到cnn，可以得到更长时间的高精度的轮速递推结果，避免摄像头和激光雷达短暂失效场景下基于惯性测量单元(imu)与轮速结合的融合定位持续较短时间问题。
[0061]
在本发明实施例中，在数据收集阶段收集历史imu数据和历史轮速脉冲是为了计算基于卡尔曼滤波模型的定位数据(例如计算预测位姿、测量位姿)，而在实际训练过程中imu、轮速脉冲传感器的数据不会进入卷积神经网络进行训练，而是通过构建的历史线性加速度和历史角速度的随机噪声以及历史轮速脉冲的随机噪声馈入到卷积神经网络。
[0062]
进一步，所述历史线性加速度的随机噪声为：
[0063][0064]
所述历史角速度的随机噪声为：
[0065][0066]
所述历史轮速脉冲的随机噪声为：
[0067]
[0068]
其中，a表示历史线性加速度，ω表示历史角速度，vcmd表示历史轮速脉冲，表示历史线性加速度的过程协方差(作为一个单位矩阵)，表示历史角速度的过程协方差(作为一个单位矩阵)，表示历史轮速脉冲的过程协方差(作为一个单位矩阵)，μ表示固定系数，sa表示历史线性加速度的测量误差的第一系数，s
ω
表示历史角速度的测量误差的第二系数，s
vcmd
表示历史轮速脉冲的测量误差的第三系数。
[0069]
在本发明实施例中，μ＝4；由于tanh()的值域为(-1，1)，通过将μ设定为4，可以将相应指标(例如历史线性加速度、历史角速度、历史轮速脉冲)的协方差的值限制在(10-4
，104)。
[0070]
在一种可选的实施例中，所述根据训练得到的第一影响参数，对所述卡尔曼滤波模型中的第二影响参数进行参数调节，包括：
[0071]
所述根据训练得到的第一影响参数，对所述卡尔曼滤波模型中的第二影响参数进行参数调节，包括：
[0072]
根据所述第一影响参数中的线性加速度的测量误差的第一系数、角速度的测量误差的第二系数，计算所述卡尔曼滤波模型的过程误差；
[0073]
根据所述影响参数中的和轮速脉冲的测量误差的第三系数，计算所述卡尔曼滤波模型的测量误差；
[0074]
根据所述过程误差和所述测量误差，对所述卡尔曼滤波模型进行参数调节。
[0075]
在一种可选的实施例中，所述根据所述第一影响参数中的线性加速度的测量误差的第一系数、角速度的测量误差的第二系数，计算所述卡尔曼滤波模型的过程误差，包括：
[0076]
根据所述影响参数中的线性加速度的测量误差的第一系数，计算所述影响参数中线性加速度的随机噪声；
[0077]
根据所述影响参数中的角速度的测量误差的第二系数，计算所述影响参数中角速度的随机噪声；
[0078]
计算所述影响参数的线性加速度的随机噪声和角速度的随机噪声的加和，得到所述卡尔曼滤波模型的过程误差。
[0079]
在一种可选的实施例中，所述根据所述影响参数中的轮速脉冲的测量误差的第三系数，计算所述卡尔曼滤波模型的测量误差，包括：
[0080]
根据所述影响参数中的轮速脉冲的测量误差的第三系数，计算所述影响参数中轮速脉冲的随机噪声，作为所述卡尔曼滤波模型的测量误差。
[0081]
本发明实施例所述的融合定位方法分为两个阶段，第一个阶段就是步骤s11-s12相关的训练阶段以及步骤s13-s14相关的应用阶段。
[0082]
对于训练阶段，为了提高训练集的数据量，可通过采集同一型号的配置有高精度的六轴imu和轮速传感器的车辆在一连续时间段内的历史定位基准信息、历史imu数据、历史轮速脉冲以及对应车辆的卡尔曼滤波模型输出的历史定位信息，构建训练集；具体地，根据上述公式(1)计算同一时刻的imu的线性加速度的随机噪声，根据上述公式(2)计算同一时刻的imu的角速度的随机噪声，根据上述公式(3)计算同一时刻的轮速脉冲的随机噪声r；然后根据同一时刻的imu的线性加速度和角速度的随机噪声，得到同一时刻的imu随机噪声q。将各个时刻的imu随机噪声q和轮速脉冲的随机噪声r作为卷积神经网络cnn的输出，对应
时刻的定位基准信息和卡尔曼滤波模型输出的定位信息作为卷积神经网络cnn的输入进行训练，其中，获取卷积神经网络cnn的损失函数最小值对应的imu随机噪声q和轮速脉冲的随机噪声r中的sa、s
ω
、s
vcmd
作为后续应用阶段的卡尔曼滤波模型的相关参数，然后基于损失函数最小值对应的imu随机噪声q和轮速脉冲的随机噪声r中的sa、s
ω
、s
vcmd
，计算出卡尔曼滤波模型的过程误差(实际上是线性加速度的随机噪声和角速度的随机噪声的加和q)和测量误差(实际上是轮速脉冲的随机噪声r)，并将该过程误差和测量误差导入到卡尔曼滤波模型中，方便后续卡尔曼滤波模型的应用。
[0083]
卷积神经网络cnn训练的目的是为了求解出损失函数最小情况下对应的imu随机噪声q和轮速脉冲的随机噪声r的具体数值，使得和达到收敛，其中，x表示卡尔曼滤波模型输出的定位位姿，此时，结合卡尔曼滤波模型进行反向迭代计算，得到在通过微积分的链式法则求出和从而得到imu随机噪声q和轮速脉冲的随机噪声r的具体数值。其中，p表示卡尔曼滤波模型的协方差、k表示卡尔曼增益、s表示卡尔曼滤波模型的协方差的预测；需要说明的是卡尔曼滤波算法属于现有技术，在这里不再对卡尔曼滤波相关的算法进行展开说明。
[0084]
作为上述方案的改进，所述卡尔曼滤波模型包括多个依次相连的ekf层；
[0085]
则，所述将历史imu数据及历史轮速脉冲馈入车辆的卡尔曼滤波模型获得历史定位信息，包括：
[0086]
第i个ekf层融合k+j时刻的历史imu数据、历史轮速脉冲以及上一个ekf层输出的历史定位信息，得到k+j时刻的历史定位信息；
[0087]
其中，第1个ekf层以k-1时刻的历史定位基准信息作为其上一个所述ekf层输出的历史定位信息；i＝1，2，...，n+1；j＝0，1，2，...，n；k表示所述卡尔曼滤波模型馈入数据的初始时刻。
[0088]
为了更清楚说明卡尔曼滤波模型进行反向迭代过程，下面结合图2所示对卡尔曼滤波模型进行正向迭代过程进行说明：
[0089]
第一步：初始k时刻以定位测量基准输出的k-1时刻的历史定位基准信息作为卡尔曼滤波模型的初始值；然后结合k时刻的历史imu数据、历史轮速脉冲进行卡尔曼滤波的计算，得到k时刻的历史定位信息；
[0090]
第二步：以k时刻的历史定位信息、k+1时刻的历史imu数据、历史轮速脉冲进行卡尔曼滤波的计算，得到k+1时刻的历史定位信息，依次类推，迭代计算n次后，可以得到k+n时刻的历史定位信息。以n＝50为例，即一次正向迭代过程包括50次卡尔曼滤波计算，然后基于该正向迭代过程后卡尔曼滤波模型输出的定位信息和对应时刻定位测量基准输出的定位基准信息，计算残差大小，例如残差其中，mse表示均方误差函数，x
k+1
表示卡尔曼滤波模型预测的k+1时刻的定位信息，表示定位测量基准输出的k+1时刻的定位基准信息。之后根据该残差，利用链式法则进行卡尔曼反向迭代计算，求出最终的影响参数q和r，其中图2所示的uk表示k时刻的历史imu数据，zk表示k时刻的历史
轮速脉冲。
[0091]
在一种可选的实施例中，所述根据所述车辆的当前imu数据和当前轮速脉冲，利用参数调节后的卡尔曼滤波模型进行定位预测，得到所述车辆当前的定位位姿，包括：
[0092]
将当前imu数据中作为所述卡尔曼滤波模型的输入，得到所述车辆的预测位姿；
[0093]
根据当前轮速脉冲，利用预设的两轮差速运动模型，计算所述车辆的当前车速；
[0094]
将所述当前车速作为所述卡尔曼滤波模型的测量输入，得到所述车辆的测量位姿；
[0095]
通过所述卡尔曼滤波模型对所述预测位姿和所述测量位姿进行数据融合，得到所述车辆当前的定位位姿。
[0096]
在卡尔曼滤波模型的应用阶段，通过基于当前imu数据，即包括x，y，z轴的线性加速度和角速度，通过卡尔曼滤波模型的预测量更新方程得到所述车辆的预测位姿；然后为了减少预测误差，当前轮速脉冲作为矫正量，具体的基于当前轮速脉冲，利用两轮差速运动模型计算出所述车辆的当前车速，将计算出来的当前车速作为量测信息，再通过卡尔曼滤波模型的测量更新方程融合预测位姿和量测信息以得到更高精度的定位位姿。其中，卡尔曼滤波相关的预测更新方程和测量更新方程属于现有技术，在这里不再展开说明。
[0097]
如图3所示，本发明实施例基于采集到的训练集，通过卷积神经网络cnn，得到影响参数，然后在应用阶段，将训练得到的影响参数导入到卡尔曼滤波模型中，并结合车辆的当前imu数据和当前轮速脉冲进行融合定位，对于gnss信号或gps信号较差的区域，仍然可以实现车辆的高精度定位。
[0098]
以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出多台改进和润饰，这些改进和润饰也视为本发明的保护范围。