【自动驾驶】控制算法（八）横向控制Ⅳ | 调试与优化

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

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

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

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

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

---

---

引言

  本篇博客是第八节的最后一部分，内容整理自 B站知名up主 忠厚老实的老王 的视频，作为博主的学习笔记，分享给大家共同学习。

  本篇博客把第八节第三小节所讲的模型再优化一下，而且这也是横向控制的最后一节，这一节讲完之后就不再讲横向控制了，之后讲纵向控制。

  思路是把纵向控制与横向控制的基本理论先走一遍，让大家有印象，有了印象之后再讲进阶的横向控制与纵向控制。

---

一、进阶横向控制与基础横向控制的区别

那么进阶的横向控制和基础的横向控制有什么区别呢？

  区别就在于考虑了更多的实际情况：

1.1 道路坡度

  道路不可能是平直的，都存在坡度。

1.2 风阻问题

  在高速控制中，风阻会成为很大的影响因素，所以要考虑风的存在，因为风阻会影响汽车的垂向力，影响汽车的侧偏刚度，所以风的影响也很重要，也要考虑在内。

1.3 识别问题

  比如要考虑坡度对控制的影响，但至少要知道坡度等于多少，即坡度该怎么测。如果无人车为了降成本，没有匹配高精地图（高精地图就是一张非常详细的地图，记载道路信息，包括坡度以及车道信息，但是高精地图非常昂贵）所以如果有的车为了降低成本而不使用高精地图，那么坡度就需要自己估算，这就是识别问题，即如何识别坡度。

1.4 车辆质量变化

  如果是普通的家用小轿车，拉人的重量和车的重量比算不了什么，不需要考虑增加载客对车辆质量的影响；但如果是专门拉货的大卡物流车，拉的货物重量可能是自身车重的好几倍，而事先不可预知拉的货到底多重，所以一旦遇到这种情况，质量就要在线估算。

那么上述这些问题如何解决呢？

  用卡尔曼滤波方法解决，不过关于卡尔曼滤波的使用，可能要到很久以后才会用到，所以现在如果不会也没关系，只要有心眼，有空就去学一学就可以了。

---

二、横向控制模型优化与仿真设置

  本篇博客的任务是把横向控制收尾。

  模型拿到之后不能直接用，要先跑一遍规划的代码以及离线 LQR 代码。当然前提是Carsim以及和 Simulink 的联合仿真设置好，这是一切的前提，然后再跑规划，这样和离线LQR代码在工作区里面就有 $x_r,y_r$之类的变量，然后再跑 Simulink 模型。

2.1 限幅模块的添加

  注意模型中加入了限幅模块：

  因为 LQR 不管物理限制，只是出算角度来。但在真实车辆上有物理限制，方向盘转角，或者车辆的前轮转角不可能转 $360$ 度，所以在模型中也需要有这样的限制，在这里限制的就是 $-1$ 到 $1$，也就是限制前轮转角只能在 $-1$ 弧度到 $1$ 弧度之间范围之内，不能超过范围。

  在上一节博客中跑过模型，看到了结果，当时评价是虽然有控制效果，动画也显示出控制效果：

  但控制效果不好，转角不停地抖来抖去，而且有突变现象，这是很不好的事情，该怎么解决？

  在写误差计算模块里面关于投影点的角度 ${\theta }_r$ 有两个算法：

• 认为投影点的 ${\theta }_r$ 就等于匹配点的 ${\theta }_r$，这是阿波罗算法。

projection_point_thetar=thetar(dmin);%apollo

• 投影点的 ${\theta }_r$，等于匹配点的 ${\theta }_r$ 在加匹配点的曲率 $\kappa$ 再乘 $e_s$

projection_point_thetar=thetar(dmin)+kappar(dmin)*es;%投影点的航向角

  当时用的是阿波罗算法，现在用使用第二个推荐的算法，再跑一遍模型，看一下结果：

  发现现在角度就平滑多了，几乎不抖动，和上面的结果相比好很多了。

2.2 车辆初速度设置

  在跑时一定要将 Carsim 里 Procedure 中的车辆初速度设为 $0$：

  不能设的太快，因为默认是 $120$，或者干脆就不做控制。如果速度太快，道路设计又特别严苛，所以就会报错。

---

三、模型抖动问题的优化

再回到模型上来，可以想想为什么这么改就不抖动了。

3.1 投影点角度算法的改进

  在计算误差 $e_d,{\dot{e}}_d,e_{\phi },{\dot{e}}_{\phi }$ 时，都需要投影点的 ${\theta }_r$ 作为输入参数。根据阿波罗的算法，投影点的 ${\theta }_r$ 等于匹配点的 ${\theta }_r$，然后根据离散的轨迹的规划，把匹配点定义为离车最近的规划点定义为匹配点。而车是不停运动的，在不停运动的过程中，匹配点肯定也会不停变化，这就会导致突变，因为不同点的匹配点 ${\theta }_r$不一样，这就是抖动的根源。

3.2 直线行驶与拐弯时抖动原因分析

为什么在直线行驶时没有抖动呢？

  因为直线行驶时，所有匹配点的 ${\theta }_r$ 都一样是0，或者都是 $180$ 度，那自然就没有抖动，但在转弯时，因为不同匹配点的 ${\theta }_r$ 不一样，所以当车在拐弯时，匹配点在切换时，${\theta }_r$ 就会发生突变。

  这就是车辆在拐弯时，方向盘在不停抖动的根本原因，因为是离散的轨迹点，就会在匹配点之间不停的切换，就会有抖动。

那为什么加了 ${\kappa }_r{e}_s$ 就变好了？

  可以看第七节关于离散轨迹误差计算：

  【自动驾驶】控制算法（七）离散规划轨迹的误差计算

  因为加了 ${\kappa }_r{e}_s$ ，$e_s$ 会随着车辆的运动而连续变化。 ${\kappa }_r{e}_s$ 就是近似于连续的变化，这样就会抵消因为匹配点切换导致 ${\theta }_r$ 突变的影响，加了这项后，就变得相对连续变化了一点，当然不能完全抵消，所以还是会有抖动，但是抖动可忽略不计，可近似认为几乎连续变化的角度。

---

四、模型突变问题及其原因

  但代码只解决了抖动问题，突变问题还是没有解决。比如这里有很大的突变：

那突变是什么原因导致的呢？

  这其实是因为规划不合理而导致曲率不连续，即规划的轨迹曲率不连续，所以导致曲率突变，从而导致计算出来的前轮转角有突变。

  在图中红框拐弯处都有曲率突变的情况出现：

  因为规划时就比较简单粗暴，直接用直线和圆连接，并没有做很好的过渡，所以就导致在连接处曲率不连续，不连续就会导致突变。

---

五、解决方案：改善规划与LQR参数调整

那问题该怎么解决呢？

5.1 样条曲线插值实现平滑过渡

  其实就是做好过渡就可以了，在曲线与曲线之间的连接处，用样条曲线插值，因为样条曲线可以保证连接处的一阶导数以及二阶导数连续，如果一阶导数、二阶导数连续的话，曲率就连续，因为曲率的公式就是
$$\kappa =\frac{y^{\prime \prime }}{{\left(1+y^{\prime 2}\right)}^{\frac{3}{2}}}$$  如果的二阶导数连续，那么一阶导数也连续，也就意味着整个曲率函数就连续。

   在《车辆动力学及控制》这本书里也介绍了另一种过渡方法，使用回旋线，回旋线是一种特殊曲线，曲线的曲率和弧长成正比。

  回旋线是用参数方程描述的，参数方程是特殊函数的积分，具体可以看《车辆动力学及控制》上所介绍的回旋线，但回旋线的作用不大。因为回旋线很难用，参数方程是菲涅尔积分。菲涅尔积分是特殊函数的积分，不太好用，而且设计也不好设计，建议各位如果想过渡就用样条曲线插值就好，最简单。

  这里就不演示如何做样条曲线的插值了。因为在真实应用中，并不是这么干的，这就涉及到规划的知识了。

5.2 实际应用中的路径规划策略

在真实的应用场景中，往往有两种规划：

• 全局路径规划
• 局部路径规划

  在全局路径规划中可以允许曲率不连续，因为是全局路径规划，粗糙的规划就可以了，但在局部路径规划里就会对曲率有要求，所以全局路径规划和局部路径规划要结合。

  一般真实情况是这样做，开车不可能按照死的路线开，还要避让行人以及动物，或是绕过障碍物，都需要局部路径规划。

5.3 平滑转角的两种方法

  但在这里并没有讲局部路径规划，在没有局部路径规划的知识，只有全局路径规划的话，想让转角变得平缓，只有两种办法：

• 做好过渡
  用样条曲线使过渡段的连曲率尽可能的连续。

• 在 LQR 里把 R 调大
  意味着对前端转角给予更多的惩罚，在 LQR 里就倾向于算出比较小的转角出来，就算有突变也不会特别明显。

  所以根据样条曲线进行插值的全局路径规划就不演示了，因为几乎用不到，以后会讲全局路径和局部路径相结合的规划方式。局部路径规划需要把纵向控制学完后才能讲，因为局部路径规划是横向控制和综合控制结合在一起才会用。

---

六、规划点密度对算法性能的影响

6.1 规划点稀疏化对转角的影响

  算法可以大幅改进转角，不会过于抖动，所以即使规划点变稀疏，算法性能也是可以的，不妨可以试一试，把 count 的从 $100$ 改到 $30$，规划点变稀疏，但转角也不会发生特别大的抖动。

  注意：规划不能特别少，特别少是不可以的，比如 count = 5。

6.2 规划点过稀疏的弊端

  规划非常稀疏，就这么一点是不行的，用这种规划点运行，一定会报错，如果点和点之间过于稀疏的话，车很可能会绕围绕着其中一点不停打转，和其他两个点之间的距离特别远。因为 LQR 会倾向于让 $e_d,{\dot{e}}_d,e_{\phi },{\dot{e}}_{\phi }$ 达到最小。所以当点离其他点特别远的话，车就会倾向于靠近一点，就会出现以某一点为圆心，不停转来转去的现象。

  当转来转去时，$e_d,{\dot{e}}_d,e_{\phi },{\dot{e}}_{\phi }$ 就会越来越小，

为什么规划稠密了后就不会有这样的现象了？

  因为规划稠密后匹配点就换了，车在不停运动，匹配点的定义就是与车辆真实位置相比最短的点就是匹配点，如果规划点够稠密，匹配点就会沿着轨迹不停变动，从而保证车沿规划轨迹行驶。

  当点特别稀疏时，意味着车可能在很长一段时间匹配点都是同一点，所以就会倾向于往匹配点上靠近变成绕来绕去的情况，从而报错。所以规划点可以在一定程度上稀疏，但不能特别稀疏。

---

七、总结

  本篇博客探讨了横向控制算法的优化，重点关注了进阶横向控制与基础横向控制之间的差异，以及如何通过卡尔曼滤波等方法解决实际问题。总结了道路坡度、风阻、识别问题和车辆质量变化对横向控制的影响，并探讨了如何使用样条曲线插值和调整LQR参数来改善转角平滑度和减少突变现象。

  通过这些改进，我们发现即使在规划点稀疏的情况下，算法性能依然可以保持，也强调了规划点不能过于稀疏的重要性，因为过于稀疏的规划会导致车辆偏离预定轨迹，甚至出现异常行为。

  本篇博客把所有要讲的内容都讲完了，接下来是纵向控制，感谢大家阅读，下一篇博客讲解车辆的纵向控制。

---

参考资料

  【基础】自动驾驶控制算法第八讲(四) 调试与优化(第八讲完结）

---

后记：

🌟 感谢您耐心阅读这篇关于 横向控制算法调试与优化 的技术博客。 📚

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

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

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

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