多项式轨迹与minisnap

轨迹优化

学习来源：
Minimum Snap轨迹规划详解（1）轨迹规划入门_minimum snap trajectory generation and control for-CSDN博客

·多项式轨迹

1. 轨迹一般用n阶多项式来表示：

写成向量形式为：

由此可以根据轨迹计算出速度、加速度、jerk、snap（分别为1,2,3,4阶导数）

一般来说轨迹分为多段，用多个多项式表示，而minimumsnap轨迹规划的目的就在于求解轨迹多项式参数p1-pk

对轨迹要求：

1. 两段轨迹之间连续
2. 轨迹经过固定点
3. 轨迹无碰撞
4. 可能还有其他要求，比如轨迹顺滑、能量最优等，minimum snap优化目标为最小化四阶导
5. 优化函数：（之所以加入平方是为了转化成对角矩阵形式，代码中体现为时间差的幂次和）

约束条件：

如上图所示为对起点的约束，左侧为时间向量与多项式系数向量的点乘，而p为所有参数，因此后面补0，对起始位置、速度、加速度进行约束。

还可以加上相邻段的连续性约束：（类似于令分段函数连续）

最终得到的约束为：（起终点位置速度加速度6+终点固定点[k-1]+分段连续点的位置速度加速度）