多旋翼无人机建模

建模之前我们先分析一下多旋翼无人机有哪些状态量：用于表示线运动的三轴位置、速度和加速度；用于表示角运动的三轴姿态角和姿态角速度；这一共是15个状态量。

首先来看线运动方程的建立过程：我们取地理坐标系为北东地(NED)，机体系为前右下：

由牛顿第二定律有：

$F=ma$
即
$F=m\left[\begin{array}{l}{a}_x\\ a_y\\ a_z\end{array}\right]$

注意此处的$F$和加速度$a$均是在地理系下的表示($F$表示四个旋翼产生的总升力)，进一步将上式展开可得

$$\left[\begin{array}{c}0\\ 0\\ mg\end{array}\right]-{\mathbf{R}}_b^e\left[\begin{array}{c}0\\ 0\\ f_1+f_2+f_3+f_4\end{array}\right]=m\left[\begin{array}{l}{a}_x\\ a_y\\ a_z\end{array}\right]$$

其中${\mathbf{R}}_b^e$表示由机体坐标系到地理坐标系的转换矩阵，由于$f_i,i=1\dots 4$分别表示四个旋翼沿机体$Z$轴负方向产生的升力，故需要将其转换到地理系中；

$${\mathbf{R}}_b^e=\left[\begin{array}{ccc}c\theta c\psi & s\varphi s\theta c\psi -c\varphi s\psi & c\varphi s\theta c\psi +s\varphi s\psi \\ c\theta s\psi & s\varphi s\theta s\psi +c\varphi c\psi & c\varphi s\theta s\psi -s\varphi c\psi \\ -s\theta & s\varphi c\theta & c\varphi c\theta \end{array}\right]$$

联立上式进一步化简即可得到线运动的模型表示：

$$\{\begin{array}{c}\ddot{x}=(\mathrm{c}\varphi \mathrm{s}\theta \mathrm{c}\psi +\mathrm{s}\varphi \mathrm{s}\psi )\frac{-F}{m}\\ \ddot{y}=(\mathrm{c}\varphi \mathrm{s}\theta \mathrm{s}\psi -\mathrm{s}\varphi \mathrm{c}\psi )\frac{-F}{m}\\ \ddot{z}=g-(\mathrm{c}\varphi \mathrm{c}\theta )\frac{F}{m}\end{array}$$

线运动的建模过程很简单，但是多旋翼不光能移动还能旋转，这个旋转过程就需要刚体的旋转相关知识。

物体如何旋转跟旋翼产生的力矩有关，力矩与力F和距离l有如下公式：$$M=F\ast l$$

而这里的力矩又与多旋翼动力布局有关

如果是十字型的布局：

$$F=\sum _{i=1}^4{T}_i=c_{\mathrm{T}}\left(w_1^2+w_2^2+w_3^2+w_4^2\right)$$

$$\{\begin{array}{l}{\tau }_x=d{c}_{\mathrm{T}}\left(-w_2^2+w_4^2\right)\\ {\tau }_y=d{c}_{\mathrm{T}}\left(w_1^2-w_3^2\right)\\ {\tau }_z=c_{\mathrm{M}}\left(w_1^2-w_2^2+w_3^2-w_4^2\right)\end{array}$$

写成矩阵形式如下：

$$\left[\begin{array}{l}F\\ {\tau }_x\\ {\tau }_y\\ {\tau }_z\end{array}\right]=\left[\begin{array}{cccc}{c}_{\mathrm{T}}& c_{\mathrm{T}}& c_{\mathrm{T}}& c_{\mathrm{T}}\\ 0& -d{c}_{\mathrm{T}}& 0& d{c}_{\mathrm{T}}\\ d{c}_{\mathrm{T}}& 0& -d{c}_{\mathrm{T}}& 0\\ c_{\mathrm{M}}& -c_{\mathrm{M}}& c_{\mathrm{M}}& -c_{\mathrm{M}}\end{array}\right]\left[\begin{array}{c}{w}_1^2\\ w_2^2\\ w_3^2\\ w_4^2\end{array}\right]$$

对X型动力布局：

$$F=\sum _{i=1}^4{T}_i=c_{\mathrm{T}}\left(w_1^2+{\varpi }_2^2+{\varpi }_3^2+{\varpi }_4^2\right)$$

$$\{\begin{array}{c}{\tau }_x=d{c}_{\mathrm{T}}\left(\sqrt{2}{\omega }_1^2/2-\sqrt{2}{\omega }_2^2/2-\sqrt{2}{\omega }_3^2/2+\sqrt{2}{\omega }_4^2/2\right)\\ {\tau }_y=d{c}_{\mathrm{T}}\left(\sqrt{2}{\omega }_1^2/2+\sqrt{2}{\omega }_2^2/2-\sqrt{2}{\omega }_3^2/2-\sqrt{2}{\omega }_4^2/2\right)\\ {\tau }_z=c_{\mathrm{M}}\left({\omega }_1^2-{\omega }_2^2+{\omega }_3^2-{\omega }_4^2\right)\end{array}$$
上式中$F$为四个旋翼产生的总升力，${\tau }_x,{\tau }_y,{\tau }_z$分别表示旋翼产生的作用在三个姿态通道的控制力矩。

根据欧拉方程$$M=J\epsilon +\omega \times J\omega$$

其中$\epsilon$表示角加速度，$\omega$表示角速度，$M$表示姿态通道控制力矩；
欧拉角速度
$$\dot{\mathbf{\Theta }}=\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]$$
与机体角速度
$$\mathbf{\Omega }=\left[\begin{array}{c}{w}_x\\ w_y\\ w_z\end{array}\right]$$
之间的关系为

$$\dot{\mathbf{\Theta }}=\mathbf{W}\mathbf{\Omega }$$
$$W=\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]$$假设多旋翼无人机姿态角在小角度内变化，则可认为$W=I$，即$\dot{\mathbf{\Theta }}=\mathbf{\Omega }$

角加速度为$$\left[\begin{array}{c}\ddot{\varphi }\\ \ddot{\theta }\\ \ddot{\psi }\end{array}\right]$$

转动惯量矩阵为
$$\mathbf{J}=\left[\begin{array}{ccc}{J}_{xx}& -J_{xy}& -J_{xz}\\ -J_{yx}& J_{yy}& -J_{yz}\\ -J_{zx}& -J_{zy}& J_{zz}\end{array}\right]$$
其中$J_{xy}=J_{yx}，J_{xz}=J_{zx}，J_{yz}=J_{zy}$,对于标准多旋翼这样的中心对成物体，有$J_{xy}=J_{xz}=J_{yz}=0$,则有
$$\left[\begin{array}{lll}{J}_{xx}& 0& 0\\ 0& J_{yy}& 0\\ 0& 0& J_{zz}\end{array}\right]$$

整理可得

$$\{\begin{array}{l}\ddot{\varphi }=\dot{\theta }\dot{\psi }\frac{J_y-J_z}{J_x}+\frac{{\tau }_x}{J_x}\\ \ddot{\theta }=\dot{\varphi }\dot{\psi }\frac{J_z-J_x}{J_y}+\frac{{\tau }_y}{J_y}\\ \ddot{\psi }=\dot{\varphi }\dot{\theta }\frac{J_x-J_y}{J_z}+\frac{{\tau }_z}{J_z}\end{array}$$

至此，多旋翼的模型建立完毕。可以看到建模的其实就是为了得到输入的力与加速度的关系，通过牛顿方程可以得到线运动的加速度，通过欧拉方程可以得到角加速度，所以这种建模方法也叫牛顿-欧拉方程。