【trajectory optimization】2 method

梯形组合

hermite-simpson 组合
分段二次函数拟合控制量和目标函数的积分过程
状态量位置需要再次积分，是三次埃尔米特样条曲线，有一阶连续微分？即求导得到速度量是连续的？

Simpson求积分：
两个时间节点之间的控制量通过Simpson规则求近似积分，分段二次函数

轨迹的约束转化到每个collocation point上面

二次函数插补需要三个控制点，正好是一段离散曲线端点加中间点。


这里用二次样条的速度量作为系统动力学量？以及控制量？梯形组合法用的是线性变化的动力学量？速度？和控制量？力？
所以分段上，每段力直接反映到速度上，位置是一次积分后得到的。