四旋翼无人机

四旋翼无人机的动力学模型

为了建立起能够描述无人机的物理和运动特性的方程，需要定义建模时的坐标系。定义两种坐标系：固定坐标系（惯性坐标系）{A}和无人机的机身坐标系{B}。使用欧拉角$\xi ={\left(\varphi ,\theta ,\psi \right)}^T$表示在机身坐标系中无人机绕各个轴转动的角度，$P={\left(x,y,z\right)}^T$表示无人机重心坐标。无人机为"X"型，无人机质量为$m$，机臂长度为$l$，绕无人机三个轴转动的转动惯量分别为$I_{xx},I_{yy},I_{zz}$。无人机结构图如下

四个旋翼的升力分别为$F_1,F_2,F_3,F_4$，都沿着无人机机身$z$轴方向。总升力$u_1=F_1+F_2+F_3+F_4$, 作用于$x$轴的力矩$u_2=\frac{l}{\sqrt{2}}(-F_1-F_2+F_3+F_4)$, 作用于$y$轴的力矩$u_3=\frac{l}{\sqrt{2}}(-F_1+F_2+F_3-F_4)$, 作用于$z$轴的力矩（扭矩） $u_4=-b(F_1+F_2-F_3+F_4)$, 其中$b$ 是力到扭矩的系数。根据牛顿-欧拉方程可得无人机运动学方程
$$I\dot{\Omega }+\Omega \times I\Omega =N$$其中I是无人机惯性张量矩阵，认为无人机机身对称，则$I=diag(I_{xx},I_{yy},I_{zz})$，$\Omega$表示无人机绕各个轴转动的角速度$\Omega =(\dot{\varphi },\dot{\theta },\dot{\psi }{)}^T$，$N$表示作用在无人机上的合力矩，$N=(u_2，u_3，u_4{)}^T$。展开得
$$\ddot{\varphi }=\dot{\theta }\dot{\psi }\frac{I_{yy}-I_{zz}}{I_{xx}}+\frac{u_2}{I_{xx}}$$$$\ddot{\theta }=\dot{\varphi }\dot{\psi }\frac{I_{zz}-I_{xx}}{I_{yy}}+\frac{u_3}{I_{yy}}$$$$\ddot{\psi }=\dot{\theta }\dot{\varphi }\frac{I_{xx}-I_{yy}}{I_{zz}}+\frac{u_4}{I_{zz}}$$由牛二定律
$$ma{=}_B^A{R}_{zyx}F$$其中$a=(\ddot{x}，\ddot{y}，\ddot{z}{)}^T$，$F=(0，0，u_1{)}^T$。${}_B^A{R}_{zyx}$的计算在后面。展开得
$$\ddot{x}=\frac{u_1(\cos \varphi \sin \theta \cos \psi +\sin \varphi \sin \psi )}{m}$$$$\ddot{y}=\frac{u_1(\cos \varphi \sin \theta \sin \psi -\sin \varphi \cos \psi )}{m}$$$$\ddot{z}=\frac{u_1\cos \varphi \cos \theta }{m}-g$$

四旋翼无人机的姿态表示

Z-Y-X 欧拉角

先将{B}坐标系绕{B}的z轴旋转 ψ