ch11_continuous_time_optimized

第十一章　连续时间轨迹估计（优化版） §

Chapter 11   Continuous-Time Trajectory Estimation (Optimized) §

---

本章导航 / Chapter Navigator §

一句话总结（TL;DR）

STEAM 算法用**高斯过程（GP）**定义连续时间轨迹先验，既能在离散时刻求解 MAP，又能以 $O(1)$ 代价插值任意查询时刻的位姿。

阅读路径建议：

• 🟢 快速浏览：读完”直觉图解”即可把握核心思想
• 🔵 理解算法：阅读”概念层”的公式和解释
• ⚫ 深度推导：展开”推导层”查看完整数学

---

11.1 为什么需要连续时间？ §

🎯 直觉：运动畸变问题 §

问题：激光雷达每秒扫描 10 万次，每个点在稍微不同的时刻采集。如果用离散时刻的位姿去校正，会产生”运动畸变”。

解决：需要一个连续轨迹表示，能在任意时刻（不限于测量时刻）查询位姿。

STEAM 的核心思想：

1. 用**随机微分方程（SDE）**定义连续时间先验——鼓励轨迹平滑
2. 在离散支撑时刻求解 MAP 估计
3. 用高斯过程插值查询任意时刻的状态

---

🔑 概念层：三种时间戳的解耦 §

连续时间方法允许三类时间戳独立选择：

时间戳类型	描述	密度
测量时刻	传感器观测发生的时刻	高（如 100Hz）
估计时刻	MAP 求解的支撑节点	可稀疏（如 10Hz）
查询时刻	需要位姿的时刻	任意（可为每激光点）

优势：估计时刻可以稀疏（降低计算量），但查询可以是任意时刻。

---

11.2 运动先验：常速 SDE §

🎯 直觉：鼓励平滑轨迹 §

物理直觉：没有外力时，物体倾向于保持匀速运动（牛顿第一定律）。

数学建模：广义加速度（速度的变化率）受白噪声驱动： $\ddot{\mathbf{\xi }}(t)=\mathbf{w}(t),\mathbf{w}(t)\sim \mathcal{G}\mathcal{P}(0,Q\delta (t-t^{\prime }))$

这称为**“白噪声加加速度”或常速先验**。

---

🔑 概念层：线性 SDE 与 GP 先验 §

将二阶 SDE 转化为一阶 Markov 形式，状态 $\mathbf{\gamma }(t)=[\mathbf{\xi }(t{)}^T,\dot{\mathbf{\xi }}(t{)}^T{]}^T$：

$\frac{d}{dt}\mathbf{\gamma }(t)=\underset{\mathbf{A}}{\underbrace{\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right]}}\mathbf{\gamma }(t)+\left[\begin{array}{c}0\\ 1\end{array}\right]\mathbf{w}(t)$

状态转移矩阵： $\Phi (t,t^{\prime })=\left[\begin{array}{cc}1& (t-t^{\prime })1\\ 0& 1\end{array}\right]$

累积协方差（时间间隔 $\Delta t$）： $\mathbf{Q}(\Delta t)=\left[\begin{array}{cc}\frac{1}{3}(\Delta t{)}^{3}Q& \frac{1}{2}(\Delta t{)}^{2}Q\\ \frac{1}{2}(\Delta t{)}^{2}Q& (\Delta t)Q\end{array}\right]$

含义：

• 位置不确定性 $\propto (\Delta t{)}^3$（漂移累积）
• 速度不确定性 $\propto \Delta t$

---

11.3 STEAM：同步轨迹估计与建图 §

🎯 直觉：SLAM 的连续时间版本 §

STEAM 状态：每个时刻包含位姿+速度 $\mathbf{x}=\{{\mathbf{T}}_0,{\mathbf{\varpi }}_0,{\mathbf{T}}_1,{\mathbf{\varpi }}_1,\dots ,{\mathbf{T}}_K,{\mathbf{\varpi }}_K,{\mathbf{p}}_1,\dots ,{\mathbf{p}}_M\}$

与离散 SLAM 的区别：

• 额外估计速度（$12$ 维/时刻，而非 $6$ 维）
• 运动先验是连续时间 SDE，而非离散积分

---

🔑 概念层：运动误差项 §

相邻时刻间的运动误差： ${\mathbf{e}}_{v,k}=\left[\begin{array}{c}(t_k-t_{k-1}){\mathbf{\varpi }}_{k-1}-\ln ({\mathbf{T}}_k{\mathbf{T}}_{k-1}^{-1}{)}^{\vee }\\ {\mathbf{\varpi }}_{k-1}-{\mathcal{J}}^{-1}{\mathbf{\varpi }}_k\end{array}\right]$

含义：

• 上半：匀速预测与实际位姿变化的差异
• 下半：速度连续性（左雅可比修正旋转非线性）

代价项： $J_{v,k}=\frac{1}{2}{\mathbf{e}}_{v,k}^T{\mathbf{Q}}_k^{-1}{\mathbf{e}}_{v,k}$

注意：时间间隔越大，${\mathbf{Q}}_k$ 越大，允许偏差越大。

---

🔑 概念层：MAP 求解与稀疏结构 §

总代价函数： $J(\mathbf{x})=\underset{\text{运动先验}}{\underbrace{\sum _k{J}_{v,k}}}+\underset{\text{视觉测量}}{\underbrace{\sum _{j,k}{J}_{y,jk}}}$

矩阵结构：

• ${\mathbf{A}}_{11}$（位姿+速度块）：块三对角（运动先验链）
• ${\mathbf{A}}_{22}$（路标块）：块对角（路标独立）

与离散 SLAM 相同：保留箭头形稀疏结构，可用 Schur 补或 Cholesky 求解。

---

11.4 GP 插值与外推 §

🎯 直觉：用两个相邻时刻插值中间 §

关键优势：求解出测量时刻的后验后，可在任意查询时刻 $\tau$ 以 $O(1)$ 代价插值。

GP 插值公式：对 $t_k\le \tau <t_{k+1}$ $\hat{\mathbf{\gamma }}(\tau )=\mathbf{\Lambda }(\tau )\hat{\mathbf{\gamma }}(t_k)+\mathbf{\Psi }(\tau )\hat{\mathbf{\gamma }}(t_{k+1})$

插值核： $\mathbf{\Lambda }(\tau )=\Phi (\tau ,t_k)-{\mathbf{Q}}_{\tau }\Phi (t_{k+1},\tau {)}^T{\mathbf{Q}}_{k+1}^{-1}\Phi (t_{k+1},t_k)$ $\mathbf{\Psi }(\tau )={\mathbf{Q}}_{\tau }\Phi (t_{k+1},\tau {)}^T{\mathbf{Q}}_{k+1}^{-1}$

转换回全局变量： $\hat{\mathbf{T}}(\tau )=\exp (\hat{\mathbf{\xi }}(\tau {)}^{\wedge }){\hat{\mathbf{T}}}_k$

---

🔑 概念层：协方差插值 §

思路：构造三时刻小型估计问题（$t_k,\tau ,t_{k+1}$）：

1. 提取 $t_k,t_{k+1}$ 处的边缘协方差
2. 加入两段运动先验（$t_k\to \tau$, $\tau \to t_{k+1}$）
3. 减去原始运动先验（$t_k\to t_{k+1}$）
4. 边缘化掉 $t_k,t_{k+1}$，得到 $\tau$ 处协方差

结果：$\hat{\mathbf{P}}(\tau ,\tau )$ 平滑变化，计算代价 $O(1)$。

---

🔑 概念层：外推 §

在最后时刻 $t_K$ 之后预测（$\tau >t_K$）：

均值外推（常速）： $\hat{\mathbf{T}}(\tau )=\exp ((\tau -t_K){\hat{\mathbf{\varpi }}}_K^{\wedge }){\hat{\mathbf{T}}}_K$

协方差外推： $\hat{\mathbf{P}}(\tau ,\tau )={\mathbf{E}}^{-1}(\mathbf{F}{\hat{\mathbf{P}}}_K{\mathbf{F}}^T+\mathbf{Q}){\mathbf{E}}^{-T}$

类似 EKF 的协方差传播。

---

11.5 与离散时间方法的比较 §

🔑 概念层：对比表 §

方面	离散时间 SLAM	连续时间 STEAM
状态	位姿序列	位姿+速度
查询时刻	仅测量时刻	任意时刻 $O(1)$
平滑性	测量驱动	GP 先验驱动
运动先验	离散积分	连续 SDE
实现复杂度	较低	较高

适用场景：

• 离散 SLAM：测量频率均匀，不需要中间时刻插值
• STEAM：传感器频率差异大（如激光+IMU+相机），需要高精度时间对齐

---

本章自检清单 §

阅读完本章，你应该能回答：

• 为什么需要连续时间方法？（运动畸变，任意时刻查询）
• 三种时间戳是什么？（测量、估计、查询）
• 常速先验的数学形式？（$\ddot{\mathbf{\xi }}=\mathbf{w}$）
• STEAM 状态与离散 SLAM 的区别？（额外估计速度）
• GP 插值的核心公式？（$\hat{\mathbf{\gamma }}(\tau )=\mathbf{\Lambda }{\hat{\mathbf{\gamma }}}_k+\mathbf{\Psi }{\hat{\mathbf{\gamma }}}_{k+1}$）
• 协方差插值的基本思路？（三时刻小估计问题）

如果以上都清楚，你掌握了连续时间估计的核心！

---

延伸阅读与代码 §

Python 实现（GP 插值核心）：

import numpy as np
 
def gp_interpolation_kernel(dt_k_tau, dt_tau_k1, Q):
    """
    计算 GP 插值核 Lambda 和 Psi
    dt_k_tau: tau - t_k
    dt_tau_k1: t_{k+1} - tau
    Q: 过程噪声协方差
    """
    # 状态转移矩阵
    Phi_k_tau = np.array([[1, dt_k_tau],
                          [0, 1]])
    Phi_tau_k1 = np.array([[1, dt_tau_k1],
                           [0, 1]])
    Phi_k_k1 = np.array([[1, dt_k_tau + dt_tau_k1],
                         [0, 1]])
 
    # 累积协方差
    Q_k_tau = np.array([[dt_k_tau**3/3, dt_k_tau**2/2],
                        [dt_k_tau**2/2, dt_k_tau]]) * Q
    Q_k_k1 = np.array([[(dt_k_tau+dt_tau_k1)**3/3, (dt_k_tau+dt_tau_k1)**2/2],
                       [(dt_k_tau+dt_tau_k1)**2/2, (dt_k_tau+dt_tau_k1)]]) * Q
 
    # 插值核
    Lambda = Phi_k_tau - Q_k_tau @ Phi_tau_k1.T @ np.linalg.inv(Q_k_k1) @ Phi_k_k1
    Psi = Q_k_tau @ Phi_tau_k1.T @ np.linalg.inv(Q_k_k1)
 
    return Lambda, Psi
 
 
def steam_solve(poses, velocities, landmarks, observations, dt, Q, max_iter=10):
    """
    简化版 STEAM 求解
    poses: [K, 4, 4] 位姿
    velocities: [K, 6] 广义速度
    landmarks: [M, 3] 路标
    observations: [(k, j, y, R)] 观测
    dt: 时间间隔数组 [K-1]
    Q: 过程噪声协方差 (6x6)
    """
    K = len(poses)
 
    for iteration in range(max_iter):
        # 构建线性系统（类似 SLAM，但状态包含速度）
        # A_11: 位姿+速度块 (12K x 12K)，块三对角
        # A_22: 路标块 (3M x 3M)，块对角
 
        # 运动先验误差和 Jacobian（12x12 块）
        for k in range(1, K):
            # 运动误差
            xi_k_k1 = se3_log(poses[k] @ np.linalg.inv(poses[k-1]))
            e_v_pos = dt[k-1] * velocities[k-1] - xi_k_k1
 
            # Jacobian 块（简化）
            # F_{k-1}, E_k 是 12x12 矩阵
 
        # 测量误差和 Jacobian（与 SLAM 相同，但 G_1 在速度列填零）
 
        # 求解线性系统 A dx = b
        # 更新位姿和速度
 
        pass  # 简化实现
 
    return poses, velocities, landmarks
 
 
def query_pose_at_time(poses, velocities, t_k, t_k1, tau, Q):
    """
    在 tau 时刻查询位姿（t_k <= tau < t_k1）
    返回: 插值后的位姿和速度
    """
    dt_k_tau = tau - t_k
    dt_tau_k1 = t_k1 - tau
 
    # 计算插值核
    Lambda, Psi = gp_interpolation_kernel(dt_k_tau, dt_tau_k1, Q)
 
    # 局部状态插值（简化，只取位置部分）
    xi_tau = Lambda[0, 0] * se3_log(poses[0]) + Psi[0, 0] * se3_log(poses[1])
 
    # 转回全局位姿
    T_tau = se3_exp(xi_tau) @ poses[0]
 
    return T_tau

---

全书完。附录提供矩阵代数速查。