写在前面：
🌟 欢迎光临 清流君 的博客小天地，这里是我分享技术与心得的温馨角落 。📝
个人主页 ：清流君_CSDN博客，期待与您一同探索 移动机器人 领域的无限可能。🔍 本文系 清流君 原创之作，荣幸在CSDN首发🐒
若您觉得内容有价值，还请评论告知一声，以便更多人受益。
转载请注明出处，尊重原创，从我做起。👍 点赞、评论、收藏 ，三连走一波，让我们一起养成好习惯😜
在这里，您将收获的不只是技术干货 ，还有思维的火花！📚 系列专栏：【运动控制】系列，带您深入浅出，领略控制之美。🖊
愿我的分享能为您带来启迪，如有不足，敬请指正，让我们共同学习，交流进步！🎭 人生如戏，我们并非能选择舞台和剧本，但我们可以选择如何演绎 🌟
感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行~~~
文章目录
- 引言
- 一、控制算法编程基础
- 二、Carsim配置
- 三、路径规划与轨迹生成
- 四、模型搭建与仿真运行
-
- 1、变量准备
- 2、预测模块实现
- 3、误差和曲率计算模块实现
- [4、AB模块 + LQR模块](#4、AB模块 + LQR模块)
- 5、前馈模块实现
- 6、避免代数环的延迟模块
- 7、仿真测试
- 五、总结
- 参考资料
引言
本篇博客是 自动驾驶控制算法 系列的第八节第Ⅲ部分。内容整理自 B站知名up主 忠厚老实的老王 的视频，作为博主的学习笔记，分享给大家共同学习。
本篇博客是最长的一节，主要讲解如何搭建模型。
大家有需要可以自己下载相关的代码模型，主页链接如下：
一、控制算法编程基础
在开始本节之前，首先要把第八节的第二部分 Carsim 设置给设置完毕。具体操作见第八节第Ⅱ部分：
【自动驾驶】控制算法（八）横向控制Ⅱ | Carsim 与 Matlab 联合仿真基本操作
在开始本节之前，首先要有算法准备，就是第八讲第一节的编程基础内容：
编程就是照着算法流程图编写程序：
这是整个编程的核心，横向控制算法主要需要三类参数：
- 整车参数
- 车辆当前状态
- 规划轨迹信息
二、Carsim配置
1、整车参数获取与侧偏刚度计算
整车参数可以在Carsim软件里读取，可得到质量 m m m、质心到前后轮的距离 a a a、 b b b 以及惯量 I I I 之类的数据。
注意：质量只是弹簧上质量，不包括悬架质量，所以
车辆总质量 = 簧上质量 + 前后两个悬架质量 车辆总质量=簧上质量+前后两个悬架质量 车辆总质量=簧上质量+前后两个悬架质量 其他参数在软件里可以读取到，包括根据曲线估算的侧偏刚度。侧偏刚度可以不用计算特别准，只要估算出大概值即可。
计算侧偏刚度，先大概估计四轮垂向力的大小，在表上查垂向力对应的曲线，求斜率。
注意 ：侧偏刚度的单位是 N / r a d N/rad N/rad，且为负值。
当输入时，算法是牛每弧度，要进行单位换算，并且所有侧偏刚度都要乘以 2 2 2，因为是自行车模型，把两个轮胎并成一个轮胎。在这里算出来的侧偏刚度是正的，但是当输入进软件里时，侧偏刚度是负的。
注意：把风阻、空气动力学关掉。
暂时不考虑风阻，先简单再复杂，把空气动力学关掉。
其实风阻在控制中是很要命的因素，特别是在高速时，空气阻力会极大影响车轮的垂向力，垂向力变化，侧偏刚度就变了，导致 LQR 相关东西全都变了，所以风阻是很重要的因素，但如果讲风阻就太复杂了，先暂时先不考虑风阻，等以后学到时再去考虑风阻。
2、车辆状态与规划轨迹信息
回到流程图，除了整车参数，算法还需要车辆状态，也就是 ( x , y , φ , v x , v y , φ ˙ ) (x,y,\varphi ,v_x,v_y,\dot{\varphi}) (x,y,φ,vx,vy,φ˙)。所以 Carsim 的输出中要添加 ( x , y , φ , v x , v y , φ ˙ ) (x,y,\varphi ,v_x,v_y,\dot{\varphi}) (x,y,φ,vx,vy,φ˙)
流程图的第三个输入是规划轨迹信息，包括 ( x r , y r , θ r , κ r ) \left( x_r,y_r,\theta _r,\kappa _r \right) (xr,yr,θr,κr)，规划轨迹的横纵坐标、规划轨迹的切线与 x x x 轴方向夹角、规划曲线的曲率。
所以做控制时不仅仅写控制算法，还要写规划算法，先把模型的框架搭建立起来，点Send to Simulink。
三、路径规划与轨迹生成
1、路径规划代码概述
规划代码不是这一节的重点，所以规划代码已经事先写好了，看起来挺复杂，实际上就写了两个子函数，在设计路径时，调用这两个子函数把拼起来就可以了。规划代码如下：
routing_planning.m
matlab
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
end2、直线与圆弧子函数详解
具体讲一下这两个子函数该怎么用，这两个子函数的名字是直线、圆弧。代码上半部分的内容是拼起来的路径，也就是要用的路径，暂时先注释掉。
(1)直线函数使用方法
这两个子函数用起来很简单，比如想生成一段直线，起点是 ( 0 , 0 ) (0,0) (0,0)，终点是 ( 5 , 0 ) (5,0) (5,0)，角度是 π 4 \frac{\pi}{4} 4π，只需要直线函数就可以：
matlab
[xr,yr,~,~] = straight([0,0],[50,50],pi/4,10);严格来说要写 θ r , κ r \theta_r,\kappa_r θr,κr，不过不需要的话就用 ~ 占位，生成这样一条离散路径点：
(2)圆弧函数使用方法
圆弧稍微复杂一点，需要输入起点坐标、终点坐标、起点角度、终点角度、点的数量。起点角度，终点角度表示点的切线方向与 x x x 轴的夹角。
matlab
[xr,yr,~,~] = arc([0,0],[50,50],0,pi/2,100);比如这里起点角度是 0 ° 0° 0°，终点角度是 π 2 \frac{\pi}{2} 2π，运行结果如下：
此圆弧是典型左转圆弧，可以修改参数，比如把 π 2 \frac{\pi}{2} 2π 改成 − π 2 -\frac{\pi}{2} −2π，同样终点坐标要从 ( 50 , 50 ) (50,50) (50,50) 变成 ( 50 , − 50 ) (50,- 50) (50,−50)，这样就变成右转圆弧。还可以修改点数量。
这两个子函数的作用就是给路径形状，能把 x r , y r , θ r , κ r x_r,y_r,\theta_r,\kappa_r xr,yr,θr,κr 直接算出来，很方便，在设计路径时，只要调用这两个接口，拼起来就可以了。可以自己设计这样的路径出来。
3、路径拼接实例
本篇博客所演示的路径比较复杂，各种直线和圆弧拼在一起，具体路径如下图所示：
每一段直线和圆弧都由 50 50 50 个点组成，比较复杂。这里的路径规划其实设计的很不合理，因为好的路径规划要保证曲率连续，但是本路径没有做到，就是简单的直线和圆弧的拼接，直线的曲率是 0 0 0，圆弧的曲率是 1 / 2 1/ 2 1/2，也就是曲率半径的倒数。
要设计比较好的路径，需要在直线和圆弧中间用回旋线做过渡，让曲率连续变化。但是因为规划并不是本篇博客的重点，所以就凑合用一用， count 是控制点的数量，选择点多一点的，这样控制效果相对会好一点。
4、Carsim路面设置
为了看起来更直观，把 Carsim 的路面也设置一下。
点Procedure，在 3D Road里面选 1200 1200 1200 米的道路：
在车道这里选 two lines，再点 stretch east，在这里根据规划设计道路走向。
上图已经设置好。如果无法修改，先把右上角的锁点开，这样就可以修改了。如果没有 11 11 11 行这样的表，点右下角有Rows输入 11 11 11，就会有 11 11 11 行表格。表格的参数按上图填写即可， straight 代表直线， radius 代表圆弧。
圆弧转过的角度，即左转右转和规划的符号不一样，左转默认半径为正，右转默认半径为负。在规划代码里，若终点角度如果大于起点角度，就是左转；反之就是右转。在这里是根据半径来的，半径为负，就是右转；半径是正，就是左转。一定要把 Treat as loop 勾上，变成闭合曲线处理。
设计的道路挺扭曲，转弯半径非常小，最小的只有 5 m 5m 5m，而且在上面有连续五米的转弯半径，实际上对车辆的通过能力是很大的考验，因为转弯半径特别小，如果不是倒车，转弯半径一般最低也就 8 m 8m 8m。
而 5 m 5m 5m 的转弯半径已经特别陡了，所以设计的路径比较严苛，创建的道路如下图所示：
有很多连续弯道，路况算比较严苛。
四、模型搭建与仿真运行
1、变量准备
先跑一遍路径规划，就是Matlab变量工作区里要有路径才可以：
打开 Simulink 看一下控制模型，先做单位换算，把千米每小时换成米每秒，以及把角度换成弧度。建立 Matlab Function 模块，在里面编程。
这六个模块从逻辑顺序来上来说，从左往右写比较好，因为左边的输出是右边的输入，所以先写左边，再写右边。因此先写预测模块，在写代码之前先写上名字，因为模型比较复杂，先标上记号，要不然可能不知道哪根线对应哪个变量。
2、预测模块实现
预测模块的输入是当前点的 ( x , y , φ , v x , v y , φ ˙ ) (x,y,\varphi ,v_x,v_y,\dot{\varphi}) (x,y,φ,vx,vy,φ˙)，输出为预测后的 ( x p r e , y p r e , φ p r e , v x p r e , v y p r e , φ ˙ p r e ) (x_{pre},y_{pre},\varphi_{pre} ,v_{xpre},v_{ypre},\dot{\varphi}_{pre}) (xpre,ypre,φpre,vxpre,vypre,φ˙pre)。照着预测模块的算法写，预测时间随便给个 0.1 0.1 0.1 秒。
predict_module
matlab
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;
end3、误差和曲率计算模块实现
计算模块需要车辆状态，还需要规划轨迹信息。所以规划要先把它写好，而且写好之后也必须跑一遍，规划代码和控制代码是独立的。所以在规划时，需要先把代码跑一下，在工作区里要有这些规划的变量。
在工作区里有这些变量后，才能传到 Simlink 模型里，
注意 ： ( x r , y r , θ r , κ r ) \left( x_r,y_r,\theta _r,\kappa _r \right) (xr,yr,θr,κr) 必须是行向量，不能是列向量。如果是列向量， Simlink 不识别，必须写成行向量形式。
下面可以开始写误差和曲率的计算模块，输入是 ( x , y , φ , v x , v y , φ ˙ ) (x,y,\varphi ,v_x,v_y,\dot{\varphi}) (x,y,φ,vx,vy,φ˙)，以及规划的 ( x r , y r , θ r , κ r ) \left( x_r,y_r,\theta _r,\kappa _r \right) (xr,yr,θr,κr) ，输出是同一点的曲率以及误差。具体代码如下：
err_kappa_calculate_module
matlab
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代码写好后拼起来即可，其中规划轨迹点 ( x r , y r , θ r , κ r ) \left( x_r,y_r,\theta _r,\kappa _r \right) (xr,yr,θr,κr) ，用From Workspace 模块，从工作区里把变量导入到 Simlink 里必须要工作区里有 ( x r , y r , θ r , κ r ) \left( x_r,y_r,\theta _r,\kappa _r \right) (xr,yr,θr,κr) 才能导。在运行 Simlink 模型时，必须要先把规划代码运行一遍，在工作区里面有这些变量，才能导入，不然会出错。
注意 ： From workspace 只能导行向量，能导列向量、不能导矩阵，也不能导结构体。实际上通过技巧操作可以导矩阵以及列向量，但省事点尽量变成行向量就可以了。
4、AB模块 + LQR模块
本来 LQR 模块应该最难写，也最复杂，但是很幸运 Matlab 自己有 LQR 模块包。用离线LQR，所以 LQR 变成了最好写的模块，而且 LQR 模块和 AB 计算模块是紧密联系的，所以把 AB 计算模块和 LQR 模块合在一起写。
先建立脚本文件，作为LQR离线查表的数据，代码如下：
lqr_offline.m
matlab
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)';有一种情况是要考虑到，车从零开始加速，启动开始跑，必然有一段速度在 0.01 0.01 0.01 秒之外，从 0 0 0 开始到 0.01 0.01 0.01，再从 0.01 0.01 0.01 到想要的速度，但如果 v x = 0 v_x=0 vx=0，代到 A A A 矩阵里， A A A 就变成奇异阵了，里面有无穷大。所以在 v < 0.01 v<0.01 v<0.01 情况下的 k k k，要进行另外处理，不能用LQR算，后面再讲。
算出来的 k k k 是由 5000 5000 5000 个行向量组成的矩阵：
如何查表找和 v x v_x vx 的对应关系呢？
因为 v x v_x vx 很有规律，从 0.01 、 0.02 、 0.03 0.01、0.02、 0.03 0.01、0.02、0.03 一直增大，所以第一行 k k k 必然对应 0.01 m / s 0.01m/s 0.01m/s ， 第 2 2 2 行对应 0.02 m / s 0.02m/s 0.02m/s ，以此类推，对应关系非常有规律。
比如假设速度为 20 m / s 20m/s 20m/s，只要让 20 20 20 除以 0.01 0.01 0.01 即可。
如果不在里面，比如速度是 20.12345678 20.12345678 20.12345678 这样的小数，对应哪个 k k k，用速度除 0.01 0.01 0.01，除出来是小数而不是整数，那怎么办呢？
解决方法就是四舍五入，把小数四舍五入变成整数。这样肯定有误差，但误差可忽略不计，因为 0.01 0.01 0.01 秒是非常小的间隔，在两个之间的 k k k 差别不是特别大，所以四舍五入即可。
LQR 算好之后，就可以写 LQR 模块。LQR 模块由 K 1 、 K 2 、 K 3 、 K 4 K_1、K_2、K_3、 K_4 K1、K2、K3、K4 以及 v x v_x vx 五个变量决定，把 v x v_x vx 输入进去，除以 0.01 0.01 0.01，做四舍五入，看和表的哪组 K 1 、 K 2 、 K 3 、 K 4 K_1、K_2、K_3、 K_4 K1、K2、K3、K4 对应，就把对应的 K 1 、 K 2 、 K 3 、 K 4 K_1、K_2、K_3、 K_4 K1、K2、K3、K4 输出出来，这就是离线 LQR 算法的基本原理。
离线 LQR 算法程序代码如下：
matlab
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把 Q Q Q 设置成了单位矩阵， R R R 取 100 100 100，可以设置不同的 Q Q Q 和 R R R，算出 k k k 来，再把 k k k 导进去。所以 LQR 和规划算法一样，得先运行跑一遍，让工作区 Workspace 里有 K 1 、 K 2 、 K 3 、 K 4 K_1、K_2、K_3、 K_4 K1、K2、K3、K4，才能进行下一步的 Simulink 仿真。
LQR中的权重矩阵 Q Q Q 和 R R R 该怎么设置呢？
只要记住两句话即可：
- Q Q Q 越大，算法性能越好，但牺牲了稳定性
控制量 u u u 可能会特别大，比如可能算出来 u u u 要达到 720 720 720 度这样非常夸张的数值。 - R R R 越大，控制量越小，过程越平缓
过程比较平缓比较重要，因为要保证平缓、舒适性、安全性。
如果无人驾驶车的算法在疯狂打方向盘，看到也害怕，但是代价就是可能跟踪效果不是特别好，所以 Q Q Q 和 R R R 权重的权衡互相矛盾，要具体调合适的 Q Q Q 和 R R R ，期望达到的效果是能很快跟踪上路径，并且控制量 u u u 也不能变化过于剧烈。
模块写好之后，用 Simlink 拼起来即可。
5、前馈模块实现
前馈模块需要整车参数、 v x v_x vx 以及投影点的曲率 κ r \kappa_r κr。
forward_control
matlab
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最后把所有结果整合起来即可。
last_angle
matlab
function angle = fcn(k,err,forword_angle)
angle=-k*err+forword_angle;
end这样所有代码就都写完了，把它打个包封装成子系统，看起来整洁一点。
最后算出来前轮转角，再反馈给 Carsim 的前轮转角即可。
注意：单位换算弧度，要化成角度才能输入到 Carsim， Carsim 根据角度控制，不是根据弧度控制的。
6、避免代数环的延迟模块
在算出角度的线上再加延迟模块，作用是如果输入是这一时刻的值，输出是上时刻的值，它就等于延迟单元。这是为了避免代数环，即输入直接决定输出，输出又直接决定输入，会导致套娃式循环。所以为了打破循环就要加延迟模块来避免代数环。
需要整车状态 ( x , y , φ , v x , v y , φ ˙ ) (x,y,\varphi ,v_x,v_y,\dot{\varphi}) (x,y,φ,vx,vy,φ˙) 作为输入才能算出角度， x , y , φ x,y,\varphi x,y,φ 又需要角度作为输入打方向盘， x , y , φ x,y,\varphi x,y,φ 肯定也都变了，也就是说车辆状态决定角度，又决定车辆状态，他们之间就会变成套娃循环。所以说要加延迟模块，避免代数环现象。
7、仿真测试
后轮转角给 0 0 0，节气门开度给 0.15 0.15 0.15，也就是给一点点油门，模型只做了横向控制，控制方向盘转角，并没有做纵向控制。纵向控制就是控制油门的节气门开度，后续再讲，所以说模型只搭了一半，在这里并不控制纵向速度，就给它恒定油门 0.15 0.15 0.15 ，运行程序看看效果，用 X Y XY XY 观测器，观察算法结果：
可能觉得是个椭圆而不是圆，这是因为横坐标和纵坐标间隔不完全一样，横坐标要宽一点，纵坐标窄一点，所以看起来不太像是圆，而像是椭圆，实际上应该是个圆。
其实很明显看到车转弯半径肯定超过 5 5 5 米，因为道路确实比较严苛。再跑一遍，看算出来方向盘转角是什么情况。
看一下算出来的前端转角，其实是不太好的，有的地方在不停鬼畜，不停地抖来抖去，其实不是一件好事情。
在有的地方突变太厉害，突然猛的打一下方向盘，在转弯时打得非常快。算法写出来有利有弊，方向盘打的太快了，而且方向盘在转弯时，不停的鬼畜抖来抖去，这是无法接受的。
但是也有好的地方，至少控制的跟踪效果做出来了，确实是按照所规划的轨迹在跑，而且规划的轨迹是比较严苛的。
转弯半径很小，而且有很多很急的弯，结果也算勉强能跟上，在没有做纵向控制且在不停加速的情况下，仍然勉强能跟上规划的轨迹，所以有一定的控制效果，但舒适性并不是特别好，方向盘在不停抖来抖去。
为什么会导致方向盘在鬼畜？而且在转弯时，方向盘会猛的突变？
这是有原因的，在下一节会讲算法性能不好的原因以及怎么改进。
五、总结
本节博客详细介绍了自动驾驶控制算法的建模过程，涵盖了从Carsim设置到模型搭建的各个方面。通过配置Carsim软件，获取整车参数并计算侧偏刚度，为后续的控制算法提供基础数据。
通过编写路径规划代码，生成所需的轨迹信息。在Matlab中，搭建了控制模型的各个模块，包括预测模块、误差和曲率计算模块、AB模块和LQR模块、前馈控制模块等。
最后，将模型整合并进行了仿真测试，展示了控制算法对规划轨迹的跟踪效果。
大家不妨在这一节照着思路把模型搭好，在下一节就会告诉各位怎么调模型，使其更平顺。欢迎关注后续内容！
参考资料
后记：
🌟 感谢您耐心阅读这篇关于 自动驾驶横向控制算法代码与模型 的技术博客。 📚
🎯 如果您觉得这篇博客对您有所帮助，请不要吝啬您的点赞和评论 📢
🌟您的支持是我继续创作的动力。同时，别忘了收藏本篇博客 ，以便日后随时查阅。🚀
🚗 让我们一起期待更多的技术分享，共同探索移动机器人 的无限可能！💡
🎭感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行 🚀