基于车辆动力学模型的自动驾驶车辆横向控制

jrrz0828

于 2024-09-05 16:32:38 发布

阅读量973

点赞数 33

文章标签：自动驾驶

本文链接：https://blog.csdn.net/jrrz0828/article/details/141928759

版权

1.车辆动力学模型

图1-2所示为车辆二自由度模型，该二自由度是指车辆沿y轴的横向运动和绕z轴的横摆运动，将其简化为两轮的自行车模型，简化过程中进行了如下假设：

（1）忽略转向系统的影响，直接以前轮转角作为输入；
（2）忽略悬架的作用，车身只做平行于地面的平面运动，即忽略沿z轴的垂向运动、绕y轴的俯仰运动以及绕x轴的侧倾运动；
（3）汽车沿x轴的前进速度u视为不变；
（4）侧向加速度不超过0.4g，确保轮胎的侧偏特性处于线性范围；
（5）驱动力不大，不考虑地面切向力对轮胎侧偏特性的影响，没有空气动力的作用；
（6）忽略左、右车轮轮胎由于载荷变化而引起轮胎特性的变化以及轮胎回正力矩的作用。

（1-1）

参数	含义
${{F_y}}$	横向合力
$M$	合力矩
$m$	车辆质量
${{a_y}}$	横向加速度
${{F_{yf}}}$	地面对前轮的横向力
${{F_{xf}}}$	地面对前轮的纵向力
${{F_{yr}}}$	地面对后轮的横向力
${{F_{xr}}}$	地面对后轮的纵向力
${{\delta _f}}$	前轮转角
${{I_z}}$	绕z轴的转动惯量
${\ddot \varphi }$	角加速度
$a$	车辆质心到前轴的距离
$b$	车辆质心到后轴的距离
${C_f}$	前轮侧偏刚度
${C_r}$	后轮侧偏刚度
${\alpha _f}$	前轮侧偏角
${\alpha _r}$	后轮侧偏角

假设前轮转角为较小值， $\sin {\delta _f} \approx 0$ ， $\cos {\delta _f} \approx 1$ ，则

（1-2）

其中 ${F_{yf}} = {C_f}{\alpha _f}$ ， ${F_{yr}} = {C_r}{\alpha _r}$ ， ${a_y} = {\dot v_y} + {v_x}\dot \varphi$ ， ${\alpha _f} = \frac{{{v_y} + \dot \varphi a}}{{{v_x}}} - {\delta _f}$ ， ${\alpha _r} = \frac{{{v_y} - \dot \varphi b}}{{{v_x}}}$ ，因此代入式（1-2）中后可得

（1-3）

（1-4）

${a_y} = {\dot v_y} + {v_x}\dot \varphi$ 是通过如下推导得到的：

如图1-3所示，车辆沿 $ox$ 轴速度分量的变化为

（1-5）

考虑到 $\Delta \theta$ 很小并忽略其中的二阶量，则上式变为 $\Delta u - v\Delta \theta$ ，同理车辆沿 $oy$ 轴速度分量的变化为 $u\Delta \theta + \Delta v$ 。

车辆质心绝对加速度沿 $ox$ 轴和 $oy$ 轴的分量分别为 ${a_x} = \dot u - vw$ 和 ${a_y} = \dot v + uw$ 。

2.横向误差控制模型

在式（1-4）的基础上，令航向角误差 ${e_\varphi } = \varphi - {\varphi _{des}}$ ，则

（2-1）

（2-2）

（2-3）

其中 ${e_d}$ 、 ${\dot e_d}$ 、 ${e_\varphi }$ 、 ${\dot e_\varphi }$ 分别为横向距离误差、横向距离误差变化率、航向角误差、航向角误差变化率，将式（2-3）写成式（2-4）的形式，此即为横向误差控制模型。

（2-4）

（2-5）

3.LQR控制原理

LQR全称为Linaer Quadratic Regulator，即线性二次型调节器，是一种现代控制理论中设计状态反馈控制器的方法。

对于一个系统 $\dot x = Ax + Bu$ ，可进行反馈控制，令 $u = - Kx$ ，则 $\dot x = (A - BK)x$ ，通过选取合适的 $K$ 令矩阵 $(A-BK)$ 的特征值实数部分为负数，便能够使得系统稳定。

由于能够使得系统稳定的 $K$ 的取值有很多，因此构造如下代价函数

（3-1）

使得选取的 $K$ 不仅能使系统达到稳定，同时又能使得代价函数最小， $K$ 的具体值可通过求解 $Riccati$ 方程得到。

4.基于LQR的反馈+前馈控制

对于一个系统 $\dot x = Ax + Bu$ ，进行反馈控制，令 $u = - Kx$ 即可使得系统稳定。但是在第二节中推导得到的式（2-4）横向误差控制模型中，多了一部分 $C{\dot \varphi _{des}}$ ，若进行反馈控制，令 $u = - Kx$ ，则式（2-4）变为

（4-1）

由式（4-1）可知，无论 $K$ 取何值， ${\dot e_{rr}}$ 和 ${e_{rr}}$ 都无法同时为0，系统存在稳态误差。因此为消除稳态误差带来的影响，考虑在反馈控制的基础上加入前馈控制，引入前馈值 ${\delta _{ff}}$ ，通过选取适当的前馈值来使得稳态误差尽可能趋近于0，控制量变为如下形式

（4-2）

则式（2-4）变为

（4-3）

控制系统稳定后， ${\dot e_{rr}} = 0$ ，则

（4-4）

${e_d}$ 的稳态误差为

（4-5）

求得前馈值 ${\delta _{ff}}$

（4-6）

其中 ${{K_3}}$ 是经LQR求解后得到的 $K$ 矩阵中第3个值。

5.LQR求解

cf=-145777;%前轮侧偏刚度
cr=-119580;%后轮侧偏刚度
m=1820;%车辆质量
Iz=4095;%绕z轴的转动惯量
a=1.265;%质心到前轴的距离
b=2.947-1.265;%质心到后轴的距离
k=zeros(5000,4);
for i=1:5000
    vx=0.01*i;
    A=[0,1,0,0;
       0,(cf+cr)/(m*vx),-(cf+cr)/m,(a*cf-b*cr)/(m*vx);
       0,0,0,1;
       0,(a*cf-b*cr)/(Iz*vx),-(a*cf-b*cr)/Iz,(a*a*cf+b*b*cr)/(Iz*vx)];
    B=[0;-cf/m;0;-a*cf/Iz];
    Q=[1 0 0 0;
       0 1 0 0;
       0 0 1 0;
       0 0 0 1];
    R=50;
    k(i,:)=lqr(A,B,Q,R);%LQR求解K矩阵
end
k1=k(:,1)';
k2=k(:,2)';
k3=k(:,3)';
k4=k(:,4)';

经LQR求解得到K矩阵，再通过式（4-6）代入相关参数求得前馈值 ${\delta _{ff}}$ ，即可求得如式（4-2）所示的前轮转角。

jrrz0828

关注

33
点赞
踩
22

收藏

觉得还不错? 一键收藏
0
评论
基于车辆动力学模型的自动驾驶车辆横向控制

分别为横向距离误差、横向距离误差变化率、航向角误差、航向角误差变化率，将式（2-3）写成式（2-4）的形式，此即为横向误差控制模型。（2）忽略悬架的作用，车身只做平行于地面的平面运动，即忽略沿z轴的垂向运动、绕y轴的俯仰运动以及绕x轴的侧倾运动；因此为消除稳态误差带来的影响，考虑在反馈控制的基础上加入前馈控制，引入前馈值。但是在第二节中推导得到的式（2-4）横向误差控制模型中，多了一部分。（5）驱动力不大，不考虑地面切向力对轮胎侧偏特性的影响，没有空气动力的作用；在式（1-4）的基础上，令航向角误差。
复制链接

扫一扫