跳到主要内容

四阶龙格库塔法

1. 数学推导​

考虑一阶常微分方程(ODE)的初值问题:

y˙(t)=f(t,y(t)),y(t0)=y0\dot{y}(t) = f(t, y(t)), \quad y(t_0) = y_0

给定步长 hh,我们希望由 yn=y(tn)y_n = y(t_n) 计算下一步的 yn+1≈y(tn+h)y_{n+1} \approx y(t_n + h)。

1.1 从欧拉法到显式龙格库塔​

将 y(tn+h)y(t_n + h) 在 tnt_n 处做泰勒展开:

y(tn+h)=yn+hy˙n+h22!y¨n+h33!yn(3)+h44!yn(4)+O(h5)y(t_n + h) = y_n + h\dot{y}_n + \frac{h^2}{2!}\ddot{y}_n + \frac{h^3}{3!}y_n^{(3)} + \frac{h^4}{4!}y_n^{(4)} + \mathcal{O}(h^5)

只保留一阶项就是经典的显式欧拉法:

yn+1=yn+hf(tn,yn)y_{n+1} = y_n + h f(t_n, y_n)

其单步截断误差为 O(h2)\mathcal{O}(h^2),全局误差为 O(h)\mathcal{O}(h)。要提高精度就必须匹配更高阶的泰勒项,但直接微分 ff 得到 y¨, y(3), y(4)\ddot{y},\ y^{(3)},\ y^{(4)} 在多维/非线性情形下代价极高。

龙格库塔的思想:用 ff 在若干个"探测点"的取值做加权平均,来间接匹配泰勒级数的高阶项,从而避免解析求导。ss 级显式 RK 方法可写为:

yn+1=yn+h∑i=1sbiki,ki=f ⁣(tn+cih, yn+h∑j=1i−1aijkj)y_{n+1} = y_n + h\sum_{i=1}^{s} b_i k_i,\qquad k_i = f\!\left(t_n + c_i h,\ y_n + h\sum_{j=1}^{i-1} a_{ij} k_j\right)

其中 ci, aij, bic_i,\ a_{ij},\ b_i 是待定系数,通常写在 Butcher 表中。

1.2 经典 RK4 的系数​

取 s=4s=4,令中间节点为 c1=0, c2=c3=12, c4=1c_1 = 0,\ c_2 = c_3 = \tfrac{1}{2},\ c_4 = 1。将 yn+1y_{n+1} 按 hh 展开并与真实解的泰勒展开逐阶匹配,可得到关于 bi, aijb_i,\ a_{ij} 的阶条件(order conditions):

∑ibi=1,∑ibici=12,∑ibici2=13,∑ibici3=14\sum_i b_i = 1,\qquad \sum_i b_i c_i = \tfrac{1}{2},\qquad \sum_i b_i c_i^2 = \tfrac{1}{3},\qquad \sum_i b_i c_i^3 = \tfrac{1}{4}∑i,jbiaijcj=16,∑i,jbiciaijcj=18,∑i,jbiaijcj2=112,∑i,j,kbiaijajkck=124\sum_{i,j} b_i a_{ij} c_j = \tfrac{1}{6},\qquad \sum_{i,j} b_i c_i a_{ij} c_j = \tfrac{1}{8},\qquad \sum_{i,j} b_i a_{ij} c_j^2 = \tfrac{1}{12},\qquad \sum_{i,j,k} b_i a_{ij} a_{jk} c_k = \tfrac{1}{24}

求解上述方程组的一个标准解就是经典 RK4:

0121212012100116131316\begin{array}{c|cccc} 0 & & & & \\ \tfrac{1}{2} & \tfrac{1}{2} & & & \\ \tfrac{1}{2} & 0 & \tfrac{1}{2} & & \\ 1 & 0 & 0 & 1 & \\ \hline & \tfrac{1}{6} & \tfrac{1}{3} & \tfrac{1}{3} & \tfrac{1}{6} \end{array}

展开为递推形式:

k1=f(tn, yn)k2=f ⁣(tn+h2, yn+h2k1)k3=f ⁣(tn+h2, yn+h2k2)k4=f ⁣(tn+h, yn+hk3)yn+1=yn+h6(k1+2k2+2k3+k4)\begin{aligned} k_1 &= f(t_n,\ y_n) \\ k_2 &= f\!\left(t_n + \tfrac{h}{2},\ y_n + \tfrac{h}{2}k_1\right) \\ k_3 &= f\!\left(t_n + \tfrac{h}{2},\ y_n + \tfrac{h}{2}k_2\right) \\ k_4 &= f\!\left(t_n + h,\ y_n + h k_3\right) \\ y_{n+1} &= y_n + \tfrac{h}{6}\left(k_1 + 2k_2 + 2k_3 + k_4\right) \end{aligned}

1.3 几何直观​

  • k1k_1:区间左端斜率;
  • k2k_2:用 k1k_1 走半步后,在中点估计的斜率;
  • k3k_3:用 k2k_2 再走半步后,在中点修正一次的斜率;
  • k4k_4:用 k3k_3 走完整步后,在右端估计的斜率。

最终加权 16(k1+2k2+2k3+k4)\tfrac{1}{6}(k_1 + 2k_2 + 2k_3 + k_4) 相当于 Simpson 积分公式在 [tn,tn+h][t_n, t_n+h] 上对 ff 的积分近似,因此天然与 Simpson 公式同阶。

1.4 精度与稳定性​

  • 单步截断误差为 O(h5)\mathcal{O}(h^5),全局误差为 O(h4)\mathcal{O}(h^4),故称"四阶"。
  • 每步需要 4 次 ff 求值,代价可控,是刚性不强的问题中默认的高阶显式方法。
  • 稳定域包含实轴上 [−2.78, 0]\left[-2.78,\ 0\right] 的一段,遇到强刚性系统仍需换用隐式方法(如 Radau、SDIRK)。

1.5 应用到四元数运动学​

IMU 姿态更新中,四元数 q=[qw, qx, qy, qz]Tq = [q_w,\ q_x,\ q_y,\ q_z]^T 在角速度 ω\boldsymbol{\omega} 作用下满足:

q˙=12 q⊗[0ω]\dot{q} = \tfrac{1}{2}\, q \otimes \begin{bmatrix} 0 \\ \boldsymbol{\omega} \end{bmatrix}

将其视作 y˙=f(y)\dot{y} = f(y)(ω\boldsymbol{\omega} 在一步内近似为常量),直接套用 §1.2 的四阶递推即可。由于数值积分不严格保范,每步末尾需将结果重新单位化 q←q/∥q∥q \leftarrow q / \lVert q \rVert,以保持其表示旋转的几何意义。

2. 代码实现​

import numpy as np

def quaternion_derivative(q, w):
"""
计算四元数的导数 q_dot = 0.5 * q * w
w: [gx, gy, gz] 补偿后的角速度 (rad/s)
"""
qw, qx, qy, qz = q
gx, gy, gz = w

# 四元数微分方程的展开形式
dq_w = 0.5 * (-qx * gx - qy * gy - qz * gz)
dq_x = 0.5 * ( qw * gx + qy * gz - qz * gy)
dq_y = 0.5 * ( qw * gy - qx * gz + qz * gx)
dq_z = 0.5 * ( qw * gz + qx * gy - qy * gx)

return np.array([dq_w, dq_x, dq_y, dq_z])

def update_pose_rk4(q, w, dt):
"""
使用四阶龙格库塔法(RK4)更新姿态四元数
q: 当前四元数 np.array([qw, qx, qy, qz])
w: 补偿后的角速度 np.array([gx, gy, gz])
dt: 采样时间
"""##
# 如果运动极快,可以假设一小步内 w 是恒定的,或者用前后两帧 w 线性插值(这里假设恒定)
k1 = quaternion_derivative(q, w)
k2 = quaternion_derivative(q + 0.5 * dt * k1, w)
k3 = quaternion_derivative(q + 0.5 * dt * k2, w)
k4 = quaternion_derivative(q + dt * k3, w)

# RK4 整合
q_next = q + (dt / 6.0) * (k1 + 2*k2 + 2*k3 + k4)

# ⚠️ 关键:四元数由于数值计算误差会失去单位化,必须强行归一化!
norm = np.linalg.norm(q_next)
return q_next / norm