轨迹优化

minimun snap 闭式求解。

导数约束和连续性约束可以和写为一个QP问题（二次规划）问题。此问题是一个凸优化问题，是一定可以求解的。

其中重要的问题是在路径采样点选出来之后，需要对多段多项式进行每一段多项式的系数进行求解。这样的解会带来

一些问题，求解的这些p1,pm是没有实际的物理意义的，会带来轨迹优化数值上的不稳定性。

 

我们要做的是，把对轨迹的多项式系数的问题转化成对各段轨迹的边界的导数约束问题。

 

在边界的起始点和目标点确定，中间点的位置信息是确定的，而中间点的速度和加速度是需要优化的信息，

这样就可以把这个问题从优化多项式的系数这个没有实际物理意义的比较不稳定的优化问题的决策变量，转化为优化轨迹上

的这些点的v、a作为决策变量。这样数值稳定性会好很多。

 

 

硬约束方法的弊端：

1.硬约束把所有的安全区域都看成是等价的。

2.对解空间是十分敏感的。

 

软约束：

让环境里都施加一个推力，把轨迹往远离障碍物的地方去推。

弊端；

其生成路径是有可能碰到障碍物的。

cost = 光滑项 + 碰撞项 + 动力学