一种基于多源信息融合的车辆质心侧偏角估计系统及方法与流程

本发明涉及一种车辆质心侧偏角估计系统及方法，尤其是涉及一种基于多源信息融合的车辆质心侧偏角估计系统及方法。

背景技术：

质心侧偏角是智能汽车运动跟踪控制的关键状态标量。质心侧偏角估计受到路面和轮胎非线性等不确定因素的影响，是车辆最难估计的状态变量之一，因此如何估计智能汽车质心侧偏角成为近年来研究重点、难点。

目前国内外质心侧偏角估计方法主要有：1、基于运动学模型的方法，鲁棒性好，估计结果几乎不受模型参数的影响，但是对传感器的信息精度要求较高。2、基于动力模型的观测器，对传感器的要求相对较低，但对模型参数的敏感度较高。随着汽车“智能化”的飞速发展，摄像头、雷达、gps和imu(惯性传感器)等传感器的装载为车辆状态信息的估计提供了更多的信息源。gps可以提供关于车辆位置和速度等信息，误差范围可控，摄像头可以获取车辆与车道线之间的位置信息，imu可获取横摆角速度、车辆纵向加速度以及车辆侧向加速度。

技术实现要素：

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于多源信息融合的车辆质心侧偏角估计系统及方法。

本发明的目的可以通过以下技术方案来实现：

一种基于多源信息融合的车辆质心侧偏角估计系统，该系统包括：多源信息采集单元、车辆横摆角观测器和车辆质心侧偏角观测器；

所述的多源信息采集单元分别连接车辆横摆角观测器和车辆质心侧偏角观测器，所述的车辆横摆角观测器和车辆质心侧偏角观测器互联；

车辆横摆角观测器得到车辆横摆角和横摆角速度偏差并作为车辆质心侧偏角观测器的信息输入，车辆质心侧偏角观测器得到车辆质心侧偏角并作为车辆横摆角观测器的输入信息，车辆横摆角观测器和车辆质心侧偏角观测器互联完成车辆质心侧偏角估计。

所述的多源信息采集单元包括：

gps：用于提供车辆航向角以及获取车辆在东北天方向的纵侧向速度；

惯性传感器：用于获取车辆纵向加速度、侧向加速度和横摆角速度；

摄像头：用于获取车辆与车道线的横向距离。

所述的gps安装在车辆质心正上方，所述的惯性传感器安装在车辆质心处。

所述的车辆横摆角观测器的状态方程为：

其中，ψ为待估计的车辆横摆角，rbias为待估计的横摆角速度偏差，为待估计的车辆横摆角的导数，为待估计的横摆角速度偏差的导数，rm为惯性传感器获取的横摆角速度测量值，为gps提供的车辆航向角，β为多源信息采集单元估计的车辆质心侧偏角。

待估计的车辆横摆角ψ以及待估计的横摆角速度偏差rbias通过卡尔曼滤波方法估计得到。

车辆质心侧偏角观测器为：

表示的导数，表示y向量的估计值，表示车辆与车道线的横向距离的估计值，表示纵向车速估计值，表示侧向车速估计值，yf为摄像头获取的车辆与车道线的横向距离，和分别为gps获取的车辆在东北天方向的纵向和侧向速度，k为反馈增益矩阵，ψ为车辆横摆角观测器估计得到的车辆横摆角，rm为惯性传感器获取的横摆角速度测量值，rbias为车辆横摆角观测器估计得到的横摆角速度偏差，lcf为摄像头预瞄点到车辆质心的距离，axm和aym分别为惯性传感器获取的车辆纵向加速度和侧向加速度，bx和by分别表示惯性传感器获取的纵向加速度和侧向加速度的测量偏差，β为待估计的车辆质心侧偏角。

一种基于多源信息融合的车辆质心侧偏角估计方法，该方法包括如下步骤：

(1)采集多源信息，包括车辆航向角、车辆在东北天方向的纵侧向速度、车辆纵向加速度、侧向加速度、横摆角速度和车辆与车道线的横向距离；

(2)建立车辆横摆角观测器和车辆质心侧偏角观测器，将车辆航向角以及横摆角速度作为车辆横摆角观测器的输入信息，将车辆在东北天方向的纵侧向速度、车辆纵向加速度、侧向加速度以及车辆与车道线的横向距离作为辆质心侧偏角观测器的输入信息，同时将车辆横摆角观测器和车辆质心侧偏角观测器进行互联；

(3)车辆横摆角观测器估计得到车辆横摆角和横摆角速度偏差并作为车辆质心侧偏角观测器的输入信息，车辆质心侧偏角观测器估计得到车辆质心侧偏角作为车辆横摆角观测器的输入信息，进而完成车辆质心侧偏角估计。

步骤(1)中车辆航向角以及车辆在东北天方向的纵侧向速度通过安装在车辆质心正上方的gps获取，车辆纵向加速度、侧向加速度以及横摆角速度通过安装在车辆质心处的惯性传感器获取，车辆与车道线的横向距离通过安装在车辆上的摄像头获取。

步骤(2)中车辆横摆角观测器的状态方程为：

其中，ψ为待估计的车辆横摆角，rbias为待估计的横摆角速度偏差，为待估计的车辆横摆角的导数，为待估计的横摆角速度偏差的导数，rm为惯性传感器获取的横摆角速度测量值，为gps提供的车辆航向角，β为多源信息采集单元估计的车辆质心侧偏角；

进而，通过卡尔曼滤波方法估计得到待估计的车辆横摆角ψ以及待估计的横摆角速度偏差rbias。

步骤(2)中车辆质心侧偏角观测器为：

表示的导数，表示y向量的估计值，表示车辆与车道线的横向距离的估计值，表示纵向车速估计值，表示侧向车速估计值，yf为摄像头获取的车辆与车道线的横向距离，和分别为gps获取的车辆在东北天方向的纵向和侧向速度，k为反馈增益矩阵，ψ为车辆横摆角观测器估计得到的车辆横摆角，rm为惯性传感器获取的横摆角速度测量值，rbias为车辆横摆角观测器估计得到的横摆角速度偏差，lcf为摄像头预瞄点到车辆质心的距离，axm和aym分别为惯性传感器获取的车辆纵向加速度和侧向加速度，bx和by分别表示惯性传感器获取的纵向加速度和侧向加速度的测量偏差，β为待估计的车辆质心侧偏角。

与现有技术相比，本发明具有如下优点：

(1)本发明利用摄像头、gps和惯性传感器采集的多种信息进行融合，同时将车辆横摆角观测器和车辆质心侧偏角观测器互联完成车辆质心侧偏角的观测，不依赖车辆动力学模型，有效地提高了估计算法在实车使用过程中的鲁棒性和可靠性；

(2)本发明车辆质心侧偏角估计方法估计精度高、计算量小、可广泛应用。

附图说明

图1为本发明车辆质心侧偏角估计结构框图；

图2为卡尔曼滤波算法的计算流程框图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。

实施例

如图1所示，一种基于多源信息融合的车辆质心侧偏角估计系统，该系统包括：多源信息采集单元、车辆横摆角观测器和车辆质心侧偏角观测器；

多源信息采集单元分别连接车辆横摆角观测器和车辆质心侧偏角观测器，车辆横摆角观测器和车辆质心侧偏角观测器互联；

车辆横摆角观测器得到车辆横摆角和横摆角速度偏差并作为车辆质心侧偏角观测器的信息输入，车辆质心侧偏角观测器得到车辆质心侧偏角并作为车辆横摆角观测器的输入信息，车辆横摆角观测器和车辆质心侧偏角观测器互联完成车辆质心侧偏角估计。

多源信息采集单元包括：gps：用于提供车辆航向角以及获取车辆在东北天方向的纵侧向速度；惯性传感器(imu)：用于获取车辆纵向加速度、侧向加速度和横摆角速度；摄像头：用于获取车辆与车道线的横向距离。其中gps安装在车辆质心正上方，惯性传感器安装在车辆质心处。

上述车辆横摆角观测器和车辆质心侧偏角观测器的构建如下：

1)车辆横摆角观测器

为了避免直接采用横摆角速度传感器积分带来的测量偏差，采用gps信号估计横摆角速度偏差值。车辆横摆角观测器的状态方程为：

其中，ψ为待估计的车辆横摆角，rbias为待估计的横摆角速度偏差，为待估计的车辆横摆角的导数，为待估计的横摆角速度偏差的导数，rm为惯性传感器获取的横摆角速度测量值，为gps提供的车辆航向角，β为多源信息采集单元估计的车辆质心侧偏角。

将上述状态方程行离散化处理：

xk+1＝φk+1,k·xk+h·uk+wk，

yk＝c·xk+vk，

式中，和c＝[10]为离散系统的系统矩阵和输入矩阵，其中状态变量wk,vk分别为系统噪声和测量噪声，互不相关且均值为0，即：

式中，e代表数学期望，q和r为系统噪声和测量噪声的协方差。

卡尔曼滤波器的递推方程可以表示如下：

pk+1|k+1＝(i-kk+1c)pk+1|k，

滤波器初值：表示k时刻状态变量x的先验值，表示表示k+1时刻状态变量x的预测值，表示k+1时刻状态变量x的修正值，pk为表示k+1时刻误差协方差矩阵的先验值，pk+1|k表示k+1时刻误差协方差矩阵的预测值，pk+1|k+1表示k+1时刻误差协方差矩阵的修正值，kk+1为k+1时刻卡尔曼反馈增益，i为单位阵。和c＝[10]。卡尔曼滤波算法的计算流程框图如图2所示。

2)车辆质心侧偏角观测器

假定惯性传感器imu安装在车辆质心处，用于测量车辆纵向加速度axm、侧向加速度aym和横摆角速度rm，车辆纵向加速度axm和侧向加速度aym可以表示为：

上式中bx和by分别表示惯性传感器获取的纵向加速度和侧向加速度的测量偏差，vx为纵向车速，vy为侧向车速，为纵向车速导数，为侧向车速导数，rm为惯性传感器获取的横摆角速度测量值，rbias为车辆横摆角观测器估计得到的横摆角速度偏差。

设定车辆与车道线的横向距离为yf，它满足如下关系式：

yf＝ycog+lcfsinψ，

ycog表示车辆质心到车道线的距离，lcf表示摄像头预瞄点到车辆质心的距离ycog可表示为：

由此可以得到：

上式中表示车辆与车道线的横向距离的导数，lcf表示摄像头预瞄点到车辆质心的距离。

假定不考虑车辆的侧倾和横摆运动，gps位于车辆质心正上方用于测量车辆在东北天方向的速度和

从而可以构造如下的状态空间形式：

上式中令：x＝[yfvxvy]^t。

因此，构建车辆质心侧偏角观测器如下：

表示的导数，表示y向量的估计值，表示车辆与车道线的横向距离的估计值，表示纵向车速估计值，表示侧向车速估计值，yf为摄像头获取的车辆与车道线的横向距离，和分别为gps获取的车辆在东北天方向的纵向和侧向速度，k为反馈增益矩阵，ψ为车辆横摆角观测器估计得到的车辆横摆角，为车辆横摆角速度估计值，rm为惯性传感器获取的横摆角速度测量值，rbias为车辆横摆角观测器估计得到的横摆角速度偏差，lcf为摄像头预瞄点到车辆质心的距离，axm和aym分别为惯性传感器获取的车辆纵向加速度和侧向加速度，bx和by分别表示惯性传感器获取的纵向加速度和侧向加速度的测量偏差，β为待估计的车辆质心侧偏角。

图1中，为车辆横摆角观测器估计得到的车辆横摆角估计值，为车辆横摆角观测器估计得到的横摆角速度偏差估计值，为车辆横摆角速度估计值，进而将和作为输入信息输入至车辆质心侧偏角观测器，车辆质心侧偏角观测器中的ψ为车辆横摆角观测器估计得到的车辆横摆角，为车辆横摆角速度估计值，即车辆质心侧偏角观测器中的ψ用代替，车辆横摆角速度估计值用代替。

一种基于多源信息融合的车辆质心侧偏角估计方法，该方法包括如下步骤：

(1)采集多源信息，包括车辆航向角、车辆在东北天方向的纵侧向速度、车辆纵向加速度、侧向加速度、横摆角速度和车辆与车道线的横向距离；

(2)建立车辆横摆角观测器和车辆质心侧偏角观测器，将车辆航向角以及横摆角速度作为车辆横摆角观测器的输入信息，将车辆在东北天方向的纵侧向速度、车辆纵向加速度、侧向加速度以及车辆与车道线的横向距离作为辆质心侧偏角观测器的输入信息，同时将车辆横摆角观测器和车辆质心侧偏角观测器进行互联；

(3)车辆横摆角观测器估计得到车辆横摆角和横摆角速度偏差并作为车辆质心侧偏角观测器的输入信息，车辆质心侧偏角观测器估计得到车辆质心侧偏角作为车辆横摆角观测器的输入信息，进而完成车辆质心侧偏角估计。

步骤(1)中车辆航向角以及车辆在东北天方向的纵侧向速度通过安装在车辆质心正上方的gps获取，车辆纵向加速度、侧向加速度以及横摆角速度通过安装在车辆质心处的惯性传感器获取，车辆与车道线的横向距离通过安装在车辆上的摄像头获取。

步骤(2)中车辆横摆角观测器的状态方程为：

其中，ψ为待估计的车辆横摆角，rbias为待估计的横摆角速度偏差，为待估计的车辆横摆角的导数，为待估计的横摆角速度偏差的导数，rm为惯性传感器获取的横摆角速度测量值，为gps提供的车辆航向角，β为多源信息采集单元估计的车辆质心侧偏角；

进而，通过卡尔曼滤波方法估计得到待估计的车辆横摆角ψ以及待估计的横摆角速度偏差rbias。

步骤(2)中车辆质心侧偏角观测器为：

表示的导数，表示y向量的估计值，表示车辆与车道线的横向距离的估计值，表示纵向车速估计值，表示侧向车速估计值，yf为摄像头获取的车辆与车道线的横向距离，和分别为gps获取的车辆在东北天方向的纵向和侧向速度，k为反馈增益矩阵，ψ为车辆横摆角观测器估计得到的车辆横摆角，rm为惯性传感器获取的横摆角速度测量值，rbias为车辆横摆角观测器估计得到的横摆角速度偏差，lcf为摄像头预瞄点到车辆质心的距离，axm和aym分别为惯性传感器获取的车辆纵向加速度和侧向加速度，bx和by分别表示惯性传感器获取的纵向加速度和侧向加速度的测量偏差，β为待估计的车辆质心侧偏角。