四旋翼无人机动力学模型建立

1、坐标系的建立及坐标变换

地理坐标系 $O_a(X_a,Y_a,Z_a)$ 的原点定义为起飞前的位置，$X$ 轴与 $Y$ 轴在同一平面内且相互垂直，$Z$ 轴垂直向上；
机体坐标系 $O_b(X_b,Y_b,Z_b)$ 以4个旋翼的相交点为原点，$X$ 轴指向电机1，$Y$ 轴指向电机4，$Z$ 轴垂直于机身向上。

机体坐标系转为地理坐标系的旋转矩阵为：
$$R_b^n=R_{Z_b}{R}_{Y_b}{R}_{X_b}=\left[\begin{array}{ccc}C(\theta )C(\psi )& S(\phi )S(\theta )C(\psi )-C(\phi )S(\psi )& C(\phi )S(\theta )C(\psi )+S(\phi )S(\psi )\\ C(\theta )S(\psi )& S(\phi )S(\theta )S(\psi )+C(\phi )C(\psi )& C(\phi )S(\theta )S(\psi )-S(\phi )C(\psi )\\ -S(\theta )& S(\phi )C(\theta )& C(\phi )C(\theta )\end{array}\right]\text{\hspace{1em}(1)}$$

2、动力学模型建立

	向量
位置向量	$\eta =[x,y,z]$
姿态向量	$\xi =[\phi ,\theta ,\psi ]$
速度向量	$v=[\dot{x},\dot{y},\dot{z}]$
欧拉角速率向量	$\omega =[\dot{\phi },\dot{\theta },\dot{\psi }]=[p,q,r]$

2.1 质心平动方程

$$F_{sum}=F_n-F_g-F_f=m\frac{dv}{dt}\text{\hspace{1em}(2)}$$

其中，$F_n$ 为四旋翼升力合，$F_g$ 为机体自身重力，$F_f$ 为空气阻力。

1、四旋翼升力合

旋翼上的电机产生的升力正比于电机转速的平方，如下：
$$F_i=K{\omega }_i^2(i=1,2,3,4)$$

其中，$K$ 为升力系数。则四旋翼的升力合为：
$$F_b=\sum _{i=1}^4{F}_i=F_1+F_2+F_3+F_4=K\sum _{i=1}^4{\omega }_i^2$$

由于高度是通过四旋翼的总升力进行控制的，由上式可得高度通道的虚拟控制量，不妨将其定义为 $U_1$，即
$$U_1=K({\omega }_1^2+{\omega }_2^2+{\omega }_3^2+{\omega }_4^2)$$

将 $U1$ 变换到地理坐标系中，既有：
$$F_n=R_b^n[0,0,U_1{]}^T\text{\hspace{1em}(3)}$$

2、机体的重力

在地理坐标系下，重力始终向下，有
$$F_g=[0,0,mg{]}^T\text{\hspace{1em}(4)}$$

3、空气阻力

空气对无人机所施加的阻力大小与其飞行速度成正比，有
$$F_f=[k_x\dot{x},k_y\dot{y},k_z\dot{z}]\text{\hspace{1em}(5)}$$

其中，$k_x,k_y,k_z$ 是由空气阻力系数在四旋翼三个坐标轴下进行分解得到的。
结合公式（1）-（5），有：
$$F_n-F_g-F_f=R_b^n\left[\begin{array}{c}0\\ 0\\ U_1\end{array}\right]-\left[\begin{array}{c}0\\ 0\\ mg\end{array}\right]-\left[\begin{array}{c}{k}_x\dot{x}\\ k_y\dot{y}\\ k_z\dot{z}\end{array}\right]=m\frac{dv}{dt}$$
两边同除以 $m$，再代入$R_b^n$，最终质心的平动方程为：
$$\{\begin{array}{l}\ddot{x}=\frac{U_1}{m}(\cos \phi \sin \theta \cos \psi +\sin \phi \sin \psi )-\frac{k_x\dot{x}}{m}\\ \ddot{y}=\frac{U_1}{m}(\cos \phi \sin \theta \sin \psi -\sin \phi \cos \psi )-\frac{k_y\dot{y}}{m}\\ \ddot{z}=\frac{U_1}{m}\cos \phi \cos \theta -g-\frac{k_z\dot{z}}{m}\end{array}\text{\hspace{1em}(6)}$$

2.2 绕质心的转动方程

根据欧拉定理，绕质心的转动方程为：
$$M=I\dot{\omega }+\omega \times I\omega$$

其中，$I$ 为转动惯量矩阵，且只有在对角线上才有数值，
$$I=\left[\begin{array}{ccc}{I}_x& 0& 0\\ 0& I_y& 0\\ 0& 0& I_z\end{array}\right]$$

四旋翼无人机的力矩来源有：旋翼力矩 $M_1$、陀螺力矩 $M_2$、空气阻力力矩 $M_3$。则合外力矩 $M$ 的表达式为：
$$M=M_1+M_2+M_3$$

1、旋翼力矩 $M_1$

在旋翼力矩中，滚转力矩 $M_{\phi }$ 和俯仰力矩 $M_{\theta }$ 的产生是由同一转轴上的两电机转速不相等，导致该转轴存在升力差，进而升力作用于旋翼上所致。
而偏航力矩 $M_{\psi }$ 是由富余的反扭矩产生的，即 $X$ 轴和 $Y$ 轴上两旋翼作用于机身的反扭距存在偏差，这样在保证四旋翼总升力不变的同时，总的反扭距不为 0，从而使四旋翼绕 $Z$ 轴转动。
$$M_1=\left[\begin{array}{c}{M}_{\phi }\\ M_{\theta }\\ M_{\psi }\end{array}\right]=\left[\begin{array}{c}{U}_2\\ U_3\\ U_4\end{array}\right]=\left[\begin{array}{c}Kl(-{\omega }_2^2+{\omega }_4^2)\\ Kl(-{\omega }_1^2+{\omega }_3^2)\\ K_{d}l(-{\omega }_1^2+{\omega }_2^2-{\omega }_3^2+{\omega }_4^2)\end{array}\right]$$

其中，$K_d$ 为反扭距系数；$l$ 为四旋翼各旋翼的中心到其质心的距离；$U_2,U_3,U_4$ 分别为滚转、俯仰和偏航通道的虚拟控制量。

2、陀螺力矩 $M_2$

根据角动量定理，当四旋翼做出倾斜的滚转或俯仰运动时，其转轴的角速度会有所改变，则此时电机会给旋翼施加一个力矩，称为陀螺力矩，它将迫使机体旋转。需注意的是，四旋翼悬停或偏航运动的过程中，陀螺力矩是不存在的。
$$M_2=\omega \times J_{RP}[0,0,1{]}^T=J_{RP}[-q,p,0{]}^T\Omega$$

其中，螺旋桨的转动惯量用 $J_{RP}$ 表示；$\Omega =-{\omega }_1+{\omega }_2-{\omega }_3+{\omega }_4$ 为电机转速的代数和。

3、空气阻力力矩 $M_3$

空气阻力力矩是由空气阻力作用于旋翼上所形成的，其在 3 个坐标轴下的分量 $M_{3x},M_{3y},M_{3z}$ 与机体角速度 ${\omega }_x,{\omega }_y,{\omega }_z$ 成正比。又因为四旋翼通常以较小的姿态角进行飞行的，所以 ${\omega }_x,{\omega }_y,{\omega }_z$ 近似为欧拉角速率向量 $\omega =[p,q,r]$，即
$$\left[\begin{array}{c}{\omega }_x\\ {\omega }_y\\ {\omega }_z\end{array}\right]=\left[\begin{array}{c}p\\ q\\ r\end{array}\right]$$

那么，空气阻力力矩的表达式为：
$$M_3=[k_{\phi }p,k_{\theta }q,k_{\psi }r{]}^T$$

其中，$k_{\phi },k_{\theta },k_{\psi }$ 分别为总阻力转矩系数在 $X,Y,Z$ 轴的分量。

四旋翼绕质心的转动方程为：
$$\{\begin{array}{l}\ddot{\phi }=\frac{I_y-I_z}{I_x}\dot{\theta }\dot{\psi }-\frac{J_{RP}}{I_x}\dot{\theta }\Omega +\frac{U_2}{I_x}-\frac{k_{\phi }p}{I_x}\\ \ddot{\theta }=\frac{I_z-I_x}{I_y}\dot{\phi }\dot{\psi }-\frac{J_{RP}}{I_y}\dot{\phi }\Omega +\frac{U_3}{I_y}-\frac{k_{\theta }q}{I_y}\\ \ddot{\psi }=\frac{I_x-I_y}{I_z}\dot{\phi }\dot{\theta }+\frac{U_4}{I_z}-\frac{k_{\psi }r}{I_z}\end{array}$$

最终得到四旋翼无人机动力学模型为：
$$\{\begin{array}{l}\ddot{x}=\frac{U_1}{m}(\cos \phi \sin \theta \cos \psi +\sin \phi \sin \psi )-\frac{k_x\dot{x}}{m}\\ \ddot{y}=\frac{U_1}{m}(\cos \phi \sin \theta \sin \psi -\sin \phi \cos \psi )-\frac{k_y\dot{y}}{m}\\ \ddot{z}=\frac{U_1}{m}\cos \phi \cos \theta -g-\frac{k_z\dot{z}}{m}\\ \ddot{\phi }=\frac{I_y-I_z}{I_x}\dot{\theta }\dot{\psi }-\frac{J_{RP}}{I_x}\dot{\theta }\Omega +\frac{U_2}{I_x}-\frac{k_{\phi }p}{I_x}\\ \ddot{\theta }=\frac{I_z-I_x}{I_y}\dot{\phi }\dot{\psi }-\frac{J_{RP}}{I_y}\dot{\phi }\Omega +\frac{U_3}{I_y}-\frac{k_{\theta }q}{I_y}\\ \ddot{\psi }=\frac{I_x-I_y}{I_z}\dot{\phi }\dot{\theta }+\frac{U_4}{I_z}-\frac{k_{\psi }r}{I_z}\end{array}$$