路径规划学习记录

minimum snap轨迹规划

https://blog.csdn.net/qq_34213260/article/details/106226935
https://www.cnblogs.com/long5683/p/11871630.html
定义路径点、阶次
https://blog.csdn.net/cabbage_hhh/article/details/110141207
实际问题中的轨迹往往是二维、三维甚至更高维，通常每个维度单独求解轨迹
轨迹一般用n阶多项式(polynomial)来表示，即

机器人的平面运动路径也应该由x和y两个多项式构成，所以共有2k段多项式。
已知条件：起始点和终点的位置p、速度v、加速度a、加加速度j（4个），每段多项式在连接点处的位置p已知，且在连接点处光滑（pvaj相等）。每一个分段都是多项式；每个分段的多项式都是相同的阶次，这样对于问题的求解比较简单；每一段的时间间隔都是已知的

限制条件数量: 4+4+（k-1)=k+7，即首末P、v、a、j加中间点位置
未知数数量：（N+1）*k (N为多项式阶数，k为路径的段数）就是说不知道用多少阶多项式和多少段
k+7<=（N+1）*k
则N>=7 / k 7/k7/k 表明段数越多，则提供的阶次越低。k最少是1， 所以minisnap求解中每段多项式至少有7阶，每段有八个未知数。

时间分配

时间分配一般有两种，一种是按路径长度分配时间，一种是按梯形运动方式分配时间，此处为图简便可直接将每段时间赋值1。

使用“梯形速度”时间曲线来获得每个片段的持续时间。·假设在每个片段中，加速到最大。 速度 -> 减速到 0.· 加速 + 最大速度 + 减速。使用预期的平均速度来获取每件作品的持续时间。

时间分配是轨迹规划中比较令人头疼的问题，给的时间太小，速度、加速度自然就很大，两段时间分配不当就会生成打结的轨迹。下一节，专门讨论时间分配问题。
轨迹规划中默认时间t已知，通常根据期望速度和总路程计算一个总时间T，再按照匀速运动和梯形速度曲线分配到每段polynomial上

约束优化

轨迹规划的目的：求轨迹的多项式参数p1,…,pk。
我们可能希望轨迹满足一系列的约束条件，比如：希望设定起点和终点的位置、速度或加速度，希望相邻轨迹连接处平滑（位置连续、速度连续等），希望轨迹经过某些路径点，设定最大速度、最大加速度等，甚至是希望轨迹在规定空间内（Obstruction check）等等。
通常满足约束条件的轨迹有无数条，而实际问题中，往往需要一条特定的轨迹，所以又需要构建一个最优的函数，在可行的轨迹中找出“最优”的那条特定的轨迹。 所以，我们将问题建模（fomulate）成一个约束优化问题

我们要做的就是：将优化问题中的f§函数和Aeq,beq,Aieq,bieq参数给列出来，然后丢到优化器中求解轨迹参数p
Minimum Snap顾名思义，Minimum Snap中的最小化目标函数是Snap（加速度的二阶导），当然你也可以最小化Acceleration（加速度）或者Jerk（加速度的导数），至于它们之间有什么区别，quora上有讨论。一般不会最小化速度。

等式约束：
某点的P、V、A和相邻段之间的P、V、A都可构成等式约束

添加两张重要的图片：