四旋翼数学模型

首先打个小广告，一直很喜欢无人机，最近有空研究了一下Pixhawk(PX4)飞控，并把总结和心得写到公众号 “SantyNotebook” 里和大家分享，感兴趣的朋友可以一起来交流一下，如何从零手写无人机飞控~
本篇同步发在公众号推送中《四旋翼数学模型》
作为控制器设计的基础，这篇介绍一下外力作用下多旋翼飞机的状态变化规律~

1 四旋翼飞行器建模

1.1 坐标系与旋转矩阵

我们建立地面坐标系$O_e{x}_e{y}_e{z}_e$以及机体坐标系$O_b{x}_b{y}_b{z}_b$，用来描述飞机受力，力矩与其位姿状态。

如果我们规定：地系沿$O{z}_e$轴$\psi$角旋转，再沿$O{y}_e$轴$\theta$角旋转，最后沿$O{x}_e$轴旋转$\varphi$得到机体系，可以得到旋转矩阵：
$$R_e^b=\left[\begin{array}{ccc}\cos \theta \cos \psi & \cos \theta \sin \psi & -\sin \theta \\ -\cos \varphi \sin \psi +\sin \varphi \sin \theta \cos \psi & \cos \varphi \cos \psi +\sin \varphi \sin \theta \sin \psi & \sin \varphi \cos \theta \\ \sin \varphi \sin \psi +\cos \varphi \sin \theta \cos \psi & -\sin \varphi \cos \psi +\cos \varphi \sin \theta \sin \psi & \cos \varphi \cos \theta \end{array}\right]$$
它的含义是对于地系中的任意坐标${\vec{a}}_e$，其在机体系中的表达为${\vec{a}}_b=R_e^b{\vec{a}}_e$
这样做是很有意义的，因为我们获取了机体内状态，就知道了在惯性系内的表达，反之亦然。
其中的三个角度，常用来表达飞行器当前姿态，即偏航角$\psi$，俯仰角$\varphi$，与滚转角$\theta$。

1.2 牛顿欧拉方程

刚体运动=质心的平动+绕质心的转动，质心的平动用牛顿第二定律描述，即：
$$F=m\ast \frac{\mathrm{d}v}{\mathrm{d}i}$$
绕质心的转动由欧拉方程描述，即：
$$M=J\dot{\omega }+\omega \times J\omega$$
物理含义为：作用在刚体上的合力矩使$M$得刚体以角速度、角加速度旋转。

1.3 四旋翼飞行器的动力学模型

动力学模型的输入为合外力、合外力矩，输出为速度、角速度。我们假设飞行器只受重力和螺旋桨拉力。

1.3.1 位置动力学模型

根据牛顿第二定律
$$m{\dot{v}}^e=G^e+T^b$$
式中,${\dot{v}}^e$与$G^e$右上角的$e$代表这是地面坐标系下的向量，$T^b$右上角的$b$代表这是机体坐标系下的向量，代入有
$$\left[\begin{array}{l}{\dot{v}}_x\\ {\dot{v}}_y\\ {\dot{v}}_z\end{array}\right]=\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]+\frac{1}{m}{\left(R_e^b\right)}^{-1}\left[\begin{array}{c}0\\ 0\\ -T\end{array}\right]$$
即
$$\left[\begin{array}{c}{\dot{v}}_x\\ {\dot{v}}_y\\ {\dot{v}}_z\end{array}\right]=\left[\begin{array}{c}-\frac{T}{m}(\cos \psi \sin \theta \sin \varphi +\sin \psi \sin \varphi )\\ -\frac{T}{m}(\sin \psi \sin \theta \cos \varphi -\cos \psi \sin \varphi )\\ g-\frac{T}{m}\cos \varphi \cos \theta \end{array}\right]$$

姿态动力学模型

由欧拉方程可得
$$J{\dot{\omega }}^b+{\omega }^b\times J{\omega }^b=G_a^b+T^b$$
式中：
$J$为惯性矩阵，是几何形状与质量分布的考量，对于沿$XY$轴对称的四旋翼无人机：
$$J=\left[\begin{array}{ccc}{I}_{xx}& -I_{xy}& -I_{xz}\\ -I_{xy}& I_{yy}& -I_{yz}\\ -I_{xz}& -I_{yz}& I_{zz}\end{array}\right]=\left[\begin{array}{lll}{I}_{xx}& & \\ & I_{yy}& \\ & & I_{zz}\end{array}\right]$$

${\omega }^b$表示在机体坐标系下的角速度，通常用$p,q,r$来表示在机体轴上的三个分量；
$G_a$表示陀螺力矩，高速旋转脱落非常稳定，有保持自身轴向不变的能力，如果用$J_1$表示整个电机转子和螺旋桨绕机体转轴的总转动惯量，${\tilde{\omega }}_1,{\tilde{\omega }}_2,{\tilde{\omega }}_3,{\tilde{\omega }}_4$表示4个螺旋桨转速，有：
$$G_a=\left[\begin{array}{c}{G}_{a,\varphi }\\ G_{a,\varphi }\\ G_{a,\psi }\end{array}\right]=\left[\begin{array}{c}{J}_{1}q\left({\tilde{\omega }}_1-{\tilde{\omega }}_2+{\tilde{\omega }}_3-{\tilde{\omega }}_4\right)\\ J_{1}p\left(-{\tilde{\omega }}_1+{\tilde{\omega }}_2-{\tilde{\omega }}_3+{\tilde{\omega }}_4\right)\\ 0\end{array}\right]$$

$\tau$表示螺旋桨拉力在机体轴上产生的力矩，有三个分量。
整理得：
$$\left[\begin{array}{c}\dot{p}\\ \dot{q}\\ \dot{r}\end{array}\right]=\left[\begin{array}{l}\frac{1}{I_{xx}}\left[{\tau }_x+qr\left(I_{yy}-I_{zz}\right)-J_{1}q\Omega \right]\\ \frac{1}{I_{yy}}\left[{\tau }_y+qr\left(I_{zz}-I_{xx}\right)-J_{1}p\Omega \right]\\ \frac{1}{I_{zz}}\left[{\tau }_z+pq\left(I_{xx}-I_{yy}\right)\right]\end{array}\right]$$
$$\Omega =-{\tilde{\omega }}_1+{\tilde{\omega }}_2-{\tilde{\omega }}_3+{\tilde{\omega }}_4$$

1.4 四旋翼飞行器的运动学模型

运动学模型的输入为速度和角速度，输出为位置和姿态。

速度位置方程
$${\left[\begin{array}{lll}\dot{x}& \dot{y}& \dot{z}\end{array}\right]}^T={\left[\begin{array}{lll}{v}_x& v_y& v_z\end{array}\right]}^T$$

姿态角的变化率与机体的旋转角速度有如下关系:
$$\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}{l}p\\ q\\ r\end{array}\right]$$
在小扰动情况下姿态角的变化率与机体的旋转角速度近似相等，上式可以近似为：
$$\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]=\left[\begin{array}{l}p\\ q\\ r\end{array}\right]$$

1.5 四旋翼飞行器的飞行控制刚体模型

四旋翼飞行器的飞行控制刚体模型由动力学模型和运动学模型结合而成，将上面结论全部代入有

位置的模型：
$$\left[\begin{array}{c}\ddot{x}\\ \ddot{y}\\ \ddot{z}\end{array}\right]=\left[\begin{array}{c}-\frac{T}{m}(\cos \psi \sin \theta \sin \varphi +\sin \psi \sin \varphi )\\ -\frac{T}{m}(\sin \psi \sin \theta \cos \varphi -\cos \psi \sin \varphi )\\ g-\frac{T}{m}\cos \varphi \cos \theta \end{array}\right]$$
姿态小扰动模型
$$\left[\begin{array}{c}\ddot{\varphi }\\ \ddot{\theta }\\ \ddot{\psi }\end{array}\right]=\left[\begin{array}{l}\frac{1}{I_{xx}}\left[{\tau }_x+\mathrm{qr}\left(I_{yy}-I_{zz}\right)-J_{1}q\Omega \right]\\ \frac{1}{I_{yy}}\left[{\tau }_y+\mathrm{qr}\left(I_{zz}-I_{xx}\right)-J_{1}p\Omega \right]\\ \frac{1}{I_{zz}}\left[{\tau }_z+pq\left(I_{xx}-I_{yy}\right)\right]\end{array}\right]$$
$$\Omega =-{\varpi }_1+{\varpi }_2-{\varpi }_3+{\varpi }_4$$
定性的认识是，飞机速度变化率受到拉力质量比、重力加速度影响并且和当前姿态有关；而旋转角速度变化与拉力产生的力矩，四个电机转速、飞机惯量以及当前姿态角变化率有关