Carsim无人车横向LQR控制仿真

Carsim是一种专门用于车辆动力学仿真的软件，它可以模拟车辆在不同路面上的行驶状态，并提供悬挂、底盘、刹车、转向等多种参数来控制车辆的行驶。Carsim也支持与其他软件的联合仿真，如MATLAB等。本文将Carsim和Simulink联合仿真，即通过将Carsim模型与Simulink模型连接起来，使得在Simulink中设计的控制器可以直接影响Carsim模型的运动状态，从而进行更加全面的系统仿真。

1 连接Carsim2019与Simulink

1.1 打开carsim2019软件,选择数据路径，这里选择安装软件时的路径

1.2进行环境的配置，进入后编辑框如果显示的页面是灰色的，点击右上角的lock进行解锁

1.3选择仿真的车型，这里我们选择C类车

1.4修改汽车的控制条件

1.5将汽车的初始速度设置为0，刹车也取消

1.6选择simulink模型进行仿真

1.7建立新的数据库

1.8命名都可以，我们这里为Demo

1.9进行数据库的参数配置

1.10配置输入数据

1.11新建输入数据库

1.12这里命名为Demo_input

1.13进入输入参数配置页面

1.14选择现有的输入数据库

1.15选择快速开始导航——Baseline

1.16添加油门输入

1.17添加轮胎转角输入

1.18配置输出数据库

1.19新建输出数据库

1.20这里命名为Demo_output

1.21 配置输出

1.22选择快速开始导航——Baseline

1.23选择输出数据，x方向距离，y方向距离，偏航角

至此carsim参数配置完成

1.24新建一个simulink文件

1.25通过carsim加载simlink模型

1.26将carsim配置完成的参数生成simulink组件发送到simulink

1.27进入simulink库查找carsim模块

1.28为carsim S-Function模块添加输入输出

1.29修改输入输出的个数，与carsim配置参数一致

1.30 simfile.sim文件用于描述Simulink模型和Carsim模型之间的连接，在联合仿真中，Carsim模型和Simulink模型交替进行仿真计算，通过simfile.sim文件所描述的接口，将Simulink模型的输出与Carsim模型的输入相连接。这样就实现了两个模型之间的数据交互和联合仿真。

1.31 添加输出输出

1.32 点击run，会运行simulink，同时数据会通过simfile.sim与carsim进行交互

1.33 点击video，会生成运行效果视频

1.34 出现以下画面，按住鼠标右键可以改变视角

确保软件可用后，进行汽车控制仿真

2 汽车的建模

运动学模型

为了简化计算，采用自动车模型近似汽车模型

变量定义：

其中

运动学模型：

输入量：前轮转角$δ_f$，加速度（油门/刹车）

状态：车辆位置(x,y),速度变化率(加速度) ,偏航角变化率(角速度)

O-转动中心，两个轮子的垂直线相交

R-转动半径

根据正弦定理：
$$\frac{\sin \left(\delta_f-\beta\right)}{\ell_f}=\frac{\sin \left(\frac{\pi}{2}-\delta_f\right)}{R} \quad \frac{\sin \left(\beta-\delta_r\right)}{\ell_r}=\frac{\sin \left(\frac{\pi}{2}+\delta_r\right)}{R}$$
展开有：
$$\begin{aligned} & \frac{\sin \left(\delta_f\right) \cos (\beta)-\sin (\beta) \cos \left(\delta_f\right)}{\ell_f}=\frac{\cos \left(\delta_f\right)}{R} \\ & \frac{\cos \left(\delta_r\right) \sin (\beta)-\cos (\beta) \sin \left(\delta_r\right)}{\ell_r}=\frac{\cos \left(\delta_r\right)}{R} \\ \end{aligned}$$

$$
\begin{aligned}
& \tan \left(\delta_{\mathrm{f}}\right) \cos (\beta)-\sin (\beta)=\frac{l_f}{R} \\
& \sin (\beta)-\tan \left(\delta_{\mathrm{r}}\right) \cos (\beta)=\frac{l_r}{R}
\end{aligned}
$$

上式相加有：
$$\left(\tan \left(\delta_f\right)-\tan \left(\delta_r\right)\right) \cos (\beta)=\frac{l_f+l_r}{R}$$
低速行驶下，车辆的方向变化速率小于车辆的角速度
$$\begin{aligned} \dot{\psi} \approx r=\frac{V}{R}=V \cdot \frac{1}{R} & =V \cdot \frac{\left(\tan \left(\delta_f\right)-\tan \left(\delta_r\right)\right) \cos (\beta)}{l_f+l_r} \\ & =\frac{V \cos (\beta)}{l_f+l_r}\left(\tan \left(\delta_f\right)-\tan \left(\delta_r\right)\right) \end{aligned}$$

质心运动学模型

X轴方向：
$$\dot{X}=V \cos (\psi+\beta)$$
Y轴方向：
$$\dot{Y}=V \sin (\psi+\beta)$$
yaw轴方向：
$$\dot{\psi}=\frac{V \cos (\beta)}{l_f+l_r}\left(\tan \delta_f-\tan \delta_r\right)$$
β角：
$$\beta=\tan ^{-1}\left(\frac{l_f \tan \delta_r+l_r \tan \delta_f}{l_f+l_r}\right)$$

动力学模型

将汽车模型简化为自行车动力学模型：假设：纵向速度恒定，左右轴集中为一个轮子（自行车模型），忽略悬架运动、道路倾斜和空气动力学影响，解耦纵向和横向运动

根据牛顿第二定律有：

$$\begin{aligned} & a_y=\left(\frac{\mathrm{d}^2 y}{\mathrm{~d} t^2}\right)_{\text {inertial }}=\dot{v}_y\text{（y方向）}+v_x \dot{\psi}\text{（向心加速度）} \\ & F_{y f}+F_{y_{\Gamma}}=m a_y=m\left(\dot{v}_y+v_x \dot{\psi}\right)\quad \text{y方向受力} \\ & l_f F_{y f}-l_r F_{y r}=I_z \ddot{\psi} \quad \text{扭矩平衡} \end{aligned}$$

轮胎的受力分析

轮胎的侧向力$F_{yf}$,$F_{yr}$,这里采用右手坐标系，所以$F_{yf}$,$F_{yr}$都是负数：

侧向力对动力学的影响：

前轮转角：$\delta$,前轮速度转角$\theta_{vf}$,前轮侧偏角角$\alpha_{f}$

打开carsim侧偏角与侧向力的关系曲线：

可以看到当侧偏角不超过5度时，可认为侧偏角与侧向力呈线性关系，线性斜率为侧偏刚度

得到：
$$\begin{gathered} F_{y f}=2 c_f \alpha_f=2 c_f\left(\delta-\theta_{v f}\right) \\ F_{y r}=2 c_r \alpha_r=2 c_r\left(-\theta_{v r}\right) \end{gathered}$$
将速度进行两个方向的分解：
$$\begin{aligned} & \theta_{v f}=\tan ^{-1}\left(\frac{v_{f y}}{v_{f x}}\right)=\tan ^{-1}\left(\frac{v_y+l_f r}{v_x}\right) \\ & \theta_{v r}=\tan ^{-1}\left(\frac{v_{r y}}{v_{r x}}\right)=\tan ^{-1}\left(\frac{v_y-l_r r}{v_x}\right) \end{aligned}$$
根据动力学模型的受力情况：
$$\begin{aligned} & \left\{\begin{array}{l} F_{y f}^{\prime}+F^{\prime} y_{\Gamma}=m a_y=m\left(\dot{v}_y+v_x \dot{\psi}\right) \\ l_f F_{y f}^{\prime}-l_r F_{y r}^{\prime}=I_z \ddot{\psi} \\ \end{array}\right. \\ & \Downarrow \\ & \left(F_{y f} \cos (\delta)-F_{x f} \sin (\delta)\right)+F_{y r}=m\left(\dot{v}_y+v_x r\right) \\ & \ell_f\left(F_{y f} \cos (\delta)-F_{x f} \sin (\delta)\right)-\ell_r F_{y r}=I_z \ddot{\psi}=I_z \dot{r} \\ & \end{aligned}$$
侧偏角：
$$\left\{\begin{array}{l} \alpha_f=\delta-\tan ^{-1}\left(\frac{v_y+\ell_f r}{v_x}\right) \\ \alpha_r=-\tan ^{-1}\left(\frac{v_y-\ell_r r}{v_x}\right) \end{array}\right.$$
侧向力：
$$\left\{\begin{array}{l} F_{y f}=\mathrm{c}_f \alpha_f=\mathrm{c}_f\left[\delta-\tan ^{-1}\left(\frac{v_y+\ell_f r}{v_x}\right)\right] \\ F_{y r}=c_r \alpha_r=-c_r \tan ^{-1}\left(\frac{v_y-\ell_r r}{v_x}\right) \end{array}\right.$$
得到：
$$\begin{aligned} & \dot{v}_y=\frac{c_f\left[\delta-\tan ^{-1}\left(\frac{v_y+\ell_f r}{v_x}\right)\right] \cos (\delta)-c_r \tan ^{-1}\left(\frac{v_y-\ell_r r}{v_x}\right)-F_{x f} \sin (\delta)}{m}-v_x r \\ & \dot{r}=\frac{\ell_f c_f\left[\delta-\tan ^{-1}\left(\frac{v_y+\ell_f r}{v_x}\right)\right] \cos (\delta)+\ell_r c_r \tan ^{-1}\left(\frac{v_y-\ell_r r}{v_x}\right)-l_f F_{x f} \sin (\delta)}{I_z} \end{aligned}$$

3 LQR汽车轨迹跟踪控制

3.1算法框图：

3.1.1 A、B矩阵计算模块-线性化动力学模型

当转角很小时：
$$\begin{aligned} & \cos (\delta) \approx 1 \\ & \sin (\delta) \approx 0 \\ & \tan ^{-1}(\theta) \approx \theta \end{aligned}$$
带入得到：
$$\begin{aligned} & \dot{v}_y=\frac{-c_f v_y-c_f \ell_f r}{m v_x}+\frac{c_f \delta}{m}+\frac{-c_r v_y+c_r \ell_r r}{m v_x}-v_x r \\ & \dot{r}=\frac{-\ell_f c_f v_y-\ell_f^2 c_f r}{I_z v_x}+\frac{\ell_f c_f \delta}{I_z}+\frac{\ell_r c_r v_y-\ell_r^2 c_r r}{I_z v_x} \end{aligned}$$
写成状态空间方程：

取状态：
$$X= \left[\begin{array}{c} v_y \\ r \end{array}\right]$$

$$u=\delta$$

写成关于状态的方程：
$$\begin{aligned} & \dot{v}_y=\frac{-\left(c_f+c_r\right)}{m v_x} v_y+\left[\frac{\left(l_r c_r-l_f c_f\right)}{m v_x}-v_x\right] r+\frac{c_f}{m} \delta \\ & \dot{r}=\frac{l_r c_r-l_f c_f}{I_z v_x} v_y+\frac{-\left(\ell_f^2 c_f+\ell_r^2 c_r\right)}{I_z v_x} r+\frac{l_f c_f}{I_z} \delta \\ & \end{aligned}$$
得到：
$$\left[\begin{array}{c} v_y \\ \dot{r} \end{array}\right]=\left[\begin{array}{cc} \frac{-\left(c_f+c_r\right)}{m v_x} & \frac{\left(l_r c_r-l_f c_f\right)}{m v_x}-v_x \\ \frac{l_r c_r-l_f c_f}{I_z v_x} & \frac{-\left(\ell_f^2 c_f+\ell_r^2 c_r\right)}{I_z v_x} \end{array}\right]\left[\begin{array}{c} v_y \\ r \end{array}\right]+\left[\begin{array}{c} \frac{c_f}{m} \\ \frac{l_f c_f}{I_z} \end{array}\right] \delta$$
如果取状态，输入:
$$X=\left[\begin{array}{l} y \\ \dot{y} \\ \psi \\ \dot{\psi} \end{array}\right] \text {,u=} \delta$$
有：
$$\frac{d}{d t}\left[\begin{array}{l} y \\ \dot{y} \\ \psi \\ \dot{\psi} \end{array}\right]=\left[\begin{array}{cccc} 0 & 1 & 0 & 0 \\ 0 & \frac{-\left(c_f+c_r\right)}{m v_x} & 0 & \frac{\left(l_r c_r-l_f c_f\right)}{m v_x}-v_x \\ 0 & 0 & 0 & 1 \\ 0 & \frac{l_r c_r-l_f c_f}{I_z v_x} & 0 & \frac{-\left(\ell_f^2 c_f+\ell_r^2 c_r\right)}{I_z v_x} \end{array}\right]\left[\begin{array}{c} y \\ \dot{y} \\ \psi \\ \dot{\psi} \end{array}\right]+\left[\begin{array}{c} 0 \\ \frac{c_f}{m} \\ 0 \\ \frac{l_f c_f}{I_z} \end{array}\right] \delta$$

$$A=\left[\begin{array}{cccc} 0 & 1 & 0 & 0 \\ 0 & \frac{-\left(c_f+c_r\right)}{m v_x} & 0 & \frac{\left(l_r c_r-l_f c_f\right)}{m v_x}-v_x \\ 0 & 0 & 0 & 1 \\ 0 & \frac{l_r c_r-l_f c_f}{I_z v_x} & 0 & \frac{-\left(\ell_f^2 c_f+\ell_r^2 c_r\right)}{I_z v_x} \end{array}\right]$$

$$B=\left[\begin{array}{c} 0 \\ \frac{c_f}{m} \\ 0 \\ \frac{l_f c_f}{I_z} \end{array}\right]$$

3.1.2 LQR模块，计算反馈系数K

LQR(线性二次调节器)是一种控制器设计方法，它可以稳定线性系统并最小化控制信号的方均根误差。通过LQR，可以得到反馈系数K，用于控制系统的控制。

离散系统：
$$x_{t+1}=A x_t+B u_t, x_0=x_{\text {init }}$$
离散LQR代价函数：
$$J(U)=\sum_{\tau=0}^{N-1}\left(x_\tau^T Q x_\tau+u_\tau^T R u_\tau\right)+x_N^T Q_f x_N$$
K=dlqr(A,B,Q,R)

由动态规划求解可得（具体推导过程）：

$$K_t=-\left(R+B^T P_{t+1} B\right)^{-1} B^T P_{t+1} A$$
其中
$$P_{t-1}=Q+A^T P_{t+1} A-A^T P_{t+1} B\left(R+B^T P_{t+1} B\right)^{-1} B^T P_{t+1} A$$

3.1.3 误差计算模块，k曲率计算模块

横向位置误差：
$$e_d=\left(\vec{x}-\vec{x}_r\right) \cdot \vec{n}_r$$
横向速度误差：
$$\dot{e_d}=|\vec{v}| \sin \left(\theta-\theta_r\right)$$
角度误差：
$$e_{\varphi}=\varphi-\theta r$$
角速度误差：
$$\dot{e_{\varphi}}=\dot{\varphi}-k \dot{s}$$
其中
$$\dot{\theta_r}=k \dot{s}$$
k为曲率
$$\dot{s}=\frac{|\vec{v}| \cos \left(\theta-\theta_r\right)}{1-k e_d}$$
对于离散轨迹：

• 找到离散轨迹规划点与真实位置（x，y）最近的点

• 匹配点不是投影点，当规划点较密集时，匹配点可近似为投影点，假设匹配点与投影点的曲率相等，可近似两点在同一圆弧上

$$\begin{aligned} & \vec{\tau}_m=\left(\cos \theta_m, \sin \theta_m\right) \quad \vec{n}_m=\left(-\sin \theta_m, \cos \theta_m\right) \\ & \vec{x}-\vec{x}_m=\left(x-x_m, y-y_m\right) \quad \vec{x}-\vec{x}_m=e_m\end{aligned}$$

• 横向误差可近似表示为：

$$e_d \approx\left(\vec{x}-\vec{x}_m\right) \cdot \vec{n}_m$$

• e_s为匹配点与投影点之间的弧长，（正代表投影在匹配点的前面，负代表投影在匹配点的后面）

$$e_s \approx\left(\vec{x}-\vec{x}_m\right) \cdot \vec{\tau}_m$$

• 参考横摆角
  $$\theta_r=\theta_m+k_m \cdot e_s$$

最终得到：
$$e_d =\left(\vec{x}-\vec{x}_m\right) \cdot \vec{n}_m\\ e_s =\left(\vec{x}-\vec{x}_m\right) \cdot \vec{\tau}_m\\ \theta_r=\theta_m+k_m \cdot e_s\\ k_r=k_m \\ \dot{s}=\frac{|\vec{v}| \cos \left(\theta-\theta_r\right)}{1-k e_d}\\ e_{\varphi}=\varphi-\theta_r\\ \dot{e_{\varphi}}=\dot{\varphi}-k \dot{s}\\ \dot{e_d}=|\vec{v}| \sin \left(\theta-\theta_r\right)\\$$
输出
$$e_d,\dot{e_d},{e_{\varphi}},\dot{e_{\varphi}},k_r$$
为了保证输出误差的可预见性，在误差计算模块后加入一个预测模块

3.1.4 预测

算法控制具有滞后性，加上预测模块，让算法有预见性，预测时间ts
$$\begin{aligned} & x_{\text {pre }}=x+v t_s \cos \theta=x+v t_s \cos (\beta+\varphi)=x+v_x t_s \cos \varphi-v_y t_s \sin \varphi \\ & y_{\text {pre }}=y+v t_s \sin \theta=y+v_y t_s \cos \varphi+v_x t_s \sin \varphi \\ & \varphi_{\text {pre }}=\varphi+\dot{\varphi} t_s \\ & v_{\text {xpre }}=v_x\\ & v_{\text {ypre }}=v_y\\ & \dot{\varphi}_{\text {pre }}=\dot{\varphi} \end{aligned}$$

3.1.5 前馈计算

$$\delta_f=k\left[l_f+l_r-l_r k_3-\frac{m v_x^2}{l_f+l_r}\left(\frac{l_r}{c_{f}}+\frac{l_f}{c_{r}} k_3-\frac{l_f}{c_{r}}\right)\right]$$

3.1.6 最终控制器形式

$$u=-K e_r+\delta_f$$

3.2 仿真实现

3.2.1环境配置：

关闭carsim风阻：

根据算法框图确定输入输出数据：

设定参考轨迹

r = 20; w = 50; count = 100;
[xr, yr, thetar, kr] = motion_path(r, w, 2*r, count);
scatter(xr,yr);
axis equal;

function [xr, yr, thetar, kr] = motion_path(r, w, h, count)
    % 长方形路径1
    xc1 = linspace(0, w, count);
    yc1 = ones(1, count)*0;
    angle1 = zeros(1, count);
    kappa1 = zeros(1, count);
    
        
    % 半圆形路径1
    theta2 = linspace( -pi/2,pi/2, count+2);
    xc2 = r*cos(theta2(2:end-1)) + w;
    yc2 = r*sin(theta2(2:end-1)) + r;
    angle2 = theta2(2:end-1)+ pi/2;
    kappa2 = ones(1, count)*(1/r);
    
     % 长方形路径2
    xc3 = linspace(w, 0, count);
    yc3 = ones(1, count)*2*r;
    angle3 = ones(1, count)*pi;
    kappa3 = zeros(1, count);
    
    % 半圆形路径2
    theta4 = linspace(pi/2,3*pi/2, count+2);
    xc4 = r*cos(theta4(2:end-1));
    yc4 = r*sin(theta4(2:end-1)) + r;
    angle4 = theta4(2:end-1) + pi/2;
    kappa4 = ones(1, count)*(1/r);
    
    % 拼接路径
    xr = [xc1, xc2, xc3, xc4];
    yr = [yc1, yc2, yc3, yc4];
    thetar = [angle1, angle2, angle3, angle4];
    kr = [kappa1, kappa2, kappa3, kappa4];
end

将参考轨迹添加到carsim

得到carsim中的参考轨迹

3.2.2搭建simulink模型：

预测模块：

function [pre_x,pre_y,pre_phi,pre_vx,pre_vy,pre_phi_dot] = fcn(x,y,phi,vx,vy,phi_dot,ts)
    pre_x=x+vx*ts*cos(phi)-vy*ts*sin(phi);
    pre_y=y+vy*ts*cos(phi)+vx*ts*sin(phi);
    pre_phi=phi+phi_dot*ts;
    pre_vx=vx;
    pre_vy=vy;
    pre_phi_dot=phi_dot;
end

误差计算模块：

function [kr,err] = fcn(x,y,phi,vx,vy,phi_dot,xr,yr,thetar,kappar)
    n=length(xr);
    d_min=(x-xr(1))^2+(y-yr(1))^2;
    min=1;
    for i=1:n
        d=(x-xr(i))^2+(y-yr(i))^2;
        if d<d_min
            d_min=d;
            min=i;
        end
    end
    dmin=min;
    tor=[cos(thetar(dmin));sin(thetar(dmin))];
    nor=[-sin(thetar(dmin));cos(thetar(dmin))];
    d_err=[x-xr(dmin);y-yr(dmin)];
    ed=nor'*d_err;
    es=tor'*d_err;

    projection_point_thetar=thetar(dmin)+kappar(dmin)*es;
    ed_dot=vy*cos(phi-projection_point_thetar)+vx*sin(phi-projection_point_thetar);

    ephi=sin(phi-projection_point_thetar);

    s_dot=vx*cos(phi-projection_point_thetar)-vy*sin(phi-projection_point_thetar);
    s_dot=s_dot/(1-kappar(dmin)*ed);
    ephi_dot=phi_dot-kappar(dmin)*s_dot;
    kr=kappar(dmin);
    err=[ed;ed_dot;ephi;ephi_dot];
end

LQR计算模块：

离线进行LQR求解，生成K表，加快运行时的速度，需要提前运行，将k值保存在工作空间

cf=-110000;
cr=cf;
m=1412;
Iz=1536.7;
a=1.015;
b=2.910-1.015;
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*eye(4);
    R=10;
    k(i,:)=lqr(A,B,Q,R);
end
k1=k(:,1)';
k2=k(:,2)';
k3=k(:,3)';
k4=k(:,4)';

调用k表

function k  = fcn(k1,k2,k3,k4,vx)
    if abs(vx)<0.01
        k=[0,0,0,0];
    else
        index=round(vx/0.01);
        k=[k1(index),k2(index),k3(index),k4(index)];
    end
end

前馈计算模块

function forword_angle = fcn(vx,lf,lr,m,cf,cr,k,kr)
    forword_angle=kr*(lf+lr-lr*k(3)-(m*vx*vx/(lf+lr))*((lr/cf)+(lf/cr)*k(3)-(lf/cr)));
end

最终角度输出计算：

function angle = fcn(k,err,forword_angle)
    angle=-k*err+forword_angle;
end

3.2.3最终实现效果

参考

本文主要参考B站UP【忠厚老实的老王】讲解视频