【自动驾驶】控制算法（八）横向控制Ⅲ | 代码与模型

写在前面：
🌟 欢迎光临 清流君 的博客小天地，这里是我分享技术与心得的温馨角落。📝
🎭 人生如戏，我们并非能选择舞台和剧本，但我们可以选择如何演绎 🌟感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行~~~

---

文章目录

• 引言
• 一、控制算法编程基础
• 二、Carsim配置

• 1、整车参数获取与侧偏刚度计算
• 2、车辆状态与规划轨迹信息

• 三、路径规划与轨迹生成

• 1、路径规划代码概述
• 2、直线与圆弧子函数详解

• (1)直线函数使用方法
• (2)圆弧函数使用方法

• 3、路径拼接实例
• 4、Carsim路面设置

• 四、模型搭建与仿真运行

• 1、变量准备
• 2、预测模块实现
• 3、误差和曲率计算模块实现
• 4、AB模块 + LQR模块
• 5、前馈模块实现
• 6、避免代数环的延迟模块
• 7、仿真测试

• 五、总结
• 参考资料

---

引言

  本篇博客是 自动驾驶控制算法 系列的第八节第Ⅲ部分。内容整理自 B站知名up主 忠厚老实的老王 的视频，作为博主的学习笔记，分享给大家共同学习。

  本篇博客是最长的一节，主要讲解如何搭建模型。

  大家有需要可以自己下载相关的代码模型，主页链接如下：

   自动驾驶决策规划算法代码模型

---

一、控制算法编程基础

  在开始本节之前，首先要把第八节的第二部分 Carsim 设置给设置完毕。具体操作见第八节第Ⅱ部分：

  【自动驾驶】控制算法(八)横向控制Ⅱ | Carsim 与 Matlab 联合仿真基本操作

  在开始本节之前，首先要有算法准备，就是第八讲第一节的编程基础内容：

  【自动驾驶】控制算法(八)横向控制Ⅰ | 算法与流程

  编程就是照着算法流程图编写程序：

这是整个编程的核心，横向控制算法主要需要三类参数：

• 整车参数
• 车辆当前状态
• 规划轨迹信息

---

二、Carsim配置

1、整车参数获取与侧偏刚度计算

、质心到前后轮的距离

、

以及惯量

注意：质量只是弹簧上质量，不包括悬架质量，所以

  计算侧偏刚度，先大概估计四轮垂向力的大小，在表上查垂向力对应的曲线，求斜率。

注意：侧偏刚度的单位是

，且为负值。

，因为是自行车模型，把两个轮胎并成一个轮胎。在这里算出来的侧偏刚度是正的，但是当输入进软件里时，侧偏刚度是负的。

注意：把风阻、空气动力学关掉。

  暂时不考虑风阻，先简单再复杂，把空气动力学关掉。

  其实风阻在控制中是很要命的因素，特别是在高速时，空气阻力会极大影响车轮的垂向力，垂向力变化，侧偏刚度就变了，导致 LQR 相关东西全都变了，所以风阻是很重要的因素，但如果讲风阻就太复杂了，先暂时先不考虑风阻，等以后学到时再去考虑风阻。

2、车辆状态与规划轨迹信息

。所以 Carsim 的输出中要添加

，规划轨迹的横纵坐标、规划轨迹的切线与

  所以做控制时不仅仅写控制算法，还要写规划算法，先把模型的框架搭建立起来，点Send to Simulink。

---

三、路径规划与轨迹生成

1、路径规划代码概述

  规划代码不是这一节的重点，所以规划代码已经事先写好了，看起来挺复杂，实际上就写了两个子函数，在设计路径时，调用这两个子函数把拼起来就可以了。规划代码如下：

routing_planning.m

登录后复制

count=50;
[x1,y1,theta1,kr1]=straight([0,0],[20,0],0,count);
[x2,y2,theta2,kr2]=arc([20,0],[30,10],0,pi/2,count);
[x3,y3,theta3,kr3]=arc([30,10],[40,20],pi/2,0,count);
[x4,y4,theta4,kr4]=arc([40,20],[40,40],0,pi,count);
[x5,y5,theta5,kr5]=arc([40,40],[35,35],pi,3*pi/2,count);
[x6,y6,theta6,kr6]=arc([35,35],[25,35],3*pi/2,pi/2,count);
[x7,y7,theta7,kr7]=arc([25,35],[15,35],pi/2,3*pi/2,count);
[x8,y8,theta8,kr8]=arc([15,35],[5,35],3*pi/2,pi/2,count);
[x9,y9,theta9,kr9]=arc([5,35],[-15,35],pi/2,3*pi/2,count);
[x10,y10,theta10,kr10]=straight([-15,35],[-15,15],3*pi/2,count);
[x11,y11,theta11,kr11]=arc([-15,15],[0,0],3*pi/2,2*pi,count);
xr=[x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11];
yr=[y1,y2,y3,y4,y5,y6,y7,y8,y9,y10,y11];
thetar=[theta1,theta2,theta3,theta4,theta5,theta6,theta7,theta8,theta9,theta10,theta11];
kappar=[kr1,kr2,kr3,kr4,kr5,kr6,kr7,kr8,kr9,kr10,kr11];

scatter(xr,yr);

function[xr,yr,thetar,kr]=straight(init_coord,end_coord,init_angle,count)
delta_x=(end_coord(1)-init_coord(1))/(count-1);
delta_y=(end_coord(2)-init_coord(2))/(count-1);
for i=1:count
    xr(i)=init_coord(1)+delta_x*i;
    yr(i)=init_coord(2)+delta_y*i;
    thetar(i)=init_angle;
    kr(i)=0;
end      
end

function[xr,yr,thetar,kr]=arc(init_coord,end_coord,init_angle,end_angle,count)
    L=sqrt((init_coord(1)-end_coord(1))^2+(init_coord(2)-end_coord(2))^2);
    R=L/sqrt(2*(1-cos(end_angle-init_angle)));
    delta_angle=(end_angle-init_angle)/(count-1) ;
  
       for i=1:count
           if delta_angle>0
               xr(i)=init_coord(1)-R*sin(init_angle)+R*sin(init_angle+delta_angle*(i-1));
               yr(i)=init_coord(2)+R*cos(init_angle)-R*cos(init_angle+delta_angle*(i-1));
               thetar(i)=init_angle+delta_angle*i;
               kr(i)=1/R;
           else
               xr(i)=init_coord(1)+R*sin(init_angle)-R*sin(init_angle+delta_angle*(i-1));

               yr(i)=init_coord(2)-R*cos(init_angle)+R*cos(init_angle+delta_angle*(i-1));
               thetar(i)=init_angle+delta_angle*i;
               kr(i)=-1/R;
           end               
       end  
end

• 1.
• 2.
• 3.
• 4.
• 5.
• 6.
• 7.
• 8.
• 9.
• 10.
• 11.
• 12.
• 13.
• 14.
• 15.
• 16.
• 17.
• 18.
• 19.
• 20.
• 21.
• 22.
• 23.
• 24.
• 25.
• 26.
• 27.
• 28.
• 29.
• 30.
• 31.
• 32.
• 33.
• 34.
• 35.
• 36.
• 37.
• 38.
• 39.
• 40.
• 41.
• 42.
• 43.
• 44.
• 45.
• 46.
• 47.
• 48.
• 49.
• 50.

2、直线与圆弧子函数详解

  具体讲一下这两个子函数该怎么用，这两个子函数的名字是直线、圆弧。代码上半部分的内容是拼起来的路径，也就是要用的路径，暂时先注释掉。

(1)直线函数使用方法

，终点是

，角度是

，只需要直线函数就可以：

登录后复制

[xr,yr,~,~] = straight([0,0],[50,50],pi/4,10);

• 1.

，不过不需要的话就用 ~ 占位，生成这样一条离散路径点：

(2)圆弧函数使用方法

登录后复制

[xr,yr,~,~] = arc([0,0],[50,50],0,pi/2,100);

• 1.

，终点角度是

，运行结果如下：

改成

，同样终点坐标要从

变成

，这样就变成右转圆弧。还可以修改点数量。

3、路径拼接实例

  本篇博客所演示的路径比较复杂，各种直线和圆弧拼在一起，具体路径如下图所示：

个点组成，比较复杂。这里的路径规划其实设计的很不合理，因为好的路径规划要保证曲率连续，但是本路径没有做到，就是简单的直线和圆弧的拼接，直线的曲率是

，圆弧的曲率是

，也就是曲率半径的倒数。

  要设计比较好的路径，需要在直线和圆弧中间用回旋线做过渡，让曲率连续变化。但是因为规划并不是本篇博客的重点，所以就凑合用一用， count 是控制点的数量，选择点多一点的，这样控制效果相对会好一点。

4、Carsim路面设置

  为了看起来更直观，把 Carsim 的路面也设置一下。

  点Procedure，在 3D Road里面选

  在车道这里选 two lines，再点 stretch east，在这里根据规划设计道路走向。

行这样的表，点右下角有 Rows输入

，就会有

行表格。表格的参数按上图填写即可， straight 代表直线， radius 代表圆弧。

  圆弧转过的角度，即左转右转和规划的符号不一样，左转默认半径为正，右转默认半径为负。在规划代码里，若终点角度如果大于起点角度，就是左转；反之就是右转。在这里是根据半径来的，半径为负，就是右转；半径是正，就是左转。一定要把 Treat as loop 勾上，变成闭合曲线处理。

，而且在上面有连续五米的转弯半径，实际上对车辆的通过能力是很大的考验，因为转弯半径特别小，如果不是倒车，转弯半径一般最低也就

。

的转弯半径已经特别陡了，所以设计的路径比较严苛，创建的道路如下图所示：

  有很多连续弯道，路况算比较严苛。

---

四、模型搭建与仿真运行

1、变量准备

  先跑一遍路径规划，就是Matlab变量工作区里要有路径才可以：

  打开 Simulink 看一下控制模型，先做单位换算，把千米每小时换成米每秒，以及把角度换成弧度。建立 Matlab Function 模块，在里面编程。

  这六个模块从逻辑顺序来上来说，从左往右写比较好，因为左边的输出是右边的输入，所以先写左边，再写右边。因此先写预测模块，在写代码之前先写上名字，因为模型比较复杂，先标上记号，要不然可能不知道哪根线对应哪个变量。

2、预测模块实现

，输出为预测后的

。照着预测模块的算法写，预测时间随便给个

predict_module

登录后复制

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

• 1.
• 2.
• 3.
• 4.
• 5.
• 6.
• 7.
• 8.

3、误差和曲率计算模块实现

  计算模块需要车辆状态，还需要规划轨迹信息。所以规划要先把它写好，而且写好之后也必须跑一遍，规划代码和控制代码是独立的。所以在规划时，需要先把代码跑一下，在工作区里要有这些规划的变量。

  在工作区里有这些变量后，才能传到 Simlink 模型里，

注意：

，以及规划的

err_kappa_calculate_module

登录后复制

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;%把最短距离设为真实位置与第1个点。可以不用开平方根，减少计算量，因为并不关心距离是多少，只想找到最短点的序号
    min=1;%先把最短点的序号设为1
    for i=1:n%遍历所有的点
        d=(x-xr(i))^2+(y-yr(i))^2;%计算它与真实的位置的之间的距离
        if d<d_min%如果能找到比所设置的最短距离还要短
            d_min=d;%将值赋为最短距离
            min=i;%把序点的序号赋给min
        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);%apollo
    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);%因为角度具有多值性，使用sin来消除因为多2pi或者少2pi而导致的算法出错
    %%%%%%%%%
    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

• 1.
• 2.
• 3.
• 4.
• 5.
• 6.
• 7.
• 8.
• 9.
• 10.
• 11.
• 12.
• 13.
• 14.
• 15.
• 16.
• 17.
• 18.
• 19.
• 20.
• 21.
• 22.
• 23.
• 24.
• 25.
• 26.
• 27.
• 28.
• 29.
• 30.

，用 From Workspace 模块，从工作区里把变量导入到 Simlink 里必须要工作区里有

注意： From workspace 只能导行向量，能导列向量、不能导矩阵，也不能导结构体。实际上通过技巧操作可以导矩阵以及列向量，但省事点尽量变成行向量就可以了。

4、AB模块 + LQR模块

  本来 LQR 模块应该最难写，也最复杂，但是很幸运 Matlab 自己有 LQR 模块包。用离线LQR，所以 LQR 变成了最好写的模块，而且 LQR 模块和 AB 计算模块是紧密联系的，所以把 AB 计算模块和 LQR 模块合在一起写。

  先建立脚本文件，作为LQR离线查表的数据，代码如下：

lqr_offline.m

登录后复制

cf=-110000;%侧偏刚度根据曲线估算
cr=cf;%严格来说 CR 和 CF 应该不一样，但差别不大，所以近似认为一样
m=1412;%根据Carsim把整车参数输进去
Iz=1536.7;
a=1.015;
b=2.910-1.015;
k=zeros(5000,4);%申请矩阵k，用来存放LQR的k
for i=1:5000
    vx=0.01*i;%只有vx是变量，所以每隔0.01米每秒，算一下对应的k。
    %这样的话就可涵盖v从 0.01 米每秒到 v 等于 50 米每秒这么大范围内所有的 k 
    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)';%建立四个变量，K1、K2、K3、K4，把表记录下来
k2=k(:,2)';%因为Simlink不能传矩阵，只能传行向量，需要做个转置
k3=k(:,3)';
k4=k(:,4)';

• 1.
• 2.
• 3.
• 4.
• 5.
• 6.
• 7.
• 8.
• 9.
• 10.
• 11.
• 12.
• 13.
• 14.
• 15.
• 16.
• 17.
• 18.
• 19.
• 20.
• 21.
• 22.
• 23.
• 24.
• 25.
• 26.

秒之外，从

开始到

，再从

到想要的速度，但如果

，代到

矩阵里，

就变成奇异阵了，里面有无穷大。所以在

情况下的

，要进行另外处理，不能用LQR算，后面再讲。

是由

如何查表找和

很有规律，从

一直增大，所以第一行

必然对应

， 第

行对应

，只要让

除以

这样的小数，对应哪个

，用速度除

，除出来是小数而不是整数，那怎么办呢？

秒是非常小的间隔，在两个之间的

以及

五个变量决定，把

输入进去，除以

，做四舍五入，看和表的哪组

对应，就把对应的

  离线 LQR 算法程序代码如下：

登录后复制

function k  = fcn(k1,k2,k3,k4,vx)
    if abs(vx)<0.01%特殊处理如果 v 的绝对值小于0.01，
        k=[0,0,0,0];%直接令 k 就等于0，它是航向量
    else
        index=round(vx/0.01);%否则的话就用 v x 除001，再用round四舍五入，值就是所要的匹配的序号
        k=[k1(index),k2(index),k3(index),k4(index)];
    end
end

• 1.
• 2.
• 3.
• 4.
• 5.
• 6.
• 7.
• 8.

设置成了单位矩阵，

取

，可以设置不同的

和

，算出

来，再把

导进去。所以 LQR 和规划算法一样，得先运行跑一遍，让工作区 Workspace 里有

，才能进行下一步的 Simulink 仿真。

LQR中的权重矩阵

和

只要记住两句话即可：

• 
  
  控制量
  
  可能会特别大，比如可能算出来
  
  要达到
  
• 
  
  过程比较平缓比较重要，因为要保证平缓、舒适性、安全性。

和

权重的权衡互相矛盾，要具体调合适的

和

，期望达到的效果是能很快跟踪上路径，并且控制量

  模块写好之后，用 Simlink 拼起来即可。

5、前馈模块实现

以及投影点的曲率

。

forward_control

登录后复制

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

• 1.
• 2.
• 3.

  最后把所有结果整合起来即可。

last_angle

登录后复制

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

• 1.
• 2.
• 3.

  这样所有代码就都写完了，把它打个包封装成子系统，看起来整洁一点。

  最后算出来前轮转角，再反馈给 Carsim 的前轮转角即可。

注意：单位换算弧度，要化成角度才能输入到 Carsim， Carsim 根据角度控制，不是根据弧度控制的。

6、避免代数环的延迟模块

  在算出角度的线上再加延迟模块，作用是如果输入是这一时刻的值，输出是上时刻的值，它就等于延迟单元。这是为了避免代数环，即输入直接决定输出，输出又直接决定输入，会导致套娃式循环。所以为了打破循环就要加延迟模块来避免代数环。

作为输入才能算出角度，

又需要角度作为输入打方向盘，

7、仿真测试

，节气门开度给

，也就是给一点点油门，模型只做了横向控制，控制方向盘转角，并没有做纵向控制。纵向控制就是控制油门的节气门开度，后续再讲，所以说模型只搭了一半，在这里并不控制纵向速度，就给它恒定油门

，运行程序看看效果，用

  可能觉得是个椭圆而不是圆，这是因为横坐标和纵坐标间隔不完全一样，横坐标要宽一点，纵坐标窄一点，所以看起来不太像是圆，而像是椭圆，实际上应该是个圆。

  看一下算出来的前端转角，其实是不太好的，有的地方在不停鬼畜，不停地抖来抖去，其实不是一件好事情。

  在有的地方突变太厉害，突然猛的打一下方向盘，在转弯时打得非常快。算法写出来有利有弊，方向盘打的太快了，而且方向盘在转弯时，不停的鬼畜抖来抖去，这是无法接受的。

  但是也有好的地方，至少控制的跟踪效果做出来了，确实是按照所规划的轨迹在跑，而且规划的轨迹是比较严苛的。

  转弯半径很小，而且有很多很急的弯，结果也算勉强能跟上，在没有做纵向控制且在不停加速的情况下，仍然勉强能跟上规划的轨迹，所以有一定的控制效果，但舒适性并不是特别好，方向盘在不停抖来抖去。

为什么会导致方向盘在鬼畜？而且在转弯时，方向盘会猛的突变？

  这是有原因的，在下一节会讲算法性能不好的原因以及怎么改进。

---

五、总结

  本节博客详细介绍了自动驾驶控制算法的建模过程，涵盖了从Carsim设置到模型搭建的各个方面。通过配置Carsim软件，获取整车参数并计算侧偏刚度，为后续的控制算法提供基础数据。

  通过编写路径规划代码，生成所需的轨迹信息。在Matlab中，搭建了控制模型的各个模块，包括预测模块、误差和曲率计算模块、AB模块和LQR模块、前馈控制模块等。

  最后，将模型整合并进行了仿真测试，展示了控制算法对规划轨迹的跟踪效果。

  大家不妨在这一节照着思路把模型搭好，在下一节就会告诉各位怎么调模型，使其更平顺。欢迎关注后续内容！

---

参考资料

   【基础】自动驾驶控制算法第八讲(三) 代码与模型

---

后记：

🌟 感谢您耐心阅读这篇关于 自动驾驶横向控制算法代码与模型 的技术博客。 📚
🎯 如果您觉得这篇博客对您有所帮助，请不要吝啬您的点赞和评论 📢
🌟您的支持是我继续创作的动力。同时，别忘了收藏本篇博客，以便日后随时查阅。🚀
🚗 让我们一起期待更多的技术分享，共同探索移动机器人的无限可能！💡
🎭感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行 🚀