网站建设飠金手指科杰十二wordpress分类门户主题

当前位置： 首页 > news >正文

网站建设飠金手指科杰十二,wordpress分类门户主题,六安网站制作哪里有,手机网站建设行业分析写在前面#xff1a; #x1f31f; 欢迎光临 清流君 的博客小天地#xff0c;这里是我分享技术与心得的温馨角落。#x1f4dd; 个人主页#xff1a;清流君_CSDN博客#xff0c;期待与您一同探索 移动机器人 领域的无限可能。 #x1f50d; 本文系 清流君 原创之作 欢迎光临 清流君 的博客小天地这里是我分享技术与心得的温馨角落。 个人主页清流君CSDN博客期待与您一同探索 移动机器人 领域的无限可能。 本文系 清流君 原创之作荣幸在CSDN首发 若您觉得内容有价值还请评论告知一声以便更多人受益。 转载请注明出处尊重原创从我做起。 点赞、评论、收藏三连走一波让我们一起养成好习惯 在这里您将收获的不只是技术干货还有思维的火花 系列专栏【运动控制】系列带您深入浅出领略控制之美。 愿我的分享能为您带来启迪如有不足敬请指正让我们共同学习交流进步 人生如戏我们并非能选择舞台和剧本但我们可以选择如何演绎 感谢您的支持与关注让我们一起在知识的海洋中砥砺前行~~~ 文章目录 引言一、规划接口1.1 轨迹规划的定义与重要性1.2 不同场景的轨迹规划1.3 轨迹规划的数学模型1.4 考虑车辆运动特性的轨迹规划 二、轨迹生成2.1 五次多项式在轨迹规划中的应用2.2 解多项式系数 三、横向控制的规划接口3.1 匹配点的更新3.2 规划点与时间的关系 四、纵向控制的规划接口4.1 横向误差的计算与理解4.2 速度误差与期望加速度4.3 横向控制的实现与调整4.4 纵向控制与规划接口的整合 五、代码实现与模型调整5.1 代码下载与准备工作5.2 Matlab 环境配置5.3 Carsim 标定过程5.4 Simlink 模型建立5.5 纵向控制模型整合与参数优化5.6 横向控制的集成与误差计算模块的修改 六、横向误差大的原因6.1 横向误差的原因分析6.2 仿真与实车的差别 七、解决横向误差的办法八、转向不足及过度转向8.1 转向不足与过度转向的原因8.2 力矩平衡与转向特性8.3 车辆设计与转向特性的关系8.4 使用 PID 控制器处理转向不足 九、总结参考资料 引言 各位小伙伴们大家好欢迎来到自动驾驶控制算法第十二节内容整理自 B 站知名 up 主 忠厚老实的老王 的视频作为博主的学习笔记分享给大家共同学习。 本节是超级大综合将用到前十一节所有的知识根据本篇博客的内容实现横纵向控制。 简单回顾一下本系列所讲的内容 第一到第二节开篇以及运动学方程第三到第八节横向控制以及 LQR第九到第十一节纵向控制也就是双 PID 算法第十二节横纵向综合控制、规划接口因为控制的上游是规划必须要有规划的输入才能做控制 所以不仅要把横纵向控制所有的模型都做搭好还要写接口接口就是在规划里面规划一条轨迹然后输入到控制里让控制根据规划的轨迹去跑所以还要写接口。 回顾整个系列会发现运动学方程几乎就没怎么讲可能只用到了 tan ⁡ δ L R \tan \delta \frac{L}{R} tanδRL​ 公式。但运动学方程实际上是非常有用的只是本系列教程将淡化了。 运动学方程适用于低速但是有很好的特点就是大小转角均可而 LQR 只适用于小转角但不限于速度高速、低速都可以控制。所以如果想做停车、掉头之类的控制一般用运动学方程可以适用于大转角即方向盘转角可以打得很大但 LQR 不行 LQR 转角必须就是比较小大家可以翻一翻第四节。 【自动驾驶】控制算法四坐标变换与横向误差微分方程 第四节推导二自由度动力学方程时就假设前轮转角较小前轮转角较大的话 LQR 也可以但控制效果没有运动学方程那么好。 在这里就简单提一下怎么用运动学方程做控制。 首先是非线性方程要线性化做一阶的泰勒展开线性化后就变成如下形式 e ˙ A e ˙ B u \dot{e}A\dot{e}Bu e˙Ae˙Bu  这样就和 LQR 的处理方式基本一样了。 一、规划接口 1.1 轨迹规划的定义与重要性 首先要讲的就是规划接口因为控制模块的功能就是接收规划轨迹让车按照规划轨迹运动。控制模块功能实际上就暗含两个事情 接受一条规划轨迹 那首先得有一条规划的轨迹才行所以那就是要有规划。让车辆按照规划轨迹运动 就是横纵向控制 所以必须要讲一下怎么规划一条轨迹。 注意轨迹规划不是路径规划。 在自动驾驶领域里路径规划和轨迹规划不一样这两个是分开的。路径规划比如导航想要去哪手机搜一下就给一条路径此路径是大概的方向性的东西只会告诉该走哪条路该往什么方向走但是不会告诉速度是多少加速度是多少因此 路径规划时间无关只告诉该往什么地方走轨迹规划包含时间所以轨迹规划不仅包含位置还包含速度、加速度以及道路曲率等。 所以轨迹规划和路径规划不一样要给出 s ( t ) s(t) s(t) 以及 d ( t ) d(t) d(t)。但在这里就不讲 s ( t ) s(t) s(t) 以及 d ( t ) d(t) d(t)了讲 x ( t ) , y ( t ) x (t), y (t) x(t),y(t)即不涉及曲线坐标系、自然坐标系只在直角坐标系下去做规划这也是由易到难的过程先做简单的再做难的。 1.2 不同场景的轨迹规划 至于在自然坐标系下的轨迹规划在进阶的课程再讲。本篇博客就讲在直角坐标系下的规划。比如这里有一辆车 在坐标圆点上想移动到右上角的红色区域坐标为 ( 100 , 10 ) (100,10) (100,10)边界条件为 坐标 x x x x ˙ \dot x x˙ x ¨ \ddot x x¨ y y y y ˙ \dot y y˙​ y ¨ \ddot y y¨​初始 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0终点 100 100 100 0 0 0 0 0 0 10 10 10 0 0 0 0 0 0 用时 t 20 s t20s t20s此场景实际上对应的就是停车。 当然终点也可以改比如 x 100 x 100 x100, x ˙ 20 \dot x20 x˙20即车辆到达终点时有速度其他都不变。此场景实际上对应的是驶入比如在辅路上旁边有花坛想行驶到主路上融入交通流的场景。 或者可以把起点也改了比如起点 x 0 x0 x0但速度 x ˙ 10 \dot x10 x˙10加速度 x ¨ 0 \ddot x0 x¨0其他不变终点处 x 100 x 100 x100 x ˙ 20 \dot x20 x˙20 x ¨ 0 \ddot x0 x¨0这其实对应就是换道和超车场景。 给定起点和终点的位置、速度、加速度信息的规划可以做很多场景停车、驶入、变道。所以本篇博客就研究给定起点和终点的位置、速度、加速度信息的规划这样的规划最简单也最常用而且能覆盖大部分直线行驶的场景但不能做转弯也不能做调头因为转化和调头必须要用到自然坐标系才可以。 关于在自然坐标系下的规划到后面进阶的课程再讲本篇博客主要讲简单的规划主要是给控制服务的就是给控制提供规划接口。 1.3 轨迹规划的数学模型 规划问题会转化成数学问题就是设计一条合适的 x ( t ) , y ( t ) x(t),y(t) x(t),y(t)满足始末的边界条件也就是给出初始端的位置、速度、加速度终点的位置、速度、加速度以及耗费的时间 T T T 起点 x ( 0 ) x(0) x(0) x ˙ ( 0 ) \dot x(0) x˙(0) x ¨ ( 0 ) \ddot x(0) x¨(0) y ( 0 ) y(0) y(0) y ˙ ( 0 ) \dot y(0) y˙​(0) y ¨ ( 0 ) \ddot y(0) y¨​(0)终点 x ( T ) x(T) x(T) x ˙ ( T ) \dot x(T) x˙(T) x ¨ ( T ) \ddot x(T) x¨(T) y ( T ) y(T) y(T) y ˙ ( T ) \dot y(T) y˙​(T) y ¨ ( T ) \ddot y(T) y¨​(T) 根据上述条件设计出一条合理的轨迹。 这是经典的规划问题很常见而且不仅仅在无人驾驶上用到在机器人规划上也有类似的算法给定始末点的位置、速度、加速度然后算轨迹。 1.4 考虑车辆运动特性的轨迹规划 但无人驾驶车辆在算法应用上不能直接照搬机器人领域的算法这是因为它们在运动特性上存在显著差异。具体来说无人驾驶车辆无法像机器人那样独立进行横向运动。在车辆行驶中横向运动通常是由纵向运动引起的即车辆不能像螃蟹那样仅依靠横向移动。而机器人则具备这种横向移动的能力它们既可以横向移动也可以纵向移动。因此无人驾驶车辆的运动规划需要考虑额外的限制。 车辆规划轨迹与机器人轨迹的不同之处在于车辆轨迹必须受到切线曲率、加速度和速度的限制。例如在确定起点和终点后不能简单地绘制一条直线作为行驶路径。这是因为车辆的实际行驶路径需要遵循物理学和动力学的基本原则以及道路和交通规则的限制。因此无人驾驶车辆的路径规划必须更加复杂和精确以确保行驶的安全性和效率。 举个例子比如给定起点和终点能直接从起点到终点拉一条直线吗显然不可以。 合适规划轨迹应该长这样才符合汽车运动的规律 因为汽车不能平白无故的就有横向速度横向运动必须要由纵向运动诱发。如果在起点建立直角坐标系就是对曲线切线斜率 d y d x \frac{dy}{dx} dxdy​ 也有要求。 所以汽车的轨迹规划边界条件要做相应的修改 起点 x ( 0 ) x(0) x(0) x ˙ ( 0 ) \dot x(0) x˙(0) x ¨ ( 0 ) \ddot x(0) x¨(0) y ( 0 ) y(0) y(0) y ′ ( 0 ) y(0) y′(0) y ′ ′ ( 0 ) y(0) y′′(0)终点 x ( T ) x(T) x(T) x ˙ ( T ) \dot x(T) x˙(T) x ¨ ( T ) \ddot x(T) x¨(T) y ( x e n d ) y(x{end}) y(xend​) y ′ ( x e n d ) y(x{end}) y′(xend​) y ′ ′ ( x e n d ) y(x{end}) y′′(xend​) 变量符号上的点代表对时间求导 d d t \frac{d}{dt} dtd​撇代表对坐标求导 d d x \frac{d}{dx} dxd​。如果是自然坐标系撇就等于 d d s \frac{d}{ds} dsd​这里是直角坐标系所以是 d d x \frac{d}{dx} dxd​车辆规划和机器人规划还是有很大不同的。 机器人就是 x x x 和 y y y都是对时间的导数因为机器人可以做纵向运动也可以做横向运动但仅用 y ′ y y′ 做规划还是不够因为真正输入到控制模块里能用的轨迹必须要是 ( x ( t ) , y ( t ) ) (x(t),y(t)) (x(t),y(t))都是和时间相关的控制就按照目标点去跑。 所以关于车辆轨迹规划车辆纵向的 x x x 方向可以是关于时间的导数因为 x x x 直接和时间相关但车辆横向的 y y y 方向不行 横向 y y y 和纵向 x x x 相关。所以还要写 y ˙ \dot y y˙​ 和 y ′ y y′ 间的转化即 d d x \frac{d}{dx} dxd​ 和 d d t \frac{d}{dt} dtd​ 间的转化。 这两者之间的转换也非常简单因为 y ( x ) y(x) y(x) 中的 x x x 和 t t t 有关 x x ( t ) xx(t) xx(t)所以 y ( x ) y(x) y(x) 和 y ( t ) y(t) y(t) 之间的关系就是 y ( t ) y ( x ( t ) ) y(t)y(x(t)) y(t)y(x(t))  那么 y ′ y y′ 和 y ˙ \dot y y˙​ 之间的关系也很好计算只要复合求导即可。 y ˙ ( t ) d y d x ⋅ d x d t y ′ ⋅ x ˙ \dot{y}\left( t \right) \frac{dy}{dx}\cdot \frac{dx}{dt}y\cdot \dot{x} y˙​(t)dxdy​⋅dtdx​y′⋅x˙   y ¨ \ddot y y¨​ 和 y ′ ′ y y′′ 之间的关系还是比较复杂的要使用稍微复杂点的复合求导最终结果是 y ¨ ( t ) d d t ( d y d t ) d d t ( d y d x ⋅ d x d t ) d ( d y d x ) d t d x d t d y d x ⋅ d 2 x d t 2 d d x ( d y d x ) ⋅ d x d t ⋅ d x d t y ′ ⋅ x ¨ y ′ ′ ⋅ x ˙ 2 y ′ ⋅ x ¨ \begin{aligned} \ddot{y}\left( t \right) \frac{d}{dt}\left( \frac{dy}{dt} \right) \frac{d}{dt}\left( \frac{dy}{dx}\cdot \frac{dx}{dt} \right) \ \frac{d\left( \frac{dy}{dx} \right)}{dt}\frac{dx}{dt}\frac{dy}{dx}\cdot \frac{d^2x}{dt^2}\ \frac{d}{dx}\left( \frac{dy}{dx} \right) \cdot \frac{dx}{dt}\cdot \frac{dx}{dt}y\cdot \ddot{x}\ y\cdot \dot{x}^2y\cdot \ddot{x}\ \end{aligned} y¨​(t)​dtd​(dtdy​)dtd​(dxdy​⋅dtdx​)dtd(dxdy​)​dtdx​dxdy​⋅dt2d2x​dxd​(dxdy​)⋅dtdx​⋅dtdx​y′⋅x¨y′′⋅x˙2y′⋅x¨​  这样问题就非常清晰了给这么多边界条件 x ( 0 ) x\left( 0 \right) x(0) x ˙ ( 0 ) \dot{x}\left( 0 \right) x˙(0) x ¨ ( 0 ) \ddot{x}\left( 0 \right) x¨(0) x ( T ) x\left( T \right) x(T) x ˙ ( T ) \dot{x}\left( T \right) x˙(T) x ¨ ( T ) \ddot{x}\left( T \right) x¨(T) y ( 0 ) y\left( 0 \right) y(0) y ′ ( 0 ) y\left( 0 \right) y′(0) y ′ ′ ( 0 ) y\left( 0 \right) y′′(0) y ( x e n d ) y\left( x{end} \right) y(xend​) y ′ ( x e n d ) y\left( x{end} \right) y′(xend​) y ′ ′ ( x e n d ) y\left( x_{end} \right) y′′(xend​) x x x 共有 6 6 6 个 y y y 也有 6 6 6 个再加上耗费的时间 T T T。求轨迹满足边界条件生成包含 x ( t ) , y ( x ) x(t),y(x) x(t),y(x) 的轨迹先算出 y y y 和 x x x 的关系再通过转化成 y y y 和 t t t 之间的关系这样就生成了一条规划轨迹。 二、轨迹生成 2.1 五次多项式在轨迹规划中的应用 对于 x ( t ) x(t) x(t)很容易想到的就是用五次多项式 x ( t ) a 0 a 1 t a 2 t 2 a 3 t 3 a 4 t 4 a 5 t 5 x(t)a_0a_1ta_2t^2a_3t^3a_4t^4a_5t^5 x(t)a0​a1​ta2​t2a3​t3a4​t4a5​t5  因为五次多项式有 6 6 6 个系数正好对应这 6 6 6 个边界条件。 对于 y ( x ) y(x) y(x) 也一样 y y y 也用五次多项式 y ( x ) b 0 b 1 x b 2 x 2 b 3 x 3 b 4 x 4 b 5 x 5 y(x)b_0b_1xb_2x^2b_3x^3b_4x^4b_5x^5 y(x)b0​b1​xb2​x2b3​x3b4​x4b5​x5  因为 y y y 也对应着 6 6 6 个边界条件五次多项式正好有 6 6 6 个未知数正好就可以对应这 6 6 6 个边界条件。 2.2 解多项式系数 通过这 12 12 12 个边界条件 x x x 方向 6 6 6 个 y y y 方向也是 6 6 6 个解出 a 0 a_0 a0​ 到 a 5 a_5 a5​ 以及 b 0 b_0 b0​ 到 b 5 b_5 b5​ 出来。 y y y 还要转化一下通过以下关系式 y ( t ) y ( x ( t ) ) y ˙ ( t ) y ′ [ x ( t ) ] ⋅ x ˙ ( t ) y ¨ ( t ) y ′ ′ [ x ( t ) ] ⋅ x ˙ ( t ) 2 y ′ [ x ( t ) ] ⋅ x ¨ ( t ) \begin{aligned} y\left( t \right) y\left( x\left( t \right) \right)\ \dot{y}\left( t \right) y\left[ x\left( t \right) \right] \cdot \dot{x}\left( t \right)\ \ddot{y}\left( t \right) y\left[ x\left( t \right) \right] \cdot \dot{x}\left( t \right) ^2y\left[ x\left( t \right) \right] \cdot \ddot{x}\left( t \right)\ \end{aligned} y(t)y˙​(t)y¨​(t)​y(x(t))y′[x(t)]⋅x˙(t)y′′[x(t)]⋅x˙(t)2y′[x(t)]⋅x¨(t)​  解出 y ( t ) , y ˙ ( t ) , y ¨ ( t ) y(t),\dot{y}(t),\ddot{y}(t) y(t),y˙​(t),y¨​(t)可以得到一条 x x x 和 t t t 的关系以及 y y y 和 t t t 的关系以及他们的导数和 t t t 的关系这样就是一条完整的轨迹规划。 三、横向控制的规划接口 得到这条轨迹规划后该怎样才能运用于横纵向控制 参考以第八节博客 【自动驾驶】控制算法八横向控制Ⅰ | 算法与流程 在第八节中用到了规划中的 ( x r , y r , θ r , κ r ) (x_r,y_r,\theta_r,\kappa_r) (xr​,yr​,θr​,κr​) 四个量。其中 θ r \theta_r θr​ 是轨迹切线方向与 x x x 轴的夹角 κ r \kappa_r κr​ 代表轨迹曲率。 3.1 匹配点的更新 匹配点由时间给出比如时间是 1 1 1 秒规划器给出匹配点坐标 ( x r , y r ) (x_r,y_r) (xr​,yr​)即匹配点 ( x r , y r , θ r , κ r ) (x_r,y_r,\theta_r,\kappa_r) (xr​,yr​,θr​,κr​) 和时间相关每过一段时间规划器就会发出匹配点坐标控制器就按照匹配点控制。 3.2 规划点与时间的关系 ( x r , y r ) (x_r,y_r) (xr​,yr​) 其实就是规划器 ( x ( t ) , y ( t ) ) (x(t),y(t)) (x(t),y(t))和时间有直接关系 ( θ r , κ r ) (\theta_r,\kappa_r) (θr​,κr​) 和时间 t t t 的关系如下 θ r ( t ) arctan ⁡ { y ′ [ x ( t ) ] } \theta _r\left( t \right) \arctan\text{{}y\left[ x\left( t \right) \right] } θr​(t)arctan{y′[x(t)]} κ r ( t ) y ′ ′ x ( t ) 3 2 \kappa _r\left( t \right) \frac{y\left[ x\left( t \right) \right]}{\left( 1y\left[ x\left( t \right) \right] ^2 \right) ^{\frac{3}{2}}} κr​(t)(1y′[x(t)]2)23​y′′[x(t)]​  这样横向控制和规划接口就讲完了接下来讲纵向控制和规划接口 四、纵向控制的规划接口 4.1 横向误差的计算与理解 在横向公式里面讲过了横向误差 e s e_s es​如果各位如果不记得的话要翻一下第七节和第八节 【自动驾驶】控制算法七离散规划轨迹的误差计算 【自动驾驶】控制算法八横向控制Ⅰ | 算法与流程 e s e_s es​ 在横向控制里有所以横向控制可以直接输出 e s e_s es​就是车辆当前位置与匹配点的距离而且此距离是基于自然坐标系下的距离不是基于直角坐标系下的距离。 这里可能会有很多人会有疑惑明明规划都是直角坐标系怎么突然就来了自然坐标系 这是因为直角坐标系只用于规划当生成了规划轨迹 ( x ( t ) , y ( t ) ) (x(t),y(t)) (x(t),y(t)) 后直角坐标系使命就完成了控制就是基于生成的曲线作为自然坐标系的坐标轴的所以 e s e_s es​ 才是位置误差。 注意横向控制里面 e s e_s es​ 还不算是真正的位置误差。 下面看一下 e s e_s es​ 到底是怎么算出来的比如这里有一条轨迹   目标点被视为匹配点在此基础上首先从车辆位置到目标点绘制一个向量接着在目标点的切线方向上再绘制一个向量。将这两个向量进行点乘运算其结果即为 e s e_s es​ 即红色向量在蓝色向量上的投影就是 e s e_s es​ 。如果各位对此概念不太熟悉可以多参考第七节的内容那里有更详细的解释。 【自动驾驶】控制算法七离散规划轨迹的误差计算 关于 e s e_s es​ 的正负问题可以这样思考如果场景如上图所示 e s e_s es​ 是否为负值由于两个向量之间的夹角为钝角因此 e s e_s es​ 应为负值。但误差的定义是目标点与当前位置之间的距离。在所描述的场景中目标点位于车辆当前位置的前方因此 e s e_s es​ 按理来说应该是正值。因此在实际的控制作用中为了使 e s e_s es​ 与位置误差的实际意义相匹配需要对 e s e_s es​ 加上负号。 4.2 速度误差与期望加速度 速度误差为 v p − s ˙ v_p-\dot{s} vp​−s˙ p p p ( planning ) 是规划速度 s ˙ \dot s s˙ 在横向控制里面有。则规划速度 v p v_p vp​ 为 v p x ˙ r ( t ) 2 y ˙ r ( t ) 2 v_p\sqrt{\dot{x}_r(t)^2\dot{y}_r(t)^2} vp​x˙r​(t)2y˙​r​(t)2 ​  期望加速度 a p a_p ap​ 为 a p x ¨ r ( t ) 2 y ¨ r ( t ) 2 a_p\sqrt{\ddot{x}_r(t)^2\ddot{y}_r(t)^2} ap​x¨r​(t)2y¨​r​(t)2 ​  这里混用了 ( x ( t ) , y ( t ) ) (x(t),y(t)) (x(t),y(t)) 和 ( x r ( t ) , y r ( t ) ) (x_r(t),y_r(t)) (xr​(t),yr​(t))他们其实是一回事在规划模块里规划轨迹是 ( x ( t ) , y ( t ) ) (x(t),y(t)) (x(t),y(t))到控制模块时轨迹就变成了 ( x r ( t ) , y r ( t ) ) (x_r(t),y_r(t)) (xr​(t),yr​(t))其实是一回事儿。这样从规划到控制的理论部分就讲完了。 把整个规划和控制逻辑梳理一下首先通过规划得到 x ( t ) , y ( x ) x(t),y(x) x(t),y(x)然后通过这两个可以得到 x ( t ) , y ( t ) , y ( x ) x(t),y(t),y(x) x(t),y(t),y(x)得到这三个的同时也可以得到各阶导数 x ˙ ( t ) , y ˙ ( t ) , y ′ ( x ) \dot x(t),\dot y(t),y(x) x˙(t),y˙​(t),y′(x) x ¨ ( t ) , y ¨ ( t ) , y ′ ′ ( x ) \ddot x(t),\ddot y(t),y(x) x¨(t),y¨​(t),y′′(x)。 4.3 横向控制的实现与调整 由此可得横向控制公式 x r ( t ) x ( t ) y r ( t ) y ( t ) θ r ( t ) arctan ⁡ [ y ′ [ x ( t ) ] ] κ r ( t ) y ′ ′ x ( t ) 3 2 \begin{aligned} x_r\left( t \right) x\left( t \right)\ y_r\left( t \right) y\left( t \right)\ \theta _r\left( t \right) \arctan \left[ y\left[ x\left( t \right) \right] \right]\ \kappa _r\left( t \right) \frac{y\left[ x\left( t \right) \right]}{\left( 1y\left[ x\left( t \right) \right] ^2 \right) ^{\frac{3}{2}}}\ \end{aligned} xr​(t)yr​(t)θr​(t)κr​(t)​x(t)y(t)arctany′[x(t)]23​y′′[x(t)]​​ 4.4 纵向控制与规划接口的整合 以及纵向控制公式 e s d ⃗ e r r ⋅ τ ⃗ s ˙ 1 1 − κ r e d ( v x cos ⁡ e φ − v y sin ⁡ e φ ) v p x ˙ ( t ) 2 y ˙ ( t ) 2 e v v p − s ˙ a p x ¨ ( t ) 2 y ¨ ( t ) 2 \begin{aligned} es\vec{d}e{rr}\cdot \vec{\tau}\ \dot{s}\frac{1}{1-\kappa _re_d}\left( vx\cos e{\varphi}-vy\sin e{\varphi} \right)\ v_p\sqrt{\dot{x}\left( t \right) ^2\dot{y}\left( t \right) ^2}\ e_vv_p-\dot{s}\ a_p\sqrt{\ddot{x}\left( t \right) ^2\ddot{y}\left( t \right) ^2}\ \end{aligned} es​s˙vp​ev​ap​​d err​⋅τ 1−κr​ed​1​(vx​coseφ​−vy​sineφ​)x˙(t)2y˙​(t)2 ​vp​−s˙x¨(t)2y¨​(t)2 ​​  其中纵向误差 e s e_s es​ 输入至纵向控制要加负号 e v e_v ev​ 为速度误差 a p a_p ap​ 为期望加速度。 纵向控制中的 e s e_s es​ 、 s ˙ \dot s s˙ 是通过横向控制计算出来的所以纵向控制需要输入的只有三个。 五、代码实现与模型调整 5.1 代码下载与准备工作 这样理论部分就讲完了接下来就是重头戏代码环节。 首先把横向控制和纵向控制的代码以及模型给下载下来链接如下 自动驾驶横纵向控制代码和模型 5.2 Matlab 环境配置 把代码和模型全部都复制到 Casim 工作目录上然后打开 Matlab Matlab 共有四个 . m m m 文件前三个是标定用的. m m m 文件是第十一节里的最后是第八节的 LQR 。可以注意到无论是 Q Q Q 还是 C f , C r C_f,C_r Cf​,Cr​ 都变了不是第八讲 LQR 了这里要改一下因为横纵向控制的 LQR 容易出现很大偏差所以需要更精确的 LQR 不能就是像第八节那样随便混一下。 侧偏刚度需要精确解必须要算出每个轮子精确的垂向力通过垂向力查表根据曲线大概估算出侧偏刚度。 重新设置新的模型 planning_control就不要用以前的模型了在新的模型里输入就是油门、刹车以及四轮的转角输出就是 x , y , φ , v x , v y , φ ˙ x,y,\varphi,v_x,v_y,\dot \varphi x,y,φ,vx​,vy​,φ˙​ 以及发动机转速。 5.3 Carsim 标定过程 首先做标定把那三个标定代码跑起来得到标定表。先不急着把横向控制模型复制进来如果拿到代码应该没有油门刹车标定表所以要先标定一下而且标定时要把 Carsim 当时的输出 x , y , φ , v x , v y , φ ˙ x,y,\varphi,v_x,v_y,\dot \varphi x,y,φ,vx​,vy​,φ˙​ 改为标定时的输出只有 v x , a x v_x,a_x vx​,ax​ 以及发动机转速并且要先从初始条件 0 0 0 开始标定油门然后把初速度改成 180 k m / h 180km/h 180km/h再标定刹车。标完后再把 Carsim 的输出改成 x , y , φ , v x , v y , φ ˙ x,y,\varphi,v_x,v_y,\dot \varphi x,y,φ,vx​,vy​,φ˙​ 以及发动机转速。 5.4 Simlink 模型建立 再建立新的 Simlink 空模型用 Carsim 把空模型关联上去最后把仿真的时间给改一下改成 40 40 40 秒因为希望仿真时间长一点。然后把纵向控制的模型就是第十一节的模型打开把里面所有的东西都复制到新的空模型里。 5.5 纵向控制模型整合与参数优化 先不急着把横向控制模型复制进来应该先做标定先把那三份标定代码给跑起来可以得到标定表。标定时要把 Carsim 的输入和输出改为标定时的输入和输出 输入为油门、刹车输出为 v x v_x vx​、 a x a_x ax​ 以及发动机转速 并且要先从初始速度为 0 0 0 开始标定油门然后再改为初速度 180 k m / h 180km/h 180km/h再标定刹车。标完之后再把 Carsim 的输出改为 x , y , φ , v x , v y , φ ˙ x,y,\varphi,v_x,v_y,\dot \varphi x,y,φ,vx​,vy​,φ˙​ 以及发动机转速。 得到标定表后把注释取消掉把纵向双 PID 弄好。 可以把位置 PID 的输入删掉因为输入 e s e_s es​ 在横向控制里直接给所以不需要用加减计算出来。 把纵向控制所需要的输入先断开用红色线起个名字再打包这样看起来不那么混乱因为模型很复杂所以能打包的尽量就打包。 纵向控制需要车的当前速度作为电机模型的输入所以把速度写一下写个函数 function v fcn(vx,vy)vsqrt(vx^2vy^2);写好后连起来规划函数如下 MATLAB Functionplanning function [vp,ap,xr,yr,thetar,kr] fcn(t)dx100;%30 秒要向前移动 100 米然后向左移动 10 米。dy10;T30;xstart[0,0,0];%起点到终点的位置、速度、加速度xend[dx,0,0];ystart[0,0,0];yend[dy,0,0];azeros(1,6);%无论是 x 和y都是五次多项式有 6 个系数bzeros(1,6);a(1)xstart(1);a(2)xstart(2);a(3)xstart(3)/2;A1[T^3,T^4,T^5;%解三元一次方程组3*T^2,4*T^3,5*T^4;6*T,12*T^2,20*T^3];%注意在推导时系数是从 A0 到A5但是从代码的编写写的是 A1 到A6%因为 Matlab 数组的下标是从一开始的没有 A0 B1[xend(1)-a(1)-a(2)*T-a(3)*T^2;xend(2)-a(2)-2*a(3)*T;xend(3)-2*a(3)];xsinv(A1)*B1;a(4)xs(1);a(5)xs(2);a(6)xs(3);b(1)ystart(1);b(2)ystart(2);b(3)ystart(3)/2;A2[dx^3,dx^4,dx^5;%方程组的t变成dx 3*dx^2,4*dx^3,5*dx^4;6*dx,12*dx^2,20*dx^3];B2[yend(1)-b(1)-b(2)*dx-b(3)*dx^2;yend(2)-b(2)-2*b(3)*dx;yend(3)-2*b(3)];ysinv(A2)*B2;b(4)ys(1);b(5)ys(2);b(6)ys(3);xra(1)a(2)*ta(3)*t^2a(4)*t^3a(5)*t^4a(6)*t^5;yrb(1)b(2)*xrb(3)*xr^2b(4)*xr^3b(5)*xr^4b(6)*xr^5;xr_dota(2)2*a(3)*t3*a(4)*t^24*a(5)*t^35*a(6)*t^4;yr_dxb(2)2*b(3)*xr3*b(4)*xr^24*b(5)*xr^35*b(6)*xr^4;yr_dotyr_dx*xr_dot;thetaratan(yr_dx);xr_dot22*a(3)6*a(4)*t12*a(5)*t^220*a(6)*t^3;yr_dx22*b(3)6*b(4)*xr12*b(5)*xr^220*b(6)*xr^3;yr_dot2yr_dx2*xr_dot^2yr_dx*xr_dot2;kryr_dx2/((1yr_dx^2)^1.5);%曲率vpsqrt(xr_dot^2yr_dot^2);if xr_dot20apsqrt(xr_dot2^2yr_dot2^2);elseap-sqrt(xr_dot2^2yr_dot2^2); end5.6 横向控制的集成与误差计算模块的修改 下面加入横向控制把横向控制的核心代码拷进来。把单位换算去掉因为在单位换算在外面已经做过了所以就不需要再做了。把 ( x r , y r , θ r , κ r ) (x_r,y_r,\theta_r,\kappa_r) (xr​,yr​,θr​,κr​) 全都删掉然后加接口进去让 ( x r , y r , θ r , κ r ) (x_r,y_r,\theta_r,\kappa_r) (xr​,yr​,θr​,κr​) 从外面输入。 修改误差计算模块的代码首先把 e s es es​ 和 s ˙ \dot s s˙ 输出出去把 d m i n d{min} dmin​ 去掉因为 ( x r , y r , θ r , κ r ) (x_r,y_r,\theta_r,\kappa_r) (xr​,yr​,θr​,κr​) 由外面的规划模块给不是直接给定轨迹找最距离最短的匹配点。误差计算模块修改如下 function [kr,err,es,s_dot] fcn(x,y,phi,vx,vy,phi_dot,xr,yr,thetar,kappar)tor[cos(thetar);sin(thetar)];nor[-sin(thetar);cos(thetar)];d_err[x-xr;y-yr];ednor*d_err;estor*d_err;%projection_point_thetarthetar(dmin);%apolloprojection_point_thetarthetarkappar*es;ed_dotvy*cos(phi-projection_point_thetar)vx*sin(phi-projection_point_thetar);%%%%%%%%%ephisin(phi-projection_point_thetar);%%%%%%%%%ss_dotvx*cos(phi-projection_point_thetar)-vy*sin(phi-projection_point_thetar);%两步算出来s_dots_dotss_dot/(1-kappar*ed);ephi_dotphi_dot-kappar*s_dot;krkappar;err[ed;ed_dot;ephi;ephi_dot]; end这样横向控制就做好了连起来就可以了。转角算出来之后要做单元换算把弧度转换为角度后轮角度给写成 0 0 0 就行了因为 LQR 是小角度所以要加 − 1 -1 −1 到 1 1 1 的限制最后别忘了 e s e_s es​ 要乘 − 1 -1 −1因为 e s e_s es​ 输入到纵向控制里要加负号。在纵向控制里再给加个 10 10 10 到 10 10 10 的限制因为 e s e_s es​ 也不希望过大。 一般实车调试的误差要小于 0.1 0.1 0.1 米而仿真至少要达到厘米级就是零点零几米 如果控制效果不好一般就是加速度超了所以首先要检查的就是加速度。 LQR 对低高速没有限制但是必须要小转角第一是小转角第二就是加速度不能超了。一般加速度 2 2 2 到 3 3 3 特别是高速情况下想再有 4 4 4 到 5 5 5 的加速几乎不可能因为在高速情况下电机的加速能力就很差了。 六、横向误差大的原因 6.1 横向误差的原因分析 纵向误差一般不会特别大如果大的话调 PID 参数就可以了有问题的一般是横向误差有以下几点原因 侧偏刚度估算太准 而且在跑时前轮和后轮的垂向力不一样因为加速度会导致轴转移垂向力不一样就会导致侧偏刚度变化。 LQR 模型的简化 是基于二自由度自行车模型本身就有一定简化所以并不能完全的模拟车辆的横向运动本身就有一定误差所以导致横向跟踪也会有一定的误差。 汽车转向不足的固有特性 转向不足导致横向控制存在一定误差。一般来说在仿真误差达到厘米级就认为可以接受了。如果是实车大概是 0.1 0.1 0.1 米左右也可以接受因为仿真没有噪声是理想情况所以要对仿真的要求更严格一点。
6.2 仿真与实车的差别 但是实车情况不太可能像仿真那样实际的汽车有以下几个特点 汽车不是自行车模型汽车运行时的侧偏刚度会变化汽车都会有存在转向不足的问题 实车肯定要考虑到转向不足所以如果自己搭建模型也有可能会遇到横向误差太大的问题。 七、解决横向误差的办法 横向误差太大该如何避免 有以下三种方法 调节LQR参数 把侧偏刚度估计准一点。调整LQR的Q矩阵 如果横向误差太大就需要调 Q Q Q 矩阵给横向误差 e d e_d ed​ 更大的惩罚值如果 e d e_d ed​过大就会给很大的惩罚值这样就尽可能的让 e d ed ed​收敛到 0 0 0 。 e φ e\varphi eφ​ 是不太可能收敛到 0 0 0 的因为 e φ e_\varphi eφ​ 稳态误差就是 − β -\beta −β。所以只要把 e d ed ed​ 的权重改大但不能太大否则导致超调。处理转向不足 因为转向不足而导致的横向误差太大 八、转向不足及过度转向 下面讲一下转向不足以及过度转向比如这里有辆车直接用自行车模型表示 如果给这样的前轮转角理论上应该按照红色弧线行驶但可能实际上并不是按红色轨迹跑可能出现过度转向或转向不足这是汽车本身的特性。 8.1 转向不足与过度转向的原因 为什么会发生转向不足或者过度转向呢 这里简单解释一下比如这里有一辆车 要往左转那么自然就会受到侧向力的作用假设为 F y 1 F{y 1} Fy1​ 和 F y 2 F_ {y2} Fy2​。如果 F y 1 F{y 1} Fy1​ 和 F y 2 F {y2} Fy2​ 之间不相匹配就会导致在车的质心处有力矩存在。 8.2 力矩平衡与转向特性 什么叫相匹配什么叫不相匹配 简单举个例子比如这样的车 前轮和后轮都受到力的作用到质心的距离分别是 a a a 和 b b b 。 若 F 1 ⋅ a F 2 ⋅ b F_1\cdot aF_2\cdot b F1​⋅aF2​⋅b则力矩平衡导致中心转向。若 F 1 ⋅ a F 2 ⋅ b F_1\cdot aF_2\cdot b F1​⋅aF2​⋅b则质心有正力矩导致过度转向。若 F 1 ⋅ a F 2 ⋅ b F_1\cdot aF_2\cdot b F1​⋅aF2​⋅b则质心有负力矩导致转向不足。 8.3 车辆设计与转向特性的关系 一般市面上买到的车基本上都是转向不足的这是为了安全考虑。但是如果是赛车一般会调教成中心转向因为赛车需要更灵敏的转向所以要调成中心转向。 为什么不调成过度转向呢 因为车天生就有过度转向的趋势即在高速情况下 F 1 ⋅ a F 2 ⋅ b F_1\cdot aF_2\cdot b F1​⋅aF2​⋅b这是轮胎的特性。 所以一般来说 如果是转向不足在高速下可能会变成中心转向如果是中心转向在高速下可能会变成过度转向 所以就没有必要调整成过度转向的即使是赛车也是中心转向为主。实车调试都有转向不足的问题就会导致横向误差比较大。 8.4 使用 PID 控制器处理转向不足 那怎么去处理这件事情 一般在模型里用 PID 但只用 PID 的积分模块其他项都是 0 0 0 放到横向误差处做积分因为有转向不足就会有误差就对误差做积分然后把误差积分和得到的转角相减再输入到前轮转角即将误差先做积分再补偿到前轮转角中。 很容易理解因为 e d e_d ed​ 向左误差为正那么角度也是向左为正所以一旦 e d e_d ed​ 为正就意味着方向盘往左打多了所以要减掉多打的角度。 注意弧度到角度的单位换算应该是先做减法再做单位换算。增益模块放到后面对于实车来说如果加了的话提升会比较大。 因为转向不足做了只用积分的 PID 积分参数可以自己调节。 九、总结 这样本系列就基本结束了全部都讲完了。如果各位一直做到现在可以发现如果纯用 Matlab 确实不行因为太慢了特别是第 12 12 12 节的模型实际上跑起来比较费时间特别是要调的话每调一次 PID 参数都要跑一次都要费很多长时间。再加上只是纯控制模型还没有把规划加进去。规划只是接口给定起点、终点然后算法自动规划一条曲线车就按照规划路径跑但这只是接口而已还没有做真正的轨迹规划。 一旦把规划集成进去运行会更慢以后的进阶课程可能就不再以纯的 Matlab 为主了可能只是用 Matlab 做简单的算法学习真正要写能用的代码的话肯定是要上 C 的以及用 Linux 的这也是没办法的事情因为快。 本系列博客正式结束了感谢大家的阅读谢谢大家下个系列再见。 参考资料 【基础】自动驾驶控制算法第十二讲 横纵向综合控制(完结) 后记 感谢您耐心阅读这篇关于 自动驾驶控制算法横纵向综合控制 的技术博客。 如果您觉得这篇博客对您有所帮助请不要吝啬您的点赞和评论 您的支持是我继续创作的动力。同时别忘了收藏本篇博客以便日后随时查阅。 让我们一起期待更多的技术分享共同探索移动机器人的无限可能 感谢您的支持与关注让我们一起在知识的海洋中砥砺前行