【无人机姿态运动学模型】_无人机运动学模型

本文需要对旋转矩阵的认识，若没有，请先阅读【旋转矩阵】。

在无人机的研究中，需要建立机体姿态的运动学模型，按照笔者的理解，运动学模型便是一个输入为速度量，输出为位移量的微分方程。假如我们用欧拉角来表示机体的姿态，那么输出量便是姿态的欧拉角数值，输入量就应该是欧拉角的角速度。但一般情况下，为了方便研究，我们会以机体旋转的角速度为输入量，所以就需要有从机体旋转角速度到欧拉角微分值的转换过程，这个转换过程我们就用到了旋转矩阵的知识，有关旋转矩阵，在 【旋转矩阵】中已经有推导。

定义欧拉角表达方式

• 坐标系定义
  首先我们假定一个固连在惯性系上的$坐标系E$，和一个固连在机体上的$坐标系B$，用欧拉角去表示姿态，就是一个从$系E$变换到$系B$的坐标变换过程。
• 坐标变换过程详解
  1. 首先$E系$绕z轴（即$E系$的基准矢量$\overrightarrow{e_3}$旋转$\psi$，这一步得到的坐标系，命名为$K系$。
  2. 然后$K系$绕y轴（即$K系$的基准矢量$\overrightarrow{k_2}$旋转$\theta$，这一步得到的坐标系，命名为$N系$。
  3. 最后$N系$绕x轴（即$N系$的基准矢量$\overrightarrow{n_1}$旋转$\varphi$，这一步得到的坐标系，便是最后的$机体坐标系B$。
     
     上面所提到的$角\varphi ,角\theta ,角\psi$，便是在无人机机体描述中常说的偏航角（yaw）、俯仰角（pitch）和滚转角（roll）。

角速度的坐标变换

这个过程，便是角速度看作成一个向量，然后利用旋转矩阵进行从$E系$到$B系$的坐标变换。从
【旋转矩阵】这篇文章我们可以知道，要表示两个坐标系之间某个矢量的坐标表达，就是左乘两个坐标系变换的旋转矩阵即可。又因为矩阵相乘不一定满足乘法交换律，所以持续的坐标变换，就得按顺序去左乘对应的旋转矩阵。上述的文章中，已经推导得到了：
$$\begin{gathered} R_z=\left[\begin{array}{ccc}cos\psi & sin\psi & 0\\ -sin\psi & cos\psi & 0\\ 0& 0& 1\end{array}\right] \\ {R}_y=\left[\begin{array}{ccc}cos\theta & 0& -sin\theta \\ 0& 1& 0\\ sin\theta & 0& cos\theta \end{array}\right] \\ {R}_x=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\varphi & sin\varphi \\ 0& -sin\varphi & cos\varphi \end{array}\right] \end{gathered}$$

• 偏航角速度（$\dot{\psi }$）的变换
  首先将角速度$\dot{\psi }$看作一个矢量，角速度的方向是根据右手定则决定的。

  右手定则：
  伸出右手，右手四指按角速度旋转的方向弯曲，大拇指所指向的方向，便是角速度的方向。

  那么偏航角速度矢量$\overrightarrow{\dot{\psi }}$方向和z轴方向相同，大小为$\dot{\psi }$，所以可以表达为$\overrightarrow{\dot{\psi }}=\left[\begin{array}{c}0\\ 0\\ \dot{\psi }\end{array}\right]$，接下来只需要对这个矢量依照欧拉角定义的顺序进行坐标变换，即左乘旋转矩阵即可，则
  $$R_x{R}_y{R}_z\overrightarrow{\dot{\psi }}$$

• 俯仰角速度（$\dot{\theta }$）的变换
  同理，俯仰角速度矢量表达为$\overrightarrow{\dot{\theta }}=\left[\begin{array}{c}0\\ \dot{\theta }\\ 0\end{array}\right]$，则对应的坐标变换为
  $$R_x{R}_y\overrightarrow{\dot{\theta }}$$

• 滚转角速度（$\dot{\varphi }$）的变换
  同理，滚转角速度矢量表达为$\overrightarrow{\dot{\varphi }}=\left[\begin{array}{c}\dot{\varphi }\\ 0\\ 0\end{array}\right]$，则对应的坐标变换为
  $$R_x\overrightarrow{\dot{\varphi }}$$

所以以上三个欧拉角速度的坐标变换叠加起来后，便可以得到$机体坐标系B$下的角速度，也就是机体角速度的坐标表达。令机体角速度的坐标表达为${\omega }_b$，则
$$\begin{gathered} {\omega }_b=R_x{R}_y{R}_x\overrightarrow{\dot{\psi }}+R_x{R}_y\overrightarrow{\dot{\theta }}+R_x\overrightarrow{\dot{\varphi }} \\ =R_x{R}_y{R}_z\left[\begin{array}{c}0\\ 0\\ \dot{\psi }\end{array}\right]+R_x{R}_y\left[\begin{array}{c}0\\ \dot{\theta }\\ 0\end{array}\right]+R_x\left[\begin{array}{c}\dot{\varphi }\\ 0\\ 0\end{array}\right] \\ =A\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right] \end{gathered}$$
其中，$A=\left[\begin{array}{ccc}1& 0& -sin\theta \\ 0& cos\varphi & sin\varphi cos\theta \\ 0& -sin\varphi & cos\varphi cos\theta \end{array}\right]$，又因为我们将姿态欧拉角速度看作输出量，机体角速度看作输入量来构造微分方程，那么一般会构建成$d\frac{输出量}{dt}=f(输入量)$的形式，所以姿态运动学模型表达为
$$\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]=A^{-1}{\omega }_b$$
其中，$A^{-1}=\left[\begin{array}{ccc}1& sin\varphi tan\theta & cos\varphi tan\theta \\ 0& cos\varphi & -sin\varphi \\ 0& sin\varphi /cos\theta & cos\varphi /cos\theta \end{array}\right]$。

最后总结

无人机姿态运动学模型的输出量为欧拉角，输入量为机体角速度，表达式为：
$$\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]=\left[\begin{array}{ccc}1& sin\varphi tan\theta & cos\varphi tan\theta \\ 0& cos\varphi & -sin\varphi \\ 0& sin\varphi /cos\theta & cos\varphi /cos\theta \end{array}\right]\left[\begin{array}{c}{\omega }_{bx}\\ {\omega }_{by}\\ {\omega }_{bz}\end{array}\right]$$