飞行器控制笔记（一）——姿态解算之微分方程数值计算

飞行器控制笔记（一）—— 姿态解算之微分方程数值计算

一、引言

飞行器的姿态解算，简单地说来，就是通过测量机体角速率来计算出机体的姿态角（俯仰角$\theta$，横滚角$\phi$,偏航角$\psi$）。 而角速率与姿态变化率之间有着很直观的联系，所以姿态解算最终是在求解一组包含角速率与姿态的一阶常微分方程。因此，有必要记录一下微分方程的数值解法，便于后面通过代码实现姿态解算。

二、 常用的数值计算方法

2.1 欧拉折线法

欧拉折线法是一种古老简单的求解一阶常微分方程的数值计算方法，尽管它的精度并不高，但是对于飞控这种使用MCU做计算的计算能力比较弱的场合，欧拉折线法计算量小，而且精度也能基本满足要求，所以应用得还是挺多的。不过我感觉如果使用STM32F4并且开启FPU，那么也完全可以使用精度更高的算法来提高解算精度，这样对飞行器的稳定性应该也会有一定的提升。下面还是先记录下欧拉折线法的过程。
一阶微分方程的一般形式如下：

$$\begin{cases} \frac{dy}{dx}=f(x,y(x))& x\in [a,b]\\ y(x_0)=y_0 \end{cases}$$
如果对 $\frac{dy}{dx}$ 做进行处理，用差分替换，即 $y\prime(x_{n})=\frac{y_{n+1}-y_n}{h}$,其中 $h=x_{n+1}-x_n$,是相邻两点之间的距离，也就是步长。这样，原来的微分方程就可以写成下面的形式了：
$$\begin{cases} \frac{y_{n+1}-y_n}{h}=f(x_n,y(x_n))& x\in [a,b]\\ y(x_0)=y_0 \end{cases}$$
这样一来， $y_{n+1}=y_n+hf(x_n,y(x_n))$ , $n=0,1,2,3\ldots$
这便是欧拉公式，式子很简单，也很直观，通过一系列的折线来近似代替函数，只要给定了初始值 $y_0$，就可以一步一步地迭代计算出在结点 $x_1,x_2\ldots$ 处的函数值 $y_1,y_2\ldots$.

2.2改进欧拉折线法

对于一阶微分方程，如果换个方式，直接对等式左右两边积分，可以得到：

$$y(x)=y_0+\int_a^b{f(x,y(x))}\,{dx}$$
若已知 $x=x_n$ 处的函数值 $y(x_n)$ ，那么上面的式子也可以写成下面的形式：

y(xn+1)=yn+∫xn+1