四旋翼无人机建模 (附github源代码)

前言

四旋翼无人机的应用十分广泛，而且四旋翼无人机是非常理想的控制模型。因为四旋翼是四输入(四个螺旋桨的升力) 六输出 (三个位置，三个姿态角)的欠驱动系统，而且四旋翼的三个姿态角之间是互相耦合的，并且位置与姿态也是耦合的，加上其固有的非线性特性，因此对于学习和熟悉控制系统是十分有帮助的。本文简要介绍无人机的建模以及串级PID控制器的设计。

无人机建模

坐标变换

四旋翼运动过程中实际上涉及很多坐标系，一般包括惯性系，地面系，机体系等等。本文假设四旋翼的运动范围不大，因此本文将惯性系和地面坐标系当做同一坐标系处理。并且为了方面显示图形，将无人机的机头定义为机体系+X，无人机的正左方定义为机体系+Y，垂直向上定义为机体系+Z；同理，水平向前定义为惯性系+X，水平向左定义为惯性系+Y，竖直向上定义为惯性系+Z(很多资料，使用前、右、下作为无人机的机体坐标系正方向，同时使用北东地作为惯性系，二者仅仅相差一个旋转矩阵，无本质差别)。给出图示：

图中，CCW代表逆时针旋转，CW代表顺时针旋转，$X_b{Y}_b{Z}_b$代表机体系，$X_i{Y}_i{Z}_i$代表地面系。

地面系($O_i{X}_i{Y}_i{Z}_i$，记为$S_i$)与机体系($O_b{X}_b{Y}_b{Z}_b$，记为$S_b$)之间的平移关系比较简单，四旋翼在$S_i$系中的位置坐标就是两个坐标系的位置关系。$S_i$与$S_b$之间的旋转关系由三个姿态角决定，分别为滚转角$\varphi$，俯仰角$\theta$和偏航角$\psi$。但是三个坐标轴的旋转顺序不同会产生不同的结果，所以这里按照一般顺序定义：先转$\psi$，再转$\theta$，最后转$\varphi$。其对应的旋转矩阵及临时坐标系定义如下：

1. 绕惯性系$S_i$的$Z$轴$O{Z}_i$旋转$\psi$，得到临时坐标系$S_{b1}$。旋转矩阵为 R i b 1 ( ψ ) = [ cos ⁡ ψ sin ⁡ ψ 0 − sin ⁡ ψ cos ⁡ ψ 0 0 0 1 ] R_i^{b1}\left(\psi\right) = \left[ \begin{matrix} \cos{\psi} & \sin{\psi} & 0 \\ -\sin{\psi} & \cos{\psi} & 0\\ 0 & 0 & 1 \end{matrix} \right] Rib1​(ψ)= ​cosψ−sinψ0​sinψcosψ0​001​ ​
2. 绕系$S_{b1}$的$Y$轴$O{Y}_{b1}$旋转$\theta$，得到临时坐标系$S_{b2}$。旋转矩阵为 R b 1 b 2 ( θ ) = [ cos ⁡ θ 0 − sin ⁡ θ 0 1 0 sin ⁡ θ 0 cos ⁡ θ ] R_{b1}^{b2}\left(\theta\right) = \left[ \begin{matrix} \cos{\theta} & 0 & -\sin{\theta} \\ 0 & 1 & 0\\ \sin{\theta} & 0 & \cos{\theta} \end{matrix} \right] Rb1b2​(θ)= ​cosθ0sinθ​010​−sinθ0cosθ​ ​
3. 绕系$S_{b2}$的$X$轴$O{X}_{b2}$旋转$\varphi$，得到机体坐标系$S_b$。旋转矩阵为 R b 2 b ( ϕ ) = [ 1 0 0 0 cos ⁡ ϕ sin ⁡ ϕ 0 − sin ⁡ ϕ cos ⁡ ϕ ] R_{b2}^{b}\left(\phi\right) = \left[ \begin{matrix} 1 & 0 & 0 \\ 0 & \cos{\phi} & \sin{\phi}\\ 0 & -\sin{\phi} & \cos{\phi} \end{matrix} \right] Rb2b​(ϕ)= ​100​0cosϕ−sinϕ​0sinϕcosϕ​ ​

综上，从地面系到机体系的旋转矩阵为
R i b = R b 2 b ( ϕ ) ⋅ R b 1 b 2 ( θ ) ⋅ R I b 1 ( ψ ) = [ c ( ψ ) c ( θ ) c ( θ ) s ( ψ ) − s ( θ ) c ( ψ ) s ( ϕ ) s ( θ ) − c ( ϕ ) s ( ψ ) c ( ϕ ) c ( ψ ) + s ( ϕ ) s ( ψ ) s ( θ ) c ( θ ) s ( ϕ ) s ( ϕ ) s ( ψ ) + c ( ϕ ) c ( ψ ) s ( θ ) c ( ϕ ) s ( ψ ) s ( θ ) − c ( ψ ) s ( ϕ ) c ( ϕ ) c ( θ ) ] \begin{align} \begin{aligned} R_i^b &= R_{b2}^{b}\left(\phi\right)\cdot R_{b1}^{b2}\left(\theta\right) \cdot R_I^{b1}\left(\psi\right)\\ &=\left[ \begin{matrix} c(\psi)c(\theta) & c(\theta)s(\psi) & -s(\theta)\\ c(\psi)s(\phi)s(\theta) - c(\phi)s(\psi) & c(\phi)c(\psi) + s(\phi)s(\psi)s(\theta) & c(\theta)s(\phi)\\ s(\phi)s(\psi) + c(\phi)c(\psi)s(\theta) & c(\phi)s(\psi)s(\theta) - c(\psi)s(\phi) & c(\phi)c(\theta) \end{matrix} \right] \end{aligned} \end{align} Rib​​=Rb2b​(ϕ)⋅Rb1b2​(θ)⋅RIb1​(ψ)= ​c(ψ)c(θ)c(ψ)s(ϕ)s(θ)−c(ϕ)s(ψ)s(ϕ)s(ψ)+c(ϕ)c(ψ)s(θ)​c(θ)s(ψ)c(ϕ)c(ψ)+s(ϕ)s(ψ)s(θ)c(ϕ)s(ψ)s(θ)−c(ψ)s(ϕ)​−s(θ)c(θ)s(ϕ)c(ϕ)c(θ)​ ​​​​
相对应的，从机体系到地面系的变换矩阵为
R b i = R i b − 1 = R i b T = [ c ( ψ ) c ( θ ) c ( ψ ) s ( ϕ ) s ( θ ) − c ( ϕ ) s ( ψ ) s ( ϕ ) s ( ψ ) + c ( ϕ ) c ( ψ ) s ( θ ) c ( θ ) s ( ψ ) c ( ϕ ) c ( ψ ) + s ( ϕ ) s ( ψ ) s ( θ ) c ( ϕ ) s ( ψ ) s ( θ ) − c ( ψ ) s ( ϕ ) − s ( θ ) c ( θ ) s ( ϕ ) c ( ϕ ) c ( θ ) . ] \begin{align} \begin{aligned} R_b^i &= {R_{i}^{b}}^{-1} = {R_i^b}^T\\ &=\left[ \begin{matrix} c(\psi)c(\theta) & c(\psi)s(\phi)s(\theta) - c(\phi)s(\psi) & s(\phi)s(\psi) + c(\phi)c(\psi)s(\theta)\\ c(\theta)s(\psi) & c(\phi)c(\psi) + s(\phi)s(\psi)s(\theta) & c(\phi)s(\psi)s(\theta) - c(\psi)s(\phi)\\ -s(\theta) & c(\theta)s(\phi) & c(\phi)c(\theta) \end{matrix}. \right] \end{aligned} \end{align} Rbi​​=Rib​−1=Rib​T= ​c(ψ)c(θ)c(θ)s(ψ)−s(θ)​c(ψ)s(ϕ)s(θ)−c(ϕ)s(ψ)c(ϕ)c(ψ)+s(ϕ)s(ψ)s(θ)c(θ)s(ϕ)​s(ϕ)s(ψ)+c(ϕ)c(ψ)s(θ)c(ϕ)s(ψ)s(θ)−c(ψ)s(ϕ)c(ϕ)c(θ)​. ​​​​

位置-速度回路动态模型

在得到上面的旋转矩阵之后，加上牛顿第二定律，就可以给出四旋翼的位置动力学模型。
$$\begin{array}{r}\{\begin{array}{rl}& {\mathbf{v}}^i={\dot{\mathbf{x}}}^i\\ & {\mathbf{a}}^i={\dot{\mathbf{v}}}^i=\frac{m{\mathbf{g}}^i+{\mathbf{f}}^b}{m}\end{array}\end{array}$$
其中，上角标$i$代表地面系，上角标$b$代表机体系。已知高中的知识点：向量可以在空间内随意平移，标量没有坐标系的概念，所以把公式$(3)$中的向量统一平移到地面系的原点是完全合理的。因此，可以利用旋转矩阵$(2)$将力转换到地面系中，有
$$\begin{array}{r}\begin{array}{r}{\mathbf{f}}^i=R_b^i\cdot f^b\cdot {\mathbf{e}}_3\end{array}\end{array}$$
其中 e 3 = [ 0 0 1 ] \boldsymbol{e}_3=\left[\begin{matrix}0\\0\\1\end{matrix}\right] e3​= ​001​ ​为机体系的Z轴对应的单位向量，所以有
[ v ˙ x v ˙ y v ˙ z ] = − g i ⋅ [ 0 0 1 ] + f b m ⋅ R b i ⋅ [ 0 0 1 ] . \begin{align} \begin{aligned} \left[\begin{matrix} \dot{v}_x \\ \dot{v}_y \\ \dot{v}_z \end{matrix}\right] = -g^i \cdot \left[\begin{matrix}0 \\ 0 \\ 1\end{matrix}\right] + \frac{f^b}{m}\cdot R_b^i \cdot \left[\begin{matrix}0 \\ 0 \\ 1\end{matrix}\right]. \end{aligned} \end{align} ​v˙x​v˙y​v˙z​​ ​=−gi⋅ ​001​ ​+mfb​⋅Rbi​⋅ ​001​ ​.​​​
将$(2)$带入$(5)$得到
$$\begin{array}{r}\{\begin{array}{rl}& \dot{x}=v_x\\ & \dot{y}=v_y\\ & \dot{x}=v_z\\ & {\dot{v}}_x=\frac{f}{m}\left[c(\psi )s(\theta )c(\varphi )+s(\psi )s(\varphi )\right]\\ & {\dot{v}}_y=\frac{f}{m}\left[s(\psi )s(\theta )c(\varphi )-c(\psi )s(\varphi )\right]\\ & {\dot{v}}_z=-g+\frac{f}{m}c(\varphi )c(\theta )\end{array}\end{array}$$
其中$f=C_T\left({\omega }_1^2+{\omega }_2^2+{\omega }_3^2+{\omega }_4^2\right)$ 是油门 (四个电机的升力和)，$C_T$是升力系数, ${\omega }_i,i=1,2,3,4$ 是第$i$个电机的转速。写成矩阵形式如下：
{ [ x ˙ y ˙ z ˙ ] = [ 1 0 0 0 1 0 0 0 1 ] [ v x v y v z ] [ v ˙ x v ˙ y v ˙ z ] = f m [ c ( ψ ) s ( θ ) c ( ϕ ) + s ( ψ ) s ( ϕ ) s ( ψ ) s ( θ ) c ( ϕ ) − c ( ψ ) s ( ϕ ) c ( ϕ ) c ( θ ) ] − [ 0 0 g ] \begin{align} \left\{ \begin{aligned} & \left[\begin{matrix}\dot{x}\\\dot{y}\\\dot{z}\end{matrix}\right] = \left[\begin{matrix} 1&0&0\\0&1&0\\0&0&1 \end{matrix}\right] \left[\begin{matrix}v_x\\v_y\\v_z\end{matrix}\right]\\ & \left[\begin{matrix}\dot{v}_x\\\dot{v}_y\\\dot{v}_z\end{matrix}\right] = \frac{f}{m} \left[\begin{matrix} c(\psi)s(\theta)c(\phi)+s(\psi)s(\phi)\\ s(\psi)s(\theta)c(\phi) - c(\psi)s(\phi)\\ c(\phi)c(\theta) \end{matrix}\right]-\left[\begin{matrix}0\\0\\g\end{matrix}\right]\\ \end{aligned} \right. \end{align} ⎩ ⎨ ⎧​​ ​x˙y˙​z˙​ ​= ​100​010​001​ ​ ​vx​vy​vz​​ ​ ​v˙x​v˙y​v˙z​​ ​=mf​ ​c(ψ)s(θ)c(ϕ)+s(ψ)s(ϕ)s(ψ)s(θ)c(ϕ)−c(ψ)s(ϕ)c(ϕ)c(θ)​ ​− ​00g​ ​​​​
至此，位置动力学模型建立完成。

姿态-旋转角速度回路动态模型

这里需要引入刚体动力学的欧拉方程。
$$\begin{array}{r}\begin{array}{r}J{\dot{\mathbf{\omega }}}^b+{\mathbf{\omega }}^b\times J{\mathbf{\omega }}^b={\mathbf{M}}^g+{\mathbf{\tau }}^b\end{array}\end{array}$$
其中，$J=diag(J_{xx},J_{yy},J_{zz})$ 无人机沿其机体轴方向的惯性张量矩阵, ${\mathbf{\omega }}^b={\left[\begin{array}{ccc}p& q& r\end{array}\right]}^T$ 无人机在机体系 $S_b$下的旋转角速度, ${\mathbf{M}}^g={\left[\begin{array}{ccc}{M}_x^g& M_y^g& M_z^g\end{array}\right]}^T$是机体系下的陀螺力矩$S_b$， ${\mathbf{\tau }}^b={\left[\begin{array}{ccc}{\tau }_x^b& {\tau }_y^b& {\tau }_z^b\end{array}\right]}^T$ 是无人机的升力为无人机提供的力矩。

Tips：
很多无人机建模忽略了陀螺力矩的影响，实际是不可以忽略的，因为电机的质量虽然只占整个机体很小的比重，但是电机是高速旋转的。众所周知，高速旋转的刚体具有“定轴性”。意思就是，当它的转轴不变的时候，没有任何事情发生，但是如果转轴的方向发生改变，陀螺就会产生“进动”，意图抗拒这个“改变”。所以，当四旋翼悬停的时候，是没有陀螺力矩的；四旋翼只有偏航角改变的时候，也是没有陀螺力矩的。但是如果四旋翼的俯仰角或滚转角发生变化的时候，就会产生陀螺力矩。如果不考虑这个力矩，最后通过调节PID控制器的参数，也能把无人机控制地很好，但是陀螺进动给模型带来的变化是不可以忽略的。

陀螺进动的方程为$\mathbf{G}=\mathbf{H}\times {\mathbf{\omega }}^{\prime }=J_0\mathbf{\omega }\times {\mathbf{\omega }}^{\prime }$，其中$\mathbf{G}$为电机 (加螺旋桨) 的进动力矩，$J_0$为电机加螺旋桨沿着电机转轴的转动惯量，$\omega$为电机的转速，${\omega }^{\prime }$为电机的转轴的转速 (这里就是无人机本身的转速)。进而有，
M g = [ M x M y M z ] = [ ∑ i = 1 4 H i × q ∑ i = 1 4 H i × 1 0 ] = [ J 0 q ( − ω 1 + ω 2 − ω 3 + ω 4 ) J 0 p ( ω 1 − ω 2 + ω 3 − ω 4 ) 0 ] \begin{align} \begin{aligned} \boldsymbol{M}^{g} = \left[\begin{matrix} M_x \\ M_y \\M_z \end{matrix}\right]=\left[\begin{matrix} \sum_{i=1}^4{\boldsymbol{H}_i\times q}\\ \sum_{i=1}^4{\boldsymbol{H}_i\times 1} \\ 0 \end{matrix}\right] = \left[\begin{matrix} J_0q\left(-\omega_1+\omega_2-\omega_3+\omega_4\right)\\ J_0p\left(\omega_1-\omega_2+\omega_3-\omega_4\right) \\ 0 \end{matrix}\right] \end{aligned} \end{align} Mg= ​Mx​My​Mz​​ ​= ​∑i=14​Hi​×q∑i=14​Hi​×10​ ​= ​J0​q(−ω1​+ω2​−ω3​+ω4​)J0​p(ω1​−ω2​+ω3​−ω4​)0​ ​​​​
其中，$p$和$q$已经在上文提到过，${\mathbf{H}}_i$的方向与电机的旋转方向相同，从上图中可以看出，1号3号向上，2号4号向下。

${\tau }_b$的表达式比较简单，就是中学阶段学过的力矩$=$力$\times$力臂。有
τ b = [ τ x τ y τ z ] = [ 2 C T d 2 ( ω 1 2 − ω 2 2 − ω 3 2 + ω 4 2 ) 2 C T d 2 ( − ω 1 2 − ω 2 2 + ω 3 2 + ω 4 2 ) C M ( − ω 1 2 + ω 2 2 − ω 3 2 + ω 4 2 ) ] \begin{align} \begin{aligned} \boldsymbol{\tau}^b = \left[\begin{matrix} \tau_x \\ \tau_y \\ \tau_z \end{matrix}\right]=\left[\begin{matrix} \frac{\sqrt{2}C_Td}{2}\left(\omega_1^2 - \omega_2^2 - \omega_3^2 + \omega_4^2\right) \\ \frac{\sqrt{2}C_Td}{2}\left(-\omega_1^2 - \omega_2^2 + \omega_3^2 + \omega_4^2\right) \\ C_M\left(-\omega_1^2 + \omega_2^2 - \omega_3^2 + \omega_4^2\right) \end{matrix}\right] \end{aligned} \end{align} τb= ​τx​τy​τz​​ ​= ​22 ​CT​d​(ω12​−ω22​−ω32​+ω42​)22 ​CT​d​(−ω12​−ω22​+ω32​+ω42​)CM​(−ω12​+ω22​−ω32​+ω42​)​ ​​​​
其中，$C_M$为无人机的力矩系数，我们可以认为俯仰和滚转力矩就是电机的螺旋桨产生的，但是偏航的力矩本身不是由电机螺旋桨产生的。偏航力矩本身是由螺旋桨高速旋转与周围空气产生作用，然后空气作用给无人机的反作用力产生的。因此1号电机产生的角动量朝上，但是它在无人机偏航上的贡献是负的，其余三个电机以此类推。 (这个过程类似于坐在可以旋转的办公椅上，用手推门，自己身体会朝反方向转一样。本人没有学过空气动力学，所以最开始误解了。)

$(8)$左边部分，直接按照向量叉乘的规则直接代入就好，没有任何细节和陷阱。综合$(8)-(10)$有
$$\begin{array}{r}\{\begin{array}{rl}& \dot{p}=\frac{1}{J_{xx}}\left[\frac{\sqrt{2}{C}_{T}d}{2}\left({\omega }_1^2-{\omega }_2^2-{\omega }_3^2+{\omega }_4^2\right)+\left(J_{yy}-J_{zz}\right)qr-J_{0}q\Omega \right]\\ & \dot{q}=\frac{1}{J_{yy}}\left[\frac{\sqrt{2}{C}_{T}d}{2}\left(-{\omega }_1^2-{\omega }_2^2+{\omega }_3^2+{\omega }_4^2\right)+\left(J_{zz}-J_{xx}\right)pr+J_{0}p\Omega \right]\\ & \dot{r}=\frac{1}{J_{zz}}\left[C_M\left(-{\omega }_1^2+{\omega }_2^2-{\omega }_3^2+{\omega }_4^2\right)+\left(J_{xx}-J_{yy}\right)pq\right]\end{array}\end{array}$$
其中 $\Omega ={\omega }_1-{\omega }_2+{\omega }_3-{\omega }_4$。

还没完，这里的角速度是机体坐标系下的，我们还需要获得无人机相对于地面系的旋转角速度。按照位置动力学里面旋转矩阵的建立顺序，计算一下从惯性系到机体系的旋转角速度的变换矩阵。

1. 将地面系的Z轴的旋转速度通过$R_{b2}^b(\varphi )$和$R_{b1}^{b2}(\theta )$变换到机体系下
   v 1 = R b 2 b ( ϕ ) ⋅ R b 1 b 2 ( θ ) [ 0 0 ψ ˙ ] \boldsymbol{v_1} = R_{b2}^{b}\left(\phi\right) \cdot R_{b1}^{b2}\left(\theta\right) \left[\begin{matrix} 0\\0\\ \dot{\psi} \end{matrix}\right] v1​=Rb2b​(ϕ)⋅Rb1b2​(θ) ​00ψ˙​​ ​
2. 再将地面系的Y轴的旋转速度通过$R_{b2}^b(\varphi )$变换到机体系下
   v 2 = R b 2 b ( ϕ ) [ 0 θ ˙ 0 ] \boldsymbol{v_2} = R_{b2}^{b}\left(\phi\right)\left[\begin{matrix} 0\\\dot{\theta}\\ 0 \end{matrix}\right] v2​=Rb2b​(ϕ) ​0θ˙0​ ​
3. 此时地面系的X轴的旋转角速度和机体系相等了
   v 3 = [ φ ˙ 0 0 ] \boldsymbol{v_3} = \left[\begin{matrix} \dot{\varphi}\\0\\ 0 \end{matrix}\right] v3​= ​φ˙​00​ ​
   进而有
   [ p q r ] = v 1 + v 2 + v 3 = [ φ ˙ 0 0 ] + R b 2 b ( φ ) [ 0 θ ˙ 0 ] + R b 2 b ( φ ) ⋅ R b 1 b 2 ( θ ) [ 0 0 ψ ˙ ] = [ 1 0 − sin ⁡ θ 0 cos ⁡ φ sin ⁡ φ cos ⁡ θ 0 − sin ⁡ φ cos ⁡ φ cos ⁡ θ ] [ φ ˙ θ ˙ ψ ˙ ] \begin{align} \begin{aligned} \left[\begin{matrix} p\\q\\r \end{matrix}\right] &= \boldsymbol{v_1} + \boldsymbol{v_2} + \boldsymbol{v_3}\\ &=\left[\begin{matrix} \dot{\varphi}\\0\\ 0 \end{matrix}\right]+R_{b2}^{b}\left(\varphi\right)\left[\begin{matrix} 0\\\dot{\theta}\\ 0 \end{matrix}\right]+R_{b2}^{b}\left(\varphi\right) \cdot R_{b1}^{b2}\left(\theta\right) \left[\begin{matrix} 0\\0\\ \dot{\psi} \end{matrix}\right]\\ &=\left[\begin{matrix} 1 & 0 & -\sin{\theta}\\ 0 & \cos{\varphi} & \sin{\varphi}\cos{\theta}\\ 0 & -\sin{\varphi} & \cos{\varphi}\cos{\theta} \end{matrix}\right]\left[\begin{matrix} \dot{\varphi}\\\dot{\theta}\\\dot{\psi} \end{matrix}\right] \end{aligned} \end{align} ​pqr​ ​​=v1​+v2​+v3​= ​φ˙​00​ ​+Rb2b​(φ) ​0θ˙0​ ​+Rb2b​(φ)⋅Rb1b2​(θ) ​00ψ˙​​ ​= ​100​0cosφ−sinφ​−sinθsinφcosθcosφcosθ​ ​ ​φ˙​θ˙ψ˙​​ ​​​​
   整理，有
   [ φ ˙ θ ˙ ψ ˙ ] = [ 1 tan ⁡ θ sin ⁡ φ tan ⁡ θ cos ⁡ φ 0 cos ⁡ φ − sin ⁡ φ 0 sin ⁡ φ / cos ⁡ θ cos ⁡ φ / cos ⁡ θ ] [ p q r ] \begin{align} \begin{aligned} \left[\begin{matrix} \dot{\varphi}\\\dot{\theta}\\\dot{\psi} \end{matrix}\right] = \left[\begin{matrix} 1 & \tan{\theta}\sin{\varphi} & \tan{\theta}\cos{\varphi} \\ 0 & \cos{\varphi} & -\sin{\varphi}\\ 0 & \sin{\varphi}/\cos{\theta} & \cos{\varphi}/\cos{\theta} \end{matrix}\right]\left[\begin{matrix} p\\q\\r \end{matrix}\right] \end{aligned} \end{align} ​φ˙​θ˙ψ˙​​ ​= ​100​tanθsinφcosφsinφ/cosθ​tanθcosφ−sinφcosφ/cosθ​ ​ ​pqr​ ​​​​
   至此，无人机旋转动力学模型建立完成。

综合模型

综合式$(7)$，$(11)$和$(13)$，可以得到无人机整理的位置-姿态动力学模型为：
{ [ x ˙ y ˙ z ˙ ] = [ 1 0 0 0 1 0 0 0 1 ] [ v x v y v z ] [ v ˙ x v ˙ y v ˙ z ] = f m [ c ( ψ ) s ( θ ) c ( φ ) + s ( ψ ) s ( φ ) s ( ψ ) s ( θ ) c ( φ ) − c ( ψ ) s ( φ ) c ( φ ) c ( θ ) ] − [ 0 0 g ] [ p ˙ q ˙ r ˙ ] = J − 1 { [ τ x τ y τ z ] + ( [ J x x 0 0 0 J y y 0 0 0 J z z ] ⋅ [ p q r ] ) × [ p q r ] + [ − J 0 q Ω J 0 p Ω 0 ] } [ φ ˙ θ ˙ ψ ˙ ] = [ 1 tan ⁡ θ sin ⁡ φ tan ⁡ θ cos ⁡ φ 0 cos ⁡ φ − sin ⁡ φ 0 sin ⁡ φ / cos ⁡ θ cos ⁡ φ / cos ⁡ θ ] [ p q r ] \begin{align} \left\{ \begin{aligned} & \left[\begin{matrix}\dot{x}\\\dot{y}\\\dot{z}\end{matrix}\right] = \left[\begin{matrix} 1&0&0\\0&1&0\\0&0&1 \end{matrix}\right] \left[\begin{matrix}v_x\\v_y\\v_z\end{matrix}\right]\\ & \left[\begin{matrix}\dot{v}_x\\\dot{v}_y\\\dot{v}_z\end{matrix}\right] = \frac{f}{m} \left[\begin{matrix} c(\psi)s(\theta)c(\varphi)+s(\psi)s(\varphi)\\ s(\psi)s(\theta)c(\varphi) - c(\psi)s(\varphi)\\ c(\varphi)c(\theta) \end{matrix}\right]-\left[\begin{matrix}0\\0\\g\end{matrix}\right]\\ & \left[\begin{matrix}\dot{p}\\\dot{q}\\\dot{r}\end{matrix}\right]= J^{-1} \left\{ \left[\begin{matrix}\tau_x\\ \tau_y\\ \tau_z\end{matrix}\right]+ \left(\left[\begin{matrix}J_{xx} & 0&0\\0&J_{yy}&0\\0&0&J_{zz}\end{matrix}\right]\cdot \left[\begin{matrix}p\\q\\r\end{matrix}\right]\right)\times \left[\begin{matrix}p\\q\\r\end{matrix}\right]+ \left[\begin{matrix}-J_0q\Omega\\J_0p\Omega\\0\end{matrix}\right] \right\}\\ &\left[\begin{matrix}\dot{\varphi}\\\dot{\theta}\\\dot{\psi}\end{matrix}\right]= \left[\begin{matrix} 1 & \tan{\theta}\sin{\varphi} & \tan{\theta}\cos{\varphi} \\ 0 & \cos{\varphi} & -\sin{\varphi}\\ 0 & \sin{\varphi}/\cos{\theta} & \cos{\varphi}/\cos{\theta} \end{matrix}\right]\left[\begin{matrix} p\\q\\r \end{matrix}\right] \end{aligned} \right. \end{align} ⎩ ⎨ ⎧​​ ​x˙y˙​z˙​ ​= ​100​010​001​ ​ ​vx​vy​vz​​ ​ ​v˙x​v˙y​v˙z​​ ​=mf​ ​c(ψ)s(θ)c(φ)+s(ψ)s(φ)s(ψ)s(θ)c(φ)−c(ψ)s(φ)c(φ)c(θ)​ ​− ​00g​ ​ ​p˙​q˙​r˙​ ​=J−1⎩ ⎨ ⎧​ ​τx​τy​τz​​ ​+ ​ ​Jxx​00​0Jyy​0​00Jzz​​ ​⋅ ​pqr​ ​ ​× ​pqr​ ​+ ​−J0​qΩJ0​pΩ0​ ​⎭ ⎬ ⎫​ ​φ˙​θ˙ψ˙​​ ​= ​100​tanθsinφcosφsinφ/cosθ​tanθcosφ−sinφcosφ/cosθ​ ​ ​pqr​ ​​​​
其中
{ J = [ J x x 0 0 0 J y y 0 0 0 J z z ] Ω = ω 1 − ω 2 + ω 3 − ω 4 [ f τ x τ y τ z ] = [ C T C T C T C T C T d 2 − C T d 2 − C T d 2 C T d 2 − C T d 2 − C T d 2 C T d 2 C T d 2 − C M C M − C M C M ] [ ω 1 2 ω 2 2 ω 3 2 ω 4 2 ] \begin{align} \left\{ \begin{aligned} & J_=\left[\begin{matrix} J_{xx} & 0 & 0 \\ 0 & J_{yy} &0 \\ 0 & 0 & J_{zz} \end{matrix}\right]\\ & \Omega = \omega_1 - \omega_2 + \omega_3 -\omega_4\\ &\left[\begin{matrix} f \\ \tau_x\\\tau_y\\\tau_z \end{matrix}\right]=\left[\begin{matrix} C_T&C_T&C_T&C_T\\ \frac{C_Td}{\sqrt{2}} & -\frac{C_Td}{\sqrt{2}} & -\frac{C_Td}{\sqrt{2}} &\frac{C_Td}{\sqrt{2}}\\ -\frac{C_Td}{\sqrt{2}} & -\frac{C_Td}{\sqrt{2}} & \frac{C_Td}{\sqrt{2}} & \frac{C_Td}{\sqrt{2}}\\ -C_M &C_M&-C_M&C_M \end{matrix}\right]\left[\begin{matrix} \omega_1^2\\\omega_2^2\\\omega_3^2\\\omega_4^2 \end{matrix}\right] \end{aligned} \right. \end{align} ⎩ ⎨ ⎧​​J=​ ​Jxx​00​0Jyy​0​00Jzz​​ ​Ω=ω1​−ω2​+ω3​−ω4​ ​fτx​τy​τz​​ ​= ​CT​2 ​CT​d​−2 ​CT​d​−CM​​CT​−2 ​CT​d​−2 ​CT​d​CM​​CT​−2 ​CT​d​2 ​CT​d​−CM​​CT​2 ​CT​d​2 ​CT​d​CM​​ ​ ​ω12​ω22​ω32​ω42​​ ​​​​