四旋翼无人机建模与实现（二）

本文是对GitHub上的一个开源的四旋翼无人机建模与编程实现的学习笔记，因为原作者的文档使用英文写的，花了很长的时间才看懂，因此，我觉得用自己的语言将这个开源项目分享出来是有意义的，爱因斯坦曾说过：“如果你不能简单地解释某一概念，说明你没有很好地掌握它”。因此便有了写这篇文档的想法。
原文链接： https://github.com/SKYnSPACE/AE450
本文的贡献在于将原作者文档中的公式做了更为详细的推导，并以更为通俗的语言来描述整个建模过程。

7、旋转运动方程

后六个状态量的变化率可以从旋转运动方程中获得，状态变化率表示如下：
[ ϕ ˙ , ψ ˙ , θ ˙ , p ˙ , q ˙ , r ˙ ] \boldsymbol{[\dot{\phi},\dot{\psi},\dot{\theta},\dot{p},\dot{q},\dot{r}]} [ϕ˙,ψ˙,θ˙,p˙,q˙,r˙]
速度方程：
R ˙ = R ⋅ s k ( ω ⃗ ) \boldsymbol{\dot{R}=R \cdot sk(\vec{\omega})} R˙=R⋅sk(ω )
我不是太理解原作者写的这些标识意义，我觉得这样写仿佛更容易理解：
R P Y ˙ = T r ⋅ ω ⃗ \boldsymbol{\dot{RPY}=Tr \cdot \vec{\omega}} RPY˙=Tr⋅ω
其中等号左边是欧拉角的变化率，等号右边时变换矩阵和角速度的乘积，这个公式表达了姿态变化率和角速度的关系。将上述方程展开如下：
[ ϕ ˙ θ ˙ ψ ˙ ] = [ 1 t a n θ s i n ϕ t a n θ c o s ϕ 0 c o s ϕ − s i n ϕ 0 s i n ϕ / c o s θ c o s ϕ / c o s θ ] [ p q r ] \boldsymbol{ \begin{bmatrix} \dot{\phi}\\ \dot{\theta} \\ \dot{\psi} \end{bmatrix}= \begin{bmatrix} 1& tan\theta sin\phi & tan\theta cos\phi\\ 0 & cos\phi & -sin\phi\\ 0 & sin\phi / cos\theta & cos\phi / cos\theta \end{bmatrix} \begin{bmatrix} p\\q\\r \end{bmatrix} } ⎣⎡ϕ˙θ˙ψ˙⎦⎤=⎣⎡100tanθsinϕcosϕsinϕ/cosθtanθcosϕ−sinϕcosϕ/cosθ⎦⎤⎣⎡pqr⎦⎤
姿态变化率和角速度之间满足线性变换关系，该线性变换关系是关于翻滚和俯仰角的线性变换关系，和偏航角无关。
加速度方程（牛顿第二定律）：
I ω ⃗ ˙ = − ω ⃗ × I ω ⃗ + M \boldsymbol{I \dot{\vec{\omega}}=-\vec{\omega} \times I\vec{\omega}+M} Iω ˙=−ω ×Iω +M
将该公式展开后得到：
p ˙ = I y y − I z z I x x ⋅ q r + 1 I x x ⋅ M 1 \boldsymbol{\dot{p}=\frac{I_{yy}-I_{zz}}{I_{xx}} \cdot qr+\frac{1}{I_{xx}}\cdot M_1} p˙=IxxIyy−Izz⋅qr+Ixx1⋅M1
q ˙ = I z z − I x x I y y ⋅ r p + 1 I y y ⋅ M 2 \boldsymbol{\dot{q}=\frac{I_{zz}-I_{xx}}{I_{yy}} \cdot rp+\frac{1}{I_{yy}}\cdot M_2} q˙=IyyIzz−Ixx⋅rp+Iyy1⋅M2
r ˙ = I z z − I y y I z z ⋅ p q + 1 I z z ⋅ M 3 \boldsymbol{\dot{r}=\frac{I_{zz}-I_{yy}}{I_{zz}} \cdot pq+\frac{1}{I_{zz}}\cdot M_3} r˙=IzzIzz−Iyy⋅pq+Izz1⋅M3

此时，已经将十二个状态变量的变化率推导完毕。接下来考虑悬停位置的姿态控制。

8、四旋翼无人机整体模型

非线性的四旋翼状态空间模型可以按照如下表示：
x ⃗ ˙ = f ( x ⃗ , u ⃗ ) \boldsymbol{\dot{\vec{x}}=f(\vec{x},\vec{u})} x ˙=f(x ,u )
其中，
x ⃗ = [ X Y Z X ˙ Y ˙ Z ˙ ϕ θ ψ p q r ] T u ⃗ = [ u 1 , u 2 , u 3 , u 4 ] T = [ T ∑ , M 1 , M 2 , M 3 ] T \boldsymbol{\vec{x}=\begin{bmatrix} X&Y& Z& \dot{X}& \dot{Y}& \dot{Z}& \phi & \theta & \psi & p& q& r \end{bmatrix}^T} \\ \boldsymbol{\vec{u}=[u_1 ,u_2 ,u_3 ,u_4]^T=[T_{\sum},M_1,M_2,M_3]^T} x =[XYZX˙Y˙Z˙ϕθψpqr]Tu =[u1,u2,u3,u4]T=[T∑,M1,M2,M3]T

都目前为止，完成了整个被控对象的模型建立，整体模型如下：
X ˙ = X ˙ Y ˙ = Y ˙ Z ˙ = Z ˙ X ¨ = − 1 m ( s i n ψ s i n ϕ + c o s ψ s i n θ c o s ϕ ) ⋅ T ∑ Y ¨ = − 1 m ( − c o s ψ s i n ϕ + s i n ψ s i n θ c o s ϕ ) ⋅ T ∑ Z ¨ = − 1 m c o s θ c o s ϕ ⋅ T ∑ ϕ ˙ = p + s i n ϕ t a n θ ⋅ q + c o s ϕ t a n θ ⋅ r ψ ˙ = s i n ϕ / c o s θ ⋅ q + c o s ϕ / c o s θ ⋅ r p ˙ = I y y − I z z I x x ⋅ q r + 1 I x x ⋅ M 1 q ˙ = I z z − I x x I y y ⋅ r p + 1 I y y ⋅ M 2 r ˙ = I z z − I y y I z z ⋅ p q + 1 I z z ⋅ M 3 \dot{X}=\dot{X}\\ \dot{Y}=\dot{Y}\\ \dot{Z}=\dot{Z}\\ \boldsymbol{\ddot{X}=-\frac{1}{m}(sin\psi sin\phi+cos\psi sin\theta cos\phi)\cdot T_{\sum}}\\ \boldsymbol{\ddot{Y}=-\frac{1}{m}(-cos\psi sin\phi+sin\psi sin\theta cos\phi)\cdot T_{\sum}}\\ \boldsymbol{\ddot{Z}=-\frac{1}{m}cos\theta cos\phi \cdot T_{\sum}}\\ \boldsymbol{\dot{\phi}=p+sin\phi tan\theta \cdot q+cos\phi tan\theta \cdot r}\\ \boldsymbol{\dot{\psi}=sin\phi /cos\theta \cdot q +cos\phi/cos\theta \cdot r}\\ \boldsymbol{\dot{p}=\frac{I_{yy}-I_{zz}}{I_{xx}} \cdot qr+\frac{1}{I_{xx}}\cdot M_1}\\ \boldsymbol{\dot{q}=\frac{I_{zz}-I_{xx}}{I_{yy}} \cdot rp+\frac{1}{I_{yy}}\cdot M_2}\\ \boldsymbol{\dot{r}=\frac{I_{zz}-I_{yy}}{I_{zz}} \cdot pq+\frac{1}{I_{zz}}\cdot M_3} X˙=X˙Y˙=Y˙Z˙=Z˙X¨=−m1(sinψsinϕ+cosψsinθcosϕ)⋅T∑Y¨=−m1(−cosψsinϕ+sinψsinθcosϕ)⋅T∑Z¨=−m1cosθcosϕ⋅T∑ϕ˙=p+sinϕtanθ⋅q+cosϕtanθ⋅rψ˙=sinϕ/cosθ⋅q+cosϕ/cosθ⋅rp˙=IxxIyy−Izz⋅qr+Ixx1⋅M1q˙=IyyIzz−Ixx⋅rp+Iyy1⋅M2r˙=IzzIzz−Iyy⋅pq+Izz1⋅M3

9、在悬停位置对无人机模型进行线性化

在悬停位置处，有以下特点：悬停位置的偏航角不为零，其余状态量都为零，输入量只有重力，表示如下：
x ⃗ e = [ 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , ψ e , 0 , 0 , 0 ] T u ⃗ e = [ m g , 0 , 0 , 0 ] T \boldsymbol{\vec{x}_e=[0,0,0,0,0,0,0,0,\psi_e,0,0,0]^T \qquad \vec{u}_e=[mg,0,0,0]^T} x e=[0,0,0,0,0,0,0,0,ψe,0,0,0]Tu e=[mg,0,0,0]T
线性化后的方程如下：
x ′ ⃗ ˙ = A x ′ ⃗ + B u ′ ⃗ \boldsymbol{\dot{\vec{x^{'}}}=A\vec{x^{'}}+B\vec{u^{'}}} x′ ˙=Ax′ +Bu′
其中，
A = ∂ f ( x ⃗ , u ⃗ ) ∂ x ⃗ x ⃗ = x ⃗ e u ⃗ = u ⃗ e \boldsymbol{ A=\frac{\partial f(\vec{x},\vec{u})}{\partial \vec{x}}_{ \begin{matrix} \vec{x}=\vec{x}_e\\\vec{u}=\vec{u}_e \end{matrix}}} A=∂x ∂f(x ,u )x =x eu =u e

B = ∂ f ( x ⃗ , u ⃗ ) ∂ u ⃗ x ⃗ = x ⃗ e u ⃗ = u ⃗ e \boldsymbol{ B=\frac{\partial f(\vec{x},\vec{u})}{\partial \vec{u}}_{ \begin{matrix} \vec{x}=\vec{x}_e\\\vec{u}=\vec{u}_e \end{matrix}}} B=∂u ∂f(x ,u )x =x eu =u e
x ′ ⃗ = x ⃗ − x ⃗ e 状态偏差 u ′ ⃗ = u ⃗ − u ⃗ e 输入偏差 \boldsymbol{\vec{x^{'}}=\vec{x}-\vec{x}_e} \qquad 状态偏差\\ \boldsymbol{\vec{u^{'}}=\vec{u}-\vec{u}_e} \qquad 输入偏差 x′ =x −x e状态偏差u′ =u −u e输入偏差
使用泰勒展开式，在悬停位置进行线性化：
x ⃗ ˙ = f ( x ⃗ e , u ⃗ e ) + ∂ f ( x ⃗ , u ⃗ ) ∂ x ⃗ x ⃗ = x ⃗ e u ⃗ = u ⃗ e ( x ⃗ − x ⃗ e ) + ∂ f ( x ⃗ , u ⃗ ) ∂ u ⃗ x ⃗ = x ⃗ e u ⃗ = u ⃗ e ( x ⃗ − x ⃗ e ) + o ( x ⃗ − x ⃗ e ⃗ ) = x ⃗ ˙ e + ∂ f ( x ⃗ , u ⃗ ) ∂ x ⃗ x ⃗ = x ⃗ e u ⃗ = u ⃗ e ( x ⃗ − x ⃗ e ) + ∂ f ( x ⃗ , u ⃗ ) ∂ u ⃗ x ⃗ = x ⃗ e u ⃗ = u ⃗ e ( x ⃗ − x ⃗ e ) + o ( x ⃗ − x ⃗ e ⃗ ) \boldsymbol{\dot{\vec{x}}=f(\vec{x}_e,\vec{u}_e)+\frac{\partial f(\vec{x},\vec{u})}{\partial \vec{x}}_{ \begin{matrix} \vec{x}=\vec{x}_e\\\vec{u}=\vec{u}_e \end{matrix}}(\vec{x}-\vec{x}_e)+ \frac{\partial f(\vec{x},\vec{u})}{\partial \vec{u}}_{ \begin{matrix} \vec{x}=\vec{x}_e\\\vec{u}=\vec{u}_e \end{matrix}}(\vec{x}-\vec{x}_e)+o(\vec{\vec{x}-\vec{x}_e}) }\\ \boldsymbol{=\dot{\vec{x}}_e+\frac{\partial f(\vec{x},\vec{u})}{\partial \vec{x}}_{ \begin{matrix} \vec{x}=\vec{x}_e\\\vec{u}=\vec{u}_e \end{matrix}}(\vec{x}-\vec{x}_e)+ \frac{\partial f(\vec{x},\vec{u})}{\partial \vec{u}}_{ \begin{matrix} \vec{x}=\vec{x}_e\\\vec{u}=\vec{u}_e \end{matrix}}(\vec{x}-\vec{x}_e)+o(\vec{\vec{x}-\vec{x}_e}) } x ˙=f(x e,u e)+∂x ∂f(x ,u )x =x eu =u e(x −x e)+∂u ∂f(x ,u )x =x eu =u e(x −x e)+o(x −x e )=x ˙e+∂x ∂f(x ,u )x =x eu =u e(x −x e)+∂u ∂f(x ,u )x =x eu =u e(x −x e)+o(x −x e )
将上述泰勒展开式忽略高阶无穷小量之后，得到如下状态空间模型:
x ′ ⃗ ˙ = A x ′ ⃗ + B u ′ ⃗ \boldsymbol{\dot{\vec{x^{'}}}=A\vec{x^{'}}+B\vec{u^{'}}} x′ ˙=Ax′ +Bu′

笔者在用自己的语言表达的过程中，难免存在诸多理解错误或者笔误，如果各位朋友在阅读中发现，敬请在评论区批评指正，一起交流！