smac 路径优化器分析——平滑度成本分析

参考

Apollo 离散点曲线平滑度优化

相关文章

smac 路径优化器分析——曲率成本分析

smac 路径优化器分析——距离成本和代价地图成本分析

平滑度成本函数

Smac 用向量和的模的平方来作为曲线离散点的平滑度，非常巧妙。
离散点顺序： pt_m -> pt -> pt_p
向量 u 是 pt -> pt_m，向量 v 是 pt -> pt_p，向量 w = u + v。

fig.1 向量 u 和 v 图示

下面蓝色圆形虚线 ptmNorm 表示向量 u 的模，紫色圆形虚线 ptpNorm 表示向量 v 的模。

1. 在 u 和 v 的模大小固定情况下，夹角 α  从大到小变化（[0,π]范围），此时 w 的模是从小到大变化。