动力学模型的建立

模型假设

1. 四旋翼为刚体，不考虑变形
2. 四旋翼质量不变，质量分布不变（严格对称），重力加速度不变
3. 认为地面坐标系不变，为惯性坐标系
4. 忽略空气阻力
5. 无人机只做小角度姿态变化
6. X型布局，1，3电机逆时针转；2，4电机顺时针转，如图：
   

变量声明

• 外部输入变量$U=\left[\begin{array}{c}U1\\ U2\\ U3\\ U4\end{array}\right]$
  U1表示对地坐标系下四个螺旋桨的升力和，即:
  $$U1=\sum _{i=0}^4{F}_i$$
  再进行坐标系转化。
  U2表示对地坐标系下俯仰力矩，即：
  $$U2=-F1-F2+F3+F4$$
  再进行坐标系转化。
  U3表示对地坐标系下滚转力矩，即：
  $$U3=F1-F2-F3+F4$$
  再进行坐标系转化。
  U4表示对地坐标系下偏航力矩，即：
  $$U4=F1-F2+F3-F4$$
  再进行坐标系转化。
• 状态变量$X=\left[\begin{array}{c}x\\ y\\ z\\ \theta \\ \varphi \\ \psi \end{array}\right]$
• 输出变量$Y=\left[\begin{array}{c}x\\ y\\ z\\ \theta \\ \varphi \\ \psi \end{array}\right]$

牛顿方程

对力的分析，采用牛顿方程

机体坐标系分析

对于机体坐标系，沿着$z^b$轴，无人机受到四个电机升力的合力U1，$F_B=\left[\begin{array}{c}0\\ 0\\ U1\end{array}\right]$

对地坐标系转换

$$F_E=C_B^E\times F_B=\left[\begin{array}{ccc}cos\theta cos\psi & sin\theta sin\varphi cos\psi -cos\varphi sin\psi & sin\theta cos\varphi cos\psi +sin\varphi sin\psi \\ cos\theta sin\psi & sin\theta sin\varphi sin\psi +cos\varphi cos\psi & sin\theta cos\varphi sin\psi -sin\varphi cos\psi \\ -sin\theta & sin\varphi cos\theta & cos\varphi cos\theta \end{array}\right]\times \left[\begin{array}{c}0\\ 0\\ U1\end{array}\right]=\left[\begin{array}{c}U1\cdot (sin\theta cos\varphi cos\psi +sin\varphi sin\psi )\\ U1\cdot (sin\theta cos\varphi sin\psi -sin\varphi cos\psi )\\ U1\cdot cos\varphi cos\theta \end{array}\right]$$
由牛顿方程：$F=\frac{d(mv)}{dt}$,即力等于动量的一阶微分，得：
$$\left[\begin{array}{c}\ddot{x}\\ \ddot{y}\\ \ddot{z}\end{array}\right]=\left[\begin{array}{c}\frac{U1\cdot (sin\theta cos\varphi cos\psi +sin\varphi sin\psi )}{m}\\ \frac{U1\cdot (sin\theta cos\varphi sin\psi -sin\varphi cos\psi ))}{m}\\ \frac{U1\cdot cos\varphi cos\theta -mg}{m}\end{array}\right]$$

欧拉方程

由欧拉方程：$M=\frac{dL}{dt}=J\cdot \dot{\omega }+\omega \times J\cdot \omega$得：

$$M=\left[\begin{array}{ccc}{J}_{xx}& 0& 0\\ 0& J_{yy}& 0\\ 0& 0& J_{zz}\end{array}\right]\cdot \left[\begin{array}{c}\ddot{\varphi }\\ \ddot{\theta }\\ \ddot{\psi }\end{array}\right]+\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]\times \left[\begin{array}{ccc}{J}_{xx}& 0& 0\\ 0& J_{yy}& 0\\ 0& 0& J_{zz}\end{array}\right]\cdot \left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]=$$

线性化

状态空间方程

传递函数