写在前面：
🌟 欢迎光临 清流君 的博客小天地，这里是我分享技术与心得的温馨角落 。📝
个人主页 ：清流君_CSDN博客，期待与您一同探索 移动机器人 领域的无限可能。🔍 本文系 清流君 原创之作，荣幸在CSDN首发🐒
若您觉得内容有价值，还请评论告知一声，以便更多人受益。
转载请注明出处，尊重原创，从我做起。👍 点赞、评论、收藏 ，三连走一波，让我们一起养成好习惯😜
在这里，您将收获的不只是技术干货 ，还有思维的火花！📚 系列专栏：【运动控制】系列，带您深入浅出，领略控制之美。🖊
愿我的分享能为您带来启迪，如有不足，敬请指正，让我们共同学习，交流进步！🎭 人生如戏，我们并非能选择舞台和剧本，但我们可以选择如何演绎 🌟
感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行~~~
文章目录
- 引言
- 一、进阶横向控制与基础横向控制的区别
-
- [1.1 道路坡度](#1.1 道路坡度)
- [1.2 风阻问题](#1.2 风阻问题)
- [1.3 识别问题](#1.3 识别问题)
- [1.4 车辆质量变化](#1.4 车辆质量变化)
- 二、横向控制模型优化与仿真设置
-
- [2.1 限幅模块的添加](#2.1 限幅模块的添加)
- [2.2 车辆初速度设置](#2.2 车辆初速度设置)
- 三、模型抖动问题的优化
-
- [3.1 投影点角度算法的改进](#3.1 投影点角度算法的改进)
- [3.2 直线行驶与拐弯时抖动原因分析](#3.2 直线行驶与拐弯时抖动原因分析)
- 四、模型突变问题及其原因
- 五、解决方案：改善规划与LQR参数调整
-
- [5.1 样条曲线插值实现平滑过渡](#5.1 样条曲线插值实现平滑过渡)
- [5.2 实际应用中的路径规划策略](#5.2 实际应用中的路径规划策略)
- [5.3 平滑转角的两种方法](#5.3 平滑转角的两种方法)
- 六、规划点密度对算法性能的影响
-
- [6.1 规划点稀疏化对转角的影响](#6.1 规划点稀疏化对转角的影响)
- [6.2 规划点过稀疏的弊端](#6.2 规划点过稀疏的弊端)
- 七、总结
- 参考资料
引言
本篇博客是第八节的最后一部分，内容整理自 B站知名up主 忠厚老实的老王 的视频，作为博主的学习笔记，分享给大家共同学习。
本篇博客把第八节第三小节所讲的模型再优化一下，而且这也是横向控制的最后一节，这一节讲完之后就不再讲横向控制了，之后讲纵向控制。
思路是把纵向控制与横向控制的基本理论先走一遍，让大家有印象，有了印象之后再讲进阶的横向控制与纵向控制。
一、进阶横向控制与基础横向控制的区别
那么进阶的横向控制和基础的横向控制有什么区别呢？
区别就在于考虑了更多的实际情况：
1.1 道路坡度
道路不可能是平直的，都存在坡度。
1.2 风阻问题
在高速控制中，风阻会成为很大的影响因素，所以要考虑风的存在，因为风阻会影响汽车的垂向力，影响汽车的侧偏刚度，所以风的影响也很重要，也要考虑在内。
1.3 识别问题
比如要考虑坡度对控制的影响，但至少要知道坡度等于多少，即坡度该怎么测。如果无人车为了降成本，没有匹配高精地图（高精地图就是一张非常详细的地图，记载道路信息，包括坡度以及车道信息，但是高精地图非常昂贵）所以如果有的车为了降低成本而不使用高精地图，那么坡度就需要自己估算，这就是识别问题，即如何识别坡度。
1.4 车辆质量变化
如果是普通的家用小轿车，拉人的重量和车的重量比算不了什么，不需要考虑增加载客对车辆质量的影响；但如果是专门拉货的大卡物流车，拉的货物重量可能是自身车重的好几倍，而事先不可预知拉的货到底多重，所以一旦遇到这种情况，质量就要在线估算。
那么上述这些问题如何解决呢？
用卡尔曼滤波方法解决，不过关于卡尔曼滤波的使用，可能要到很久以后才会用到，所以现在如果不会也没关系，只要有心眼，有空就去学一学就可以了。
二、横向控制模型优化与仿真设置
本篇博客的任务是把横向控制收尾。
模型拿到之后不能直接用，要先跑一遍规划的代码以及离线 LQR 代码。当然前提是Carsim以及和 Simulink 的联合仿真设置好，这是一切的前提，然后再跑规划，这样和离线LQR代码在工作区里面就有 x r , y r x_r,y_r xr,yr之类的变量，然后再跑 Simulink 模型。
2.1 限幅模块的添加
注意模型中加入了限幅模块：
因为 LQR 不管物理限制，只是出算角度来。但在真实车辆上有物理限制，方向盘转角，或者车辆的前轮转角不可能转 360 360 360 度，所以在模型中也需要有这样的限制，在这里限制的就是 − 1 - 1 −1 到 1 1 1，也就是限制前轮转角只能在 − 1 - 1 −1 弧度到 1 1 1 弧度之间范围之内，不能超过范围。
在上一节博客中跑过模型，看到了结果，当时评价是虽然有控制效果，动画也显示出控制效果：
但控制效果不好，转角不停地抖来抖去，而且有突变现象，这是很不好的事情，该怎么解决？
在写误差计算模块里面关于投影点的角度 θ r \theta_r θr 有两个算法：
- 认为投影点的 θ r \theta_r θr 就等于匹配点的 θ r \theta_r θr，这是阿波罗算法。
matlab
projection_point_thetar=thetar(dmin);%apollo- 投影点的 θ r \theta_r θr，等于匹配点的 θ r \theta_r θr 在加匹配点的曲率 κ \kappa κ 再乘 e s e_s es
matlab
projection_point_thetar=thetar(dmin)+kappar(dmin)*es;%投影点的航向角当时用的是阿波罗算法，现在用使用第二个推荐的算法，再跑一遍模型，看一下结果：
发现现在角度就平滑多了，几乎不抖动，和上面的结果相比好很多了。
2.2 车辆初速度设置
在跑时一定要将 Carsim 里 Procedure 中的车辆初速度设为 0 0 0：
不能设的太快，因为默认是 120 120 120，或者干脆就不做控制。如果速度太快，道路设计又特别严苛，所以就会报错。
三、模型抖动问题的优化
再回到模型上来，可以想想为什么这么改就不抖动了。
3.1 投影点角度算法的改进
在计算误差 e d , e ˙ d , e φ , e ˙ φ e_d,\dot e_d,e_\varphi,\dot e_\varphi ed,e˙d,eφ,e˙φ 时，都需要投影点的 θ r \theta_r θr 作为输入参数。根据阿波罗的算法，投影点的 θ r \theta_r θr 等于匹配点的 θ r \theta_r θr，然后根据离散的轨迹的规划，把匹配点定义为离车最近的规划点定义为匹配点。而车是不停运动的，在不停运动的过程中，匹配点肯定也会不停变化，这就会导致突变，因为不同点的匹配点 θ r \theta_r θr不一样，这就是抖动的根源。
3.2 直线行驶与拐弯时抖动原因分析
为什么在直线行驶时没有抖动呢？
因为直线行驶时，所有匹配点的 θ r \theta_r θr 都一样是0，或者都是 180 180 180 度，那自然就没有抖动，但在转弯时，因为不同匹配点的 θ r \theta_r θr 不一样，所以当车在拐弯时，匹配点在切换时， θ r \theta_r θr 就会发生突变。
这就是车辆在拐弯时，方向盘在不停抖动的根本原因，因为是离散的轨迹点，就会在匹配点之间不停的切换，就会有抖动。
那为什么加了 κ r e s \kappa_r e_s κres 就变好了？
可以看第七节关于离散轨迹误差计算：
因为加了 κ r e s \kappa_r e_s κres ， e s e_s es 会随着车辆的运动而连续变化。 κ r e s \kappa_r e_s κres 就是近似于连续的变化，这样就会抵消因为匹配点切换导致 θ r \theta_r θr 突变的影响，加了这项后，就变得相对连续变化了一点，当然不能完全抵消，所以还是会有抖动，但是抖动可忽略不计，可近似认为几乎连续变化的角度。
四、模型突变问题及其原因
但代码只解决了抖动问题，突变问题还是没有解决。比如这里有很大的突变：
那突变是什么原因导致的呢？
这其实是因为规划不合理而导致曲率不连续，即规划的轨迹曲率不连续，所以导致曲率突变，从而导致计算出来的前轮转角有突变。
在图中红框拐弯处都有曲率突变的情况出现：
因为规划时就比较简单粗暴，直接用直线和圆连接，并没有做很好的过渡，所以就导致在连接处曲率不连续，不连续就会导致突变。
五、解决方案：改善规划与LQR参数调整
那问题该怎么解决呢？
5.1 样条曲线插值实现平滑过渡
其实就是做好过渡就可以了，在曲线与曲线之间的连接处，用样条曲线插值，因为样条曲线可以保证连接处的一阶导数以及二阶导数连续，如果一阶导数、二阶导数连续的话，曲率就连续，因为曲率的公式就是
κ = y ′ ′ ( 1 + y ′ 2 ) 3 2 \kappa =\frac{y''}{\left( 1+y'^2 \right) ^{\frac{3}{2}}} κ=(1+y′2)23y′′ 如果的二阶导数连续，那么一阶导数也连续，也就意味着整个曲率函数就连续。
在《车辆动力学及控制 》这本书里也介绍了另一种过渡方法，使用回旋线，回旋线是一种特殊曲线，曲线的曲率和弧长成正比。
回旋线是用参数方程描述的，参数方程是特殊函数的积分，具体可以看《车辆动力学及控制》上所介绍的回旋线，但回旋线的作用不大。因为回旋线很难用，参数方程是菲涅尔积分。菲涅尔积分是特殊函数的积分，不太好用，而且设计也不好设计，建议各位如果想过渡就用样条曲线插值就好，最简单。
这里就不演示如何做样条曲线的插值了。因为在真实应用中，并不是这么干的，这就涉及到规划的知识了。
5.2 实际应用中的路径规划策略
在真实的应用场景中，往往有两种规划：
- 全局路径规划
- 局部路径规划
在全局路径规划中可以允许曲率不连续，因为是全局路径规划，粗糙的规划就可以了，但在局部路径规划里就会对曲率有要求，所以全局路径规划和局部路径规划要结合。
一般真实情况是这样做，开车不可能按照死的路线开，还要避让行人以及动物，或是绕过障碍物，都需要局部路径规划。
5.3 平滑转角的两种方法
但在这里并没有讲局部路径规划，在没有局部路径规划的知识，只有全局路径规划的话，想让转角变得平缓，只有两种办法：
-
做好过渡
用样条曲线使过渡段的连曲率尽可能的连续。
-
在 LQR 里把 R 调大
意味着对前端转角给予更多的惩罚，在 LQR 里就倾向于算出比较小的转角出来，就算有突变也不会特别明显。
所以根据样条曲线进行插值的全局路径规划就不演示了，因为几乎用不到，以后会讲全局路径和局部路径相结合的规划方式。局部路径规划需要把纵向控制学完后才能讲，因为局部路径规划是横向控制和综合控制结合在一起才会用。
六、规划点密度对算法性能的影响
6.1 规划点稀疏化对转角的影响
算法可以大幅改进转角，不会过于抖动，所以即使规划点变稀疏，算法性能也是可以的，不妨可以试一试，把 count 的从 100 100 100 改到 30 30 30，规划点变稀疏，但转角也不会发生特别大的抖动。
注意 ：规划不能特别少，特别少是不可以的，比如 count = 5。
6.2 规划点过稀疏的弊端
规划非常稀疏，就这么一点是不行的，用这种规划点运行，一定会报错，如果点和点之间过于稀疏的话，车很可能会绕围绕着其中一点不停打转，和其他两个点之间的距离特别远。因为 LQR 会倾向于让 e d , e ˙ d , e φ , e ˙ φ e_d,\dot e_d,e_\varphi,\dot e_\varphi ed,e˙d,eφ,e˙φ 达到最小。所以当点离其他点特别远的话，车就会倾向于靠近一点，就会出现以某一点为圆心，不停转来转去的现象。
当转来转去时， e d , e ˙ d , e φ , e ˙ φ e_d,\dot e_d,e_\varphi,\dot e_\varphi ed,e˙d,eφ,e˙φ 就会越来越小，
为什么规划稠密了后就不会有这样的现象了？
因为规划稠密后匹配点就换了，车在不停运动，匹配点的定义就是与车辆真实位置相比最短的点就是匹配点，如果规划点够稠密，匹配点就会沿着轨迹不停变动，从而保证车沿规划轨迹行驶。
当点特别稀疏时，意味着车可能在很长一段时间匹配点都是同一点，所以就会倾向于往匹配点上靠近变成绕来绕去的情况，从而报错。所以规划点可以在一定程度上稀疏，但不能特别稀疏。
七、总结
本篇博客探讨了横向控制算法的优化，重点关注了进阶横向控制与基础横向控制之间的差异，以及如何通过卡尔曼滤波等方法解决实际问题。总结了道路坡度、风阻、识别问题和车辆质量变化对横向控制的影响，并探讨了如何使用样条曲线插值和调整LQR参数来改善转角平滑度和减少突变现象。
通过这些改进，我们发现即使在规划点稀疏的情况下，算法性能依然可以保持，也强调了规划点不能过于稀疏的重要性，因为过于稀疏的规划会导致车辆偏离预定轨迹，甚至出现异常行为。
本篇博客把所有要讲的内容都讲完了，接下来是纵向控制，感谢大家阅读，下一篇博客讲解车辆的纵向控制。
参考资料
【基础】自动驾驶控制算法第八讲(四) 调试与优化(第八讲完结）
后记：
🌟 感谢您耐心阅读这篇关于 横向控制算法调试与优化 的技术博客。 📚
🎯 如果您觉得这篇博客对您有所帮助，请不要吝啬您的点赞和评论 📢
🌟您的支持是我继续创作的动力。同时，别忘了收藏本篇博客 ，以便日后随时查阅。🚀
🚗 让我们一起期待更多的技术分享，共同探索移动机器人 的无限可能！💡
🎭感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行 🚀