自动驾驶---Motion Planning之多段五次多项式

1 前言

        在之前的博客系列文章中和读者朋友们聊过Apollo的 Motion Planning方案：

        《自动驾驶---Motion Planning之LaneChange》

        《自动驾驶---Motion Planning之Path Boundary》

        《自动驾驶---Motion Planning之Speed Boundary》

        《自动驾驶---Motion Planning之轨迹Path优化》

        《自动驾驶---Motion Planning之轨迹Speed优化》

        也聊过其它Motion Planning的量产方案：

        《自动驾驶---Motion Planning之构建SLT Driving Corridor》

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

        同时也包括特斯拉的量产方案迭代历程：

        《自动驾驶---Tesla的自动驾驶技术进化史（Perception&Planning）》

        基于传统Planning框架的文章基本告一段落，后面分享的内容主要包括Planning子模块的小任务以及端到端方案。今天，想聊的话题是：多项式曲线。其实在自动驾驶的不少场景下，经常会使用多种曲线（多项式曲线，样条曲线，螺旋曲线，贝塞尔曲线等）来进行轨迹规划或者平滑，主要考虑到以下场景：

1. 通过动态规划/全局规划的方法得到离散点列，然后采用多项式曲线进行连接。

2. 轨迹平滑（Smoother）

        本篇博客主要为读者朋友们介绍五次多项式在自动驾驶轨迹规划模块中的应用。

2 五次多项式介绍

        自动驾驶规划中的五次多项式在生成平滑、连续的轨迹中扮演着重要角色，特别是在速度规划和路径规划中。以下是关于自动驾驶规划五次多项式的介绍：

2.1 定义

        五次多项式是一个数学函数，其形式通常为：

       其中， 代表车辆随时间  变化的纵向位移或路径，而 是待确定的系数。

        上面的形式表示纵向，那么横向同样可以表示为：

        其中， 代表车辆随时间  变化的横向位移或路径，而  是待确定的系数。

2.2 特点

• 平滑性：五次多项式可以确保生成的轨迹在时间和空间上都是连续的，并且其一阶（速度）、二阶（加速度）和三阶导数（加速度的变化率，也称为Jerk）也都是连续的。这种连续性对于自动驾驶车辆来说至关重要，因为它能确保车辆行驶的平稳性和乘客的舒适性。
• 可调性：通过调整五次多项式的系数，可以根据特定的需求（如起始点、终止点、速度、加速度等）来定制轨迹。

2.3 应用

        在自动驾驶规划中，五次多项式常用于：

• 速度规划：在给定起始速度、终止速度、起始加速度和终止加速度的条件下，通过五次多项式来规划速度随时间变化的曲线。这样可以确保车辆在整个行驶过程中速度的变化是平滑的。
• 路径规划：在全局路径规划的基础上，通过五次多项式来生成局部路径的轨迹。这种方法可以在车辆行驶过程中根据实时环境信息（如障碍物位置、道路曲率等）来动态调整轨迹，确保车辆能够安全、平稳地到达目的地。

3 单段五次多项式规划

        自动驾驶规划中的五次多项式是一种强大的工具，它可以根据给定的边界条件生成平滑、连续的轨迹。这种轨迹不仅有利于确保车辆行驶的平稳性和乘客的舒适性，还有助于提高自动驾驶系统的安全性和可靠性。下面分别针对笛卡尔坐标系以及车道坐标系对单段的五次多项式做相应的介绍。

3.1 XY坐标系下的路径规划

         单段是指已知起点和终点，通过起点和终点的约束信息来求解多项式的系数。已知五次多项式的起点和终点信息ÿ