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

写在前面:
🌟 欢迎光临 清流君 的博客小天地,这里是我分享技术与心得的温馨角落。📝
个人主页:清流君_CSDN博客,期待与您一同探索 移动机器人 领域的无限可能。

🔍 本文系 清流君 原创之作,荣幸在CSDN首发🐒
若您觉得内容有价值,还请评论告知一声,以便更多人受益。
转载请注明出处,尊重原创,从我做起。

👍 点赞、评论、收藏,三连走一波,让我们一起养成好习惯😜
在这里,您将收获的不只是技术干货,还有思维的火花

📚 系列专栏:【运动控制】系列,带您深入浅出,领略控制之美。🖊
愿我的分享能为您带来启迪,如有不足,敬请指正,让我们共同学习,交流进步!

🎭 人生如戏,我们并非能选择舞台和剧本,但我们可以选择如何演绎 🌟
感谢您的支持与关注,让我们一起在知识的海洋中砥砺前行~~~


文章目录

  • 引言
  • 一、控制算法编程基础
  • 二、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、整车参数获取与侧偏刚度计算

  整车参数可以在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

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:countxr(i)=init_coord(1)+delta_x*i;yr(i)=init_coord(2)+delta_y*i;thetar(i)=init_angle;kr(i)=0;
end      
endfunction[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:countif delta_angle>0xr(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;elsexr(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

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

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

(1)直线函数使用方法

  这两个子函数用起来很简单,比如想生成一段直线,起点是 ( 0 , 0 ) (0,0) (0,0),终点是 ( 5 , 0 ) (5,0) (5,0),角度是 π 4 \frac{\pi}{4} 4π,只需要直线函数就可以:

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

  严格来说要写 θ r , κ r \theta_r,\kappa_r θr,κr,不过不需要的话就用 ~ 占位,生成这样一条离散路径点:
在这里插入图片描述

(2)圆弧函数使用方法

  圆弧稍微复杂一点,需要输入起点坐标、终点坐标、起点角度、终点角度、点的数量。起点角度,终点角度表示点的切线方向与 x x x 轴的夹角。

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

  比如这里起点角度是 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

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

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

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

  在工作区里有这些变量后,才能传到 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

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;%先把最短点的序号设为1for i=1:n%遍历所有的点d=(x-xr(i))^2+(y-yr(i))^2;%计算它与真实的位置的之间的距离if d<d_min%如果能找到比所设置的最短距离还要短d_min=d;%将值赋为最短距离min=i;%把序点的序号赋给minendend%遍历一遍后,一定能找到最短的距离规划点的序号,其实就是选择排序法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);%apolloprojection_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

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:5000vx=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.010.020.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 K1K2K3K4 以及 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 K1K2K3K4 对应,就把对应的 K 1 、 K 2 、 K 3 、 K 4 K_1、K_2、K_3、 K_4 K1K2K3K4 输出出来,这就是离线 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,它是航向量elseindex=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 K1K2K3K4,才能进行下一步的 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

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

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模块、前馈控制模块等。

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

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


参考资料

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


后记:

🌟 感谢您耐心阅读这篇关于 自动驾驶横向控制算法代码与模型 的技术博客。 📚

🎯 如果您觉得这篇博客对您有所帮助,请不要吝啬您的点赞和评论 📢

🌟您的支持是我继续创作的动力。同时,别忘了收藏本篇博客,以便日后随时查阅。🚀

🚗 让我们一起期待更多的技术分享,共同探索移动机器人的无限可能!💡

🎭感谢您的支持与关注,让我们一起在知识的海洋中砥砺前行 🚀

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.rhkb.cn/news/418908.html

如若内容造成侵权/违法违规/事实不符,请联系长河编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

Java | Leetcode Java题解之第393题UTF-8编码验证

题目&#xff1a; 题解&#xff1a; class Solution {static final int MASK1 1 << 7;static final int MASK2 (1 << 7) (1 << 6);public boolean validUtf8(int[] data) {int m data.length;int index 0;while (index < m) {int num data[index];…

从零开始学习JVM(七)- StringTable字符串常量池

1 概述 String应该是Java使用最多的类吧&#xff0c;很少有Java程序没有使用到String的。在Java中创建对象是一件挺耗费性能的事&#xff0c;而且我们又经常使用相同的String对象&#xff0c;那么创建这些相同的对象不是白白浪费性能吗。所以就有了StringTable这一特殊的存在&…

Python爬虫:通过js逆向获取某瓜视频的下载链接

爬虫:通过js逆向获取某瓜视频的下载链接 1. 前言2. 获取script标签下的视频加密数据3. 第一步:获取解密后的视频下载链接4. 第二步:模拟生成加密的webid值 1. 前言 就小编了解&#xff0c;某瓜视频这个网站对应视频下载链接加密处理至少经过三个版本。之前在CSDN发布了一篇关于…

华为 HCIP-Datacom H12-821 题库 (5)

有需要题库的可以看主页置顶 需要题库的加Q裙 V群仅进行学习交流 1.以下关于堆叠 MAD 检测说法错误的是&#xff1f; A、堆系统互为代理进行 MAD 检测时&#xff0c;两个堆系统可以使用相同的D omain ID B、MAD 检测的方式分为直连检测、代理检测 C、MAD 代理检测要求所有堆叠…

Vim笔记

【指尖飞舞&#xff1a;vscode vim 高效开发&#xff08;系列视频&#xff09;】https://www.bilibili.com/video/BV1z541177Jyp16&vd_source23e4761174881d73295e362ffd706749 Vscode vim插件配置-CSDN博客 g_跳到行尾最后一个非空字符 gd :go to definition ctrl (o): …

【基础】Three.js 自定义几何体和复制几何体

通过自定义顶点数据&#xff0c;可以创建任意的几何体。像threejs的长方体BoxGeometry、球体SphereGeometry等几何体都是基于BufferGeometry类构建的&#xff0c;它表示一个没有任何形状的空几何体。 1. 自定义点模型 通过javascript 类型化数组 Float32Array创建一组xyz坐标…

比特币客户端和API

1. 比特比客户端的安装 Bitcoin Core 客户端适用于从 x86 Windows 到 ARM Linux 的不同架构和平台&#xff0c;如下图所示&#xff1a; 2. Bitcoin Core客户端的类型 2.1 Bitcoind Bitcoind 末尾的字母 d 表示 daemon (守护程序&#xff09;。所谓守护程序&#xff0c;就是指常…

基于深度学习 卷积神经网络resnext50的中医舌苔分类系统

项目概述 本项目旨在通过深度学习技术&#xff0c;特别是利用卷积神经网络&#xff08;Convolutional Neural Networks, CNNs&#xff09;中的ResNeXt50架构&#xff0c;实现对中医舌象图像的自动分类。该系统不仅能够识别不同的舌苔类型&#xff0c;还能够在PyQt5框架下提供一…

简单实用的php全新实物商城系统

免费开源电商系统,提供灵活的扩展特性、高度自动化与智能化、创新的管理模式和强大的自定义模块,让电商用户零成本拥有安全、高效、专业的移动商城。 代码是全新实物商城系统源码版。 代码下载

ngrok | 内网穿透,支持 HTTPS、国内访问、静态域名

前言 当我们需要把本地开发的应用展示给外部用户时&#xff0c;常常会因为无法直接访问而陷入困境。 就为了展示一下&#xff0c;买服务、域名&#xff0c;搭环境&#xff0c;费钱又费事。 那有没有办法&#xff0c;让客户直接访问自己本机开发的应用呢&#xff1f; 这种需…

Vue 使用接口返回的背景图片和拼图图片进行滑动拼图验证

一、背景 前两天发了一篇 vue-monoplasty-slide-verify 滑动验证码插件使用及踩坑_vue-monoplasty-slide-verify 引用后不显示-CSDN博客 这两天项目又需要通过接口校验&#xff0c;接口返回了背景图片和拼图图片&#xff0c;于是在网上找了一篇帖子&#xff0c;vue 图片滑动…

安卓玩机工具------小米工具箱扩展工具 小米机型功能拓展

小米工具箱扩展版 小米工具箱扩展版 iO_Box_Mi_Ext是由晨钟酱开发的一款适用于小米&#xff08;MIUI&#xff09;、多亲&#xff08;2、2Pro&#xff09;、多看&#xff08;多看电纸书&#xff09;的多功能工具箱。该工具所有功能均可以免root实现&#xff0c;使用前&…

2024/9/6黑马头条跟学笔记(三)

D3 内容介绍 jdk8新特性&#xff0c;stream流&#xff0c;lambda表达式 ​ 自媒体前后端搭建 步骤 sql—— 实体—— 微服务拷贝&#xff0c;配置nacos—— spring:datasource:driver-class-name: com.mysql.jdbc.Driverurl: jdbc:mysql://192.168.233.136:3306/leadnews_…

基于Spring Boot的火车订票管理系统

你好呀&#xff0c;我是计算机学姐码农小野&#xff01;如果有相关需求&#xff0c;可以私信联系我。 开发语言&#xff1a;Java 数据库&#xff1a;MySQL 技术&#xff1a;JAVA语言 Spring Boot框架 工具&#xff1a;IDEA/Eclipse、Navicat、Tomcat 系统展示 首页 管理…

Android Jetpact Lifecycle 解析

认识 Lifecycle Lifecycle 是什么 Lifecycle 是 Jetpack 组件库中的一个生命周期感知型组件。在 Lifecycle 出现之前&#xff0c;需要手动从外部宿主&#xff08;如 Activity、Fragment 或自定义宿主&#xff09;中将生命周期事件分发到功能组件内部&#xff0c;这势必会造成…

SAM 2:分割图像和视频中的任何内容

文章目录 摘要1 引言2 相关工作3 任务&#xff1a;可提示视觉分割4 模型5 数据5.1 数据引擎5.2 SA-V数据集 6 零样本实验6.1 视频任务6.1.1 提示视频分割6.1.2 半监督视频对象分割6.1.3 公平性评估 6.2 图像任务 7 与半监督VOS的最新技术的比较8 数据和模型消融8.1 数据消融8.2…

RT-Thread(Nano版本)的快速移植(基于NUCLEO-F446RE)

目录 概述 1 RT-Thread 1.1 RT-Thread的版本 1.2 认识Nano版本 2 STM32F446U上移植RT-Thread 2.1 STM32Cube创建工程 2.2 移植RT-Thread 2.2.1 安装RT-Thread Packet 2.2.2 加载RT-Thread 2.2.3 匹配相关接口 2.2.3.1 初次编译代码 2.2.3.2 匹配端口 2.2.4 移植FinSH…

时间同步服务

多主机协作工作时&#xff0c;各个主机的时间同步很重要&#xff0c;时间不一致会造成很多重要应用的故障&#xff0c;如&#xff1a;加密协 议&#xff0c;日志&#xff0c;集群等。 利用NTP&#xff08;Network Time Protocol&#xff09; 协议使网络中的各个计算机时间达到…

OceanBase 4.x 存储引擎解析:如何让历史库场景成本降低50%+

据国际数据公司&#xff08;IDC&#xff09;的报告显示&#xff0c;预计到2025年&#xff0c;全球范围内每天将产生高达180ZB的庞大数据量&#xff0c;这一趋势预示着企业将面临着更加严峻的海量数据处理挑战。随着数据日渐庞大&#xff0c;一些存储系统会出现诸如存储空间扩展…

AF透明模式/虚拟网线模式组网部署

透明模式组网 实验拓扑 防火墙基本配置 接口配置 eth1 eth3 放通策略 1. 内网用户上班时间&#xff08;9:00-17:00&#xff09;不允许看视频、玩游戏及网上购物&#xff0c;其余时 间访问互联网不受限制&#xff1b;&#xff08;20 分&#xff09; 应用控制策略 2. 互联…