写在前面:
🌟 欢迎光临 清流君 的博客小天地,这里是我分享技术与心得的温馨角落。📝
🎭 人生如戏,我们并非能选择舞台和剧本,但我们可以选择如何演绎 🌟感谢您的支持与关注,让我们一起在知识的海洋中砥砺前行~~~
文章目录
- 前言
- 一、算法输入与输出
- 1、算法输入
- 2、算法输出
- 二、算法流程
- 1、算法过程概述
- 2、算法流程图
- 三、具体算法
- 1、AB 计算模块
- 2、LQR模块
- (1)急加速
- (2)急速过弯
- 3、误差与曲率计算模块
- (1)规划点
- (2)匹配点
- (3)方向向量
- (4)误差的距离的向量
- (5)横向误差
- (6)纵向误差
- (7)投影点切线方向与x轴夹角
- (8)横向误差导数
- (9)航向角误差
- (10)弧速度
- (11)横摆角速度误差
- (12)投影点曲率
- (13)输出
- 4、前馈控制计算模块
- 5、最终控制计算模块
- 四、预测模块
- 1、预测模块的重要性
- 2、预测模块算法
- 五、完整算法流程图
- 六、总结
- 参考资料
前言
本篇博客是 自动驾驶控制算法 系列的第八节第Ⅰ部分。内容整理自 B站知名up主 忠厚老实的老王 的视频,作为博主的学习笔记,分享给大家共同学习。
本节内容是整个横向控制的核心,要把前七节所有内容都用在第八节上。第八节分四小节,本小节讲解具体算法,下一小节讲解模型搭建和代码编程,再下一小节讲解代码优化、注意事项以及仿真结果。
算法这一讲可以说是整个第八节的核心所在。写代码、做仿真的来源是算法,有了具体算法后,无论用什么平台仿真,用什么语言编写程序,都是水到渠成的事情。只要照着算法编程逻辑是十分清晰的,因为算法已经把逻辑梳理好了,只要照着算法编就可以了。
相反如果没有写过算法,然后就直接就去编程也可以,但只针对较简单的项目、简单的算法。对于复杂算法,建议各位先把算法写好,把逻辑理清楚,然后再编程。
本篇博客讲解横向控制算法,前面讲到的控制律:
关于这些量的计算:
第四节讲的是连续规划轨迹的误差计算,第七节是讲的是离散规划轨迹的误差计算。
将前七节所有的知识把综合起来,就变成了第八节,讲解横向控制的完整算法。横向控制很复杂,模块非常多,所以必须要把算法梳理一遍,不然很容易出错。
一、算法输入与输出
1、算法输入
根据前七讲,算法输入分为三类:
- 整车参数
轴距L、质心到前后轮的距离 - 车辆位置与状态
包括车辆的 - 规划轨迹点
轨迹点为四维数组,包含坐标
2、算法输出
算法输出为
二、算法流程
1、算法过程概述
第一步:通过整车参数加上
第二步:车辆的位置状态,加上规划轨迹点,可以算出误差
第三步:第一步得到反馈矩阵
第四步:第一步得到了
2、算法流程图
流程图可以直观地反映算法的结构,基本上流程画出来,算法逻辑顺序就理清楚了,流程图如下:
-
首先是整车参数和 - LQR模块
把 - 误差曲率计算模块
接下来是车辆状态及速度,加上规划轨迹点以及 - 前馈控制计算模块
整车参数再加上 - 最终控制计算模块
最后将反馈矩阵
流程图包含了从第三节到第七节的所有内容,这就是后续编程的理论依据,以后写代码就是按照流程图来写。
可以看一下整个算法分为五个模块,写算法具体就是写这五个模块,模块写出来了,算法也就编写出来了。
三、具体算法
下面看一下这五个模块的算法到底该如何编写。
1、AB 计算模块
注意:要考虑当
2、LQR模块
注意:整车参数只是近似认为不变,并不是一成不变。在某些情况下就是整车参数不能认为近似不变,必须要考虑整车参数变化所带来对
在什么情况下不可近似处理呢?
(1)急加速
如果车辆在疯狂加速,即加速度非常大、非常猛时:
(2)急速过弯
非常急速的过弯时,整个车都倾斜起来:
此时车左右轮明显不对称,因为右轮几乎承受了车的全部重量,而左轮几乎没有承受任何力,导致自行车模型不再适用。
所有的理论都基于自行车模型,即把左右两个轮认为是一个轮,等于是把车压扁了,意味着自行车模型只适用于左右对称或是近似对称的情况。而在极速过弯情况下,不能认为左右轮对称。
比如有如下形式的对应表:
|
|
0.01 |
|
0.02 |
|
0.03 |
|
… | … |
50 |
|
查表法计算 LQR 的优缺点如下:
- 优点
大大加快计算速度 - 缺点
耗费存储空间
这是典型为了加快程序运算速度而采用空间换时间的算法,通过查表法算, LQR 可以把速度提升好几个数量级,特别是在控制中,对实时性的要求是最高的,这就意味着必须要尽一切可能加快程序运算速度。
一般在实际应用中的 LQR 不是通过调包计算的,而是查表查出来的。而且查表法算LQR 对高速运动的控制也有效果,因为高速和低速的区别就在于侧偏刚度变化,其他变化不大。
3、误差与曲率计算模块
该模块是整个算法的核心。
(1)规划点
首先要有规划轨迹,就是一系列的四维数组点:
(2)匹配点
【序列号】是距离最短的点的下标序列,遍历所有规划点,比如找到第四个规划点距离车辆当前位置
最近,那么。最短距离的点叫做匹配点,
(3)方向向量
(4)误差的距离的向量
(5)横向误差
(6)纵向误差
(7)投影点切线方向与x轴夹角
(8)横向误差导数
(9)航向角误差
(10)弧速度
(11)横摆角速度误差
(12)投影点曲率
(13)输出
第三个模块的算法是整个控制算法中最复杂的,这一块涉及到的公式很多,而且很容易出问题。
所以第九步最容易出错,在具体写代码时注意避免。
4、前馈控制计算模块
5、最终控制计算模块
四、预测模块
但这样还不够,因为这五个模块只是前七节讲到的内容,为了更好地体现算法性能,还要再加预测模块。
1、预测模块的重要性
为什么要加预测模块?
首先因为车本身具有惯性,导致控制天生有滞后性。为了解决这个问题,要进行提前控制,加入预测模块。
这里具体解释一下。比如有一段规划轨迹:
如果是人开车,看到这样一条路径规划,会不会去打方向盘?应该不会,因为人具有预见性,能看到未来路径到底长什么样。虽然现在的状态和规划路径有误差,但是如果保持当前状态行驶,在未来一段时间内,车和规划路径就没有误差了。所以人不会打方向盘,因为人能知道未来的路径规划是什么。
但算法不行,算法开车只会机械地寻找与真实位置最接近的规划轨迹点。如果算出误差不等于0,那么算法就会打方向盘。
下面的例子也说明了同样的道理:
人开车的话,知道未来的路径规划不是直线,所以即使现在的运动状态和规划的轨迹完全贴合,没有误差,人也会提前动方向盘。
所以要加上预测模块,为了更好地控制,算法需要具有一定的预见性。
预测就是在车辆当前状态下,假设车辆在一段时间内做匀速直线运动,运动后的点就是预测点。比如说下面这样:
2、预测模块算法
下面编写具体的预测算法,比如下图的直角坐标系:
根据几何关系有:
五、完整算法流程图
加入预测模块后的算法流程图:
六、总结
这才是想要的完整算法,后续编程就按照算法流程图来编写,一共六大模块,已经全部讲完。
本篇博客就写到这里,但第八节还没有完,下一小节讲解具体的代码编写以及模型搭建。欢迎关注!
参考资料
后记:
🌟 感谢您耐心阅读这篇关于 横向控制Ⅰ | 算法与流程 的技术博客。 📚
🎯 如果您觉得这篇博客对您有所帮助,请不要吝啬您的点赞和评论 📢
🌟您的支持是我继续创作的动力。同时,别忘了收藏本篇博客,以便日后随时查阅。🚀
🚗 让我们一起期待更多的技术分享,共同探索移动机器人的无限可能!💡
🎭感谢您的支持与关注,让我们一起在知识的海洋中砥砺前行 🚀