ch09_pose_estimation_optimized

第九章　位姿估计应用（优化版） §

Chapter 9   Pose Estimation Applications (Optimized) §

---

本章导航 / Chapter Navigator §

一句话总结（TL;DR）

本章用 Lie 群工具解决四个经典 3D 估计问题：点云对齐（Wahba）、点云跟踪（EKF）、位姿图优化（SLAM 后端）、惯性导航（IMU）。

阅读路径建议：

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

---

9.1 点云对齐：找最佳旋转和平移 §

🎯 直觉：拼三维拼图 §

你有一张局部照片（车体系中的点云）和一份地图（固定系中的点云），要找到相机的位姿。

关键约束：

• 旋转矩阵 $\mathbf{C}$ 必须满足 ${\mathbf{C}}^T\mathbf{C}=1$，$\det (\mathbf{C})=+1$
• 这使它不能用普通最小二乘

好消息：这个问题没有局部极小值！任何满足一阶条件的临界点要么是全局最小，要么是鞍点/极大值。

---

🔑 概念层：三种等价解法 §

方法	核心工具	优点
四元数	4×4 特征值问题	解析解，一次性算出最优旋转
旋转矩阵（SVD）	3×3 SVD	全局分析，处理退化情形
$SE(3)$ 迭代	左扰动 Gauss-Newton	同时优化旋转+平移

SVD 解法步骤：

1. 计算去心点对的互协方差矩阵： $\mathbf{W}={\sum }_j{w}_j({\mathbf{y}}_j-\bar{\mathbf{y}})({\mathbf{p}}_j-\bar{\mathbf{p}}{)}^T$

2. SVD 分解：$\mathbf{W}=\mathbf{UD}{\mathbf{V}}^T$

3. 最优旋转：$\hat{\mathbf{C}}=\mathbf{U}\mathrm{diag}(1,1,\det (\mathbf{U})\det (\mathbf{V})){\mathbf{V}}^T$

4. 最优平移：$\hat{\mathbf{r}}=\bar{\mathbf{y}}-\hat{\mathbf{C}}\bar{\mathbf{p}}$

注意：$\mathbf{S}$ 的最后一个对角元取 $\det (\mathbf{U})\det (\mathbf{V})$ 是为了确保 $\det (\hat{\mathbf{C}})=+1$（真旋转而非反射）。

---

🧮 数值示例：ICP 核心步骤 §

场景：两个点云各有 3 个点（已找到对应关系）

• 固定系点云：${\mathbf{p}}_1=[1,0,0{]}^T$, ${\mathbf{p}}_2=[0,1,0{]}^T$, ${\mathbf{p}}_3=[0,0,1{]}^T$
• 车体系观测：${\mathbf{y}}_1=[0,1,0{]}^T$, ${\mathbf{y}}_2=[-1,0,0{]}^T$, ${\mathbf{y}}_3=[0,0,1{]}^T$

计算：

1. 质心：$\bar{\mathbf{p}}=[1/3,1/3,1/3{]}^T$, $\bar{\mathbf{y}}=[-1/3,1/3,1/3{]}^T$
2. 去心后计算互协方差：
   • $\mathbf{W}={\sum }_j({\mathbf{y}}_j-\bar{\mathbf{y}})({\mathbf{p}}_j-\bar{\mathbf{p}}{)}^T$
   • 结果：$\mathbf{W}\approx \left[\begin{array}{ccc}0& 1& 0\\ -1& 0& 0\\ 0& 0& 1\end{array}\right]$（接近绕z轴90°旋转）
3. SVD：$\mathbf{W}=\mathbf{UD}{\mathbf{V}}^T$
4. 最优旋转：$\hat{\mathbf{C}}=\mathbf{US}{\mathbf{V}}^T=\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\end{array}\right]$（绕 z 轴转 90°）

验证：$\hat{\mathbf{C}}{\mathbf{p}}_1\approx {\mathbf{y}}_1$，$\hat{\mathbf{C}}{\mathbf{p}}_2\approx {\mathbf{y}}_2$ ✓

---

📐 推导层：为什么 SVD 给出最优解 §

展开：从优化目标到 SVD 解

目标：最大化 $\mathrm{tr}(\mathbf{C}{\mathbf{W}}^T)$ 约束 $\mathbf{C}\in SO(3)$。

对任意正交矩阵 $\mathbf{C}$，有： $\mathrm{tr}(\mathbf{C}{\mathbf{W}}^T)=\mathrm{tr}(\mathbf{CVD}{\mathbf{U}}^T)=\mathrm{tr}({\mathbf{V}}^T\mathbf{CV}\mathbf{D})$

令 $\mathbf{M}={\mathbf{V}}^T\mathbf{CV}$，它也是正交矩阵。 $\mathrm{tr}(\mathbf{MD})={\sum }_i{m}_{ii}{d}_i\le {\sum }_i{d}_i$

当 $\mathbf{M}=1$ 时取等号，即 $\mathbf{C}=\mathbf{V}{\mathbf{U}}^T$。

如果 $\det (\mathbf{V}{\mathbf{U}}^T)=-1$，需要调整最后一个对角元为 -1 以保证 $\det (\mathbf{C})=+1$。

---

9.2 点云跟踪：时间序列的位姿估计 §

🎯 直觉：同时用”运动模型”和”观测” §

问题：机器人移动时，每个时刻观测地标点，估计整条轨迹。

两种信息：

1. 运动先验：从里程计/控制输入知道”大概怎么移动”
2. 测量信息：地标观测告诉”实际在哪里”

EKF 的做法：

• 预测：按运动模型推进
• 校正：用地标观测修正

---

🔑 概念层：李群版 EKF §

状态：$\mathbf{x}=\{{\mathbf{T}}_0,{\mathbf{T}}_1,\dots ,{\mathbf{T}}_K\}$，每个 ${\mathbf{T}}_k\in SE(3)$

关键创新：均值在 $SE(3)$，协方差在 $\mathfrak{se}(3)$（$6\times 6$ 矩阵）

预测步骤：

• 均值：${\check{\mathbf{T}}}_k=\exp (\Delta t{\mathbf{\varpi }}_k^{\wedge }){\hat{\mathbf{T}}}_{k-1}$（矩阵乘法）
• 协方差：${\check{\mathbf{P}}}_k={\mathbf{F}}_{k-1}{\hat{\mathbf{P}}}_{k-1}{\mathbf{F}}_{k-1}^T+{\mathbf{Q}}_k$

其中 ${\mathbf{F}}_{k-1}=\mathrm{Ad}(\exp (\Delta t{\mathbf{\varpi }}_k^{\wedge }))$ 是伴随矩阵。

重要性质：${\mathbf{F}}_{k-1}$ 只依赖于输入 ${\mathbf{\varpi }}_k$，不依赖于当前状态估计！

校正步骤：

• 卡尔曼增益：${\mathbf{K}}_k={\check{\mathbf{P}}}_k{\mathbf{G}}_k^T({\mathbf{G}}_k{\check{\mathbf{P}}}_k{\mathbf{G}}_k^T+{\mathbf{R}}_k{)}^{-1}$
• 协方差更新：${\hat{\mathbf{P}}}_k=(1-{\mathbf{K}}_k{\mathbf{G}}_k){\check{\mathbf{P}}}_k$
• 均值更新：${\hat{\mathbf{T}}}_k=\exp (({\mathbf{K}}_k{\mathbf{e}}_k{)}^{\wedge }){\check{\mathbf{T}}}_k$

测量 Jacobian：${\mathbf{G}}_{jk}={\mathbf{D}}^T({\check{\mathbf{T}}}_k{\mathbf{p}}_j{)}^{\odot }$，其中 $(\cdot {)}^{\odot }$ 是第 8 章定义的齐次点算子。

---

🔑 概念层：批量 MAP 方法 §

同时优化整条轨迹，目标函数： $J(\mathbf{x})=\underset{\text{运动误差}}{\underbrace{\sum _k\frac{1}{2}{\mathbf{e}}_{v,k}^T{\mathbf{W}}_{v,k}^{-1}{\mathbf{e}}_{v,k}}}+\underset{\text{测量误差}}{\underbrace{\sum _{j,k}\frac{1}{2}{\mathbf{e}}_{y,jk}^T{\mathbf{R}}_{jk}^{-1}{\mathbf{e}}_{y,jk}}}$

运动误差（在李代数中）：${\mathbf{e}}_{v,k}=\ln ({\Xi }_k{\hat{\mathbf{T}}}_{k-1}{\mathbf{T}}_k^{-1}{)}^{\vee }$

高斯-牛顿迭代：

1. 在当前猜测 ${\mathbf{T}}_{\mathrm{op},k}$ 处线性化
2. 构建块稀疏矩阵方程 $\mathbf{A}\delta \mathbf{x}=\mathbf{b}$
3. 求解，更新：${\mathbf{T}}_{\mathrm{op},k}\leftarrow \exp ({\mathbf{ϵ}}_k^{\star \wedge }){\mathbf{T}}_{\mathrm{op},k}$
4. 重复直到收敛

---

9.3 位姿图松弛：SLAM 后端 §

🎯 直觉：解 inconsistent 的拼图 §

场景：机器人绕了一圈回到起点，但里程计说”还没到”。

位姿图：

• 节点：各时刻的位姿 ${\mathbf{T}}_k$
• 边：相对位姿测量 ${\bar{\mathbf{T}}}_{k\ell }$

问题：测量 inconsistent（绕闭环不等于单位），需要”松弛”找到全局一致的估计。

---

🔑 概念层：误差函数与优化 §

移动系不变误差： ${\mathbf{e}}_{k\ell }=\ln ({\bar{\mathbf{T}}}_{k\ell }{\mathbf{T}}_{\ell }{\mathbf{T}}_k^{-1}{)}^{\vee }$

直觉：${\mathbf{T}}_{\ell }{\mathbf{T}}_k^{-1}$ 是从 $k$ 到 $\ell$ 的真实相对变换，与测量 ${\bar{\mathbf{T}}}_{k\ell }$ 比较。

目标函数： $J(\mathbf{x})=\frac{1}{2}{\sum }_{(k,\ell )}{\mathbf{e}}_{k\ell }^T{\mathbf{\Sigma }}_{k\ell }^{-1}{\mathbf{e}}_{k\ell }$

高斯-Newton 步骤：

1. 线性化误差：${\mathbf{e}}_{k\ell }\approx {\mathbf{e}}_{k\ell }({\mathbf{x}}_{\mathrm{op}})-{\mathbf{G}}_{k\ell }\delta {\mathbf{x}}_{k\ell }$
2. 构建稀疏系统 $\mathbf{A}\delta \mathbf{x}=\mathbf{b}$
3. $\mathbf{A}$ 的稀疏性 = 位姿图的连通性

初始化：找生成树，从根节点通过复合测量初始化所有位姿。

---

9.4 惯性导航：IMU 融合 §

🎯 直觉：高速传感器 + 低速校正 §

IMU 特性：

• 输出频率高（100-1000 Hz）
• 积分会漂移（误差累积）
• 需要与其他传感器（GPS、视觉）融合

核心问题：如何在 Lie 群框架中处理加速度积分？

---

🔑 概念层：$S{E}_2(3)$ 扩展位姿 §

创新：把旋转、位置、速度打包成 $5\times 5$ 矩阵： ${\mathbf{T}}_{iv}=\left[\begin{array}{ccc}{\mathbf{C}}_{iv}& \mathbf{r}& \mathbf{v}\\ 0^T& 1& 0\\ 0^T& 0& 1\end{array}\right]\in S{E}_2(3)$

优势：一次矩阵乘法完成”旋转+平移+速度更新”！

右扰动方案（本书唯一使用右扰动的地方）： $\mathbf{T}=\bar{\mathbf{T}}\exp (\delta {\mathbf{\xi }}^{\wedge })$

原因：IMU 噪声自然出现在运动方程的”车体侧”。

---

🔑 概念层：IMU 运动学积分 §

标称更新（假设测量在采样间隔内恒定）： $\bar{\mathbf{C}}({\tau }_j)=\bar{\mathbf{C}}({\tau }_{j-1})\exp (\Delta {\tau }_j{\bar{\mathbf{\omega }}}_j^{\wedge })$

$\bar{\mathbf{v}}({\tau }_j)=\bar{\mathbf{v}}({\tau }_{j-1})+\Delta {\tau }_j(\bar{\mathbf{C}}({\tau }_{j-1})\mathbf{J}{\bar{\mathbf{a}}}_j+\mathbf{g})$

$\bar{\mathbf{r}}({\tau }_j)=\bar{\mathbf{r}}({\tau }_{j-1})+\Delta {\tau }_j\bar{\mathbf{v}}({\tau }_{j-1})+\frac{1}{2}\Delta {\tau }_j^2(\bar{\mathbf{C}}({\tau }_{j-1})\mathbf{N}{\bar{\mathbf{a}}}_j+\mathbf{g})$

其中 $\mathbf{J}$ 是左雅可比，$\mathbf{N}$ 是新函数（考虑旋转时的加速度积分）。

状态维度：15（9 维扩展位姿扰动 + 6 维偏置）

EKF 流程：

1. 预测：对每个 IMU 数据传播均值和协方差
2. 更新：当有外部测量（GPS、相机）时，标准 EKF 校正

---

本章自检清单 §

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

• 点云对齐的 SVD 解法步骤是什么？（互协方差 → SVD → $\mathbf{US}{\mathbf{V}}^T$）
• 为什么点云对齐没有局部极小值？（任何临界点要么是全局最小，要么是鞍点）
• 李群版 EKF 的均值和协方差分别在哪里？（均值在 $SE(3)$，协方差在 $\mathfrak{se}(3)$）
• 位姿图优化的核心误差函数是什么？（$\ln ({\bar{\mathbf{T}}}_{k\ell }{\mathbf{T}}_{\ell }{\mathbf{T}}_k^{-1}{)}^{\vee }$）
• $S{E}_2(3)$ 包含哪些量？（旋转、位置、速度）
• IMU 导航为什么用右扰动？（噪声出现在运动方程的车体侧）

如果以上都清楚，你掌握了 3D 位姿估计的核心！

---

延伸阅读与代码 §

Python 实现（ICP 核心）：

import numpy as np
 
def skew(v):
    return np.array([[0, -v[2], v[1]],
                     [v[2], 0, -v[0]],
                     [-v[1], v[0], 0]])
 
def point_cloud_alignment(src_pts, dst_pts, weights=None):
    """
    用 SVD 求解点云对齐（Wahba 问题）
    src_pts, dst_pts: [N, 3] 数组
    返回: (R, t) 使得 dst ≈ R @ src + t
    """
    N = src_pts.shape[0]
    if weights is None:
        weights = np.ones(N) / N
    else:
        weights = weights / np.sum(weights)
 
    # 质心
    src_mean = np.sum(weights[:, None] * src_pts, axis=0)
    dst_mean = np.sum(weights[:, None] * dst_pts, axis=0)
 
    # 去心
    src_centered = src_pts - src_mean
    dst_centered = dst_pts - dst_mean
 
    # 互协方差矩阵
    W = np.sum(weights[:, None, None] *
               dst_centered[:, :, None] @ src_centered[:, None, :], axis=0)
 
    # SVD
    U, D, Vt = np.linalg.svd(W)
 
    # 修正符号确保 det(R) = +1
    S = np.eye(3)
    S[2, 2] = np.sign(np.linalg.det(U @ Vt))
 
    R = U @ S @ Vt
    t = dst_mean - R @ src_mean
 
    return R, t
 
 
def se3_ekf_predict(T, P, xi, Q, dt):
    """
    SE(3) EKF 预测步骤
    T: 当前位姿 (4x4)
    P: 协方差 (6x6)
    xi: 广义速度 [v; omega] (6,)
    Q: 过程噪声 (6x6)
    """
    # 均值预测
    Xi = np.eye(4)
    Xi[:3, :3] = so3_exp(dt * xi[3:])
    Xi[:3, 3] = dt * xi[:3]  # 简化；实际用左雅可比
    T_pred = Xi @ T
 
    # 协方差预测（伴随矩阵）
    F = se3_adjoint(Xi)
    P_pred = F @ P @ F.T + Q
 
    return T_pred, P_pred
 
 
def se3_ekf_update(T_pred, P_pred, y, p, R):
    """
    SE(3) EKF 校正步骤
    y: 观测 (3,)
    p: 地标在固定系中的位置 (3,)
    R: 测量噪声协方差 (3x3)
    """
    # 预测观测
    p_homo = np.append(p, 1)
    z = T_pred @ p_homo
    y_pred = z[:3]
 
    # 测量 Jacobian
    G = np.zeros((3, 6))
    G[:, :3] = np.eye(3)
    G[:, 3:] = -skew(y_pred)
 
    # 卡尔曼增益
    S = G @ P_pred @ G.T + R
    K = P_pred @ G.T @ np.linalg.inv(S)
 
    # 更新
    innovation = y - y_pred
    delta_xi = K @ innovation
 
    # 均值更新（指数映射）
    delta_T = np.eye(4)
    delta_T[:3, :3] = so3_exp(delta_xi[3:])
    delta_T[:3, 3] = delta_xi[:3]
    T = delta_T @ T_pred
 
    # 协方差更新
    P = (np.eye(6) - K @ G) @ P_pred
 
    return T, P
 
 
def pose_graph_optimization(poses, edges, max_iter=10):
    """
    位姿图优化（简化版）
    poses: [N, 4, 4] 初始位姿
    edges: [(i, j, T_ij_meas, Sigma_inv)] 边列表
    """
    T = poses.copy()
 
    for _ in range(max_iter):
        # 构建线性系统 A x = b
        # 这里简化，只展示核心思想
 
        for (i, j, T_meas, Sigma_inv) in edges:
            # 误差: ln(T_meas @ T_j @ T_i^{-1})^vee
            error_T = T_meas @ T[j] @ np.linalg.inv(T[i])
            e = so3_log(error_T[:3, :3])  # 简化，只取旋转部分
 
            # 构建 Jacobian（省略细节）
            # 求解更新量并应用
 
        # 收敛检查
 
    return T

---

下一章将处理同时估计位姿和地标位置的 SLAM 问题。