Unleashing Robotics: Mastering Quaternion Kinematics with Python - Chapter6(原创系列教程)（最关键一章）
本系列教程禁止转载，主要是为了有不同见解的同学可以方便联系我，我的邮箱 [email protected]
第6章 旋转的数值积分方法和角误差理论
1.Runge-Kutta数值积分方法
我们的目标是积分形如下式的非线性微分方程:
x ˙ = f ( t , x ) \dot{x} = f(t, x) x˙=f(t,x)
在有限的时间区间 Δ t \Delta t Δt内,将其转化为差分方程,即:
x ( t + Δ t ) = x ( t ) + ∫ t t + Δ t f ( τ , x ( τ ) ) d τ x(t + \Delta t) = x(t) + \int_{t}^{t+\Delta t}f(\tau, x(\tau))d\tau x(t+Δt)=x(t)+∫tt+Δtf(τ,x(τ))dτ
或等价地,如果假设 t n = n Δ t t_n = n\Delta t tn=nΔt且 x n ≡ x ( t n ) x_n \equiv x(t_n) xn≡x(tn),则有:
x n + 1 = x n + ∫ n Δ t ( n + 1 ) Δ t f ( τ , x ( τ ) ) d τ x_{n+1} = x_n + \int_{n\Delta t}^{(n+1)\Delta t}f(\tau, x(\tau))d\tau xn+1=xn+∫nΔt(n+1)Δtf(τ,x(τ))dτ
最常用的方法之一是Runge-Kutta(简称RK)方法。这些方法使用多次迭代来估计区间内的导数,然后使用该导数在步长 Δ t \Delta t Δt内进行积分。
在下面的小节中,我们将从最简单的方法到最通用的方法介绍几种RK方法,并根据最常见的名称给它们命名。
1.1 Euler方法
Euler方法假设导数 f ( ⋅ ) f(\cdot) f(⋅)在区间内是常数,因此有:
x n + 1 = x n + Δ t ⋅ f ( t n , x n ) x_{n+1} = x_n + \Delta t\cdot f(t_n, x_n) xn+1=xn+Δt⋅f(tn,xn)
作为一个通用的RK方法,这相当于单阶段法,可以描述如下。计算初始点的导数:
k 1 = f ( t n , x n ) k_1 = f(t_n, x_n) k1=f(tn,xn)
然后用它计算终点的积分值:
x n + 1 = x n + Δ t ⋅ k 1 x_{n+1} = x_n + \Delta t\cdot k_1 xn+1=xn+Δt⋅k1
代码示例:
def euler(f, x0, t0, dt, N):
"""Euler方法
Args:
f: 函数 f(t, x)
x0: 初始状态
t0: 初始时间
dt: 步长
N: 迭代次数
Returns:
ts: 时间点数组
xs: 状态数组
"""
ts = [t0]
xs = [x0]
for i in range(N):
x = xs[-1]
t = ts[-1]
k1 = f(t, x)
x_new = x + dt * k1
t_new = t + dt
xs.append(x_new)
ts.append(t_new)
return ts, xs
1.2 中点法
中点法假设区间中点处的导数是有效的,并进行一次迭代计算该中点的 x x x值,即:
x n + 1 = x n + Δ t ⋅ f ( t n + 1 2 Δ t , x n + 1 2 Δ t ⋅ f ( t n , x n ) ) x_{n+1} = x_n + \Delta t\cdot f\left(t_n + \frac{1}{2}\Delta t, x_n + \frac{1}{2}\Delta t\cdot f(t_n, x_n)\right) xn+1=xn+Δt⋅f(tn+21Δt,xn+21Δt⋅f(tn,xn))
中点法可以解释为两步法。首先,用Euler方法积分到中点,使用之前定义的 k 1 k_1 k1:
k 1 = f ( t n , x n ) k_1 = f(t_n, x_n) k1=f(tn,xn)
x ( t n + 1 2 Δ t ) = x n + 1 2 Δ t ⋅ k 1 x(t_n + \frac{1}{2}\Delta t) = x_n + \frac{1}{2}\Delta t\cdot k_1 x(tn+21Δt)=xn+21Δt⋅k1
然后用该值计算中点的导数 k 2 k_2 k2,进而得到积分结果:
k 2 = f ( t n + 1 2 Δ t , x ( t n + 1 2 Δ t ) ) k_2 = f(t_n + \frac{1}{2}\Delta t, x(t_n + \frac{1}{2}\Delta t)) k2=f(tn+21Δt,x(tn+21Δt))
x n + 1 = x n + Δ t ⋅ k 2 x_{n+1} = x_n + \Delta t\cdot k_2 xn+1=xn+Δt⋅k2
代码示例:
def midpoint(f, x0, t0, dt, N):
"""中点法
Args:
f: 函数 f(t, x)
x0: 初始状态
t0: 初始时间
dt: 步长
N: 迭代次数
Returns:
ts: 时间点数组
xs: 状态数组
"""
ts = [t0]
xs = [x0]
for i in range(N):
x = xs[-1]
t = ts[-1]
k1 = f(t, x)
x_mid = x + dt/2 * k1
t_mid = t + dt/2
k2 = f(t_mid, x_mid)
x_new = x + dt * k2
t_new = t + dt
xs.append(x_new)
ts.append(t_new)
return ts, xs
1.3 RK4方法
这通常被简称为Runge-Kutta方法。它在区间的起点、中点和终点处计算 f ( ) f() f()的值。它使用四个阶段或迭代来计算积分,获得四个导数 k 1 … k 4 k_1 \dots k_4 k1…k4,这些导数是按顺序计算的。然后对这些导数进行加权平均,以获得区间内导数的4阶估计值。
RK4方法用一个小算法比用一步公式更好地描述。RK4积分步骤为:
x n + 1 = x n + Δ t 6 ( k 1 + 2 k 2 + 2 k 3 + k 4 ) x_{n+1} = x_n + \frac{\Delta t}{6}(k_1 + 2k_2 + 2k_3 + k_4) xn+1=xn+6Δt(k1+2k2+2k3+k4)
也就是说,增量是通过假设斜率为 k 1 , k 2 , k 3 , k 4 k_1, k_2, k_3, k_4 k1,k2,k3,k4的加权平均值来计算的,其中:
k 1 = f ( t n , x n ) k_1 = f(t_n, x_n) k1=f(tn,xn)
k 2 = f ( t n + 1 2 Δ t , x n + Δ t 2 k 1 ) k_2 = f\left(t_n + \frac{1}{2}\Delta t, x_n + \frac{\Delta t}{2}k_1\right) k2=f(tn+21Δt,xn+2Δtk1)
k 3 = f ( t n + 1 2 Δ t , x n + Δ t 2 k 2 ) k_3 = f\left(t_n + \frac{1}{2}\Delta t, x_n + \frac{\Delta t}{2}k_2\right) k3=f(tn+21Δt,xn+2Δtk2)
k 4 = f ( t n + Δ t , x n + Δ t ⋅ k 3 ) k_4 = f(t_n + \Delta t, x_n + \Delta t\cdot k_3) k4=f(tn+Δt,xn+Δt⋅k3)
不同的斜率有以下解释:
- k 1 k_1 k1是区间开始时的斜率,使用 x n x_n xn(Euler方法);
- k 2 k_2 k2是区间中点的斜率,使用 x n + 1 2 Δ t ⋅ k 1 x_n + \frac{1}{2}\Delta t\cdot k_1 xn+21Δt⋅k1(中点法);
- k 3 k_3 k3再次是中点的斜率,但现在使用 x n + 1 2 Δ t ⋅ k 2 x_n + \frac{1}{2}\Delta t\cdot k_2 xn+21Δt⋅k2;
- k 4 k_4 k4是区间结束时的斜率,使用 x n + Δ t ⋅ k 3 x_n + \Delta t\cdot k_3 xn+Δt⋅k3。
代码示例:
def rk4(f, x0, t0, dt, N):
"""RK4方法
Args:
f: 函数 f(t, x)
x0: 初始状态
t0: 初始时间
dt: 步长
N: 迭代次数
Returns:
ts: 时间点数组
xs: 状态数组
"""
ts = [t0]
xs = [x0]
for i in range(N):
x = xs[-1]
t = ts[-1]
k1 = f(t, x)
k2 = f(t + dt/2, x + dt/2*k1)
k3 = f(t + dt/2, x + dt/2*k2)
k4 = f(t + dt, x + dt*k3)
x_new = x + dt/6 * (k1 + 2*k2 + 2*k3 + k4)
t_new = t + dt
xs.append(x_new)
ts.append(t_new)
return ts, xs
1.4 通用Runge-Kutta方法
可以设计出更精细的RK方法。它们的目标是要么减少误差,要么提高稳定性。它们采用通用形式:
x n + 1 = x n + Δ t ∑ i = 1 s b i k i x_{n+1} = x_n + \Delta t\sum_{i=1}^{s}b_i k_i xn+1=xn+Δti=1∑sbiki
其中:
k i = f ( t n + Δ t ⋅ c i , x n + Δ t ∑ j = 1 s a i j k j ) k_i = f\left(t_n + \Delta t\cdot c_i, x_n + \Delta t\sum_{j=1}^{s}a_{ij}k_j\right) ki=f(tn+Δt⋅ci,xn+Δtj=1∑saijkj)
也就是说,迭代次数(方法的阶数)为 s s s,平均权重由 b i b_i bi定义,评估时间点由 c i c_i ci定义,斜率 k i k_i ki使用值 a i j a_{ij} aij确定。根据 a i j a_{ij} aij项的结构,可以有显式或隐式的RK方法。
-
在显式方法中,所有 k i k_i ki都是按顺序计算的,即只使用之前计算的值。这意味着矩阵 [ a i j ] [a_{ij}] [aij]是下三角且对角线元素为零(即对于 j ≥ i j \geq i j≥i有 a i j = 0 a_{ij} = 0 aij=0)。Euler、中点和RK4方法都是显式的。
-
隐式方法具有完整的 [ a i j ] [a_{ij}] [aij]矩阵,需要求解线性方程组才能确定所有 k i k_i ki。因此计算成本更高,但与显式方法相比,它们能够提高精度和稳定性。
2.闭式积分方法
在很多情况下,可以得到积分步骤的闭式表达式。现在我们考虑一阶线性微分方程的情况:
x ˙ ( t ) = A ⋅ x ( t ) \dot{x}(t) = A\cdot x(t) x˙(t)=A⋅x(t)
也就是说,关系是线性的,在区间内是常数。在这种情况下,在区间 [ t n , t n + Δ t ] [t_n, t_n + \Delta t] [tn,tn+Δt]上积分得到:
x n + 1 = e A ⋅ Δ t x n = Φ x n x_{n+1} = e^{A\cdot \Delta t}x_n = \Phi x_n xn+1=eA⋅Δtxn=Φxn
其中 Φ \Phi Φ称为转移矩阵。该转移矩阵的Taylor展开为:
Φ = e A ⋅ Δ t = I + A Δ t + 1 2 A 2 Δ t 2 + 1 3 ! A 3 Δ t 3 + ⋯ = ∑ k = 0 ∞ 1 k ! A k Δ t k \Phi = e^{A\cdot \Delta t} = I + A\Delta t + \frac{1}{2}A^2 \Delta t^2 + \frac{1}{3!}A^3 \Delta t^3 + \cdots = \sum_{k=0}^{\infty}\frac{1}{k!}A^k \Delta t^k Φ=eA⋅Δt=I+AΔt+21A2Δt2+3!1A3Δt3+⋯=k=0∑∞k!1AkΔtk
当为已知 A A A的实例编写此级数时,有时可以在结果中识别已知级数。这允许以闭式形式写出所得积分。下面给出几个示例。
2.1 角误差的积分
例如,考虑没有偏差和噪声的角误差动力学:
δ θ ˙ = − [ ω ] × δ θ \dot{\delta\theta} = -[\omega]_\times \delta\theta δθ˙=−[ω]×δθ
它的转移矩阵可以写成Taylor级数:
Φ = e − [ ω ] × Δ t \Phi = e^{-[\omega]_\times \Delta t} Φ=e−[ω]×Δt
= I − [ ω ] × Δ t + 1 2 [ ω ] × 2 Δ t 2 − 1 3 ! [ ω ] × 3 Δ t 3 + 1 4 ! [ ω ] × 4 Δ t 4 − ⋯ = I - [\omega]_\times \Delta t + \frac{1}{2}[\omega]_\times^2\Delta t^2 - \frac{1}{3!}[\omega]_\times^3 \Delta t^3 + \frac{1}{4!}[\omega]_\times^4 \Delta t^4 - \cdots =I−[ω]×Δt+21[ω]×2Δt2−3!1[ω]×3Δt3+4!1[ω]×4Δt4−⋯
现在定义 ω Δ t ≡ u Δ θ \omega\Delta t \equiv u\Delta\theta ωΔt≡uΔθ,即旋转轴和旋转角,并应用式(75),我们可以对项进行分组,得到:
Φ = I − [ u ] × Δ θ + 1 2 [ u ] × 2 Δ θ 2 − 1 3 ! [ u ] × 3 Δ θ 3 + 1 4 ! [ u ] × 4 Δ θ 4 − ⋯ = I − [ u ] × ( Δ θ − Δ θ 3 3 ! + Δ θ 5 5 ! − ⋯ ) + [ u ] × 2 ( Δ θ 2 2 ! − Δ θ 4 4 ! + Δ θ 6 6 ! − ⋯ ) = I − [ u ] × sin Δ θ + [ u ] × 2 ( 1 − cos Δ θ ) \begin{aligned} \Phi &= I - [u]_\times \Delta\theta + \frac{1}{2}[u]_\times^2\Delta\theta^2 - \frac{1}{3!}[u]_\times^3 \Delta \theta^3 + \frac{1}{4!}[u]_\times^4 \Delta\theta^4 - \cdots\\ &= I - [u]_\times (\Delta\theta - \frac{\Delta\theta^3}{3!} + \frac{\Delta\theta^5}{5!} - \cdots) + [u]_\times^2 (\frac{\Delta\theta^2}{2!} - \frac{\Delta\theta^4}{4!} + \frac{\Delta\theta^6}{6!} - \cdots)\\ &= I - [u]_\times \sin\Delta\theta + [u]_\times^2(1 - \cos\Delta\theta) \end{aligned} Φ=I−[u]×Δθ+21[u]×2Δθ2−3!1[u]×3Δθ3+4!1[u]×4Δθ4−⋯=I−[u]×(Δθ−3!Δθ3+5!Δθ5−⋯)+[u]×2(2!Δθ2−4!Δθ4+6!Δθ6−⋯)=I−[u]×sinΔθ+[u]×2(1−cosΔθ)
这是一个闭式解。
该解对应于一个旋转矩阵 Φ = R { − u Δ θ } = R { ω Δ t } ⊤ \Phi = R\{-u\Delta\theta\}=R\{\omega\Delta t\}^\top Φ=R{−uΔθ}=R{ωΔt}⊤,根据Rodrigues旋转公式(77),可以通过直接检查式(346)并回忆式(69)来获得这一结果。因此,让我们将此作为最终闭式结果:
Φ = R { ω Δ t } ⊤ \Phi = R\{\omega\Delta t\}^\top Φ=R{ωΔt}⊤
2.2 简化IMU示例
考虑由以下误差状态动力学控制的简化IMU驱动系统:
δ p ˙ = δ v δ v ˙ = − R [ a ] × δ θ δ θ ˙ = − [ ω ] × δ θ \begin{aligned} \dot{\delta p} &= \delta v\\ \dot{\delta v} &= -R[a]_\times \delta\theta\\ \dot{\delta\theta} &= -[\omega]_\times \delta\theta \end{aligned} δp˙δv˙δθ˙=δv=−R[a]×δθ=−[ω]×δθ
其中 ( a , ω ) (a, \omega) (a,ω)是IMU读数,我们省略了重力和传感器偏差。该系统由状态向量和动态矩阵定义:
x = [ δ p δ v δ θ ] , A = [ 0 P v 0 0 0 V θ 0 0 Θ θ ] x = \begin{bmatrix} \delta p\\ \delta v\\ \delta\theta \end{bmatrix},\quad A = \begin{bmatrix} 0 & P_v & 0\\ 0 & 0 & V_\theta\\ 0 & 0 & \Theta_\theta \end{bmatrix} x= δpδvδθ ,A= 000Pv000VθΘθ
其中:
P v = I V θ = − R [ a ] × Θ θ = − [ ω ] × \begin{aligned} P_v &= I\\ V_\theta &= -R[a]_\times\\ \Theta_\theta &= -[\omega]_\times \end{aligned} PvVθΘθ=I=−R[a]×=−[ω]×
对采样周期 Δ t \Delta t Δt的积分为 x n + 1 = e ( A Δ t ) ⋅ x n = Φ ⋅ x n x_{n+1} = e^{(A\Delta t)}\cdot x_n = \Phi\cdot x_n xn+1=e(AΔt)⋅xn=Φ⋅xn。转移矩阵 Φ \Phi Φ可以用Taylor展开(344)来表示,以 A Δ t A\Delta t AΔt的递增幂次表示。我们可以写出 A A A的几个幂次来说明它们的一般形式:
A = [ 0 P v 0 0 0 V θ 0 0 Θ θ ] , A 2 = [ 0 0 P v V θ 0 0 V θ Θ θ 0 0 Θ θ 2 ] A 3 = [ 0 0 P v V θ Θ θ 0 0 V θ Θ θ 2 0 0 Θ θ 3 ] , A 4 = [ 0 0 P v V θ Θ θ 2 0 0 V θ Θ θ 3 0 0 Θ θ 4 ] \begin{aligned} A &= \begin{bmatrix} 0 & P_v & 0\\ 0 & 0 & V_\theta\\ 0 & 0 & \Theta_\theta \end{bmatrix}, \quad A^2 = \begin{bmatrix} 0 & 0 & P_v V_\theta\\ 0 & 0 & V_\theta \Theta_\theta\\ 0 & 0 & \Theta_\theta^2 \end{bmatrix}\\ A^3 &= \begin{bmatrix} 0 & 0 & P_v V_\theta \Theta_\theta\\ 0 & 0 & V_\theta \Theta_\theta^2\\ 0 & 0 & \Theta_\theta^3 \end{bmatrix}, \quad A^4 = \begin{bmatrix} 0 & 0 & P_v V_\theta \Theta_\theta^2\\ 0 & 0 & V_\theta \Theta_\theta^3\\ 0 & 0 & \Theta_\theta^4 \end{bmatrix} \end{aligned} AA3= 000Pv000VθΘθ ,A2= 000000PvVθVθΘθΘθ2 = 000000PvVθΘθVθΘθ2Θθ3 ,A4= 000000PvVθΘθ2VθΘθ3Θθ4
从中我们可以观察到,对于 k > 1 k > 1 k>1有:
A k > 1 = [ 0 0 P v V θ Θ θ k − 2 0 0 V θ Θ θ k − 1 0 0 Θ θ k ] A^{k>1} = \begin{bmatrix} 0 & 0 & P_v V_\theta \Theta_\theta^{k-2}\\ 0 & 0 & V_\theta \Theta_\theta^{k-1}\\ 0 & 0 & \Theta_\theta^k \end{bmatrix} Ak>1= 000000PvVθΘθk−2VθΘθk−1Θθk
我们可以观察到 A A A递增幂次中的项具有固定部分和 Θ θ \Theta_\theta Θθ的递增幂次。这些幂次可能导致闭式解,就像前一节中那样。
让我们将矩阵 Φ \Phi Φ划分如下:
Φ = [ I Φ p v Φ p θ 0 I Φ v θ 0 0 Φ θ θ ] \Phi = \begin{bmatrix} I & \Phi_{pv} & \Phi_{p\theta}\\ 0 & I & \Phi_{v\theta}\\ 0 & 0 & \Phi_{\theta\theta} \end{bmatrix} Φ= I00ΦpvI0ΦpθΦvθΦθθ
让我们逐步探索 Φ \Phi Φ的所有非零块。
平凡的对角线项 从对角线上的两个上方项开始,它们如图所示是单位矩阵。
旋转对角线项 接下来是旋转对角线项 Φ θ θ \Phi_{\theta\theta} Φθθ,它将新的角度误差与旧的角度误差相关联。为该项写出完整的Taylor级数导致:
Φ θ θ = ∑ k = 0 ∞ 1 k ! Θ θ k Δ t k = ∑ k = 0 ∞ 1 k ! [ − ω ] × k Δ t k = R { ω Δ t } ⊤ \begin{aligned} \Phi_{\theta\theta} &= \sum_{k=0}^{\infty}\frac{1}{k!}\Theta_\theta^k \Delta t^k = \sum_{k=0}^{\infty}\frac{1}{k!}[-\omega]_\times^k \Delta t^k\\ &= R\{\omega\Delta t\}^\top \end{aligned} Φθθ=k=0∑∞k!1ΘθkΔtk=k=0∑∞k!1[−ω]×kΔtk=R{ωΔt}⊤
正如我们在上一节中看到的,这对应于我们熟知的旋转矩阵。
位置对速度项 最简单的非对角线项是 Φ p v \Phi_{pv} Φpv,它是:
Φ p v = P v Δ t = I Δ t \Phi_{pv} = P_v \Delta t = I\Delta t Φpv=PvΔt=IΔt
速度对角度项 现在让我们转到 Φ v θ \Phi_{v\theta} Φvθ项,通过写出其级数:
Φ v θ = V θ Δ t + 1 2 V θ Θ θ Δ t 2 + 1 3 ! V θ Θ θ 2 Δ t 3 + ⋯ = Δ t V θ ( I + 1 2 Θ θ Δ t + 1 3 ! Θ θ 2 Δ t 2 + ⋯ ) \begin{aligned} \Phi_{v\theta} &= V_\theta \Delta t + \frac{1}{2}V_\theta \Theta_\theta \Delta t^2 + \frac{1}{3!}V_\theta \Theta_\theta^2 \Delta t^3 + \cdots\\ &= \Delta t V_\theta (I + \frac{1}{2}\Theta_\theta \Delta t + \frac{1}{3!}\Theta_\theta^2 \Delta t^2 + \cdots) \end{aligned} Φvθ=VθΔt+21VθΘθΔt2+3!1VθΘθ2Δt3+⋯=ΔtVθ(I+21ΘθΔt+3!1Θθ2Δt2+⋯)
这可以化简为:
Φ v θ = Δ t V θ ( I + ∑ k ≥ 1 ( Θ θ Δ t ) k ( k + 1 ) ! ) \Phi_{v\theta} = \Delta t V_\theta \left(I + \sum_{k\geq 1}\frac{(\Theta_\theta \Delta t)^k}{(k+1)!}\right) Φvθ=ΔtVθ(I+k≥1∑(k+1)!(ΘθΔt)k)
此时我们有两个选择。我们可以在第一个显著项处截断级数,得到 Φ v θ = V θ Δ t \Phi_{v\theta} = V_\theta \Delta t Φvθ=VθΔt,但这不是闭式的。下一节将给出使用这种简化方法的结果。或者,让我们将 V θ V_\theta Vθ分离出来并写出:
Φ v θ = V θ Σ 1 \Phi_{v\theta} = V_\theta \Sigma_1 Φvθ=VθΣ1
其中:
Σ 1 = I Δ t + 1 2 Θ θ Δ t 2 + 1 3 ! Θ θ 2 Δ t 3 + ⋯ \Sigma_1 = I\Delta t + \frac{1}{2}\Theta_\theta \Delta t^2 + \frac{1}{3!}\Theta_\theta^2 \Delta t^3 + \cdots Σ1=IΔt+21ΘθΔt2+3!1Θθ2Δt3+⋯
级数 Σ 1 \Sigma_1 Σ1与我们为 Φ θ θ \Phi_{\theta\theta} Φθθ写的级数(358)类似,但有两个例外:
-
Σ 1 \Sigma_1 Σ1中的 Θ θ \Theta_\theta Θθ幂次与 1 k ! \frac{1}{k!} k!1的有理系数和 Δ t \Delta t Δt的幂次不匹配。实际上,这里我们注意到下标"1"表示每个成员缺少一个 Θ θ \Theta_\theta Θθ幂次。
-
级数开头的一些项丢失了。同样,下标"1"表示缺少这样一项。
产生了恒等式:
Θ θ = [ ω ] × 3 ∥ ω ∥ 2 = − Θ θ 3 ∥ ω ∥ 2 \Theta_\theta = \frac{[\omega]_\times^3}{\|\omega\|^2} = \frac{-\Theta_\theta^3}{\|\omega\|^2} Θθ=∥ω∥2[ω]×3=∥ω∥2−Θθ3
这个表达式允许我们将级数中 Θ θ \Theta_\theta Θθ的指数增加两次,如果 ω ≠ 0 \omega \neq 0 ω=0,可以写成:
Σ 1 = I Δ t − Θ θ ∥ ω ∥ 2 ( 1 2 Θ θ 2 Δ t 2 + 1 3 ! Θ θ 3 Δ t 3 + ⋯ ) \Sigma_1 = I\Delta t - \frac{\Theta_\theta}{\|\omega\|^2}\left(\frac{1}{2}\Theta_\theta^2 \Delta t^2 + \frac{1}{3!}\Theta_\theta^3 \Delta t^3 + \cdots\right) Σ1=IΔt−∥ω∥2Θθ(21Θθ2Δt2+3!1Θθ3Δt3+⋯)
否则 Σ 1 = I Δ t \Sigma_1 = I\Delta t Σ1=IΔt。新级数中的所有幂次都与正确的系数匹配。当然,如前所述,有些项丢失了。第二个问题可以通过添加和减去缺失的项,并将不完整的级数替换为其闭式来解决。我们得到:
Σ 1 = I Δ t − Θ θ ∥ ω ∥ 2 ( R { ω Δ t } ⊤ − I − Θ θ Δ t ) \Sigma_1 = I\Delta t - \frac{\Theta_\theta}{\|\omega\|^2}\left(R\{\omega\Delta t\}^\top - I - \Theta_\theta \Delta t\right) Σ1=IΔt−∥ω∥2Θθ(R{ωΔt}⊤−I−ΘθΔt)
这是一个闭式解,如果 ω ≠ 0 \omega \neq 0 ω=0则有效。因此我们最终可以写出:
Φ v θ = { − R [ a ] × Δ t ω → 0 − R [ a ] × ( I Δ t + [ ω ] × ∥ ω ∥ 2 ( R { ω Δ t } ⊤ − I + [ ω ] × Δ t ) ) ω ≠ 0 \Phi_{v\theta} = \begin{cases} -R[a]_\times \Delta t & \omega \to 0\\ -R[a]_\times \left(I\Delta t + \frac{[\omega]_\times}{\|\omega\|^2}\left(R\{\omega\Delta t\}^\top - I + [\omega]_\times \Delta t\right)\right) & \omega \neq 0 \end{cases} Φvθ={−R[a]×Δt−R[a]×(IΔt+∥ω∥2[ω]×(R{ωΔt}⊤−I+[ω]×Δt))ω→0ω=0
位置对角度项 最后让我们研究 Φ p θ \Phi_{p\theta} Φpθ项。它的Taylor级数为:
Φ p θ = 1 2 P v V θ Δ t 2 + 1 3 ! P v V θ Θ θ Δ t 3 + 1 4 ! P v V θ Θ θ 2 Δ t 4 + ⋯ \Phi_{p\theta} = \frac{1}{2}P_v V_\theta \Delta t^2 + \frac{1}{3!}P_v V_\theta \Theta_\theta \Delta t^3 + \frac{1}{4!}P_v V_\theta \Theta_\theta^2 \Delta t^4 + \cdots Φpθ=21PvVθΔt2+3!1PvVθΘθΔt3+4!1PvVθΘθ2Δt4+⋯
我们将常数项分离出来得到:
Φ p θ = P v V θ Σ 2 \Phi_{p\theta} = P_v V_\theta \Sigma_2 Φpθ=PvVθΣ2
其中:
Σ 2 = 1 2 I Δ t 2 + 1 3 ! Θ θ Δ t 3 + 1 4 ! Θ θ 2 Δ t 4 + ⋯ \Sigma_2 = \frac{1}{2}I\Delta t^2 + \frac{1}{3!}\Theta_\theta \Delta t^3 + \frac{1}{4!}\Theta_\theta^2 \Delta t^4 + \cdots Σ2=21IΔt2+3!1ΘθΔt3+4!1Θθ2Δt4+⋯
其中我们注意到下标"2"在 Σ 2 \Sigma_2 Σ2中有以下解释:
- 在级数的每一项中都缺少两个 Θ θ \Theta_\theta Θθ幂次,
- 级数的前两项丢失。
再次使用式(366)来增加 Θ θ \Theta_\theta Θθ的指数,得到:
Σ 2 = 1 2 I Δ t 2 − 1 ∥ ω ∥ 2 ( 1 3 ! Θ θ 3 Δ t 3 + 1 4 ! Θ θ 4 Δ t 4 + ⋯ ) \Sigma_2 = \frac{1}{2}I\Delta t^2 - \frac{1}{\|\omega\|^2}\left(\frac{1}{3!}\Theta_\theta^3 \Delta t^3 + \frac{1}{4!}\Theta_\theta^4 \Delta t^4 + \cdots\right) Σ2=21IΔt2−∥ω∥21(3!1Θθ3Δt3+4!1Θθ4Δt4+⋯)
我们将不完整的级数替换为其闭式,得到:
Σ 2 = 1 2 I Δ t 2 − 1 ∥ ω ∥ 2 ( R { ω Δ t } ⊤ − I − Θ θ Δ t − 1 2 Θ θ 2 Δ t 2 ) \Sigma_2 = \frac{1}{2}I\Delta t^2 - \frac{1}{\|\omega\|^2}\left(R\{\omega\Delta t\}^\top - I - \Theta_\theta \Delta t - \frac{1}{2}\Theta_\theta^2 \Delta t^2\right) Σ2=21IΔt2−∥ω∥21(R{ωΔt}⊤−I−ΘθΔt−21Θθ2Δt2)
这导致最终结果:
Φ p θ = { − R [ a ] × Δ t 2 2 ω → 0 − R [ a ] × ( 1 2 I Δ t 2 − 1 ∥ ω ∥ 2 ( R { ω Δ t } ⊤ − ∑ k = 0 2 ( − [ ω ] × Δ t ) k k ! ) ) ω ≠ 0 \Phi_{p\theta} = \begin{cases} -\frac{R[a]_\times \Delta t^2}{2} & \omega \to 0\\ -R[a]_\times \left(\frac{1}{2}I\Delta t^2 - \frac{1}{\|\omega\|^2}\left(R\{\omega\Delta t\}^\top - \sum_{k=0}^{2}\frac{(-[\omega]_\times \Delta t)^k}{k!}\right)\right) & \omega \neq 0 \end{cases} Φpθ=⎩ ⎨ ⎧−2R[a]×Δt2−R[a]×(21IΔt2−∥ω∥21(R{ωΔt}⊤−∑k=02k!(−[ω]×Δt)k))ω→0ω=0
2.3 完整IMU示例
为了给出推广前一示例中所述方法的手段,我们需要从更近的距离检查完整IMU情况。
考虑完整IMU系统,它可以写成:
δ x ˙ = A δ x + B w \dot{\delta x} = A\delta x + Bw δx˙=Aδx+Bw
其离散时间积分需要转移矩阵:
Φ = ∑ k = 0 ∞ 1 k ! A k Δ t k = I + A Δ t + 1 2 A 2 Δ t 2 + ⋯ \Phi = \sum_{k=0}^{\infty}\frac{1}{k!}A^k \Delta t^k = I + A\Delta t + \frac{1}{2}A^2 \Delta t^2 + \cdots Φ=k=0∑∞k!1AkΔtk=I+AΔt+21A2Δt2+⋯
我们想要计算它。动态矩阵 A A A是块稀疏的,通过检查原始方程可以很容易地确定它的块:
A = [ 0 P v 0 0 0 0 0 0 V θ V a 0 V g 0 0 Θ θ 0 Θ ω 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ] A = \begin{bmatrix} 0 & P_v & 0 & 0 & 0 & 0\\ 0 & 0 & V_\theta & V_a & 0 & V_g\\ 0 & 0 & \Theta_\theta & 0 & \Theta_\omega & 0\\ 0 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 0 \end{bmatrix} A= 000000Pv000000VθΘθ0000Va000000Θω0000Vg0000
和之前一样,让我们写出 A A A的几个幂次:
A 2 = [ 0 0 P v V θ P v V a 0 P v V g 0 0 V θ Θ θ 0 V θ Θ ω 0 0 0 Θ θ 2 0 Θ θ Θ ω 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ] A 3 = [ 0 0 P v V θ Θ θ 0 P v V θ Θ ω 0 0 0 V θ Θ θ 2 0 V θ Θ θ Θ ω 0 0 0 Θ θ 3 0 Θ θ 2 Θ ω 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ] A 4 = [ 0 0 P v V θ Θ θ 2 0 P v V θ Θ θ Θ ω 0 0 0 V θ Θ θ 3 0 V θ Θ θ 2 Θ ω 0 0 0 Θ θ 4 0 Θ θ 3 Θ ω 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ] \begin{aligned} A^2 &= \begin{bmatrix} 0 & 0 & P_v V_\theta & P_v V_a & 0 & P_v V_g\\ 0 & 0 & V_\theta \Theta_\theta & 0 & V_\theta \Theta_\omega & 0\\ 0 & 0 & \Theta_\theta^2 & 0 & \Theta_\theta \Theta_\omega & 0\\ 0 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 0 \end{bmatrix}\\ A^3 &= \begin{bmatrix} 0 & 0 & P_v V_\theta \Theta_\theta & 0 & P_v V_\theta \Theta_\omega & 0\\ 0 & 0 & V_\theta \Theta_\theta^2 & 0 & V_\theta \Theta_\theta \Theta_\omega & 0\\ 0 & 0 & \Theta_\theta^3 & 0 & \Theta_\theta^2 \Theta_\omega & 0\\ 0 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 0 \end{bmatrix}\\ A^4 &= \begin{bmatrix} 0 & 0 & P_v V_\theta \Theta_\theta^2 & 0 & P_v V_\theta \Theta_\theta \Theta_\omega & 0\\ 0 & 0 & V_\theta \Theta_\theta^3 & 0 & V_\theta \Theta_\theta^2 \Theta_\omega & 0\\ 0 & 0 & \Theta_\theta^4 & 0 & \Theta_\theta^3 \Theta_\omega & 0\\ 0 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 0 \end{bmatrix} \end{aligned} A2A3A4= 000000000000PvVθVθΘθΘθ2000PvVa000000VθΘωΘθΘω000PvVg00000 = 000000000000PvVθΘθVθΘθ2Θθ3000000000PvVθΘωVθΘθΘωΘθ2Θω000000000 = 000000000000PvVθΘθ2VθΘθ3Θθ4000000000PvVθΘθΘωVθΘθ2ΘωΘθ3Θω000000000
基本上,我们观察到以下几点:
-
A A A对角线上的唯一项,即旋转项 Θ θ \Theta_\theta Θθ,在 A k A^k Ak序列中向右和向上传播。所有不受这种传播影响的项都消失了。这种传播在以下三个方面影响 { A k } \{A^k\} {Ak}序列的结构:
-
从第3次幂开始, A A A幂次的稀疏性就稳定下来了。也就是说,对于高于3的 A A A幂次,不再出现或消失非零块。
-
左上角的 3 × 3 3\times 3 3×3块,对应于前一示例中的简化IMU模型,与该示例相比没有变化。因此,之前为它开发的闭式解仍然成立。- 与陀螺仪偏差误差相关的项(第五列的那些)引入了 Θ θ \Theta_\theta Θθ幂次的类似级数,可以用我们在简化示例中使用的相同技术来求解。
此时,我们有兴趣找到构造转移矩阵 Φ \Phi Φ的闭式元素的通用方法。让我们回顾一下关于级数 Σ 1 \Sigma_1 Σ1和 Σ 2 \Sigma_2 Σ2的评论:
- 下标与每个级数成员中缺少的 Θ θ \Theta_\theta Θθ幂次一致。
- 下标与级数开头丢失的项数一致。
考虑到这些性质,让我们引入级数 Σ n ( X , y ) \Sigma_n(X, y) Σn(X,y),定义为:
Σ n ( X , y ) ≡ ∑ k = n ∞ 1 k ! X k − n y k = ∑ k = 0 ∞ 1 ( k + n ) ! X k y k + n = y n ∑ k = 0 ∞ 1 ( k + n ) ! X k y k \Sigma_n(X, y) \equiv \sum_{k=n}^{\infty}\frac{1}{k!}X^{k-n}y^k = \sum_{k=0}^{\infty}\frac{1}{(k+n)!}X^k y^{k+n} = y^n \sum_{k=0}^{\infty}\frac{1}{(k+n)!}X^k y^k Σn(X,y)≡k=n∑∞k!1Xk−nyk=k=0∑∞(k+n)!1Xkyk+n=ynk=0∑∞(k+n)!1Xkyk
其中求和从第 n n n项开始,项缺少 n n n个矩阵 X X X的幂次。立即可以看出 Σ 1 \Sigma_1 Σ1和 Σ 2 \Sigma_2 Σ2满足:
Σ n = Σ n ( Θ θ , Δ t ) \Sigma_n = \Sigma_n(\Theta_\theta, \Delta t) Σn=Σn(Θθ,Δt)
并且 Σ 0 = R { ω Δ t } ⊤ \Sigma_0 = R\{\omega\Delta t\}^\top Σ0=R{ωΔt}⊤。我们现在可以将转移矩阵(377)写成这些级数的函数:
Φ = [ I P v Δ t P v V θ Σ 2 1 2 P v V a Δ t 2 P v V θ Σ 3 , θ ω 1 2 P v V g Δ t 2 0 I V θ Σ 1 V a Δ t V θ Σ 2 , θ ω V g Δ t 0 0 Σ 0 0 Σ 1 , θ ω 0 0 0 0 I 0 0 0 0 0 0 I 0 0 0 0 0 0 I ] \Phi = \begin{bmatrix} I & P_v \Delta t & P_v V_\theta \Sigma_2 & \frac{1}{2}P_v V_a \Delta t^2 & P_v V_\theta \Sigma_{3, \theta\omega} & \frac{1}{2}P_v V_g \Delta t^2\\ 0 & I & V_\theta \Sigma_1 & V_a \Delta t & V_\theta \Sigma_{2,\theta\omega} & V_g \Delta t\\ 0 & 0 & \Sigma_0 & 0 & \Sigma_{1,\theta\omega} & 0\\ 0 & 0 & 0 & I & 0 & 0\\ 0 & 0 & 0 & 0 & I & 0\\ 0 & 0 & 0 & 0 & 0 & I \end{bmatrix} Φ= I00000PvΔtI0000PvVθΣ2VθΣ1Σ000021PvVaΔt2VaΔt0I00PvVθΣ3,θωVθΣ2,θωΣ1,θω0I021PvVgΔt2VgΔt000I
我们的问题现在导出为找到 Σ n \Sigma_n Σn的通用闭式表达式的问题。让我们观察到目前为止得到的 Σ 0 … Σ 3 \Sigma_0 \dots \Sigma_3 Σ0…Σ3的闭式结果:
Σ 0 = R { ω Δ t } ⊤ Σ 1 = I Δ t − Θ θ ∥ ω ∥ 2 ( R { ω Δ t } ⊤ − I − Θ θ Δ t ) Σ 2 = 1 2 I Δ t 2 − 1 ∥ ω ∥ 2 ( R { ω Δ t } ⊤ − I − Θ θ Δ t − 1 2 Θ θ 2 Δ t 2 ) Σ 3 = 1 3 ! I Δ t 3 + Θ θ ∥ ω ∥ 4 ( R { ω Δ t } ⊤ − I − Θ θ Δ t − 1 2 Θ θ 2 Δ t 2 − 1 3 ! Θ θ 3 Δ t 3 ) \begin{aligned} \Sigma_0 &= R\{\omega\Delta t\}^\top\\ \Sigma_1 &= I\Delta t - \frac{\Theta_\theta}{\|\omega\|^2}\left(R\{\omega\Delta t\}^\top - I - \Theta_\theta \Delta t\right)\\ \Sigma_2 &= \frac{1}{2}I\Delta t^2 - \frac{1}{\|\omega\|^2}\left(R\{\omega\Delta t\}^\top - I - \Theta_\theta \Delta t - \frac{1}{2}\Theta_\theta^2 \Delta t^2\right)\\ \Sigma_3 &= \frac{1}{3!}I\Delta t^3 + \frac{\Theta_\theta}{\|\omega\|^4}\left(R\{\omega\Delta t\}^\top - I - \Theta_\theta \Delta t - \frac{1}{2}\Theta_\theta^2 \Delta t^2 - \frac{1}{3!}\Theta_\theta^3 \Delta t^3\right) \end{aligned} Σ0Σ1Σ2Σ3=R{ωΔt}⊤=IΔt−∥ω∥2Θθ(R{ωΔt}⊤−I−ΘθΔt)=21IΔt2−∥ω∥21(R{ωΔt}⊤−I−ΘθΔt−21Θθ2Δt2)=3!1IΔt3+∥ω∥4Θθ(R{ωΔt}⊤−I−ΘθΔt−21Θθ2Δt2−3!1Θθ3Δt3)
通过仔细检查 Σ 0 … Σ 3 \Sigma_0 \dots \Sigma_3 Σ0…Σ3的级数,我们现在可以为 Σ n \Sigma_n Σn导出通用的闭式表达式,如下所示:
Σ n = { 1 n ! I Δ t n ω → 0 R { ω Δ t } ⊤ n = 0 1 n ! I Δ t n − ( − 1 ) n + 1 2 [ ω ] × ∥ ω ∥ n + 1 ( R { ω Δ t } ⊤ − ∑ k = 0 n ( − [ ω ] × Δ t ) k k ! ) n odd 1 n ! I Δ t n + ( − 1 ) n 2 ∥ ω ∥ n ( R { ω Δ t } ⊤ − ∑ k = 0 n ( − [ ω ] × Δ t ) k k ! ) n even \Sigma_n = \begin{cases} \frac{1}{n!}I\Delta t^n & \omega \to 0\\ R\{\omega\Delta t\}^\top & n = 0\\ \frac{1}{n!}I\Delta t^n - \frac{(-1)^\frac{n+1}{2}[\omega]_\times}{\|\omega\|^{n+1}}\left(R\{\omega\Delta t\}^\top - \sum_{k=0}^{n}\frac{(-[\omega]_\times \Delta t)^k}{k!}\right) & n \text{ odd}\\ \frac{1}{n!}I\Delta t^n + \frac{(-1)^\frac{n}{2}}{\|\omega\|^n}\left(R\{\omega\Delta t\}^\top - \sum_{k=0}^{n}\frac{(-[\omega]_\times \Delta t)^k}{k!}\right) & n \text{ even} \end{cases} Σn=⎩ ⎨ ⎧n!1IΔtnR{ωΔt}⊤n!1IΔtn−∥ω∥n+1(−1)2n+1[ω]×(R{ωΔt}⊤−∑k=0nk!(−[ω]×Δt)k)n!1IΔtn+∥ω∥n(−1)2n(R{ωΔt}⊤−∑k=0nk!(−[ω]×Δt)k)ω→0n=0n oddn even
Φ \Phi Φ的最终结果可以通过将 Σ n , n ∈ { 0 , 1 , 2 , 3 } \Sigma_n, n \in \{0,1,2,3\} Σn,n∈{0,1,2,3}的适当值代入(381)中的相应位置立即得到。
值得注意的是,现在出现在这些新表达式中的级数具有有限个项,因此可以有效计算。也就是说,只要 n < ∞ n < \infty n<∞, Σ n \Sigma_n Σn的表达式就是闭式的,而这在实际中总是成立的。对于当前示例,我们有 n ≤ 3 n \leq 3 n≤3
机器人IMU应用实例:
让我们考虑一个常见的机器人应用 - 使用IMU进行航位推算。假设我们有一个小型移动机器人,配备了一个六轴IMU(3个加速度计和3个陀螺仪)。我们的目标是使用IMU数据估计机器人的位置、速度和姿态。
我们可以设置如下误差状态系统:
δ p ˙ = δ v δ v ˙ = − R [ a ] × δ θ − R δ a b + δ g − R a n δ θ ˙ = − [ ω ] × δ θ − δ ω b − ω n δ a ˙ b = a w δ ω ˙ b = ω w δ g ˙ = 0 \begin{aligned} \delta\dot{p} &= \delta v\\ \delta\dot{v} &= -R[a]_\times \delta\theta - R\delta a_b + \delta g - Ra_n\\ \delta\dot{\theta} &= -[\omega]_\times \delta\theta - \delta\omega_b - \omega_n\\ \delta\dot{a}_b &= a_w\\ \delta\dot{\omega}_b &= \omega_w\\ \delta\dot{g} &= 0 \end{aligned} δp˙δv˙δθ˙δa˙bδω˙bδg˙=δv=−R[a]×δθ−Rδab+δg−Ran=−[ω]×δθ−δωb−ωn=aw=ωw=0
其中 δ p , δ v , δ θ \delta p, \delta v, \delta \theta δp,δv,δθ分别是位置、速度和姿态误差, δ a b , δ ω b \delta a_b, \delta \omega_b δab,δωb是加速度计和陀螺仪偏差误差, δ g \delta g δg是重力误差。 a n , ω n , a w , ω w a_n, \omega_n, a_w, \omega_w an,ωn,aw,ωw分别是加速度计噪声、陀螺仪噪声、加速度计偏差噪声和陀螺仪偏差噪声。
使用前面推导的结果,我们可以计算转移矩阵 Φ \Phi Φ并进行离散化:
δ x n + 1 = Φ n δ x n + F i ⋅ i \delta x_{n+1} = \Phi_n \delta x_n + F_i \cdot i δxn+1=Φnδxn+Fi⋅i
其中 δ x = [ δ p ⊤ , δ v ⊤ , δ θ ⊤ , δ a b ⊤ , δ ω b ⊤ , δ g ⊤ ] ⊤ \delta x = [\delta p^\top, \delta v^\top, \delta \theta^\top, \delta a_b^\top, \delta \omega_b^\top, \delta g^\top]^\top δx=[δp⊤,δv⊤,δθ⊤,δab⊤,δωb⊤,δg⊤]⊤是误差状态向量, i i i是过程噪声。
然后我们可以设计一个误差状态卡尔曼滤波器:
预测步骤:
δ
x
^
n
+
1
=
Φ
n
δ
x
^
n
P
n
+
1
=
Φ
n
P
n
Φ
n
⊤
+
F
i
Q
i
F
i
⊤
\begin{aligned} \hat{\delta x}_{n+1} &= \Phi_n \hat{\delta x}_n\\ P_{n+1} &= \Phi_n P_n \Phi_n^\top + F_i Q_i F_i^\top \end{aligned}
δx^n+1Pn+1=Φnδx^n=ΦnPnΦn⊤+FiQiFi⊤
更新步骤(假设有位置测量
y
n
y_n
yn):
K
n
=
P
n
H
⊤
(
H
P
n
H
⊤
+
R
)
−
1
δ
x
^
n
=
K
n
(
y
n
−
h
(
x
^
n
)
)
P
n
=
(
I
−
K
n
H
)
P
n
\begin{aligned} K_n &= P_n H^\top (H P_n H^\top + R)^{-1}\\ \hat{\delta x}_n &= K_n(y_n - h(\hat{x}_n))\\ P_n &= (I - K_n H) P_n \end{aligned}
Knδx^nPn=PnH⊤(HPnH⊤+R)−1=Kn(yn−h(x^n))=(I−KnH)Pn
其中 H H H是观测模型, R R R是观测噪声协方差。
最后,我们使用估计的误差状态来校正名义状态:
x ^ n ← x ^ n ⊕ δ x ^ n \hat{x}_n \leftarrow \hat{x}_n \oplus \hat{\delta x}_n x^n←x^n⊕δx^n
其中 ⊕ \oplus ⊕表示状态流形上的加法。
通过这种方式,我们可以递归地估计机器人的位置、速度和姿态,从而实现IMU的航位推算。闭式解和精确的离散化保证了估计器的精度和数值稳定性。
这只是IMU在机器人中应用的一个简单例子。实际系统通常会融合IMU与其他传感器如视觉、激光等,形成更鲁棒和准确的状态估计。但核心的状态传播部分通常与这里描述的类似。
综上,我们系统地推导了旋转矩阵和四元数的指数映射、闭式解和近似解,并讨论了它们在IMU积分和误差状态卡尔曼滤波中的应用。这些技术构成了许多现代机器人导航系统的基础。
这里我给出使用Python的代码示例,并解释角误差积分的用途和应用场景。
首先,让我们看一下角误差积分的Python实现:
import numpy as np
def exp_so3(phi):
"""李代数so3到李群SO3的指数映射。
Args:
phi: 角度误差向量, shape为(3,)。
Returns:
R: 旋转矩阵, shape为(3, 3)。
"""
phi_norm = np.linalg.norm(phi)
if phi_norm < 1e-8:
return np.eye(3)
else:
phi_skew = np.array([[0, -phi[2], phi[1]],
[phi[2], 0, -phi[0]],
[-phi[1], phi[0], 0]])
return np.eye(3) + np.sin(phi_norm)/phi_norm*phi_skew + (1-np.cos(phi_norm))/phi_norm**2*phi_skew@phi_skew
def right_jacobian_so3(phi):
"""SO3上的右雅可比矩阵。
Args:
phi: 角度误差向量, shape为(3,)。
Returns:
Jr: 右雅可比矩阵, shape为(3, 3)。
"""
phi_norm = np.linalg.norm(phi)
if phi_norm < 1e-8:
return np.eye(3)
else:
phi_skew = np.array([[0, -phi[2], phi[1]],
[phi[2], 0, -phi[0]],
[-phi[1], phi[0], 0]])
return np.eye(3) - (1-np.cos(phi_norm))/phi_norm**2*phi_skew + (phi_norm-np.sin(phi_norm))/phi_norm**3*phi_skew@phi_skew
在上面的代码中:
-
exp_so3函数实现了从so3到SO3的指数映射,即将角误差向量转换为旋转矩阵。它使用了罗德里格斯公式的闭式解。 -
right_jacobian_so3函数计算SO3流形上的右雅可比矩阵。该矩阵将局部角误差的变化映射到流形上旋转矩阵的变化。
这些函数在实现涉及3D旋转的状态估计器(如Extended Kalman Filter)时非常有用。
那么,什么时候需要用到角误差的积分呢?主要有以下几个场景:
-
姿态估计: 在使用IMU进行姿态估计时,我们通常会积分陀螺仪数据得到姿态的增量。然而,由于陀螺仪存在偏差和噪声,直接积分会导致估计误差快速增长。这时,我们可以在估计器中引入角误差状态,并使用如上所示的闭式解进行更新,以得到更准确和稳定的姿态估计。
-
视觉里程计: 在视觉里程计中,相机姿态通常用旋转矩阵或四元数表示。当我们优化姿态时,使用李代数so3作为局部参数化,可以避免过度参数化和奇异性问题。这时,需要在so3和SO3之间进行映射,也就是上面的指数映射和对数映射。
-
机器人运动学: 对于许多关节式机器人,其关节运动通常用旋转来描述。当我们进行运动学计算或控制时,需要在关节角速度(在so3中)和关节姿态(在SO3中)之间进行转换,这也需要用到指数映射。
-
轨迹优化: 在一些轨迹优化问题中,机器人的姿态也是需要优化的变量。为了保证优化过程中姿态的有效性,通常使用李代数参数化姿态,并在每次迭代后将优化结果投影回SO3。这个投影过程就是通过指数映射实现的。
好的,让我详细说明一下为什么引入角误差状态,并使用闭式解进行更新可以得到更准确和稳定的姿态估计。
Q&A 角误差理论到底是什么
我们知道IMU的陀螺仪测量的是角速度,要得到姿态需要对角速度进行积分。最简单的方法是使用欧拉积分:
R t + Δ t = R t exp ( ω t Δ t ) R_{t+\Delta t} = R_t \exp(\omega_t \Delta t) Rt+Δt=Rtexp(ωtΔt)
其中 R t R_t Rt是时刻 t t t的旋转矩阵, ω t \omega_t ωt是陀螺仪测量的角速度, Δ t \Delta t Δt是采样时间。
然而,这种直接积分的方法有以下问题:
- 陀螺仪存在偏差和噪声,直接积分会导致误差快速累积。
- 欧拉积分本身是一阶近似,也会引入数值误差。
- 旋转矩阵必须满足正交性约束,但数值积分无法保证积分结果满足此约束。
为了解决这些问题,我们引入角误差状态。假设我们有一个姿态的估计值 R ^ t \hat{R}_t R^t,它与真实值 R t R_t Rt之间存在一个小的误差 δ R t \delta R_t δRt:
R t = δ R t R ^ t R_t = \delta R_t \hat{R}_t Rt=δRtR^t
我们将 δ R t \delta R_t δRt参数化为一个李代数向量 ϕ t \phi_t ϕt:
δ R t = exp ( ϕ t ) \delta R_t = \exp(\phi_t) δRt=exp(ϕt)
这里的 exp \exp exp是指从so3到SO3的指数映射。
现在,我们的目标是估计误差状态 ϕ t \phi_t ϕt,而不是直接估计 R t R_t Rt。假设我们已经有了一个估计器(如EKF),它给出了 ϕ t \phi_t ϕt的估计值 ϕ ^ t \hat{\phi}_t ϕ^t和协方差矩阵 P t P_t Pt。
当新的陀螺仪测量值 ω t + 1 \omega_{t+1} ωt+1到来时,我们首先更新姿态的估计值:
R ^ t + 1 = R ^ t exp ( ω t + 1 Δ t ) \hat{R}_{t+1} = \hat{R}_t \exp(\omega_{t+1} \Delta t) R^t+1=R^texp(ωt+1Δt)
然后,我们使用角误差状态的闭式解来更新 ϕ ^ t \hat{\phi}_t ϕ^t和 P t P_t Pt:
ϕ ^ t + 1 = exp ( − ω t + 1 Δ t ) ϕ ^ t \hat{\phi}_{t+1} = \exp(-\omega_{t+1} \Delta t) \hat{\phi}_t ϕ^t+1=exp(−ωt+1Δt)ϕ^t
P t + 1 = F t P t F t ⊤ + Q t P_{t+1} = F_t P_t F_t^\top + Q_t Pt+1=FtPtFt⊤+Qt
其中 F t F_t Ft是状态转移矩阵,可以使用 exp ( ω t + 1 Δ t ) \exp(\omega_{t+1} \Delta t) exp(ωt+1Δt)的右雅可比矩阵来近似:
F t ≈ I − [ ω t + 1 ] × Δ t F_t \approx I - [\omega_{t+1}]_\times \Delta t Ft≈I−[ωt+1]×Δt
Q t Q_t Qt是过程噪声协方差矩阵。
最后,我们使用更新后的 ϕ ^ t + 1 \hat{\phi}_{t+1} ϕ^t+1来修正姿态估计值:
R ^ t + 1 ← exp ( ϕ ^ t + 1 ) R ^ t + 1 \hat{R}_{t+1} \leftarrow \exp(\hat{\phi}_{t+1}) \hat{R}_{t+1} R^t+1←exp(ϕ^t+1)R^t+1
这样,我们就得到了新的姿态估计值 R ^ t + 1 \hat{R}_{t+1} R^t+1。
下面是一个简单的Python实现:
import numpy as np
def attitude_estimation(R_prev, omega, dt, phi_prev, P_prev, Q):
"""使用角误差状态的姿态估计。
Args:
R_prev: 上一时刻的姿态估计值, shape为(3, 3)。
omega: 当前时刻的角速度测量值, shape为(3,)。
dt: 采样时间。
phi_prev: 上一时刻的角误差状态估计值, shape为(3,)。
P_prev: 上一时刻的角误差状态协方差矩阵, shape为(3, 3)。
Q: 过程噪声协方差矩阵, shape为(3, 3)。
Returns:
R_curr: 当前时刻的姿态估计值, shape为(3, 3)。
phi_curr: 当前时刻的角误差状态估计值, shape为(3,)。
P_curr: 当前时刻的角误差状态协方差矩阵, shape为(3, 3)。
"""
# 预测
R_pred = R_prev @ exp_so3(omega * dt)
phi_pred = exp_so3(-omega * dt) @ phi_prev
# 更新
F = np.eye(3) - dt * skew(omega)
P_pred = F @ P_prev @ F.T + Q
# 修正
R_curr = exp_so3(phi_pred) @ R_pred
phi_curr = np.zeros(3)
P_curr = P_pred
return R_curr, phi_curr, P_curr
在这个实现中,我们首先使用上一时刻的姿态估计值 R ^ t − 1 \hat{R}_{t-1} R^t−1和当前的角速度测量值 ω t \omega_t ωt来预测当前时刻的姿态 R ^ t \hat{R}_t R^t。然后,我们使用角误差状态的闭式解来预测当前时刻的角误差状态 ϕ ^ t \hat{\phi}_t ϕ^t。接下来,我们计算状态转移矩阵 F F F,并用它来更新角误差状态的协方差矩阵 P t P_t Pt。最后,我们使用预测的角误差状态 ϕ ^ t \hat{\phi}_t ϕ^t来修正预测的姿态 R ^ t \hat{R}_t R^t,得到最终的姿态估计值。
这种方法之所以有效,是因为它利用了SO3流形的几何结构。通过在局部切平面(即so3)上进行估计和更新,然后再映射回SO3,我们可以保证估计结果总是一个有效的旋转矩阵,同时避免了欧拉积分的数值误差问题。此外,由于角误差状态通常很小,我们可以使用一阶近似(即指数映射的一阶泰勒展开)来简化计算,提高效率。
当然,实际的姿态估计器通常会更加复杂,需要处理更多的状态量(如速度、位置等),并融合多种传感器的测量值(如加速度计、磁力计等)。但角误差状态的使用几乎是所有这些估计器的核心组件之一。
标签：Unleashing,frac,Python,Chapter6,phi,Delta,Theta,theta,omega From： https://blog.csdn.net/jiayoushijie/article/details/139246358