车辆方向盘零位、yawrate估计

本文链接：https://blog.csdn.net/qq_40271651/article/details/134744172

1、车辆动力学模型

在余志生写的《汽车理论》一书中，详细介绍了线性二自由度动力学模型的建立方法。但是在实际应用的过程中，很多参数难以获得，对此本人对车辆动力学模型进行了变形，从而更加容易建模或者调参。

侧偏刚度的定义是“单位侧偏角所需要的力”，这个力是施加在车辆上的。现在我们需要转变思路，定义一个侧偏柔度γ，它表达的含义是“单位侧向加速度所能产生的侧偏角”。如果定义其为正值的话，考虑到车辆的轴荷分布，那么可以得到

（1） $\left\{ \begin{matrix} {{\gamma }_{2}}=-\frac{ma}{L{{k}_{2}}} \\ {{\gamma }_{1}}=-\frac{mb}{L{{k}_{1}}} \\ \end{matrix} \right.$

其中γ2，γ1表示后轴、前轴侧偏柔度，a，b分别表示质心到前后轴的距离，L表示为轴距，m表示为质量。考虑轮胎的侧偏刚度跟其载荷一定区间内成正比，可以得出一个结论，轴荷越大，侧偏刚度越大，但是侧偏柔度几乎不变。根据常用的轮胎模型，大致可以得出，后轴的侧偏柔度在0.004-0.008区间，前轴由于包含了转向机构，存在更多的弹性元件，前轴的侧偏柔度一般比后轴大一些，取值区间在0.006-0.012之间。

将一辆车假设为一个质量均匀分布的长方体，杆子长度设为l，宽为w，那么可以得到杆子的转动惯量为：

（2） $I_z=m(l^2+w^2)/12$

其数量级大致与mab相同，记车辆绕质心的转动惯量表达式如下：

（3） ${{I}_{z}}=\eta mab$

其中η为转动惯量修正因子，其值范围大致为0.7-1.1之间。将上面三个表达式代入到二自由度动力学方程中，并求解其输入输出传递函数，可以得到：

（4） $\frac{\omega (s)}{\delta (s)}=\frac{{{B}_{1}}s+{{B}_{0}}}{{{s}^{2}}+2\zeta {{\omega }_{0}}s+\omega _{0}^{2}}$

（5） ${{\omega }_{0}}=\frac{L}{u}\sqrt{\frac{{{k}_{\text{f}}}{{k}_{\text{r}}}}{m{{I}_{z}}}\left( 1+K{{u}^{2}} \right)}$

（6） $\zeta =\frac{-m\left( {{a}^{2}}{{k}_{\text{f}}}+{{b}^{2}}{{k}_{\text{r}}} \right)-{{I}_{z}}\left( {{k}_{\text{f}}}+{{k}_{\text{r}}} \right)}{2L\sqrt{m{{I}_{z}}{{k}_{\text{f}}}{{k}_{\text{r}}}\left( 1+K{{u}^{2}} \right)}}$

(7) ${{B}_{0}}=\frac{L{{k}_{\text{f}}}{{k}_{\text{r}}}}{mu{{I}_{z}}}$

(8) ${{B}_{1}}=\frac{a{{k}_{\text{r}}}}{{{I}_{z}}}$

(9) $K=\frac{m}{{{L}^{2}}}\left( \frac{{{l}_{\text{f}}}}{{{k}_{\text{r}}}}-\frac{{{l}_{\text{r}}}}{{{k}_{\text{f}}}} \right)$

其简单推导过程见我的另一篇文章：基于动力学建模的PID路径跟踪控制算法。在这里暂时不求解质心侧偏角（实际工程应用中很少用到），感兴趣的同学可以自行推导。

将公式（1）（2）（3）带入，得到：

（10） $\left\{ \begin{matrix} {{B}_{1}}=\frac{1}{\eta L{{\gamma }_{1}}} \\ {{B}_{0}}=\frac{1}{\eta uL{{\gamma }_{1}}{{\gamma }_{1}}} \\ {{\omega }_{0}}=\frac{1}{u}\sqrt{\frac{1}{\eta {{\gamma }_{1}}{{\gamma }_{2}}}\left( 1+K{{u}^{2}} \right)} \\ \zeta =\frac{\left( \eta a+b \right){{\gamma }_{1}}+\left( a+\eta b \right){{\gamma }_{2}}}{2L\sqrt{\left( 1+K{{u}^{2}} \right){{\gamma }_{1}}{{\gamma }_{2}}}} \\ \end{matrix} \right.$

(11) $K=(\gamma_1-\gamma_2)/L$

对于不同的车辆车重、侧偏刚度等参数往往差别比较大，但是γ1，γ2，η的取值范围一般不会差很多，公式（10）所有参数均与整车质量无关，除了阻尼比ζ，其他参数均与质心位置无关。另外阻尼比虽然与质心的位置有关，但是η的取值在0.7-1.1之间，可以看出质心的位置对阻尼比的影响的波动并不大。公式（11）非常容易求解得到车辆的稳定性参数。其中一个很重要的创新型是约掉了力和质量，直接使用时间、空间参数（位置、速度，加速度）来描述运动，与传统二自由度模型等效，但是建模难度和参数的获取难度降低了很多。

2、状态估计

在智能驾驶控制算法中，车辆的横摆角速度信号非常重要，在视觉感知、融合以及控制的相关算法中都起着至关重要的作用。但是车载横摆角速度信号，往往存在着较大的信号噪声，如果采用简单的低通滤波会使得求解得到的横摆角速度存在较大的信号延迟，且车辆存在一定的通信延迟，因此，准确实时的获取横摆角速度信号非常重要。

前轮转角到横摆角速度的传递函数，有

（12） $\ddot{\omega }+2\zeta {{\omega }_{0}}\dot{\omega }+\omega _{0}^{2}\omega ={{B}_{1}}\dot{\delta }+{{B}_{0}}\delta$

从而有

（13） $\left[ \begin{matrix} {\dot{\omega }} \\ {\ddot{\omega }} \\ \end{matrix} \right]=\left[ \begin{matrix} 0 & 1 \\ -\omega _{0}^{2} & -2{{\omega }_{0}}\zeta \\ \end{matrix} \right]\left[ \begin{matrix} \omega \\ {\dot{\omega }} \\ \end{matrix} \right]+\left[ \begin{matrix} 0 \\ {{B}_{1}}\dot{\delta }+{{B}_{0}}\delta \\ \end{matrix} \right]$

由于实际车辆的方向盘转角往往存在零位偏差，上述式子需要改写为

（14） $\left[ \begin{matrix} {\dot{\omega }} \\ {\ddot{\omega }} \\ \end{matrix} \right]=\left[ \begin{matrix} 0 & 1 \\ -\omega _{0}^{2} & -2{{\omega }_{0}}\zeta \\ \end{matrix} \right]\left[ \begin{matrix} \omega \\ {\dot{\omega }} \\ \end{matrix} \right]+\left[ \begin{matrix} 0 \\ {{B}_{1}}\dot{\delta }+{{B}_{0}}\delta \\ \end{matrix} \right]$

由于零位偏差往往无法获得，因此将前轮转角的零位偏差作为状态量，可以得到：

（15） $\left[ \begin{matrix} {\dot{\omega }} \\ {\ddot{\omega }} \\ {{{\dot{\delta }}}_{0}} \\ \end{matrix} \right]=\left[ \begin{matrix} 0 & 1 & 0 \\ -\omega _{0}^{2} & -2{{\omega }_{0}}\zeta & -{{B}_{0}} \\ 0 & 0 & 0 \\ \end{matrix} \right]\left[ \begin{matrix} \omega \\ {\dot{\omega }} \\ {{\delta }_{0}} \\ \end{matrix} \right]+\left[ \begin{matrix} 0 \\ {{B}_{1}}\dot{\delta }+{{B}_{0}}\delta \\ 0 \\ \end{matrix} \right]$

记输入量 ${{B}_{1}}\dot{\delta }+{{B}_{0}}\delta$ 为一个整体，观测矩阵为 $\left[ \begin{matrix} 1 & 0 & 0 \\ \end{matrix} \right]$

对状态方程（15）使用线性卡尔曼滤波，可以得到横摆角速度的估计值和方向盘转角的零位偏差。

3、matlab代码

下面附上了简要的matlab代码，copy到simulink模型的m-function中即可，注意函数调用周期，车辆动力学参数可自行设置

function [yawrate,dyawrate,delta0] = VehicleEstimate(u,delta,ddelta,yawrate_maeas)
% delta 前轮转向角 rad
% ddelta 前轮转向角速度 rad/s
[r0,zeta,B1,B0] = GetVehicleParam(u);

[Mat_A,Mat_B,Mat_C] = get_state_func(r0,zeta,B1,B0,delta,ddelta);
ts = 0.02; %时间间隔
input = 1;
Q = 0.04; %相对过程噪声
R = 4e-6; %绝对测量噪声
persistent xstate P;
if isempty(xstate)
    xstate = [0;0;0];
    P = [0.1 0 0;0 0.1 0;0 0 0.00001];
end
[xstate, P] = KF_StatEst(Mat_A,Mat_B,Mat_C,ts,xstate,yawrate_maeas,input,P,Q,R);
yawrate = xstate(1);
dyawrate = xstate(2);
delta0 = xstate(3);
end


function [r0,zeta,B1,B0] = GetVehicleParam(u)  %获取方向盘转角->yawrate传感函数相关参数
%k1->gamma1，前轴侧偏柔度
%k2->gamma2，后轴侧偏柔度
%u 车速
%a 质心到前轴
%b 质心到后轴
%L 轴距
%eta
gamma1 = 0.009;
gamma2 = 0.0061;
L = 2.912;
a = 1.4;
b =L - a;
eta = 0.85;
u = max(u,1);
K = (gamma1-gamma2)/L;
r0 = 1/u*sqrt((1+K*u^2)/(eta*gamma1*gamma2));
zeta = ((eta*a+b)*gamma1 + (eta*b+a)*gamma2)/(2*L*sqrt((1+K*u^2)*gamma1*gamma2));
B1 = 1/(eta*L*gamma1);
B0 = 1/(eta*u*L*gamma1*gamma2);

end

function [Mat_A,Mat_B,Mat_C] = get_state_func(r0,zeta,B1,B0,delta,ddelta) %获取状态空间转移矩阵
%#codegen

Mat_A = [0,1 0 ;-r0*r0,-2*r0*zeta,-B0;0 0 0];
Mat_B = [0; B0 * delta + B1 * ddelta;0];
% Mat_B = [0; 1];
Mat_C = [1 0 0];
end


function [xNew, PNew] = KF_StatEst(A,B,C,ts,x,y,u,P,Q,R) %卡尔曼滤波
Dim = size(A,1);
I = eye(Dim);
TransMat = I + A*ts;
InputMat = B*ts;
Tau = InputMat;
x_pre = TransMat*x+InputMat*u;
P1 = TransMat*P*TransMat'+Tau*Q*Tau';
K = (C*P1*C'+R)\(P1*C');
% K(2) = 0;
xNew = x_pre + K*(y-C*x_pre);
PNew = (I-K*C)*P1;
end