自动驾驶---Motion Planning之STSC轨迹优化

1 背景

        在之前的博客《自动驾驶---Motion Planning之构建SLT Driving Corridor》中，为读者讲解了SLT图构建的思路---通过构建Driving Corridor的方式确定SL两个方向的boundary。但是并没有去详细讲解如何去构造优化问题，以及如何去生成最终的轨迹，所以本篇博客将继续为读者讲解STSC后半部分的内容。

        本篇博客的主要参考文献依然为《Safe Trajectory Generation for Complex Urban Environments Using Spatio-temporal Semantic Corridor》，香港科技大学大疆联合实验室为第一作者。

2 STSC轨迹规划

        在博客 《自动驾驶---Motion Planning之构建SLT Driving Corridor》中，详细说明了Corridor的生成过程，如下图（详细内容可参考之前的博客）：

2.1 基于贝塞尔曲线的轨迹规划

        首先介绍Bezier曲线，Bezier曲线在路径规划中也经常用到，因此在这里简要阐述一下。贝塞尔曲线分为几种，一般2阶、3阶、4阶，或者更高阶的比较少见到，基于  的参数方程如下。

• 一阶Bezier曲线：

• 二阶Bezier曲线：

• 三阶Bezier曲线：

        贝塞尔曲线具有众多特性，例如凸包性、对称性、几何不变性、仿射不变性、拟局部性，这些特性保证了生成曲线的平滑性、连续性和可控性。

        如果知道了起点 、终点 以及起终点附近的两个控制点 的坐标（即上图中的），