在惯性导航以及VIO等实际问题中利用IMU求解位姿需要对IMU测量值进行积分得到需要的位置和姿态,其中主要就是求解微分方程。但之前求解微分方程的解析方法主要是应用于一些简单和特殊的微分方程求解中,对于一般形式的微分方程,一般很难用解析方法求出精确解,只能用数值方法求解。该系列主要介绍一些常用的常微分方程的数值解法,主要包括:
这个系列后面文章会用到前面文章的理论和技术,所以建议按照顺序查看。
欧拉法是求解初值问题最常用的方法,欧拉法的基本公式为
{ x i + 1 = x i + Δ t ⋅ f ( t i , x i ) , i = 0 , 1 , 2 , ⋯ x 0 = x ( t 0 ) , (1) \begin{cases} x_{i+1}=x_{i} + \Delta t \cdot f(t_{i}, x_{i}),\quad i=0,1,2,\cdots \\ x_0 = \mathbf{x}(t_0), \tag{1} \end{cases} {xi+1=xi+Δt⋅f(ti,xi),i=0,1,2,⋯x0=x(t0),(1)
欧拉公式是常微分方程的数值解法非常常用的一种解法,类似[常微分方程的数值解法系列一] 常微分方程介绍的那样,欧拉法本质是将连续问题如下
{ x ′ ( t ) = f ( t , x ( t ) ) , a ≤ t ≤ b x ( t 0 ) = x 0 \begin{cases} \mathbf{x}^{\prime}(t)=f(t, \mathbf{x}(t)), \quad a \leq t \leq b \\ \mathbf{x}({t_0}) = x_0 \end{cases} {x′(t)=f(t,x(t)),a≤t≤bx(t0)=x0
转化为在给定离散点上的差分方程(离散问题)。
以 f ( t 0 , x 0 ) f(t_0,x_0) f(t0,x0)为斜率过点 ( t 0 , x 0 ) (t_0,x_0) (t0,x0)作一条直线,与直线 t = t 1 t=t_1 t=t1的交点就是 x 1 x_1 x1。以此类推,以 f ( t i , x i ) f(t_i,x_i) f(ti,xi)为斜率过点 ( t i , x i ) (t_i,x_i) (ti,xi)作一条直线,与直线 t = t i + 1 t=t_{i+1} t=ti+1的交点就是 x i + 1 x_{i+1} xi+1,所以欧拉法也称为欧拉折线法。
首先说明这里给出的公式只是欧拉公式的一种形式,一般叫做向前欧拉公式,也是最常用的一种欧拉公式方式,后面会给出该公式的几种证明。同时也会介绍其他的几种欧拉公式。
欧拉基本公式可以在已知 x i x_{i} xi的情况下递推出 x i + 1 x_{i+1} xi+1,下面给出几种解释证明
给出 x ( t ) \mathbf{x}(t) x(t)在 t = t i t=t_i t=ti处的一阶泰勒展开式为
x ( t i + 1 ) = x ( t i ) + Δ t x ′ ( t i ) + Δ t 2 2 ! x ′ ′ ( t i ) + O ( Δ t 3 ) , (2) \mathbf{x}(t_{i+1}) = \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{2} x(ti+1)=x(ti)+Δtx′(ti)+2!Δt2x′′(ti)+O(Δt3),(2)
对上面的泰勒展开公式社区二阶导数之后的余项,并令 x i + 1 ≈ x ( t i + 1 ) , x i ≈ x ( t i ) x_{i+1} \approx \mathbf{x}(t_{i+1}), x_{i} \approx \mathbf{x}(t_{i}) xi+1≈x(ti+1),xi≈x(ti),便得:
x i + 1 = x i + Δ t ⋅ f ( t i , x i ) x_{i+1}=x_{i} + \Delta t \cdot f(t_{i}, x_{i}) xi+1=xi+Δt⋅f(ti,xi)
对于非常小的数 Δ t \Delta t Δt,微分方程中的导数 d x d t \frac{d\mathbf{x}}{dt} dtdx可以用差商表示,即
d x d t = x ( t i + 1 ) − x ( t i ) t i + 1 − t i ≈ x i + 1 − x i Δ t = x ′ ( t i ) = f ( t i , x i ) , (3) \frac{d\mathbf{x}}{dt} = \frac{\mathbf{x}(t_{i+1})-\mathbf{x}(t_i)}{t_{i+1}-t_i} \approx \frac{x_{i+1}-x_{i}}{\Delta t} = \mathbf{x}^{\prime}(t_{i}) = f(t_{i}, x_{i}), \tag{3} dtdx=ti+1−tix(ti+1)−x(ti)≈Δtxi+1−xi=x′(ti)=f(ti,xi),(3)
便得:
x i + 1 = x i + Δ t ⋅ f ( t i , x i ) x_{i+1}=x_{i} + \Delta t \cdot f(t_{i}, x_{i}) xi+1=xi+Δt⋅f(ti,xi)
其中 Δ t \Delta t Δt称为步长,一般步长为 Δ t = b − a n \Delta t = \frac{b-a}{n} Δt=nb−a。
在[常微分方程的数值解法系列一] 常微分方程文中公式 ( 1 ) (1) (1)可得
x ( t i + 1 ) − x ( t i ) = ∫ τ ∈ [ t i , t i + 1 ] f ( τ , x ( τ ) ) d τ , (4) \mathbf{x}(t_{i+1})-\mathbf{x}(t_i) = \int_{\tau \in [t_i, t_{i+1}]} f(\tau, \mathbf{x}(\tau)) d \tau, \tag{4} x(ti+1)−x(ti)=∫τ∈[ti,ti+1]f(τ,x(τ))dτ,(4)
从 t i t_i ti到 t i + 1 t_{i+1} ti+1进行积分可得 x ( t i ) \mathbf{x}(t_i) x(ti)到 x ( t i + 1 ) \mathbf{x}(t_{i+1}) x(ti+1)的增量。
如果采用积分的左矩形公式近似计算上式中的积分,并令 x i + 1 ≈ x ( t i + 1 ) , x i ≈ x ( t i ) x_{i+1} \approx \mathbf{x}(t_{i+1}), x_{i} \approx \mathbf{x}(t_{i}) xi+1≈x(ti+1),xi≈x(ti),便得:
x i + 1 = x i + Δ t ⋅ f ( t i , x i ) x_{i+1}=x_{i} + \Delta t \cdot f(t_{i}, x_{i}) xi+1=xi+Δt⋅f(ti,xi)
之前主要介绍的就是向前欧拉公式,也称为显式欧拉公式(求解下一步的参数都是已知的)。
后面部分的相关介绍以及例子都是利用的向前欧拉公式。
利用之前的积分近似方式来说明,如果在求解公式 ( 4 ) (4) (4)中积分是采用积分的右矩形公式可得
x i + 1 = x i + Δ t ⋅ f ( t i + 1 , x i + 1 ) , (5) x_{i+1}=x_{i} + \Delta t \cdot f(t_{i+1}, x_{i+1}), \tag{5} xi+1=xi+Δt⋅f(ti+1,xi+1),(5)
该公式称为向后欧拉公式,也称为隐式欧拉公式。由于未知量 x i + 1 x_{i+1} xi+1在公式右端,无法直接进行计算,一般使用迭代法求解进行求解,这里不做过多讨论。
上面讲解的两种公式都是利用矩形求积分公式来近似右端的积分得到的。但由于矩形公式精度比较低,所以一般导出的欧拉公式精度也很低。
这里如果利用数值积分中的梯形公式近似计算公式 ( 4 ) (4) (4)中右端的积分,则有
∫ τ ∈ [ t i , t i + 1 ] f ( τ , x ( τ ) ) d τ = Δ t 2 [ f ( t i , x ( t i ) ) + f ( t i + 1 , x ( t i + 1 ) ) ] \int_{\tau \in [t_i, t_{i+1}]} f(\tau, \mathbf{x}(\tau)) d \tau = \frac{\Delta t}{2}\left[ f(t_i,\mathbf{x}(t_i)) + f(t_{i+1},\mathbf{x}({t_{i+1}}))\right] ∫τ∈[ti,ti+1]f(τ,x(τ))dτ=2Δt[f(ti,x(ti))+f(ti+1,x(ti+1))]
令 x i + 1 ≈ x ( t i + 1 ) , x i ≈ x ( t i ) \mathbf{x}_{i+1} \approx \mathbf{x}(t_{i+1}), \mathbf{x}_{i} \approx \mathbf{x}(t_{i}) xi+1≈x(ti+1),xi≈x(ti),便得:
x i + 1 = x i + Δ t 2 [ f ( t i , x i ) + f ( t i + 1 , x i + 1 ) ] , (6) x_{i+1}=x_{i} +\frac{\Delta t}{2}\left[ f(t_i,x_i) + f(t_{i+1},x_{i+1})\right], \tag{6} xi+1=xi+2Δt[f(ti,xi)+f(ti+1,xi+1)],(6)
该公式称为梯形公式,由于未知量 x i + 1 x_{i+1} xi+1在公式右端,所以也是隐式方程。
利用之前的积分近似方式来说明,如果在求解公式 ( 4 ) (4) (4)中积分是采用积分的中矩形公式可得
x i + 1 = x i + Δ t ⋅ f ( t i + 1 2 , x i + 1 2 ) , (7) x_{i+1}=x_{i} + \Delta t \cdot f(t_{i+\frac{1}{2}}, x_{i+\frac{1}{2}}), \tag{7} xi+1=xi+Δt⋅f(ti+21,xi+21),(7)
其中 t i + 1 2 = t i + 1 2 Δ t , x i + 1 2 ≈ x ( t i + 1 2 ) t_{i+\frac{1}{2}} = t_i + \frac{1}{2} \Delta t, x_{i+\frac{1}{2}} \approx \mathbf{x}(t_{i+\frac{1}{2}}) ti+21=ti+21Δt,xi+21≈x(ti+21)。
该公式称为中点公式,由于未知量 x i + 1 2 x_{i+\frac{1}{2}} xi+21在公式右端,所以也是隐式方程。
在[常微分方程的数值解法系列一] 常微分方程文中公式 ( 3 ) (3) (3)可知常微分方程的数值解法的局部截断误差为
e i = x i − x ( t i ) \boldsymbol{e}_i = x_i - \mathbf{x}(t_i) ei=xi−x(ti)
利用公式 ( 2 ) (2) (2)的泰勒展开式以及向前欧拉公式可得
e i + 1 = x i + 1 − x ( t i + 1 ) = − Δ t 2 2 ! x ′ ′ ( t i ) = O ( Δ t 2 ) \boldsymbol{e}_{i+1} = x_{i+1} - \mathbf{x}(t_{i+1}) = -\frac{\Delta t^2}{2!} \mathbf{x}^{\prime\prime}(t_{i}) = \mathbf{O}(\Delta t^2) ei+1=xi+1−x(ti+1)=−2!Δt2x′′(ti)=O(Δt2)
根据[常微分方程的数值解法系列一] 常微分方程文中局部截断误差定义得
向前欧拉公式具有一阶精度(一阶解法)。
在实际过程中一般是给定
{ x ′ ( t ) = f ( t , x ( t ) ) , a ≤ t ≤ b x ( t 0 ) = x 0 \begin{cases} \mathbf{x}^{\prime}(t)=f(t, \mathbf{x}(t)), \quad a \leq t \leq b \\ \mathbf{x}({t_0}) = x_0 \end{cases} {x′(t)=f(t,x(t)),a≤t≤bx(t0)=x0
求解 x ( t k ) , a ≤ t k ≤ b x(t_k),a \leq t_k \leq b x(tk),a≤tk≤b的值,同时 Δ t = t k − t 0 \Delta t = t_k-t_0 Δt=tk−t0值比较大,如果只用单步欧拉法求解会导致误差比较到,之前在证明过程都是假设 Δ t \Delta t Δt是小值。所以需要分步求解,
向前欧拉公式具体步骤为:
利用欧拉法(向前)求解初值问题
{ x ′ ( t ) = 2 t x , 0 ≤ t ≤ 1 x ( t 0 ) = 1 \begin{cases} \mathbf{x}^{\prime}(t)=2tx, \quad 0 \leq t \leq 1 \\ \mathbf{x}({t_0}) = 1 \end{cases} {x′(t)=2tx,0≤t≤1x(t0)=1
求 x ( t = 1 ) \mathbf{x}(t=1) x(t=1)的近似值,步长 Δ t = 0.2 \Delta t = 0.2 Δt=0.2。
这里原函数为 x ( t ) = e − t 2 \mathbf{x}(t) = e^{-t^2} x(t)=e−t2,用于后面验证结果
解:
{ x i + 1 = x i + 0.2 ( − 2 t i x i ) = ( 1 − 0.4 t i ) x i x 0 = 1 \begin{cases} x_{i+1}=x_{i} + 0.2(-2t_ix_i)=(1-0.4t_i)x_i \\ x_0 = 1 \end{cases} {xi+1=xi+0.2(−2tixi)=(1−0.4ti)xix0=1
计算得:
i i i | t i t_i ti | x i x_i xi | x ( t i ) \mathbf{x}(t_i) x(ti) | e i e_i ei |
---|---|---|---|---|
0 | 0 | 1 | 1 | 0 |
1 | 0.2 | 1 | 0.9608 | -0.0392 |
2 | 0.4 | 0.92 | 0.8521 | -0.0679 |
3 | 0.6 | 0.7728 | 0.6977 | -0.0751 |
4 | 0.8 | 0.5873 | 0.5273 | -0.06 |
5 | 1 | 0.3994 | 0.3679 | -0.0315 |