四轴轨迹规划-Minimum snap总结

理论知识

参考：1. kummar 2011年 论文 Minimum Snap Trajectory Generation and Control for Quadrotors.
2. 香港科技大沈邵劫老师的相关论文；
3. 深蓝学院 高飞老师课件。
微分平坦：系统状态可以被平坦空间的状态以及他们的导数所组成的代数组合所表示。

系统状态12维，首先可以看出 x,y,z 和 dot_x, dot_y, dot_z， 以及 \phi , \theta, \psi和 \omega_x \omega_y \omega_z之间的导数关系，所以整个系统可以先规划成6个，然后无人机的角度和\dot\dot_x相关，所以，最终可以缩减为4个输入，x,y,z和\psi

Minimum jerk: minimize angular velocity, good for visual tracking
Minimum snap: minimize differential thrust, save energy
注：如果是针对minimum jerk，则需要提供位置，速度，角速度3个状态量。如果只考虑起点和终点，则有23个状态。同时考虑多项式形式包含有x_{0}(也就是常数项），所以最终的N=5
同样的道理，如果minimum snap，则需要提供位置，速度，加速度，以及角速度，4个状态量。所以N = 24-1 = 7
注2： 如果考虑多段的情况，例如K段，则minimum jerk ，需要 (k-1) +3 +3= k+5 , 这里只要求能够连续的到达中间点，至于以怎样的速度，怎样的加速度到达这个点，是优化出来的，不属于约束 。
假设每一段的阶数为N ,则每一段轨迹所能提供的自由度为为N+1。N阶多项式可以提供N次导数，加上原多项式，即为N+1。 所以，总计 （N+1）*k.
(N+1)*k>= k+5. 则 N_{min} = 5/k. 表明段数越多，则提供的阶次越低。
每一个分段都是多项式；每个分段的多项式都是相同的阶次，这样对于问题的求解比较简单；每一段的时间间隔都是已知的

上图显示了，对于Minimum snap问题的限制条件：初始条件以及结束条件的限制（包含有位置，速度，加速度等）；中间节点连续（包含有位置，速度，加速度）

优化过程包含有等式约束与导数约束：1.等式约束，start point 以及 end point的条件。只和当前段相关

2. 导数约束, 在中间点满足位置连续，速度连续，加速度连续，jerk连续。和前后两段相关

所以，最后minimum snap变成了一种二次型QP求解问题，典型的凸优化问题
Disciplined convex optimization programs

实践过程

1. 在ROS中，设定起始点，终止点和中间点的坐标。
2. 通过C++/ROS 计算出最有轨迹。其实生成的为多项式的系数矩阵以及对应的每一段的时间点。
3. 然后我将系数矩阵以及对应的时间点导入到Matlab中画图，可以看到整个轨迹的样子。一个简单的圆形的轨迹如下图所示。
   
4. 同样的方法，新建一个ROS_node，然后类似matlab程序，将生成的轨迹按照轨迹时间，发送到mavros中，完成无人机按照目标轨迹的跟踪问题。
5. 最终实现样例程序，为无人机首先起飞到空中固定坐标点。然后发送消息，完成起飞动作。最后无人机再次按照minimum snap生成的轨迹，完成飞行。视频截图如下图所示。视频链接传送门。
   
   后续，下图摘自Kumar 论文，给出了一种较好的控制策略。（最初将MinimumSnap生成的位置信息传递给Px4，得到的效果并不是很好，总有滞后效果），后面需要尝试下下图的控制模式。