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-转动半径

根据正弦定理：
sin⁡(��−�)ℓ�=sin⁡(�2−��)�sin⁡(�−��)ℓ�=sin⁡(�2+��)�ℓf​sin(δf​−β)​=Rsin(2π​−δf​)​ℓr​sin(β−δr​)​=Rsin(2π​+δr​)​
展开有：
sin⁡(��)cos⁡(�)−sin⁡(�)cos⁡(��)ℓ�=cos⁡(��)�cos⁡(��)sin⁡(�)−cos⁡(�)sin⁡(��)ℓ�=cos⁡(��)�​ℓf​sin(δf​)cos(β)−sin(β)cos(δf​)​=Rcos(δf​)​ℓr​cos(δr​)sin(β)−cos(β)sin(δr​)​=Rcos(δr​)​​

$$
\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}
$$

上式相加有：
(tan⁡(��)−tan⁡(��))cos⁡(�)=��+���(tan(δf​)−tan(δr​))cos(β)=Rlf​+lr​​
低速行驶下，车辆的方向变化速率小于车辆的角速度
�˙≈�=��=�⋅1�=�⋅(tan⁡(��)−tan⁡(��))cos⁡(�)��+��=�cos⁡(�)��+��(tan⁡(��)−tan⁡(��))ψ˙​≈r=RV​=V⋅R1​​=V⋅lf​+lr​(tan(δf​)−tan(δr​))cos(β)​=lf​+lr​Vcos(β)​(tan(δf​)−tan(δr​))​

质心运动学模型

X轴方向：
�˙=�cos⁡(�+�)X˙=Vcos(ψ+β)
Y轴方向：
�˙=�sin⁡(�+�)Y˙=Vsin(ψ+β)
yaw轴方向：
�˙=�cos⁡(�)��+��(tan⁡��−tan⁡��)ψ˙​=lf​+lr​Vcos(β)​(tanδf​−tanδr​)
β角：
�=tan⁡−1(��tan⁡��+��tan⁡����+��)β=tan−1(lf​+lr​lf​tanδr​+lr​tanδf​​)

动力学模型

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

根据牛顿第二定律有：

��=(d2� d�2)inertial =�˙�（y方向）+���˙（向心加速度）���+��Γ=���=�(�˙�+���˙)y方向受力�����−�����=���¨扭矩平衡​ay​=( dt2d2y​)inertial ​=v˙y​（y方向）+vx​ψ˙​（向心加速度）Fyf​+FyΓ​​=may​=m(v˙y​+vx​ψ˙​)y方向受力lf​Fyf​−lr​Fyr​=Iz​ψ¨​扭矩平衡​

轮胎的受力分析

轮胎的侧向力���Fyf​,���Fyr​,这里采用右手坐标系，所以���Fyf​,���Fyr​都是负数：

侧向力对动力学的影响：

前轮转角：�δ,前轮速度转角���θvf​,前轮侧偏角角��αf​

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

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

得到：
���=2����=2��(�−���)���=2����=2��(−���)Fyf​=2cf​αf​=2cf​(δ−θvf​)Fyr​=2cr​αr​=2cr​(−θvr​)​
将速度进行两个方向的分解：
���=tan⁡−1(������)=tan⁡−1(��+�����)���=tan⁡−1(������)=tan⁡−1(��−�����)​θvf​=tan−1(vfx​vfy​​)=tan−1(vx​vy​+lf​r​)θvr​=tan−1(vrx​vry​​)=tan−1(vx​vy​−lr​r​)​
根据动力学模型的受力情况：
{���′+�′�Γ=���=�(�˙�+���˙)�����′−�����′=���¨⇓(���cos⁡(�)−���sin⁡(�))+���=�(�˙�+���)ℓ�(���cos⁡(�)−���sin⁡(�))−ℓ����=���¨=���˙​{Fyf′​+F′yΓ​=may​=m(v˙y​+vx​ψ˙​)lf​Fyf′​−lr​Fyr′​=Iz​ψ¨​​⇓(Fyf​cos(δ)−Fxf​sin(δ))+Fyr​=m(v˙y​+vx​r)ℓf​(Fyf​cos(δ)−Fxf​sin(δ))−ℓr​Fyr​=Iz​ψ¨​=Iz​r˙​
侧偏角：
{��=�−tan⁡−1(��+ℓ����)��=−tan⁡−1(��−ℓ����)⎩⎨⎧​αf​=δ−tan−1(vx​vy​+ℓf​r​)αr​=−tan−1(vx​vy​−ℓr​r​)​
侧向力：
{���=c���=c�[�−tan⁡−1(��+ℓ����)]���=����=−��tan⁡−1(��−ℓ����)⎩⎨⎧​Fyf​=cf​αf​=cf​[δ−tan−1(vx​vy​+ℓf​r​)]Fyr​=cr​αr​=−cr​tan−1(vx​vy​−ℓr​r​)​
得到：
�˙�=��[�−tan⁡−1(��+ℓ����)]cos⁡(�)−��tan⁡−1(��−ℓ����)−���sin⁡(�)�−����˙=ℓ���[�−tan⁡−1(��+ℓ����)]cos⁡(�)+ℓ���tan⁡−1(��−ℓ����)−�����sin⁡(�)��​v˙y​=mcf​[δ−tan−1(vx​vy​+ℓf​r​)]cos(δ)−cr​tan−1(vx​vy​−ℓr​r​)−Fxf​sin(δ)​−vx​rr˙=Iz​ℓf​cf​[δ−tan−1(vx​vy​+ℓf​r​)]cos(δ)+ℓr​cr​tan−1(vx​vy​−ℓr​r​)−lf​Fxf​sin(δ)​​

3 LQR汽车轨迹跟踪控制

3.1算法框图：

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

当转角很小时：
cos⁡(�)≈1sin⁡(�)≈0tan⁡−1(�)≈�​cos(δ)≈1sin(δ)≈0tan−1(θ)≈θ​
带入得到：
�˙�=−����−��ℓ�����+����+−����+��ℓ�����−����˙=−ℓ�����−ℓ�2�������+ℓ������+ℓ�����−ℓ�2�������​v˙y​=mvx​−cf​vy​−cf​ℓf​r​+mcf​δ​+mvx​−cr​vy​+cr​ℓr​r​−vx​rr˙=Iz​vx​−ℓf​cf​vy​−ℓf2​cf​r​+Iz​ℓf​cf​δ​+Iz​vx​ℓr​cr​vy​−ℓr2​cr​r​​
写成状态空间方程：

取状态：
�=[���]X=[vy​r​]

�=�u=δ

写成关于状态的方程：
�˙�=−(��+��)�����+[(����−����)���−��]�+�����˙=����−����������+−(ℓ�2��+ℓ�2��)�����+�������​v˙y​=mvx​−(cf​+cr​)​vy​+[mvx​(lr​cr​−lf​cf​)​−vx​]r+mcf​​δr˙=Iz​vx​lr​cr​−lf​cf​​vy​+Iz​vx​−(ℓf2​cf​+ℓr2​cr​)​r+Iz​lf​cf​​δ​
得到：
[���˙]=[−(��+��)���(����−����)���−������−��������−(ℓ�2��+ℓ�2��)����][���]+[���������]�[vy​r˙​]=[mvx​−(cf​+cr​)​Iz​vx​lr​cr​−lf​cf​​​mvx​(lr​cr​−lf​cf​)​−vx​Iz​vx​−(ℓf2​cf​+ℓr2​cr​)​​][vy​r​]+[mcf​​Iz​lf​cf​​​]δ
如果取状态，输入:
�=[��˙��˙],u=�X=​yy˙​ψψ˙​​​,u=δ
有：
���[��˙��˙]=[01000−(��+��)���0(����−����)���−��00010����−��������0−(ℓ�2��+ℓ�2��)����][��˙��˙]+[0���0������]�dtd​​yy˙​ψψ˙​​​=​0000​1mvx​−(cf​+cr​)​0Iz​vx​lr​cr​−lf​cf​​​0000​0mvx​(lr​cr​−lf​cf​)​−vx​1Iz​vx​−(ℓf2​cf​+ℓr2​cr​)​​​​yy˙​ψψ˙​​​+​0mcf​​0Iz​lf​cf​​​​δ

�=[01000−(��+��)���0(����−����)���−��00010����−��������0−(ℓ�2��+ℓ�2��)����]A=​0000​1mvx​−(cf​+cr​)​0Iz​vx​lr​cr​−lf​cf​​​0000​0mvx​(lr​cr​−lf​cf​)​−vx​1Iz​vx​−(ℓf2​cf​+ℓr2​cr​)​​​

�=[0���0������]B=​0mcf​​0Iz​lf​cf​​​​

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

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

离散系统：
��+1=���+���,�0=�init xt+1​=Axt​+But​,x0​=xinit ​
离散LQR代价函数：
�(�)=∑�=0�−1(������+������)+�������J(U)=τ=0∑N−1​(xτT​Qxτ​+uτT​Ruτ​)+xNT​Qf​xN​
K=dlqr(A,B,Q,R)

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

��=−(�+����+1�)−1����+1�Kt​=−(R+BTPt+1​B)−1BTPt+1​A
其中
��−1=�+����+1�−����+1�(�+����+1�)−1����+1�Pt−1​=Q+ATPt+1​A−ATPt+1​B(R+BTPt+1​B)−1BTPt+1​A

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

横向位置误差：
��=(�⃗−�⃗�)⋅�⃗�ed​=(x−xr​)⋅nr​
横向速度误差：
��˙=∣�⃗∣sin⁡(�−��)ed​˙​=∣v∣sin(θ−θr​)
角度误差：
��=�−��eφ​=φ−θr
角速度误差：
��˙=�˙−��˙eφ​˙​=φ˙​−ks˙
其中
��˙=��˙θr​˙​=ks˙
k为曲率
�˙=∣�⃗∣cos⁡(�−��)1−���s˙=1−ked​∣v∣cos(θ−θr​)​
对于离散轨迹：

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

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

�⃗�=(cos⁡��,sin⁡��)�⃗�=(−sin⁡��,cos⁡��)�⃗−�⃗�=(�−��,�−��)�⃗−�⃗�=��​τm​=(cosθm​,sinθm​)nm​=(−sinθm​,cosθm​)x−xm​=(x−xm​,y−ym​)x−xm​=em​​

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

��≈(�⃗−�⃗�)⋅�⃗�ed​≈(x−xm​)⋅nm​

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

��≈(�⃗−�⃗�)⋅�⃗�es​≈(x−xm​)⋅τm​

• 参考横摆角
  ��=��+��⋅��θr​=θm​+km​⋅es​

最终得到：
��=(�⃗−�⃗�)⋅�⃗���=(�⃗−�⃗�)⋅�⃗���=��+��⋅����=���˙=∣�⃗∣cos⁡(�−��)1−�����=�−����˙=�˙−��˙��˙=∣�⃗∣sin⁡(�−��)ed​=(x−xm​)⋅nm​es​=(x−xm​)⋅τm​θr​=θm​+km​⋅es​kr​=km​s˙=1−ked​∣v∣cos(θ−θr​)​eφ​=φ−θr​eφ​˙​=φ˙​−ks˙ed​˙​=∣v∣sin(θ−θr​)
输出
��,��˙,��,��˙,��ed​,ed​˙​,eφ​,eφ​˙​,kr​
为了保证输出误差的可预见性，在误差计算模块后加入一个预测模块

3.1.4 预测

算法控制具有滞后性，加上预测模块，让算法有预见性，预测时间ts
�pre =�+���cos⁡�=�+���cos⁡(�+�)=�+����cos⁡�−����sin⁡��pre =�+���sin⁡�=�+����cos⁡�+����sin⁡��pre =�+�˙���xpre =���ypre =���˙pre =�˙​xpre ​=x+vts​cosθ=x+vts​cos(β+φ)=x+vx​ts​cosφ−vy​ts​sinφypre ​=y+vts​sinθ=y+vy​ts​cosφ+vx​ts​sinφφpre ​=φ+φ˙​ts​vxpre ​=vx​vypre ​=vy​φ˙​pre ​=φ˙​​

3.1.5 前馈计算

��=�[��+��−���3−���2��+��(����+�����3−����)]δf​=k[lf​+lr​−lr​k3​−lf​+lr​mvx2​​(cf​lr​​+cr​lf​​k3​−cr​lf​​)]

3.1.6 最终控制器形式

�=−���+��u=−Ker​+δ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【忠厚老实的老王】讲解视频