基于carsim的线控转向仿真(2)--齿条力观测

news2024/11/14 20:46:47

观测器更详细的介绍文章可以关注博主以下两篇文章

从小车倒立摆系统看系统建模控制LQR+LQE仿真_lqr平衡小车仿真模型-CSDN博客

好玩的直流电机调速实验、PID、极点配置、LQR、观测器；不讲大道理_观测器极点配置-CSDN博客

三个实例迅速掌握经典卡尔曼滤波用法_卡尔曼滤波温度估计-CSDN博客

本篇文章中，博主使用matlab提供的kalman函数，进行观测器设计。

一、系统状态空间方程

在上一节我们搭建的目标角度跟踪模型中，只有角度控制的pid是属于控制器，其余都属于被控对象。我们的目标是得到齿条力(即转向阻力)，输入的电机扭矩可以通过传感器采集换算得到，输出的齿条位移可以由前轮转角换算得到；

总结一下，齿条力模型

可以看成两输入(电机力Fm=Tm/rp,齿条力Fz)，一输出(齿条位置Xr)的一个模型，其中Tm,Xr已知，现在要观测得到Fz，这里我们使用simulink自带的卡尔曼滤波模块来进行观测。

系统微分方程：

$F_{m}-F_{z}=M_{r}\ddot{x_r}+B_{r}\dot{x_r}$

选取 $\left [ \dot{x},x \right ]^T$ 为状态量，建立状态空间方程如下：

$\begin{pmatrix} \ddot{x_r}\\\dot{x_r} \end{pmatrix}= \begin{pmatrix} -B_r/M_r & 0\\ 1&0 \end{pmatrix} \begin{pmatrix} \dot{x_r}\\x_r \end{pmatrix} + \begin{pmatrix} 1/M_r&-1/M_r \\ 0&0 \end{pmatrix} \begin{pmatrix} F_m\\F_z \end{pmatrix}$

$x=\begin{pmatrix} 0 & 1 \end{pmatrix} \begin{pmatrix} \dot{x_r}\\x_r \end{pmatrix} + \begin{pmatrix} 0 & 0 \end{pmatrix} \begin{pmatrix} F_m\\F_z \end{pmatrix}$

我们要观测的Fz却在输入位置，没法用卡尔曼观测器观测得到，需要把它挪到状态量的位置才可以，可以将Fz的微分认为0。

$\dot{F_z}=0*F_{z}+1*\dot{F_{z}} \\ F_{z}=1*F_{z}+0*F_{z}$

将这个矩阵与上个矩阵放一起，重新整理可以得到：

$\begin{pmatrix} \ddot{x_r}\\\dot{x_r}\\\dot{F_{z}} \end{pmatrix}= \begin{pmatrix} -B_r/M_r & 0&-1/M_{r}\\ 1&0&0 \\0&0&0\end{pmatrix} \begin{pmatrix} \dot{x_r}\\x_r \\F_{z}\end{pmatrix} + \begin{pmatrix} 1/M_r&0 \\ 0&0 \\0&1\end{pmatrix} \begin{pmatrix} F_m\\\dot{F_{z}} \end{pmatrix}$

$x=\begin{pmatrix} 0 & 1&0 \end{pmatrix} \begin{pmatrix} \dot{x_r}\\x_r \\F_{z}\end{pmatrix} + \begin{pmatrix} 0 & 0 \end{pmatrix} \begin{pmatrix} F_m\\\dot{F_z} \end{pmatrix}$