本文主要分享一下四旋翼无人机的建模过程，然后在Matlab的simulink模块搭建起四旋翼无人机的模型，本篇文章主要参考了康日晖的《四旋翼无人机建模》与南京邮电大学周帆同学的硕士毕业论文，最后我会给出参考文章网址，有兴趣的同学可以看看。

一、无人机建模过程

1.坐标系建立

为了更好的描述无人机的姿态，需要建立两个坐标系：一是地面坐标系；二是机体坐标系。首先建立地面坐标系。
地面坐标系，顾名思义就是一种固定在地球表面的坐标系。在地面上任选一点O作为原点，X轴指向地球表面任意一个方向，Z轴沿着铅直方向指向天空，Y轴在水平面内与X轴垂直，指向通过右手法则来确定。飞行器的位姿，速度，角速度等都是相对于这一坐标系来衡量的，在这里，我们将地面坐标系记为{E-OXYZ}系，其图如下所示：

机体坐标系，很显然，该坐标系建立在飞行器上。原点o位于飞行器的质心处，对于四旋翼而言，其实为四旋翼中心；x轴在飞机的对称平面内，并且平行于飞行器的设计轴线，指向机头前方；y轴垂直于机身对称平面，并指向机身右方；z轴过o点并于xoy平面垂直，指向飞行器上方。该坐标系我们一般记为{B-oxyz}系，其示意图如下：

机体坐标系和地面坐标系之间的转换关系可以通过三个欧拉角进行描述，分别是俯仰角$\theta$,横滚角$\varphi$,偏航角$\psi$。记地面坐标系为E系，机体坐标系为B系，从地面坐标系变换到机体坐标系的变换过程如下：

1.{B}的初始方位与坐标系{E}重合，首先{B}绕$X_E轴$,即地面坐标系的X轴旋转横滚角$\varphi$，得到新坐标系记作$B_1$
2.将$B_1$坐标系绕$Y_E$轴，即地面坐标系的Y轴旋转俯仰角$\theta$，得到新坐标系记作$B_2$
3.将$B_2$坐标系绕$Z_E$轴，即地面坐标系的Z轴旋转偏航角$\psi$，得到新坐标系记作$B_3$，该坐标系就是我们飞行器运动后的机体坐标系
由于上述一系列坐标系变换都是相对于固定坐标系{E}旋转合成得到，因此可以通过左乘基本旋转矩阵得到机体坐标系{B}转换到地面坐标系{E}的旋转矩阵，计算公式如下：
$${}_B^E\text{R}=R_z(\psi )R_y(\theta )R_x(\varphi )（1）$$其中${}_B^E\text{R}$是机体坐标系B转换到地面坐标系{E}的旋转矩阵，$R_x,R_y,R_z$分别是绕x,y,z轴旋转的基本旋转矩阵，将(1)式展开
$${}_B^E\text{R}=\left[\begin{array}{ccc}cos(\psi )& -sin(\psi )& 0\\ sin(\psi )& cos(\psi )& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}cos(\theta )& 0& sin(\theta )\\ 0& 1& 0\\ -sin(\theta )& 0& cos(\theta )\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& cos(\varphi )& -sin(\varphi )\\ 0& sin(\varphi )& cos(\varphi )\end{array}\right]$$将上式化简，可得$${}_B^E\text{R}=\left[\begin{array}{ccc}cos(\theta )cos(\psi )& sin(\theta )sin(\varphi )cos(\psi )-sin(\psi )cos(\varphi )& sin(\psi )sin(\varphi )+sin(\theta )cos(\varphi )cos(\psi )\\ sin(\psi )cos(\theta )& cos(\varphi )cos(\psi )+sin(\varphi )sin(\psi )sin(\theta )& sin(\theta )sin(\psi )cos(\varphi )-sin(\varphi )cos(\psi )\\ -sin(\theta )& sin(\varphi )cos(\theta )& cos(\theta )cos(\varphi )\end{array}\right](2)$$则机体坐标系{B}与地面坐标系{E}之间的转换关系满足下式：$$\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]{=}_B^E\text{R}\left[\begin{array}{c}x\\ y\\ z\end{array}\right]（3）$$其中${\left[\begin{array}{ccc}X,& Y,& Z\end{array}\right]}^T$是某点p在地面坐标系中的表示，${\left[\begin{array}{ccc}x,& y,& z\end{array}\right]}^T$是某点p在机体坐标系中的表示。至此，无人机系统的坐标系建立完成，接下来进行动力学建模。

2.无人机动力学建模

（1）无人机位置模型

无人机动力学建模主要依据牛顿第二定律，即
$$\{\begin{array}{cc}\overrightarrow{F}=& m\frac{d\overrightarrow{V}}{dt}\\ \overrightarrow{M}=& \frac{d\overrightarrow{H}}{dt}\end{array}(4)$$$\overrightarrow{F}$是作用于四旋翼上所有外力的和；$\overrightarrow{V}$是四旋翼质心速度；m为四旋翼质量；$\overrightarrow{H}$为四旋翼相对于地面坐标系的动量矩。
首先考虑四旋翼所受外力，则可以得到如下公式：
$$ma=F_D+F_G+f+D_i(5)$$其中：$a={\left[\begin{array}{ccc}\ddot{x}& \ddot{y}& \ddot{z}\end{array}\right]}^T$表示无人机位置信息的加速度，$F_D$表示四旋翼无人机四个旋翼产生的升力之和，上面的公式是在地面坐标系下表示，因此四旋翼四个旋翼的升力之和需要在地面坐标系下表示，定义四旋翼无人机四个旋翼产生的升力之和在机体坐标系下的表示为${}_{}^B{\text{F}}_D$,则其大小为$${}_{}^B{\text{F}}_D=\sum _{j=1}^4\rho {\omega }_j^2(6)$$其中$\rho$是升力系数，${\omega }_j$是电机旋转的角速度。该力的方向与机体坐标系的z轴方向一致，因此可以将其写为：$${}_{}^B{\text{F}}_D=\left[\begin{array}{c}0\\ 0\\ {\sum }_{j=1}^4\rho {\omega }_j^2\end{array}\right](7)$$将其转换到地面坐标系$$F_D{=}_{}^E{\text{F}}_D{=}_B^E{\text{R}}_{}^B{\text{F}}_D（8）$$$F_G$表示四旋翼受到的重力，在地面坐标系中方向垂直向下，大小为mg；$f$表示四旋翼所受空气阻力，其大小为$f=kv$,k为空气阻力系数，v为飞行器速度；$D_i$表示四旋翼无人机内部不可建模的部分及外部扰动，其表达式：$$D_i=\left[\begin{array}{c}{d}_1\\ d_2\\ d_3\end{array}\right](9)$$将(8),(9)代入(5)中，可得下式：$$m\left[\begin{array}{c}\ddot{x}\\ \ddot{y}\\ \ddot{z}\end{array}\right]{=}_B^E\text{R}\left[\begin{array}{c}0\\ 0\\ {\sum }_{j=1}^4\rho {\omega }_j^2\end{array}\right]-m\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]-\left[\begin{array}{c}{k}_1\dot{x}\\ k_2\dot{y}\\ k_3\dot{z}\end{array}\right]+\left[\begin{array}{c}{d}_1\\ d_2\\ d_3\end{array}\right](10)$$其中$k_1,k_2,k_3$分别是x，y，z三个方向的阻力系数，$\dot{x},\dot{y},\dot{z}$代表了在x，y，z三个方向的速度，将${}_B^E\text{R}$的值代入上式，可得$$\{\begin{array}{cccc}\ddot{x}=& \frac{{\sum }_{j=1}^4\rho {\omega }_j^2}{m}(s\psi s\varphi +c\psi c\varphi s\theta )& -\frac{k_1}{m}\dot{x}+d_1& \\ \ddot{y}=& \frac{{\sum }_{j=1}^4\rho {\omega }_j^2}{m}(s\psi s\theta c\varphi -c\psi s\varphi )& -\frac{k_2}{m}\dot{y}+d_2& \\ \ddot{z}=& \frac{{\sum }_{j=1}^4\rho {\omega }_j^2}{m}c\theta c\varphi -g-\frac{k_3}{m}\dot{z}+d_3& & \end{array}(11)$$上式就是无人机位置模型，其中$s\theta =sin(\theta ),c\theta =cos(\theta )$

（2）无人机姿态模型

在获得无人机位置模型后，要考虑四旋翼的姿态,为了更加方便描述姿态，下面公式描述是在机体坐标系下描述。根据(4)式，对四旋翼进行力矩分析，设M为作用于四旋翼无人机上的合力矩，则$$M=M_L+M_f(12)$$
$M_f$是空气阻力对旋翼产生的反力矩。当飞行器两组旋翼产生的力矩不相等时，便会导致四旋翼无人机的偏航运动。由此可得，空气阻力对旋翼产生的反力矩为$$M_f=M_{f2}+M_{f4}-M_{f1}-M_{f3}=\left[\begin{array}{c}0\\ 0\\ {\sum }_{j=1}^4(-1{)}^j\gamma {\omega }_j^2\end{array}\right](13)$$其中,$\gamma$为转动空气阻力系数。
$M_L=[M_{Lx},M_{Ly},M_{Lz}{]}^T$表示四旋翼无人机四个旋翼产生的升力矩之和。$L$表示每个旋翼到四旋翼无人机重心的距离，可由右手定则得到旋翼产生的升力矩如下 (示意图见上面)
$$M_L=\left[\begin{array}{c}{M}_{Lx}\\ M_{Ly}\\ M_{Lz}\end{array}\right]=\left[\begin{array}{c}L(F_{L4}-F_{L2})\\ L(F_{L3}-F_{L1})\\ 0\end{array}\right](14)$$其中，$F_{L1},F_{L2},F_{L3},F_{L4}$分别是Motor1----Motor4四个电机产生的力，将(13),(14)式代入(12)中，可得下式$$M=M_L+M_f=\left[\begin{array}{c}L(F_{L4}-F_{L2})\\ L(F_{L3}-F_{L1})\\ {\sum }_{j=1}^4(-1{)}^j\gamma {\omega }_j^2\end{array}\right](15)$$由角动量定理及哥式定理有转动方程 $$M=I\dot{\varpi }+\varpi \times I\varpi (16)$$其中，$I$表示四旋翼无人机在机体坐标系下的转动惯量，为对角矩阵，表达式如下$$I=\left[\begin{array}{ccc}{I}_{xx}& 0& 0\\ 0& I_{yy}& 0\\ 0& 0& I_{zz}\end{array}\right](17)$$$\varpi =[p,q,r{]}^T$,$p,q,r$是机体坐标系下的横滚，俯仰，偏航角速度，所以(16)式可改写如下：$$M=\left[\begin{array}{ccc}{I}_{xx}& 0& 0\\ 0& I_{yy}& 0\\ 0& 0& I_{zz}\end{array}\right]\left[\begin{array}{c}\dot{p}\\ \dot{q}\\ \dot{r}\end{array}\right]+\left[\begin{array}{c}p\\ q\\ r\end{array}\right]\times \left[\begin{array}{ccc}{I}_{xx}& 0& 0\\ 0& I_{yy}& 0\\ 0& 0& I_{zz}\end{array}\right]\left[\begin{array}{c}p\\ q\\ r\end{array}\right](18)$$结合式(15)与式(18),可得四旋翼的姿态方程$$\{\begin{array}{c}\dot{p}=qr(\frac{I_{yy}-I_{zz}}{I_{xx}})+\frac{L(F_{L4}-F_{L2})}{I_{xx}}\\ \dot{q}=pr(\frac{I_{zz}-I_{xx}}{I_{yy}})+\frac{L(F_{L3}-F_{L1})}{I_{yy}}\\ \dot{r}=pq(\frac{I_{xx}-I_{yy}}{I_{zz}})+\frac{{\sum }_{j=1}^4(-1{)}^j\gamma {\omega }_j^2}{I_{zz}}\end{array}(19)$$
由于角速度$\varpi =[p,q,r{]}^T$是在机体坐标系下的描述，而前面四旋翼运动方程是在地面坐标系下的描述，角速度$\varpi$与欧拉角$[\varphi ,\theta ,\psi {]}^T$有如下转换关系（坐标变换，乘以旋转矩阵）：
$$\{\begin{array}{cc}\dot{\varphi }=& p+qsin(\varphi )tan(\theta )+rcos(\varphi )tan(\theta )\\ \dot{\theta }=& qcos(\varphi )-rsin(\varphi )\\ \dot{\psi }=& qsin(\varphi )/cos\theta +rcos(\varphi )/cos(\theta )\end{array}(20)$$当姿态角小位移运动时，$$sin\varphi \approx sin\theta \approx sin\psi \approx 0$$$$cos\varphi \approx cos\theta \approx cos\psi \approx 1$$因此(20)改写如下$$\{\begin{array}{c}\dot{\varphi }=p\\ \dot{\theta }=q\\ \dot{\psi }=r\end{array}(21)$$将(21)代入(19),可得四旋翼姿态方程$$\{\begin{array}{c}\ddot{\varphi }=\dot{\theta }\dot{\psi }(\frac{I_{yy}-I_{zz}}{I_{xx}})+\frac{L(F_{L4}-F_{L2})}{I_{xx}}\\ \ddot{\theta }=\dot{\varphi }\dot{\psi }(\frac{I_{zz}-I_{xx}}{I_{yy}})+\frac{L(F_{L3}-F_{L1})}{I_{yy}}\\ \ddot{\psi }=\dot{\varphi }\dot{\theta }(\frac{I_{xx}-I_{yy}}{I_{zz}})+\frac{{\sum }_{j=1}^4(-1{)}^j\gamma {\omega }_j^2}{I_{zz}}\end{array}(22)$$

（3）无人机完整非线性模型

现定义$u_i(i=1,2,3,4)$为四旋翼无人机的实际控制输入量，表达式如下$$\left[\begin{array}{c}{u}_1\\ u_2\\ u_3\\ u_4\end{array}\right]=\left[\begin{array}{c}{F}_{L1}+F_{L2}+F_{L3}+F_{L4}\\ L(F_{L4}-F_{L2})\\ L(F_{L3}-F_{L1})\\ M_{f2}+M_{f4}-M_{f1}-M_{f3}\end{array}\right]=\left[\begin{array}{c}{\sum }_{j=1}^4\rho {\omega }_j^2\\ L\rho ({\omega }_4^2-{\omega }_2^2)\\ L\rho ({\omega }_3^2-{\omega }_1^2)\\ {\sum }_{j=1}^4(-1{)}^j\gamma {\omega }_j^2\end{array}\right](23)$$其中，$u_1$控制四旋翼无人机的垂直运动;$u_2$控制四旋翼无人机的滚转运动;$u_3$控制四旋翼无人机的俯仰运动;$u_4$控制四旋翼无人机的偏航运动。

将式(23)代入(11)与(22)中，可得无人机完整非线性方程$$\{\begin{array}{c}\ddot{x}=\frac{u_1}{m}(s\psi s\varphi +c\psi c\varphi s\theta )-\frac{k_1}{m}\dot{x}+d_1\\ \ddot{y}=\frac{u_1}{m}(s\psi s\theta c\varphi -c\psi s\varphi )-\frac{k_2}{m}\dot{y}+d_2\\ \ddot{z}=\frac{u_1}{m}c\theta c\varphi -g-\frac{k_3}{m}\dot{z}+d_3\\ \ddot{\varphi }=\dot{\theta }\dot{\psi }(\frac{I_{yy}-I_{zz}}{I_{xx}})+\frac{u_2}{I_{xx}}+d_4\\ \ddot{\theta }=\dot{\varphi }\dot{\psi }(\frac{I_{zz}-I_{xx}}{I_{yy}})+\frac{u_3}{I_{yy}}+d_5\\ \ddot{\psi }=\dot{\varphi }\dot{\theta }(\frac{I_{xx}-I_{yy}}{I_{zz}})+\frac{u_4}{I_{zz}}+d_6\end{array}(24)$$式中，$d_i,(i=1,2...6)$是系统扰动，四旋翼模型建立完毕。

二、无人机simulink模型

为了简化模型，我们令$k_i=0(i=1,2,3)$,即忽略空气阻力;令$d_i=0(i=1,2...6)$，即认为不存在系统扰动，有需要的同学这两样可以自己加，我建立的模型不包括这两部分。
由于simulink不方便打希腊字母，我用了一些简单符号替代，具体替代如下表：

原符号	simulink中替代符号	含义
$\varphi$	fan	横滚角
$\theta$	theta	俯仰角
$\psi$	psi	偏航角
$\rho$	rho	四旋翼升力系数
$\gamma$	gama	空气阻力系数

模型无人机参数选择如下：

可以根据自己需要修改；模型已经上传，可自行下载（两个版本：Matlab2020b与Matlab2013b）
https://github.com/hjl-up/Quadrotor

三、模型推导补充

关于公式(20)的推导，参考链接: 欧拉角速率与机体角速度转换详细推导
.