多传感器融合的车辆状态参数预估方法与流程

1.本发明涉及汽车数据采集技术领域，尤其涉及一种多传感器融合的车辆状态参数预估方法。


背景技术：

2.随着智能网联、人工智能等技术取得长足进步，汽车行业开始沿智能化和电动化趋势不断发展，自动驾驶、线控底盘、智能网联和高级驾驶辅助技术成为当下的汽车热门技术。以上技术为实现其预期的目标功能，均需要获取高精度的车辆状态参数作为基础，但是现有技术中出现的方法一般计算复杂，成本较高。


技术实现要素：

3.本发明所要解决的技术问题是如何提供一种融合传感器信息多、参数预估精度高、计算成本低的多传感器融合的车辆状态参数预估方法。
4.为解决上述技术问题，本发明所采取的技术方案是：一种多传感器融合的车辆状态参数预估方法，其特征在于包括如下步骤：
5.简化车辆为在水平面运动的刚体模型，简化大地坐标系为xoy的平面坐标系；
6.车载工控机采集传感器信号；
7.设计车辆位姿与速度预估卡尔曼滤波器；
8.使用所设计的卡尔曼滤波器，预估车辆位姿和速度信息；
9.由输出的车辆位姿和速度信号，求解tk时刻车身坐标系下车辆纵向行驶速度,侧向滑移速度和横摆角速度；
10.设计车辆侧向滑移速度预估卡尔曼滤波器；
11.使用所设计的车辆侧向滑移速度预估卡尔曼滤波器，预估车辆侧向滑移速度。
12.采用上述技术方案所产生的有益效果在于：相比于采取gps、imu、轮速传感器融合的车辆状态参数预估方法，本方法采取了更多传感器融合和基于动力学模型的方式，实现了传感器数据采集时的冗余设计，减轻了单一传感器因故障导致输出错误数据对车辆状态参数预估精度的影响。
13.相比于基于多自由度非线性车辆动力学模型的车辆状态参数预估方法，本方法采用二自由度线性车辆动力学模型预估车辆侧向滑移速度，计算成本较低，便于向实车部署，同时融合多类型传感器数据，可保证高精度的车辆状态参数预估。
附图说明
14.下面结合附图和具体实施方式对本发明作进一步详细的说明。
15.图1是本发明实施例中在xoy坐标系下车辆刚体模型图；
16.图2是本发明本发明实施例中车辆二自由度动力学模型图；
17.图3本发明本发明实施例所述方法的流程图；
具体实施方式
18.下面结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
19.在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是本发明还可以采用其他不同于在此描述的其它方式来实施，本领域技术人员可以在不违背本发明内涵的情况下做类似推广，因此本发明不受下面公开的具体实施例的限制。
20.如图3所示，本发明实施例公开了一种多传感器融合的车辆状态参数预估方法，所述方法包括如下步骤：
21.步骤1：简化车辆为在水平面运动的刚体模型，简化大地坐标系为xoy的平面坐标系。
22.步骤2：车载工控机采集传感器信号。
23.车载工控机通过激光雷达采集环境点云数据，运行特征提取和位姿匹配算法，获取得到由激光雷达测定的车辆质心在xoy坐标系下的x轴坐标x
lidar
,y轴坐标y
lidar
和车辆纵向行驶方向与大地坐标系x轴间夹角，即航向角
24.车载工控机通过与车载rtk(real-time kinematic)实时差分定位传感器通讯，获取得到由rtk传感器测定的车辆质心在xoy坐标系下的x轴坐标x
sensor
,y轴坐标y
sensor
和航向角
25.车载工控机通过摄像头采集图像数据，运行lucas-kanade光流算法，获取得到车辆质心在xoy坐标系下的x轴方向速度y轴坐标方向速度
26.车载工控机通过与imu传感器通讯，获取得到车辆在xoy坐标系下的横摆角速度和车身坐标系下的车辆侧向加速度
27.车载工控机通过与方向盘转角传感器通讯，获取得到车辆的前轮转角δ。
28.步骤3：设计车辆位姿与速度预估卡尔曼滤波器。
29.1)由简化后车辆模型在xoy坐标系中运动学关系，构建系统状态预测方程。
30.在xoy坐标系下，简化后的车辆模型具有三个自由度，即沿x轴方向直线运动、沿y轴方向直线运动和绕过质心垂直于xy平面的轴旋转运动。以车辆沿x轴方向运动为例，设x(k 1)，x(k)分别为t
k 1
，tk时刻车辆在坐标系xoy中的x轴坐标值，系统的采样周期为t，则有：
[0031][0032]
式中，为车辆在xoy坐标系下沿x轴的速度，由运动学关系则有：
[0033][0034]
式中，为由t
k-1
时刻至tkx轴方向的速度变化量，将式(2)代入式(1)并整理得：
[0035][0036]
设定式(1)权重值为q,q∈(0,1),式(2)权重值为(1-q)，由式(1)
×
q 式(2)
×
(1-q)可得:
[0037][0038]
设分别为t
k 1
，tk时刻在坐标系xoy中车辆沿x轴方向的速度，则有：
[0039][0040]
式中，为车辆在xoy坐标系下车辆沿x轴的加速度，由运动学关系则有：
[0041][0042]
式中，为由t
k-1
时刻至tk车辆沿x轴方向的加速度变化量，将式(6)代入式(5)并整理得：
[0043][0044]
设定y(k 1)，y(k)分别为t
k 1
，tk时刻车辆在xoy坐标系下的y轴坐标值，分别为t
k 1
，tk时刻车辆在xoy坐标系下的沿y轴的速度，同理满足：
[0045][0046][0047]
式中分别为由t
k-1
时刻至tk车辆沿y轴方向的速度变化量和加速度变化量。
[0048]
设定分别为t
k 1
，tk时刻在xoy坐标系下车辆纵向行驶方向与x轴间夹角，即航向角值，分别为t
k 1
，tk时刻车辆的横摆角速度，同理满足：
[0049][0050][0051]
式中分别为由t
k-1
时刻至tk车辆横摆角速度变化量和横摆角加速度变化量。
[0052]
取状态向量为x1(k),
[0053]
设则
[0054]
设系统状态转移方程为：
[0055]
x1(k 1)＝φ1(k)x1(k) w1(k)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(12)
[0056]
式中φ1(k)为状态预测矩阵，
[0057]
式中w1(k)为预测噪声，
[0058]
2)组合步骤2采集到的传感器信号，构建系统状态观测方程。
[0059]
设x
lidar
(k)，y
lidar
(k)，分别为tk时刻由激光雷达测定的x轴坐标，y轴坐标和航向角，则有：
[0060][0061]
式中，v
xlidar
(k)，v
ylidar
(k)，分别为tk时刻由激光雷达测定的x轴坐标，y轴坐标和航向角信号的噪声。
[0062]
取观测向量为y1(k)，
[0063]
设观测方程为：
[0064]
y1(k)＝h1(k)x1(k) v1(k)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(14)
[0065]
式中，h1为观测矩阵，
[0066]
v1(k)为观测噪声矩阵，
[0067]
3)设定系统预测噪声方差矩阵为非负正定矩阵q1，设定系统观测噪声方差矩阵为正定矩阵r1。
[0068]
步骤4：使用步骤3所设计的卡尔曼滤波器，预估车辆位姿和速度信息。
[0069]
1)给定预估计的系统状态初值和误差方差矩阵初值p1(0)。
[0070]
2)由t
k-1
时刻误差方差矩阵预估值p1(k-1)，求解tk时刻的误差方差矩阵预测值p1(k,k-1)。
[0071][0072]
3)求解tk时刻的滤波增益矩阵k1(k)。
[0073][0074]
4)求解tk时刻的误差方差矩阵预估值p1(k)。
[0075]
p1(k)＝[i-k1(k)h1(k)]p1(k,k-1)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(17)
[0076]
5)由t
k-1
时刻的系统状态预测值预估值求解tk时刻的系统状态预测值
[0077][0078]
6)求解tk时刻的系统状态预测值预估值
[0079][0080]
7)提取tk时刻预估的系统状态中的元素，输出tk时刻预估的车辆位姿和速度
[0081]
步骤5：由步骤4输出的车辆位姿和速度信号，求解tk时刻车身坐标系下车辆纵向行驶速度v
x1
(k),侧向滑移速度v
y1
(k)和横摆角速度ω1(k)。
[0082][0083][0084][0085]
步骤6：设计车辆侧向滑移速度预估卡尔曼滤波器。
[0086]
1)简化车辆为二自由度动力学模型，构建状态预测方程。
[0087]
假定车辆的左右前轮转向角相同，且车辆的转向角、轮胎侧偏角和质心侧偏角都很小，忽略车辆的俯仰、横摆运动和载荷转移，将车辆简化为经典的车辆二自由度动力学模型。以汽车质心o为坐标原点，汽车纵向运动方向为x轴，车辆侧向运动方向为y轴，垂直地面方向为z轴建立坐标系。
[0088]
对车辆受力分析，可得：
[0089][0090]
式中∑fy为沿y轴方向合力，∑mz为绕z轴的合力矩，f
yf
、f
yr
分别为前、后轮胎的侧向合力，lf、lr分别为前、后轴与车辆质心间距离，δ为前轮转角。
[0091]
设沿x、y方向车辆质心处的加速度为a
x
、ay，可得：
[0092][0093]
式中，v
x
为车辆纵向车速,为车辆纵向车速变化率，vy为车辆横向滑移速度,为车辆横向滑移速度变化率，ω为横摆角速度。
[0094]
前轮转角δ一般比较小，故取cosδ＝1，由牛顿第二定律可得：
[0095][0096]
式中，iz为车辆绕z轴的转动惯量，为车辆绕z轴旋转的角加速度。
[0097]
轮胎侧向力fy与轮胎侧偏角α的关系为：
[0098]fy
＝-cα
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(26)
[0099]
前轮侧偏角αf可表示为：
[0100][0101]
后轮侧偏角αr可表示为：
[0102][0103]
车辆二自由度动力学模型的方程为：
[0104][0105]
将上式整理为标准的状态空间方程:
[0106][0107]
设v
x
(k)为tk时刻车辆的纵向行驶速度，vy(k 1)，vy(k)分别为t
k 1
，tk时刻车辆的侧向滑移速度，ω(k 1)，ω(k)分别为t
k 1
，tk时刻车辆的横摆角速度，系统的采样周期为t，则有：
[0108][0109]
取状态向量为x2(k),设则设系统状态转移方程为：
[0110]
x2(k 1)＝φ2(k)x2(k) γ2(k)δ(k) w2(k)
ꢀꢀꢀ
(32)
[0111]
式中φ2(k)为状态预测矩阵，(k)为状态预测矩阵，δ(k)为tk时刻车辆前轮转角，是系统的输入量，w2(k)为状态预测噪声矩阵。
[0112]
2)组合步骤2采集到的传感器信号和步骤5求解得到的车辆运动参数，构建状态转移方程。
[0113]
由公式和联立求解可得：
[0114][0115]
设为tk时刻由imu传感器测定的车辆侧向加速度，取观测向量为y2(k)，设观测方程为：
[0116]
y2(k)＝h2(k)x2(k) d(k)δ(k) v2(k)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(34)
[0117]
式中v2(k)为观测噪声矩阵。
[0118]
3)设定系统预测噪声方差矩阵为非负正定矩阵q2，设定系统观测噪声方差矩阵为正定矩阵r2。
[0119]
步骤7：使用步骤6所设计的卡尔曼滤波器，预估车辆侧向滑移速度。
[0120]
1)给定预估计的系统状态初值和误差方差矩阵初值p2(0)。
[0121]
2)获取步骤5计算求得的tk时刻车辆纵向行驶车速v
x1
(k)，令v
x
(k)＝v
x1
(k)更新φ2(k)，h2(k)，获取并更新tk时刻前轮转角δ(k)。
[0122]
3)由t
k-1
时刻误差方差矩阵预估值p2(k-1)，求解tk时刻的误差方差矩阵预测值p2(k,k-1)。
[0123][0124]
4)求解tk时刻的滤波增益矩阵k2(k)。
[0125][0126]
5)求解tk时刻的误差方差矩阵预估值p2(k)。
[0127]
p2(k)＝[i-k2(k)h2(k)]p2(k,k-1)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(37)
[0128]
6)由t
k-1
时刻的系统状态预测值预估值求解tk时刻的系统状态预测
值
[0129][0130]
7)求解tk时刻的系统状态预测值预估值
[0131][0132]
8)提取tk时刻预估的系统状态中的元素，输出tk时刻预估的车辆侧向滑移速度和二次滤波后得到的车辆横摆角速度