四旋翼无人机建模与实现（二）

四旋翼无人机建模与实现（二）

本文是对GitHub上的一个开源的四旋翼无人机建模与编程实现的学习笔记，因为原作者的文档使用英文写的，花了很长的时间才看懂，因此，我觉得用自己的语言将这个开源项目分享出来是有意义的，爱因斯坦曾说过：“如果你不能简单地解释某一概念，说明你没有很好地掌握它”。因此便有了写这篇文档的想法。
原文链接： https://github.com/SKYnSPACE/AE450
本文的贡献在于将原作者文档中的公式做了更为详细的推导，并以更为通俗的语言来描述整个建模过程。

---

7、旋转运动方程

后六个状态量的变化率可以从旋转运动方程中获得，状态变化率表示如下：
$$[\dot{\mathbf{\varphi }},\dot{\mathbf{\psi }},\dot{\mathbf{\theta }},\dot{\mathbf{p}},\dot{\mathbf{q}},\dot{\mathbf{r}}]$$
速度方程：
$$\dot{\mathbf{R}}=\mathbf{R}\cdot \mathbf{s}\mathbf{k}(\vec{\mathbf{\omega }})$$
我不是太理解原作者写的这些标识意义，我觉得这样写仿佛更容易理解：
$$\dot{\mathbf{R}\mathbf{P}\mathbf{Y}}=\mathbf{T}\mathbf{r}\cdot \vec{\mathbf{\omega }}$$
其中等号左边是欧拉角的变化率，等号右边时变换矩阵和角速度的乘积，这个公式表达了姿态变化率和角速度的关系。将上述方程展开如下：
$$\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]=\left[\begin{array}{ccc}1& tan\theta sin\varphi & tan\theta cos\varphi \\ 0& cos\varphi & -sin\varphi \\ 0& sin\varphi /cos\theta & cos\varphi /cos\theta \end{array}\right]\left[\begin{array}{c}p\\ q\\ r\end{array}\right]$$
姿态变化率和角速度之间满足线性变换关系，该线性变换关系是关于翻滚和俯仰角的线性变换关系，和偏航角无关。
加速度方程（牛顿第二定律）：
$$\mathbf{I}\dot{\vec{\mathbf{\omega }}}=-\vec{\mathbf{\omega }}\times \mathbf{I}\vec{\mathbf{\omega }}+\mathbf{M}$$
将该公式展开后得到：
$$\dot{\mathbf{p}}=\frac{{\mathbf{I}}_{\mathbf{y}\mathbf{y}}-{\mathbf{I}}_{\mathbf{z}\mathbf{z}}}{{\mathbf{I}}_{\mathbf{x}\mathbf{x}}}\cdot \mathbf{q}\mathbf{r}+\frac{1}{{\mathbf{I}}_{\mathbf{x}\mathbf{x}}}\cdot {\mathbf{M}}_1$$
$$\dot{\mathbf{q}}=\frac{{\mathbf{I}}_{\mathbf{z}\mathbf{z}}-{\mathbf{I}}_{\mathbf{x}\mathbf{x}}}{{\mathbf{I}}_{\mathbf{y}\mathbf{y}}}\cdot \mathbf{r}\mathbf{p}+\frac{1}{{\mathbf{I}}_{\mathbf{y}\mathbf{y}}}\cdot {\mathbf{M}}_2$$
$$\dot{\mathbf{r}}=\frac{{\mathbf{I}}_{\mathbf{z}\mathbf{z}}-{\mathbf{I}}_{\mathbf{y}\mathbf{y}}}{{\mathbf{I}}_{\mathbf{z}\mathbf{z}}}\cdot \mathbf{p}\mathbf{q}+\frac{1}{{\mathbf{I}}_{\mathbf{z}\mathbf{z}}}$$