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

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

一、引言

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

二、常用的数值计算方法

2.1 欧拉折线法

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

{\begin{cases} \frac{d y}{d x} = f (x, y (x)) & x \in [a, b] \\ y (x_{0}) = y_{0} \end{cases}

$\begin{cases} \frac{dy}{dx}=f(x,y(x))& x\in [a,b]\\ y(x_0)=y_0 \end{cases}$
如果对

\frac{d y}{d x}

$\frac{dy}{dx}$ 做进行处理，用差分替换，即

y' (x_{n}) = \frac{y_{n + 1} - y_{n}}{h}

$y\prime(x_{n})=\frac{y_{n+1}-y_n}{h}$ ,其中

h = x_{n + 1} - x_{n}

$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}

$\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} + h f (x_{n}, y (x_{n}))

$y_{n+1}=y_n+hf(x_n,y(x_n))$ ,

n = 0, 1, 2, 3 \dots

$n=0,1,2,3\ldots$
这便是欧拉公式，式子很简单，也很直观，通过一系列的折线来近似代替函数，只要给定了初始值

y_{0}

$y_0$ ，就可以一步一步地迭代计算出在结点

x_{1}, x_{2} \dots

$x_1,x_2\ldots$ 处的函数值

y_{1}, y_{2} \dots

$y_1,y_2\ldots$ .

2.2改进欧拉折线法

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

y (x) = y_{0} + \int_{a}^{b} f (x, y (x)) d x

$y(x)=y_0+\int_a^b{f(x,y(x))}\,{dx}$
若已知

x = x_{n}

$x=x_n$ 处的函数值

y (x_{n})

$y(x_n)$ ，那么上面的式子也可以写成下面的形式：

y (x_{n + 1}) = y_{n} + \int_{x_{n}}^{x_{n + 1}} f (x, y (x)) d x

$y(x_{n+1})=y_n+\int_{x_n}^{x_{n+1}}{f(x,y(x))}\,dx$
对上面的等式右边的积分进行处理，用矩形公式计算积分，即

\int_{x_{n}}^{x_{n + 1}} f (x, y (x)) d x = h f (x_{n}, y (x_{n})),

$\int_{x_n}^{x_{n+1}}{f(x,y(x))}\,dx=hf(x_n,y(x_{n})),$ 就可以得到下面的式子：

y_{n + 1} = y_{n} + h f (x_{n}, y_{n}), n = 0, 1, 2, 3 \dots

$y_{n+1}=y_n+hf(x_n,y_n) ,n=0,1,2,3\ldots$
这就是上面提到的欧拉折线法啊，挺有意思的，对同一个问题从不同的角度都得到了相同的结果。从这个式子也可以说明欧拉折线法的精度确实不高，因为它是用矩形公式近似计算积分的，很自然地，为了提高精度，可以用梯形公式计算积分，即

\int_{x_{n}}^{x_{n + 1}} f (x, y (x)) d x = \frac{h}{2} (f (x_{n}, y (x_{n})) + f (x_{n + 1}, y (x_{n + 1}))

$\int_{x_n}^{x_{n+1}}{f(x,y(x))}\,dx=\frac{h}{2}(f(x_n,y(x_n))+f(x_{n+1},y(x_{n+1}))$ 这样就能得到下面的式子：

y_{n + 1} = y_{n} + \frac{h}{2} (f (x_{n}, y_{n}) + f (x_{n} + h, y_{n} + h f (x_{n}, y_{n})), n = 0, 1, 2, 3 \dots

$y_{n+1}=y_n+\frac{h}{2}(f(x_n,y_n)+f(x_n+h,y_n+hf(x_n,y_n)) ,n=0,1,2,3\ldots$
这就是使用梯形公式改进后的欧拉公式了，为了便于计算，又可将上式改写成下面的形式：

{\begin{cases} k_{1} = h f (x_{n}, y_{n}) \\ k_{2} = h f (x_{n} + h, y_{n} + k 1) \\ y_{n + 1} = y_{n} + \frac{1}{2} (k_{1} + k_{2}) \end{cases}

$\begin{cases} k_1=hf(x_n,y_n)\\ k_2=hf(x_n+h,y_n+k1)\\ y_{n+1}=y_n+\frac{1}{2}(k_1+k_2) \end{cases}$
这个形式更容易通过代码实现.

2.3龙格—库塔法

如果对于方程解的精度要求比较高，上诉的两种方法都不能满足要求，则可以考虑使用四阶的龙格—库塔法进行计算。我先记录二阶龙格—库塔法的推导过程，这也是一个有意思的过程，推导之后可以发现，其实与上述的欧拉折线法以及改进的欧拉折线法都是龙格—库塔法的一阶与二阶形式的特例.
假定上述微分方程存在解析解 $y=y(x)$ ，对它进行泰勒级数展开可得到：

y (x_{n + 1}) = y (x_{n} + h) = y (x_{n}) + \frac{y' (x_{n})}{1!} h + \frac{y'' (x_{n})}{2!} h^{2} + \frac{y''' (x_{n})}{3!} h^{3} + \dots + \frac{y^{(k)}}{k!} h^{k} + O (h^{k})

$y(x_{n+1})=y(x_n+h)=y(x_n)+\frac{y\prime(x_n)}{1!}h+\frac{y\prime\prime(x_n)}{2!}h^2+\frac{y\prime\prime\prime(x_n)}{3!}h^3+\ldots+\frac{y^{(k)}}{k!}h^k+O(h^k)$
若令k=1，可得一阶泰勒格式为

y (x_{n + 1}) = y (x_{n}) + h f (x_{n}, y_{n})

$y(x_{n+1})=y(x_n)+hf(x_n,y_n)$ ,该式就是欧拉折线法的公式.随着k取值的增大，泰勒格式的阶数上升，也就越来越逼近真实的函数

y = y (x)

$y=y(x)$ .所以自然想到，可以直接使用函数的泰勒展开式来计算.但是求解函数的高阶导数又不是一件容易的事，因此，这种方法并不实用.但可以间接使用泰勒级数法引导推出龙格—库塔法的计算公式.
首先欧拉公式可以改写成：

{\begin{cases} k_{1} = h f (x_{n}, y_{n}) \\ y_{n + 1} = y_{n} + k_{1} \end{cases}

$\begin{cases} k_1=hf(x_n,y_n)\\ y_{n+1}=y_n+k_1 \end{cases}$
改进的欧拉公式可以写成更一般的形式：

{\begin{cases} k_{1} = h f (x_{n}, y_{n}) \\ k_{2} = h f (x_{n} + a h, y_{n} + b k_{1}) \\ y_{n + 1} = y_{n} + R_{1} k_{1} + R_{2} k_{2} \end{cases}

$\begin{cases} k_1=hf(x_n,y_n)\\ k_2=hf(x_n+ah,y_n+bk_1)\\ y_{n+1}=y_n+R_1k_1+R_2k_2 \end{cases}$
其中，

a, b, R_{1}, R_{2}

$a,b,R_1,R_2$ 都是需要求的待定系数
对

k_{2} = h f (x_{n} + a h, y_{n} + b k_{1})

$k_2=hf(x_n+ah,y_n+bk_1)$ 做泰勒展开，即：

\begin{aligned} (1) & k_{2} & = h f (x_{n} + a h, y_{n} + b k_{1}) \\ (2) & = h [f (x_{n}, y_{n}) + a h f_{x} (x_{n}, y_{n}) + b k_{1} f_{y} (x_{n}, y_{n}) + \dots] \\ (3) & = h f (x_{n}, y_{n}) + a h^{2} f_{x} (x_{n}, y_{n}) + b h^{2} f_{y} (x_{n}, y_{n}) f (x_{n}, y_{n}) + \dots \end{aligned}

$\begin{align} k_2&=hf(x_n+ah,y_n+bk_1)\\ &=h[f(x_n,y_n)+ahf_x(x_n,y_n)+bk_1f_y(x_n,y_n)+\ldots]\\ &=hf(x_n,y_n)+ah^2f_x(x_n,y_n)+bh^2f_y(x_n,y_n)f(x_n,y_n)+\ldots \end{align}$
将

k_{2} 和 k_{2}

$k_2和 k_2$ 的表达式代入

y_{n + 1} = y_{n} + R_{1} k_{1} + R_{2} k_{2}

$y_{n+1}=y_n+R_1k_1+R_2k_2$ 中可以得到下面的式子：

y_{n + 1} = y_{n} + (R_{1} + R_{2}) f (x_{n}, y_{n}) h + [R_{2} a f_{x} (x_{n}, y_{n}) + R_{2} b f_{y} (x_{n}, y_{n})] h^{2} + \dots

$y_{n+1}=y_n+(R_1+R_2)f(x_n,y_n)h+[R_2af_x(x_n,y_n)+R_2bf_y(x_n,y_n)]h^2+\ldots$
与二阶泰勒格式

y (x_{n + 1}) = y (x_{n} + h) = y (x_{n}) + \frac{y' (x_{n})}{1!} h + \frac{y'' (x_{n})}{2!} h^{2} + O (h^{2})

$y(x_{n+1})=y(x_n+h)=y(x_n)+\frac{y\prime(x_n)}{1!}h+\frac{y\prime\prime(x_n)}{2!}h^2+O(h^2)$ 对比可以得到下面的关系：

{\begin{cases} R_{1} + R_{2} & = 1 \\ R_{2} a & = \frac{1}{2} \\ R_{1} b & = \frac{1}{2} \end{cases}

$\begin{cases} R_1+R_2&=1\\ R_2a&=\frac{1}{2}\\ R_1b&=\frac{1}{2} \end{cases}$
上面的方程组有无穷多组解，每组解对应的式子都称为二阶龙格—库塔公式.若取

R_{1} = R_{2} = \frac{1}{2}, a = b = 1

$R_1=R_2=\frac{1}{2},a=b=1$ ,则可以得出改进的的欧拉公式.可以发现改进的欧拉公式就是二阶龙格—库塔公式的特例.因此这几种方法的本质其实是一样的，都是间接应用了泰勒级数，不过发明人看待问题的角度不同.
用同样的方法，可以推出三阶、四阶龙格—库塔公式，其中四阶龙格—库塔公式在较高精度的计算中应用得较多，它的一种常用形式如下：

{\begin{cases} k_{1} & = h f (x_{n}, y_{n}) \\ k_{2} & = h f (x_{n} + \frac{h}{2}, y_{n} + \frac{k_{1}}{2}) \\ k_{3} & = h f (x_{n} + \frac{h}{2}, y_{n} + \frac{k_{2}}{2}) \\ k_{4} & = h f (x_{n} + h, y_{n} + k_{3}) \\ y_{n + 1} & = y_{n} + \frac{1}{6} (k_{1} + 2 k_{2} + 2 k_{3} + k_{4}) \end{cases}

$\begin{cases} k_1&=hf(x_n,y_n)\\ k_2&=hf(x_n+\frac{h}{2},y_n+\frac{k_1}{2})\\ k_3&=hf(x_n+\frac{h}{2},y_n+\frac{k_2}{2})\\ k_4&=hf(x_n+h,y_n+k_3)\\ y_{n+1}&=y_n+\frac{1}{6}(k_1+2k_2+2k_3+k_4) \end{cases}$

2.4 上述数值计算方法的C语言实现及精度对比

上面的计算公式都很容易转化为C语言代码，为了方便测试，我定义一个结构体来存储各个计算方法的相关参数。然后以h=0.1的步长计算下面这个微分方程在区间[0,1]的数值解

{\begin{cases} y' & = y - \frac{2 * x}{y} \\ y (0) & = 1 \end{cases}

$\begin{cases} y\prime&=y-\frac{2*x}{y}\\ y(0)&=1 \end{cases}$
它的解析解为

y = \sqrt{2 * x + 1}

$y=\sqrt{2*x+1}$

#include <stdio.h>
#include <math.h>

#define f(t,y) ((y) - (2.0f*t) / (y))   
#define F(t) sqrt(2*t+1)

typedef struct {
    float a;//区间左端点
    float b;//区间右端点
    float t;//自变量
    float y;//函数值
    float h;//步长
    float k1;
    float k2;
    float k3;
    float k4;
}Calc_t;

int main()
{
    Calc_t Euler =  { 0,1,0,1,0.1,0,0,0,0 };
    Calc_t Euler2 = { 0,1,0,1,0.1,0,0,0,0 };
    Calc_t RK_4 =   { 0,1,0,1,0.1,0,0,0,0 };
    int n = (Euler.b - Euler.a) / Euler.h;
    printf("t\t欧拉折线法\t改进欧拉折线法\t四阶龙格库塔法\t真实值\n");
    printf("%.1f\t%.8f\t%.8f\t%.8f\t%.8f\n", Euler.t, Euler.y, Euler2.y, RK_4.y, F(Euler.t));
    for (int i = 0;i < n;i++) {
        /*欧拉折线法*/
        Euler.k1 = Euler.h*f(Euler.t, Euler.y);
        Euler.y += Euler.k1;
        Euler.t += Euler.h;
        /*改进欧拉折线法*/
        Euler2.k1 = Euler2.h*f(Euler2.t, Euler2.y);
        Euler2.k2 = Euler2.h*f((Euler2.t + Euler2.h), (Euler2.y + Euler2.k1));
        Euler2.y += (Euler2.k1 + Euler2.k2)/2.0f;
        Euler2.t += Euler2.h;
        /*四阶龙格库塔法*/
        RK_4.k1 = RK_4.h*f(RK_4.t, RK_4.y);
        RK_4.k2 = RK_4.h*f((RK_4.t + RK_4.h / 2.0f), (RK_4.y + RK_4.k1 / 2.0f));
        RK_4.k3 = RK_4.h*f((RK_4.t + RK_4.h / 2.0f), (RK_4.y + RK_4.k2 / 2.0f));
        RK_4.k4 = RK_4.h*f((RK_4.t + RK_4.h), (RK_4.y + RK_4.k3));
        RK_4.y += (RK_4.k1 + 2.0f*RK_4.k2 + 2.0f*RK_4.k3 + RK_4.k4) / 6.0f;
        RK_4.t += RK_4.h;
        printf("%.1f\t%.8f\t%.8f\t%.8f\t%.8f\n", Euler.t, Euler.y, Euler2.y, RK_4.y, F(Euler.t));
    }
    getchar();
}

各种方法的计算结果如下图所示，从图中可以看出欧拉折线法精度最差，四阶龙格库塔法的精度相当高了，而改进的欧拉折线法的精度也挺高的。在飞控当中，陀螺仪角速率的更新频率一般能达到100Hz左右，也就是说步长能降到0.01左右，因此，欧拉折线法的精度也是挺高的，要提高姿态解算的精度，感觉使用二阶龙格库塔公式就足够了。
这里写图片描述

参考文献

蔡锁章,杨明,雷英杰.数值计算方法.2版.北京.国防工业出版社.2016