基于卡尔曼滤波算法对汽车的质心侧偏角估计（2023-08-24）

最新推荐文章于 2024-09-23 23:22:45 发布

___March___

最新推荐文章于 2024-09-23 23:22:45 发布

阅读量972

点赞数 3

分类专栏：车辆算法研究文章标签：汽车算法

本文链接：https://blog.csdn.net/marcher__/article/details/132479009

版权

车辆算法研究专栏收录该内容

1 篇文章 3 订阅

订阅专栏

#我的编程语言学习笔记#

👨‍💻 ___March___：个人空间

🚓个人专栏：《车辆算法研究》

文章目录

一、前言

二、Kalman Filtering的基本模型

1.状态估计协方差

2.状态变量

二、车辆动力学机理分析

三、Kalman Filtering预测函数

四、车辆动力学参数说明

一、前言

卡尔曼滤波（Kalman Filtering）是以控制领域大神R. E. Kalman命名的一种滤波技术，还可以称之为状态估计（State Estimation）方法。若不以R. E. Kalman来进行命名，这个方法还可以叫做Linear Least-Mean-Squares Estimator（线性最小均方差估计）或者Linear Quadratic Estimator（线性二次型估计）。

卡尔曼滤波使用对象通常是一个线性随机系统(Linear Stochastic System)，通过测量的含有随机噪音的输出来估计出最优的系统状态。该方法的最优性是建立是使Mean Squared Errors(MSE)是数值最小。故扩展卡尔曼滤波器（Extended Kalman Filter，EKF)，自适应卡尔曼滤波（Adaptive Kalman Filtering，AKF）也就应运而生。

卡尔曼滤波的应用范围十分广泛，提供了可以真正实用的针对有限维随机系统的实时状态最优估计。它作为一种工具，主要有两方面的应用：状态估计 与估计系统的性能分析。

因此，本文针对车辆质心侧偏角的状态估计进行讲解。

二、Kalman Filtering的基本模型

对于Kalman Filter的理解，用过的都知道“黄金五条公式”，且通过“预测步”与“更新步”两个过程来对系统的状态进行最优估计。

假设：一个离散线性动态系统的模型如下所示：

$x_k =Ax_{k-1}+Bu_k+\omega _{k-1}$ - - - - - - - - - - （1）

$y_k =Hx_{k-1}+\upsilon _{k-1}$ - - - - - - - - - - - - - - - - （2）

其中，各变量表征的意义为：

$x_k\rightarrow$ 系统状态矩阵 - - - - - - - - - - - - - - - - $y _k\rightarrow$ 状态阵的观测量（实测）

$A\rightarrow$ 状态转移矩阵 - - - - - - - - - - - - - - - - $B\rightarrow$ 控制输入矩阵

$H\rightarrow$ 状态观测矩阵

$\omega _{k-1}\rightarrow$ 过程噪声 - - - - - - - - - - - - - - - - $\upsilon _{k-1}\rightarrow$ 测量噪声

对于状态估计算法而言，我们可以获取状态量的三个值：状态预测值（ $\hat{x_k}$ ）、最优估计值（ $\tilde{x_k}$ ）以及真实值（ $x_k$ ），卡尔曼滤波的原理就是利用卡尔曼增益来修正状态预测值，使其逼近真实值。

对卡尔曼滤波的推导过程，第一个过程分为状态估计协方差 $P_k$ 的推导，即代价函数的求取；第二步则是卡尔曼增益矩阵及其他准则的推导。

1.状态估计协方差

状态预测值 $\hat{x_k}$ 由状态预测方程可得：黄金1条

$\hat{x}_k=A\tilde{x}_{k-1}+Bu_k$ - - - - - - - - - - - - - - - - （3）

状态最优估计值 $\tilde{x_k}$ 可由状态更新方程可得：黄金2条

$\tilde{x}_{k}=\hat{x}_k+K(y_k-H\hat{x}_k)$ - - - - - - - - - - - （4）

通过该方程可知，卡尔曼增益 $K$ 实际上表征了状态最优估计过程中模型预测误差（Predicted Error）与测量误差（Measurement Error）的比重，即 $K\in [0,1]$ 。当 $K= 0$ 时，即预测误差为0，系统的状态值完全取决与预测值；而当 $K= 1$ 时，即量测误差为0，系统的状态值完全取决于量侧值。

因此，可令：

$\hat{e}_k=x_k-\hat{x}_k$ - - - - - - - - - - - - - - - - - - （5）
$e_k=x_k-\tilde{x}_k$ - - - - - - - - - - - - - - - - - - （6）

$\tilde{P}_{k}=E[\hat{e}_k\hat{e}_k^{\tau })]$ - - - - - - - - - - - - - - - - （7）
$P_{k}=E[e_ke_k^\tau ]$ - - - - - - - - - - - - - - - - - - -（8）

其中，各变量表征的意义为：

$\hat{e_k}\rightarrow$ 先验状态误差 - - - - - - - - - - - - - - - - $e_k\rightarrow$ 后验状态误差

$\tilde{P_{k}}\rightarrow$ 真实值与预测值之间的协方差

$P_{k}\rightarrow$ 真实值与最优估计值之间的协方差

由方程（2）（4）可知：

$\tilde{x}_{k}=\hat{x}_k+K(Hx_k+\upsilon _k-H\hat{x}_k)$ - - - - - - - - - - - - - - - （9）

将上式子变换可得：

$\tilde{x}_{k}-x_k=\hat{x}_k-x_k+KH(x_k-\hat{x}_k)+K\upsilon _k$ - - - - - - - （10）

由方程（5）（6）可知：

$e_k=(I-KH)\hat{e}_k-K\upsilon_k$ - - - - - - - - - - - - - - - - - - - - - - （11）

因此，由方程(8)可知估计误差方差阵 $P_{k}$ 为：

$P_{k}=E[e_ke_k^\tau ]=(I-KH)\hat{P}_k(I-KH)^{\tau }+KRK^{\tau }$ - -（12）

上式展开得:

$P_{k}=\hat{P}_k-KH\hat{P}_k-\hat{P_k}H^\tau K^\tau +K(H\hat{P}_kH^\tau+R)K^\tau$ -（13）

2.状态变量

卡尔曼滤波的估计原则就是使最优状态估计的协方差 $P_{k}$ 最小，使其越来越逼近于真实值。因此，其目标函数为：

$J=\sum_{min}^{}P_k$ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - （14）

对卡尔曼增益矩阵 $K$ 求求偏导，可知：

$\frac{\partial P_k}{\partial K}=-2(\hat{P}_kH^\tau)+2K(H\hat{P}_kH^\tau+R)=0$ - - - - - - - - （15）

由此可知最优估计条件下的卡尔曼增益矩阵 $K$ 为：黄金三条

$K=\hat{P}_kH^\tau(H\hat{P}_kH^\tau+R)^{-1}$ - - - - - - - - - - - - - - - - - - - - - （16）

联立式(10)(13)即可知道估计误差方差阵为：黄金四条

$P_k=(I-KH)\hat{P}_k$ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - （17）

由式（5）可知：

$\hat{e}_{k+1}=x_k-\hat{x}_k=(Ax_k+Bu_k+\omega _k)-(A\tilde{x}_k+Bu_k)$ - - （18）

化简可知：

$\hat{e}_{k+1}=A(x_k-\tilde{x}_k)+\omega _k=Ae_k+\omega _k$ - - - - - - - - - - - - - - - - - （19）

由式（7）可知：

$\tilde{P}_{k}=E[\hat{e}_k\hat{e}_k^{\tau })]=E[(Ae_k+\omega_k)(Ae_k+\omega_k)^{\tau}]$ - - - - - - - - - - - - （20）

$\tilde{P}_{k}=E[(Ae_k)(Ae_k)^{\tau}]+E[(\omega_k)(\omega_k)^{\tau}]$ - - - - - - - - - - - - - - - - - - （21）

由式（21）可知，预测协方差矩阵 $\hat{P}_{k+1}$ 为：黄金5条

$\hat{P}_{k+1}=AP_kA^\tau+Q$ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - （22）

至此，卡尔曼滤波的黄金5条全部推导完成，其“预测步”与“更新步”过程如下表1-1所示。

表1-1 传统Kalman Filtering
预测步	$\hat{x}=Fx$
预测步	$\hat{P}=FPF^\tau+Q$
更新步	$\hat{K}={PH^\tau}/{(HPH^\tau+R)}$
	$\tilde{x}=\hat{x}+\hat{K}(y-H\hat{x})$
	$\tilde{P}=\hat{P}-\hat{K}H\hat{P}$

二、车辆动力学机理分析

目前，不易借助传感器直接测量质心侧偏角，通常要搭建车辆状态观测器进行估算。根据实际需求的不同，搭建的车辆模型是有所区别的，其复杂程度决定了状态观测器的运算精度与计算效率。因此，本节以线性二自由度模型为参考模型，搭建基于Kalman Filtering的状态观测器。

根据前面对Kalman Filtering的讲解和推导，为了便于对汽车质心侧偏角的预测，应对车辆的动力学系统进行分析，本文章主要基于汽车二自由度对汽车的质心侧偏角进行估计。

汽车二自由度包含汽车的横摆和侧倾两个角度进行分析计算，这部分较为基础，小编就不做多赘述了，如果有机会小编对汽车二自由度进行推到简单讲解。

二自由度汽车模型是一种经典的汽车模型，在汽车底盘控制器的研究中使用广泛。建模假设如下：

忽略转向系统的影响，直接以前轮转角作为输入；
忽略悬架的作用，认为汽车车身只作平行于地面的平面运动；
汽车的纵向速度视为不变，即纵向加速度为0;
汽车的侧向加速度限定在0.4g以下，轮胎侧偏特性处于线性范围内；
假设路面平整，无坡度。

汽车二自由度公式：

$k_1(\beta+\frac{\gamma L_{f} }{vx}-\delta )+k_2(\beta-\frac{\gamma L_{r} }{vx} )=m(\dot{v}_y+v_x\gamma)$ - - - - - - - - - - - - - - - （23）

$(ak_1-bk_2)\beta+\tfrac{\gamma(L_f^2k_1+L_r^2k_2)}{v_x}-ak_1\delta =I_z\dot{\gamma}$ - - - - - - - - - - - - - - - - - - - （24）

为了对非线性系统进行处理，需对车辆方程离散化，将线性二自由度模型的空间方程写成如下标准形式：

$\dot{\beta}=(\frac{v_y}{v_x})'=\frac{v_x\dot{v}_y-\dot{v}_xv_y}{v_x^2} =\frac{v_x\dot{v}_y}{v_x^2}-\frac{\dot{v}_xv_y}{v_x^2}(\dot{v}_x=0)$ - - - - - - - - - - - - - - - - （25）

$\dot{\beta}=\frac{\dot{v}_y}{v_x}=\frac{k_1+k_2}{mv_x}\beta+(\frac{L_fk_1+L_rk_2}{mv_x^2}-1)\gamma -\frac{k_1}{mv_x}\delta$ - - - - - - - - - - - - - - - - -（26）

$\dot{\gamma}=\frac{(L_fk_1-L_rk_2)}{I_z}\beta+\tfrac{(L_f^2k_1+L_r^2k_2)}{v_xI_z}\gamma-\frac{ak_1}{I_z}\delta$ - - - - - - - - - - - - - - - - - - - - - - （27）

$\dot{v}_y=\frac{(k_1-k_2)}{m}\beta+\tfrac{(L_fk_1+L_rk_2)}{mv_x}\gamma-\frac{k_1}{m}\delta$ - - - - - - - - - - - - - - - - - - - - - - - - - -（28）

根据上述，可求其状态方程矩阵：

$\begin{bmatrix} \dot{ \beta} \\ \dot{\gamma } \end{bmatrix}=A\begin{bmatrix} \beta\\ \gamma \end{bmatrix}+B\begin{bmatrix} \delta \end{bmatrix}$ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -（29）

$\begin{bmatrix} \dot{ v}_y \\ \dot{\gamma } \end{bmatrix}=C\begin{bmatrix} \beta\\ \gamma \end{bmatrix}+D\begin{bmatrix} \delta \end{bmatrix}$ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -（30）

表2-1 预测Kalman Filtering参数
A	$\begin{bmatrix} \frac{k_1+k_2}{mv_x}\Delta t+1&(\frac{L_fk_1+L_rk_2}{mv_x^2}-1)\Delta t \\ \frac{(L_fk_1-L_rk_2)}{I_z}\Delta t & \tfrac{(L_f^2k_1+L_r^2k_2)}{v_xI_z}\Delta t+1\end{bmatrix}$
B	$\begin{bmatrix} -\frac{k_1}{mv_x}\Delta t\\-\frac{ak_1}{I_z}\Delta t \end{bmatrix}$
C	$\begin{bmatrix} \frac{k_1+k_2}{m}&(\frac{L_fk_1+L_rk_2}{mv_x}-1)\\ 0&1\end{bmatrix}$
D	$\begin{bmatrix} -\frac{k_1}{m}\\0 \end{bmatrix}$

三、Kalman Filtering预测函数

表3-1 预测Kalman Filtering
预测步	$\hat{x}=Ax+Bu$
预测步	$\hat{P}=APA^\tau+Q$
更新步	$\hat{K}={PC^\tau}/{(CPC^\tau+R)}$
	$\tilde{x}=\hat{x}+\hat{K}(y-(C\hat{x}+Du))$
	$\tilde{P}=\hat{P}(1-\hat{K}H)$

四、车辆动力学参数说明

表4-1 车辆动力学参数说明
符号	名称
$m$	车辆质量
$\gamma$	横摆角速度
$\beta$	质心侧偏角
$v_x$	纵向速度
$v_y$	侧向速度
$L_f$	前轮到质心的距离
$L_r$	后轮到质心的距离
$\delta$	前轮转角
$k_1$	前轮侧偏刚度
$k_2$	后轮侧偏刚度