中sqp的算法原理_Apollo ReferenceLine Smooth--离散点平滑原理

随着人工智能兴起，无人驾驶热潮（泡沫）到来，planning 模块作为无人驾驶大脑部分更是重中之重，一般在进行路径规划算法之前会对原始参考线做一个平滑。因为不管是在SL坐标系下规划还是在XY坐标系下规划，参考线的平滑程度直接影响后续规划轨迹点上曲率，进而影响无人驾驶车辆的横纵向性能表现。本着知其然更要知其所以然的精神，作者斗胆在这篇文章中给出基于Apollo开源代码关于参考线平滑模块中离散点平滑相关部分原理给出公式级别几何解释，追求通俗易懂。如发现错误，欢迎行业大佬批评指正。

如下图所示，由

个离散点组成原始参考线。本文介绍一种方法通过对原始参考线上离散点的有限偏移对原始参考线进行平滑。

优化变量：离散点坐标

优化目标：

• 平滑度
• 长度
• 相对原始点偏移量

目标函数设计：

其中：

为平滑度代价，

为长度代价，

为相对原始点偏移量代价

物理意义：如下图中红色向量

的模，它可以理解为：向量

和 向量

相加结