【控制实践——四旋翼无人机】【一】四旋翼无人机运动分析和建模

系列博客

前言

在C站摸爬滚打一段时间后，发现控制类相关的圈子较小（话题热度低），想顺便跟各位同行读者了解一些喜好。

平时在检索这方面资料信息时，通常采用什么渠道或者通过什么平台获得。有什么好的建议意见，欢迎私聊或评论。

在初学入门阶段，本人也会在c站搜索一些相关的内容，但总感觉信息繁复琐碎，要么过于保姆级别，内容简单但不利于深入理解，要么过于潦草敷衍，公式原理推导含糊不明，以致学习效率低下，甚至片面理解和误解。因此在撰写博客前，总结出自己的一些理解：

首先，要系统性的从原理出发进行推导（需要具备基础物理和数学知识，有一定的门槛！）
其次，借助仿真工具验证公式推导合理性
最后，通过实物实践再次论证（需要有很好的动手能力和实验思维！）

回到正题，在现在大环境下，对于自动化控制类的同学来说，四旋翼无人机已经成为必备技能之一。也搜索过C站的四旋翼相关博客，个人认为相对比较片面。因此决定开一个新的四旋翼控制的系列，结合一些四旋翼论文，系统性地分析四旋翼控制原理，并设计实物进行验证。

坐标系定义及姿态位置描述

坐标系定义

这一步是所有机器人控制的前提，因为所有的运动都可以认为是相对的，又是绝对的，随着参考对象的变化，也会得到不同的答案。因此明确坐标系定义，就明确了运动的相对性，并且能够定义运动的指标。

在飞行器的运动中，通常定义机体坐标系（非惯性系）和地球坐标系（惯性系）。
“为什么通常是这两个，不能是其他的”
“因为这两个能够简单高效地达到飞行器运动描述的目的。”
以上是我入门时杠过的一个问题。

机体系是根据飞行器结构定义的，是固连于飞行器的结构上的；
地球系是以地球为参考系建立的惯性坐标系，是固连于地球表面的，通常指向东北天。

在进行运动描述时，通常将飞行器视作刚体（其结构不会发生变形，质心位置不会发生变化），这是两个理想假设，通常会变化但是不会特别大。

因此，飞行器的位置，就可以通过飞行器质心点在地球系中的坐标点($x,y,z$)来表示。这就完成了飞行器平动运动的描述。

同时，飞行器的姿态也很关键，因为他决定了四个螺旋桨的推力方向，是实现飞行器控制的关键状态信息。因此也需要有一些变量来描述这个转动运动。这里用到了机体系和地球系的相对角度位置来表述。

这里讲的很墨迹，主要是考虑到刚入门的同学对于坐标系变换和姿态表述不理解，需要慢慢引导。
这方面有基础的同学可以跳过！
关于姿态表述的方式，并不是绝对的，有若干种表述的方式。但每种姿态表述都有一个坐标轴的前提定义。
即假设转动过程是按Z-Y-X（三个轴都是机体系的坐标轴）顺序进行：

• 转动前
  
• 先按机体系Z轴转动$\mathbf{\psi }$
  
• 再绕Z轴转动后的机体系Y轴转动$\mathbf{\theta }$
  
• 最后在前面的基础绕机体系X轴转动$\mathbf{\varphi }$
  

而这三个旋转的角度($\mathbf{\varphi },\mathbf{\theta },\mathbf{\psi }$)分别对应横滚角(机体系X轴转动角度)，俯仰角(机体系Y轴转动角度)，航向角(机体系Z轴转动角度)。

• 你仔细思考应该会发现，如果转动顺序变化，不再是Z-Y-X，而是按照X-Y-Z，那么得到三个姿态角的几何意义也将不同，这也是我前面提到的姿态角是相对的，具有前提的意思。
• 坐标系变换是一个变换的结果而非过程，在进行坐标系变换的数学描述时尤其体现这一点，到这里会有点抽象，之前看到B站上有一个不错的视频，链接

姿态描述及坐标系变换

这里我不想赘述，我之前有一篇博客做过相关推导。
Mahony姿态解算——坐标系变换
里面姿态角的符号不对应，但推导过程不变！

本文的坐标系旋转顺序是$Z->Y->X$

由机体系到地球系的坐标系转换矩阵为$\mathbf{R}$
$$\mathbf{R}=\left[\begin{array}{ccc}{C}_{\psi }{C}_{\theta }& C_{\psi }{S}_{\theta }{S}_{\varphi }-S_{\psi }{C}_{\varphi }& C_{\psi }{S}_{\theta }{C}_{\varphi }+S_{\psi }{S}_{\varphi }\\ S_{\psi }{C}_{\theta }& S_{\psi }{S}_{\theta }{S}_{\varphi }+C_{\psi }{C}_{\varphi }& S_{\psi }{S}_{\theta }{C}_{\varphi }-C_{\psi }{S}_{\varphi }\\ -S_{\theta }& C_{\theta }{S}_{\varphi }& C_{\theta }{C}_{\varphi }\end{array}\right]$$
其中，$\varphi$为横滚角，$\theta$为俯仰角，$\psi$为偏航角。$C_x$为对应角度的余弦值，$S_x$为对应角度的正弦值。
设机体系的角速度向量为$\mathbf{\nu }=\left[\begin{array}{c}p\\ q\\ r\end{array}\right]$，欧拉角向量$\mathbf{\eta }=\left[\begin{array}{c}\varphi \\ \theta \\ \psi \end{array}\right]$，二者满足以下关系
$$\begin{array}{ll}\dot{\mathbf{\eta }}={\mathbf{W}}_{\eta }^{-1}\mathbf{\nu },& \left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]=\left[\begin{array}{ccc}1& S_{\varphi }{T}_{\theta }& C_{\varphi }{T}_{\theta }\\ 0& C_{\varphi }& -S_{\varphi }\\ 0& S_{\varphi }/C_{\theta }& C_{\varphi }/C_{\theta }\end{array}\right]\left[\begin{array}{c}p\\ q\\ r\end{array}\right]\end{array}$$
$$\begin{array}{ll}\mathbf{\nu }={\mathbf{W}}_{\eta }\dot{\mathbf{\eta }},& \left[\begin{array}{c}p\\ q\\ r\end{array}\right]=\left[\begin{array}{ccc}1& 0& -S_{\theta }\\ 0& C_{\varphi }& C_{\theta }{S}_{\varphi }\\ 0& -S_{\varphi }& C_{\theta }{C}_{\varphi }\end{array}\right]\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]\end{array}$$

这里再标记一下，从上面的描述中可以看出，姿态角是过程量，不是结果量，即你无法直接从转动后的机体系中明确指出哪个角是横滚/俯仰/航向角。而且机体系的角速度，是指绕当前机体系坐标轴转动的速度。因此可以得到一个结论：
欧拉角的变化率不等于机体系的角速度
这里强调这个结论是因为我之前就是误解了二者的关系，造成很多后续的认知错误。
这里还要考虑矩阵不可逆的情况。

受力分析

飞行器的主动力有四个螺旋桨产生的力，主动扭矩只有四个螺旋桨产生的反扭矩。受力情况如图：

四个螺旋桨的力${\mathbf{f}}_{\mathbf{i}},\mathbf{i}=1,2,3,4$
$${\mathbf{f}}_{\mathbf{i}}=\mathbf{k}{\mathbf{\omega }}_{\mathbf{i}}^2$$
四个螺旋桨的反扭矩${\mathbf{\tau }}_{\mathbf{Mi}},\mathbf{i}=1,2,3,4$
$${\mathbf{\tau }}_{\mathbf{Mi}}=\mathbf{b}{\mathbf{\omega }}_{\mathbf{i}}^2+{\mathbf{I}}_{\mathbf{M}}{\stackrel{\cdot }{\mathbf{\omega }}}_{\mathbf{i}}$$
通常由于螺旋桨转速变化产生的自转力矩较小，因此将其忽略，即
$${\mathbf{\tau }}_{\mathbf{Mi}}=\mathbf{b}{\mathbf{\omega }}_{\mathbf{i}}^2$$
其中${\mathbf{\omega }}_{\mathbf{i}},\mathbf{i}=1,2,3,4$分别对应四个螺旋桨的转速

假设四旋翼机架四边对称，质心位置到四个螺旋桨的中心距离均为$\mathbf{l}$，则可以得到螺旋桨产生的四旋翼推力$T$为：
$$T=\sum _{i=1}^4{\mathbf{f}}_{\mathbf{i}}=k\sum _{i=1}^4{\mathbf{\omega }}_{\mathbf{i}}^2，{\mathbf{T}}_B=\left[\begin{array}{c}0\\ 0\\ T\end{array}\right]$$
同样，由螺旋桨所产生的四旋翼机体下的力矩矢量${\mathbf{\tau }}_{\mathbf{B}}$：
$${\mathbf{\tau }}_{\mathbf{B}}=\left[\begin{array}{c}\mathbf{lk}(-{\mathbf{\omega }}_2^2+{\mathbf{\omega }}_4^2)\\ \mathbf{lk}(-{\mathbf{\omega }}_1^2+{\mathbf{\omega }}_3^2)\\ \mathbf{b}(-{\mathbf{\omega }}_1^2+{\mathbf{\omega }}_2^2-{\mathbf{\omega }}_3^2+{\mathbf{\omega }}_4^2)\end{array}\right]$$

牛顿-欧拉方程

• 牛顿方程

  牛顿方程是描述刚体在地球坐标系下平动运动的方程，满足牛顿第二定律，其中受到的外力有重力、螺旋桨推力、空气阻力，共同组成飞行器合外力。
  定义飞行器位置向量 $\mathbf{\xi }=\left[\begin{array}{l}x\\ y\\ z\end{array}\right]$，则惯性系下的牛顿方程为：
  $$m\ddot{\mathbf{\xi }}=\mathbf{G}+\mathbf{R}{\mathbf{T}}_B-c\dot{\mathbf{\xi }}$$
  其中，$c$是空气阻力系数

• 欧拉方程

  欧拉方程是描述机体系转动运动的方程，其中受到的扭矩有螺旋桨产生的扭矩${\mathbf{\tau }}_{\mathbf{B}}$，陀螺力矩$\mathbf{\Gamma }$，和离心力矩$\nu \times (I\nu )$。

  设机体系的角速度向量为$\mathbf{\nu }=\left[\begin{array}{c}p\\ q\\ r\end{array}\right]$，
  $$\mathbf{I}\dot{\mathbf{\nu }}+\mathbf{\nu }\times (\mathbf{I\nu })+\mathbf{\Gamma }={\mathbf{\tau }}_{\mathbf{B}}$$
  其中，$\mathbf{I}$为飞行器的转动张量，表示为：
  $$\mathbf{I}=\left[\begin{array}{ccc}{I}_{xx}& 0& 0\\ 0& I_{yy}& 0\\ 0& 0& I_{zz}\end{array}\right]$$
  $\mathbf{\Gamma }$是陀螺力矩，其表达式为：
  $$\mathbf{\Gamma }=I_r\left[\begin{array}{c}q/I_{xx}\\ q/I_{yy}\\ 0\end{array}\right]{\omega }_{\Gamma }$$
  其中，$I_r$是螺旋桨的转动惯量，${\omega }_{\Gamma }={\omega }_1-{\omega }_2+{\omega }_3-{\omega }_4$

这里简单解释一下各个力矩。

• 陀螺力矩
  这是由于陀螺效应产生的进动力矩，可以根据这些关键词进行进一步的了解，不赘述。
• 离心力矩
  这是由于质量分布不均产生的离心力矩，可以结合实际生活现象辅助理解，可以搜到相关推导和讲解，这里不赘述。
  通常在控制计算时可以忽略这两个力矩，因为四旋翼结构对称，分布均匀，并且四个螺旋桨AB桨叶会抵消陀螺效应，产生的影响也会变得很小。

状态空间方程

基于牛顿欧拉方程，建立四旋翼飞行器的状态空间方程，为后续的控制器设计铺垫。

定义角运动状态向量，${\mathbf{X}}_{\mathbf{a}}=\left[\begin{array}{c}\varphi \\ \theta \\ \psi \\ p\\ q\\ r\end{array}\right]$，则状态空间方程为
$${\left[\begin{array}{c}\varphi \\ \theta \\ \psi \\ p\\ q\\ r\end{array}\right]}^{\prime }=\{\begin{array}{c}p+S_{\varphi }{T}_{\theta }q+C_{\varphi }{T}_{\theta }r\\ C_{\varphi }q-S_{\varphi }r\\ S_{\varphi }/C_{\theta }q{C}_{\varphi }/C_{\theta }r\\ (I_{yy}-I_{zz})qr/I_{xx}-I_{r}q{\omega }_{\Gamma }/I_{xx}+{\tau }_{\varphi }/I_{xx}\\ (I_{zz}-I_{xx})pr/I_{yy}+I_{r}p{\omega }_{\Gamma }/I_{yy}+{\tau }_{\theta }/I_{yy}\\ (I_{xx}-I_{yy})/pq/I_{zz}+{\tau }_{\psi }/I_{zz}\end{array}$$
定义线运动状态向量，${\mathbf{X}}_{\mathbf{p}}=\left[\begin{array}{c}x\\ y\\ z\\ \stackrel{\cdot }{x}\\ \stackrel{\cdot }{y}\\ \stackrel{\cdot }{z}\end{array}\right]$，状态空间方程为
$${\left[\begin{array}{c}x\\ y\\ z\\ \stackrel{\cdot }{x}\\ \stackrel{\cdot }{y}\\ \stackrel{\cdot }{z}\end{array}\right]}^{\prime }=\{\begin{array}{c}\stackrel{\cdot }{x}\\ \stackrel{\cdot }{y}\\ \stackrel{\cdot }{z}\\ (C_{\psi }{S}_{\theta }{C}_{\varphi }+S_{\psi }{S}_{\varphi })T/m-c\dot{x}\\ (S_{\psi }{S}_{\theta }{C}_{\psi }-C_{\psi }{S}_{\varphi })T/m-c\dot{y}\\ (C_{\theta }{C}_{\varphi })T/m-g-c\dot{z}\end{array}\text{\hspace{1em}(1)}$$

总结

• 从状态空间方程可以看出，想要精准的控制四旋翼飞行器的位置和平动运动，需要通过调节飞行器的姿态角度来实现，因此姿态控制就显得尤为重要。

• 本章博客先介绍四旋翼飞行器的建模推导过程，后续将基于本篇博客的模型来开展各类控制算法的实现和仿真，并尽可能做出系统性地分析和讨论。
  感谢阅读！