【自动驾驶】决策规划算法（二）参考线模块Ⅰ| 平滑算法与二次规划

写在前面：
🌟 欢迎光临 清流君 的博客小天地，这里是我分享技术与心得的温馨角落。📝
🎭 人生如戏，我们并非能选择舞台和剧本，但我们可以选择如何演绎 🌟感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行~~~

文章目录

• 引言
• 一、自动驾驶决策规划算法概述
• 二、参考线

• 2.1 参考线的作用
• 2.2 过长路径的缺点

• (1) 匹配点难以确定
• (2) 障碍物投影不唯一
• (3) 导航路径不平滑

• 2.3 生成参考线的方法
• 2.4 参考线的优点

• 三、参考线平滑算法

• 3.1 平滑算法的代价函数
• 3.2 转化为二次规划问题

• (1) 平滑代价
• (2) 紧凑代价
• (3) 几何相似代价

• 3.3 约束问题

• (1) 距离约束
• (2) 曲率约束

• 四、算法加速方法

• 4.1 降低执行频率
• 4.2 轨迹拼接

• 五、参考线平滑算法难点

• 5.1 快速找到车在全局路径下的投影点
• 5.2 执行频率的调度问题

• 六、总结
• 参考资料

引言

  各位小伙伴们大家好，欢迎收看自动驾驶决策规划算法第二节，内容整理自 B站知名up主 忠厚老实的老王 的视频，作为博主的学习笔记，分享给大家共同学习。

一、自动驾驶决策规划算法概述

  本篇博客所讲的内容是 参考线(Reference Line)，在讲参考线之前，先讲一下决策规划的总体流程。假设已经有了导航路径，决策规划流程如下：

• 

  定位 + 导航：生成参考线

• 

  障碍物投影：静态障物投影到以参考线为坐标轴的 Frenet 坐标系上

• 

  开辟凸空间：决策算法对障碍物做决策(往左绕、往右绕、忽略)开辟凸空间

• 

  搜索最优路径：规划算法在决策算法所开辟的凸空间内搜索出一条最优路径

• 

  后处理：在规划中轨迹中选点坐标转化成笛卡尔坐标系，再输出给控制去跟踪。

  比如在自然坐标系下有如下障碍物：

  在第三步中，如果决策算法选择了往左绕，就意味着决策算法开辟了上图紫色的凸空间；如果决策算法决定往右绕，开辟的是蓝色的凸空间，也就是最终规划轨迹要么在紫色的凸空间中搜索，要么在蓝色凸空间中搜索，到底在哪里搜索是决策算法所干的事情。

  决策算法开辟最优凸空间，在凸空间内搜索轨迹，把轨迹交给控制执行。

  在第四步中，比如决策算法已经决策出来了，在蓝色的凸空间上搜索，规划就是在蓝色的凸空间上搜索出一条最优路径。

  这就是整个决策规划的具体步骤，相对来说比较粗略，而且没有考虑动态障碍物。因为暂时先不管动态障碍，先把静态障碍物做出来，凡事都由简到难、由简单到复杂。

二、参考线

  本篇博客就是讲怎么通过定位加导航的路径生成参考线。

2.1 参考线的作用

  首先要讲一下参考线是干什么的，参考线是一种解决方案：解决导航路径过长且不平滑的问题。

  比如如果导航计算出来的路径是非常长的路径，过长的路径不利于坐标转换，比如下图：

  在本系列博客中，数学基础部分第三节的坐标转换，核心步骤就是找匹配点，具体参见：

  【自动驾驶】决策规划算法 | 数学基础(三)直角坐标与自然坐标转换Ⅰ

  【自动驾驶】决策规划算法 | 数学基础(三)直角坐标与自然坐标转换Ⅱ

2.2 过长路径的缺点

(1) 匹配点难以确定

  路径越长，找匹配点就越麻烦，因为找匹配点需要遍历，路径越长点就越多，越多找匹配点就越慢。

• 如果是只是自车的坐标转换，那还好办，因为只需要做一次。
• 如果是障碍物也要做坐标转换，每个障碍物都要做投影，计算量就非常大。

(2) 障碍物投影不唯一

  比如在上图路径中的绿色障碍物，投影可能有两个，俩距离都相等，到底哪个才是它的投影？这就是个非常麻烦的问题。

(3) 导航路径不平滑

  导航路径一般为平滑曲线，上图中的蓝色点导数都不连续、不平滑。

  所以解决方案就是参考线，就是在全局路径中截取一小段较短的路径，进行平滑，平滑后的曲线即为参考线，将参考线作为障碍物投影的坐标轴线。

2.3 生成参考线的方法

  比如下图是不平滑的导航路径：

，往前取

，取这些范围内的点。

，弧长是

，后面取紫红色线

2.4 参考线的优点

  参考线能解决上面所说的导航路径过长的问题，因为较短的参考线投影比较好找，而且短的话一般曲率也不会太大，投影就是唯一的，而且做过平滑，比全局路径更平滑，这就是参考线作用。

三、参考线平滑算法

如何平滑参考线呢？

  参考线平滑算法不唯一，这里只讲其中一种平滑算法，认为点与点之间越接近直线就越平滑，越不接近直线就越不平滑。

。

向量的长度就是衡量平滑与不平滑的标准。如果

  但参考线不是越平滑越好，比如下面黑色的三个点是原来全局路径的点，如果越平滑越好，可能会优化成像紫色的这样一条线。

  虽然平滑了，但几何形状和原来的路径点差距实在是太大，这样也不好。

3.1 平滑算法的代价函数

如何衡量和几何形状相关的标准？

，优化后的点记为

。衡量与几何相似度的标准就是，如果这三条绿线的长度

  除了平滑性因素，还有道路几何因素，以及长度要尽可能均匀和紧凑的因素。

  比如这是三个原来的黑色点：

  希望优化点变成紫色线。但如果像绿色线过于分散也不好，所以长度要尽可能均匀、紧凑。

那怎么衡量呢？

  如果

  反之，如果

，后面比较分散的是

，这样就能看出来，用

来衡量，即

  综上就可以写出平滑算法的代价函数，比如下图：

，优化后的点为

。

已知，

  代价函数为

  包含三个代价，第一项为与原路径点相似代价，第二项为平滑代价，第三项为紧凑代价。

  希望代价函数越小越好，因为越小就与原路径点越相似、越平滑、越紧凑。

，使得代价函数最小。

3.2 转化为二次规划问题

  这是典型的二次规划问题，二次规划问题的典型描述为：

(1) 平滑代价

  首先写一下平滑代价的表达式，平方和可以写成向量乘以向量的转置。

  前面向量可以写成

记

  则平滑代价函数为

  这只是

个点的情况，先把平滑代价函数写出来

  一共是

  展开写成向量的形式：

  写成矩阵的形式：

  大括号内的矩阵是将

按照对角线不断复制，每隔两行、每隔两列不断复制。其中，

为

的矩阵，大括号矩阵为

，则平滑代价可以写成：

  因为

是

的矩阵，则

就是

(2) 紧凑代价

紧凑代价直接写

个点的情况，代价函数为：

改写成向量形式：

写成矩阵的形式：

  其中，

为

的矩阵，大括号矩阵为

，则紧凑代价可以写成:

  因为

是

的矩阵，则

就是

(3) 几何相似代价

代价函数为：

  因为

是常数或已知量，所以

是常数，不受

的影响，所以最后一项可以去掉，即

写成二次规划的形式：

  其中，大括号矩阵为单位矩阵，记为

，为

的矩阵。

写成二次规划形式：

其中，

  综上所述，把这三个代价写成二次规划的形式，统一起来：

二次规划的标准形式：

则

3.3 约束问题

  接下来讲约束问题，就是怎么求二次规划的约束。

(1) 距离约束

与

  有人可能觉得有代价函数保证了，就是不有几何形状代价，但可能参数调的不对，更倾向于平滑和紧凑，而几何形状的权重不高，就有可能比较散，所以说有必要再加约束。

和

之间的距离要小于值
buff ，值可以自己确定：

展开

  可取值
buff = 0.1，也可以自己标定，觉得

(2) 曲率约束

和

  而且曲率约束的与车的最大侧向加速度有关，可以放到后面再考虑。

为什么要对曲率做约束？

  因为有些弯可能太急了过不去，车有最大侧向加速度的限制，如果侧向加速度特别大，车可能会翻。但侧向加速度本身既和曲率有关，也和速度有关，所以最大曲率限制没有必要在一开始时就在参考线平滑中考虑，可以在后面速度规划时再考虑相关的曲率。

  所以在这里就先介绍一下曲率到底该怎么计算，看一下为什么曲率约束是非线性，至于曲率约束，在参考线平滑算法暂时不考虑。如果不放心想约束的话，更推荐把 平滑算法目标函数中，关于平滑代价函数权重增大一点，曲率自然会变小。

曲率的计算

  下面看一下曲率该怎么算。

  首先声明一下曲率的计算，是近似的算法，不是精确算法，不过近似程度也够了。

  比如这里有三个点，三点确定圆：

相等，即

，向量求和

遵循平行四边形定则，所以蓝色图形肯定是平行四边形，但又因为

，也是等腰三角形。橙黄色角就是这两个等腰三角形之间的公共角。有两个等腰三角形其中有角是公共角，意味着两个三角形相似，则

则曲率为

  可见

  所以曲率约束在参考线平滑这里一般不加，因为处理起来比较麻烦。

  这样整个参考线以及参考线平滑内容讲解完毕。至于具体实践部分，放到下一篇博客再介绍。

  参考线平滑理论上比较简单，内容不多，目标函数就是三个代价相加，写上二次规划，加约束就可以求解了。

四、算法加速方法

  前面的理论看起来好像不是特别难，但实际上这一节很难。因为难度不难在理论上，而是难在实践上。实践有最大的难点就是快。因为算法不能太慢，因为参考线算法是一切的基础，看开头所说的决策规划流程，第一步就是要计算参考线，剩下的步骤都得以此为基础。所以的参考线平滑算法不能太慢，必须要快。写出参考线平滑算法不难，但让程序运行得非常快，是非常费时间和费功夫。

  从 GitHub 上的模型就能看出来，真正的二次规划算平滑参考线只占非常一小块的地方，有很大部分都是解决怎样让代码运行得更快的。方法不是唯一的，在 GitHub 上的模型写的只是一种加速方法。

4.1 降低执行频率

4.2 轨迹拼接

，往后取

  当然不可能和旧的结果完全一样，因为肯定会有新点加入进来，只需要处理新点即可，把新加进来的点做二次规划，这样二次规划的规模和计算量就会小很多，因为处理的点比较少。

五、参考线平滑算法难点

5.1 快速找到车在全局路径下的投影点

、往后取

，如何去快速找到车在全局路径下的投影点，这也是一个问题。由此可见，解决这些问题有多难，要写轨迹拼接算法。

5.2 执行频率的调度问题

执行一次的话，得写调度，如何让 Simulink 每

六、总结

  在自动驾驶决策规划算法中，参考线是解决导航路径过长且不平滑问题的关键。通过截取全局路径中的一段较短路径并进行平滑处理，简化了障碍物投影和匹配点的确定，使得规划算法能够在较小的范围内搜索最优路径。参考线的优点在于，较短的参考线投影更容易确定，且经过平滑处理后，路径更加平滑。

  参考线平滑算法通过代价函数来优化，代价函数包含了与原路径点相似代价、平滑代价和紧凑代价。通过将代价函数写成二次规划的形式，可以求解出最优的参考线点。在实际应用中，参考线平滑算法面临着许多挑战，特别是在算法的执行速度上。为了提高算法效率，可以采取降低执行频率和轨迹拼接的方法。

  本篇博客就讲解参考线平滑的理论部分，下一篇博客会讲解如何让算法跑得更快，编写具体的实践代码，欢迎关注后续内容！

参考资料

   自动驾驶决策规划算法第二章第二节(上) 参考线模块

后记：

🌟 感谢您耐心阅读这篇关于 参考线平滑算法与二次规划 的技术博客。 📚
🎯 如果您觉得这篇博客对您有所帮助，请不要吝啬您的点赞和评论 📢
🌟您的支持是我继续创作的动力。同时，别忘了收藏本篇博客，以便日后随时查阅。🚀
🚗 让我们一起期待更多的技术分享，共同探索移动机器人的无限可能！💡
🎭感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行 🚀