【飞控】从零开始建模 (二)-姿态更新

0.如何使用模型

上篇从零开始建模(一)-牛顿欧拉方程我们建立的飞行器的模型。
得到了飞行器的模型，该如何使用呢？这个方程的输入是什么？之前说过输入是力，输出是加速度，那这个方程里还有其他变量不知道，有输入得不到输出呀。

$$\begin{aligned} & m\ddot{x} = (\cos\phi\sin\theta\cos\psi+\sin\theta\sin\psi)u_1 \\ & m\ddot{y} = (\cos\phi\sin\theta\sin\psi-\sin\theta\cos\psi)u_1 \\ & m\ddot{z} = \cos\phi\cos\theta*u_1-mg \\ & J_{xx}\ddot{\phi} = \dot{\theta}\dot{\psi}(J_{yy}-J_{zz})+L(f_1-f_3) \\ & J_{yy}\ddot{\theta} = \dot{\phi}\dot{\psi}(J_{zz}-J_{xx})+L(f_2-f_4) \\ & J_{zz}\ddot{\psi} = \dot{\theta}\dot{\phi}(J_{xx}-J_{yy})+b(f_1-f_2+f_3-f_4) \end{aligned}$$

我们来回忆一下小车的速度是如何更新的。

$$v=v_0+at$$

在我们更新速度的时候也是需要根据上一时刻速度信息的。
所以模型的更新，也是一个迭代过程。

如何使用这个模型呢？你得有个初始状态，然后根据当前的受力，就可以得到下一时刻的加速度，和角速度。

1.状态更新

我们在控制飞行器时候，最需要的状态并不是加速度，我们想要的是飞行器的状态，什么是状态速度，位置，角速度，和角度等等都是它的状态，其中最关键的状态就是角度，也就是我们说的欧拉角(一种姿态的描述方法)。

跟小车一样，加速度积分得到速度，速度积分得到位移。
那么同理，角加速度积分得到角速度，角速度积分得到角度。

结束了，姿态更新不过就是几个积分过程。

其实还真可以就到这里结束，你用这种方式得到状态就可以着手设计你的控制器了，那么下期为大家带来简单的控制器思路，敬请期待。

好了好了不皮，上面的思路确实是可以的，用于快速验证思路完全没问题的。

重新思考一下，状态更新的目的是什么，状态更新的目的是通过角速度得到姿态，姿态是什么？是旋转，旋转可以如何表示？欧拉角，四元数，轴角，旋转矩阵。
我们按照这个目标，从模型的输出角速度一步一步分析。

2.角加速度的积分是什么

在模型中我们求出的角加速度，是根据每个轴的力矩分析得到的，是相互独立的，所以它的积分得到的就是每个轴独立的角速度w。

$$w_x=\smallint\ddot\phi$$

3.姿态变化率和机体角速度

我们通常认为

$$\left[\begin{matrix} w_x \\w_y \\w_z \end{matrix}\right]=\left[\begin{matrix} \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \end{matrix}\right]$$

所以

$$\begin{matrix} \smallint{w_x}=\phi\\ \smallint{w_y}=\theta\\ \smallint{w_z}=\psi\\ \end{matrix}$$

但是实际上是这样的，欧拉角的微分方程是这样的，已知角速度求角度。

$$\left[\begin{matrix} w_x \\w_y \\w_z \end{matrix}\right]=\left[\begin{matrix} 1 & 0 & -\sin\theta\\ 0 & \cos\phi & \sin\phi\cos\theta \\ 0 & -\sin\phi & \cos\phi\cos\theta \\ \end{matrix}\right]\left[\begin{matrix} \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \end{matrix}\right]$$

欧拉角是一个跟旋转过程有关的三个角，并不是完全独立的，是非正交的，所以三个角之间是有耦合的。而通过力矩分析得到的角速度则是完全独立的，正交的。(具体推导过程我会放在资料里)

欧拉发现，从任何一个参考系，经过三种特定的转动，可以转到三维空间内任何参考系。这三种特定转动的角度就是欧拉角。——维基百科

根据这个方程，只有在三个角度非常小的时候，才有：

$$\left[\begin{matrix} w_x \\w_y \\w_z \end{matrix}\right]=\left[\begin{matrix} \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \end{matrix}\right]$$

这就是为什么我们建模的时候会强调一句，假设角度非常小。只有在角度非常小的时候，角速度的积分才会等于欧拉角。

所以如何通过角速度得到欧拉角呢？如果你不想假设角度非常小，解这个方程就行了。

但是要解这方程可可太难了，系数是个变化的量而且角度之间有耦合，现在是不是可以直观理解到为什么书上说欧拉角不好用，非线性，耦合，在方程中一目了然。这个方程解不了怎么办呢？(数学重要不!不好好学，给你个方程你都解不了)

4.四元数微分方程

别急，我们想要的是姿态，姿态除了欧拉角还有四元数形式呢。

$$\left[\begin{matrix} \dot{q_0} \\ \dot{q_1} \\ \dot{q_2}\\ \dot{q_3} \end{matrix}\right]=\frac{1}{2} \left[\begin{matrix} 0 & -w_x & -w_y & -w_z \\ w_x & 0 & w_z & -w_y \\ w_y & -w_z & 0 & w_x \\ w_z & w_y & -w_x & 0 \\ \end{matrix} \right] \left[\begin{matrix} q_0 \\ q_1 \\ q_2 \\q_3 \end{matrix}\right]$$

这个就是四元数微分方程

这个方程，系数是已知数的微分方程在数学上有很多方法可以，我们经常听见的毕卡法，龙格库塔法，都是解微分方程的方法，其实我们通常用积分的方法解微分方程在数学上叫矩形法，可以查关键词“数值分析”里面有介绍如何通过离散的方法解微分方程。(这些方程的解法具体过程在《惯性导航》中有非常详细的过程，我会放在资料中)

现在你是不是更加理解为什么欧拉角这么直观，这么好看，还有有其他表示姿态的方法呢？因为欧拉角不好用呀，方程不好解呀！

5.总结

1.模型使用时是个迭代过程。
2.姿态的更新的目的是通过角速度得到姿态
3.为了得到姿态，我们需要解微分方程。
4.姿态有很多种形式可以表示

这个姿态更新过程为什么要做其实都是为了之后控制做准备，知识点其实比较多，关键词我都给出来了，感兴趣的可以自己继续深入查资料哦。虽然知识点越来越多，但是随着了解的深入,你会感受到更多的魅力，而且我会持续更新，一起学习吧。

我还想请认真看的同学思考几个问题，为什么我们一定要求姿态呢？三个独立的角速度直接积分得到的是三个独立的角，用这个角不行吗？

欢迎加我的个人微信交流,听说加我的人不用建模也有对象哦^_^

关注微信公众号【zinghd的思考】，回复【姿态】为您提供本文的全部参考资料。