Minimum Snap轨迹规划详解（2）corridor与时间分配

• min：snap
• 等式约束：起点pva(3)+终点pva(3)+中间点pva连续(3*(n_ploy-1))
• 不等式约束：中间点位置corridor(2*(n_ploy-1))

优化结果如下图所示，绿色的点为按固定步长新采样的中间点，每个中间点都有一个矩形corridor，红色为优化后的轨迹。可以看到，去掉中间点的位置强约束(等式约束)改成弱约束(corridor不等式约束)后，轨迹比之前要好很多，而且尽管轨迹有小部分超出了corridor（因为只加了采样时刻的位置corridor约束），但基本不会超出太多。