[常微分方程的数值解法系列五] 龙格-库塔(RK4)法

在惯性导航以及VIO等实际问题中利用IMU求解位姿需要对IMU测量值进行积分得到需要的位置和姿态，其中主要就是求解微分方程。但之前求解微分方程的解析方法主要是应用于一些简单和特殊的微分方程求解中，对于一般形式的微分方程，一般很难用解析方法求出精确解，只能用数值方法求解。该系列主要介绍一些常用的常微分方程的数值解法，主要包括：

• [常微分方程的数值解法系列一] 常微分方程
• [常微分方程的数值解法系列二] 欧拉法
• [常微分方程的数值解法系列三] 改进欧拉法(预估校正法)
• [常微分方程的数值解法系列四] 中值法
• [常微分方程的数值解法系列五] 龙格-库塔(RK4)法

这个系列后面文章会用到前面文章的理论和技术，所以建议按照顺序查看。

简介

在之前常微分方程的数值解法系列中，已经介绍了欧拉法，改进欧拉法以及中值法等多种常微分方程的数值解法。但是之前讲解的方法的局部截断误差相对来说比较大，精度有限，在某些情况下需要更高精度的方法。
本来主要介绍的龙格-库塔法，可以提供更高精度的常微分方程的数值解法。而且龙格-库塔法是常微分方程的数值解法的基本理论，后面讲解的过程中会发现，之前讲解的多种方法都可以归类到龙格-库塔法的体系中。而且不考虑计算量的情况下，理论上是可以构造任意高阶的龙格-库塔公式，这里阶数可以认为和 [常微分方程的数值解法系列一] 常微分方程介绍的截断误差的阶数概念是一致的，阶数越高代表截断误差越小，精度越高。但比较常用的是四阶龙格-库塔公式，后面会给出具体详细的讲解。

基本思想

由微分中值定理可知，存在点$\xi$使得
$$\frac{\mathbf{x}(t_{i+1})-\mathbf{x}(t_i)}{\Delta t}={\mathbf{x}}^{\prime }(\xi ),\xi \in (t_i,t_{i+1})$$
即
$$\mathbf{x}(t_{i+1})=\mathbf{x}(t_i)+\Delta t{\mathbf{x}}^{\prime }(\xi )=\mathbf{x}(t_i)+\Delta t\cdot f(\xi ,\mathbf{x}(\xi ))=\mathbf{x}(t_i)+\Delta t\bar{k},\text{\hspace{1em}(1)}$$
其中$\bar{k}=f(\xi ,\mathbf{x}(\xi ))$称为$\mathbf{x}(t)$在$[t_i,t_{i+1}]$上的平均斜率。所以基于这点考虑，只要对平均斜率$\bar{k}$提供一种近似算法，那么就可以利用$x_i$去递推求解$x_{i+1}$，这就是龙格-库塔方法的基本思想。
当然不同的近似算法求解精度不同，像之前讲解的向前欧拉法、改进欧拉法和中值法利用龙格-库塔方法思想解释就是分别利用$t_i$点斜率、$t_i$点和$t_{i+1}$点斜率平均值和$t_i$点和$t_{i+1}$点间中点斜率来近似$\bar{k}$。

具体方法

一阶

以$\mathbf{x}(t)$在$t_i$点斜率作为$\mathbf{x}(t)$在$[t_i,t_{i+1}]$上的平均斜率$\bar{k}$，并令$x_i\approx \mathbf{x}(t_i)$，可得
$$\bar{k}={\mathbf{x}}^{\prime }(t_i)=f(t_i,\mathbf{x}(t_i))\approx f(t_i,x_i)$$
所以
$$x_{i+1}=x_i+\Delta t\cdot f(t_i,x_i),\text{\hspace{1em}(2)}$$
这就是[常微分方程的数值解法系列二] 欧拉法文中讲解的向前欧拉法，也叫作一阶龙格-库塔方法。公式$(2)$就是一阶龙格-库塔公式。

$x_i$是递推过程中$\mathbf{x}(t_i)$的近似值，后面都用$x_n$表示$\mathbf{x}(t_n)$。

二阶

在$[t_i,t_{i+1}]$上取两点$t_i,t_{i+p}(0<p\le 1)$的斜率值分别为$k_1,k_2$，把它们的线性组合${\lambda }_1{k}_1+{\lambda }_2{k}_2$作为$\bar{k}$的近似值，其中${\lambda }_1,{\lambda }_2,p$都是待确定常数，所以根据公式$(1)$可得
$$x_{i+1}=x_i+\Delta t({\lambda }_1{k}_1+{\lambda }_2{k}_2)$$
其中$k_1=f(t_i,x_i)$，$k_2$是$(t_i,t_{i+1}]$内任意一点$t_{i+p}=t_i+p\Delta t(0<p\le 1)$的斜率$k_2=f(t_{i+p},x_{i+p})$。
但这里在求解过程中$x_{i+p}$是未知量，所以需要先求解出，可以仿照改进欧拉公式的思想，得
$$x_{i+p}=x_i+p\Delta tf(t_i,x_i)=x_i+p\Delta t{k}_1$$
所以整理得
$$\{\begin{array}{l}{x}_{i+1}=x_i+\Delta t({\lambda }_1{k}_1+{\lambda }_2{k}_2)\\ k_1=f(t_i,x_i)\\ k_2=f(t_i+p\Delta t,x_i+p\Delta t{k}_1)\end{array}\text{\hspace{1em}(3)}$$
其中公式$(3)$有三个待确认参数${\lambda }_1,{\lambda }_2,p$，利用泰勒展开选择合适的参数使上诉公式具有二阶精度，即局部截断误差为$\mathbf{O}(\Delta t^3)$，满足这类条件的公式$(3)$就是二阶龙格-库塔公式。

求解参数

下面利用泰勒展开求解待确认参数${\lambda }_1,{\lambda }_2,p$，使公式$(3)$的局部截断误差为$\mathbf{O}(\Delta t^3)$。
分别对公式$3$的$k_1,k_2$作泰勒展开为
$$\begin{array}{rl}{k}_1& =f(t_i,x_i)={\mathbf{x}}^{\prime }(t_i)\\ k_2& =f(t_i+p\Delta t,x_i+p\Delta t{k}_1)=f(t_i+p\Delta t,x_i+p\Delta t\cdot f(t_i,x_i))\\ & =f(t_i,x_i)+p\Delta t{f}_t^{\prime }(t_i,x_i)+(p\Delta t\cdot f(t_i,x_i))\cdot f_x^{\prime }(t_i,x_i)+\mathbf{O}(\Delta t^2)\\ & ={\mathbf{x}}^{\prime }(t_i)+p\Delta t{\mathbf{x}}^{\prime \prime }(t_i)+\mathbf{O}(\Delta t^2)\end{array}$$
所以可得
$$\begin{array}{rl}{x}_{i+1}& =x_i+\Delta t({\lambda }_1{k}_1+{\lambda }_2{k}_2)\\ & =x_i+\Delta t({\lambda }_1{\mathbf{x}}^{\prime }(t_i)+{\lambda }_2({\mathbf{x}}^{\prime }(t_i)+p\Delta t{\mathbf{x}}^{\prime \prime }(t_i)+\mathbf{O}(\Delta t^2)))\\ & =x_i+\Delta t({\lambda }_1+{\lambda }_2){\mathbf{x}}^{\prime }(t_i)+{\lambda }_{2}p\Delta t^2{\mathbf{x}}^{\prime \prime }(t_i)+\mathbf{O}(\Delta t^3)\end{array},\text{\hspace{1em}(4)}$$
又$\mathbf{x}(t_{i+1})$的二阶泰勒展开为
$$\mathbf{x}(t_{i+1})=\mathbf{x}(t_i+\Delta t)=\mathbf{x}(t_i)+\Delta t{\mathbf{x}}^{\prime }(t_i)+\frac{\Delta t^2}{2!}{\mathbf{x}}^{\prime \prime }(t_i)+\mathbf{O}(\Delta t^3),\text{\hspace{1em}(5)}$$
比较公式$(4)$和公式$(5)$，如果要使公式$(3)$的局部截断误差满足$x_{i+1}-\mathbf{x}(t_{i+1})=\mathbf{O}(\Delta t^3)$，即要求公式具有二阶精度，需要$\mathbf{O}(\Delta t^3)$前面的项相等，即需要满足
$$\{\begin{array}{l}{\lambda }_1+{\lambda }_2=1\\ {\lambda }_{2}p=\frac{1}{2}\end{array},\text{\hspace{1em}(6)}$$
满足上诉条件的公式$(3)$统称为二阶龙格-库塔公式。

特殊二阶

如果当$p=1,{\lambda }_1=\frac{1}{2},{\lambda }_2=\frac{1}{2}$，二阶龙格-库塔公式就成了改进欧拉公式，具体详解见[常微分方程的数值解法系列三] 改进欧拉法(预估校正法)。简单来说改进欧拉公式就是以$\mathbf{x}$函数在$t_i$点和$t_{i+1}$点处的斜率$k_1$和$k_2$的算数平均值作为$\mathbf{x}$在$[t_i,t_{i+1}]$上的平均斜率$\bar{k}$。
如果当$p=\frac{1}{2},{\lambda }_1=0,{\lambda }_2=1$，二阶龙格-库塔公式就成了变形欧拉公式，具体详解见[常微分方程的数值解法系列四] 中值法。简单来说变形欧拉公式或者说是中值法就是以$\mathbf{x}$函数在$t_i$点和$t_{i+1}$中点处的斜率$k$作为$\mathbf{x}$在$[t_i,t_{i+1}]$上的平均斜率$\bar{k}$。

三阶

在$[t_i,t_{i+1}]$上取三点$t_i,t_{i+p},t_{i+q}(0<p\le q\le 1)$的斜率值分别为$k_1,k_2,k_3$，把它们的线性组合${\lambda }_1{k}_1+{\lambda }_2{k}_2+{\lambda }_3{k}_3$作为$\bar{k}$的近似值，所以根据公式$(1)$可得
$$x_{i+1}=x_i+\Delta t({\lambda }_1{k}_1+{\lambda }_2{k}_2+{\lambda }_3{k}_3)$$
其中
$$\{\begin{array}{l}{k}_1=f(t_i,x_i)\\ k_2=f(t_i+p\Delta t,x_i+p\Delta t{k}_1)\end{array}$$
为了求$x_{i+q}$点的斜率$k_3$，最直接方法就是利用改进欧拉法进行预报得
$$k_3=f(t_{i+q},{\hat{x}}_{i+q})=f(t_i+q\Delta t,x_i+q\Delta t{k}_1)$$
但是这样做效率比较差，因为在区间$[t_i,t_{i+q}]$区间内已经有$k_1,k_2$两个斜率可以使用了，所以可以把$k_1,k_2$的线性组合当做$[x_i,x_{i+q}]$上平均斜率的近似值，这肯定比上面求${\hat{x}}_{i+q}$精度要好。由此，可得
$${\hat{x}}_{i+q}=x_i+q\Delta t({\mu }_1{k}_1+{\mu }_2{k}_2)$$
所以
$$k_3=f(t_{i+q},{\hat{x}}_{i+q})=f(t_i+q\Delta t,x_i+q\Delta t({\mu }_1{k}_1+{\mu }_2{k}_2))$$
所以整理得
$$\{\begin{array}{l}{x}_{i+1}=x_i+\Delta t({\lambda }_1{k}_1+{\lambda }_2{k}_2+{\lambda }_3{k}_3)\\ k_1=f(t_i,x_i)\\ k_2=f(t_i+p\Delta t,x_i+p\Delta t{k}_1)\\ k_3=f(t_i+q\Delta t,x_i+q\Delta t({\mu }_1{k}_1+{\mu }_2{k}_2))\end{array},\text{\hspace{1em}(7)}$$
其中公式$(7)$有七个待确认参数${\lambda }_1,{\lambda }_2,{\lambda }_3,{\mu }_1,{\mu }_2,p,q$，类似前面的推导利用泰勒展开选择合适的参数使上诉公式具有三阶精度，即局部截断误差为$\mathbf{O}(\Delta t^4)$，满足这类条件的公式$(7)$就是三阶龙格-库塔公式。
推导过程和二阶类似，再此省略，只给出比较常用的一致形式
$$\{\begin{array}{l}{x}_{i+1}=x_i+\frac{1}{6}\Delta t(k_1+4k_2+k_3)\\ k_1=f(t_i,x_i)\\ k_2=f(t_i+\frac{1}{2}\Delta t,x_i+\frac{1}{2}\Delta t\cdot k_1)\\ k_3=f(t_i+\Delta t,x_i-\Delta t\cdot k_1+2\Delta t\cdot k_2))\end{array},\text{\hspace{1em}(7)}$$

高阶

为了进一步提高精度，在$[t_i,t_{i+1}]$上可以选取更多点的，利用改进欧拉法中预报法，预报这些点的斜率值(之前在三阶也讨论过，为了预报更准确，利用前面所有点的斜率来预报后面点的估计值进一步求得斜率，而不仅仅只利用第一个点)，然后对这些斜率值加权平均作为作为$\mathbf{x}$在$[t_i,t_{i+1}]$上的平均斜率$\bar{k}$。然后在利用泰勒展开，对比相应的系数，从而确定在满足特定$n$阶精度下所有未知参数需要满足的条件。
前面也提到过，不考虑计算量的情况下，理论上是可以构造任意高阶的龙格-库塔公式。但在实际中发现，高于四阶的龙格-库塔公式，不但计算量非常大，而且精度进一步提升的有限。所以在实际使用中，四阶的龙格-库塔公式是精度和计算量都比较理想的公式。
推导过程和二阶类似，再此省略，只给出最经典的四阶的龙格-库塔公式：
$$\{\begin{array}{l}{x}_{i+1}=x_i+\Delta t(\frac{1}{6}{k}_1+\frac{1}{3}{k}_2+\frac{1}{3}{k}_3+\frac{1}{6}{k}_4)=\frac{1}{6}\Delta t(k_1+2k_2+2k_3+k_4)\\ k_1=f(t_i,x_i)\\ k_2=f(t_i+\frac{1}{2}\Delta t,x_i+\frac{1}{2}\Delta t\cdot k_1)\\ k_3=f(t_i+\frac{1}{2}\Delta t,x_i+\frac{1}{2}\Delta t\cdot k_2)\\ k_4=f(t_i+\Delta t,x_i+\Delta t\cdot k_3)\end{array},\text{\hspace{1em}(8)}$$

步长选择

之前讨论的所有的龙格-库塔方法都是以$\Delta t$定步长来展开的，但从$x_i\Rightarrow x_{i+1}$单步递推过程来说，步长$\Delta t$越小，局部截断误差越小(方法确定情况下)，但是随着步长的缩小，不但会引起计算量的增加，而且也有可能引起舍入误差的严重积累；但步长$\Delta t$太大又不能达到预期的精度要求，所以选择合适的步长$\Delta t$，在实际计算中也是比较重要的。其实有时候在实际使用中步长并不需要算法确定，而是需要根据数据帧率来确定的，比如imu数据。
下面给出求解步长的步骤：

1. 以步长$\Delta t$开始，利用龙格-库塔公式计算$x_i\Rightarrow x_{i+1}$得到一个近似值$x_{i+1}^{\Delta t}$；
2. 然后步长减半为$\Delta t/2$，利用龙格-库塔公式分两步计算$x_i\Rightarrow x_{i+\frac{1}{2}}\Rightarrow x_{i+1}$得到一个近似值$x_{i+1}^{\Delta t/2}$；
3. 计算$|x_{i+1}^{\Delta t/2}-x_{i+1}^{\Delta t}<ϵ|$是否成立，如果成立直接步长选择$\Delta t$，否则继续步长减半$\Delta t/4$，重复上诉步骤直到满足精度要求。

例子

待续~~