minimumsnap（1）微分平坦特性（Differential Flatness）

本文内容参考论文:Minimum Snap Trajectory Generation and Control for Quadrotors,Daniel Mellinger and Vijay Kumar

从名字可以看出，我们需要最小化snap，snap就是加加加速度，我们知道位置是p，位置的导数是速度v，速度的导数是加速度a，加速度的导数是加加速度jerk，加加速度的导数是加加加速度snap，因此我们最小化的就是加加加速度snap。论文开始先证明的微分平坦的特性，我们知道，无人机的全状态空间为12维，位置，速度，姿态角，角速度，如下式子所示

我们在做轨迹规划的过程中不可能对12维的全维度空间进行规划，这是非常复杂的，但可以找到一个平坦输出的空间，这个空间只有四个维度的变量，位置x,y,z和偏航角，剩下所有的状态都能用这四个变量和其有限阶导数的代数组合所表示，因此在做无人机轨迹规划的时候只需要对这四个变量进行规划就可以了。因此我们平坦输出空间选择为

其中

下面证明为什么可以用这四个变量来表示全维度的空间。
首先，位置x,y,z和偏航角不用说了，速度x,y,z的导数也不用说，现证明滚转角和俯仰角可以用这四个变量和其有限阶导数的代数组合所表示，这里给出两种证明方法。
第一种：我们知道对于无人机来说有两个坐标系，一个是地面坐标系，一个是机体坐标系，而机体坐标系就是通过地面坐标系旋转得来的，比如我们常用的欧拉角表示为Z-Y-X那么机体坐标系就是通过先绕Z轴旋转，然后绕Y轴旋转，然后饶X轴旋转，实际上旋转的角度就是三个姿态角，那么如果机体坐标系的三个坐标轴能用这四个变量和其有限阶导数的代数组合所表示，也就表明姿态角能用这四个变量和其有限阶导数的代数组合所表示。看图片，显然机体坐标系的Z轴是可以表示为下列式子的，

这个应该很好理解了，实际上无人机在飞行过程中一共就受两个力的作用，一个为升力F，其垂直于机体平面向上，一个为重力G，而机体坐标系的Z轴实际上就是升力F的方向，升力在x,y方向分解分别为其在x,y方向的加速度，在Z轴方向分解再减掉一个mg即为在Z轴的加速度方向，因此想得到力在Z轴分解的方向，就用Z方向的加速度加个g就可以了。因此机体坐标系的Z轴可以用这四个变量和其有限阶导数的代数组合所表示。那接下来需要证明机体坐标系的X轴和Y轴也可以用这四个变量和其有限阶导数的代数组合所表示。
首先构造一个坐标系，这个坐标系是将地面坐标系绕着Z轴旋转偏航角获得的。也就是图中的C坐标系，
其中

然后将C坐标系平移到机体坐标系，与机体坐标系的中心重合。在minimumsnap这篇论文中使用的是Z-X-Y欧拉角，那么机体坐标系可以看做世界坐标系先绕$z_B$轴旋转，此时得到的坐标系与C重合，然后绕着$x_B$,此时$x_B$与$x_C$依然是重合的，最后绕着$y_B$旋转，那么显然此时$z_B$，$x_B$，与$x_C$是在一个平面上的，正常我们知道$y_B$是由$z_B$和$x_B$叉乘得来的，又因为上述三个向量在同一个平面内，因此可以得到$y_B$实际上可以由下面这个式子进行表示，那我们知道$z_B$和$x_C$都能用平坦输出空间上的变量及其各阶导数的代数组合表示，那么$y_B$就也可以用平坦输出空间上的变量及其各阶导数的代数组合表示，此时$z_B$和$y_B$确定，那么$x_B$也随之确定了，R实际上就是旋转矩阵。实际上这一部分的证明就是想证明$y_B$垂直于$x_C$，那更简单的一种方法是直接拿出旋转矩阵发现二者直接点乘等于零就行了。至此，两个姿态角也就可以用平坦输出空间上的变量及其各阶导数的代数组合表示，最后就剩下角速度了。

实际上，姿态角的证明这一部分还有一个直接的方法，就是如果能解出来滚转角和俯仰角与四个平坦输出变量的关系，那也能证明，这个证明在这篇论文中有Vector Field Following for Quadrotors using Differential Flatness，DingJiang Zhou and Mac Schwager具体在这里就不给出了，实际上就是应用无人机的一些方程解出来的。解出来就是这样的形式


最后还剩p,q,r三个变量，也就是在x,y,z三个方向上的角速度，这个的证明如下：首先根据牛顿等式有

然后求导得

其中的$u_1$指的是升力，这个是无人机建模的牛顿方程，基本上随便一篇无人机的文章都能找到，
对$u_1$求导有

右乘一个$z_B$得到

代入到上面式子，得到

又因为

对其叉乘一个$z_B$得到

然后分别点乘$y_B$和$x_B$就能得到


至此x和y方向上的角速度可以表示出来，我们看一下式子的形式，实际上可以发现，其可以用平坦的四个变量及其各阶导数的代数组合表示，最后就剩下y轴上的角速度r，由于

且

在$z_B$上时没有分量的，因为这个角速度的含义是在C系下看机体坐标系，实际上C系就是世界系转了一个偏航角，那自然再到机体坐标系时就不需要转偏航角了，因此没有分量，因此

因此r也能用平坦的四个变量及其各阶导数的代数组合表示，至此微分平坦的性质就证明完毕了。

注意：上面证明姿态角的时候使用的是Z-X-Y欧拉角，因为原文使用的就是这个欧拉角表示，那正常我们在做无人机的时候，使用的欧拉角表示为更一般的Z-Y-X欧拉角，那证明的方法实际上大同小异，只是我们应该先拿出$y_C$,然后先求$x_B$。有兴趣可以自己证一下。