Unleashing Robotics: Mastering Quaternion Kinematics with Python - Chapter6（原创系列教程）

Unleashing Robotics: Mastering Quaternion Kinematics with Python - Chapter6(原创系列教程)（最关键一章）
本系列教程禁止转载，主要是为了有不同见解的同学可以方便联系我，我的邮箱 fanzexuan135@163.com

第6章 旋转的数值积分方法和角误差理论

1.Runge-Kutta数值积分方法

我们的目标是积分形如下式的非线性微分方程:

$$\dot{x}=f(t,x)$$

在有限的时间区间$\Delta t$内,将其转化为差分方程,即:

$$x(t+\Delta t)=x(t)+{\int }_t^{t+\Delta t}f(\tau ,x(\tau ))d\tau$$

或等价地,如果假设$t_n=n\Delta t$且$x_n\equiv x(t_n)$,则有:

$$x_{n+1}=x_n+{\int }_{n\Delta t}^{(n+1)\Delta t}f(\tau ,x(\tau ))d\tau$$

最常用的方法之一是Runge-Kutta(简称RK)方法。这些方法使用多次迭代来估计区间内的导数,然后使用该导数在步长$\Delta t$内进行积分。

在下面的小节中,我们将从最简单的方法到最通用的方法介绍几种RK方法,并根据最常见的名称给它们命名。

1.1 Euler方法

Euler方法假设导数$f(\cdot )$在区间内是常数,因此有:

$$x_{n+1}=x_n+\Delta t\cdot f(t_n,x_n)$$

作为一个通用的RK方法,这相当于单阶段法,可以描述如下。计算初始点的导数:

$$k_1=f(t_n,x_n)$$

然后用它计算终点的积分值:

$$x_{n+1}=x_n+\Delta t\cdot k_1$$

代码示例:

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_{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)$$

中点法可以解释为两步法。首先,用Euler方法积分到中点,使用之前定义的$k_1$:

$$k_1=f(t_n,x_n)$$

$$x(t_n+\frac{1}{2}\Delta t)=x_n+\frac{1}{2}\Delta t\cdot k_1$$

然后用该值计算中点的导数$k_2$,进而得到积分结果:

$$k_2=f(t_n+\frac{1}{2}\Delta t,x(t_n+\frac{1}{2}\Delta t))$$

$$x_{n+1}=x_n+\Delta t\cdot k_2$$

代码示例:

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()$的值。它使用四个阶段或迭代来计算积分,获得四个导数$k_1\dots k_4$,这些导数是按顺序计算的。然后对这些导数进行加权平均,以获得区间内导数的4阶估计值。

RK4方法用一个小算法比用一步公式更好地描述。RK4积分步骤为:

$$x_{n+1}=x_n+\frac{\Delta t}{6}(k_1+2k_2+2k_3+k_4)$$

也就是说,增量是通过假设斜率为$k_1,k_2,k_3,k_4$的加权平均值来计算的,其中:

$$k_1=f(t_n,x_n)$$

$$k_2=f\left(t_n+\frac{1}{2}\Delta t,x_n+\frac{\Delta t}{2}{k}_1\right)$$

$$k_3=f\left(t_n+\frac{1}{2}\Delta t,x_n+\frac{\Delta t}{2}{k}_2\right)$$

$$k_4=f(t_n+\Delta t,x_n+\Delta t\cdot k_3)$$

不同的斜率有以下解释:

• $k_1$是区间开始时的斜率,使用$x_n$(Euler方法);
• $k_2$是区间中点的斜率,使用$x_n+\frac{1}{2}\Delta t\cdot k_1$(中点法);
• $k_3$再次是中点的斜率,但现在使用$x_n+\frac{1}{2}\Delta t\cdot k_2$;
• $k_4$是区间结束时的斜率,使用$x_n+\Delta t\cdot k_3$。

代码示例:

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+\Delta t\sum _{i=1}^s{b}_i{k}_i$$

其中:

$$k_i=f\left(t_n+\Delta t\cdot c_i,x_n+\Delta t\sum _{j=1}^s{a}_{ij}{k}_j\right)$$

也就是说,迭代次数(方法的阶数)为$s$,平均权重由$b_i$定义,评估时间点由$c_i$定义,斜率$k_i$使用值$a_{ij}$确定。根据$a_{ij}$项的结构,可以有显式或隐式的RK方法。

• 在显式方法中,所有$k_i$都是按顺序计算的,即只使用之前计算的值。这意味着矩阵$[a_{ij}]$是下三角且对角线元素为零(即对于$j\ge i$有$a_{ij}=0$)。Euler、中点和RK4方法都是显式的。

• 隐式方法具有完整的$[a_{ij}]$矩阵,需要求解线性方程组才能确定所有$k_i$。因此计算成本更高,但与显式方法相比,它们能够提高精度和稳定性。

2.闭式积分方法

在很多情况下,可以得到积分步骤的闭式表达式。现在我们考虑一阶线性微分方程的情况:

$$\dot{x}(t)=A\cdot x(t)$$

也就是说,关系是线性的,在区间内是常数。在这种情况下,在区间$[t_n,t_n+\Delta t]$上积分得到:

$$x_{n+1}=e^{A\cdot \Delta t}{x}_n=\Phi x_n$$

其中$\Phi$称为转移矩阵。该转移矩阵的Taylor展开为:

$$\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$$

当为已知$A$的实例编写此级数时,有时可以在结果中识别已知级数。这允许以闭式形式写出所得积分。下面给出几个示例。

2.1 角误差的积分

例如,考虑没有偏差和噪声的角误差动力学:

$$\dot{\delta \theta }=-[\omega {]}_{\times }\delta \theta$$

它的转移矩阵可以写成Taylor级数:

$$\Phi =e^{-[\omega {]}_{\times }\Delta t}$$

$$=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$$

现在定义$\omega \Delta t\equiv u\Delta \theta$,即旋转轴和旋转角,并应用式(75),我们可以对项进行分组,得到:

$$\begin{array}{rl}\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{array}$$

这是一个闭式解。

该解对应于一个旋转矩阵 Φ = 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驱动系统:

$$\begin{array}{rl}\dot{\delta p}& =\delta v\\ \dot{\delta v}& =-R[a{]}_{\times }\delta \theta \\ \dot{\delta \theta }& =-[\omega {]}_{\times }\delta \theta \end{array}$$

其中$(a,\omega )$是IMU读数,我们省略了重力和传感器偏差。该系统由状态向量和动态矩阵定义:

$$x=\left[\begin{array}{c}\delta p\\ \delta v\\ \delta \theta \end{array}\right],A=\left[\begin{array}{ccc}0& P_v& 0\\ 0& 0& V_{\theta }\\ 0& 0& {\Theta }_{\theta }\end{array}\right]$$

其中:

$$\begin{array}{rl}{P}_v& =I\\ V_{\theta }& =-R[a{]}_{\times }\\ {\Theta }_{\theta }& =-[\omega {]}_{\times }\end{array}$$

对采样周期$\Delta t$的积分为$x_{n+1}=e^{(A\Delta t)}\cdot x_n=\Phi \cdot x_n$。转移矩阵$\Phi$可以用Taylor展开(344)来表示,以$A\Delta t$的递增幂次表示。我们可以写出$A$的几个幂次来说明它们的一般形式:

$$\begin{array}{rl}A& =\left[\begin{array}{ccc}0& P_v& 0\\ 0& 0& V_{\theta }\\ 0& 0& {\Theta }_{\theta }\end{array}\right],A^2=\left[\begin{array}{ccc}0& 0& P_v{V}_{\theta }\\ 0& 0& V_{\theta }{\Theta }_{\theta }\\ 0& 0& {\Theta }_{\theta }^2\end{array}\right]\\ A^3& =\left[\begin{array}{ccc}0& 0& P_v{V}_{\theta }{\Theta }_{\theta }\\ 0& 0& V_{\theta }{\Theta }_{\theta }^2\\ 0& 0& {\Theta }_{\theta }^3\end{array}\right],A^4=\left[\begin{array}{ccc}0& 0& P_v{V}_{\theta }{\Theta }_{\theta }^2\\ 0& 0& V_{\theta }{\Theta }_{\theta }^3\\ 0& 0& {\Theta }_{\theta }^4\end{array}\right]\end{array}$$

从中我们可以观察到,对于$k>1$有:

$$A^{k>1}=\left[\begin{array}{ccc}0& 0& P_v{V}_{\theta }{\Theta }_{\theta }^{k-2}\\ 0& 0& V_{\theta }{\Theta }_{\theta }^{k-1}\\ 0& 0& {\Theta }_{\theta }^k\end{array}\right]$$

我们可以观察到$A$递增幂次中的项具有固定部分和${\Theta }_{\theta }$的递增幂次。这些幂次可能导致闭式解,就像前一节中那样。

让我们将矩阵$\Phi$划分如下:

$$\Phi =\left[\begin{array}{ccc}I& {\Phi }_{pv}& {\Phi }_{p\theta }\\ 0& I& {\Phi }_{v\theta }\\ 0& 0& {\Phi }_{\theta \theta }\end{array}\right]$$

让我们逐步探索$\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}⊤​

正如我们在上一节中看到的,这对应于我们熟知的旋转矩阵。

位置对速度项 最简单的非对角线项是${\Phi }_{pv}$,它是:

$${\Phi }_{pv}=P_v\Delta t=I\Delta t$$

速度对角度项 现在让我们转到${\Phi }_{v\theta }$项,通过写出其级数:

$$\begin{array}{rl}{\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{array}$$

这可以化简为:

$${\Phi }_{v\theta }=\Delta t{V}_{\theta }\left(I+\sum _{k\ge 1}\frac{({\Theta }_{\theta }\Delta t{)}^k}{(k+1)!}\right)$$

此时我们有两个选择。我们可以在第一个显著项处截断级数,得到${\Phi }_{v\theta }=V_{\theta }\Delta t$,但这不是闭式的。下一节将给出使用这种简化方法的结果。或者,让我们将$V_{\theta }$分离出来并写出:

$${\Phi }_{v\theta }=V_{\theta }{\Sigma }_1$$

其中:

$${\Sigma }_1=I\Delta t+\frac{1}{2}{\Theta }_{\theta }\Delta t^2+\frac{1}{3!}{\Theta }_{\theta }^2\Delta t^3+\cdots$$

级数${\Sigma }_1$与我们为${\Phi }_{\theta \theta }$写的级数(358)类似,但有两个例外:

• ${\Sigma }_1$中的${\Theta }_{\theta }$幂次与$\frac{1}{k!}$的有理系数和$\Delta t$的幂次不匹配。实际上,这里我们注意到下标"1"表示每个成员缺少一个${\Theta }_{\theta }$幂次。

• 级数开头的一些项丢失了。同样,下标"1"表示缺少这样一项。

产生了恒等式:

$${\Theta }_{\theta }=\frac{[\omega {]}_{\times }^3}{\Vert \omega {\Vert }^2}=\frac{-{\Theta }_{\theta }^3}{\Vert \omega {\Vert }^2}$$

这个表达式允许我们将级数中${\Theta }_{\theta }$的指数增加两次,如果$\omega \ne 0$,可以写成:

$${\Sigma }_1=I\Delta t-\frac{{\Theta }_{\theta }}{\Vert \omega {\Vert }^2}\left(\frac{1}{2}{\Theta }_{\theta }^2\Delta t^2+\frac{1}{3!}{\Theta }_{\theta }^3\Delta t^3+\cdots \right)$$

否则${\Sigma }_1=I\Delta 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)

这是一个闭式解,如果$\omega \ne 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​

位置对角度项 最后让我们研究${\Phi }_{p\theta }$项。它的Taylor级数为:

$${\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$$

我们将常数项分离出来得到:

$${\Phi }_{p\theta }=P_v{V}_{\theta }{\Sigma }_2$$

其中:

$${\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"在${\Sigma }_2$中有以下解释:

• 在级数的每一项中都缺少两个${\Theta }_{\theta }$幂次,
• 级数的前两项丢失。

再次使用式(366)来增加${\Theta }_{\theta }$的指数,得到:

$${\Sigma }_2=\frac{1}{2}I\Delta t^2-\frac{1}{\Vert \omega {\Vert }^2}\left(\frac{1}{3!}{\Theta }_{\theta }^3\Delta t^3+\frac{1}{4!}{\Theta }_{\theta }^4\Delta t^4+\cdots \right)$$

我们将不完整的级数替换为其闭式,得到:

Σ 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​=21​IΔ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]×​(21​IΔt2−∥ω∥21​(R{ωΔt}⊤−∑k=02​k!(−[ω]×​Δt)k​))​ω→0ω=0​

2.3 完整IMU示例

为了给出推广前一示例中所述方法的手段,我们需要从更近的距离检查完整IMU情况。

考虑完整IMU系统,它可以写成:

$$\dot{\delta x}=A\delta x+Bw$$

其离散时间积分需要转移矩阵:

$$\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$$

我们想要计算它。动态矩阵$A$是块稀疏的,通过检查原始方程可以很容易地确定它的块:

$$A=\left[\begin{array}{cccccc}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{array}\right]$$

和之前一样,让我们写出$A$的几个幂次:

$$\begin{array}{rl}{A}^2& =\left[\begin{array}{cccccc}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{array}\right]\\ A^3& =\left[\begin{array}{cccccc}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{array}\right]\\ A^4& =\left[\begin{array}{cccccc}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{array}\right]\end{array}$$

基本上,我们观察到以下几点:

• $A$对角线上的唯一项,即旋转项${\Theta }_{\theta }$,在$A^k$序列中向右和向上传播。所有不受这种传播影响的项都消失了。这种传播在以下三个方面影响 { A k } \{A^k\} {Ak}序列的结构:

• 从第3次幂开始,$A$幂次的稀疏性就稳定下来了。也就是说,对于高于3的$A$幂次,不再出现或消失非零块。

• 左上角的$3\times 3$块,对应于前一示例中的简化IMU模型,与该示例相比没有变化。因此,之前为它开发的闭式解仍然成立。- 与陀螺仪偏差误差相关的项(第五列的那些)引入了${\Theta }_{\theta }$幂次的类似级数,可以用我们在简化示例中使用的相同技术来求解。

此时,我们有兴趣找到构造转移矩阵$\Phi$的闭式元素的通用方法。让我们回顾一下关于级数${\Sigma }_1$和${\Sigma }_2$的评论:

• 下标与每个级数成员中缺少的${\Theta }_{\theta }$幂次一致。
• 下标与级数开头丢失的项数一致。

考虑到这些性质,让我们引入级数${\Sigma }_n(X,y)$,定义为:

$${\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$项开始,项缺少$n$个矩阵$X$的幂次。立即可以看出${\Sigma }_1$和${\Sigma }_2$满足:

$${\Sigma }_n={\Sigma }_n({\Theta }_{\theta },\Delta t)$$

并且 Σ 0 = R { ω Δ t } ⊤ \Sigma_0 = R\{\omega\Delta t\}^\top Σ0​=R{ωΔt}⊤。我们现在可以将转移矩阵(377)写成这些级数的函数:

$$\Phi =\left[\begin{array}{cccccc}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{array}\right]$$

我们的问题现在导出为找到${\Sigma }_n$的通用闭式表达式的问题。让我们观察到目前为止得到的${\Sigma }_0\dots {\Sigma }_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)=21​IΔt2−∥ω∥21​(R{ωΔt}⊤−I−Θθ​Δt−21​Θθ2​Δt2)=3!1​IΔt3+∥ω∥4Θθ​​(R{ωΔt}⊤−I−Θθ​Δt−21​Θθ2​Δt2−3!1​Θθ3​Δt3)​

通过仔细检查${\Sigma }_0\dots {\Sigma }_3$的级数,我们现在可以为${\Sigma }_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!1​IΔtnR{ωΔt}⊤n!1​IΔtn−∥ω∥n+1(−1)2n+1​[ω]×​​(R{ωΔt}⊤−∑k=0n​k!(−[ω]×​Δt)k​)n!1​IΔtn+∥ω∥n(−1)2n​​(R{ωΔt}⊤−∑k=0n​k!(−[ω]×​Δ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<\infty$,${\Sigma }_n$的表达式就是闭式的,而这在实际中总是成立的。对于当前示例,我们有$n\le 3$

简单来说，在第3节中的完整IMU示例主要是为了说明如何在实际的惯性导航系统中应用前面所推导的旋转理论和积分方法。

在惯性导航中,我们通常使用IMU(惯性测量单元)来估计物体的姿态、速度和位置。一个典型的IMU包含三轴陀螺仪和三轴加速度计,分别测量物体的角速度和加速度。然而,由于传感器存在偏差和噪声,直接对测量值进行积分会导致估计误差快速累积。因此,我们需要设计一个状态估计器(如卡尔曼滤波器)来融合IMU数据,并估计和校正这些误差。

我们首先定义了IMU系统的误差状态向量:

$$\delta x=[\delta p^{\top },\delta v^{\top },\delta {\theta }^{\top },\delta a_b^{\top },\delta {\omega }_b^{\top },\delta g^{\top }{]}^{\top }$$

其中$\delta p,\delta v,\delta \theta$分别是位置、速度和姿态的误差,$\delta a_b,\delta {\omega }_b$是加速度计和陀螺仪的偏差误差,$\delta g$是重力向量的误差。

然后,我们给出了这些误差状态的微分方程:

$$\begin{array}{rl}\delta \dot{p}& =\delta v\\ \delta \dot{v}& =-R[a{]}_{\times }\delta \theta -R\delta a_b+\delta g-R{a}_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{array}$$

这里的$a_n,{\omega }_n,a_w,{\omega }_w$分别是加速度计噪声、陀螺仪噪声、加速度计偏差噪声和陀螺仪偏差噪声。

为了在离散时间下实现状态估计器,我们需要对这些微分方程进行积分。这就是这节的主要内容。通过使用前面推导的指数映射和齐次积分方法,我们可以得到误差状态转移矩阵$\Phi$的闭式解,从而实现精确而高效的状态传播。

下面是一个简化版的Python实现,演示如何使用转移矩阵进行状态传播:

import numpy as np

def imu_predict(delta_x_prev, P_prev, a_m, omega_m, dt, Q):
    """IMU误差状态预测。

    Args:
        delta_x_prev: 上一时刻的误差状态估计值, shape为(15,)。
        P_prev: 上一时刻的误差状态协方差矩阵, shape为(15, 15)。
        a_m: 当前时刻的加速度计测量值, shape为(3,)。
        omega_m: 当前时刻的陀螺仪测量值, shape为(3,)。
        dt: 采样时间。
        Q: 过程噪声协方差矩阵, shape为(12, 12)。

    Returns:
        delta_x_pred: 当前时刻的误差状态预测值, shape为(15,)。
        P_pred: 当前时刻的误差状态协方差矩阵, shape为(15, 15)。
    """
    # 提取状态变量
    delta_p, delta_v, delta_theta, delta_a_b, delta_omega_b, delta_g = np.split(delta_x_prev, [3, 6, 9, 12, 15])
    
    # 计算状态转移矩阵
    F = np.eye(15)
    F[0:3, 3:6] = np.eye(3) * dt
    F[3:6, 6:9] = -skew(a_m - delta_a_b) * dt
    F[3:6, 9:12] = -np.eye(3) * dt
    F[3:6, 12:15] = np.eye(3) * dt
    F[6:9, 6:9] = exp_so3(-omega_m * dt).T
    F[6:9, 12:15] = -np.eye(3) * dt
    
    # 预测误差状态和协方差
    delta_x_pred = F @ delta_x_prev
    P_pred = F @ P_prev @ F.T + Q
    
    return delta_x_pred, P_pred

在这个实现中,我们首先从误差状态向量中提取出各个分量。然后,我们根据这节中给出的公式计算状态转移矩阵$\Phi$(这里记为F)。需要注意的是,我们使用了前面定义的exp_so3函数来计算旋转矩阵的指数映射。最后,我们使用转移矩阵来预测新的误差状态和协方差矩阵。

这个函数可以作为一个基本的IMU预测步骤,与卡尔曼滤波器的更新步骤相结合,就可以实现一个完整的惯性导航系统。当然,实际系统中还需要考虑更多的因素,如初始对准、坐标系转换、零偏补偿等,这里不再赘述。

总之,第3节的主要目的是展示如何将抽象的旋转理论应用到实际的工程问题中。通过对IMU误差状态进行建模和积分,我们可以设计出鲁棒和高效的状态估计算法,这是现代惯性导航系统的基础。

机器人IMU应用实例:

让我们考虑一个常见的机器人应用 - 使用IMU进行航位推算。假设我们有一个小型移动机器人,配备了一个六轴IMU(3个加速度计和3个陀螺仪)。我们的目标是使用IMU数据估计机器人的位置、速度和姿态。

我们可以设置如下误差状态系统:

$$\begin{array}{rl}\delta \dot{p}& =\delta v\\ \delta \dot{v}& =-R[a{]}_{\times }\delta \theta -R\delta a_b+\delta g-R{a}_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{array}$$

其中$\delta p,\delta v,\delta \theta$分别是位置、速度和姿态误差,$\delta a_b,\delta {\omega }_b$是加速度计和陀螺仪偏差误差,$\delta g$是重力误差。$a_n,{\omega }_n,a_w,{\omega }_w$分别是加速度计噪声、陀螺仪噪声、加速度计偏差噪声和陀螺仪偏差噪声。

使用前面推导的结果,我们可以计算转移矩阵$\Phi$并进行离散化:

$$\delta x_{n+1}={\Phi }_n\delta x_n+F_i\cdot i$$

其中$\delta x=[\delta p^{\top },\delta v^{\top },\delta {\theta }^{\top },\delta a_b^{\top },\delta {\omega }_b^{\top },\delta g^{\top }{]}^{\top }$是误差状态向量,$i$是过程噪声。

然后我们可以设计一个误差状态卡尔曼滤波器:

预测步骤:
$$\begin{array}{rl}{\widehat{\delta x}}_{n+1}& ={\Phi }_n{\widehat{\delta x}}_n\\ P_{n+1}& ={\Phi }_n{P}_n{\Phi }_n^{\top }+F_i{Q}_i{F}_i^{\top }\end{array}$$

更新步骤(假设有位置测量$y_n$):
$$\begin{array}{rl}{K}_n& =P_n{H}^{\top }(H{P}_n{H}^{\top }+R{)}^{-1}\\ {\widehat{\delta x}}_n& =K_n(y_n-h({\hat{x}}_n))\\ P_n& =(I-K_{n}H)P_n\end{array}$$

其中$H$是观测模型,$R$是观测噪声协方差。

最后,我们使用估计的误差状态来校正名义状态:

$${\hat{x}}_n\leftarrow {\hat{x}}_n\oplus {\widehat{\delta 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)时非常有用。

那么,什么时候需要用到角误差的积分呢?主要有以下几个场景:

1. 姿态估计: 在使用IMU进行姿态估计时,我们通常会积分陀螺仪数据得到姿态的增量。然而,由于陀螺仪存在偏差和噪声,直接积分会导致估计误差快速增长。这时,我们可以在估计器中引入角误差状态,并使用如上所示的闭式解进行更新,以得到更准确和稳定的姿态估计。

2. 视觉里程计: 在视觉里程计中,相机姿态通常用旋转矩阵或四元数表示。当我们优化姿态时,使用李代数so3作为局部参数化,可以避免过度参数化和奇异性问题。这时,需要在so3和SO3之间进行映射,也就是上面的指数映射和对数映射。

3. 机器人运动学: 对于许多关节式机器人,其关节运动通常用旋转来描述。当我们进行运动学计算或控制时,需要在关节角速度(在so3中)和关节姿态(在SO3中)之间进行转换,这也需要用到指数映射。

4. 轨迹优化: 在一些轨迹优化问题中,机器人的姿态也是需要优化的变量。为了保证优化过程中姿态的有效性,通常使用李代数参数化姿态,并在每次迭代后将优化结果投影回SO3。这个投影过程就是通过指数映射实现的。
   好的,让我详细说明一下为什么引入角误差状态,并使用闭式解进行更新可以得到更准确和稳定的姿态估计。

Q&A

角误差理论到底是什么

我们知道IMU的陀螺仪测量的是角速度,要得到姿态需要对角速度进行积分。最简单的方法是使用欧拉积分:

$$R_{t+\Delta t}=R_t\exp ({\omega }_t\Delta t)$$

其中$R_t$是时刻$t$的旋转矩阵,${\omega }_t$是陀螺仪测量的角速度,$\Delta t$是采样时间。

然而,这种直接积分的方法有以下问题:

1. 陀螺仪存在偏差和噪声,直接积分会导致误差快速累积。
2. 欧拉积分本身是一阶近似,也会引入数值误差。
3. 旋转矩阵必须满足正交性约束,但数值积分无法保证积分结果满足此约束。

为了解决这些问题,我们引入角误差状态。假设我们有一个姿态的估计值${\hat{R}}_t$,它与真实值$R_t$之间存在一个小的误差$\delta R_t$:

$$R_t=\delta R_t{\hat{R}}_t$$

我们将$\delta R_t$参数化为一个李代数向量${\varphi }_t$:

$$\delta R_t=\exp ({\varphi }_t)$$

这里的$\exp$是指从so3到SO3的指数映射。

现在,我们的目标是估计误差状态${\varphi }_t$,而不是直接估计$R_t$。假设我们已经有了一个估计器(如EKF),它给出了${\varphi }_t$的估计值${\hat{\varphi }}_t$和协方差矩阵$P_t$。

当新的陀螺仪测量值${\omega }_{t+1}$到来时,我们首先更新姿态的估计值:

$${\hat{R}}_{t+1}={\hat{R}}_t\exp ({\omega }_{t+1}\Delta t)$$

然后,我们使用角误差状态的闭式解来更新${\hat{\varphi }}_t$和$P_t$:

$${\hat{\varphi }}_{t+1}=\exp (-{\omega }_{t+1}\Delta t){\hat{\varphi }}_t$$

$$P_{t+1}=F_t{P}_t{F}_t^{\top }+Q_t$$

其中$F_t$是状态转移矩阵,可以使用$\exp ({\omega }_{t+1}\Delta t)$的右雅可比矩阵来近似:

$$F_t\approx I-[{\omega }_{t+1}{]}_{\times }\Delta t$$

$Q_t$是过程噪声协方差矩阵。

最后,我们使用更新后的${\hat{\varphi }}_{t+1}$来修正姿态估计值:

$${\hat{R}}_{t+1}\leftarrow \exp ({\hat{\varphi }}_{t+1}){\hat{R}}_{t+1}$$

这样,我们就得到了新的姿态估计值${\hat{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

在这个实现中,我们首先使用上一时刻的姿态估计值${\hat{R}}_{t-1}$和当前的角速度测量值${\omega }_t$来预测当前时刻的姿态${\hat{R}}_t$。然后,我们使用角误差状态的闭式解来预测当前时刻的角误差状态${\hat{\varphi }}_t$。接下来,我们计算状态转移矩阵$F$,并用它来更新角误差状态的协方差矩阵$P_t$。最后,我们使用预测的角误差状态${\hat{\varphi }}_t$来修正预测的姿态${\hat{R}}_t$,得到最终的姿态估计值。

这种方法之所以有效,是因为它利用了SO3流形的几何结构。通过在局部切平面(即so3)上进行估计和更新,然后再映射回SO3,我们可以保证估计结果总是一个有效的旋转矩阵,同时避免了欧拉积分的数值误差问题。此外,由于角误差状态通常很小,我们可以使用一阶近似(即指数映射的一阶泰勒展开)来简化计算,提高效率。

当然,实际的姿态估计器通常会更加复杂,需要处理更多的状态量(如速度、位置等),并融合多种传感器的测量值(如加速度计、磁力计等)。但角误差状态的使用几乎是所有这些估计器的核心组件之一。

闭式解(Closed-form solution)

闭式解(Closed-form solution)是一种可以用有限个基本操作(如算术运算、根式运算、指数、对数等)显式表示的解。与之相对的是数值解或近似解,它们通常需要使用迭代算法来逐步逼近真实的解。

在许多工程和科学问题中,我们经常需要求解微分方程、积分方程或优化问题。如果能找到闭式解,那么问题就可以一次性地得到精确的答案,而不需要进行数值计算。这不仅可以提高计算效率,还能避免数值方法引入的误差。

举几个例子:

1. 一阶线性常微分方程$\dot{x}(t)=ax(t)+b$的闭式解为$x(t)=C{e}^{at}+\frac{b}{a}$,其中$C$是由初值条件确定的常数。

2. 几何级数$S_n={\sum }_{i=0}^{n-1}a{r}^i$的闭式解为$S_n=\frac{a(1-r^n)}{1-r}$ (当$r\ne 1$时)。

3. 旋转矩阵$R$的指数映射$R=\exp ([\omega {]}_{\times })$有闭式解$R=I+\sin \theta [\omega {]}_{\times }+(1-\cos \theta )[\omega {]}_{\times }^2$,其中$\theta =\Vert \omega \Vert$。

4. 最小二乘问题${\min }_x\Vert Ax-b{\Vert }^2$的闭式解为$x=(A^{\top }A{)}^{-1}{A}^{\top }b$(当$A$列满秩时)。

然而,并非所有问题都有闭式解。很多复杂的微分方程、积分方程和优化问题都需要使用数值方法来求解。即使存在闭式解,其形式也可能非常复杂,以至于实际计算时还是需要借助数值近似。

在状态估计领域,许多估计器(如卡尔曼滤波器)都涉及到矩阵指数$e^{At}$的计算。如果能找到$e^{At}$的闭式解(如B.1节所示),那么就可以显著提高估计器的计算效率和精度。相比之下,如果使用泰勒展开等数值方法来近似$e^{At}$,就会引入截断误差和舍入误差。

因此,掌握常见问题的闭式解,并在可能的情况下将其应用到实际算法中,是提高计算效率和精度的重要手段。当然,这也需要在理论分析和工程实践之间进行权衡。有时候,即使存在闭式解,使用数值方法可能还是更加简单和鲁棒。