在惯性导航以及VIO等实际问题中利用IMU求解位姿需要对IMU测量值进行积分得到需要的位置和姿态,其中主要就是求解微分方程。但之前求解微分方程的解析方法主要是应用于一些简单和特殊的微分方程求解中,对于一般形式的微分方程,一般很难用解析方法求出精确解,只能用数值方法求解。该系列主要介绍一些常用的常微分方程的数值解法,主要包括:
这个系列后面文章会用到前面文章的理论和技术,所以建议按照顺序查看。
简介
在之前常微分方程的数值解法系列中,已经介绍了欧拉法,改进欧拉法以及中值法等多种常微分方程的数值解法。但是之前讲解的方法的局部截断误差相对来说比较大,精度有限,在某些情况下需要更高精度的方法。
本来主要介绍的龙格-库塔法,可以提供更高精度的常微分方程的数值解法。而且龙格-库塔法是常微分方程的数值解法的基本理论,后面讲解的过程中会发现,之前讲解的多种方法都可以归类到龙格-库塔法的体系中。而且不考虑计算量的情况下,理论上是可以构造任意高阶的龙格-库塔公式,这里阶数可以认为和 [常微分方程的数值解法系列一] 常微分方程介绍的截断误差的阶数概念是一致的,阶数越高代表截断误差越小,精度越高。但比较常用的是四阶龙格-库塔公式,后面会给出具体详细的讲解。
基本思想
由微分中值定理可知,存在点ξ \xiξ使得
x ( t i + 1 ) − x ( t i ) Δ t = x ′ ( ξ ) , ξ ∈ ( t i , t i + 1 ) \frac{\mathbf{x}(t_{i+1})-\mathbf{x}(t_i)}{\Delta t} = \mathbf{x}^{\prime}(\xi), \quad \xi \in (t_i, t_{i+1})Δtx(ti+1)−x(ti)=x′(ξ),ξ∈(ti,ti+1)
即
x ( t i + 1 ) = x ( t i ) + Δ t x ′ ( ξ ) = x ( t i ) + Δ t ⋅ f ( ξ , x ( ξ ) ) = x ( t i ) + Δ t k ˉ , (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}, \tag{1}x(ti+1)=x(ti)+Δtx′(ξ)=x(ti)+Δt⋅f(ξ,x(ξ))=x(ti)+Δtkˉ,(1)
其中k ˉ = f ( ξ , x ( ξ ) ) \bar{k} = f(\xi, \mathbf{x}(\xi))kˉ=f(ξ,x(ξ))称为x ( t ) \mathbf{x}(t)x(t)在[ t i , t i + 1 ] [t_i, t_{i+1}][ti,ti+1]上的平均斜率。所以基于这点考虑,只要对平均斜率k ˉ \bar{k}kˉ提供一种近似算法,那么就可以利用x i x_ixi去递推求解x i + 1 x_{i+1}xi+1,这就是龙格-库塔方法的基本思想。
当然不同的近似算法求解精度不同,像之前讲解的向前欧拉法、改进欧拉法和中值法利用龙格-库塔方法思想解释就是分别利用t i t_iti点斜率、t i t_iti点和t i + 1 t_{i+1}ti+1点斜率平均值和t i t_iti点和t i + 1 t_{i+1}ti+1点间中点斜率来近似k ˉ \bar{k}kˉ。
具体方法
一阶
以x ( t ) \mathbf{x}(t)x(t)在t i t_iti点斜率作为x ( t ) \mathbf{x}(t)x(t)在[ t i , t i + 1 ] [t_i, t_{i+1}][ti,ti+1]上的平均斜率k ˉ \bar{k}kˉ,并令x i ≈ x ( t i ) x_{i} \approx \mathbf{x}(t_{i})xi≈x(ti),可得
k ˉ = x ′ ( t i ) = f ( t i , x ( t i ) ) ≈ f ( t i , x i ) \bar{k} = \mathbf{x}^{\prime}(t_i) = f(t_i, \mathbf{x}(t_i)) \approx f(t_{i}, x_{i})kˉ=x′(ti)=f(ti,x(ti))≈f(ti,xi)
所以
x i + 1 = x i + Δ t ⋅ f ( t i , x i ) , (2) x_{i+1}=x_{i} + \Delta t \cdot f(t_{i}, x_{i}), \tag{2}xi+1=xi+Δt⋅f(ti,xi),(2)
这就是[常微分方程的数值解法系列二] 欧拉法文中讲解的向前欧拉法,也叫作一阶龙格-库塔方法。公式( 2 ) (2)(2)就是一阶龙格-库塔公式。
x i x_{i}xi是递推过程中x ( t i ) \mathbf{x}(t_{i})x(ti)的近似值,后面都用x n x_nxn表示x ( t n ) \mathbf{x}(t_n)x(tn)。
二阶
在[ t i , t i + 1 ] [t_i, t_{i+1}][ti,ti+1]上取两点t i , t i + p ( 0 < p ≤ 1 ) t_i,t_{i+p}(0
x i + 1 = x i + Δ t ( λ 1 k 1 + λ 2 k 2 ) x_{i+1}=x_{i} + \Delta t (\lambda_1k_1+\lambda_2k_2)xi+1=xi+Δt(λ1k1+λ2k2)
其中k 1 = f ( t i , x i ) k_1 = f(t_{i}, x_{i})k1=f(ti,xi),k 2 k_2k2是( t i , t i + 1 ] (t_i, t_{i+1}](ti,ti+1]内任意一点t i + p = t i + p Δ t ( 0 < p ≤ 1 ) t_{i+p} = t_i +p \Delta t (0
但这里在求解过程中x i + p x_{i+p}xi+p是未知量,所以需要先求解出,可以仿照改进欧拉公式的思想,得
x i + p = x i + p Δ t f ( t i , x i ) = x i + p Δ t k 1 x_{i+p} = x_i + p\Delta t f(t_{i}, x_{i}) = x_i + p \Delta t k_1xi+p=xi+pΔtf(ti,xi)=xi+pΔtk1
所以整理得
{ x i + 1 = x i + Δ t ( λ 1 k 1 + λ 2 k 2 ) k 1 = f ( t i , x i ) k 2 = f ( t i + p Δ t , x i + p Δ t k 1 ) (3) \begin{cases} x_{i+1}=x_{i} + \Delta t (\lambda_1k_1+\lambda_2k_2) \\ k_1 = f(t_{i}, x_{i}) \\ k_2 = f(t_i +p \Delta t, x_i + p \Delta t k_1) \end{cases} \tag{3}⎩⎪⎨⎪⎧xi+1=xi+Δt(λ1k1+λ2k2)k1=f(ti,xi)k2=f(ti+pΔt,xi+pΔtk1)(3)
其中公式( 3 ) (3)(3)有三个待确认参数λ 1 , λ 2 , p \lambda_1,\lambda_2,pλ1,λ2,p,利用泰勒展开选择合适的参数使上诉公式具有二阶精度,即局部截断误差为O ( Δ t 3 ) \mathbf{O}(\Delta t^3)O(Δt3),满足这类条件的公式( 3 ) (3)(3)就是二阶龙格-库塔公式。
求解参数
下面利用泰勒展开求解待确认参数λ 1 , λ 2 , p \lambda_1,\lambda_2,pλ1,λ2,p,使公式( 3 ) (3)(3)的局部截断误差为O ( Δ t 3 ) \boldsymbol{O}(\Delta t^3)O(Δt3)。
分别对公式3 33的k 1 , k 2 k_1,k_2k1,k2作泰勒展开为
k 1 = f ( t i , x i ) = x ′ ( t i ) k 2 = f ( t i + p Δ t , x i + p Δ t k 1 ) = f ( t i + p Δ t , x i + p Δ t ⋅ f ( t i , x i ) ) = f ( t i , x i ) + p Δ t f t ′ ( t i , x i ) + ( p Δ t ⋅ f ( t i , x i ) ) ⋅ f x ′ ( t i , x i ) + O ( Δ t 2 ) = x ′ ( t i ) + p Δ t x ′ ′ ( t i ) + O ( Δ t 2 ) \begin{aligned} 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{aligned}k1k2=f(ti,xi)=x′(ti)=f(ti+pΔt,xi+pΔtk1)=f(ti+pΔt,xi+pΔt⋅f(ti,xi))=f(ti,xi)+pΔtft′(ti,xi)+(pΔt⋅f(ti,xi))⋅fx′(ti,xi)+O(Δt2)=x′(ti)+pΔtx′′(ti)+O(Δt2)
所以可得
x i + 1 = x i + Δ t ( λ 1 k 1 + λ 2 k 2 ) = x i + Δ t ( λ 1 x ′ ( t i ) + λ 2 ( x ′ ( t i ) + p Δ t x ′ ′ ( t i ) + O ( Δ t 2 ) ) ) = x i + Δ t ( λ 1 + λ 2 ) x ′ ( t i ) + λ 2 p Δ t 2 x ′ ′ ( t i ) + O ( Δ t 3 ) , (4) \begin{aligned} x_{i+1} &=x_{i} + \Delta t (\lambda_1k_1+\lambda_2k_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_2p\Delta t^2\mathbf{x}^{\prime\prime}(t_i) + \mathbf{O}(\Delta t^3) \end{aligned}, \tag{4}xi+1=xi+Δt(λ1k1+λ2k2)=xi+Δt(λ1x′(ti)+λ2(x′(ti)+pΔtx′′(ti)+O(Δt2)))=xi+Δt(λ1+λ2)x′(ti)+λ2pΔt2x′′(ti)+O(Δt3),(4)
又x ( t i + 1 ) \mathbf{x}(t_{i+1})x(ti+1)的二阶泰勒展开为
x ( t i + 1 ) = x ( t i + Δ t ) = x ( t i ) + Δ t x ′ ( t i ) + Δ t 2 2 ! x ′ ′ ( t i ) + O ( Δ t 3 ) , (5) \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), \tag{5}x(ti+1)=x(ti+Δt)=x(ti)+Δtx′(ti)+2!Δt2x′′(ti)+O(Δt3),(5)
比较公式( 4 ) (4)(4)和公式( 5 ) (5)(5),如果要使公式( 3 ) (3)(3)的局部截断误差满足x i + 1 − x ( t i + 1 ) = O ( Δ t 3 ) x_{i+1} - \mathbf{x}(t_{i+1}) = \mathbf{O}(\Delta t^3)xi+1−x(ti+1)=O(Δt3),即要求公式具有二阶精度,需要O ( Δ t 3 ) \mathbf{O}(\Delta t^3)O(Δt3)前面的项相等,即需要满足
{ λ 1 + λ 2 = 1 λ 2 p = 1 2 , (6) \begin{cases} \lambda_1 + \lambda_2 = 1 \\ \lambda_2p = \frac{1}{2} \end{cases}, \tag{6}{λ1+λ2=1λ2p=21,(6)
满足上诉条件的公式( 3 ) (3)(3)统称为二阶龙格-库塔公式。
特殊二阶
如果当p = 1 , λ 1 = 1 2 , λ 2 = 1 2 p=1,\lambda_1=\frac{1}{2},\lambda_2=\frac{1}{2}p=1,λ1=21,λ2=21,二阶龙格-库塔公式就成了改进欧拉公式,具体详解见[常微分方程的数值解法系列三] 改进欧拉法(预估校正法)。简单来说改进欧拉公式就是以x \mathbf{x}x函数在t i t_iti点和t i + 1 t_{i+1}ti+1点处的斜率k 1 k_1k1和k 2 k_2k2的算数平均值作为x \mathbf{x}x在[ t i , t i + 1 ] [t_i, t_{i+1}][ti,ti+1]上的平均斜率k ˉ \bar{k}kˉ。
如果当p = 1 2 , λ 1 = 0 , λ 2 = 1 p=\frac{1}{2},\lambda_1=0,\lambda_2=1p=21,λ1=0,λ2=1,二阶龙格-库塔公式就成了变形欧拉公式,具体详解见[常微分方程的数值解法系列四] 中值法。简单来说变形欧拉公式或者说是中值法就是以x \mathbf{x}x函数在t i t_iti点和t i + 1 t_{i+1}ti+1中点处的斜率k kk作为x \mathbf{x}x在[ t i , t i + 1 ] [t_i, t_{i+1}][ti,ti+1]上的平均斜率k ˉ \bar{k}kˉ。
三阶
在[ t i , t i + 1 ] [t_i, t_{i+1}][ti,ti+1]上取三点t i , t i + p , t i + q ( 0 < p ≤ q ≤ 1 ) t_i,t_{i+p},t_{i+q}(0
x i + 1 = x i + Δ t ( λ 1 k 1 + λ 2 k 2 + λ 3 k 3 ) x_{i+1}=x_{i} + \Delta t (\lambda_1k_1+\lambda_2k_2+\lambda_3k_3)xi+1=xi+Δt(λ1k1+λ2k2+λ3k3)
其中
{ k 1 = f ( t i , x i ) k 2 = f ( t i + p Δ t , x i + p Δ t k 1 ) \begin{cases} k_1 = f(t_{i}, x_{i}) \\ k_2 = f(t_i +p \Delta t, x_i + p \Delta t k_1) \end{cases}{k1=f(ti,xi)k2=f(ti+pΔt,xi+pΔtk1)
为了求x i + q x_{i+q}xi+q点的斜率k 3 k_3k3,最直接方法就是利用改进欧拉法进行预报得
k 3 = f ( t i + q , x ^ i + q ) = f ( t i + q Δ t , x i + q Δ t k 1 ) k_3 = f(t_{i+q}, \hat{x}_{i+q}) = f(t_i +q \Delta t, x_i + q \Delta t k_1)k3=f(ti+q,x^i+q)=f(ti+qΔt,xi+qΔtk1)
但是这样做效率比较差,因为在区间[ t i , t i + q ] [t_i,t_{i+q}][ti,ti+q]区间内已经有k 1 , k 2 k_1,k_2k1,k2两个斜率可以使用了,所以可以把k 1 , k 2 k_1,k_2k1,k2的线性组合当做[ x i , x i + q ] [x_i,x_{i+q}][xi,xi+q]上平均斜率的近似值,这肯定比上面求x ^ i + q \hat{x}_{i+q}x^i+q精度要好。由此,可得
x ^ i + q = x i + q Δ t ( μ 1 k 1 + μ 2 k 2 ) \hat{x}_{i+q} = x_{i} + q\Delta t (\mu_1k_1+\mu_2k_2)x^i+q=xi+qΔt(μ1k1+μ2k2)
所以
k 3 = f ( t i + q , x ^ i + q ) = f ( t i + q Δ t , x i + q Δ t ( μ 1 k 1 + μ 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_1k_1+\mu_2k_2))k3=f(ti+q,x^i+q)=f(ti+qΔt,xi+qΔt(μ1k1+μ2k2))
所以整理得
{ x i + 1 = x i + Δ t ( λ 1 k 1 + λ 2 k 2 + λ 3 k 3 ) k 1 = f ( t i , x i ) k 2 = f ( t i + p Δ t , x i + p Δ t k 1 ) k 3 = f ( t i + q Δ t , x i + q Δ t ( μ 1 k 1 + μ 2 k 2 ) ) , (7) \begin{cases} x_{i+1}=x_{i} + \Delta t (\lambda_1k_1+\lambda_2k_2+\lambda_3k_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_1k_1+\mu_2k_2)) \end{cases}, \tag{7}⎩⎪⎪⎪⎨⎪⎪⎪⎧xi+1=xi+Δt(λ1k1+λ2k2+λ3k3)k1=f(ti,xi)k2=f(ti+pΔt,xi+pΔtk1)k3=f(ti+qΔt,xi+qΔt(μ1k1+μ2k2)),(7)
其中公式( 7 ) (7)(7)有七个待确认参数λ 1 , λ 2 , λ 3 , μ 1 , μ 2 , p , q \lambda_1,\lambda_2,\lambda_3,\mu_1,\mu_2,p,qλ1,λ2,λ3,μ1,μ2,p,q,类似前面的推导利用泰勒展开选择合适的参数使上诉公式具有三阶精度,即局部截断误差为O ( Δ t 4 ) \mathbf{O}(\Delta t^4)O(Δt4),满足这类条件的公式( 7 ) (7)(7)就是三阶龙格-库塔公式。
推导过程和二阶类似,再此省略,只给出比较常用的一致形式
{ x i + 1 = x i + 1 6 Δ t ( k 1 + 4 k 2 + k 3 ) k 1 = f ( t i , x i ) k 2 = f ( t i + 1 2 Δ t , x i + 1 2 Δ t ⋅ k 1 ) k 3 = f ( t i + Δ t , x i − Δ t ⋅ k 1 + 2 Δ t ⋅ k 2 ) ) , (7) \begin{cases} 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{cases}, \tag{7}⎩⎪⎪⎪⎨⎪⎪⎪⎧xi+1=xi+61Δt(k1+4k2+k3)k1=f(ti,xi)k2=f(ti+21Δt,xi+21Δt⋅k1)k3=f(ti+Δt,xi−Δt⋅k1+2Δt⋅k2)),(7)
高阶
为了进一步提高精度,在[ t i , t i + 1 ] [t_i, t_{i+1}][ti,ti+1]上可以选取更多点的,利用改进欧拉法中预报法,预报这些点的斜率值(之前在三阶也讨论过,为了预报更准确,利用前面所有点的斜率来预报后面点的估计值进一步求得斜率,而不仅仅只利用第一个点),然后对这些斜率值加权平均作为作为x \mathbf{x}x在[ t i , t i + 1 ] [t_i, t_{i+1}][ti,ti+1]上的平均斜率k ˉ \bar{k}kˉ。然后在利用泰勒展开,对比相应的系数,从而确定在满足特定n nn阶精度下所有未知参数需要满足的条件。
前面也提到过,不考虑计算量的情况下,理论上是可以构造任意高阶的龙格-库塔公式。但在实际中发现,高于四阶的龙格-库塔公式,不但计算量非常大,而且精度进一步提升的有限。所以在实际使用中,四阶的龙格-库塔公式是精度和计算量都比较理想的公式。
推导过程和二阶类似,再此省略,只给出最经典的四阶的龙格-库塔公式:
{ x i + 1 = x i + Δ t ( 1 6 k 1 + 1 3 k 2 + 1 3 k 3 + 1 6 k 4 ) = 1 6 Δ t ( k 1 + 2 k 2 + 2 k 3 + k 4 ) k 1 = f ( t i , x i ) k 2 = f ( t i + 1 2 Δ t , x i + 1 2 Δ t ⋅ k 1 ) k 3 = f ( t i + 1 2 Δ t , x i + 1 2 Δ t ⋅ k 2 ) k 4 = f ( t i + Δ t , x i + Δ t ⋅ k 3 ) , (8) \begin{cases} 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{cases}, \tag{8}⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧xi+1=xi+Δt(61k1+31k2+31k3+61k4)=61Δt(k1+2k2+2k3+k4)k1=f(ti,xi)k2=f(ti+21Δt,xi+21Δt⋅k1)k3=f(ti+21Δt,xi+21Δt⋅k2)k4=f(ti+Δt,xi+Δt⋅k3),(8)
步长选择
之前讨论的所有的龙格-库塔方法都是以Δ t \Delta tΔt定步长来展开的,但从x i ⇒ x i + 1 x_i \Rightarrow x_{i+1}xi⇒xi+1单步递推过程来说,步长Δ t \Delta tΔt越小,局部截断误差越小(方法确定情况下),但是随着步长的缩小,不但会引起计算量的增加,而且也有可能引起舍入误差的严重积累;但步长Δ t \Delta tΔt太大又不能达到预期的精度要求,所以选择合适的步长Δ t \Delta tΔt,在实际计算中也是比较重要的。其实有时候在实际使用中步长并不需要算法确定,而是需要根据数据帧率来确定的,比如imu数据。
下面给出求解步长的步骤:
以步长Δ t \Delta tΔt开始,利用龙格-库塔公式计算x i ⇒ x i + 1 x_i \Rightarrow x_{i+1}xi⇒xi+1得到一个近似值x i + 1 Δ t x_{i+1}^{\Delta t}xi+1Δt;
然后步长减半为Δ t / 2 \Delta t /2Δt/2,利用龙格-库塔公式分两步计算x i ⇒ x i + 1 2 ⇒ x i + 1 x_i \Rightarrow x_{i+\frac{1}{2}} \Rightarrow x_{i+1}xi⇒xi+21⇒xi+1得到一个近似值x i + 1 Δ t / 2 x_{i+1}^{\Delta t/2}xi+1Δt/2;
计算∣ x i + 1 Δ t / 2 − x i + 1 Δ t < ϵ ∣ |x_{i+1}^{\Delta t/2} - x_{i+1}^{\Delta t} < \epsilon|∣xi+1Δt/2−xi+1Δt
例子
待续~~
本文同步分享在 博客“无比机智的永哥”(CSDN)。
如有侵权,请联系 support@ 删除。
本文参与“OSC源创计划”,欢迎正在阅读的你也加入,一起分享。