OSQP-多项式路径平滑

参考：二次规划（QP）样条路径优化_shawfy的博客-CSDN博客_qp二次规划

主要是参考上面这篇文章实现了下代码，几个要点记录一下：

1. 这里的建模是在Frenet系下，用SL之间的多项式关系进行建模的。

2. 我自己建模过程中两个重要概念：

• knot（分段点，或称为锚点）：要用knot - 1个多项式来表征整条路径。
• segment（细分小段，即每两个锚点之间还要多分几个小段）：主要用来画曲线 & 设置边界条件的。

3. 我自己实现过程中，目标函数、边界条件里只考虑到二阶信息，二阶连续，感觉够用了。

4. 补充一下重要的公式：

• Hessian矩阵里，二阶导的积分：

• Hessian矩阵在构造的时候，不同阶的积分是直接加权在一起的
• 约束方面：每个knot处有初值约束 or 连续性约束，非两端点处有上下界约束

5. 直接贴一下效果：

整体是在一个大直道上去做的实验，初值给了个-1.0的偏移，整体看上去还是可以绕过边界 + 保证曲线平滑的。

 6. 代码放在这里了，就花了几个小时写了一下，写的很随意，估计各种bug，仅供参考：

https://github.com/BITcsy/path_smooth