无人机的微分平坦性详细推导+Mini snap

无人机的微分平坦性详细推导

目标：将无人机的状态一个12维的向量用4维表示
即将无人机的位置，方向，速度和角速度用位置(X,Y,Z)和绕z轴的$\psi$角表示

1.无人机非线性动力学

2.机体坐标系表示

无人机的推力始终与机架垂直因此
$Z_B=\frac{t}{\Vert t\Vert }$,$t=[\ddot{x},\ddot{y},\ddot{z}+g{]}^{\mathbf{T}}$
然后我们通过一个中间坐标系来求$Y_B$,坐标系如图1的，$X_C，Y_C,Z_C$。
$X_C=[cos\psi ,sin\psi ,0{]}^{\mathbf{T}}$
通过$X_C$我们得到$Y_B$
$Y_B=\frac{Z_B\times X_C}{\Vert Z_B\times X_C\Vert }$(因为$X_C$和$X_B,Z_B$在一个平面内)
$X_B=Y_B\times Z_B$
$R_B=[X_B{Y}_B{Z}_B]$（它表示的旋转可以由$\varphi ,\theta ,\psi$的旋转矩阵表示，因此这三个角可以被消除掉）

3.角速度替换

对下式两边求导，（对一个旋转矩阵求导，等于它的角速度乘以本身这个坐标系。旋转矩阵求导看这里: 旋转矩阵求导

${\omega }_{BW}={\omega }_x{X}_B+{\omega }_y{Y}_B+{\omega }_z{Z}_B$ 表示体坐标系下的角速度
上式中的

将它带入上式得到

这里的$h_{\omega }$的所有量都是已知量
$h_{\omega }={\omega }_{BW}\times Z_B=({\omega }_x{X}_B+{\omega }_y{Y}_B+{\omega }_z{Z}_B)\times Z_B$
因为
$X_B\times Z_B=-Y_B$
$Y_B\times Z_B=X_B$
$Z_B\times Z_B=\overrightarrow{0}$
因此上式可以展开为
$h_{\omega }=-{\omega }_x\times Y_B+{\omega }_y\times X_B$
然后两边点乘$Y_B$,得到
${\omega }_x=-h_{\omega }\cdot Y_B$
两边点乘$X_B$,得到
${\omega }_Y=h_{\omega }\cdot X_B$
因为${\omega }_{BW}={\omega }_{BC}+{\omega }_{CW}$,而${\omega }_{BC}$在$Z_B$上没有分量，
因此${\omega }_z={\omega }_{BW}\cdot Z_B={\omega }_{CW}\cdots Z_B=\dot{\psi }{Z}_W\cdot Z_B$
至此12维变量变成了4维变量表示。
在实际的路径规划中我们只需要规划出轨迹的位置，$\psi$角通常表示为速度的切向方向（群里咨询的，不知是否还有什么知识不知道）。具体的控制回路如下；
例如在mini snap规划时我们只需要将规划的轨迹加上速度的切线方向丢这个控制回路，就可以实现无人机的飞行。

mini snap我的博客园: link

https://www.cnblogs.com/gongliqiang/p/14375447.html

参考资料：深蓝学院

https://www.cnblogs.com/long5683/p/11871630.html