密西根大学移动机器人笔记-全-

密西根大学移动机器人笔记（全）

001：课程介绍

在本节课中，我们将要学习移动机器人学的基本概念、研究动机以及课程的整体框架。我们将探讨为什么机器人技术如此重要，并初步了解构建自主机器人系统所需的核心算法和数学基础。

为什么研究机器人学？🤖

你来到这里，说明你对机器人学有内在的兴趣。但除了兴趣，机器人学对社会有着切实的价值。

首先，机器人能让我们的生活更轻松、更安全。有些工作非常繁重或危险，例如搬运重物、进入火灾或灾难现场、采矿等。让机器人来完成这些任务，可以保护人类的安全。

其次，随着社会发展，一些重复性、高强度的劳动岗位可能面临人力短缺。机器人可以填补这些空缺，让人们能从事更具创造性和尊严的工作。

此外，机器人技术是具身智能的体现。与纯粹的软件人工智能不同，机器人是能够与环境进行物理交互的智能硬件。它融合了多种感知模态，如视觉、触觉、听觉等，这是机器人学独特且迷人的地方。

在医疗、护理、教育、娱乐、灾难响应、环境监测等领域，机器人都有着巨大的应用潜力和市场前景。例如，外骨骼机器人可以帮助行动不便的人重获独立；在疫情期间，多机器人系统可以高效规划并配送物资。

机器人学面临的挑战与核心问题 ⚙️

上一节我们介绍了机器人学的价值，本节中我们来看看实现机器人自主化面临哪些挑战，以及需要解决哪些核心问题。

观看一个机器人（如Cassie）在复杂环境中行走的视频时，我们不仅要欣赏其动作，更要思考背后的技术。核心挑战在于算法的鲁棒性。在实验室运行良好的算法，往往难以直接应用到真实世界，因为：

  • 模型不精确：我们对机器人动力学和环境的数学模型总是近似的。
  • 执行与感知的不完美：执行器（电机）和传感器（摄像头、IMU等）都存在误差和噪声。

一个自主机器人系统需要解决几个基本问题：

  1. 定位：机器人如何知道“我在哪里”？如果不知道自己的位置，就无法进行后续任务。
  2. 建图：机器人如何感知并构建周围环境的地图？
  3. 规划：在已知（或部分已知）的地图中，机器人如何规划从起点到目标点的路径？
  4. 控制：机器人如何执行规划好的动作，并保持稳定（例如双足行走的平衡）？

以森林行走的机器人为例，它可能仅依靠关节编码器（测量关节角度）和惯性测量单元（IMU，测量加速度和角速度）进行航迹推算，再通过接触检测（判断脚是否着地）来校正误差，最终利用不变卡尔曼滤波等先进算法实现高精度的轨迹跟踪，即使在没有视觉（“盲走”）的情况下也能工作。这展示了不依赖环境外观的本体感知的重要性。

自主系统的通用框架：感知-推理-行动 🔄

理解了具体挑战后，我们可以抽象出一个构建自主系统的通用框架。这个框架通常被称为“感知-推理-行动”循环或“信念-计划-行动”循环。

其运作方式如下：

  1. 世界与信念：存在一个真实世界，机器人对其有一个内部表示，称为“信念”。信念是机器人对世界状态（包括自身状态）的估计，由于信息不完备，它总是不确定和概率性的。
  2. 感知：机器人通过传感器（如摄像头、激光雷达）从世界获取数据（Z）。
  3. 更新信念：机器人利用感知数据，根据贝叶斯规则更新其信念。更新后的信念 bel(x_t) 是给定所有历史感知数据 z_{1:t} 和动作 u_{1:t} 后，状态 x_t 的概率分布。
    bel(x_t) = P(x_t | z_{1:t}, u_{1:t})
  4. 推理与规划：基于当前的信念，机器人进行推理，决定为了实现目标应该采取什么行动序列。
  5. 行动：机器人执行规划好的动作（u_t），作用于世界。
  6. 循环：行动改变了世界（也可能改变机器人自身状态），机器人获得新的感知数据，循环继续。

这个框架将复杂的自主系统分解为可研究的模块，每个模块（感知、状态估计、规划、控制）都有丰富的算法和研究内容。

机器人中的概率基础 📊

上一节我们介绍了自主系统的抽象框架，本节中我们来看看支撑该框架，特别是信念更新的数学基础——概率论。

在机器人学中，我们大量使用概率来描述不确定性。机器人的状态、传感器测量值、控制命令都不是绝对精确的，因此我们用随机变量和概率分布来建模它们。

以下是几个核心概念：

  • 概率密度函数：对于连续随机变量 x，其概率密度函数 p(x) 描述了 x 取不同值的可能性。满足 ∫ p(x) dx = 1。
  • 联合概率与条件概率：p(x, y) 是 x 和 y 同时发生的概率。如果已知 y 发生，那么 x 发生的条件概率为 p(x|y) = p(x, y) / p(y)。
  • 边缘化：如果只想求 x 的概率分布，可以对联合概率关于 y 积分（或求和）：p(x) = ∫ p(x, y) dy。这被称为“求和掉”或“边缘化”变量 y。
  • 贝叶斯规则：这是连接先验知识、观测数据和后验信念的桥梁。
    P(假设|数据) = [P(数据|假设) * P(假设)] / P(数据)
    其中：
    • 后验 = P(假设|数据)：看到数据后，对假设的更新信念。
    • 似然 = P(数据|假设)：在给定假设下，观察到当前数据的可能性。
    • 先验 = P(假设)：在看到数据之前，对假设的初始信念。
    • 证据 = P(数据)：数据本身的概率，通常作为一个归一化常数。

高斯（正态）分布 在机器人学中尤为常见，这主要得益于中心极限定理。其概率密度函数为：
p(x; μ, Σ) = (1/√((2π)^k |Σ|)) * exp(-1/2 (x-μ)^T Σ^{-1} (x-μ))
其中 μ 是均值向量，Σ 是协方差矩阵。

高斯分布有许多优良性质：

  • 线性变换下的封闭性：若 x ~ N(μ, Σ)，且 y = Ax + b，则 y ~ N(Aμ + b, AΣA^T)。
  • 边缘化和条件化的封闭性：高维联合高斯分布的边缘分布和条件分布仍然是高斯分布，且有解析解公式。这使得基于高斯假设的滤波器（如卡尔曼滤波器）计算非常高效。

一个简单的贝叶斯规则例子：一辆自动驾驶汽车在交通灯前停下。已知灯光循环（先验）和传感器在不同灯光下停车的概率（似然），我们可以计算在汽车停下的条件下，灯光是黄灯的概率（后验）。这展示了如何将因果信息（传感器模型）转化为诊断信息（状态推断）。

贝叶斯滤波器：一个统一的算法框架 🧠

基于前面的概率基础，我们现在可以形式化地描述自主系统核心的信念更新过程——贝叶斯滤波器。

贝叶斯滤波器为估计动态系统的状态（如机器人的位姿）提供了一个递归的、概率性的框架。它假设系统是马尔可夫的，即当前状态只依赖于前一个状态和当前控制输入。

算法需要两个模型：

  1. 运动模型：p(x_t | u_t, x_{t-1})。描述在控制命令 u_t 下，状态从 x_{t-1} 转移到 x_t 的概率。
  2. 观测模型：p(z_t | x_t)。描述在状态 x_t 下，产生观测 z_t 的概率。

贝叶斯滤波器的递归步骤如下：

  • 预测步：利用运动模型，结合上一时刻的信念和控制输入，预测当前时刻的先验信念。
    bel̄(x_t) = ∫ p(x_t | u_t, x_{t-1}) * bel(x_{t-1}) dx_{t-1}
  • 更新步：利用观测模型，结合新的观测数据 z_t，更新先验信念，得到后验信念。
    bel(x_t) = η * p(z_t | x_t) * bel̄(x_t)
    其中 η 是归一化常数，确保 bel(x_t) 积分为1。

这个框架非常通用，但直接计算（如在连续空间上积分）通常不可行。根据对噪声分布和系统线性程度的假设，会派生出不同的具体滤波器实现：

  • 如果状态空间离散且较小：可以使用马尔可夫定位，直接在离散网格上计算和存储每个状态的概率。
  • 如果系统是线性的，且噪声为高斯分布：卡尔曼滤波器提供了最优的闭式解，它递归地更新高斯分布的均值和协方差。
  • 如果系统是非线性的：可以使用扩展卡尔曼滤波器（对非线性进行线性化）或无迹卡尔曼滤波器（使用采样点近似分布）。
  • 对于更一般的非高斯、非线性系统：粒子滤波器通过一组随机样本（粒子）来近似表示概率分布，具有极大的灵活性。

本课程将引导你深入理解这些滤波器的原理、实现和应用。

课程安排与期望 📚

在本节中，我们将了解本课程的具体安排、学习工具以及对大家的期望。

本课程的目标是为你打下移动机器人算法研究的坚实基础。通过学习，你将能够阅读相关研究论文，理解不同算法的原理，并为在工业界或学术界开展机器人项目做好准备。

以下是课程相关的具体信息：

  • 交流平台：我们将使用 Piazza 进行课程问答。这是一个高效的平台，便于集中管理问题、分享解答和进行讨论（支持数学公式）。请避免通过邮件询问课程内容问题。
  • 学习小组：我们鼓励你通过Canvas上的学习小组功能结识同学，组成学习小组。你们可以一起讨论课程内容、作业（但最终作业需独立完成），这不仅能加深理解，也能建立宝贵的人际网络。
  • 作业：课程共有7次作业。我们会自动去掉你得分最低的一次作业（第1到第6次中的一次）。第7次作业是必做的。作业设计具有挑战性，旨在促进深度思考。如果遇到困难，请及时在Piazza上提问或寻求帮助。
  • 寻求帮助：如果你在课程学习中遇到任何个人困难（健康、家庭等），请尽早与我沟通。我们可以一对一交流，寻找解决方案。不要等到课程末期。

请合理安排时间，不要将所有作业拖到最后一天完成。有些作业需要较多时间思考和实现。

总结

本节课中我们一起学习了移动机器人学的入门知识。我们探讨了研究机器人学的多重意义，从提升生活品质到应对极端环境挑战。我们剖析了实现机器人自主性所面临的核心问题：定位、建图、规划与控制，并引入了“感知-推理-行动”这一通用框架来理解自主系统。

在数学基础上，我们回顾了概率论的核心概念，特别是贝叶斯规则和高斯分布，它们是描述和推理不确定性的基石。最后，我们介绍了贝叶斯滤波器这一统一的算法框架，它如何递归地更新机器人对世界的信念，并概述了其各种具体实现（如卡尔曼滤波器、粒子滤波器）。

本课程将带你深入这些主题，通过理论学习和实践编程，掌握移动机器人感知、估计和决策的关键方法与算法。期待与你共同探索机器人技术的精彩世界。

002：贝叶斯滤波器与卡尔曼滤波

在本节课中，我们将要学习贝叶斯滤波器的基本框架，并深入探讨其最经典的应用之一——卡尔曼滤波。我们将从递归概率推理的概念出发，逐步构建出完整的滤波算法，并理解其背后的假设与局限性。

概述

贝叶斯滤波器提供了一个概率框架，用于在存在不确定性的情况下，利用序列数据（如传感器测量和运动命令）来推断我们无法直接观测的变量（状态）。这种递归估计方法对机器人学尤其有吸引力，因为机器人数据本质上是时序的，并且系统需要实时运行。

贝叶斯滤波框架

上一节我们介绍了递归概率推理的基本动机。本节中，我们来看看如何将其形式化为一个通用的贝叶斯滤波框架。

在状态估计中，我们通常处理三类变量：

  • 状态 X：我们希望估计的变量，例如机器人的位置、速度或校准参数。
  • 控制输入 U：导致状态变化的动作或命令，如电机指令。
  • 观测 Z：从传感器获得的、关于状态的（带有噪声的）信息。

我们的目标是计算后验概率，即在给定所有历史观测 Z_{1:t} 和控制输入 U_{1:t} 的条件下，当前状态 X_t 的概率分布。这个后验分布被称为在时间 t 的置信度。

核心递归公式

贝叶斯滤波通过两个核心步骤递归地更新置信度：预测和校正。

  1. 预测步骤：利用运动模型，基于上一时刻的置信度和当前的控制输入，预测当前时刻的状态。
    bel_bar(x_t) = ∫ p(x_t | u_t, x_{t-1}) * bel(x_{t-1}) dx_{t-1}
    其中 p(x_t | u_t, x_{t-1}) 是概率运动模型。

  1. 校正步骤：利用传感器模型，将当前的观测信息融入预测的置信度中。
    bel(x_t) = η * p(z_t | x_t) * bel_bar(x_t)
    其中 p(z_t | x_t) 是传感器模型（似然），η 是一个归一化常数。

这个“预测-校正”的循环构成了所有贝叶斯滤波器的核心。

马尔可夫假设

为了使上述推导可行，我们引入了一个关键假设：马尔可夫假设。它声明，在已知当前状态 X_t 的条件下，未来的状态和观测与过去的历史无关。这意味着系统是“无记忆”的，当前状态包含了预测未来所需的全部历史信息。这个假设极大地简化了概率计算，使得递归更新成为可能。

从贝叶斯滤波器到卡尔曼滤波器

上一节我们介绍了通用的贝叶斯滤波框架。本节中，我们来看看当我们在该框架中加入一组特定的假设时，如何得到著名的卡尔曼滤波器。

卡尔曼滤波器是贝叶斯滤波器在线性高斯系统下的最优实现。它做出了以下关键假设：

  1. 线性动态系统：状态转移和观测模型都是线性的。
    x_k = A * x_{k-1} + B * u_k + w_k,  其中 w_k ~ N(0, Q)
z_k = H * x_k + v_k,               其中 v_k ~ N(0, R)
  2. 高斯噪声：过程噪声 w_k 和观测噪声 v_k 均为零均值高斯白噪声。
  3. 高斯初始状态：初始状态的置信度 bel(x_0) 是一个高斯分布。
  4. 独立性：所有噪声项之间以及它们与初始状态之间均相互独立。

在这些假设下，可以证明，置信度 bel(x_t) 在任何时刻都将保持高斯分布。因此，我们只需要递归地更新这个高斯分布的均值（状态估计值）和协方差（估计不确定性）即可。

卡尔曼滤波算法

以下是卡尔曼滤波算法的标准方程，它直接实现了贝叶斯滤波的“预测-校正”循环。

预测步骤（时间更新）：

x_hat_bar_k = A * x_hat_{k-1} + B * u_k          // 预测状态均值
P_bar_k = A * P_{k-1} * A^T + Q                 // 预测状态协方差

校正步骤（测量更新）：

z_hat_bar_k = H * x_hat_bar_k                   // 预测观测
y_k = z_k - z_hat_bar_k                         // 创新（Innovation）
S_k = H * P_bar_k * H^T + R                     // 创新协方差
K_k = P_bar_k * H^T * S_k^{-1}                  // 卡尔曼增益
x_hat_k = x_hat_bar_k + K_k * y_k               // 更新状态均值
P_k = (I - K_k * H) * P_bar_k                   // 更新状态协方差

卡尔曼增益 K_k 是一个关键的矩阵，它根据预测不确定性 (P_bar_k) 和传感器噪声 (R) 的相对大小，决定了我们应该在多大程度上信任新的观测 z_k。

卡尔曼滤波的应用与局限性

卡尔曼滤波器因其最优性和计算效率，被广泛应用于各个领域。

以下是卡尔曼滤波器的一些典型应用场景：

  • 导航与姿态估计：在无人机、航天器、手机中，融合IMU（陀螺仪、加速度计）数据来估计位置、速度和姿态。
  • 目标跟踪：在雷达或视觉系统中，跟踪车辆、飞机等运动目标的位置和速度。
  • 信号处理：过滤通信信号或传感器读数中的噪声。
  • 经济与金融：预测市场趋势（尽管模型假设常被打破）。

然而，卡尔曼滤波器的强大功能建立在严格的假设之上，这也导致了它的局限性：

  1. 非线性模型：如果系统的动态模型或观测模型是非线性的（例如，使用距离和方位角观测一个目标），标准卡尔曼滤波器将不再适用。这时需要扩展卡尔曼滤波或无迹卡尔曼滤波等方法。
  2. 非高斯噪声：如果噪声分布不是高斯的（例如，存在显著的离群值），卡尔曼滤波器可能不是最优估计器。
  3. 模型失配：如果目标的运动模式突然改变（如汽车急转弯），而滤波器的运动模型未能描述这种行为，滤波器可能会发散。
  4. 数据关联：在多目标跟踪场景中，需要将观测正确关联到对应的目标状态。错误的关联会向滤波器注入错误信息，导致失效。

总结

本节课中我们一起学习了贝叶斯滤波器的核心思想及其最著名的实例——卡尔曼滤波器。

我们首先建立了递归概率状态估计的框架，理解了如何通过“预测”和“校正”两步，融合运动与传感器信息来更新我们对系统状态的认知。随后，我们在线性高斯的特定假设下，推导出了卡尔曼滤波器的高效算法，该算法通过递归更新高斯分布的均值和协方差来实现最优估计。

最后，我们探讨了卡尔曼滤波器的广泛应用场景，也指出了其对模型线性、高斯噪声等假设的依赖所带来的局限性。这些局限性正是推动我们学习更高级滤波算法（如扩展卡尔曼滤波、粒子滤波）的动机。掌握卡尔曼滤波器，是深入理解现代机器人状态估计与导航技术的坚实基础。

003：非线性卡尔曼滤波

概述

在本节课中，我们将要学习非线性卡尔曼滤波。这是对上一讲贝叶斯滤波和线性卡尔曼滤波的延续。我们将看到两种处理非线性滤波问题的方法。学完本讲后，你应当能够理解为什么在某些情况下我们需要扩展卡尔曼滤波，而不是直接使用标准卡尔曼滤波，并清楚何时应选择非线性版本。

从贝叶斯滤波到非线性问题

上一节我们介绍了贝叶斯滤波的图形化表示，它展示了变量间的因果关系以及状态 X 如何随时间演变。这种图形模型有助于我们理解条件独立性，并推导出联合概率分布的因子化形式。

在本节中，我们来看看状态估计问题的核心目标：在给定观测 Z 和控制输入 U 的情况下，估计状态变量 X 的后验概率分布。具体实现这一目标的不同方法，就引出了卡尔曼滤波、粒子滤波等算法。我们关注的是滤波问题，即只估计当前时刻的状态，而不追踪历史。

贝叶斯滤波递归公式包含预测和更新两个步骤。预测步骤利用上一时刻的信念（后验）和运动模型来预测当前时刻的信念。更新步骤则利用贝叶斯规则，结合观测似然和预测的信念（作为先验）来进行修正。

我们之前处理的是线性系统，其模型形式如下：

  • 运动模型：x_t = F * x_{t-1} + B * u_t + w_t
  • 观测模型：z_t = H * x_t + v_t

其中 w_t 和 v_t 是噪声项。模型之所以是概率性的，正是因为这些噪声的存在。

非线性滤波的核心挑战

然而，许多实际问题的模型是非线性的。过程模型和观测模型可能具有如下形式：

  • 过程模型：x_t = f(x_{t-1}, u_t, w_t)
  • 观测模型：z_t = h(x_t, v_t)

这里的 f 和 h 是非线性函数。噪声可能是加性的（如 f(x) + w），也可能是更一般的乘性形式（噪声与状态耦合在非线性函数内部）。

这就引出了本讲的核心问题：如何将一个概率分布（例如高斯分布）通过一个非线性映射进行传播？ 对于线性（仿射）映射，我们可以精确地传播高斯分布的均值和协方差。但对于非线性映射，当我们尝试写出新分布均值和协方差的定义并展开时，会遇到无法处理的非线性项，无法得到闭式解。

以下是解决此问题的三种主要思路：

  1. 线性化：在某个操作点（如当前均值）对非线性函数进行泰勒展开，并仅保留一阶项。这引出了扩展卡尔曼滤波。
  2. 确定性采样：基于当前分布的均值和协方差，按照确定性规则生成一组样本点（Sigma点），将它们通过非线性函数传播，然后用这些输出点的加权均值和协方差来近似新分布。这引出了无迹变换及无迹卡尔曼滤波。
  3. 随机采样：从当前分布中随机抽取大量样本，将它们通过非线性函数传播，然后用输出样本的直方图来近似新分布。这属于蒙特卡洛方法，是粒子滤波的基础。

接下来，我们将详细探讨前两种方法。

扩展卡尔曼滤波

扩展卡尔曼滤波的核心思想是在每个滤波步骤中，对非线性模型进行局部线性化。

线性化方法

假设有一个非线性函数 f: R^n -> R^m。在操作点 x0 处进行一阶泰勒展开：

f(x) ≈ f(x0) + J_f(x0) * (x - x0)

其中 J_f(x0) 是函数 f 在 x0 处的雅可比矩阵（即导数）。对于加性噪声模型 x_t = f(x_{t-1}, u_t) + w_t，我们主要需要对状态 x 求导。对于更一般的模型 x_t = f(x_{t-1}, u_t, w_t)，还需要对噪声 w 求导。

EKF 算法步骤

基于线性卡尔曼滤波算法，我们将其修改以融入线性化步骤。输入是上一时刻的信念均值 μ_{k-1} 和协方差 Σ_{k-1}，以及当前的控制 u_k 和观测 z_k。

  1. 预测：

    • 均值预测：μ̄_k = f(μ_{k-1}, u_k) （直接使用非线性模型）
    • 协方差预测：Σ̄_k = F_k * Σ_{k-1} * F_k^T + W_k * Q * W_k^T
      • F_k 是 f 对状态 x 在 μ_{k-1} 处的雅可比矩阵。
      • W_k 是 f 对过程噪声 w 在 μ_{k-1} 处的雅可比矩阵。对于加性噪声，W_k 是单位阵。
      • Q 是过程噪声协方差。

  2. 更新：

    • 预测观测：z̄_k = h(μ̄_k)
    • 计算新息：y_k = z_k - z̄_k
    • 计算新息协方差：S_k = H_k * Σ̄_k * H_k^T + V_k * R * V_k^T
      • H_k 是 h 对状态 x 在 μ̄_k 处的雅可比矩阵。
      • V_k 是 h 对观测噪声 v 在 μ̄_k 处的雅可比矩阵。对于加性噪声，V_k 是单位阵。
      • R 是观测噪声协方差。
    • 计算卡尔曼增益：K_k = Σ̄_k * H_k^T * S_k^{-1}
    • 更新状态估计：μ_k = μ̄_k + K_k * y_k
    • 更新协方差估计：Σ_k = (I - K_k * H_k) * Σ̄_k

EKF 的特点与局限性

  • 高效性：与线性KF一样，计算复杂度是状态维度的多项式级（通常为平方或立方），适合实时应用。
  • 非最优性：由于线性化近似，EKF失去了线性KF的最优性保证。它只是一个次优滤波器。
  • 可能发散：如果非线性程度很强，或者线性化点选择不佳，线性近似误差会累积，可能导致滤波器发散。
  • 依赖雅可比计算：需要计算非线性函数的导数，可通过解析、符号计算或自动微分实现。

目标跟踪示例

考虑一个二维目标跟踪问题。我们自己的船位于原点，传感器测量目标的距离 r 和方位角 θ。

  • 状态：x = [x_pos, y_pos]^T
  • 观测：z = [r, θ]^T
  • 观测模型（非线性）：
    r = sqrt(x_pos^2 + y_pos^2)
θ = atan2(y_pos, x_pos)
  • 运动模型：假设目标匀速运动，则 x_t = x_{t-1} + w_t（线性）。

在这个例子中，运动模型是线性的，但观测模型是非线性的。我们可以计算观测模型 h 的雅可比矩阵 H，并在每个滤波步骤中，在预测的均值 μ̄_k 处计算 H_k，然后应用上述EKF算法。

无迹卡尔曼滤波

无迹卡尔曼滤波采用了一种不同的思路：它使用无迹变换来近似非线性传播，而不是进行显式的线性化。

无迹变换

UT的目的是计算经过非线性变换后的随机变量的均值和协方差。步骤如下：

  1. 选择Sigma点：根据当前分布的均值 μ 和协方差 Σ，按照确定性规则生成一组样本点（Sigma点）。一种常见的方法是：

    • 计算协方差矩阵的乔列斯基分解：Σ = L * L^T
    • 设置参数 κ（通常对于高斯分布，κ = 3 - n，n为状态维度）。
    • 生成 2n + 1 个Sigma点：
      χ_0 = μ
χ_i = μ + sqrt(n + κ) * L_i, for i = 1...n
χ_{i+n} = μ - sqrt(n + κ) * L_i, for i = 1...n
      其中 L_i 是矩阵 L 的第 i 列。

  2. 分配权重：为每个Sigma点分配权重，用于计算加权均值和协方差。

    w_0^{(m)} = κ / (n + κ)
w_0^{(c)} = κ / (n + κ) + (1 - α^2 + β)
w_i^{(m)} = w_i^{(c)} = 1 / [2(n + κ)], for i = 1...2n

    其中 α 和 β 是调节参数。简化版本常设 w_0^{(m)} = w_0^{(c)} = κ/(n+κ)，其余点为 1/[2(n+κ)]。

  3. 传播Sigma点：将每个Sigma点通过非线性函数 g 传播：Y_i = g(χ_i)。

  4. 计算近似均值和协方差：

    • 近似均值：μ_y ≈ Σ_{i=0}^{2n} w_i^{(m)} * Y_i
    • 近似协方差：Σ_y ≈ Σ_{i=0}^{2n} w_i^{(c)} * (Y_i - μ_y)(Y_i - μ_y)^T

UKF 算法步骤

将UT融入卡尔曼滤波框架：

  1. 预测：

    • 利用上一时刻的 μ_{k-1}, Σ_{k-1} 生成Sigma点。
    • 将每个Sigma点通过过程模型 f 传播。
    • 利用传播后的点计算预测均值 μ̄_k 和预测协方差 Σ̄_k。如果是加性噪声，最后加上过程噪声协方差 Q。

  2. 更新：

    • 利用预测的 μ̄_k, Σ̄_k 生成一组新的Sigma点。
    • 将每个Sigma点通过观测模型 h 传播，得到预测的观测点 Z_i。
    • 计算预测观测的均值 z̄_k 和协方差 S_k（以及状态与观测的互协方差 Σ_{xz}）。同样，对于加性观测噪声，需加上 R。
    • 计算卡尔曼增益：K_k = Σ_{xz} * S_k^{-1}
    • 更新状态和协方差。

UKF 的特点与局限性

  • 无需导数：UKF不需要计算雅可比矩阵，实现更方便。
  • 更好的非线性近似：UT通过采样捕捉了非线性变换的更多特征，通常比一阶线性化更准确。
  • 计算量稍大：需要生成和传播 2n+1 个Sigma点，计算量比EKF略高，但仍在同一数量级。
  • 仍为高斯近似：输出仍然是用高斯分布来近似后验，对于高度非高斯的结果（如多峰分布）仍然无能为力。
  • 采样效率：在高维空间中，Sigma点的数量线性增长，可能仍不足以充分捕捉分布形态。

极坐标到笛卡尔坐标变换示例

考虑一个简单例子：将一个点在极坐标 (r, θ) 中的高斯分布（均值 [1.5, π/6]，对角协方差）变换到笛卡尔坐标 (x, y)。

  • EKF方法：直接将均值 [1.5, π/6] 代入非线性函数 x = r*cosθ, y = r*sinθ 得到变换后的均值。
  • UKF方法：在极坐标空间生成Sigma点，将它们分别变换到笛卡尔坐标，然后计算这些输出点的加权均值。

有趣的是，这两种方法得到的“均值”可能不同。UKF得到的均值考虑了输入分布的形状经过非线性扭曲后的效果，通常更接近变换后分布的真实均值（通过蒙特卡洛大量采样估计）。而EKF的均值只是函数在输入均值处的取值。

总结

本节课我们一起学习了非线性卡尔曼滤波的两种主要方法：扩展卡尔曼滤波和无迹卡尔曼滤波。

  • EKF通过对非线性模型进行局部线性化（泰勒展开）来工作。它相对高效，但需要计算导数，且线性化误差在强非线性或不良线性化点下可能导致性能下降甚至发散。
  • UKF通过无迹变换来工作，这是一种确定性采样方法。它无需计算导数，通常能提供比EKF更准确的非线性近似，但计算量稍大。

两种方法都假设后验可以用高斯分布充分近似。它们都是次优的，但在许多实际问题中表现良好。选择哪种方法取决于具体问题：如果模型简单、导数易得且非线性不强，EKF是经典选择；如果模型复杂、导数难求或希望得到更稳健的性能，UKF可能更合适。

理解这些方法背后的核心思想——如何处理概率分布通过非线性映射的传播问题——对于掌握更高级的滤波和状态估计技术至关重要。下一讲我们将探讨第三种思路：基于随机采样的粒子滤波，它能处理非高斯的后验分布。

004：粒子滤波

在本节课中，我们将学习粒子滤波。粒子滤波是我们在探索状态估计或不确定性传播方法时涉及的第二个核心思想。它是一种基于随机采样的方法，特别适用于处理非线性、非高斯系统。

概述：非线性系统与不确定性传播

上一节我们介绍了基于确定性采样的无迹卡尔曼滤波。本节中，我们来看看基于随机采样的第三种方法——粒子滤波。

在现实中，我们通常处理的是非线性系统和测量模型。这些系统无处不在，例如自动驾驶飞行出租车或赛车，其运动模型往往是非线性的。我们的核心动机是解决状态估计任务，如跟踪、定位或感知。给定一个概率分布（由于现实中不存在完美的执行器和传感器），我们需要递归地通过非线性模型来传播这个“信念”。对于非线性系统，通常没有精确的解析解。

我们已经学习了两种线性化方法：基于泰勒展开的扩展卡尔曼滤波和基于确定性采样的无迹卡尔曼滤波。今天，我们将讨论基于随机采样的第三种思想。

粒子滤波的核心思想

粒子滤波的概念非常直观。其基本思想是：利用计算机模拟后验概率分布的演化，使用测量值对不同的模拟假设（即“粒子”）进行加权，从而追踪最可能的状态。每个粒子代表一个可能的假设或状态实例。

这种方法属于更广泛的蒙特卡洛方法范畴，其基础是随机采样和模拟。与之前需要复杂代数推导的滤波器相比，粒子滤波通常更易于实现。此外，它能够处理非高斯、多模态的后验分布，而卡尔曼滤波器族通常仅限于跟踪高斯分布。

我们将重点讨论顺序蒙特卡洛方法，因为机器人数据是按时间顺序到达的，我们需要递归地实时处理它们。当问题具有非高斯、高维和非线性特征时，就应考虑使用粒子滤波。一个典型例子是“机器人绑架问题”，即机器人被突然移动到别处，粒子滤波因其能跟踪多个假设而能更好地处理此类问题。

数学基础：狄拉克δ函数与理想蒙特卡洛采样

在深入算法之前，我们需要回顾一个重要的数学概念：狄拉克δ函数。δ函数在特定点a处为无穷大，在其他所有地方为零，并且其积分为1。在概率意义上，它可以被视为将所有概率质量集中在一个点上的概率测度。δ函数的一个关键性质是：∫ f(x) δ(x - a) dx = f(a)，这使得它能够“评估”函数在特定点的值。

现在，让我们设想一个理想情况：假设我们已知真实的后验概率分布 p(x)，并且可以从中独立同分布地抽取N个样本 {x⁽ⁱ⁾}。

我们可以用这些样本来构建后验分布的一个经验估计：

p̂_N(x) = (1/N) * Σ_{i=1}^{N} δ(x - x⁽ⁱ⁾)

这个估计将概率质量均匀地（每个权重为 1/N）放置在每个样本点上。基于此，我们可以近似计算关于后验分布的任意函数 f(x) 的期望值：

E[f(x)] ≈ (1/N) * Σ_{i=1}^{N} f(x⁽ⁱ⁾)

根据大数定律，随着样本数 N 趋于无穷，这个估计会以概率1收敛到真实值，且是无偏的。其收敛速率与积分维度无关，这避免了传统数值积分方法（如黎曼积分）中的“维度灾难”问题。

然而，这个方法存在一个根本性问题：我们并不知道真实的后验分布 p(x)。如果知道，问题就已经解决了。因此，我们需要一种方法来绕过这个障碍。

重要性采样：从已知分布中采样

为了解决未知真实分布的问题，我们引入了重要性采样。其核心思想是：选择一个我们已知且易于采样的分布 π(x) 作为提议分布（或重要性分布），并要求其支撑集包含真实后验 p(x) 的支撑集。

我们通过一个巧妙的变换来重写期望积分：

E[f(x)] = ∫ f(x) p(x) dx = ∫ [f(x) * (p(x)/π(x))] π(x) dx

定义重要性权重 w(x) = p(x) / π(x)。那么，期望可以近似为：

E[f(x)] ≈ (1/N) * Σ_{i=1}^{N} f(x⁽ⁱ⁾) w(x⁽ⁱ⁾)

在实际计算中，我们通常使用归一化的重要性权重：

E[f(x)] ≈ Σ_{i=1}^{N} f(x⁽ⁱ⁾) w̃⁽ⁱ⁾

其中，归一化权重 w̃⁽ⁱ⁾ = w⁽ⁱ⁾ / Σ_{j=1}^{N} w⁽ⁱ⁾。

这样，我们通过从已知的提议分布 π(x) 中采样，并用权重 w(x) 来修正样本的重要性，从而近似了关于未知分布 p(x) 的期望。

顺序重要性采样：递归权重更新

然而，上述方法仍不足以进行递归的状态估计。我们需要一个能随时间递归更新权重的方法，即顺序重要性采样。

通过利用贝叶斯规则、马尔可夫假设以及模型的链式分解，我们可以推导出权重更新的递归公式。假设在时间步 k，第 i 个粒子的权重 w_k⁽ⁱ⁾ 可以递归地计算为：

w_k⁽ⁱ⁾ ∝ w_{k-1}⁽ⁱ⁾ * [p(z_k | x_k⁽ⁱ⁾) * p(x_k⁽ⁱ⁾ | x_{k-1}⁽ⁱ⁾)] / π(x_k⁽ⁱ⁾ | x_{0:k-1}⁽ⁱ⁾, z_{1:k})

这个公式看起来很复杂，但它包含了我们熟悉的项：似然函数 p(z_k | x_k) 和运动模型 p(x_k | x_{k-1})。提议分布 π 是我们选择的设计参数。

SIR粒子滤波：机器人学的实用选择

对于机器人定位问题，一个特别实用且常见的选择是：将提议分布设置为先验分布，即运动模型：π(x_k | ...) = p(x_k | x_{k-1})。

代入递归权重公式后，得到了极大的简化：

w_k⁽ⁱ⁾ ∝ w_{k-1}⁽ⁱ⁾ * p(z_k | x_k⁽ⁱ⁾)

这意味着，新的权重正比于旧权重乘以当前观测的似然值。这非常直观：粒子根据运动模型移动，然后根据传感器测量值的好坏来调整其权重。

由此，我们得到了采样重要性重采样粒子滤波算法，其核心步骤如下：

  1. 预测（采样）：对于每个粒子，根据运动模型 p(x_k | x_{k-1}) 采样其下一个状态。
  2. 更新（加权）：根据观测模型（似然）p(z_k | x_k) 计算每个粒子的权重，并归一化。
  3. 重采样：根据权重，复制高权重粒子，淘汰低权重粒子，从而得到一组新的、权重均匀的粒子集。

以下是算法的伪代码描述：

# SIR粒子滤波算法
初始化：生成N个服从初始信念 p(x0) 的粒子 {x0⁽ⁱ⁾}，权重 w⁽ⁱ⁾ = 1/N

对于每个时间步 k：
  对于每个粒子 i：
    # 1. 重要性采样（预测）
    从提议分布（运动模型）中采样：x_k⁽ⁱ⁾ ~ p(x_k | x_{k-1}⁽ⁱ⁾)
    # 2. 重要性加权（更新）
    计算权重：w_k⁽ⁱ⁾ = w_{k-1}⁽ⁱ⁾ * p(z_k | x_k⁽ⁱ⁾)
  结束循环

  # 权重归一化
  总权重 = Σ w_k⁽ⁱ⁾
  对于每个粒子 i：
    w_k⁽ⁱ⁾ = w_k⁽ⁱ⁾ / 总权重
  结束循环

  # 3. 重采样（可选，但通常需要）
  如果有效粒子数 N_eff = 1 / Σ (w_k⁽ⁱ⁾)² 低于阈值（如 N/2）：
    根据归一化权重 {w_k⁽ⁱ⁾} 重采样N次，生成新的粒子集 {x_k⁽ⁱ⁾}，并重置所有权重为 1/N
  结束如果
结束循环

算法挑战：退化与样本贫化

SIR粒子滤波虽然强大，但也面临两个主要挑战：

  1. 退化问题：在顺序重要性采样中，经过几次迭代后，除少数粒子外，大多数粒子的权重会变得非常小，这意味着计算资源被浪费在无关紧要的假设上。衡量退化的一个常用指标是有效粒子数：N_eff = 1 / Σ (w⁽ⁱ⁾)²。当 N_eff 很小时，表明发生了退化。

  2. 样本贫化问题：重采样是解决退化问题的关键，它淘汰低权重粒子，复制高权重粒子。然而，这会导致样本多样性丧失。多次重采样后，许多粒子可能源于同一个祖先粒子，从而无法有效表示多模态分布或应对突然的状态变化（如绑架问题）。

在实践中，我们通过设置重采样阈值（不每步都重采样）和使用诸如低方差重采样等技巧来在退化与样本贫化之间寻求平衡。

应用示例：目标跟踪

让我们看一个简单的目标跟踪例子。假设目标运动模型为恒定速度模型，状态包括位置和速度。观测模型可能是传感器获取的距离和方位角。

通过运行SIR粒子滤波，我们可以同时估计目标的位置和速度。与卡尔曼滤波相比，粒子滤波在此类非线性、可能非高斯的问题上通常表现更鲁棒，尤其是在初始不确定性很大或存在多模态假设时。

总结

本节课中，我们一起学习了粒子滤波。它是一种基于随机采样的非参数滤波方法，核心思想是通过模拟大量假设（粒子）的演化，并用观测数据对其加权，来近似复杂的后验概率分布。

我们回顾了理想蒙特卡洛采样，引入了重要性采样以处理未知真实分布，并推导了顺序重要性采样以实现递归更新。最终，我们得到了适用于机器人学的SIR粒子滤波算法，并讨论了其核心步骤（预测、更新、重采样）以及面临的挑战（退化、样本贫化）。

粒子滤波的优势在于能够处理非线性、非高斯、多模态的估计问题，并能应对全局不确定性（如机器人绑架问题）。其代价是计算效率，特别是在高维状态空间中，需要大量粒子才能保证估计质量。然而，对于许多机器人定位和跟踪任务，粒子滤波仍然是一种强大而实用的工具。

005：对称性与刚体运动

在本节课中，我们将学习刚体变换。我们将从基础开始，讨论线性变换，并触及对称性这一主题。我们还将探讨对称性与这些变换之间的关系。课程内容将尽可能简单直白，确保初学者能够理解。

线性变换

首先，我们来看看什么是线性变换。线性变换是一种映射，它保持向量空间的两个基本运算：向量加法和标量乘法。这意味着，如果你有一个作用于向量空间的映射，它将保持线性组合的结构。

具体来说，对于一个线性映射 T，以及任意向量 a、b 和标量 c，它满足以下性质：

  • T(c * a) = c * T(a)
  • T(a + b) = T(a) + T(b)

更一般地，对于任意标量 α、β 和向量 a、b，有：
T(αa + βb) = αT(a) + βT(b)

线性变换的一个重要特性是它固定原点。这意味着零向量经过变换后仍然是零向量：T(0) = 0。因此，线性变换不会平移整个空间。

在编程中，我们可以轻松验证这些性质。例如，在 Julia 中定义一个 2x2 的变换矩阵 T 和两个向量 a、b，你可以验证 T * (a + b) 是否等于 T*a + T*b。

以下是线性变换的一些例子：

  • 缩放：将向量按一定比例放大或缩小。这是一个连续变换。
  • 反射：关于某条直线或平面对向量进行镜像。这是一个离散变换。
  • 旋转：围绕一个点或轴转动向量。这是我们研究的重点之一。

那么，平移是线性变换吗？答案是否定的。平移变换的形式是 T(x) = x + t。如果我们应用零向量，T(0) = t，这并不等于零，因此它不固定原点，违反了线性变换的定义。平移属于仿射变换。

旋转矩阵推导

上一节我们介绍了线性变换，本节中我们来看看如何推导旋转矩阵。旋转是一种线性变换，我们可以用矩阵来表示它。

让我们以二维旋转为例进行推导。假设我们有一个标准的二维坐标系，基向量为 e₁ 和 e₂。现在，我们将整个坐标系（作为一个刚体）绕原点旋转一个角度 θ，得到新的基向量 e₁‘ 和 e₂’。

我们的目标是找到这样一个线性映射（矩阵），它能够将原始坐标系下的向量坐标，转换到旋转后的新坐标系下来描述（或者等价地，将向量本身在固定坐标系中旋转 θ 角度）。

关键在于，我们可以将新基向量 e₁‘ 和 e₂’ 用原始基向量来表示。通过投影，我们得到：

  • e₁‘ = [cosθ, sinθ]ᵀ
  • e₂‘ = [-sinθ, cosθ]ᵀ

现在，考虑空间中的一个固定向量 v。它在原始坐标系中的坐标为 (v₁, v₂)，即 v = v₁e₁ + v₂e₂。在旋转后的坐标系中观察同一个向量，其坐标 (v₁‘, v₂’) 满足 v = v₁‘e₁’ + v₂‘e₂’。

将 e₁‘ 和 e₂’ 的表达式代入，并比较 v 在原始基下的表达式，我们可以推导出坐标变换关系：

v₁‘ = cosθ * v₁ - sinθ * v₂
v₂‘ = sinθ * v₁ + cosθ * v₂

将其写成矩阵形式：

[v₁‘]   [cosθ  -sinθ] [v₁]
[v₂‘] = [sinθ   cosθ] [v₂]

这个矩阵就是二维旋转矩阵 R(θ)。这种推导方式体现了线性系统的叠加性质。

对称性与李群

我们刚刚推导了旋转矩阵，现在让我们思考一个更深层次的概念：对称性。对称性在数学和物理学中至关重要，而李群是描述连续对称性的最佳数学工具。

考虑一个完美的圆。有哪些变换作用于这个圆之后，会使它看起来完全没有变化？显然，绕其圆心的任意角度旋转都不会改变圆的外观。前提是这个变换不能缩放（改变大小）或扭曲圆。这种保持任意两点间距离不变的变换称为刚体变换。

因此，所有二维旋转的集合构成了圆的对称群。这是一种连续对称性。类似地，一个完美球体的对称群是所有三维旋转的集合。

李群之所以重要，正是因为它完美地捕捉了空间的这种连续对称结构。在机器人学中，我们处理关节、传感器和坐标系的几何关系时，李群会自然而然地出现。

一个有趣的观察是二维旋转与复数单位圆之间的联系。根据欧拉公式，e^(iθ) = cosθ + i sinθ，它代表了复平面上的单位圆。用这个复数乘以一个向量（视为复数），等价于将该向量旋转 θ 角度。更重要的是，角度相加对应于旋转相乘（e^(iθ₁) * e^(iθ₂) = e^(i(θ₁+θ₂))）。这个性质在高维（矩阵旋转）中依然成立，只是乘法的顺序变得重要（矩阵乘法不可交换）。这启示我们可以用代数（加法）来处理几何（旋转乘法）问题，这正是李代数的核心思想：将复杂的几何运算转化为相对简单的向量空间运算。

三维旋转与万向节锁

现在，我们将概念扩展到三维空间。三维旋转可以用绕 X、Y、Z 轴的基本旋转矩阵组合而成。例如，绕 Z 轴的旋转矩阵为：

R_z(ψ) = [cosψ  -sinψ   0]
         [sinψ   cosψ   0]
         [  0      0     1]

绕 X 轴和 Y 轴的矩阵结构类似。

一种常见的组合方式是使用欧拉角（例如 Z-Y-X 顺序），即 R = R_z(ψ) * R_y(θ) * R_x(φ)，分别对应偏航角、俯仰角和滚转角。然而，欧拉角存在一个著名的问题：万向节锁。

当俯仰角 θ 为 ±90° 时，滚转和偏航轴会重合，导致失去一个旋转自由度。从数学上看，这是因为三维旋转空间（一个三维流形）无法用一个三参数坐标系（如欧拉角）无奇异地覆盖整个空间。在表示旋转时，轴-角对 (n, θ) 和 (-n, -θ) 表示相同的旋转，这种双重性导致了奇异性。

在实践中，万向节锁是真实存在的问题（例如阿波罗任务中曾遇到）。因此，在许多先进的姿态估计器中，人们使用四元数。四元数是复数的扩展，用四个数表示三维旋转，它没有万向节锁问题，尽管每个旋转对应一对符号相反的四元数。

旋转矩阵 R 具有两个关键性质：

  1. 正交性：Rᵀ R = R Rᵀ = I，因此其逆等于其转置：R⁻¹ = Rᵀ。
  2. 行列式为1：det(R) = 1。这保证了变换是纯旋转（保持手性），而不是反射（行列式为 -1）。

所有满足 RᵀR = I 且 det(R) = 1 的 3x3 矩阵构成的集合，称为特殊正交群 SO(3)。它是描述刚体所有可能朝向的数学空间。

刚体运动与齐次坐标

到目前为止，我们主要讨论了旋转。但刚体的完整运动包括旋转和平移。这种组合变换称为刚体运动或欧几里得运动，其形式为：
p‘ = R * p + t
其中 R 是旋转矩阵，t 是平移向量。

为了更紧凑和方便地处理这种变换（特别是连续变换），我们引入齐次坐标。我们将一个三维点 p = [x, y, z]ᵀ 表示为四维向量 [x, y, z, 1]ᵀ。相应地，平移向量 t 表示为 [t_x, t_y, t_z, 0]ᵀ。

利用齐次坐标，刚体变换可以表示为一个 4x4 矩阵：

    [ R    t ]
H = [        ]
    [ 0ᵀ   1 ]

其中 0ᵀ 是行向量 [0, 0, 0]。当用 H 乘以一个点的齐次坐标 P = [p, 1]ᵀ 时：
H * P = [R*p + t, 1]ᵀ
这正好实现了旋转加平移的操作。

这种表示法的巨大优势在于，连续施加多个刚体变换只需进行矩阵乘法即可。变换 H_ab 表示从坐标系 B 到 A 的变换。那么，从 C 到 A 的变换为：H_ac = H_ab * H_bc。矩阵乘法自动处理了旋转和平移的顺序（先旋转后平移）。

所有这种 4x4 刚体变换矩阵构成的集合称为特殊欧几里得群 SE(3)。需要注意的是，与旋转一样，刚体变换的乘法也不可交换：H1 * H2 ≠ H2 * H1。

应用与动机：状态估计中的刚体运动

在移动机器人中，刚体运动是许多核心问题的基础。例如，在定位和建图中，我们需要持续估计机器人（传感器）相对于一个固定世界坐标系的位姿（即 SE(3) 中的变换）。

考虑一个搭载激光雷达的移动机器人。激光雷达每秒旋转多次，同时机器人自身也在运动。为了构建环境的一致地图，我们必须精确地补偿机器人的运动，将每一时刻扫描到的点云都变换到一个共同的参考系中。这个过程本质上就是在求解一系列随时间变化的 SE(3) 变换。

此外，在状态估计（如卡尔曼滤波）中，我们需要对机器人的位姿（属于 SO(3) 或 SE(3)）及其不确定性进行建模。直接在非线性流形（如旋转矩阵空间）上处理概率分布非常困难。然而，李群理论为我们提供了指数坐标——一种附着在流形每一点的局部线性空间（李代数）。在李代数中，分布可以近似为高斯分布，许多非线性问题会变得线性化，从而简化滤波器的设计和计算。这揭示了学习李群李代数对于高级机器人状态估计的重要性。

总结

本节课中，我们一起学习了刚体运动的核心数学工具。

  1. 我们从线性变换的定义和性质出发，理解了旋转是一种线性变换，而平移不是。
  2. 我们详细推导了二维旋转矩阵，并理解了其几何意义。
  3. 我们引入了对称性和李群的概念，认识到旋转群是圆或球体的对称群，李群是描述连续对称性的强大框架。
  4. 我们探讨了三维旋转的表示方法（旋转矩阵、欧拉角），并指出了欧拉角的万向节锁问题。
  5. 我们将旋转和平移结合，定义了刚体运动，并引入齐次坐标和 4x4 变换矩阵来方便地表示和计算它，对应的数学结构是 SE(3) 群。
  6. 最后，我们了解了刚体运动在机器人定位、建图和状态估计中的关键作用，并指出了李群李代数在这些问题中用于处理非线性几何约束和不确定性的优势。

这些概念为后续学习机器人运动学、动力学以及高级状态估计方法奠定了坚实的基础。

006：机器人学中的矩阵李群 I 🧮

在本节课中，我们将学习矩阵李群。李群是以数学家索菲斯·李的名字命名的。我们将探讨为什么在机器人学中需要学习这个概念，并介绍群、矩阵群、流形、切空间和李代数等核心数学工具。这些工具是理解和处理机器人运动、感知和状态估计中连续对称性的基础。

为什么需要李群？🤔

我们首先探讨为什么需要讨论李群。对于初学者来说，这可能是第一次以这种系统的方式接触这个概念。你可能会问，能否跳过这些，只使用简单的工具？当然可以，但你的知识可能会落后于当前主流文献20到30年。再过10年，你可能就完全与社区脱节了。我们不想成为数学家，但需要理解这些概念，为什么需要它们，以及为什么在某些应用中它们是首选的工具。

以下是一些更实际的动机：

  • 学习对图像旋转不变的特征：从概念上讲，我们都理解，如果要分类猫的照片，我们不会因为图像的朝向而被误导。无论图像是正放还是倒放，我们都能认出这是一张猫的照片。然而，对于计算机来说，要做到对旋转不敏感却非常困难。在深度学习和机器学习中，人们需要进行数据增强，考虑图像所有不同的朝向以扩展数据集。但不可能考虑所有无限种可能的朝向。现代方法是通过对称性的视角来看待这个问题，关键在于我们希望特征对于回归是等变的（即与输入具有相同的朝向），或者对于分类是不变的（即独立于图像的朝向）。如果能实现这一点，就能得到一个对图像朝向不敏感的分类器。这是一个非常强大的思想，其核心在于利用对称性。

  • 三维点云与地点识别：在三维点云中，我们也希望特征不会因为观察视角的不同而发生巨大变化。例如，在一个购物中心，我们希望无论从哪个视角看，都能识别出这个地方。这虽然看似与计算机视觉或机器学习相关，但最终我们也需要它来进行机器人视觉和感知。所有这些都可以统一在利用对称性这个概念之下。

  • 连续对称性与数学框架：对于像图像旋转这样的连续对称性，你可以连续地、任意小地旋转图像。我们需要一个能有效描述这种连续对称性的数学框架，这就是李群的主题。李群是我们研究不同空间连续对称性的最佳工具。

  • 机器人状态估计与刚体运动：在本课程中，我们将处理机器人状态估计问题，即如何对物体或关节机器人的刚体运动进行建模。我们在刚体运动讲座中已经简单涉及过。矩阵李群特别是刚体运动理论的更大框架，它是该主题的超集。我们特别关注旋转、平移和刚体运动。

  • 单目相机三维重建：在计算机视觉和图形学中，使用单目相机的问题是缺乏深度信息。没有深度，就无法获得尺度。在运动恢复结构等任务中，尺度是一个每次求解场景重建问题时都会变化的参数。因此，存在一个比刚体变换群更大的李群，它多了一个尺度维度，在计算机视觉中称为相似变换群，用于单目视觉。这也是一个李群。

  • 惯性测量单元建模：我们购买的每一部手机、平板电脑、机器人几乎都配备了IMU（惯性测量单元）。如何正确地对IMU进行建模和积分，是一个困扰了人们30多年的问题。如果能够正确做到这一点，就解决了航位推算问题。这本质上就是正确积分的方法。所谓“正确”，是指理解角速度、线速度和加速度等自然测量量所在的空间，并以尽可能接近它们在自然界中行为的方式对它们进行建模。

所有这些都使得李群的研究成为一个非常有吸引力的领域。

群的定义 📚

现在，李群本身是一个广阔的主题，是数学和物理学中的一个研究领域。我们只关注我们关心的部分。首先，让我们回顾一下什么是群。这里的“群”是一个数学意义上的技术术语。

当我们说群时，指的是满足特定性质的一组对象的集合。以下是群必须满足的四个性质：

  1. 封闭性：群由一些元素（例如 G 和 H）和一个二元运算（例如 ∘）组成。无论这个运算是什么，两个群元素运算的结果必须仍然属于这个群。你不能通过该运算跑到群外面去。
  2. 结合律：运算的顺序（在括号意义上）不重要。即 (G ∘ H) ∘ L = G ∘ (H ∘ L)。
  3. 单位元：每个群都有一个单位元 E。单位元是群的一个元素，使得从任何一侧用它进行运算都不会改变结果：E ∘ G = G ∘ E = G。
  4. 逆元：每个群元素都必须有逆元。逆元 G^{-1} 的定义是，无论从左还是右乘以它，都会得到单位元：G^{-1} ∘ G = G ∘ G^{-1} = E。

你可能熟悉这些结构。让我们验证两个群：

  • 实数加法群 (ℝ, +)：
    • 封闭性：两个实数相加仍是实数。
    • 结合律：(a + b) + c = a + (b + c)。
    • 单位元：0，因为 a + 0 = 0 + a = a。
    • 逆元：-a，因为 a + (-a) = 0。

  • 非零实数乘法群 (ℝ\{0}, ×)：
    • 封闭性：两个非零实数相乘仍是非零实数。
    • 结合律：(a × b) × c = a × (b × c)。
    • 单位元：1，因为 a × 1 = 1 × a = a。
    • 逆元：1/a，因为 a × (1/a) = 1。

这些概念是抽象的，可以推广到许多不同类型的运算和对象。我们关心的是实数和矩阵，这已经足够我们使用了。

矩阵群 🧩

当我们说矩阵群时，你可以想到上述四个性质。此外，矩阵群的元素是矩阵。除了矩阵构成一个群之外，我们还意味着这些矩阵是可逆的。因此，当我们说矩阵群时，指的是可逆矩阵的群。

可逆矩阵只对方阵有定义。所以，矩阵群通常由 n × n 的可逆矩阵组成（其行列式不为零）。这个群有一个名字，叫做一般线性群 GL(n, ℝ)。这是你能得到的最大的矩阵群。所有实数的 n × n 可逆矩阵都属于这个群。

我们可以为矩阵群添加更多结构，使其变得更特殊。

仿射群与特殊欧几里得群 🏗️

第一个重要的群是仿射变换群。仿射变换对一个向量施加一个线性映射再加上一个平移：y = A x + t。我们上次学到的技巧是，如果我们把矩阵构造成以下形式：

[ A   t ]
[ 0   1 ]

其中 A 是 n × n 线性部分，t 是 n × 1 平移向量，底部添加一行零和一个1。对于 ℝ^n 中的每个向量，如果我们给它附加一个1，即 [x; 1]，我们就可以通过矩阵乘法得到仿射群的作用。这就是该群在该向量空间上的作用。

仿射群非常通用，包含了缩放、旋转、平移以及任何可能的剪切畸变。平移本身也可以看作一个矩阵群。在机器人运动学中，这非常方便，因为无论我们是平移还是旋转，都可以始终进行矩阵乘法，例如用于建模正向运动学或逆向运动学。

现在，让我们回到一般线性群，构造另一个群。考虑满足以下约束的矩阵集合：

R^T R = I

这是旋转矩阵的约束，即正交矩阵。正交矩阵的逆就是它的转置。这个条件给出了正交群 O(n)。这些是旋转和反射。因为对一个物体施加线性变换而不缩放或扭曲它，并保持两点间距离的唯一方式就是旋转它或通过一个轴反射它。

从 R^T R = I 两边取行列式，我们得到 (det(R))^2 = 1。因此，det(R) = +1 或 det(R) = -1。这最终得到两个矩阵集合：行列式为 +1 的矩阵（旋转）和行列式为 -1 的矩阵（反射）。它们不能合并，因为它们是独立的集合。正交群是这两个变换集合的不交并。

我们特别感兴趣的是特殊正交群 SO(n)，即行列式为 +1 的正交矩阵群。在矩阵群的语言中，“特殊”通常意味着行列式为 +1。

研究这些群的一个特殊原因是，我们想讨论欧几里得空间的等距变换。等距变换是一个一一对应且满射的变换，它保持某种距离（例如通常的欧几里得距离）。对于刚体，这听起来像我们对刚体的定义：刚体是一组粒子，任何两个粒子之间的距离在空间中移动时必须保持固定。当你听到等距变换时，它谈论的是同一个主题。

如果它是线性等距变换，我们知道线性变换固定原点。然而，你也可以有平移，那么它就不是线性的。我们知道平移不会改变两点之间的距离。所以平移不是线性变换，但它是等距变换。因此，旋转和平移一起构成了直接等距变换。反射被称为间接等距变换或非正常等距变换。

SO(n) 物理上是什么？它是 n 个垂直平面同时旋转的描述。在三维中，你有三个平面，可以同时旋转它们。这与将它们视为绕 e1, e2, e3 轴的独立旋转（如欧拉角）有根本不同。当我们使用李代数时，每个基向量会给出一个基，我们在将它们转换为旋转矩阵之前组合它们；而使用欧拉角时，你将每个角独立地转换为旋转矩阵然后组合它们。这不是一个好方法。

流形与切空间 🗺️

现在我们来谈谈流形。流形是任何可以局部“展平”为欧几里得空间的形状或曲面，例如圆、球体、环面，甚至欧几里得空间本身也是一个（平坦的）流形。这个概念很重要，因为我们所有的微积分都是针对欧几里得空间的。如果我们能在局部将曲面描述为平坦的，我们就可以将微积分应用到那个区域。

为了理解曲面的几何，我们需要讨论切空间和切向量。想象一个曲面嵌入在三维环境空间中。在这个曲面上某一点，考虑所有经过该点的光滑轨迹（如纹在手臂上的图案）。在特定点，我们可以谈论切向量（例如轨迹的速度）。所有可能的切向量（不同方向、不同大小）的集合构成了该点处的切空间 T_x M，其中 M 是流形，x 是该点。

对于欧几里得空间，任意点处的切空间都是相同的。但对于曲面，切空间取决于你所处的点。流形的维度就是其切空间的维度。例如，圆的维度是1（即使它嵌入在 ℝ^2 中），球面的维度是2。

为什么我们需要这个？因为当我们谈到李代数时，我们需要这种几何直观来定义李代数。

作为几何对象的矩阵群 📐

代数对象不一定给出几何图形。我们将声称矩阵也是几何对象，即矩阵群。因为如果你有一个 2×2 的实矩阵，你可以很容易地将它们排列成一个四维向量，然后在四维空间中工作。矩阵群的定义中的约束会形成该环境空间的一个子集。因此，在这个意义上，我们可以说服自己矩阵是几何对象，因为它们是某个欧几里得空间的子集。你还可以证明它们是非常好的对象，因为乘法、可逆性等操作都是光滑的，不会产生任何奇点。所以我们确实得到了一个光滑流形。

李代数 ⚙️

现在我们可以谈论矩阵群的切空间。我们刚刚定义了切空间。李代数 就是一个矩阵群在单位元处的切空间。我们用哥特体字母表示，例如 𝔤 表示群 G 的李代数。

选择单位元有一个非常简单的理由：所有群都有单位元。你不需要为每个群重新制定规则。单位元处的切空间是一个向量空间，是所有初始速度的空间。我们将看到，陀螺仪给出角速度，该角速度就在 SO(3) 的李代数中。它自然地在这个空间中被建模。当你对它积分时，你基本上就是在积分陀螺仪数据以跟踪方向。

我们可以讨论每个特定矩阵群的李代数。这部分需要通过作业和练习来掌握。

  • 一般线性群 GL(n, ℝ) 的李代数：可以证明，GL(n, ℝ) 的李代数 𝔤𝔩(n, ℝ) 就是所有 n × n 矩阵的集合（包括奇异矩阵）。逻辑是：取一条路径 γ(t) = I + t A，其中 A 是某个 n × n 矩阵。这条路径经过单位元 I。其导数 γ'(0) = A。对于足够小的 t，由于行列式是连续函数，γ(t) 的行列式非零，因此 γ(t) 仍在一般线性群中。所以 A 是李代数中的元素。由于 A 可以是任何矩阵，所以李代数就是所有矩阵。

  • 特殊正交群 SO(n) 的李代数：对于 SO(n)，我们有约束 R^T R = I。假设有一条路径 R(t) 经过单位元 I，即 R(0) = I。将约束对 t 求导并在 t=0 处求值：R'(0)^T + R'(0) = 0。令初始速度 R'(0) = A，则得到 A^T + A = 0，即 A^T = -A。满足这个条件的矩阵是斜对称矩阵（反对称矩阵）。因此，SO(n) 的李代数 𝔰𝔬(n) 是所有 n × n 斜对称矩阵的集合。对于 SO(3)，这就是角速度矩阵的空间。

SO(n) 流形的维度就是其李代数的维度。一个 n × n 斜对称矩阵，对角线元素为零，且 a_{ij} = -a_{ji}。独立参数的数量是 n(n-1)/2。所以 SO(3) 的维度是 3*2/2 = 3，SO(4) 的维度是 4*3/2 = 6。这意味着在四维空间中，有六种旋转方式。

切空间的平移与矩阵指数 🧭

那么在其他点（不是单位元）的切空间呢？李群的一个美妙之处在于，你只需要做一个左平移就能将整个向量空间移动到另一点。换句话说，所有可能的速度集合不会改变。无论你处于哪个构型，你仍然可以以相同类型的速度旋转。数学上，这通过左平移来捕捉：群 G 在点 g 的切空间 T_g G 等于 g 乘以李代数 𝔤（即 g · 𝔤）。整个切空间只是被群元素本身旋转了。

这个观察对所有李群都成立。由此我们得到一个基本结果：对于任何李群，你都有一个一阶矩阵微分方程：

\dot{g}(t) = g(t) · ξ

其中 ξ ∈ 𝔤 是李代数中的常数速度。给定初始条件 g(0)，这个微分方程的解是：

g(t) = g(0) · exp(t ξ)

这里 exp 是矩阵指数。你可能在向量微分方程中见过它，它对矩阵也适用。这个结果描述了所有李群的运动学。矩阵指数是“自然”的路径，因为它是积分该微分方程的正确方式。

矩阵指数的定义是无穷级数：

exp(A) = I + A + A^2/2! + A^3/3! + ...

这个级数对所有矩阵都收敛。对于李群，当我们谈论李代数中的一个初始速度 ξ 和一条路径时，你总是有这条路径 γ(t) = exp(t ξ)。不同的 ξ 会产生不同的路径。

总结 📝

本节课我们一起学习了机器人学中矩阵李群的基础知识。

  • 我们首先探讨了学习李群的动机，包括处理图像旋转不变性、点云识别、刚体运动建模、单目视觉和IMU积分等问题，其核心在于利用连续对称性。
  • 我们回顾了数学中群的定义，它需要满足封闭性、结合律、存在单位元和逆元四个性质。
  • 我们将群的概念扩展到矩阵群，特别是可逆方阵构成的一般线性群 GL(n, ℝ)。
  • 我们介绍了仿射群和特殊正交群 SO(n)，后者是描述旋转的矩阵群，其行列式为 +1。
  • 我们引入了流形和切空间的几何概念，用于描述局部类似欧几里得空间的曲面及其在一点处的所有可能速度方向。
  • 我们定义了李群的李代数，即群在单位元处的切空间。例如，SO(3) 的李代数 𝔰𝔬(3) 是所有3×3斜对称矩阵的集合，自然对应角速度。
  • 最后，我们了解到，对于李群，连接两点的“自然”路径由矩阵指数给出，它是一阶微分方程 \dot{g} = g ξ 的解，并且不同点处的切空间可以通过群的左平移相互关联。

在下一讲中，我们将讨论伴随映射，它用于将速度从一个切空间映射到另一个切空间。

007：矩阵李群在机器人学中的应用 II

在本节课中，我们将继续学习矩阵李群，特别是如何利用李代数来处理刚体运动中的速度变换问题。我们将从旋转群SO(3)入手，理解其李代数的结构，并推导出描述旋转矩阵随时间变化的微分方程。最后，我们将探讨伴随映射（Adjoint Map）的概念及其在坐标系变换中的重要作用。

上一节我们介绍了矩阵李群的基本概念。本节中，我们来看看如何具体应用这些概念来处理机器人学中的实际问题。

首先，我们回顾一下SO(3)群，它由所有满足 R^T R = I 且 det(R) = 1 的3x3矩阵组成。其对应的李代数so(3)由所有满足 S^T = -S 的3x3反对称矩阵组成。

我们可以方便地在向量和矩阵形式之间转换。给定一个角速度向量 ω = [ω1, ω2, ω3]^T，我们可以通过“楔积”（wedge）运算将其映射为反对称矩阵：

ω^∧ = [ 0, -ω3, ω2;
         ω3, 0, -ω1;
        -ω2, ω1, 0 ]

反之，通过“反楔积”（vee）运算可以将反对称矩阵映射回向量：(ω∧)∨ = ω。

李代数so(3)可以由三个生成元（generator）张成：

G1 = [0, 0, 0; 0, 0, -1; 0, 1, 0]
G2 = [0, 0, 1; 0, 0, 0; -1, 0, 0]
G3 = [0, -1, 0; 1, 0, 0; 0, 0, 0]

任何角速度对应的反对称矩阵都可以表示为：ω^∧ = ω1 G1 + ω2 G2 + ω3 G3。

一个有趣的现象是，两个生成元之间的李括号（Lie Bracket）运算，定义为 [Gi, Gj] = Gi Gj - Gj Gi，其结果对应于三维向量叉积。例如，[G1, G2] = G3，这类似于 e1 × e2 = e3。李括号是向量叉积在李代数空间中的推广，并且适用于更高维度。

现在，我们来探讨旋转矩阵的微分方程。从SO(3)的约束 R^T R = I 出发，对时间求导可得：
R^T \dot{R} + \dot{R}^T R = 0
这意味着 R^T \dot{R} 是一个反对称矩阵，因此属于李代数so(3)。我们将其记作 ω_b^∧，即：
R^T \dot{R} = ω_b^∧
这里，ω_b 被解释为在物体坐标系（Body Frame）中测量的角速度。

将等式两边左乘旋转矩阵 R，我们得到：
\dot{R} = R ω_b^∧
这个微分方程描述了旋转矩阵随时间的变化率。假设在一个微小时间间隔 Δt 内角速度 ω_b 恒定（零阶保持），我们可以对这个微分方程进行积分，得到离散时间的更新公式：
R_{k+1} = R_k exp(ω_b^∧ Δt)
其中 exp(·) 是矩阵指数，它将李代数中的元素映射回李群，代表一个微小的旋转增量。

然而，从约束 R^T R = I 求导，我们还能得到另一个关系：
\dot{R} R^T = ω_s^∧
这里，ω_s 被解释为在空间坐标系（Spatial Frame，即固定坐标系）中测量的角速度。

为了理解 ω_b 和 ω_s 之间的关系，我们需要一个关键性质：对于任意旋转矩阵 R 和反对称矩阵 ω^∧，有：
R ω^∧ R^T = (R ω)^∧
利用这个性质，我们可以将 \dot{R} = R ω_b^∧ 改写为：
\dot{R} = (R ω_b)^∧ R = ω_s^∧ R
比较两式，我们得到空间角速度与物体角速度的变换关系：
ω_s = R ω_b
这正是一个向量从一个坐标系（物体系）旋转到另一个坐标系（空间系）的标准操作。

因此，旋转矩阵的微分方程有两种等价形式，取决于我们使用哪个坐标系下的角速度：

  1. \dot{R} = R ω_b^∧ （使用物体系角速度）
  2. \dot{R} = ω_s^∧ R （使用空间系角速度）
    对应的离散积分公式分别为：
    R_{k+1} = R_k exp(ω_b^∧ Δt) 和 R_{k+1} = exp(ω_s^∧ Δt) R_k

在实现滤波器或状态估计器时，选择其中一种形式并保持一致性至关重要。

上述关键性质 R ω^∧ R^T = (R ω)^∧ 的证明，引出了更一般的概念——伴随映射（Adjoint Map）。

在线性代数中，对于一个线性变换 A，在不同基底下表示的矩阵 T 和 A 通过一个相似变换关联：T = P A P^{-1}，其中 P 是基变换矩阵。

在李群理论中，伴随映射是这种相似变换的推广。对于李群元素 G 和李代数元素 B，定义伴随作用 Ad_G 为：
Ad_G(B) = G B G^{-1}
这个运算的结果仍然在李代数中。Ad_G 本质上是一个线性变换，可以表示为一个矩阵。对于SO(3)群，Ad_R 对应的矩阵就是旋转矩阵 R 本身，即 Ad_R(ω^∧) = R ω^∧ R^T = (R ω)^∧。

对于包含平移的刚体变换群SE(3)，其李代数se(3)中的元素是“ twists”（包含角速度和线速度）。此时，伴随映射 Ad_T 会是一个6x6的矩阵，它不仅包含旋转部分，还包含一个由平移引起的耦合项，这对应于物理中“旋转运动会产生线速度”的现象。

如果我们对伴随映射 Ad_G(B) 先关于 B 求导，再关于 G 求导，就会得到李括号运算。因此，李括号度量了李群的“不可交换性”（非交换性）。在平坦的欧氏空间（可交换），伴随映射是恒等变换；在SO(3)或SE(3)（不可交换）中，伴随映射就变得非常重要。

伴随映射的核心作用在于，它允许我们将不同切空间上的向量（如不同位姿处的速度、误差）映射到统一的切空间（通常是单位元处的切空间，即李代数）中进行处理。这对于在向量空间中进行协方差传播、优化等计算至关重要，因为李代数是一个向量空间，而李群是一个流形，直接在其上进行线性运算并不方便。

最后，我们提及一个重要的公式：Baker-Campbell-Hausdorff公式。对于李代数中的两个元素 X 和 Y，矩阵指数的乘积并不等于它们和的指数，除非它们可交换：
exp(X) exp(Y) ≠ exp(X + Y)
BCH公式给出了 Z 的表达式，使得 exp(X) exp(Y) = exp(Z)，其中 Z 是一个关于 X, Y 及其李括号的无穷级数：
Z = X + Y + 1/2 [X, Y] + 1/12 [X, [X, Y]] - 1/12 [Y, [X, Y]] + ...
这个公式在非线性系统的线性化（例如扩展卡尔曼滤波中）非常有用，它允许我们在李代数这个向量空间中近似处理李群上的乘法运算。

本节课中我们一起学习了：

  1. SO(3)李代数的具体形式及其与向量叉积的联系。
  2. 旋转矩阵的微分方程及其物理意义（物体系 vs. 空间系角速度）。
  3. 离散时间下的旋转积分公式。
  4. 伴随映射（Adjoint Map）的概念、推导及其重要性，它实现了不同切空间向量到李代数的规范映射。
  5. BCH公式的作用，它揭示了李群乘法在李代数中的近似表达。

理解这些概念是后续在流形上进行状态估计和传感器融合的基础。下一讲，我们将把这些工具应用于具体的机器人运动模型和传感器模型中。

008：机器人运动与不确定性传播 🚀

在本节课中，我们将学习如何对机器人运动进行建模，并理解如何在这种运动模型中传播不确定性。我们将重点关注使用指数坐标（李代数）来描述机器人位姿，这种方法能让我们以更自然、更线性的方式处理包含旋转和平移的运动。

上一节我们介绍了矩阵李群的基础概念，本节中我们来看看如何将这些概念应用到机器人运动建模中。

机器人运动建模 🤖

我们从一个常见的移动机器人平台——Roomba扫地机器人开始。它是一个地面机器人，有两个轮子，可以在平面上移动和旋转。对于状态估计和感知任务，我们关心的是如何根据运动命令来估计机器人的轨迹，而不是控制它如何精确地跟踪轨迹。因此，我们可以忽略导致运动的物理原因（如力和扭矩），而专注于运动学模型。

机器人的运动可以用一个附着在机器人本体上的坐标系来描述。这个坐标系相对于一个固定的世界坐标系，具有位置和朝向（位姿）。通过测量两个轮子的角速度，我们可以计算机器人本体的线速度和角速度，这被称为“扭转”。

扭转 是一个向量，它包含了角速度和一个线速度分量。在二维平面中，它是一个3维向量：
twist = [ω, v_x, v_y]^T

其中，ω 是关于垂直于平面的轴的角速度，v_x 和 v_y 是机器人本体坐标系下的线速度分量。

非完整约束与建模简化 ⚙️

像Roomba这样的机器人存在“非完整约束”，这意味着它不能瞬时地向任意方向移动（例如，不能直接横向移动）。它必须通过先前进再旋转的组合运动来到达目标点。然而，对于状态估计，我们有一个优势：我们是在机器人执行完命令后去估计其位姿，而不是在命令它如何运动。因此，我们可以忽略这些约束，将机器人视为一个可以在平面上自由移动的刚体，只要其运动速度符合物理实际即可。这简化了我们的模型。

更重要的是，我们选择在机器人本体坐标系下描述运动。机器人作为一个物理实体，其与环境的交互全部发生在本体坐标系中。使用全局坐标系（如世界坐标系）虽然对人类工程师方便，但在数学上会导致描述运动的方程变得非线性且复杂。相反，在本体坐标系下使用指数坐标（李代数），运动方程会呈现出一种“类线性”的特性，这使得后续的不确定性传播变得异常简单。

二维刚体与李群 SE(2) 📐

我们的机器人是一个二维刚体。描述其旋转的群是 SO(2)，描述其旋转加平移的群是 SE(2)。SE(2) 中的元素是 3x3 矩阵，形式如下：

X = [ R,   p
      0^T, 1 ]

其中 R 是一个 2x2 旋转矩阵（属于 SO(2)），p 是一个 2x1 平移向量。

对应的李代数 se(2) 元素可以用一个“楔积”运算符表示为 3x3 矩阵：

ξ^ = [ ω^, v
       0^T, 0 ]

其中 ω 是一个标量（角速度），v 是一个 2x1 向量（线速度），ω^ 是 ω 对应的 2x2 斜对称矩阵。

确定性运动过程 ⏱️

在离散时间中，机器人的运动过程可以描述为：

X_{k+1} = X_k * U_k

其中 X_k 是当前位姿（SE(2)矩阵），U_k 是从时间 k 到 k+1 的相对运动增量。这个增量可以通过对本体坐标系下的扭转进行积分得到：

U_k = exp( (twist_k * Δt)^ )

这里 exp() 是矩阵指数映射，它将李代数中的元素映射回李群中的位姿。

定义噪声与误差 🔍

在实际中，运动存在噪声。我们巧妙地将噪声定义在李代数中：

U_k_noisy = exp( (twist_k * Δt + w_k)^ )

其中 w_k 是一个零均值的三维高斯噪声向量，其协方差为 Q_k。这种定义方式非常干净，因为噪声源位于向量空间（李代数）中。

为了进行状态估计和协方差传播，我们需要定义位姿的误差。我们采用“左不变误差”的定义：

η_k = X_k^{-1} * X̄_k ≈ exp( ε_k^ )

其中 X_k 是真实位姿，X̄_k 是估计位姿，ε_k 是李代数中的误差向量。这个定义的优点是，无论用哪个全局坐标系作为参考，只要对位姿进行相同的左乘变换，误差值保持不变，这符合我们对一致算法的期望。

误差传播与 BCH 公式 📈

我们的目标是找到误差 ε_k 如何随时间传播。将带有噪声的运动过程代入误差定义，并利用李群中的伴随（Adjoint）性质来移动矩阵项，我们得到：

exp( ε_{k+1}^ ) ≈ exp( -Adj(U_k^{-1}) * ε_k^ ) * exp( w_k^ )

这里出现了两个矩阵指数相乘。为了将它们合并，我们使用 Baker-Campbell-Hausdorff (BCH) 公式。当误差 ε_k 和噪声 w_k 都较小时，BCH公式允许我们进行一阶近似，将指数乘积近似为指数内的和：

ε_{k+1} ≈ Adj(U_k^{-1}) * ε_k + w_k

这是一个线性的误差传播方程！它描述了上一时刻的误差如何通过相对运动 U_k 的伴随矩阵映射，并与当前的运动噪声叠加，得到当前时刻的误差。

不确定性（协方差）传播 📊

基于上述线性误差方程，我们可以直接推导协方差的传播公式。假设噪声 w_k 的协方差为 Q_k，误差 ε_k 的协方差为 Σ_k，则有：

Σ_{k+1} = Adj(U_k^{-1}) * Σ_k * Adj(U_k^{-1})^T + Q_k

这个公式非常优美且强大：

  • 与绝对位姿无关：不确定性（协方差）的增长只取决于相对运动 U_k 和噪声强度，而与机器人当前在空间中的绝对位置和朝向无关。这非常符合直觉：从A点移动到B点所积累的不确定性，应该只与这段移动本身有关。
  • 在向量空间中运算：所有运算都在李代数（一个向量空间）中进行，避开了在李群（弯曲空间）中直接处理协方差的复杂性。
  • 伴随矩阵的作用：Adj(U_k^{-1}) 充当了一个“基变换”的角色。它将上一时刻的误差协方差从旧的切空间映射到新的切空间（以新位姿为基准），以便与当前时刻的噪声协方差正确相加。

伴随矩阵的计算 🧮

对于 SE(2) 和 SE(3)，伴随矩阵有具体的矩阵形式。对于 SE(3)，如果扭转表示为 ξ = [ω, v]^T（6维向量），位姿为 X = [R, p; 0^T, 1]，则其伴随矩阵为：

Adj(X) = [ R,   0
           p^ R, R ]

这是一个 6x6 的矩阵。它精确地描述了如何将一个坐标系下的扭转映射到另一个坐标系下，包含了旋转以及由旋转中心偏移引起的线速度分量。

总结 🎯

本节课中我们一起学习了：

  1. 机器人运动学建模：我们将机器人视为刚体，在其本体坐标系下使用扭转来描述运动。
  2. 指数坐标的优势：使用李群 SE(2)/SE(3) 和李代数 se(2)/se(3) 来描述位姿和运动，能将复杂的非线性运动在误差层面转化为近似线性的关系。
  3. 左不变误差：定义了与参考系选择无关的位姿误差，为一致性估计奠定了基础。
  4. BCH 公式与线性近似：利用 BCH 公式，在小误差假设下，得到了李代数空间中线性的误差传播方程。
  5. 不确定性传播公式：推导出了清晰的不确定性（协方差）传播方程 Σ_{k+1} = Adj(U^{-1}) Σ_k Adj(U^{-1})^T + Q。该公式表明不确定性增长只与相对运动和过程噪声有关，这是一个非常有力且符合物理直觉的结果。

这种方法为后续在复杂运动场景下进行鲁棒的状态估计（如卡尔曼滤波）提供了强大的理论工具。在接下来的课程中，我们将探索如何利用这些坐标进行具体的状态估计算法设计。

009：不变卡尔曼滤波 I

概述

在本节课中，我们将学习一种称为不变卡尔曼滤波的状态估计方法。这种方法利用了李群和李代数的结构，特别是指数坐标，来设计滤波器。它的核心优势在于能够保持系统的几何对称性，从而在某些非线性问题上获得比传统扩展卡尔曼滤波更优、更一致的性能。我们将从动机出发，理解其核心思想，并学习如何推导和应用这种滤波器。

动机：为什么需要不变滤波？

上一节我们介绍了基于李群的误差定义。本节中，我们来看看为什么这种定义方式如此重要。

许多机器人系统（如移动机器人、无人机）的状态自然地存在于矩阵李群上（例如 SE(2), SE(3), SO(3)）。当使用传统的扩展卡尔曼滤波处理这类问题时，如果错误地将旋转和平移的误差分离开来建模，会破坏系统的几何结构。

这会导致两个主要问题：

  1. 不一致的协方差传播：在机器人同时移动和旋转时，滤波器无法正确捕捉不确定性在状态空间中的真实形状（例如产生“香蕉形”分布），可能导致过于乐观或悲观的不确定性估计。
  2. 虚假的相关性：当系统在某个维度（如航向角）完全不确定时，这种不确定性会被错误地传播到其他不相关的维度（如位置），导致整体估计迅速发散，变得无用。

不变卡尔曼滤波通过尊重李群结构的误差定义，能够自然地保持这种对称性，从而避免上述问题，实现更一致、更鲁棒的估计。

核心思想：从线性系统获得的启示

为了理解不变滤波的设计目标，我们先回顾一个线性系统的理想特性。

考虑一个确定性线性时不变系统：
dx/dt = A x + B u
定义估计误差为：e = x - x_bar。
对其求导并代入系统方程，我们可以得到误差的动态方程：
de/dt = A e

这个结果非常吸引人：

  • 误差动态是自治的：它只依赖于误差本身，而与真实的系统轨迹 x(t) 或估计轨迹 x_bar(t) 无关。
  • 误差传播可精确求解：给定初始误差 e(0)，我们可以精确地预测未来任意时刻的误差：e(t) = exp(A t) e(0)。

这意味着，无论初始估计有多差，误差的演化规律都是独立且可预测的。不变卡尔曼滤波的目标，就是为满足特定结构的非线性系统（在李群上演化），也找到一种误差定义和坐标，使得误差动态具有类似的自治线性形式。

反面教材：欧拉角参数化的问题

让我们通过一个具体例子，看看如果不用李群结构会有什么麻烦。考虑使用IMU中的陀螺仪进行3D姿态估计。

如果选择欧拉角 q = [φ, θ, ψ]^T 作为状态，其动力学方程为：
dq/dt = E(q)^{-1} ω
其中 ω 是陀螺仪测量的角速度，E(q) 是依赖于当前欧拉角的矩阵。

定义欧拉角误差为 δq = q - q_bar，并对其进行线性化，得到的误差动态方程形如：
d(δq)/dt ≈ A(q_bar, ω) δq

这里的关键问题是：矩阵 A 依赖于当前的姿态估计 q_bar。如果初始估计误差很大，那么用于传播误差的 A 矩阵本身就是错误的。这会导致“错误的估计 -> 错误的传播 -> 更错的估计”这样的正反馈循环，可能导致滤波器发散。这正是我们想要避免的。

解决方案：基于李群的不变误差

现在，我们切换到李群的框架下解决同一个问题。

不变误差的定义

设真实状态 X 和估计状态 X_bar 都是李群 G 中的元素。我们定义两种不变误差：

  • 右不变误差：η_r = X_bar * X^{-1}
  • 左不变误差：η_l = X^{-1} * X_bar

这里的“不变”是指：如果我们用同一个群元素 L 对状态进行变换（例如同时旋转整个坐标系），误差值 η 不会改变。这体现了滤波器对系统对称性的尊重。

自治误差动态定理（定理一）

对于一个状态在李群上演化的系统，其过程模型为 dX/dt = f(X, u)。如果函数 f 满足特定的群仿射性质，那么对应的不变误差 η 的动态方程将具有以下形式：
dη/dt = g(η, u)
关键：这个方程不依赖于状态估计 X_bar，只依赖于误差 η 和输入 u。这与线性系统中的自治误差动态目标一致。

对数线性性质定理（定理二）

这是不变滤波的核心理论结果。对于满足群仿射性质的过程模型，如果我们定义李代数中的误差向量 ξ（满足 η = exp(ξ^∧)），那么 ξ 的动态方程是严格线性的：
dξ/dt = A(u) ξ
并且，这个线性关系是精确的，而不仅仅是一阶近似。这意味着，我们可以像处理线性系统一样，精确地预测和传播李代数空间中的误差：
ξ(t) = Φ(t, 0) ξ(0)，其中 Φ 是状态转移矩阵。

证明概要（以SO(3)上的左不变误差为例）：

  1. 对于陀螺仪模型 dR/dt = R ω^∧，定义左不变误差 η = R^T R_bar。
  2. 推导出其动态方程：dη/dt = -ω^∧ η + η ω^∧。
  3. 令 η = exp(ξ^∧)，并进行一阶近似（exp(ξ^∧) ≈ I + ξ^∧），代入上式。
  4. 利用SO(3)李括号的性质 [A, B] = A^∧ B^∧ - B^∧ A^∧，化简得到：dξ^∧/dt ≈ -ω^∧ ξ^∧ + ξ^∧ ω^∧ = [ξ^∧, ω^∧]。
  5. 对于向量，这等价于 dξ/dt = -ω × ξ = (-ω^×) ξ。这里 A = -ω^×。
  6. 可以进一步证明，从精确解出发也能得到相同的线性方程，说明一阶近似在此情况下没有丢失信息，该线性关系是精确的。

这个定理告诉我们，对于这类系统，在李代数空间中，误差传播是全局线性的。这为设计高性能滤波器奠定了坚实基础。

构建不变卡尔曼滤波器

有了理论准备，我们现在可以构建不变扩展卡尔曼滤波器的具体步骤。算法分为预测（传播）和更新（校正）两步。

1. 预测步骤

预测步骤负责利用过程模型来推进状态估计和其不确定性。

  • 状态估计传播：直接积分李群上的过程模型。
    d(X_bar)/dt = f(X_bar, u)
    离散形式通常为：X_bar_{k+1} = X_bar_k * exp((u_k Δt)^∧)

  • 协方差传播：

    1. 根据过程模型和选择的误差定义（左不变或右不变），确定李代数空间中的误差动态矩阵 A。对于像 dX/dt = X u^∧ 这样的运动学模型，若选择右不变误差，则 A = 0。
    2. 离散化连续时间的协方差微分方程。对于线性误差动态 dξ/dt = A ξ + w（w 为噪声），其协方差 P 的传播方程为：
      dP/dt = A P + P A^T + Q
      其中 Q 是过程噪声协方差密度矩阵。
    3. 离散化后得到：
      P_{k+1}^- = Φ_k P_k^+ Φ_k^T + Q_d
      其中 Φ_k ≈ exp(A Δt) ≈ I + A Δt 是离散状态转移矩阵，Q_d 是离散化的过程噪声协方差。

2. 更新步骤

更新步骤在收到观测值时，修正状态估计和协方差。

  • 观测模型：为了与不变框架兼容，观测模型需要具有特定形式。例如，对于右不变观测模型：
    y = X^{-1} * b + v
    其中 b 是一个常向量（例如地图中的地标位置），v 是观测噪声。这种形式常见于机器人定位问题，即传感器测量的是地标在机器人本体坐标系下的位置。

  • 创新与增益计算：

    1. 计算创新（Innovation）：ν = y - (X_bar^-)^{-1} * b。注意，这是在李代数空间或对应向量空间中的量。
    2. 计算观测矩阵 H：通过线性化观测模型得到，它映射李代数误差 ξ 到观测空间。H 的具体形式取决于误差定义和观测模型。
    3. 计算卡尔曼增益 K：
      K = P^- H^T (H P^- H^T + R)^{-1}
      其中 R 是观测噪声协方差矩阵。这个公式与标准卡尔曼滤波完全相同。

  • 状态与协方差更新：

    1. 状态更新：更新发生在李群上，通过指数映射将李代数空间的修正量加回到状态中。
      • 对于右不变滤波：X_bar^+ = exp((K ν)^∧) * X_bar^-
      • 对于左不变滤波：X_bar^+ = X_bar^- * exp((K ν)^∧)
    2. 协方差更新：使用标准的卡尔曼更新公式。
      P^+ = (I - K H) P^-

应用实例：机器人定位

考虑一个在平面SE(2)上移动的机器人，它带有测距/测向传感器，可以观测到环境中已知位置的地标。

  • 过程模型：dX/dt = X u^∧，其中 u = [v, ω]^T 包含线速度和角速度。这是一个群仿射模型。
  • 观测模型：机器人观测到地标 m 在本体坐标系下的位置。这可以写为：
    y = X^{-1} * \tilde{m} + v （其中 \tilde{m} 是地标的齐次坐标 [m_x, m_y, 1]^T）
    这正是一个右不变观测模型（b = \tilde{m}）。
  • 滤波器设计：由于过程模型是群仿射，且观测模型是右不变的，我们可以构建一个右不变扩展卡尔曼滤波器。
    • 预测：A = 0，因此协方差传播简化为 P_{k+1}^- = P_k^+ + Q_d。
    • 更新：需要计算该观测模型对应的 H 矩阵。通过推导可得，对于单个地标，H 矩阵能揭示系统的可观测性：仅观测一个地标无法确定机器人的绝对航向；但同时观测两个不共线的地标，则整个状态（位置和航向）都是可观测的。

通过仿真可以看到，使用不变卡尔曼滤波，即使在观测稀疏的情况下，协方差椭圆也能更真实地反映机器人实际的不确定性（例如在旋转时保持不确定性主要分布在切向方向），并且滤波器表现更加稳定。

总结

本节课我们一起学习了不变卡尔曼滤波器的基本原理和设计方法。

  • 核心动机：传统EKF在处理李群状态时可能破坏几何结构，导致不一致的估计。不变滤波通过保持对称性来解决这一问题。
  • 理论基础：关键在于定义左不变或右不变误差，并利用群仿射系统特有的两个定理，使得李代数空间中的误差动态是自治且线性的。
  • 滤波器结构：算法流程与EKF类似，但状态更新在李群上进行（指数映射），误差和协方差在李代数（向量空间）中处理。预测步需要求取误差动态矩阵 A，更新步需要求取观测矩阵 H。
  • 优势：能提供更一致、更鲁棒的估计性能，特别适用于状态在李群上演化、且传感器模型符合不变形式的机器人定位、姿态估计等问题。

下一节课，我们将探讨如何将不变卡尔曼滤波器应用于更复杂的IMU/GPS组合导航问题。

010：不变卡尔曼滤波 II

概述

在本节课中，我们将继续学习不变状态估计。我们将深入探讨一个非常有趣的传感器——惯性测量单元（IMU），并学习如何将其建模并融入不变状态估计的框架中。我们将通过一个结合IMU和GPS的左不变扩展卡尔曼滤波（IEKF）示例，来理解为什么观测模型有“左”或“右”之分，以及如何推导误差更新方程。

IMU传感器建模

上一节我们介绍了不变扩展卡尔曼滤波的基本思想和示例。本节中，我们来看看如何对惯性测量单元（IMU）进行数学建模。

IMU是一种小型传感器，常见于智能手机中。其最基本的形式包含一个陀螺仪和一个线性加速度计。

  • 陀螺仪：测量传感器自身坐标系下的旋转角速度。
  • 线性加速度计：测量传感器自身坐标系下的线性加速度。

传感器的精度和成本相关。我们关注的是其数学模型和算法，而非硬件设计。

状态变量

我们需要跟踪传感器的姿态、速度和位置。状态变量定义如下：

  • 旋转 R：属于特殊正交群 SO(3)，是一个旋转矩阵。
  • 速度 V：传感器相对于某个世界坐标系的速度（三维向量）。
  • 位置 P：传感器相对于世界坐标系的位置（三维向量）。

为了有序地组织这些不同类型的变量（矩阵和向量），我们使用元组来表示状态：X = (R, V, P)。元组能保持顺序，而集合则不能。一旦选定顺序，在后续所有矩阵运算中必须保持一致。

输入（测量值）

IMU直接提供的测量值（输入）为：

  • 角速度 ω：三维向量。
  • 线性加速度 a：三维向量。

我们可以将输入定义为六维向量：u = [ω^T, aT]T。这些测量值都是在传感器自身坐标系（即“机体坐标系”）下读取的。

为何需要速度状态？

因为加速度是位置的二阶导数。为了建立一阶常微分方程组（ODE）并进行级联积分，我们需要引入速度作为中间状态。传感器提供加速度，一次积分得到速度，二次积分得到位置。积分次数越多，误差累积越快，这是IMU定位的主要挑战之一。

双直接等距群 SE₂(3)

为了在群论框架下对IMU状态进行建模，我们引入一个矩阵李群：双直接等距群 SE₂(3)。这个群是SE(3)群的扩展，额外包含了一个速度向量空间。

群元素

该群中的矩阵形式如下：

X = [ R   V   P ]
    [ 0   1   0 ]
    [ 0   0   1 ]

其中 R ∈ SO(3), V ∈ R³, P ∈ R³。这是一个5x5的矩阵。该群的维度是9（旋转3维 + 速度3维 + 位置3维）。

李代数

对应的李代数元素（广义的“ twist ”）是一个九维向量：

ξ = [ ω, v, p ]^T

其“楔形”运算符（∧）将其映射为李代数矩阵：

ξ^∧ = [ ω^∧   v   p ]
      [   0    0   0 ]
      [   0    0   0 ]

伴随映射

伴随映射 Ad_X 是一个9x9的矩阵，其形式取决于状态变量的顺序。对于上述顺序，其结构大致如下：

Ad_X = [  R     0     0 ]
       [ V^∧R   R     0 ]
       [ P^∧R   0     R ]

伴随映射的作用与在SO(3)和SE(3)中类似：用于在不同点之间映射“速度”或协方差。所有为矩阵李群推导的公式（如协方差传播方程）对此群同样成立，这体现了该框架的便利性——无需为每个新问题重新推导。

IMU过程模型

现在，我们为IMU建立连续时间过程模型。我们假设跟踪传感器相对于一个固定世界坐标系的姿态、速度和位置。

确定性模型

运动学方程如下：

  1. 旋转：Ṙ = R ω^∧
  2. 速度：V̇ = R a - g （其中 g 是世界坐标系下的重力向量）
  3. 位置：Ṗ = V

将上述方程写成矩阵形式，状态 X 的导数为：

Ẋ = [ Ṙ   V̇   Ṗ ]   =   [ R ω^∧   R a - g   V ]
    [ 0    0    0 ]       [   0       0       0 ]
    [ 0    0    0 ]       [   0       0       0 ]

可以验证，这个确定性过程模型满足群仿射性质。这意味着，尽管模型是非线性的，但其误差动力学具有对数线性的优良性质。因此，在确定性情况下，我们可以精确地积分状态并传播协方差，无需线性化近似。

含噪声模型

现实中，IMU测量存在噪声。我们通常在测量源处添加噪声：

  • 陀螺仪测量：ω_m = ω + n_ω
  • 加速度计测量：a_m = a + n_a

其中 n_ω 和 n_a 是零均值加性高斯白噪声。将噪声代入确定性模型，得到随机过程模型：

Ẋ = [ R (ω_m^∧ - n_ω^∧)   R (a_m - n_a) - g   V ]
    [          0                   0            0 ]
    [          0                   0            0 ]

这可以整理为 Ẋ = f(X, u) - X (n^∧) 的形式，其中 n = [n_ω, n_a, 0]^T 是一个九维噪声向量。这种噪声结构是从传感器物理模型中自然推导出来的。

偏置

实际IMU还存在随时间缓慢变化的偏置。完整的模型需要将角速度偏置 b_ω 和加速度偏置 b_a 作为状态变量进行估计，并从测量值中减去：ω = ω_m - b_ω, a = a_m - b_a。加入偏置会破坏完美的群对称性，但滤波器在实际中仍能表现优异。

左不变EKF的传播步骤

我们将构建一个融合IMU（用于预测）和GPS（用于校正）的左不变扩展卡尔曼滤波器。

离散化

我们需要对连续时间模型进行离散化以在计算机上实现。假设在一个采样周期 Δt 内，IMU的角速度和加速度测量值保持恒定（零阶保持），我们可以对过程模型进行精确积分。

离散化的状态转移方程形式如下：

X_{k+1} = X_k * exp( (A(u_k) * Δt)^∧ )

其中 A(u) 是来自对数线性误差动力学的系统矩阵。对于左不变误差定义，该矩阵为：

A^L = [ 0     0    0 ]
      [ a^∧   0    0 ]
      [ 0     I    0 ]

离散化还会产生一些积分项（如 Γ₀, Γ₁, Γ₂），它们是关于 ω 的矩阵指数及其积分的函数，有闭式解。

协方差传播

连续时间下的协方差传播遵循李雅普诺夫方程：

Ṗ = A P + P A^T + Q

其中 Q 是过程噪声协方差矩阵的连续时间等效。该方程可以从离散时间卡尔曼滤波的协方差更新公式，通过一阶近似（Φ ≈ I + A Δt）并取 Δt → 0 的极限推导出来。

在离散时间实现中，我们使用离散化的状态转移矩阵 Φ 和离散过程噪声协方差 Q_d 来传播协方差：P_{k+1|k} = Φ P_{k|k} Φ^T + Q_d。

GPS观测模型与校正

预测步骤由IMU处理。现在，我们来看校正步骤，即如何将GPS观测融入滤波器。

观测模型

GPS提供传感器在世界坐标系中的位置测量 z_GPS。我们的状态矩阵 X 中包含位置 P。因此，观测方程可以写为：

z = H * X   （左不变观测模型）

其中观测矩阵 H 用于从状态矩阵中“提取”位置。为了匹配矩阵乘法维度，我们定义：

H = [ 0  0  I ] （一个3x5的矩阵）
B = [ 0  0  I ]^T （一个5x3的矩阵）

使得 z = H X = P。这正是一个左不变观测模型的形式：z = B^T X。

观测雅可比矩阵

对于左不变EKF，我们需要计算观测雅可比矩阵 H。根据公式 H = (B^T ξ∧)∨ 进行计算，可以得到一个常数矩阵：

H = [ 0  0  I ] （一个3x9的矩阵，去除了冗余零行）

这意味着校正与状态估计值无关，这非常简洁。

实践挑战与总结

实施挑战

在实际构建该滤波器时，可能会遇到以下挑战：

  1. 数据同步与速率：IMU频率高（如100Hz），GPS频率低（如10Hz），需要处理不同步的测量。
  2. 偏置估计：必须估计并补偿IMU的偏置，否则在GPS更新间隔内会产生显著漂移。
  3. 传感器标定：IMU和GPS可能安装在不同位置，需要知道它们之间的刚性变换（外参），并将GPS测量值转换到IMU坐标系。
  4. 噪声调参：GPS数据可能跳跃较大，需要仔细调整过程噪声和观测噪声的协方差矩阵。
  5. 环境限制：GPS在室内、城市峡谷或多路径效应严重的区域不可靠。

左右不变滤波器的关系

左不变和右不变滤波器可以通过伴随映射 Ad_X 相互转换误差和协方差。即使因为加入偏置而破坏了完美的理论性质，这种映射关系仍然近似存在，为灵活设计滤波器提供了可能。

总结

本节课中，我们一起学习了：

  1. 如何对惯性测量单元（IMU）进行数学建模，并将其状态定义为双直接等距群 SE₂(3) 的元素。
  2. IMU的确定性过程模型具有群仿射性质，导致其误差动力学是对数线性的，允许精确的协方差传播。
  3. 如何建立IMU的含噪声过程模型，并离散化用于预测步骤。
  4. 如何将GPS的位置测量建模为左不变观测模型，并推导其常数观测雅可比矩阵。
  5. 构建了一个IMU-GPS左不变扩展卡尔曼滤波器的理论框架，并讨论了实际应用中可能遇到的挑战。

不变卡尔曼滤波为处理自然演化在李群上的状态估计问题提供了一个强大且一致的框架，尽管存在噪声和偏置等非理想因素，它在实践中仍然是首选方法，性能优于许多替代方案。

011：定位 🧭

在本节课中，我们将学习机器人学中的一个基本问题——定位。定位是机器人了解自身在已知环境中所处位置和朝向的过程。我们将探讨定位问题的定义、重要性、面临的挑战以及几种核心的解决方法。

定位问题概述

定位是机器人学中的一个基础问题。如果机器人不知道自身位置，在许多实际问题中，规划行动将变得低效或不切实际。通常，我们需要某种形式的定位信息，无论是相对的、局部的还是全局的。我们需要了解机器人的位置，以便基于此进行规划和行动。

例如，如果你想抓取一个物体，可能需要知道该物体相对于抓取手臂的相对位置。如果你能完美地知道这个位置，那么抓取就会变得非常容易。即使存在一些小的不确定性，你仍然可以规划并抓取物体。如果你想从A点导航到B点，知道自己的位置就能预测未来任何时刻的位置，从而规划路径、避开障碍物，并在到达目的地时停止。

这是一个非常重要且历史悠久的问题。

定位问题的设定

在定位问题中，我们通常被给定一个环境地图。我们以某种方式对环境有完全的了解。根据你对环境的了解程度和建模方式，可能需要使用不同的算法。但作为一个抽象概念，地图是机器人周围环境的表示，我们拥有这个知识。

然后，我们有一系列测量数据。机器人移动时，我们可能会从机器人的运动中收集数据，也可能从摄像头、激光雷达、测距仪、雷达、Wi-Fi信号等传感器中收集一系列数据。我们可以利用这些数据进行状态估计。

因此，整体问题是：我们在移动时收集某种数据，并希望解决一个推断问题，即找到机器人的位置。我们将研究状态估计算法，状态估计是推断的一个方面。

通常，你拥有数据，希望推断某个量。状态估计算法是实现这一目标的一种非常高效的方法。它们之所以有吸引力，是因为它们很好地契合了贝叶斯滤波的框架。数据是顺序到达的，我们可以递归地运行滤波器，无需存储整个历史数据，就能跟踪机器人的位置。这使得它们对许多机器人应用非常有吸引力。

机器人的位姿是位置和朝向的组合。当我们说“位姿”时，指的是位置和朝向。有时我们非正式地说“位置”，可能两者都包括。

定位中的挑战

定位问题主要分为三类：

  1. 跟踪：如果我们想知道在任何时刻自己在哪里，就需要进行跟踪。
  2. 全局定位：如果我们一开始不知道自己在哪，就需要解决一个更大的挑战，即全局定位问题。因为像卡尔曼滤波器这样的算法通常假设初始条件是已知的，这对应于我们最初的位置。如果我们不知道，这就成为一个重要问题，我们称之为全局定位，因为我们需要在全局范围内搜索自己的位置。
  3. 绑架机器人问题：如果我们知道自己的位置并进行跟踪，但由于某种原因（例如机器人被搬动），跟踪丢失或算法中断，我们需要再次恢复定位。这通常被称为绑架机器人问题。

一个具体例子：机器人足球赛

考虑一个机器人足球赛的例子。在比赛中，多个机器人在场地上移动，需要进球得分。场地周围有观众、广告等干扰，而且两边的球门是相同的，场地也是对称的。这带来了一个问题：如果你在一个对称的环境中获取观测数据，算法如何知道哪一边是正确的？因为两边都与数据相符。

为了解决这个问题，可以添加一些具有不同图案的路标。如果摄像头能检测到它们，我们就有了具有唯一ID的路标。你可以在代码中为它们命名（例如1，2，3…）。然后，你需要一个计算机视觉算法来检测这些路标，分析颜色，并告诉你这是第几号路标。由于路标数量有限（例如6个），你甚至可以穷举搜索，但这在路标数量成千上万时不可扩展。

你知道地图的尺寸（比赛文档会告诉你场地的确切尺寸和线条布局）。因此，即使你只有一个单目摄像头并检测到场上的线条，也可以将其与模型匹配，这非常有帮助。

所以，我们知道环境，并且环境在这里以路标的形式存在。例如，你可以将某个点设为原点(0,0)，然后所有事物都相对于这个坐标系进行定位。你知道每个路标的确切位置（例如x3, y3或x1, y1）。只要你能在线检测到这些路标并获得某种测量值（例如，机器人需要知道距离和角度），你就拥有了大量信息。如果你能以某种方式跟踪机器人运动（例如通过腿部编码器或IMU），或者即使没有任何传感器而只有一些假设，这都类似于运行“预测-校正-预测-校正”的过程。听起来我们可以使用我们学过的任何滤波器。

因此，每个机器人都可以在场上运行一个定位算法。

关于同时观测多个路标：在实际设置中，摄像头的视野有限，可能无法同时看到所有路标。但假设你有一个深度神经网络，它可以同时为你提供所有路标的距离和方位角，这是可能的。那么，你可以使用顺序更新或批量更新。批量更新更好，因为你同时使用了所有信息来校正状态。而顺序更新时，第一次更新后，线性化点发生了变化，效果可能变好或变差。批量更新则帮助你一次性整合所有信息。

关于所需的最少路标数量：是的，你需要最少数量的路标来使你的状态可观测。这就是为什么我们在不变扩展卡尔曼滤波的讲座中进行了可观测性分析。当我们只观测一个路标的距离和方位角，而状态是位置和朝向时，朝向将不可观测。因此，你至少需要同时观测两个路标，才能进行一次使整个状态可观测的校正。对于非线性滤波器，进行可观测性分析并不简单。通常，信息越多越好（假设你的状态估计器是一致的）。理论上，信息越多，估计越好。但在实践中，可能存在异常值等问题。

解决方法概览

基于我们目前所学的知识，至少有四种方法可以解决定位问题：

  1. 扩展卡尔曼滤波器
  2. 无迹卡尔曼滤波器
  3. 粒子滤波器
  4. 不变扩展卡尔曼滤波器

此外，我们还可以使用最小二乘法（非线性最小二乘）来解决这个问题。我们可以离线批量处理，或者增量式处理。当我们学习SLAM时，会看到滤波更具体的形式。当我们保持历史数据并以线性化最小二乘或图模型的结构来看待时，它具有一种稀疏的特殊结构，这很有趣。当我们解决SLAM时，我们会研究最小二乘公式。在线性情况下，线性卡尔曼滤波和最小二乘是等价的，线性卡尔曼滤波和使用高斯分布的最大后验估计也是等价的。然而，最小二乘问题比卡尔曼滤波更通用。当问题非线性时，这种等价性并不平凡。

在这一点上，你更应关注问题本身和设定，然后选择合适的工具来解决问题。

基于扩展卡尔曼滤波器的定位

假设有一个特定的运动模型适用于位置和朝向（X, Y, θ）。运动模型称为 g，其关于状态 x 的雅可比矩阵是 G。由于噪声是乘性的，关于噪声的雅可比矩阵也存在。在预测步骤中，我们计算预测状态，然后更新协方差。由于是乘性噪声，运动模型关于噪声（或输入）的雅可比矩阵不是单位矩阵，我们得到 V * M * V^T。这是熟悉的卡尔曼滤波预测步骤。

因此，你不需要担心卡尔曼滤波部分，那是相同的。你需要担心的是模型部分：你的运动模型是什么？你的传感器模型是什么？

然后，在信号处理部分完成后，我们会得到一些几何约束，即路标相对于机器人观测位置的距离和相对角度。测量值是距离 r 和方位角 φ。状态是一个3维向量（X, Y, θ）。当然，如果你要实现不变扩展卡尔曼滤波器，你会使用SE(2)。但如果你想实现传统的扩展卡尔曼滤波器或无迹卡尔曼滤波器，你可以使用X、Y和航向角θ。需要注意将角度包裹在[-π, π]区间内。

雅可比矩阵是2x3维的。其余部分就是熟悉的扩展卡尔曼滤波器校正步骤。

所以，当你拥有测量值并知道机器人如何移动时，你实现你的模型，然后将其传递给这个滤波器，希望就能实现定位。

实际实现中需要考虑的事项：

  • 数据流水线：需要设置数据采集、处理、获取测量值的流水线，确保在状态估计器循环中处理时延迟最小。
  • 参数调优：真实世界非常复杂，我们无法给出确切的噪声协方差。需要根据问题进行调优。
  • 初始猜测：如果你对初始位置没有大致了解，算法很可能无法跟踪，因为如果偏差太大，你总是处于不一致的状态，计算出的新息都是错误的。

关于动态环境中的自适应协方差：这是一个自然的想法，但也是一个挑战。目前没有非常成熟的方法。一些近期工作使用深度学习来在线估计这些协方差。这是一个研究领域。

关于使用深度学习直接估计位姿：如果深度学习可以直接恢复位姿，为什么还要使用卡尔曼滤波？一个直接的答案是：如果它能工作，我们当然会用。但深度学习是另一种推断框架，它不具卡尔曼滤波在线性情况下的那些良好性质和保证。然而，对于非线性情况，不变扩展卡尔曼滤波器在确定性情况下具有收敛保证。深度学习和滤波器的关系更多是互补而非替代。深度学习可以改进信号处理部分，提供更好的测量值。例如，深度学习可以提供相对位姿变化，这解决了运动模型的问题；或者提供全局位置，这直接观测了状态。然后，不变扩展卡尔曼滤波器可以轻松地使用这些数据（如果它们一致）来给出带有不确定性的良好估计。从深度学习中获得不确定性本身也是一个问题。

基于无迹卡尔曼滤波器的定位

我们可以用无迹卡尔曼滤波器完成完全相同的过程。算法与我们讨论过的完全一样，因为噪声是乘性的。我们需要增广状态，然后传播Sigma点。无迹卡尔曼滤波器的预测和校正步骤与标准算法相同。

我们增广状态（加入零均值的噪声向量），生成Sigma点时，通过创建状态和噪声的块对角协方差矩阵来从噪声中采样。在预测中，我们生成Sigma点，然后计算加权样本均值和加权样本协方差。在校正步骤中，我们生成Sigma点来预测测量值，计算新息协方差、互协方差，并运行校正。

无迹卡尔曼滤波器使用无迹变换进行不确定性传播，而不是使用泰勒展开和分析方法的线性化。我们拥有确定性的样本，即Sigma点。根据可调参数，你基于缩放后的Cholesky因子的列选择不同的Sigma点。

与扩展卡尔曼滤波器相比，需要关注的独立项更少，因为它是基于采样的方法。你绘制这些确定性样本，得到加权样本，计算加权样本均值和协方差。

你可以解决相同的问题。如果设置基本，可能会得到相同的答案。在某些情况下，无迹卡尔曼滤波器可能工作得更好。但很多时候，我们可能得到相似的结果。它的吸引力在于它是无导数的，你不需要推导雅可比矩阵。

基于粒子滤波器的定位

现在，我们来讨论粒子滤波器。你可能期望这是一个更好的选择，因为它是多模态的，可以同时跟踪多个假设。这个滤波器可以解决全局定位问题。

这是一个带有可视化的示意图，展示了获取测量值、预测、运动预测和重采样的有用序列。

初始化：最初，我们不知道自己在哪。一个合理的选择是从均匀分布开始，随机散布粒子。

测量更新：假设我们第一次获得测量值，我们在一个门旁边。这会提高所有靠近这些门的粒子的权重（概率）。因为对于每个粒子，我们基于给定的地图计算预测测量值（例如到门的距离）。传感器告诉你另一个距离，你得到差值。这类似于新息。在粒子滤波中，我们使用传感器噪声协方差。通常，最好使用比实际噪声值更大的噪声参数，因为你希望滤波器能多探索一点，而不是坍缩到少数几个点上。我们运行更新，因为我们接收到的测量值提高了靠近门的粒子的权重。显然，我们无法区分三个门，因为它们对我们来说看起来一样。

预测（运动）：预测就像卷积，积分会根据你的模型将所有东西向前移动一个Δ。然后，如果你进行重采样，在重采样后，我们归一化权重，重新开始。现在所有权重相等，但在这三个点附近我们有更多的粒子质量。

再次测量：如果我们得到另一次校正，在其他两个假设位置很可能没有门。因此，位于其他两个门附近的粒子（我们直方图中显示的部分）不会获得更高的权重。仍然有一个靠近门。因此，两次观测和中间的一次运动帮助我们全局恢复了机器人的位置。我们刚刚解决了全局定位问题。

持续跟踪：然后，也许你再次移动，进行重采样，现在我们正在跟踪机器人。我们需要提取机器人位姿。你可以获取所有粒子的样本均值（对于角度要小心包裹），也可以获取样本协方差。最简单的方法是取样本均值。

关于数据关联模糊性：在这个问题中，存在数据关联模糊性。我们不知道门的ID。地图是已知的，但我们不知道门的ID。如果两个门不完全相同，但假设它们相同，你怎么知道这是1号门，那是2号门？特别是当你相对地观测它们时，你并不知道自己的全局朝向和位置。这就是为什么传感模型会给出三个门附近的峰值。

与马尔可夫定位的比较：马尔可夫定位是栅格世界贝叶斯滤波的介绍。它在内存上效率低下。我们不希望评估状态空间中每个点的似然。如果是2D，这已经太大了；3D则根本不可能。粒子滤波器通过跟踪一组固定的假设来实现内存高效，而不是评估世界中每个栅格的假设。

粒子滤波器在2D定位中的实际应用：这是一个在2D定位中实际工作得非常出色的算法。它是机器人定位的标准。实现简单，在实践中效果很好。这就是为什么粒子滤波器在定位中如此流行。

关于计算资源：过去，机器人处理器很弱，运行粒子滤波器或计算机视觉算法可能消耗所有资源。但今天，这不是问题，你可以在智能手机或小型嵌入式计算机上轻松运行此类算法。与深度神经网络相比，即使神经网络能解决问题，你也需要GPU。而粒子滤波器可以在任何计算机上运行，无需大量计算资源，降低了产品构建成本，因此仍然非常有吸引力。

总结与展望

在本节课中，我们一起学习了机器人定位的基本概念、问题设定和核心挑战。我们探讨了三种主要的解决方法：扩展卡尔曼滤波器、无迹卡尔曼滤波器和粒子滤波器，并了解了它们各自的原理和适用场景。

总结来说，对于2D定位问题，粒子滤波器通常是一个出色且实用的选择。然而，对于3D或更高维度的问题，由于采样效率问题，基于优化的方法（如我们将在SLAM中学习的）可能更受青睐。

定位是机器人感知环境、实现自主导航和交互的基石。掌握这些方法为我们后续学习地图构建以及同步定位与地图构建打下了坚实的基础。

012：占据栅格地图构建 🗺️

在本节课中，我们将要学习机器人建图的基础知识。具体来说，我们将探讨如何基于机器人搭载的传感器获取的相对噪声观测，来构建其周围环境的地图表示。

地图表示的概念

上一节我们介绍了建图的基本目标，本节中我们来看看地图的表示方法。地图的定义、表示和可视化方式并非唯一。这是一个根本性问题，因为如果没有唯一的方法，我们就可以提出多种想法。根据应用场景和计算资源，某些表示方法可能更优或更高效。

以下是几种不同的地图表示方式：

  • 特征地图：例如，著名的维多利亚公园数据集中，通过处理2D激光雷达点云检测到的树干位置，可以作为一系列地标点来表示地图。如果我们知道这些地标的全局坐标，就可以利用它们进行定位。
  • 密集地图：另一种方式是为环境中的每个部分（如栅格或点）建模一个概率，表示该位置被占据的可能性。这可以是2D的，也可以是3D的。
  • 体积地图 vs. 特征地图：体积地图（如3D网格）计算密集，而特征地图（如地标点）则更轻量。计算机图形学中的网格建模方法虽然精细，但通常计算量过大，不适合机器人机载计算。

为什么需要占据栅格地图？🧱

上一节我们了解了不同的地图表示，本节中我们来看看为什么需要占据栅格地图这种密集表示。对于导航任务而言，仅追踪地标点可能不够，我们需要知道哪些区域是可通行的自由空间。占据栅格地图提供了这种信息，尽管它并不排除与特征地图结合使用的可能性。

占据栅格地图的核心思想是将世界离散化为一个个单元格（栅格）。这个栅格结构是固定的，存储在内存中。我们的目标是根据接收到的观测，递归地更新每个栅格被占据的后验概率。

该方法的一个基本假设是：这些栅格在统计上与其邻居相互独立。每个栅格被假设为要么被占据（是障碍物），要么是自由的。我们主要关注“占据”状态。如果某个栅格被占据，意味着机器人撞上它会遇到障碍。如果它是自由的，则表明机器人可以安全通过。

占据栅格地图示例与假设

下图展示了一个占据栅格地图的例子，这是德国弗莱堡大学校园内一栋建筑的建图结果。通常使用灰度颜色映射：黑色单元格表示被占据（概率接近1），亮色单元格表示自由（概率接近0），灰色区域表示未知（概率接近0.5）。地图的分辨率在算法中是固定的，一旦建图完成就无法提高，这既是限制也是特点。

自由空间的概念对于路径规划至关重要。规划器（如A或RRT）可以在已知的自由单元格上为机器人规划从A点到B点的路径。然而，这种方法内置了一个重要假设：环境是静态的。如果环境中存在移动的物体（如行人），标准占据栅格地图无法直接建模这种动态变化。

此外，构建一致的地图通常依赖于准确的机器人位姿。如果仅使用存在漂移的里程计信息，可能会导致地图不一致（例如墙壁出现重影）。一个好的SLAM算法可以同时解决定位和建图问题，从而生成干净、一致的地图。

建图的动机与现实挑战 🚗

除了环境可能是动态的，还有其他构建地图的动机。即使对于室内环境，其布局（如家具摆放）也可能随时间改变。能够快速、频繁地重建场景对机器人非常有用，这样就不需要手动维护和更新地图供机器人导航。

这是一个非常基础且实际的问题。以自动驾驶为例，它们依赖高精地图进行定位和导航。但如果因为事故或道路施工导致路网改变，预先绘制的地图就不再准确。这就需要在线场景理解和建图能力，尽管这（例如理解交警手势）是更复杂的问题。

在实践中，我们使用测距传感器（如激光雷达）扫描环境。传感器发射红外光束并测量返回时间来计算距离。当环境中有人移动时，扫描中会出现不属于静态环境的异常点（离群值）。我们不希望这些点在地图中留下伪影。

因此，我们需要一种概率方法，理想情况下能够生成干净的地图，并对离群值和轻微移动的物体具有一定的鲁棒性。观测本身并非绝对和确定性的，所以基于概率推断建图是有意义的。虽然可以进行后处理来移除离群值，但我们的目标是在看到数据的同时在线构建地图。

占据栅格地图的历史与发展 📜

占据栅格地图的想法由Moravec和Elfes在20世纪80年代中期引入。他们当时使用有噪声的声纳（声学信号）信息，而非激光雷达。该方法的假设是机器人位姿已知，并尝试以此为基础构建地图。其核心思想是进行概率推理以增量式更新地图。

自那时起，这一领域不断发展。例如，后来的研究使用高斯过程进行回归来构建连续地图，它能捕捉环境中不同点之间的相关性，从而获得更准确的推断，但其计算复杂度较高（立方级）。在接下来的课程中，我们将探讨一种来自核方法家族的新方法，它是对高斯过程的一种近似，能将推断时间降至对数级，从而实现连续、3D甚至语义地图的构建。

然而，挑战依然存在。例如，如何选择合适的地图分辨率？对于一个25m x 25m的区域，若选择25厘米分辨率，将得到10,000个单元格。这已经是一个很大的状态变量数量。如果考虑每个单元格为二值状态（占据或自由），可能的地图配置数量是巨大的（2的10000次方）。显然，为整个地图运行传统的状态估计器（如卡尔曼滤波器）是不可行的。

核心假设与二值贝叶斯滤波 ⚙️

上一节我们回顾了历史并看到了问题的规模，本节中我们深入探讨解决这个问题的核心思想——二值贝叶斯滤波。首先，我们明确算法的假设，这有助于他人理解并在其基础上改进。

  1. 单元格状态是二值的：每个单元格要么被占据，要么是自由的。我们用概率 P(m_i = 1) 表示单元格 i 被占据的概率，P(m_i = 0) 表示其自由的概率。概率为0.5表示先验未知。
  2. 世界是静态的：我们假设处理的是静态环境，而不是动态物体。这意味着一旦推断某个单元格被占据，它将始终保持被占据状态。
  3. 单元格相互独立：这是该算法的一个基本假设。在进行单元格推断时，我们只关注该单元格本身，不考虑其邻居。这使得推断速度极快，易于在计算能力有限的设备上实现。

由于假设单元格独立，整个地图的后验概率可以分解为各个单元格概率的乘积：
P(m | z_{1:t}, x_{1:t}) = ∏_i P(m_i | z_{1:t}, x_{1:t})

我们的目标是推断单个单元格 i 的概率 P(m_i | z_{1:t}, x_{1:t})。这引出了一个滤波器，因为状态（单元格的占据情况）是二值的，所以称为静态二值贝叶斯滤波。

其图模型如下：在时间 t，机器人位于位姿 x_t，并观察到测量值 z_t。测量值由机器人位姿和地图 m 的形状共同引起。地图 m 是静态的（白色节点表示待估计的未知量），而位姿 x 和观测 z 是已知的（灰色节点）。这区别于SLAM（同时估计位姿和地图）和纯定位问题。

从贝叶斯规则到递归更新 🔄

我们从贝叶斯规则出发，目标是推导出一个递归更新公式。我们处理的是已知机器人位姿 x_{1:t} 和观测 z_{1:t} 的情况下，估计地图 m 的后验概率：P(m | z_{1:t}, x_{1:t})。

通过应用贝叶斯规则和马尔可夫假设（当前观测只依赖于当前位姿和地图），我们可以进行推导。直接使用前向模型 P(z_t | m, x_t) 很困难，因为它涉及复杂的地图假设。因此，通常采用逆传感器模型 P(m | z_t, x_t) 的思路，它表示在给定当前位姿和观测下，地图单元格状态的概率。

经过一系列推导（包括利用二值变量的特性，以及忽略机器人当前位置单元格信息以简化计算），我们最终得到一个关于概率比（称为几率）的递归更新公式。然而，直接使用概率进行连乘会遇到数值计算问题（下溢）。

为了解决数值问题，我们使用对数几率。令 l_{t,i} = log[ P(m_i | z_{1:t}, x_{1:t}) / (1 - P(m_i | z_{1:t}, x_{1:t})) ]。

最终的递归更新公式非常简洁：
l_{t,i} = l_{t-1,i} + log[ P(m_i | z_t, x_t) / (1 - P(m_i | z_t, x_t)) ] - log[ P(m_i) / (1 - P(m_i)) ]

其中：

  • l_{t-1,i} 是上一时刻的对数几率（后验）。
  • log[ P(m_i | z_t, x_t) / (1 - P(m_i | z_t, x_t)) ] 是逆传感器模型提供的对数几率（观测更新）。
  • log[ P(m_i) / (1 - P(m_i)) ] 是先验对数几率。如果先验概率为0.5，则此项为0。

这个公式的优势在于，我们将概率的连乘转换成了对数的相加，有效避免了数值下溢问题。并且，我们可以随时通过对数几率反推出概率：P = 1 - 1 / (1 + exp(l))。

占据栅格建图算法与逆传感器模型 🧠

基于上述推导，我们可以总结出占据栅格建图算法。该算法遍历地图中的所有单元格 i，对于每个单元格：

  1. 如果单元格 i 在当前传感器感知范围内，则根据以下公式更新其对数几率信念值：
    l_{t,i} = l_{t-1,i} + inverse_sensor_model(m_i, x_t, z_t) - l_0
    其中 l_0 是先验对数几率（例如，先验概率0.5对应 l_0=0）。
  2. 如果单元格 i 不在感知范围内，则保持其对数几率不变 l_{t,i} = l_{t-1,i}。

算法的计算复杂度与地图中单元格数量呈线性关系。为了提高效率，可以使用四叉树（2D）或八叉树（3D）等空间数据结构，避免遍历所有单元格，只更新感知范围内的区域。

现在，关键是如何实现 inverse_sensor_model(m_i, x_t, z_t)，即逆传感器模型。以下是一个基于激光雷达的典型简化模型思路：

假设一束激光测量到一个距离 z。我们不是只更新光束线上的单个点，而是考虑一个有一定厚度的区域（例如一个锥形），以加速建图。

  • 自由区域：从传感器到测量点 z 之间的区域，我们推断为自由空间。因为如果其中有障碍物，光束会被提前反射。
  • 占据区域：在测量点 z 附近的一个小邻域（例如 ±α/2 范围内），我们推断为被占据。
  • 未知区域：测量点 z 之后（被遮挡）以及光束锥形角 ±β/2 之外的区域，我们不予更新，保持先验。

在算法实现中，对于每个地图单元格 i，我们：

  1. 计算其相对于机器人位姿 x_t 的距离 r 和方位角 φ。
  2. 找到传感器光束中方位角最接近 φ 的那一束。
  3. 判断单元格 i 相对于该光束测量的位置：
    • 如果 r 超出传感器最大量程或 φ 偏差过大，则返回先验（不更新）。
    • 如果 r 在测量值 z 附近（|r - z| < α/2），则返回一个高的“占据”对数几率值（如对应概率0.65）。
    • 如果 r 小于测量值 z，则返回一个低的“自由”对数几率值（如对应概率0.35）。
    • 否则（r 大于 z 且不在占据区内），返回先验。

这里的阈值（如0.35, 0.65）和厚度参数（α, β）需要根据传感器特性和场景进行调优。许多开源库（如ROS中的Gmapping）提供了经过验证的参数和实现。

总结与展望 🎯

本节课中我们一起学习了机器人建图的基础方法——占据栅格地图构建。这是学习机器人密集重建问题的首要方法。

我们了解到，占据栅格地图将环境离散化为相互独立的栅格，并使用二值贝叶斯滤波递归更新每个栅格被占据的概率。其核心在于对数几率更新公式，它避免了数值问题并实现了高效的在线建图。该方法假设环境静态且机器人位姿已知，在满足条件时非常有效。

然而，该方法也有其局限性，例如单元格独立性假设可能不符合现实，以及只能提供点估计而无法得到不确定性度量。在接下来的课程中，我们将探讨如何放宽独立性假设，并使用更先进的机器学习方法（如基于核函数的连续映射）来获得更丰富、更准确的地图表示，包括3D和语义信息。同时，我们也会看到，尽管基础，占据栅格建图因其简单高效，仍然是许多实际机器人系统中的重要组成部分。

013：机器人建图 🗺️

在本节课中，我们将学习机器人建图。这是关于此主题的第二讲。我们将回顾占据栅格地图，并探讨其局限性。接着，我们将介绍一种更强大的贝叶斯推理方法——共轭先验，它能让我们构建带有不确定性估计的语义地图。最后，我们会讨论如何构建连续地图，并展望更高级的场景理解方法。

回顾占据栅格地图

上一讲我们介绍了一种基础方法，用于根据移动机器人的测距仪观测来重建二维场景。我们提到，理想情况下，我们希望获得真实轨迹，但由于无法直接获得，通常会运行SLAM算法来估计轨迹。

以下是一个位姿图的例子：

  • 每个节点代表机器人在特定时间步的姿态。
  • 边显示了轨迹的连接性。
  • 红色边代表机器人的连续运动。
  • 绿色边被称为“回环闭合”，即当机器人重访同一区域时，在图中现有节点之间建立的几何约束，这有助于改善估计结果。

理论上，图中重访和边越多，我们获得的信息就越多，结果应该越好。在求解出轨迹后，我们利用每个位姿的观测（例如测距数据）来重建地图，这就是占据栅格地图。

占据栅格地图的局限性

虽然占据栅格地图方法有效，但它存在几个关键问题。

首先，由于观测存在噪声且稀疏（激光测距仪只能提供离散的采样点），我们最终得到的地图可能不完整或存在不一致的区域。例如，透明物体可能导致激光穿透，在后方墙壁上留下错误的“空洞”。

其次，这种方法被扩展到三维，产生了如 OctoMap 这样的库。OctoMap使用体素（三维空间中的立方体）而非二维栅格来离散化地表示三维场景。它采用八叉树数据结构，允许多分辨率表示：在需要细节的区域使用小体素，在空旷或不重要的区域使用大体素，从而高效利用内存。

然而，无论是2D还是3D，占据栅格方法都共享以下核心局限：

  1. 独立性假设：栅格/体素被假设为统计独立的，我们无法利用环境固有的空间相关性。这导致稀疏点云会在地图中留下孔洞和间隙，算法无法“填补”这些空白。
  2. 缺乏不确定性估计：我们为每个单元计算占据概率，但这些概率在经过几次观测后会迅速饱和（接近0或1）。我们无法获知对不同区域的确信程度（即方差）。在决策时，仅依赖均值（占据概率）可能是危险的。
  3. 固定分辨率：地图一旦构建完成，其分辨率就固定了。我们无法在不从头开始的情况下提高地图分辨率，原始观测信息在转化为概率后便丢失了。

我们的目标是解决这三个问题：引入空间相关性、获得不确定性估计、实现连续地图表示。

共轭先验：高效的贝叶斯推理

为了克服上述局限，我们需要一种能推断完整分布（而不仅仅是点估计）的贝叶斯推理方法。这里的关键是共轭先验的概念。

在统计学中，如果后验分布与先验分布属于同一分布族，则它们被称为共轭分布。这意味着，如果我们选择适当的先验和似然函数，后验分布的形式将是已知的，我们只需要计算其参数即可。这带来了巨大的代数便利和计算效率。

以下是一些常见的共轭分布对：

  • 高斯似然 + 高斯先验 -> 高斯后验（用于估计浓度等连续值）
  • 伯努利似然 + Beta先验 -> Beta后验（用于二值占据估计）
  • 分类似然 + 狄利克雷先验 -> 狄利克雷后验（用于多类语义估计）

这种设置使得我们可以进行闭式递归贝叶斯更新。当我们听到新的观测数据时，只需简单地更新分布的超参数（例如，计数），而无需进行复杂的积分运算。

应用于语义建图

对于语义建图，我们处理的是多类别问题（例如：人行道、草地、墙壁、树木）。每个体素需要估计一个概率向量，表示它属于各个类别的可能性。

我们采用以下模型：

  • 测量：来自感知系统（如神经网络）的独热编码标签（例如，[0, 0, 1, 0] 表示类别3）。
  • 似然：分类分布。它是伯努利分布的多类别推广。
  • 先验/后验：狄利克雷分布。它是Beta分布的多类别推广。

在这个共轭模型下，贝叶斯更新变得异常简单：它本质上是一个计数传感器模型。对于每个体素，我们只需累加观测到的每个类别的次数。后验狄利克雷分布的参数（α）就是先验参数加上观测计数。

由此，我们可以轻松计算出每个类别的均值（期望概率）和方差（不确定性）：

  • 均值：E[θ_k] = α_k / (∑_j α_j)
  • 方差：Var[θ_k] = (α_k (α_0 - α_k)) / (α_0^2 (α_0 + 1))，其中 α_0 = ∑_j α_j

这样，我们不仅得到了地图（各类别的均值），还得到了每个区域的不确定性估计（方差），这可以用于引导机器人探索高不确定性区域。

迈向连续地图

计数模型虽然能提供不确定性，但仍假设体素独立。为了引入空间相关性并实现真正连续的地图，我们可以使用核函数。

核化贝叶斯广义推理的基本思想是：一个观测点不仅影响它所在的体素，还按照一定的权重影响其邻域。这个权重由核函数（如高斯径向基函数）决定，它根据两点之间的距离给出一个0到1之间的值。

具体操作是，在更新后验超参数时，我们用观测值乘以该观测点与目标查询点之间的核函数值。这样，靠近观测点的区域获得更强的更新，远离的区域获得较弱的更新。

这种方法的好处是：

  1. 引入局部相关性：通过核函数在局部空间内平滑信息，填补小间隙。
  2. 实现连续查询：地图模型不再绑定于离散的体素网格。我们可以在空间中任意点查询其语义类别概率和不确定性。
  3. 可调节分辨率：由于模型是连续的，我们可以在任何时候以任意分辨率查询地图。

总结与展望

本节课我们一起学习了机器人建图的进阶内容。

我们首先回顾了占据栅格地图及其在独立性、不确定性和分辨率方面的局限性。接着，我们引入了共轭先验这一强大的统计工具，它允许我们进行高效的递归贝叶斯推理。通过使用狄利克雷-分类共轭对，我们实现了语义建图，并能同时估计每个区域的均值和方差。最后，我们探讨了通过核函数引入空间相关性，从而构建连续地图的方法。

当前的研究正在向更丰富的场景理解迈进，例如动态地图（处理移动物体）和3D场景图（结合度量、物体、场景和拓扑层次的信息）。这些高级表示对于机器人进行复杂的交互和决策至关重要。

建图是一个充满活力且视觉直观的研究领域，其算法和软件库（如OctoMap、Open3D等）已成为许多机器人系统的核心组成部分。

014：优化与平滑 I

在本节课中，我们将学习优化与平滑的基础知识，这是该主题两讲中的第一讲。我们将重点聚焦于线性最小二乘问题，并逐步构建对一类可通过非线性最小二乘求解器（如高斯-牛顿法及其扩展）解决的问题的理解。

符号与定义

首先，我们建立一些基本符号和定义。

一个矩阵是正定或半正定的。当我们看到符号 A > B 时，意味着 A - B 是正定的，即 A - B 具有正的特征值。

L2范数，即欧几里得范数，可以用符号 ||x|| 或 ||x||₂ 表示，其计算公式为 √(xᵀx)。

L1范数是向量 x 各元素绝对值的和，即 ∑|xᵢ|。

更一般地，Lp范数的计算公式为 (∑|xᵢ|ᵖ)^(1/p)。

点积或内积可以用 xᵀy 或 <x, y> 表示。

范数球是一个以 x_c 为中心、半径为 r 的邻域，记作 B(x_c, r)。默认情况下，我们使用欧几里得范数来定义这个球。

优化问题基础

我们处理的目标函数 f(x) 将一个 Rⁿ 中的向量映射为一个实数值。这个函数的自变量称为决策变量或设计变量。我们的目标是找到使目标函数最小化的 x 值。

如果找到的解在整个定义域内都能使函数值最小，则称为全局最小值。如果解仅在一个邻域内使函数值最小，则称为局部最小值。

例如，一个函数可能有多个局部最小值和局部最大值，以及一个全局最大值和一个全局最小值。

连续函数的分析工具

我们通常假设目标函数是光滑且连续的，这意味着我们可以对函数进行微分。

函数的一阶导数是一个向量，称为梯度，记作 ∇f(x)。它是函数对所有变量的偏导数组成的向量。在列向量的上下文中，梯度通常是一个列向量。

函数的二阶导数是一个对称矩阵，称为海森矩阵，记作 H(x) 或 ∇²f(x)。它由函数的二阶偏导数组成。由于函数连续可微的假设，海森矩阵是对称的。

利用泰勒定理，我们可以对一个连续可微（解析）的函数进行局部近似。这意味着我们可以用多项式来逼近函数。

一阶近似（线性化）的公式为：
f(x + d) ≈ f(x) + ∇f(x)ᵀ d

二阶近似（二次近似）的公式为：
f(x + d) ≈ f(x) + ∇f(x)ᵀ d + (1/2) dᵀ H(x) d

我们可以忽略更高阶的项。在欧几里得空间中，这是标准的近似方法。

最优性条件

从微积分中我们知道，找到函数极值的一阶必要条件是令梯度等于零：
∇f(x) = 0

对于寻找最小值，二阶必要条件是海森矩阵半正定。这意味着在局部邻域内函数是凸的，从而导向一个最小值。如果海森矩阵负定，则对应局部最大值。我们总是处理最小化问题，若要最大化，只需将目标函数乘以负号。

二阶充分条件是海森矩阵的所有特征值均为正。如果特征值符号混合，则为鞍点，这不是一个理想的解。

凸性

另一个重要概念是凸性。如果我们遇到的目标函数是凸的，那么局部搜索将给出全局解，这非常理想。

一个函数是凸的，当且仅当对于任意两点 x₁, x₂ 和任意 θ ∈ [0, 1]，满足：
f(θx₁ + (1-θ)x₂) ≤ θf(x₁) + (1-θ)f(x₂)

几何上，这意味着连接函数图像上任意两点的线段总是位于函数图像之上。凸性的二阶条件是海森矩阵处处半正定。

线性最小二乘问题

我们讨论的第一个具体问题是线性最小二乘问题。理解线性情况很重要，因为非线性方法通常通过迭代求解线性子问题（通过线性化）来工作。

线性最小二乘问题通常形式为 Ax = b，其中 A 是一个“高瘦”矩阵（行数多于列数），且列满秩。在实践中，由于观测数据存在噪声和不完美，b 通常不在 A 的列空间中。

我们定义残差为模型与测量值之间的差异：r(x) = Ax - b。我们的目标是最小化该残差的L2范数的平方：
minimize f(x) = (1/2) ||r(x)||² = (1/2) (Ax - b)ᵀ (Ax - b)

计算该二次项的梯度，得到：
∇f(x) = AᵀAx - Aᵀb

海森矩阵为常数矩阵 AᵀA。如果 A 列满秩，则 AᵀA 正定。

根据一阶必要条件，令梯度为零，得到正规方程：
AᵀA x = Aᵀb

其解可符号化地写为：
x = (AᵀA)⁻¹ Aᵀb*

二阶条件告诉我们，由于 AᵀA 正定，这是一个全局最小值。我们通常不直接求逆，而是通过乔列斯基分解（因式分解 AᵀA）或直接对 A 进行QR分解来求解。

示例：目标跟踪与平滑

我们以目标跟踪为例。之前我们使用不同的滤波器递归地解决这个问题。线性最小二乘也可以递归求解，其解与线性卡尔曼滤波器相同。

现在我们讨论批量解法：我们拥有所有数据，并希望一次性求解一个最小二乘问题来估计整个轨迹。由于使用了未来信息来修正过去的估计，这被称为平滑问题，通常以优化或处理时间窗口数据的形式出现。

在目标跟踪平滑问题中，我们有一个线性运动模型和一个线性测量模型。我们需要构建残差，并将所有时间步的残差堆叠起来，最终形成一个形如 Ax = b 的线性系统，其中 x 是整个离散化的轨迹（所有时间步的状态）。

当我们堆叠所有变量时，得到的矩阵 A 通常是稀疏矩阵，即大部分元素为零。利用稀疏性可以高效地存储和求解，例如使用稀疏矩阵格式，并通过QR或乔列斯基分解进行因式分解。在SLAM中，由于闭环等因素，稀疏模式会更加有趣，但保持稀疏性对于高效求解至关重要。

示例：带L2正则化的线性回归

另一个有趣的例子是机器学习中的带L2正则化的线性回归。我们有一个数据集，包含输入 xᵢ 和目标值 yᵢ。我们想建立一个线性模型：y = wᵀ φ(x)，其中 φ(x) 是一组基函数（可以是线性的或非线性的），w 是待估计的权重向量。

目标是最小化预测误差与正则项之和：
minimize (1/2) ||Φw - y||² + (λ/2) ||w||²
其中 Φ 是设计矩阵，其第 i 行为 φ(xᵢ)ᵀ，λ 是正则化参数。

计算梯度并令其为零，得到解：
w = (ΦᵀΦ + λI)⁻¹ Φᵀ y*

正则化项 λI 改变了正规方程，有助于防止过拟合，并确保在欠定情况下问题仍然是适定的。通过调整 λ，我们可以在拟合数据和保持模型简单性之间进行权衡。

非线性最小二乘问题

我们的主要目标是学习如何求解非线性最小二乘问题。在这种情况下，残差 r(x) 是 x 的非线性函数，但目标函数仍然是残差范数的平方：f(x) = (1/2) ||r(x)||²。

残差是一个向量值函数：r: Rⁿ → Rᵐ。我们假设它是光滑的。目标函数的梯度为：
∇f(x) = J(x)ᵀ r(x)
其中 J(x) 是残差 r(x) 的雅可比矩阵（每行是单个残差分量对 x 的梯度）。

海森矩阵为：
H(x) = J(x)ᵀ J(x) + S(x)
其中 S(x) 包含残差的二阶导数项。

高斯-牛顿算法

高斯-牛顿算法是求解非线性最小二乘问题的一种方法。它是一种局部求解器，需要一个初始猜测值 x₀。

算法迭代进行：

  1. 在当前点 x_k 线性化残差：r(x_k + d) ≈ r(x_k) + J(x_k) d
  2. 求解对应的线性最小二乘子问题，找到步长 d_k：
    minimize ||J(x_k) d + r(x_k)||²
    这等价于求解：(JᵀJ) d = -Jᵀ r
  3. 更新状态：x_{k+1} = x_k + d_k
  4. 重复直到收敛。

高斯-牛顿法的关键在于，它用 JᵀJ 近似了海森矩阵 H(x)，避免了计算二阶导数 S(x)。只要雅可比矩阵列满秩，JᵀJ 就是半正定的，这通常能保证下降方向。

牛顿法

相比之下，标准的牛顿法使用完整的海森矩阵来构建目标函数的二次近似：
minimize f(x_k) + ∇f(x_k)ᵀ d + (1/2) dᵀ H(x_k) d

令其导数为零，得到牛顿步长：
H(x_k) d = -∇f(x_k)

牛顿法收敛速度快（局部二次收敛），但需要计算海森矩阵，并且不能保证海森矩阵正定，可能收敛到鞍点或极大值点。

高斯-牛顿与牛顿的对比

对于最小二乘问题，高斯-牛顿法可以看作是牛顿法的一种近似，其中海森矩阵被 JᵀJ 替代。如果残差很小或接近线性，S(x) 项可以忽略，两者表现相似。高斯-牛顿法的优势在于无需计算二阶导数，且 JᵀJ 总是半正定的，更容易产生下降方向。

示例：非线性目标跟踪

让我们将目标跟踪问题变为非线性。假设运动模型仍是线性的，但测量模型是测距和测角。此时，测量残差是非线性的。

我们需要构建残差并计算其雅可比矩阵。高斯-牛顿算法的步骤是：

  1. 给定初始猜测（例如，从噪声测量中三角化得到）。
  2. 在当前位置计算残差 r 和雅可比矩阵 J。
  3. 求解线性系统 (JᵀJ) d = -Jᵀ r 得到步长 d。
  4. 更新状态。
  5. 重复直到轨迹平滑。

实践中，我们可以使用现成的求解器（如Ceres、g2o、GTSAM），它们要求用户提供残差和雅可比矩阵的计算函数。

全局化策略

高斯-牛顿等方法是局部求解器，其收敛性依赖于初始猜测。全局化策略旨在提高算法收敛到（某个）局部最小值的能力。

一种流行的方法是阻尼法或线搜索。其思想是：在计算出方向 d_k 后，不直接采用全步长，而是寻找一个步长 α ∈ (0, 1]，使得目标函数充分下降：
f(x_k + α d_k) < f(x_k)

方向 d 是下降方向需满足 ∇f(x)ᵀ d < 0。牛顿方向在高斯-牛顿中，若 J 满秩，则是下降方向。

精确线搜索求解 min_α f(x_k + α d_k)，但计算昂贵。更常用的是回溯线搜索：从一个较大的 α（如1）开始，不断乘以一个衰减因子（如0.9）缩小 α，直到满足某个下降条件（如Armijo条件）。

阻尼版本可能增加迭代次数，但能提高稳定性。

信赖域方法

另一种全局化策略是信赖域方法。其思想是：在每次迭代中，定义一个信赖域半径 Δ，并在这个区域内求解近似模型（线性或二次）的最优步长。

我们通过比较实际函数下降值与模型预测下降值的比率，来评估近似模型的可信度，并动态调整 Δ。如果比率好，则扩大信赖域；如果差，则缩小信赖域并重新求解。

莱文贝格-马夸特方法

莱文贝格-马夸特方法（LM）是高斯-牛顿法的一种扩展，它结合了信赖域思想。它在高斯-牛顿的正规方程中加入了一个阻尼项：
(JᵀJ + λ I) d = -Jᵀ r

这里 λ 是阻尼参数。其作用类似于信赖域半径：

  • 当 λ 很大时，方程主导项为 λI d ≈ -Jᵀr，步长 d 很小，方向接近最速下降方向。
  • 当 λ 很小时，方程接近高斯-牛顿方程 (JᵀJ) d = -Jᵀr。

算法根据近似模型的表现动态调整 λ：如果步长被接受且模型拟合好，则减小 λ（扩大信赖域）；否则增加 λ（缩小信赖域）。马夸特提出了一种改进，使用 diag(JᵀJ) 进行缩放，以提高数值稳定性。

LM方法在实践中非常鲁棒，是许多非线性最小二乘求解器的默认选择。

线性系统求解

无论使用高斯-牛顿、牛顿还是LM方法，最终都需要求解一个形如 A d = b 的线性系统。我们绝不应该显式地计算矩阵的逆。

两种主要方法是：

  1. 乔列斯基分解：适用于对称正定矩阵 A（如 JᵀJ 或 JᵀJ + λI）。将 A 分解为 LLᵀ，然后通过前向和后向代入求解。
  2. QR分解：直接对雅可比矩阵 J（或增广矩阵）进行分解。得到 J = QR，其中 Q 正交，R 上三角。然后求解 R d = -Qᵀ r。

QR分解数值稳定性更好，但计算量通常比乔列斯基分解大。一个经验法则是：优先使用QR分解以保证稳定性，除非对速度有极端要求且问题条件良好。

总结与展望

本节课中，我们一起学习了优化与平滑的基础知识。我们从线性最小二乘问题入手，学习了其定义、解法（正规方程、乔列斯基分解、QR分解）以及在目标跟踪平滑和正则化线性回归中的应用。

接着，我们探讨了非线性最小二乘问题，并介绍了高斯-牛顿算法，它通过线性化残差并迭代求解线性子问题来工作。我们比较了高斯-牛顿法与牛顿法，并解释了为何前者在最小二乘问题中更受欢迎。

为了提升算法的鲁棒性和全局收敛性，我们介绍了全局化策略，包括线搜索和信赖域方法，并详细阐述了莱文贝格-马夸特方法如何将阻尼项与信赖域思想结合。

最后，我们讨论了求解内部线性系统的数值方法，强调了使用分解而非求逆的重要性。

所有这些概念为我们接下来解决同步定位与地图构建问题奠定了基础。在SLAM和图优化中，我们将面对大规模的非线性最小二乘问题。然而，一个关键的挑战是，SLAM中的参数（机器人的位姿）属于李群（如SE(2), SE(3)），而非欧几里得空间。因此，下节课我们将深入探讨如何在李代数上进行最小二乘优化，从而最终解决图优化SLAM问题。

015：优化与平滑 II 🧮

在本节课中，我们将要学习优化与平滑的进阶内容。我们将从非线性最小二乘问题出发，探讨当目标函数不是最小二乘形式时该如何处理，并介绍一种名为迭代重加权最小二乘（IRLS）的新算法。接着，我们将讨论鲁棒估计中的M估计方法。最后，我们将学习如何在流形，特别是李群上进行优化。

回顾：非线性最小二乘问题 🔄

上一节我们介绍了非线性最小二乘问题。其目标函数是残差向量的欧几里得范数平方。残差是一个从输入空间到向量空间的映射。由于残差通常是非线性的，我们采用了高斯-牛顿算法：在当前迭代点对残差进行线性化，得到一个仿射近似，然后求解这个线性最小二乘问题，更新变量，并迭代直到收敛。

高斯-牛顿算法的核心思想是：当问题非线性时，线性化并迭代。其基本形式易于实现：设置一个循环，当梯度（即雅可比矩阵转置乘以残差）的范数为零时，我们满足最优性必要条件，可以停止迭代。

从L2范数到P范数 📈

一个自然的问题是：如果目标函数使用的不是欧几里得范数（L2范数）怎么办？根据具体问题，我们可能选择其他范数。例如，我们可以使用P范数，其定义为对每个残差项的绝对值取P次幂，求和后再取1/P次幂。

当P不等于2时，这就不再是最小二乘问题，我们无法直接使用高斯-牛顿算法。虽然可以使用通用的非线性求解器，但它们无法像高斯-牛顿那样利用问题的特殊结构（通过雅可比矩阵近似海森矩阵，获得类似二阶求解器的效果）。

迭代重加权最小二乘（IRLS）算法 ⚙️

历史上（大约在20-30年前），一种常见的技巧是将P范数最小化问题转化为加权最小二乘问题。具体做法是重新定义优化问题，引入一个权重矩阵。这个权重矩阵是对角阵，其对角线元素是每个残差绝对值的(P-2)次幂。

这样，当权重固定时，问题就变成了一个加权最小二乘问题，易于求解。关键在于，权重本身依赖于残差，而残差又依赖于解。因此，IRLS算法在固定权重求解最小二乘问题和根据新解更新权重之间交替进行。这就是“迭代重加权”的由来。

这是一个实用的技巧，虽然没有严格的理论保证，但在机器人学（如鲁棒优化）等领域有广泛应用。实践中，可以从单位权重开始，求解最小二乘问题得到初始猜测，然后迭代直至收敛。

注意：即使残差本身是线性的，只要范数是P范数（非L2），IRLS仍然需要迭代。

示例：L1范数线性回归与鲁棒估计 🛡️

让我们回到线性回归的例子。假设数据中存在离群值——这些数据点不符合潜在的趋势，会严重干扰估计结果。处理离群值在实践中非常重要。

模型本身不变，我们仍然使用带高斯基的线性模型。改变的是目标函数：我们将L2范数替换为L1范数。L1范数（也称为曼哈顿距离）传统上用于鲁棒优化，因为它倾向于忽略离群值。此外，L1范数还能促进解的稀疏性（许多权重会趋于零），这在稀疏编码等领域很有用。

目标函数变为最小化L1范数下的残差。这不是最小二乘问题，但我们可以使用IRLS将其转化为加权最小二乘问题。对于L1范数（P=1），权重矩阵是残差绝对值的倒数（即P-2 = -1）。求解过程涉及设置两个新的权重矩阵，计算梯度，并找到最优解。然后，用新的解重新计算残差和权重，进行下一次迭代。

实现注意：当残差接近零时，权重计算会出现除以零的问题。实践中，我们通常设置一个小的阈值（如 max(|残差|, δ)）来保证数值稳定性。

L1范数最小化也称为最小绝对偏差回归，它对离群值鲁棒并能促进稀疏性。IRLS不一定保证收敛，因此我们通常设置最大迭代次数，并需要检查解的正确性。

在示例中，使用L1回归后，许多权重变为零，我们只使用了少量基函数来估计趋势，同时自动忽略了离群值。这展示了范数选择如何显著改变求解器的行为。

M估计：鲁棒估计的一般框架 🧰

L1回归只是鲁棒估计的一个例子。更一般的一类方法是M估计（“M”代表最大似然型估计）。其思想是用一个鲁棒核函数（或称为损失函数）来“包裹”残差，而不是直接使用残差的平方和。

在最大似然估计的背景下，如果我们假设噪声为高斯分布，取负对数似然就会得到最小二乘问题。但当存在离群值时，这可能不是最佳选择。M估计使用一个对离群值鲁棒的核函数 ρ(·) 来构造新的目标函数。

通过计算新目标函数关于决策变量的梯度（应用链式法则），并设其为零，我们可以得到最优性条件。经过一些代数变换，这个条件可以写成一个加权最小二乘问题的形式，其中权重依赖于残差和核函数 ρ 的导数。

因此，M估计问题最终也归结为一个权重不固定的加权最小二乘问题，我们可以用IRLS算法来求解。权重的具体形式由核函数 ρ 决定：权重 = ρ‘(残差) / 残差。

示例：柯西损失函数 📉

一个常见的选择是柯西损失函数。它的形式是 ρ(r) = log(1 + (r/α)^2)，其中 α 是一个可调超参数。其导数很容易计算，从而得到IRLS中所需的权重。

直观上，L2损失（二次函数）会随着残差增大而急剧增加，导致离群值的梯度主导整个优化过程。鲁棒核函数（如柯西损失）的作用是“弯曲”或“截断”二次函数的增长，使得当残差超过某个由 α 控制的阈值后，其对目标函数的贡献不再急剧增加，从而被优化器忽略。

使用柯西损失的M估计目标不再是促进稀疏性，而是专注于鲁棒性，所有权重通常都是非零的。当然，你也可以额外添加正则化项来同时获得稀疏性。

李群上的优化 🌀

在机器人状态估计中，我们经常需要估计位姿（旋转或刚体变换），这些变量是李群的元素（如旋转矩阵）。如果状态是欧几里得空间中的点，我们已经知道如何优化。但当决策变量是李群元素时，情况就不同了。

回顾一下，矩阵李群是一组可逆矩阵，在矩阵乘法下构成群。李代数是对应李群在单位元处的切空间，它是一个向量空间。指数映射将李代数中的元素映射到李群中。

关键问题是：李群对加法不封闭（旋转矩阵相加不一定还是旋转矩阵），但对乘法封闭。因此，在优化迭代中，我们不能简单地使用 R_{k+1} = R_k + ΔR 来更新。我们必须尊重李群的乘法结构。

流形优化框架：提升、求解、收回 📤➡️🧮➡️📥

解决李群优化问题的一般框架称为提升、求解、收回。

  1. 提升：由于直接在流形上求解困难，我们将问题“提升”到李代数（切空间）中。我们通过在当前点 X_k 处乘以一个李代数中小扰动 d 的指数来参数化扰动：X_{k+1} = X_k · exp(d)。然后，我们对残差在这个扰动 d 处（在 d=0 点）进行线性化，得到雅可比矩阵。
  2. 求解：在向量空间（李代数）中求解线性化后的最小二乘问题，得到最优扰动 d*。
  3. 收回：将解“收回”到流形上。对于李群，收回映射就是指数映射：X_{k+1} = X_k · exp(d*)。对于欧几里得变量，则简单相加。

这个框架允许我们利用线性代数的工具（高斯-牛顿等）来解决李群上的非线性优化问题。雅可比矩阵的计算通常利用指数映射的一阶近似和链式法则，有时比在欧氏空间中直接求导更结构化、更简单。

李群线性化与BCH公式 ➕

线性化涉及处理像 exp(A)exp(B) 这样的乘积。对于矩阵，exp(A)exp(B) ≠ exp(A+B)，除非 A 和 B 可交换。一般的公式由Baker-Campbell-Hausdorff（BCH）公式给出，它是一个无穷级数。

幸运的是，在优化中，我们的扰动 d 总是很小的。当两个指数因子中有一个是小量时，BCH公式可以简化。这导出了左雅可比和右雅可比矩阵，它们有闭式解（对于SO(3)和SE(3)已知）。这些雅可比矩阵使我们能够将群乘法线性地近似为李代数中的加法。

应用示例：位姿图优化（SLAM后端）🗺️

一个重要的应用是位姿图优化，这是同步定位与建图（SLAM）的后端核心。我们有一系列机器人位姿（关键帧）作为图节点。连续关键帧之间的相对运动估计（来自里程计等）以及非连续的关键帧之间的闭环检测，构成了图的边。

每条边提供一个相对位姿测量 Z_{ij}，它应该满足 X_j = X_i · Z_{ij}。考虑到噪声，我们定义残差为 r_{ij} = log( (Z_{ij})^{-1} · X_i^{-1} · X_j )，理想情况下应为零。

为了优化所有位姿 {X_i} 以最小化所有残差的平方和，我们使用上述李群优化框架。对每个位姿变量进行扰动（例如从右侧扰动），利用伴随表示和BCH公式进行线性化，最终得到关于所有李代数扰动向量 {d_i} 的线性最小二乘问题。求解后，用指数映射更新位姿，并迭代。

现代SLAM库（如g2o, GTSAM）的后端就是基于这种图优化原理。前端负责从原始数据（图像、激光等）中检测特征、跟踪并建立这个图（包括检测闭环），后端则负责求解这个优化问题。

另一个示例：点云配准 ☁️↔️☁️

点云配准是另一个经典问题：给定两个点云 {x} 和 {y}，寻找一个刚体变换 T，使得变换后的 y 点云与 x 点云对齐。目标函数是最小化对应点之间的距离平方和。

这本质上是一个关于 T （属于SE(3)）的最小二乘问题。为了使用高斯-牛顿法，我们需要计算残差关于 T 的雅可比矩阵。通过从左侧扰动 T （T <- exp(d) · T），并利用指数映射的一阶近似，我们可以分离出残差和雅可比矩阵项。经过代数运算，可以得到一个3x6的雅可比矩阵，从而线性化问题并求解。

这种方法避免了复杂的微积分，主要依靠代数和指数映射的近似，通常更为简洁优雅。

总结 📚

本节课我们一起学习了优化与平滑的进阶主题：

  1. 迭代重加权最小二乘：一种通过交替求解加权最小二乘和更新权重，来解决一般P范数最小化问题的实用算法。
  2. 鲁棒估计与M估计：通过使用鲁棒核函数（如L1范数、柯西损失）构造目标函数，使估计对数据中的离群值不敏感。M估计问题可以转化为加权最小二乘，并用IRLS求解。
  3. 李群上的优化：当决策变量属于李群（如旋转矩阵）时，我们不能直接使用向量加法更新。通过“提升-求解-收回”框架，我们将问题线性化到李代数（向量空间）中求解，再利用指数映射将解收回流形。这为机器人学中的位姿图优化、点云配准等问题提供了强大的解决工具。

这些方法构成了现代移动机器人状态估计、SLAM等核心算法的基础。理解它们有助于你深入使用相关库（如g2o, Ceres）并推动算法前沿。

016：点云配准 I 🧩

在本节课中，我们将要学习点云配准的基础知识。点云配准是机器人感知中的一个核心问题，旨在找到两个点云数据集之间的几何变换关系。我们将从问题定义开始，介绍经典的迭代最近点算法及其改进版本，并探讨实际应用中的挑战与解决方案。

点云数据来源 📷

上一节我们介绍了点云配准的基本概念，本节中我们来看看点云数据通常从哪些传感器获取。

以下是几种常见的点云数据来源：

  • RGB-D相机：这类深度相机能提供三维点坐标、深度值以及对应的RGB彩色图像。每个RGB像素都有一个对应的深度值，从而生成密集的深度图。它们通常适用于室内环境，有效范围通常在10米以内。
  • 立体相机：通过左右两个图像，利用视差原理计算像素的深度值。这是一种被动式深度感知方法。著名的ELAS库可用于从图像对中恢复深度。
  • 激光雷达：一种主动式传感器，通过发射光束并测量飞行时间来获取距离信息。它不依赖环境光照，通常提供点云的反射强度信息，但不直接提供颜色。

什么是配准问题？ 🎯

在了解了数据来源后，我们正式定义点云配准问题。

点云配准问题是指寻找两个数据集之间的几何关系。在机器人学背景下，数据通常来自传感器（如激光雷达或深度相机）。我们通常寻找的几何关系是一个刚体变换。

我们可以将配准算法按不同维度分类：

  • 分辨率：分为粗配准（快速获取大致变换，用于先验或地点识别）和精配准（追求精确对齐，用于场景重建）。
  • 方法：分为直接法（使用原始观测值，如原始点云）和间接法（使用从数据中提取的特征，如SIFT、ORB等具有尺度或旋转不变性的特征）。
  • 求解范围：分为局部求解（需要较好的初始猜测）和全局求解（能处理更大的初始不对齐，但通常更慢）。

问题形式化与ICP算法 🔄

现在我们已经对问题有了基本认识，本节我们来形式化点云配准问题并介绍经典的ICP算法。

我们有一个由三维点组成的集合，每个点有3个坐标：x_i ∈ R^3。

一个刚体变换的作用定义为：T(x) = R * x + p，其中 R ∈ SO(3) 是旋转矩阵，p ∈ R^3 是平移向量。因此，变换参数 T = (R, p) 是特殊欧几里得群 SE(3) 的一个元素，这正是我们需要估计的参数。

我们定义两个点云：

  • 目标点云：被视为位于固定参考系中的点云。
  • 源点云：我们需要对其应用变换 T，使其与目标点云对齐。

迭代最近点 算法是解决该问题最著名的方法之一，其核心思想分为两步：

  1. 关联查找：给定当前变换猜测，为源点云中的每个点，在目标点云中寻找最近邻点作为对应点。
  2. 求解变换：根据找到的对应点对，通过最小化点对之间的距离来求解最优变换。

然后迭代重复这两个步骤，直到收敛。

为什么需要关联查找？因为传感器只提供点云，我们不知道源点云中的哪个点对应目标点云中的哪个点。如果我们知道完美的对应关系，问题有闭式解。但在现实中，对应关系是未知的，ICP通过迭代最近邻来近似。

距离残差通常定义为对应点之间的欧氏距离：r_i = y_i - (R * x_i + p)，其中 (x_i, y_i) 是一对对应点。优化问题是最小化所有残差的平方和。

广义ICP与概率模型 📊

基本的ICP算法存在局限性，例如对初始猜测敏感、容易陷入局部最优。本节我们介绍其改进版本——广义ICP，它引入了概率模型。

GICP的核心思想是不再将点视为精确位置，而是视为具有不确定性的高斯分布。我们为每个点估计一个局部协方差矩阵，该矩阵描述了该点周围的局部形状（例如，墙面上的点，其法线方向的方差很小）。

通过特征值分解，我们可以调整协方差矩阵，使其在假设的表面法线方向变得“平坦”（即特征值很小）。这样，配准就从点对点匹配变成了概率分布对概率分布的匹配。

在新的概率模型下，残差 r_i = y_i - T(x_i) 被视为两个高斯分布的差，其协方差为：C_i = Σ_{y_i} + R * Σ_{x_i} * R^T。

优化目标变为最小化马氏距离：∑ r_i^T * C_i^{-1} * r_i。这等价于在假设观测为高斯分布下的最大似然估计。这种加权距离度量能更好地利用环境的结构信息，提高配准的鲁棒性和准确性。

实现细节与挑战 ⚙️

理解了GICP的原理后，我们来看看具体的实现细节以及实际应用中面临的挑战。

最近邻搜索：高效查找最近邻是关键。通常使用 KD-Tree 数据结构，它可以将搜索复杂度从 O(n) 降低到 O(log n)。常用的库包括FLANN和nanoflann。

在SE(3)上优化：我们需要在流形 SE(3) 上求解优化问题。通过定义扰动 Δξ（一个李代数元素），我们可以计算残差关于扰动的雅可比矩阵，然后使用高斯-牛顿或列文伯格-马夸尔特方法迭代求解。

代码流程概述：

  1. 输入目标点云、源点云和初始变换猜测（常设为单位阵）。
  2. 为每个点计算局部协方差矩阵。
  3. While 未收敛且未超最大迭代次数：
    • 用当前变换移动源点云。
    • 在目标点云中为移动后的源点寻找最近邻（可设置最大距离阈值剔除离群点）。
    • 构建雅可比矩阵和残差，求解增量 Δξ。
    • 更新变换：T_{k+1} = exp(Δξ) · T_k。
    • 检查变换更新量是否小于阈值以判断收敛。

主要挑战：

  • 未知对应关系：这是根本难点。
  • 需要良好初始猜测：非线性优化易陷入局部最优。
  • 部分与未知重叠：两个点云可能只有部分区域重叠，且重叠区域未知。
  • 离群点：错误关联会严重干扰优化。

应对策略：

  • 使用鲁棒损失函数（如柯西损失）替代平方损失，以软忽略离群点。
  • 结合其他传感器（如IMU）提供初始猜测。
  • 在序列配准中，使用上一帧的结果作为下一帧的初始值。

扩展：语义ICP 🏷️

最后，我们探讨一个现代扩展方向——语义ICP，它利用点云的语义信息来改进配准。

基本思想是：如果知道每个点的语义类别（如“道路”、“建筑”、“车辆”），那么配准时应主要让同类别的点相互匹配。这可以极大地缩小数据关联的搜索空间，减少歧义。

实现上，可以将语义概率信息融入GICP的框架。通过期望最大化 步骤，软性地计算点与点之间属于同一类别的权重，并将此权重融入关联和加权过程中。这相当于在几何距离的基础上，增加了一个语义相似性度量，从而能更准确地引导变换求解。

实验表明，语义ICP能在传统GICP的基础上，进一步提升配准精度，尤其是在平移估计方面。

总结 📝

本节课中我们一起学习了点云配准的基础知识。我们从问题定义出发，介绍了点云数据的常见来源。然后，我们详细讲解了经典的ICP算法及其核心迭代步骤：关联查找与变换求解。为了克服ICP的局限性，我们引入了广义ICP，它通过概率模型和局部协方差描述，将点对点匹配提升为分布对分布匹配，提高了鲁棒性。我们还讨论了实现中的关键细节，如KD-Tree搜索和在SE(3)流形上的优化，并指出了实际应用中的主要挑战。最后，我们展望了利用语义信息进一步改进配准的语义ICP方法。点云配准是机器人感知、SLAM和三维重建的基石，理解这些基本方法为进一步探索更先进的算法打下了坚实基础。

017：RKHS点云配准

在本节课中，我们将学习一种用于点云配准的新框架——RKHS配准。RKHS代表再生核希尔伯特空间，它源于机器学习中的核方法。我们将了解这个框架能做什么以及如何使用它，而不必深入其复杂的数学基础。

上一节我们介绍了点云配准的传统方法，本节中我们来看看一种将数据视为连续函数的全新视角。

概述

传统的点云配准方法（如ICP）通常依赖于点对点的硬对应关系，并要求点云具有相同数量的点。然而，在实际应用中，传感器在不同时间步长采集的点云通常点数不同、密度不均，且可能只有部分重叠。RKHS配准框架旨在解决这些问题。它将离散的点云数据“提升”到一个连续的函数空间，通过最大化两个函数之间的内积（即最小化它们之间的夹角）来求解最优变换。这种方法天然地处理了未知对应关系、点数可变和部分重叠的情况，并能自然地融合几何信息（如3D坐标）与非几何信息（如颜色、语义标签）。

背景概念

在深入框架之前，我们需要回顾并理解几个核心概念。

流形

流形是局部类似于欧几里得空间的拓扑空间。简单来说，一个光滑的曲面（没有尖锐边缘）就是一个流形。例如：

  • 三维欧几里得空间 R³：我们熟悉的3D空间。
  • 二维球面 S²：地球表面的理想化模型。
  • 圆环面 T²：由一个圆绕另一个圆旋转形成的曲面。

在配准问题中，我们的数据点“生活”在这样的流形上。例如，3D点云生活在R³流形上；而如果我们处理的是时钟同步问题，数据点则可能生活在圆形S¹流形上。

内积空间

内积是点积的推广，用于衡量两个向量的“对齐”程度。对于向量空间V中的向量x和y，内积 <x, y> 满足对称性、双线性和正定性。两个向量的内积为0表示它们正交；内积值越大，表示它们的方向越一致。

这个概念将扩展到函数空间。我们将把整个点云表示为一个函数，并通过计算两个函数的内积来衡量它们的对齐程度。

核心思想：从点到函数

RKHS配准的核心是将离散的点云转换为连续空间中的函数。给定一个目标点云X（包含几何位置和标签，如颜色）和一个源点云Z，我们为每个点云构造一个函数：

对于目标点云X的函数：
F_X(y) = Σ_i [L_X(x_i) * k(x_i, y)]

对于源点云Z的函数：
F_Z(y) = Σ_j [L_Z(z_j) * k(z_j, y)]

其中：

  • L_X(x_i) 是点 x_i 的标签（例如，RGB颜色向量）。
  • L_Z(z_j) 是点 z_j 的标签。
  • k(., .) 是一个核函数（例如高斯核），用于衡量两点在几何空间中的相似性。
  • y 是流形上的任意点。

公式解释：函数 F_X 在任意点 y 的值，是所有目标点云中点 x_i 的标签，以该点与 y 的核函数值为权重，进行加权求和的结果。这实际上是用数据点作为基函数，构建了一个覆盖整个流形的连续函数。

配准目标

我们的目标是找到一个李群变换 H（例如，对于3D点云是SE(3)刚体变换），使得将变换应用于源点云函数后，其与目标点云函数尽可能“对齐”。在函数空间中，“对齐”通过内积来度量。

因此，配准问题转化为以下优化问题：
H* = argmax_H <F_X, F_Z ∘ H^{-1}>

其中 <., .> 表示函数空间中的内积。如果变换 H 是等距的（如刚体变换），那么函数范数在变换下不变，最大化内积就等价于最小化两个函数之间的夹角。

关键优势：这个内积计算涉及两个点云中所有点对的双重求和，但核函数 k 通常只在局部非零（如高斯核衰减很快）。因此，实际计算是稀疏的，效率可以接受。更重要的是，这个公式天然包含了软对应关系，不需要预先求解点对点匹配。

框架与求解

现在，我们形式化地描述这个通用框架，并了解如何求解。

通用框架定义

传感器配准问题可以定义为一个五元组 (G, M, A, g, k)：

  1. G (李群)：变换所属的群，如 SE(3)。
  2. M (光滑流形)：数据点所在的流形，如 R³。
  3. A (群作用)：李群在流形上的光滑作用。
  4. g (李代数上的内积)：用于在李代数上定义度量，以计算梯度。
  5. k (核函数)：一个对称正定核函数，如高斯核，用于构建函数。

此外，对于标签信息（如颜色），我们需要一个信息空间三元组 (L, <., .>_L, L)：

  • L：标签空间（如所有RGB向量的集合）。
  • <., .>_L：标签空间上的内积（如RGB向量的点积）。
  • L：从数据点到标签的映射函数（即每个点对应的颜色）。

求解：梯度上升法

我们的目标是最大化目标函数 J(H) = <F_X, F_Z ∘ H^{-1}>。我们可以使用梯度上升法在流形上进行优化。

对于在R³流形上使用SE(3)变换和高斯核的特定情况，目标函数关于变换 H 的梯度可以推导出来。梯度方向指示了使内积增加最快的变换方向。

在流形上进行梯度更新不能简单相加，需要使用指数映射进行积分。更新规则如下：

  1. 计算当前梯度（在李代数中表示为 twist ξ）。
  2. 用步长缩放梯度：Δξ = α * gradient。
  3. 通过指数映射将李代数增量映射到李群：ΔH = exp(Δξ)。
  4. 更新当前变换：H_{k+1} = H_k * ΔH。

通过迭代执行上述步骤，我们可以收敛到一个局部最优解。

应用实例

以下是该框架在不同设置下的应用示例，展示了其通用性。

以下是该框架可以解决的不同问题实例：

  • RGB-D点云配准：流形为R³，变换群为SE(3)。除了3D坐标，每个点还带有RGB颜色信息作为标签。该方法在KITTI等数据集上实现了比传统方法更低的漂移。
  • 时钟同步：流形为圆形S¹，变换群为SO(2)。将计算机记录的时间序列映射到相位，即可通过圆上的配准来同步时钟。
  • 球面图像配准：流形为球面S²，变换群为SO(3)。可用于对齐卫星拍摄的地球图像，其中颜色标签可能代表海拔或其他属性。
  • 二维形状配准：流形为R²，变换群为SE(2)。可以对齐像笑脸这样的二维形状，即使它们的点数不同。

与现有方法的联系

RKHS框架是更通用的方法，许多现有方法可以看作其特例。

如果我们做出三个强假设：

  1. 两个点云点数相同。
  2. 点之间已知一一对应关系。
  3. 只考虑对应点之间的核函数值（即双重求和退化为单重求和）。

那么，最大化高斯核函数的和，在泰勒展开近似下，就等价于最小化一个加权最小二乘问题。这正是许多传统配准方法的核心。

因此，RKHS框架概括并统一了许多现有方法，同时通过软对应和函数空间视角，提供了处理更复杂情况（点数不同、无对应、部分重叠）的能力。

总结

本节课我们一起学习了RKHS点云配准框架。我们首先了解了将离散点云提升为连续函数的核心思想。然后，我们探讨了该框架的通用数学定义，它通过最大化两个函数的内积来求解变换，天然处理了对应关系未知、点数可变和部分重叠的挑战。我们还看到了该框架在多种流形和变换群下的应用实例，并理解了它与传统最小二乘方法的联系。这个框架为融合几何与语义信息、开发下一代鲁棒配准算法提供了强大的基础。

下一讲预告：在下一节课中，我们将学习另一种重要的RGB-D配准方法——直接视觉里程计，它基于最小二乘优化来求解配准问题。

018：RGB-D视觉里程计

在本节课中，我们将学习RGB-D视觉里程计。我们将了解如何使用一种深度相机，它能够提供RGB图像形式的测量值，以及另一幅图像形式的深度图，该深度图告诉你每个像素到相机镜头的距离。然后，我们可以结合这些信息，构建带有颜色信息的点云。我们将探讨如何使用这种相机来重建场景。为此，我们需要随时间追踪相机的位姿。这就是视觉里程计的范畴，它可以使用单目相机、立体相机或RGB-D相机来完成。

我将使用TUM RGB-D数据集中的一个例子进行说明。

首先，这是一个房间，你可以看到一只泰迪熊。数据集中有很多序列，但这个特定序列是围绕泰迪熊移动的。我们的目标是重建这只熊。

如图所示，图像是模糊的，这是一个问题，因为这台特定的相机使用了卷帘快门。当相机移动时，快门也在移动以捕捉场景，这导致了运动模糊。据我们所知，这最好被理解为沿时间通道的卷积。因此，这导致了运动模糊，进而使得将当前图像与下一帧图像配准以找出相机位姿变得困难。

我们确实有全局快门的相机，显然这更受青睐，因为它们在一个瞬间捕捉整个图像。这样，你就不会像这里看到的那样出现那么多运动模糊。这是一个深度图的例子。你需要一个比例尺来知道实际的度量值，但基于图像，你可以看出泰迪熊离相机更近，所以较暗的值离相机更近，较亮的像素则更远。

这完美吗？很难说。由于现场光线等原因，在课堂上可能很难看清。但这远非完美，因为有些像素缺失了某些形状的边界。例如，你可以看到这里有一台笔记本电脑，但其形状与我们在图像中看到的并不完全一致，这是一个问题。

并非所有像素都有精确的深度值，有时我们处理的是缺失的深度值。

这是一个视频，一个GIF图像，播放了相机记录的所有图像。这展示了相机在房间中的移动方式。这是六自由度运动：相机可以滚动、俯仰、偏航，以及XYZ方向的移动。我们可以看到它可以放大、缩小。所有这些运动可以同时进行。因此，你移动得越快，看到的运动模糊就越多，配准也就越困难。连续图像之间的重叠越多，问题就越容易解决，因为我们求解的是较小的运动；移动越快，挑战就越大，有时甚至变得无法追踪，这时我们可能需要依赖互补传感器来估计相机运动。

例如，现代相机带有IMU，你可以将IMU与图像信息融合，以获得视觉惯性里程计。另一个你可能注意到的点是，虽然有人站在那里，但环境是静态的。如果物体在移动，相机也在移动，这将使问题变得更加困难，因为我们不知道物体的运动信息，也不知道相机的运动信息。我们需要其中一个作为参考来估计另一个，如果两者都未知，那就非常具有挑战性。

我们可以在相机移动时追踪相机帧中的物体，但这将是相对的。一个想法是，如果你能检测到图像的动态部分并将其移除，那么也许你可以使用静态部分来解决问题。这是一个显而易见的想法：丢弃动态信息。更具挑战性的是，如何实际利用场景的动态性来改善你的追踪，但这是一个更难解决的问题。

现在，你在这里看到的深度视频是针对相同的图像。然而，我们购买的相机有两个不同的传感器来获取RGB数据和深度数据，这两个传感器不一定完美地共位。它们位于相机上的某个位置，这些图像没有完美地校正，它们不在同一个坐标系中。我们不会深入探讨如何进行这种低层信号处理。

幸运的是，当你购买这类相机时，通常它们会附带驱动程序。驱动程序通常由制造商或像你一样的人开发，他们坐下来构建驱动程序并通常使其开源，这样我们就可以使用它。它可能是一个ROS包、LCM包或其他可以在你的操作系统上运行的软件。

然后，我们得到的是已配准的深度和图像数据。首先，它们是同步的。它们不一定同步。想象一下RGB传感器的频率与深度传感器不同。一旦我们配准它们，我们就可以得到同步的深度和RGB图像。并且，可能通过工厂提供的固有校准，你可以获得点云。如果需要，我们可能需要重新校准相机的内参。

但是，当你第一次处理问题时，你可以使用通常附带校准参数和所有预处理的数据集，使用提供的工具。这是一个专注于算法方面的好方法，之后如果你感兴趣，可以深入研究这些部分。

有了这些，让我们开始看看如何共同构建一个公式化的问题。

我想构建问题的方式是使用非线性最小二乘法。之所以是非线性的，是因为我们会看到它最终是非线性的。我们将尝试提出想法，看看如何构建一个最小二乘问题来通过配准两个连续图像找到这个相机位姿。从某种意义上说，这是帧到帧的RGB-D配准。它类似于点云配准，因为我们可以直接处理点云，事实上，我们可以使用我之前讨论过的方法。我们可以整合颜色和点云。RKHS配准的想法出现在这之后。我们今天讨论的是之前发表的东西。

但是，我们今天推导的目标函数很重要，因为它被称为光度损失函数。下次我们讨论SLAM时，当我在图中展示一条边时，我们会说，例如，这是一个光度因子。然后你就知道该怎么想了。本讲座是一个构建模块，也许是图中用于构建SLAM问题的一条边。

那么，让我们从帧到帧RGB-D配准的问题开始。或者你可以把这个想法看作是RGB-D视觉里程计。现在，视觉里程计不一定只是一帧到另一帧。我们可以处理一系列帧，这通常是它的做法，称为滑动窗口优化。或者我们可以维护整个历史，类似于SLAM。但当它是里程计时，意味着没有全局闭环。没有长时间后回到同一个地方并知道那是同一个地方的全局追踪。我们试图消除漂移。当我们有那个时，那就是SLAM，即同时定位与建图。如果你使用滑动窗口并依赖局部约束来改进你在时间窗口内的估计，那通常被称为里程计。

因此，帧到帧配准是构建模块。它是解决里程计问题的最简单方法。如果学习它是必要的，那么你可以在此基础上通过同时解决许多这样的帧到帧问题来构建，在一个时间窗口上，并且窗口随时间滑动，对吧？你添加新帧，忽略旧帧。这就是想法。

我给你看一些这种相机的例子。你们很多人可能见过微软Kinect，也许你玩过Xbox。那是我们最早用于RGB-D SLAM的设备。它不是为机器人技术而建的，但聪明的人可以为Linux制作驱动程序，然后我们可以在Linux中运行它。这就是RGB-D感知研究的开始。

因此，拥有硬件、拥有传感器以便我们可以收集数据然后解决问题的重要性是巨大的，因为它突然让人们可以将许多我们可能知道的想法变为现实。所以，它是由拥有传感器实现的。也许最近的一个例子是Intel的RGB-D相机，它内部带有IMU。你可以在很多机器人上看到它。他们使用这种相机是因为它体积小，相对便宜，并且工作良好，尽管它适用于室内。如果你在户外使用它，你可以解决短距离5到10米的问题，但点云会变得嘈杂。我们将看到亮度恒定假设会被违反。我们会讨论这一点。

这一页充满了动机。之前有人要求在讲数学之前先讲动机。我专门用一页来讲动机。我希望这能满足你。所以，相机。我们的算法在某种程度上与你购买什么品牌的传感器无关。我们将假设数据已经过处理，我们有同步的RGB和深度图像，加上我们知道相机的校准、内参校准，这样我们就可以使用相机的投影模型将3D点投影到图像上，因为如果没有校准，投影将不准确。那么，模型就无法像我们希望的那样捕捉我们在现实中看到的情况。

相机提供RGB和深度图像。从哪里开始？你认为我们应该从哪里开始构建问题？第一天，你开始工作，你的主管给你一台相机，并要求你看看能用它做什么。我们想看看这条工作线对机器人是否有前景。你知道这门课程中的非线性最小二乘法。你会怎么做？

我不想完全为你写出来。我们希望基于我们所学的知识，使其成为一个案例研究。你是在尝试定义模型吗？我们需要一个模型。我们需要一个约束。我们需要某种约束，将我们感兴趣的位姿与我们能够最小化某种距离、某种残差成本的地方联系起来。这是关键。一旦我们有了这个，我们就可以通过取这些残差的平方和来定义最小二乘问题。

所以，我们需要一个模型。然后我们需要为优化定义一个残差、一个成本函数。一个好的起点是列出我们拥有的东西。目标是什么？随时间追踪相机位姿。目标是追踪相机位姿。随时间，我的意思是，我们处理的是连续图像。我们将专注于最简单的情况，因为如果你不能解决这个问题，那就真的没有必要让它变得更复杂。

所以，我们将从相机收集一些数据。也许如果我添加图像，我们可以看到我们能做什么。它们大致相同大小。好的，所以我们拥有的，我们将专注于时间步T1和T2。在时间步1，我有一个图像。并且可以理解它也有深度图像。我们知道我们有那个。在时间步2，我有另一个图像。你可以称这个为图像一，也许这个是图像二。现在，我想知道的是，相机如何在3D中移动，以便它在时间步T2观察到第二幅图像。

现在，根据我们以前的知识，我们知道这个六自由度运动可以有旋转和平移，它必然是一个李群元素，那就是SE(3)。所以我们立即认识到我们想要估计的参数不是一个典型的向量。它是一个矩阵。它是一个矩阵李群的元素。我们谈论的是相机，也许。对，它从时间步T1移动到时间步T2。

现在，根据传感器的文档，我假设你读过那些，就像你读过我在作业中给你的IMU文档的每一百页一样。我和几个学生谈过，他们知道那份文档的每一个字，所以我信任你也读过这个。所以我们有一个相机投影模型。对于这台特定的Intel相机，你可以在GitHub上找到它。有一个仓库。它也与ROS兼容。所以如果你买了相机，你可以直接运行ROS包，就可以在RViz中流式传输数据。这非常简单。看到数据后，剩下的就是我们的工作了。

相机投影模型是提供的。所以假设有一个点。现在，如果你在本科计算机视觉课程中学过这个，或者研究生院的计算机视觉课程可能涵盖了这个。这是一个几何模型，当你看到一个3D点时，它告诉你一个投影到图像平面上的模型。如果你以前没见过，那也没问题，你可以想象这是这个传感器的给定模型。

所以模型告诉我们，如果我有一个点，比如P(X, Y, Z)。这是3D空间中的一个点。我可以将这个点投影到像素(u, v)上。所以我的投影函数π将取这个点。u将是这样的：相机沿X方向的焦距f_x乘以点沿X方向的坐标值除以Z，再加上图像中心的某个偏移c_x。我会告诉你为什么我们需要c_x和c_y。现在，相机沿Y方向的焦距f_y可能不同。像素不一定是正方形。也许像素是矩形。这只是你需要从制造商那里获得的信息。但是当你校准相机时，通常你可以求解f_x和f_y的不同值。所以，v = f_y * y / Z + c_y。现在你可以以一种解耦的方式写这个，通常有一个标定矩阵K。然后你有你的3D点除以Z。所以这是。我相信你在作业3中见过这个。当你有两个相机时，有一个物体。你想估计物体的位置。所以这就是模型。这个模型是线性的还是非线性的？非线性。为什么？因为我们除以Z。这使它成为非线性。

所以这是我们的模型。这是一个2x1向量。为了清楚起见，我们有。在计算机视觉中，通常使用这样的坐标系：当你有一个轴沿着图像的高度，从图像的左上角开始，正方向指向下方和右方。通常这是u，这是v。如果这是图像的中心，那么我们将需要这些c值来将其与我们选择的原点（图像的角落，而不是图像的中心）关联起来。所以有c_x和c_y。

所以我们有了模型，下一步是什么？寻找想法。当你在思考时，我要做另一个简化。RGB图像有三个通道：红、绿、蓝。我们可以尝试将每种颜色用作单独的测量值。或者，在这个上下文中，文献中通常的做法是将图像转换为灰度图。然后将所有三个通道合并为一个通道，然后我们处理强度值。这使得问题稍微简单一些。但是当你使用深度学习时，你可以直接将所有三个通道作为输入，不需要合并它们。

所以，遵循我们在文献中读到的内容，图像被转换为灰度图像。现在，将彩色图像转换为RGB的最简单方法只是将它们相加并除以3。但存在更好的模型，它们对RGB使用不同的权重。但是如果你使用OpenCV或任何其他软件或库，你可能有一个函数为你做这件事。

好的，所以现在我们有了强度。我再次使用I表示强度，但也表示图像。从图像1到图像2。这里的约束是什么？我需要定义我的约束，以便我可以测量误差。如果你不能测量误差，你就不能最小化它。如果我们不能最小化误差，就没有目标函数来估计你感兴趣的变量。所以我们可以处理一些我们可以测量的东西。

可以是任何类型的变换吗？相机的变换。所以如果我们在图像上追踪一个点，那么我们可以追踪误差。从一个图像投影到另一个图像，这个差异然后可以转化为相机的运动。这有道理。所以你是说，首先，我们应该将这些强度值视为测量值。我们有很多测量值。这就是为什么像光或相机这样的感知传感器是高带宽的。我们获得的数据量远高于像IMU这样的东西，IMU只是一个信号，每个维度随时间变化的1D信号。

例如，这可以是一个320x240的图像。这是一个合理的工作大小，但这相当小。我们可能可以从VGA相机获得高清图像。但可能像素太多，处理时间更长。所以这已经超过76,800个像素。对于一个时间步来说，这是很多测量值。

所以你指出的约束是，如果相机移动，环境是静态的，我可以假设我看到的东西应该与我之前观察到的相似，只是视角改变了，如果这个位姿能够捕捉到这一点，我应该再次看到相同的像素。确实，这就是我们要假设的。这个假设被称为亮度恒定。

当你建模时，列出你的假设很重要，如果你的假设不明确，就会让别人想知道你的模型的范围，何时有效，何时无效。当假设被违反时，模型无效。这就是你可以解释为什么某些东西在实践中效果不佳的方式。你回去看看你的假设。希望你的假设是合理的，并反映了你的模型。然后看看它们是否被违反。如果被违反，你可以解释。可能这就是为什么你的性能不是很好。

所以这个假设被称为亮度恒定。它说的是，如果你移动一点点，不是很多。在小运动下，对应像素的强度值是相同的。因此，这给了我们一种在下一帧中重新观察这个像素的方法。因此，现在我们可以讨论它们的差异。如果我最小化重新观察到的像素之间的差异，那么描述那个最小化的位姿就是我的解。所以这就是想法。

现在，这个假设很容易被违反，对吧？如果相机移动很多，这个假设很容易被违反。另一个违反假设的情况是什么？有一个非常明显的情况，当你使用这种方法时应该非常小心，并查看是否存在那种环境条件。光照变化。对。这就是为什么在户外使用这样的方法如此困难。也许是一个晴天，你移动的方式可能会在图像中看到眩光，这可能会破坏一切。当阴天均匀时，天空光线均匀，那么SLAM方法就会大放异彩。当天空晴朗时，SLAM方法就“天气不佳”了。所以，当你看到一个数据集是阴天时，你就知道了。他们知道。你会获得最佳论文奖。它甚至在户外也有效。

所以这有点令人沮丧，因为它有时可能有效，有时可能无效，如果假设被违反。我们喜欢用于机器人感知的方法，它们能在可能变化的一般条件下工作。所以，这是我们在机器人感知中的一个基本限制。我们正在努力使算法更好，以便在不同的情况下工作，比如那种情况。

好的，假设这是真的。顺便问一下，当我知道像素坐标时，如何读取像素值？想象我有这个像素。它的强度是什么？所以我有我的3D点。有一个3D点。我使用相机投影模型，它会给我一个像素。我称这个为点i，u_i, v_i。我问你，强度是什么？或者也许你称它为I(u_i, v_i)。它是一个标量，是的，当然是。它只是一个实数，并且是一个正数，因为颜色值是正的。它是一个实数。或者有时人们这样表示它。

我如何访问这个强度值？你把它保存在内存中，这样中心与u在同一个坐标系中吗？是的，我把它保存在内存中，以任何你想要的方式保存。我想我不理解。所以，这很琐碎，你甚至没有考虑它。所以你从内存中读取它，对吧？所以这就是你做的。所以没有函数，没有关系。它存储在内存中，你根据索引值从内存中读取它。对于这个方法来说，这没问题。

在上一讲中，RKHS正是解决了这个问题，即没有模型描述这些强度值与3D点之间的关系。你看到的核密度估计是标签的加权和。它定义了一个函数。它是一个分析模型，描述了3D点P和这个强度、你的颜色值之间的关系。所以在这个上下文中，那个模型不存在。你可以回去的东西。

现在比较这两者，在这种情况下，我们只是从内存中读取它。这很好。这就是我们做的。顺便说一下，我们受到图像分辨率和数据的限制。它不是连续的。它是离散的，尽管我们有很多像素。在实践中效果很好。

所以我现在需要定义残差。看看我是否有。看看我是否在图像1中观察到一个点。记住，我们有深度值。对于RGB-D相机，我们有3D点。如果我在图像1中观察到一个点。让我先写下来。所以我在图像1中看到一个3D点。当然，对于每个点，你可以读取像素值，因为对于RGB-D相机，存在一一对应关系。我们在3D点和图像像素之间存在一一对应关系。所以无论你知道(u, v)还是3D点P，你都可以从内存中读取强度值。

所以我在图像1中看到这个点，然后我在图像2中再次看到这个点。连接这两个点的关系是，如果我使用这个刚体变换变换图像2中的点。然后，如果我把它投影到图像2上，我应该得到一个与我在图像1中相似的强度。严格来说，我认为我们应该这样写。但是再次，这部分是常数，因为它不依赖于变换。你只知道它，它是一个值，一个标量值。在帧2中看到点。变换它。抱歉，在帧1中看到点，将其变换到帧2，投影到图像2上。读取图像2上的像素值、强度值，从我们在图像1中变换的点的对应强度值中减去它。这就是残差。

我们可以用另一种方式做吗？在帧2中看到点，将其变换到帧1，并投影到图像1上？是的，我们可以。真的没有区别。你需要选择一个。至少在帧到帧的情况下，真的没有区别。你估计T或T的逆。无论如何，它是一个刚体变换，优化器不在乎你试图求解哪一个。也许如果你在做滑动窗口或SLAM，你可能关心顺序。但无论如何，它将类似于这种情况。

所以，刚体变换对3D点的作用，因为我没有将点设为齐次坐标。我没有在X, Y, Z后面附加1。所以我使用这个点值来定义T对P的作用，即旋转然后平移。我的参数，优化的决策变量是SE(3)的一个元素。

那么我有多少个这样的残差？76,800。假设这是图像大小。你可能比我更擅长算术。乘一下看看你得到什么。超过76,800，对吧？所以残差向量非常长。如果你想使用所有图像，它长得令人难以置信。如果是高清图像呢？什么是1080乘以700左右？新电视是8K。

所以计算机视觉中有一个想法叫做图像金字塔。所以，历史上，当他们有高分辨率图像时。这个问题不是凸的。有几个原因，首先，刚体变换有约束，因为R^T R必须是单位矩阵。这使得问题成为有约束的。现在，当我们在李群上做时，我们使用指数映射的技巧来积分。我们忽略约束。但这给了我们一个局部解。强度值，如果你可视化，残差目标函数的景观不是凸的。它不会是一个你可以找到最小值的漂亮的二次函数。不，将有许多局部最小值。然后图像越大，分辨率越高。当它更高时，你会得到更多这些局部最小值。有时这甚至使问题更难解决。

高分辨率有时使问题更难解决。所以图像金字塔的想法是从非常粗糙的分辨率开始，当图像粗糙时，分辨率非常低。你大致知道你想去哪里，因为所有那些微小的最小值都会消失。你将专注于图像和成本函数中最深的最小值。所以在粗糙级别进行配准，当然，你需要决定你想要执行多少层。使用该解决方案来初始化下一个更精细的步骤。现在，你处于一个最小值附近，希望比你在更高分辨率图像中看到的许多糟糕的局部最小值更好。然后再解决它。移动到下一步，下一步。所以，你重复这个，解决同样的问题也许10次，5次。使用图像金字塔的想法。

所以同样的问题，只是改变了图像分辨率。显然，如果你必须解决10次而不是一次，这将增加负载。这是一个需要考虑的因素。但它将显著提高你的性能，因为粗略版本大致会进行粗略配准，将引导问题走向更深的最小值。然后高分辨率部分将尝试在该最小值周围最小化误差。

这是一个非常成熟的想法，你可以在计算机视觉教科书中找到它。再次将其与上一讲联系起来，RKHS配准。不需要多次解决问题，因为连续性，高斯核的带宽，你看到的那个长度尺度。当你改变那个核时，你记得在环面上当L很大时，我们有一个凸问题，当我们减小L时，我们得到了很多局部最小值，对吧？所以图像金字塔的想法与那个带宽有关，从某种意义上说，使用带宽，你可以在一次优化中连续控制沿着那个金字塔移动。在任何时候，你都可以沿着那个金字塔移动，无论你想去哪里。只是很难知道如何移动。但你能够在一个优化中做到这一点。所以这是该方法的另一个强大特性。

所以这是图像金字塔，知道这一点很重要。有时人们使用它，只是在论文中写一行，你读到时就知道它的意思。

在计算方面，或者只是优化输入分辨率到你的系统的问题？我想，更多的是硬件设计问题还是软件设计？图像分辨率。是的，就我们正在做的设计而言。但是，就像降低分辨率。不，图像，硬件。问题是，这是一个硬件问题还是算法问题？硬件可以给你非常高分辨率的图像，我们只是算法上，如果我们尝试使用全分辨率图像来解决这个问题，你可能会认为你会得到更好的结果。因为软件是局部的，你的结果可能更差。因为高斯牛顿法会根据你开始的位置找到局部最小值。那是否是一个好的答案，取决于你从哪里开始。当你寻找粗糙版本时，嗯，它最初会使其更快，对吧？然后很多那些细微的最小值会消失。就好像你有一个表面有很多起伏，对吧？如果你去掉信号的高频部分，你就剩下一个漂亮的表面，对吧？所以你首先去损失函数深的地方，然后你回到高分辨率表面进行精细配准。所以这是一个算法问题。如果你有一个全局求解器，你可以使用那个，但通常全局求解器无论如何都更慢，如果你使用高分辨率图像，可能无法实时。所以这是一个持续的挑战。

那么，我如何调和这一点，因为我们使用一一对应来构建残差？对，好问题。有一个问题，上次我们有一个双重求和，它是无关联的。我们怎么知道从I2中减去哪个I1，对吧？所以，答案是。当，从图像一开始，你有所有这些点，对吧？你从第一个开始。再次，你知道3D点，因为它是RGB-D相机。所以你读取强度值，这给了你I1的这一项，对吧？现在，当你变换它并投影它时，你得到图像2上的坐标。你可能需要做舍入，因为索引是离散的，对吧？基于投影值，找到最接近的索引。或者有时人们取一个邻域。我们说，这使它变得更复杂，因为现在每个残差都变成了基于你选择多少个邻居的残差。但无论如何，如果你选择一个坐标，即这里的投影，我们舍入它并从I2读取该像素值。所以这也是另一个想法，在文献中被称为投影数据关联。所以你只需要投影，然后通过读取像素坐标来解决关联问题。

我们将使用RGB还是强度值来做什么？我们不知道变换。在优化的任何时间，在任何迭代中，给定当前的猜测，我们必须做这个关联。所以它们不一定正确。但希望每次我们都更接近正确的关联。对，这也是一个问题，很好的观点。无论猜测是什么，这个差异不一定相同，对吧？即使存在完美匹配。在图像一和二中。对。好的。这证明了什么？像素的几何一致性。听起来你只是没有，最接近的强度。在相应的图像中。问题是，是否有任何几何约束，因为我们只使用？被称为光度损失函数的东西。我们构建的东西被称为光度损失函数。所以我们不在3D中执行配准，这就是我们所有的。在这个公式中。再次，在上一讲中，它在3D中。有一个可以更好地捕捉碰撞的几何关系。对于度量，显然是在图像中，它受到遮挡的影响，是在2D中。尽管如此，它是一个非常强大的公式。它已经给了我们很多。

所以我们准备好构建第一个问题。最优变换是使这些残差的平方和最小化的变换。这是问题的最小二乘公式。为什么是最小二乘？因为那是我们知道的最简单和顺序求解器。它将给我们一个解决方案。我们完成了吗？为什么是。几次，几次，这不是P。那是一个大小预先向量。好像我们在对P应用I。应该是。这就是为什么在Z中，不是UV。像信息是什么，实际上T是什么？那不是X, Y, Z吗？它是。它是π是X, Y, Z。哦，我们为每个像素找到它。对，对于每个像素，我们有P，对吧？所以这里我们有。对，这是一次性的，如果你通过代码或实现它。你，没有其他方法可以做到这一点。所以那是当你读取数据时。你看到，嗯，有u_i和v_i，对吧？但也对应P_i，因为那个投影模型的逆将给你3D点。然而，要得到那个，你需要深度值Z，RGB-D相机会给你Z。所以因为你知道Z，你以度量尺度恢复X和Y。那么你就有X_i, Y_i, Z_i。所以这是给定的，对吧？然后我们读取像素。所以基本上，当你存储在内存中时，你有u_i, v_i。那是一个像素值。然后你有那个像素的X, Y, Z，然后你有强度。取决于你想要哪一个，你只需从你的数据结构中读取它。

所以我们有了问题的第一个公式。在这一点上，我期望你更多，因为我告诉你比这更好。问题是什么？再说一遍。我们需要约束吗？不，我们有损失函数。我们能做得更好吗？或者这就是我们能做的全部。我的意思是，我想一个问题是你有点忽略了对应关系。可能只是图像的一小部分。即使很大，也可能只有一小部分重叠，所以这是一个很好的观点。无论如何，当我们移动时，将有一个非重叠部分，无论多小，在两幅图像之间。那很容易是图像的一半。所以当我们读取这个强度并评估损失函数时，它们不应该完全匹配。事实上，如果变换不在解附近，它们可能非常大。我们处理的是二次损失函数。当某些像素的残差非常大时会发生什么？它们不匹配。它们基本上不对应于彼此。我看到你摇头，但那不能解决问题。我们应该做什么？所以你承认这是一个问题。对于某些像素，这将是一个非常大的残差。你几乎就在那里。是的，M估计损失。那么，我们把它变成一个鲁棒的最小二乘怎么样？总是更好。总是，总是给你比没有它更好的东西，对吧？99.99%的时间。所以当你有它时，让它变得鲁棒。通常，我们不会失去任何东西，但你需要调整那个超参数。你可以在一些样本帧上做，不是很难。

所以，让我们做M估计。我们将创建一个M估计器，又名鲁棒。当你在论文中添加时。增加你被接受的机会，50%。当你添加M估计时，那是一小部分。相信我，它有效。所以我们有很多选择，但我们了解了卡方损失函数。假设那是我们使用的例子。

所以新问题将是。好的。也应该在这里读取。我们正在求解，在T上优化，并且T属于这个集合或群SE(3)。所以那里。鲁棒核。这是鲁棒核或范数。例如。这是一个柯西损失函数。如何解决这个问题。前一个我们可以使用高斯-牛顿或Levenberg-Marquardt来解决。我们已经学过了。当它在李群上时，会有什么不同？嗯，我们需要推导雅可比矩阵。你见过一些例子，我们可以做到。然后积分规则将不同。我们必须使用指数映射来积分变换。所以我们在优化讲座中涵盖了这一点。但这不是，这不再是一个最小二乘问题，因为现在它被包裹在另一个损失函数中。这不是最小二乘问题。我们如何解决这个问题？当你思考的时候，聊天中有一个评论，我们可以在两幅图像中识别地标。Matt在说那个，所以我要把这个评论作为一个机会来解释直接方法和间接方法之间的区别。

我们没有从图像中提取地标或特征。你可以提取ORB特征、SIFT特征、SURF特征。使用深度学习提取特征，那将是一种间接方法。间接方法将图像抽象为一组特征。测量集，而不是超过76,800个，将大大减少。也许你有1000个图像，100个特征。但特征的质量会更高。但你必须丢弃大量信息来处理那些高质量的特征。

直接方法消耗原始数据。我们直接使用像素。所以我们在某种意义上使用了许多低质量的特征。这些是像素值。嗯，我们使用了很多。这里所有的。这被称为直接方法。我们直接处理相机图像。所以有一个错误。有很多争论。哪一个好。间接方法在变换很大时更好。间接方法也非常擅长解决非刚性配准问题。但特别是在闭环中，当我们检测到同一个地方时，我们从完全不同的角度看到同一个地方。对于直接方法来说，解决配准问题具有挑战性，因为初始变换非常大。而间接方法可以解决这个问题，该系统的一个例子是ORB-SLAM，它使用BRIEF二进制词来提取特征，检测闭环，然后使用特征和RANSAC来得出非常远帧的粗略配准。然后在后端，图优化将解决SLAM问题。直接方法不擅长解决那个问题。但是当你处理顺序的小运动时，这是时间追踪的情况，它们往往非常强大。它们非常鲁棒。

间接方法，如果环境缺乏特征。嗯，算法注定要失败。想象它是均匀的。全是草，全是雪，就像行星数据集，NASA的直升机在火星上空飞行。互联网上有一个视频，对吧？地形非常均匀，你根本没有特征。或者特征非常不可靠。你可能检测到，可能检测不到。现在，直接方法在这种情况下工作良好，当你移动时，你使用所有像素来追踪相机图像。你也可以组合它们。有混合方法。比如SVO来组合它们。

我们讨论的这个特定方法被称为DVO，直接视觉里程计或密集视觉里程计。所以直接方法也可以是密集的或半密集的。我们在这里讨论的是密集直接方法。我们使用整个图像。你也可以子采样，也许提取边缘或图像中一些更好的高梯度像素，例如。然后你可以使用半直接方法。半直接方法的一个例子是直接稀疏里程计，DSO。但那是滑动窗口优化的一个例子，不是帧到帧的。所以这就是直接、间接、特征、像素、密集和半密集的故事。

那么如何解决这个。继续谈。你刷新一下记忆。我们需要一个答案，我们不会继续。在哪里。对，这是个好主意，它叫什么名字，我们有一个算法？IRLS，是的。一起，我们弄明白了。所以将其转换为加权最小二乘，其中权重将取决于残差。每次更新它们。我们有一个算法。我们称之为迭代重加权最小二乘，IRLS。我不在这里重复，因为你会感到无聊，但你知道在哪里找。如果你想使用IRLS解决这个问题。所以我们可以。是的。解决这个问题。使用IRLS。好的，这个。现在，我确实给你一些笔记，我有之前推导雅可比矩阵的笔记。为了你的心理健康，我不在课堂上推导它。但这真的只是机械的程序，链式法则。得到投影模型的导数。然后你有刚体对点的作用，推导雅可比矩阵。我们在优化讲座中做过。所以雅可比矩阵将是多个步骤的乘积，对吧？所以，就到此为止。如果你有一个给你这个求解器的库，那将是美好的。你可以学习如何构建你的问题，并将其交给那个求解器来为你解决。这通常是节省时间和加速进度的好方法。事实上，你可以这样做。你可以使用一个叫做Ceres Solver的东西。这是一个非线性最小二乘问题的求解器。它确实提供鲁棒核，它也有其他求解器，如共轭梯度、梯度下降等。

所以没问题。当我们构建这个问题时。鉴于我们有很多点，有人计算一下240乘以320。所以我们有一个数字。多少？我们需要解决的法方程的大小是多少，这个问题是大规模的吗？还是不是。它是。问题的大小是多少？我们需要解决的线性系统的大小。假设我们验证了。我们将问题转换为加权最小二乘，对吧？它将是RT乘以某个W乘以R。现在，当然，我们在T上求解。再次，这个W取决于残差，并且是IRLS，但对于一步来说，它是一个加权最小二乘。现在，如果你计算这个。这可能是一个矩阵。假设我们计算一个残差项的雅可比矩阵，仅针对两个像素。那将是。这个的维度是多少？损失函数的映射是什么？映射是从刚体变换T。我们将求解的参数，增量将是twist，对吧？我们使用指数映射积分。Twist有六个维度。而损失函数，残差，在这种情况下，实际上是标量。在最小二乘中它可能是一个向量，但这里只是强度。所以雅可比矩阵是1x6，对吧？好的。所以它是从变量到残差的映射。无论残差的空间是什么，因为你的残差是twist的函数，对吧？当你扰动时，它将基于某个雅可比矩阵线性化。加上某个常数，对吧？类似这样的东西。6x1。1x6。如果你线性化它，你将从这个泰勒展开或其他推导雅可比矩阵的技术中得到这个模型。如果是这种情况。这本质上是这个权重的平方根。乘以残差相对于twist的偏导数，所以这是一个1x6矩阵或行向量。如果我把它们全部堆叠起来，那将给出问题的整个雅可比矩阵。那是。嗯。乘以六。并且是。240乘以320。那是一个非常非常高的雅可比矩阵，但幸运的是，列的数量实际上并不大。所以我们求解的变量只有6个参数，使它成为六维向量。所以如果你构建法方程。它将是AT A。你可能需要一些权重的平方根在这里为残差，但。但无论如何，这是。现在，它是什么？它仍然是大规模的吗？所以它只是一个6x6的方程组，实际上很容易解决。我们甚至不需要存储那个长矩阵。我们可以处理每个像素，然后求和这个A_i^T乘以A_i。所以6x1乘以1x6得到6x6。然后不断添加它们。这就是它如何变得可逆，否则。对于单个像素，它显然是低秩的，因为它是6x1乘以1x6。所以当我们对所有像素求和时，如果你有足够的像素，它将使其可逆。这就是你知道你有足够测量的方式。然后我们基本上可以定义这个为。嗯。成本。所以，我们只解决6x6的线性方程组。所以。这非常快。这部分。根本不需要很长时间。这是好消息。这就是你如何用低分辨率图像实现实时。所以，这就是我们如何总结IRLS求解，因为现在你知道每次内部发生了什么，你更新权重，解决一个6x6方程组，更新变换。再做一次，直到达到最大迭代次数或收敛。

现在，在我结束之前，我还有最后一个问题给你。嗯。那是当我们有其他传感器时。如果你想组合其他信息来源，比如IMU，或者你可能有来自另一个传感器的位姿追踪。也许你得到。我不知道，轮式里程计或。也许你对你的位姿有先验。你想把它添加到这个问题中，你能做什么？传感器融合，不仅仅使用RGB-D相机。如果你有其他信息来源，你能做什么？我们可以使用EKF。可能，但那非常痛苦。我们必须。我们需要在这个状态中追踪两个位姿。我们必须正确推导所有滤波器方程。它将变得可能，但将变得更加复杂。但滤波的问题，我们下次想讨论它，是一旦你线性化，你永远无法回到过去纠正它。我们现在追求的是，既然我们处理更多的传感器和更多的信息来源，我们追求更模块化、更灵活的东西。我们可以回到过去纠正过去的错误和线性化。先前的信念。得到整合。随着我们获得新的测量值，我们可以添加更多的约束。通过约束，你指的是约束问题还是额外的目标，使其成为多目标。我认为没有。是的，所以。这是一个很好的观察。我们使用的传感器是我们的RGB-D相机，结果，我们推导了一个残差、一个测量模型和一个误差定义，使我们能够解决一个优化问题。在SLAM的上下文中，我们将称这个为一个因子。对。事实上，图。这是一个因子。所以一个因子将是图中连接两个节点的一条边，那条边代表这个残差。所以我们讨论的一切都可以是图中的一条边。

所以，下次我们。想回答这个问题。使用因子图。或图SLAM。好的。然后我们将讨论不同的节点和不同的边，对吧？这可能是你的相机。相机随着时间向前移动而变大。这不是我们在这里做的。是同一个相机。这叫做狭义相对论。你移动得快的物体。长度可能不同。如果它们移动并向你靠近或远离你，它们会变长。当你看到物体时，你会知道。所以然后你有，嗯，在那个上下文中，你有，比如说，第一个变换，第二个变换和第三个第一个，也许它是。也许你不知道。也许它是单位矩阵，对吧？也许某个先验。或者也许你知道，那是无论你有什么知识。你可以考虑那个。这将是，例如，我们的光度因子。它是什么？就是我们刚刚讨论的同一个问题。所以它是这个图的一个构建模块。现在，我们将同时解决许多这样的问题。所以我们将组装一个巨大的非线性最小二乘问题，然后同时解决所有变量。

所以这将使我们能够使其模块化。现在，如果你有另一个传感器，你可以轻松地添加更多的边。也许这是你的IMU。好的。这很好。这是可扩展的。通过不断添加目标和推导方程来操作可能太难

019：SLAM I 🗺️

在本节课中，我们将要学习同步定位与建图的核心概念。我们将回顾课程中已构建的知识体系，并探讨如何将它们整合到一个统一的框架中，以同时解决定位和建图问题。

概述

同步定位与建图旨在让机器人在未知环境中，一边构建环境地图，一边确定自身在地图中的位置。这是一个“鸡生蛋，蛋生鸡”的问题：为了定位，我们需要地图；为了建图，我们需要知道机器人的位置。在本讲中，我们将聚焦于基于图优化的SLAM方法，这是当前的主流范式。

SLAM的三种主要范式

上一节我们介绍了SLAM的基本目标，本节中我们来看看历史上解决SLAM问题的三种主要范式。

  1. 基于滤波的方法：例如卡尔曼滤波及其变种（扩展卡尔曼滤波、稀疏扩展信息滤波）。这类方法在每次预测时都会边缘化过去的信息，只估计最新的机器人位姿。其核心问题在于需要处理数据关联，并且一旦关联错误，由于无法回溯修正，容易导致系统发散。
    • 核心公式：在卡尔曼滤波中，状态估计通过预测和更新两个步骤进行：x_k = F_k * x_{k-1} + B_k * u_k（预测），x_k = x_k + K_k * (z_k - H_k * x_k)（更新）。

  1. 基于粒子滤波的方法：例如FastSLAM。每个粒子代表一个机器人位姿假设，并维护一个独立的地图。这种方法在2D室内环境中效果良好（如Gmapping算法），但面临可扩展性问题。为每个粒子维护一个世界模型会迅速变得难以处理。

  2. 基于平滑的方法（图优化SLAM）：这是当前的主流方法。与滤波方法边缘化历史信息不同，平滑方法保留完整的历史数据。虽然处理的数据量更大，但SLAM问题具有特殊的稀疏结构，保留历史反而使得大规模问题的求解更加高效。这种方法通常使用概率图模型进行表述。

图优化SLAM的核心思想

上一节我们了解了SLAM方法的发展脉络，本节中我们深入探讨当前主流的图优化方法。

图优化SLAM将问题建模为一个图。图中的节点代表需要估计的变量（如机器人位姿），边代表变量之间的约束（来自传感器的观测）。

以下是构建这种图的关键要素：

  • 位姿节点：代表机器人在离散时间步的位姿，例如 x_t，可能属于SE(2)或SE(3)群。
  • 时序约束边：来自里程计（轮式编码器、IMU）、视觉里程计或点云配准（如ICP）的相对位姿测量。它们连接连续的位姿节点，公式为：z_{ij} = x_i^{-1} * x_j。然而，仅靠时序约束会因误差累积导致漂移。
  • 闭环约束边：当机器人重新观测到之前访问过的区域时，建立的非时序约束。例如，通过点云配准或视觉特征匹配，建立位姿x_i和x_j之间的相对关系。闭环约束提供了纠正累积漂移的关键信息。
  • 先验因子：为了给整个优化问题提供一个绝对的参考系，通常需要为第一个（或前几个）位姿节点添加一个先验约束（例如，固定在原点）。否则，整个图可以发生任意的旋转和平移。

一个包含节点和边的图构建完成后，SLAM问题就转化为一个优化问题：寻找一组最优的节点变量值，使得所有约束边上的误差最小。

因子图：一种强大的建模工具

上一节我们介绍了图的基本构成，本节中我们来看看一种更具体的图模型——因子图。

因子图是一种二分图，明确区分了变量节点（机器人位姿、路标点）和因子节点（各种测量约束）。这种表示方法非常清晰，便于理解不同传感器信息如何融合。

以下是一个因子图的示例组成部分：

  • 一元因子：仅连接一个变量节点。例如，GPS提供的绝对位置观测。
  • 二元因子：连接两个变量节点。这是最常见的类型，包括里程计约束、闭环约束以及机器人观测到路标点产生的约束。
  • 多元因子：连接三个或更多变量节点。例如，需要多视角观测才能初始化的三角化因子，或某些延迟融合的传感器模型。

因子图的强大之处在于其灵活性和通用性。只要能为传感器数据定义出连接状态变量的残差模型，就可以将其作为因子加入图中，从而实现多源异构传感器的融合。

前端与后端

在图优化SLAM的工程实现中，通常分为前端和后端两个模块。

  • 前端：负责从原始传感器数据（点云、图像、IMU等）中提取并构造因子图。这包括特征提取、数据关联、位姿估计、闭环检测等。前端是研究的热点，决定了系统的精度和鲁棒性。
  • 后端：接收前端构造的因子图，并执行优化计算，求解出所有变量的最优估计。在假设噪声为高斯分布的前提下，后端优化通常归结为求解一个非线性最小二乘问题。

两者之间的交互是一个开放的设计问题。在简单流程中，前端单向为后端提供图；更复杂的系统可能允许后端将优化结果反馈给前端，以改进数据关联或重定位。

图优化SLAM的优势：稀疏性与效率

你可能会问，保留所有历史数据的图优化方法，难道不会比只估计当前状态的滤波方法更慢吗？答案恰恰相反，这正是图优化方法的关键优势。

当我们将因子图转化为最小二乘问题时，其雅可比矩阵和最终的正规方程矩阵是高度稀疏的。这种稀疏性源于图的拓扑结构：一个因子只连接少数几个变量。

然而，如果采用滤波方法并边缘化历史状态，会导致所有变量通过当前状态相互关联，使得信息矩阵变得稠密。求解稠密大规模线性系统的计算复杂度是立方级的，而利用稀疏性求解则高效得多。

因此，保留历史以维持稀疏性，是图优化SLAM能够实时处理大规模问题的核心原因。这一发现是SLAM发展史上的一个里程碑。

实践挑战与解决方案

在实际应用中，纯粹的图优化SLAM仍面临挑战，并催生了一系列解决方案：

  • 关键帧机制：不是将所有帧都作为节点加入图中，而是只选择具有代表性的关键帧。这能显著减少优化问题的规模，同时保持足够的约束信息。
  • 增量式求解：为了避免每次新增数据都进行全局批量优化，可以使用增量平滑与建图算法。例如，iSAM算法通过更新矩阵分解（如Givens旋转），以近似常数时间处理新的观测，实现在线优化。
  • 异常值处理：错误的闭环约束是致命问题。除了使用鲁棒核函数降低异常值影响，还需要研究更高级的异常检测与剔除方法。

总结

本节课中我们一起学习了同步定位与建图的基础知识。我们回顾了SLAM的三种主要范式，并深入探讨了当前主流的图优化SLAM方法。我们了解到如何将SLAM问题建模为包含位姿节点和各种约束边的图，特别是因子图模型。这种方法的核心优势在于其灵活性（便于传感器融合）和高效性（利用问题的稀疏结构）。我们还简要讨论了前端、后端的分工，以及关键帧、增量优化等应对实际挑战的策略。下一讲，我们将具体推导如何从因子图得到最小二乘问题，并深入其求解细节。

020：SLAM II

在本节课中，我们将要学习图优化SLAM。我们将理解动态贝叶斯网络、因子图与最小二乘法的关系，并探讨最大后验概率估计如何将SLAM问题转化为非线性最小二乘问题。掌握这些核心概念对于阅读SLAM领域的文献至关重要。

预备知识：加权范数

我们首先定义加权范数，这在后续推导中会用到。对于向量 e 和权重矩阵 Σ（通常是协方差矩阵的逆，即信息矩阵），加权范数定义为：

||e||_Σ^2 = e^T Σ e

通过Cholesky分解，可以将加权范数转换为标准的欧几里得范数。设 Σ = L L^T，其中 L 是下三角矩阵，则：

||e||_Σ^2 = e^T (L L^T) e = (L^T e)^T (L^T e) = ||L^T e||^2

这样，加权最小二乘问题就转化为了标准的最小二乘问题。

问题建模与联合分布

在SLAM中，我们处理一组状态变量 X_{0:K}（例如位姿、速度）和一组观测变量 Z_{1:K}。我们的目标是估计给定所有观测数据后，所有状态变量的联合后验概率分布 P(X_{0:K} | Z_{1:K})。

根据贝叶斯定理和马尔可夫假设，我们可以将这个联合分布分解为一系列更简单的概率因子的乘积：

P(X_{0:K}, Z_{1:K}) = P(X_0) ∏_{i=1}^{K} P(X_i | X_{i-1}) ∏_{j=1}^{K} P(Z_j | X_{j})

其中：

  • P(X_0) 是先验概率。
  • P(X_i | X_{i-1}) 是运动模型（或状态转移模型）。
  • P(Z_j | X_{j}) 是观测模型（或似然模型）。

这种分解可以用动态贝叶斯网络（一种有向图模型）直观地表示。图中的节点代表随机变量（状态 X 和观测 Z），箭头表示变量间的条件依赖关系。这种图模型清晰地编码了变量间的条件独立性，使得复杂的联合分布易于处理和建模。

因子图：一种更通用的建模框架

虽然动态贝叶斯网络很直观，但因子图为概率建模提供了更通用和模块化的框架。因子图是一种二分图，包含两种节点：

  • 变量节点：用圆圈表示，对应我们要估计的状态变量（如 x0, x1, z1）。
  • 因子节点：用黑色小方块表示，代表连接变量节点之间的关系或约束。

在SLAM的上下文中，这些因子对应着我们的概率模型：

  • 先验因子 φ_0(x0) 对应 P(X_0)。
  • 二元因子 ψ_{ij}(x_i, x_j) 对应运动模型 P(X_j | X_i)。
  • 二元因子 α_{ij}(x_i, z_j) 对应观测模型 P(Z_j | X_i)。

因子图的强大之处在于，整个联合概率分布简单地等于所有因子的乘积：

P(所有变量) ∝ ∏ (所有因子)

这种模块化特性使得添加新的传感器或约束（只需定义新的因子类型）变得非常容易，也便于实现像GTSAM这样的优化库。

最大后验概率估计与非线性最小二乘

我们的目标是通过最大化后验概率 P(X | Z) 来估计最优状态 X*，这称为最大后验概率估计。由于对数函数是单调的，最大化后验概率等价于最小化其负对数：

X* = argmax_X P(X | Z) = argmin_X -log P(X | Z)

将前面因子化的联合分布代入，并假设运动噪声和观测噪声均为零均值高斯噪声，我们可以得到具体的优化目标函数。

假设运动模型和观测模型如下：

  • 运动模型：x_j = f(x_i, u_{ij}) + w_{ij}， 其中 w_{ij} ~ N(0, Σ_{ij})
  • 观测模型：z_k = h(x_k) + v_k， 其中 v_k ~ N(0, Σ_ k)

在高斯噪声假设下，概率因子具有指数形式。取负对数后，指数项被消除，最终我们得到一个非线性最小二乘问题：

X* = argmin_X { ||r_0||^2 + ∑_{运动边} ||r_{ij}||_{Σ_{ij}}^2 + ∑_{观测边} ||r_k||_{Σ_k}^2 }

其中：

  • r_0 = L_0^T (x_0 - a) 是先验残差（a 是先验均值，L_0 是 Σ_0 的Cholesky因子）。
  • r_{ij} = L_{ij}^T (x_j - f(x_i, u_{ij})) 是运动残差。
  • r_k = L_k^T (z_k - h(x_k)) 是观测残差。

至此，我们将SLAM问题转化为了一个可以通过高斯-牛顿法、列文伯格-马夸尔特法等优化算法求解的非线性最小二乘问题。

实例：构建与求解线性系统

为了更具体地理解，我们来看一个简单的位姿图优化例子。考虑一个包含4个位姿节点（x0, x1, x2, x3）的因子图，它具有以下因子：

  1. 先验因子连接 x0。
  2. 运动因子连接 x0-x1, x1-x2, x2-x3。
  3. 一个回环因子连接 x3-x0。

假设模型都是线性的（例如 f(x_i, u) = x_i + u），那么每个因子都对应一个线性约束。将所有因子的残差堆叠起来，我们可以构建一个线性最小二乘问题 A x = b。

其雅可比矩阵 A 具有高度的稀疏性。每一行对应一个因子（一个约束），每一列对应一个状态变量。对于连接变量 x_i 和 x_j 的因子，其对应的行只在第 i 列和第 j 列有非零块（分别是负和正的雅可比矩阵），其余列全为零。这种稀疏模式是由因子图的结构直接决定的：只有被因子直接连接的变量才会在该因子对应的行中产生非零项。

正是这种稀疏性，使得我们可以使用高效的稀疏矩阵分解算法（如稀疏Cholesky分解或QR分解）来求解大规模的最小二乘问题，这是图优化SLAM能够实时处理成千上万个变量的关键。

总结

本节课我们一起学习了图优化SLAM的核心内容。我们首先从概率角度将SLAM问题表述为求解状态变量联合后验分布的问题，并通过马尔可夫假设将其分解。接着，我们引入了因子图这一强大且模块化的建模工具来描述问题。在高斯噪声假设下，最大后验概率估计自然地导出了一个非线性最小二乘优化问题。最后，通过一个简单实例，我们看到了如何构建并利用问题的稀疏性来高效求解。理解这一套从概率图模型到稀疏非线性优化的框架，是深入阅读现代SLAM文献和算法的基础。

021：从经典SLAM到3D空间感知

在本节课中，我们将跟随MIT的Antoni Rosinol博士，探讨如何将经典的SLAM（同时定位与建图）技术提升至3D空间感知的层次。我们将了解其核心工作Kimera，一个能够从RGB-D和IMU数据生成稠密3D度量语义地图的系统，并进一步探索其扩展——3D动态场景图，这是一种包含高级语义和拓扑信息的环境表示方法。

从图像到3D度量语义地图

上一节我们介绍了课程背景，本节中我们来看看Kimera系统的核心目标：从传感器输入生成一个稠密的、带有语义标签的3D地图。

Kimera的输入是一个RGB-D相机和一个惯性测量单元（IMU）。IMU提供线性加速度和陀螺仪信息。系统的目标是将一系列图像转换为一个稠密的3D度量语义地图。一个理想的结果是一个3D网格，其中场景中的物体（如椅子、笔记本电脑）被赋予不同的颜色标签，地板和墙壁等结构也被清晰地重建出来。

虽然早期成果已接近真实情况，但仍存在一些挑战，例如物体重建不完整、不同物体在网格中错误连接，以及需要处理动态物体（如移动的人）。

Kimera的核心架构与流程

上一节我们了解了Kimera的目标输出，本节中我们来看看它是如何一步步实现这个目标的。Kimera的核心是一个经典的稀疏视觉惯性里程计（VIO）管道。

Kimera的核心流程如下：

  1. 稀疏VIO：系统首先运行一个状态领先的视觉惯性里程计管道（Kimera-VIO），从图像和IMU数据中估计出相机的3D位姿和稀疏的3D路标点。这些路标点是通过三角化图像中跟踪的特征点得到的，能提供场景深度的粗略估计，主要用于避障。
  2. 稠密重建：利用估计出的位姿，系统将RGB-D相机提供的稠密深度信息融合到一个体素网格表示中。这个过程类似于占用网格映射，使用贝叶斯更新来估计每个体素被占据的概率。
  3. 语义融合：为了获得语义信息，系统将左相机图像输入一个卷积神经网络（CNN）来获取像素级的语义标签。然后，将这些标签与对应的3D点云一起，融合到上述的体素表示中。每个体素不仅存储占据概率，还存储一个分类概率分布，表示它属于各个语义类别（如“椅子”、“墙”）的概率。
  4. 系统集成：Kimera-VIO模块提供位姿和稀疏路标点。稀疏路标点用于快速生成每帧的局部3D深度图，然后被融合成多帧的3D图像。同时，位姿被送入Kimera-PGO模块进行位姿图优化，以减小漂移。所有这些信息（优化后的位姿、深度、语义）最终被送入Kimera-Semantics模块，在线生成全局的、实时的度量语义重建。

整个系统设计高效，大部分模块（除神经网络推理外）可在CPU上以约10Hz的频率运行。

从地图到高级理解：3D动态场景图

上一节我们看到了Kimera如何构建稠密的几何语义地图，本节中我们来看看为什么这还不够，以及如何引入更高层次的环境理解。

一个仅包含几何和物体类别标签的地图，对于执行“去厨房给我拿杯咖啡”这样的自然语言命令仍然不够。机器人需要理解更高级的概念，例如：

  • 房间：什么是厨房、走廊、卧室。
  • 可通行路径：环境中可以从A点移动到B点的拓扑结构。
  • 物体实例：区分“椅子1”和“椅子2”，而不仅仅是“椅子”这个类别。
  • 动态实体：处理在场景中移动的人和物体。

为了捕获这些信息，研究团队提出了3D动态场景图的概念。这是一种树状的分层表示，从底层的度量语义网格（Kimera的输出）中提取高级抽象。

3D动态场景图的层次结构如下：

  1. 度量语义层：基础层，即Kimera生成的3D网格。
  2. 物体与智能体层：
    • 物体：静态物体（如沙发、椅子）。进一步分为有已知CAD模型可拟合的物体，和没有先验模型的未知物体。
    • 智能体：动态实体（如机器人、人）。通过检测并跟踪每帧图像中的人体网格（形状和姿态参数）来实现。
  3. 位置与结构层：
    • 位置：机器人可以占据且不与障碍物碰撞的3D空间点，它们之间通过边连接形成拓扑地图。
    • 结构：从语义标签中提取的静态结构元素，如墙、地板、天花板。
  4. 房间层：通过对位置拓扑图进行分割得到。分割方法利用了从体素地图中计算出的截断符号距离函数（TSDF） 和欧几里得符号距离函数（ESDF）。ESDF给出了空间任一点到最近障碍物的距离，其2D切片可以近似为楼层平面图，进而用于识别和连接不同的房间。
  5. 建筑层：最顶层，包含所有房间和位置。

在这个表示中，每个节点（如一个物体）都知道其父节点（如它所在的房间），形成了一个丰富的、可查询的语义关系网络。例如，要“去沙发4”，机器人可以先导航到对应的房间，再通过拓扑地图找到该沙发所在的位置。

处理动态物体与系统优化

上一节我们构建了高级的场景理解表示，本节中我们来看看实现过程中的两个关键挑战：动态物体处理和全局一致性优化。

处理动态物体：
在重建静态场景时，移动的人会造成“重影”问题。Kimera采用两种策略：

  1. 动态掩码：在融合深度信息到体素网格时，如果某个像素被识别为动态物体（如“人”），则忽略该像素的深度信息，避免将其集成到静态地图中。
  2. IMU辅助：利用IMU信息帮助VIO模块区分静态和动态的特征点，防止动态物体的特征点污染位姿估计。

位姿图与网格联合优化（Kimera-PGMO）：
经典的SLAM在检测到回环后，会进行位姿图优化来校正漂移。但这只会优化相机位姿，而之前重建的3D网格会与之脱节。Kimera-PGMO将这个问题形式化为一个联合优化问题。

  • 优化变量：不仅包括机器人位姿，还包括从3D网格中采样的一部分顶点。
  • 约束边：包括来自原始3D测量的约束、顶点与观察到它的位姿之间的可见性约束、里程计约束以及回环约束。
  • 效果：通过求解这个优化问题，可以同时得到校正后的位姿和与之对齐的、变形后的3D网格，从而保证地图的全局一致性。

前沿展望与未来方向

上一节我们探讨了Kimera系统的具体实现，本节中我们来看看该领域仍面临的挑战和未来的研究方向。

紧密耦合学习与SLAM推理：
目前，Kimera将深度学习（如语义分割、深度估计）当作黑盒使用，SLAM流程不反馈信息给神经网络以改进其权重。未来的方向是以SLAM为环来训练神经网络。这可以通过一个双层优化问题实现：内层是SLAM推理（优化位姿和地图），外层是优化神经网络参数，使得内层SLAM估计出的轨迹尽可能接近真实轨迹。这需要可微分的因子图优化框架和大型逼真数据集的支持。

可微分渲染用于SLAM：
神经辐射场（NeRF）等可微分渲染技术展示了令人印象深刻的场景建模能力。它们通过一个多层感知机（MLP）将3D坐标和视角映射为颜色和密度，并能通过比较渲染图像和真实图像来优化MLP参数。将这类技术引入SLAM，可以：

  • 实现更完整、连续的场景表示：克服传统网格重建的不完整性。
  • 支持联合优化：像iMAP等工作所示范的，可以同时优化场景表示（NeRF）和相机位姿。
  • 赋能高级应用：与3D场景图结合，可以实现逼真的光照效果模拟和场景编辑，为机器人提供更灵活的环境模型。

总结

本节课中我们一起学习了从经典SLAM迈向3D空间感知的旅程。我们从Kimera系统开始，了解了如何构建稠密的3D度量语义地图。接着，我们探索了3D动态场景图，这是一种分层表示，能捕获环境中的物体、房间、拓扑等高级语义信息，使机器人能理解并执行复杂的自然语言指令。最后，我们展望了通过紧密耦合学习与SLAM、以及利用可微分渲染技术来推动该领域发展的未来方向。这些进展共同致力于让机器人能够在我们的家庭、办公室等复杂环境中实现完全自主的导航与交互。

022：矩阵李群

在本节课中，我们将学习矩阵李群的基本概念。这是机器人学中描述刚体运动（如旋转和平移）的强大数学工具。我们将从群的定义开始，逐步介绍几种关键的矩阵群，并探讨它们的李代数、指数映射等核心概念。

群的定义

上一节我们介绍了课程概述，本节中我们来看看什么是“群”。群是一个集合，配备了一种“粘合”运算，并满足四条规则。

以下是构成一个群 G 的四个条件：

  1. 封闭性：对于任意两个元素 g 和 h 属于 G，它们的运算结果 g · h 也属于 G。
  2. 结合律：对于任意 g, h, k 属于 G，有 (g · h) · k = g · (h · k)。
  3. 单位元：存在一个元素 e 属于 G，使得对所有 g 属于 G，有 e · g = g · e = g。
  4. 逆元：对于每个元素 g 属于 G，存在一个元素 g^{-1} 属于 G，使得 g · g^{-1} = g^{-1} · g = e。

常见的例子包括整数集与加法运算（单位元是0，逆元是相反数），以及非零实数集与乘法运算（单位元是1，逆元是倒数）。

矩阵群

上一节我们了解了抽象的群，本节中我们来看看具体的矩阵群。矩阵群就是满足上述群规则的矩阵集合，其运算是矩阵乘法。

一个矩阵群首先是一个由可逆矩阵组成的集合。可逆意味着矩阵的行列式不为零。矩阵乘法天然满足结合律，单位矩阵 I 是单位元，矩阵的逆就是逆元。

一般线性群

我们首先介绍最广泛的矩阵群：一般线性群。

一般线性群 GL(n, ℝ) 是所有 n × n 可逆实矩阵的集合。其定义为：

GL(n, ℝ) = { A ∈ M_{n×n}(ℝ) | det(A) ≠ 0 }

其中 M_{n×n}(ℝ) 表示 n × n 实矩阵的集合。

对于 GL(n, ℝ)：

  • 群运算是矩阵乘法。
  • 两个可逆矩阵相乘仍可逆，满足封闭性。
  • 矩阵乘法满足结合律。
  • 单位元是单位矩阵 I。
  • 每个矩阵 A 都有逆矩阵 A^{-1}。

一般线性群是后续所有矩阵群的“母体”。

仿射群与特殊欧几里得群

上一节我们讨论了最一般的矩阵群，本节中我们来看在机器人学中更常用的、结构更具体的群。

仿射群

仿射群 Aff(n) 的元素可以表示为以下形式的矩阵：

[ A   t ]
[ 0   1 ]

其中 A 属于 GL(n, ℝ)，t 是一个 n 维平移向量。这种矩阵作用于齐次坐标 [x, 1]^T 上，实现旋转（由 A 描述）和平移（由 t 描述）。

特殊欧几里得群

在机器人学中，我们主要关注保持距离和方向的刚体运动，这对应于特殊欧几里得群 SE(n)。它是仿射群的一个子群，要求旋转部分 A 是特殊正交矩阵（即旋转矩阵）。

对于 SE(3)（三维空间），其元素可表示为：

[ R   p ]
[ 0   1 ]

其中 R 属于 SO(3)（三维旋转矩阵群，满足 R^T R = I 且 det(R) = 1），p 属于 ℝ^3（平移向量）。

SE(3) 描述了三维空间中所有可能的刚体运动（旋转加平移）。

旋转群与特殊正交群

上一节我们引入了包含平移的群，本节我们聚焦于纯旋转。

特殊正交群 SO(n) 是所有 n × n 正交矩阵且行列式为1的集合：

SO(n) = { R ∈ M_{n×n}(ℝ) | R^T R = I, det(R) = 1 }

SO(3) 就是三维旋转矩阵群。一个矩阵是旋转矩阵，当且仅当其列向量是两两正交的单位向量。

SO(n) 中的元素是直接等距变换（保持距离和方向）。如果去掉行列式为1的限制，得到正交群 O(n)，它包含反射（间接等距变换）。在物理世界中，我们通常只关心 SO(n)。

SO(3) 的维度是3（对应绕x、y、z轴的旋转）。有趣的是，SO(4) 的维度是6，这意味着在四维空间中有六种独立的旋转方式。

齐次坐标表示

为了统一处理旋转和平移，我们使用齐次坐标。

一个三维点 q = [x, y, z]^T 的齐次坐标是 [x, y, z, 1]^T。
一个三维向量（如速度）v = [v_x, v_y, v_z]^T 的齐次坐标是 [v_x, v_y, v_z, 0]^T。

SE(3) 中的变换 (R, p) 可以用一个 4×4 齐次变换矩阵表示：

H = [ R   p ]
    [ 0   1 ]

它对点 q 的变换为：H * [q, 1]^T = [Rq + p, 1]^T。
这种表示法便于通过矩阵连乘实现变换的复合。

需要注意的是，SE(3) 并不是简单的 SO(3) 和 ℝ^3 的直积 SO(3) × ℝ^3，因为旋转和平移不可交换。它的结构是一种半直积 SO(3) ⋉ ℝ^3。

李代数：群的“速度”空间

上一节我们讨论了群元素本身（“位置”），本节我们来看看如何描述在群上运动的“速度”。

我们可以将矩阵群 G 视为嵌入在高维空间中的曲面或流形。在群上某点（特别是单位元 I）处的所有可能“速度”向量构成的切空间，称为该群的李代数，记作 𝔤。

李代数元素代表了群元素的变化率。对于旋转群 SO(3)，其李代数 𝔰𝔬(3) 由所有 3×3 的反对称矩阵组成：

𝔰𝔬(3) = { Ω ∈ M_{3×3}(ℝ) | Ω^T = -Ω }

一个反对称矩阵 Ω 可以对应一个三维角速度向量 ω：

Ω = [ 0     -ω_z   ω_y  ]
    [ ω_z    0    -ω_x  ]
    [ -ω_y   ω_x    0   ]

记作 Ω = ω^。反之，ω = Ω^∨。

李代数 𝔤 的维度与李群 G 的维度相同。例如，SO(3) 和 𝔰𝔬(3) 的维度都是3。

指数映射：从代数回到群

李代数描述了群单位元附近的“无穷小”运动。那么，如何从这些“速度”恢复出有限的“位移”（即群元素）呢？答案是指数映射。

对于矩阵李群，指数映射 exp: 𝔤 → G 将李代数中的元素映射回李群。对于矩阵，指数映射定义为矩阵指数：

exp(A) = I + A + A^2/2! + A^3/3! + ...

关键性质：如果 γ(t) = exp(tA)，那么 γ(0) = I（单位元），且 γ'(0) = A（切向量）。因此，exp(tA) 定义了群中一条从单位元出发、初始“速度”为 A 的曲线。

对于 SO(3)，指数映射将反对称矩阵 Ω（角速度）映射为旋转矩阵 R，这正好就是罗德里格斯旋转公式：

R = exp(Ω) = I + (sinθ/θ) Ω + ((1-cosθ)/θ^2) Ω^2

其中 θ = ||ω|| 是旋转角度。

指数映射建立了李代数（局部线性空间）与李群（整体弯曲空间）之间的桥梁。

伴随映射与李括号

在群上做坐标变换时，需要一种工具来变换李代数元素，这就是伴随映射。

对于 g ∈ G 和 ξ ∈ 𝔤，伴随作用 Ad_g: 𝔤 → 𝔤 定义为：

Ad_g(ξ) = g ξ g^{-1}

这相当于在群上用 g 进行共轭作用，然后取在单位元处的导数。它描述了当坐标系随 g 变换时，李代数元素 ξ 应如何相应变换。

如果我们进一步对 g 本身进行微分，就得到李括号 [·, ·]: 𝔤 × 𝔤 → 𝔤：

ad_ξ(η) = [ξ, η] = ξη - ηξ

李括号衡量了两个李代数元素的“不可交换性”。如果 [ξ, η] = 0，则它们可交换。对于 𝔰𝔬(3)，李括号运算与三维向量的叉积运算同构：[ω_1^, ω_2^] = (ω_1 × ω_2)^。

李代数连同其李括号运算，完全刻画了李群的局部结构。

雅可比矩阵

最后，我们介绍一个在计算中非常重要的工具：雅可比矩阵。

当进行复合变换时，例如 exp(ξ) exp(η)，其结果一般不等于 exp(ξ + η)，因为指数映射不是线性的。但是，我们可以引入左雅可比矩阵 J_l(η)，使得以下近似公式成立：

exp(ξ) exp(η) ≈ exp(ξ + J_l(η)^{-1} ξ)

或者更精确地，在处理微小扰动时：

exp((ξ + Δξ)^) ≈ exp(ξ^) exp(J_l(ξ) Δξ)^

其中 J_l(ξ) 是一个将李代数中的加法扰动，转换为群上乘法扰动的线性变换矩阵。它对于在流形上进行优化、求导和误差传播至关重要。SO(3) 的左右雅可比都有闭式解。

总结

本节课中我们一起学习了矩阵李群的核心知识。我们从群的基本定义出发，认识了一般线性群 GL(n)。然后聚焦于机器人学中的关键群：描述纯旋转的特殊正交群 SO(3)，以及描述完整刚体运动的特殊欧几里得群 SE(3)。我们学习了用齐次坐标和 4×4 变换矩阵来表示 SE(3) 元素。

为了分析运动速度，我们引入了李代数 𝔤 作为李群在单位元处的切空间。对于 SO(3)，李代数由反对称矩阵组成，对应角速度向量。指数映射 exp 建立了从李代数（速度）到李群（位姿）的桥梁，其特例即罗德里格斯公式。伴随映射 Ad_g 用于在不同坐标系间变换李代数元素，而李括号 [·, ·] 则刻画了李代数结构的非交换性。最后，雅可比矩阵 J 是处理群上微积分和扰动分析的关键工具。

掌握这些概念，为我们在李群流形上表述和求解机器人运动学、动力学及状态估计问题奠定了坚实的数学基础。

023：黎曼几何

概述

在本节课中，我们将学习黎曼几何的基础知识。黎曼几何是研究弯曲空间（即流形）上几何性质的数学分支。我们将从理解如何在流形上定义长度和角度开始，最终学习如何将黎曼几何应用于实际问题，例如梯度下降和图像配准。

流形与切空间

上一节我们介绍了矩阵群和流形。本节中，我们来看看如何在这些结构上进行几何测量。

流形是局部看起来像平坦空间（如平面）的对象。例如，地球表面是一个流形，因为从我们站立的位置看，它看起来是平的。流形的正式定义涉及将其分割成多个“图册”，每个图册都能平滑地映射到平坦空间。

与流形密切相关的是切空间的概念。给定流形上一点，该点的切空间包含了所有可能通过该点的曲线的速度向量。切空间本身是一个向量空间，其维度与流形的维度相同。

对于矩阵群（如旋转群SO(3)或特殊欧几里得群SE(3)），其李代数就是单位元处的切空间。例如，SO(3)的李代数由所有反对称矩阵组成。

几何学基础：长度与角度

为了在流形上进行几何测量，我们需要定义长度和角度。让我们从熟悉的平坦空间R²开始。

假设我们有两条在点0处相交的曲线γ₁和γ₂。这两条曲线之间的夹角如何计算？我们计算它们在交点处的切线之间的夹角。具体公式如下：

角度公式：
[
\cos(\theta) = \frac{\langle \dot{\gamma}_1(0), \dot{\gamma}_2(0) \rangle}{|\dot{\gamma}_1(0)| \cdot |\dot{\gamma}_2(0)|}
]

其中，(\langle \cdot, \cdot \rangle) 表示点积，(|\cdot|) 表示向量的长度（范数）。

接下来，考虑一条从0到1的曲线γ(t)。其长度如何计算？我们对其速度向量的长度进行积分。

长度公式：
[
L(\gamma) = \int_0^1 |\dot{\gamma}(t)| , dt
]

从这两个公式可以看出，定义角度和长度的核心在于点积和范数。实际上，范数可以由点积定义（(|v| = \sqrt{\langle v, v \rangle})）。因此，几何学的核心就是理解点积。

内积与度量矩阵

在向量空间Rⁿ中，标准的点积定义为 (u^T v)。但我们可以将其推广为更一般的内积。

内积是一个将两个向量映射为一个实数的函数，它必须满足三个条件：

  1. 对称性：(\langle u, v \rangle = \langle v, u \rangle)
  2. 双线性：对任意标量α, β，有 (\langle \alpha u_1 + \beta u_2, v \rangle = \alpha \langle u_1, v \rangle + \beta \langle u_2, v \rangle)（对第二个变量同理）
  3. 正定性：(\langle v, v \rangle \ge 0)，且等号成立当且仅当 (v = 0)

在Rⁿ中，每一个这样的内积都可以表示为一个对称正定矩阵M的作用：
[
\langle u, v \rangle_M = u^T M v
]
这个矩阵M被称为度量矩阵。它决定了我们如何测量向量的长度和夹角。改变M，就相当于以扭曲或拉伸的方式来看待空间，从而改变了“正交”和“单位长度”的含义。

黎曼度量

现在，我们将内积的概念推广到流形上。一个黎曼流形是一个配备了黎曼度量的流形。

黎曼度量在流形M的每一点x上，都给出了其切空间TₓM上的一个内积。也就是说，对于每一点x，我们都有一个对称正定矩阵G(x)，它定义了该点切向量之间的点积：
[
\langle u, v \rangle_x = u^T G(x) v, \quad \forall u, v \in T_xM
]
因此，黎曼度量本质上是一个在流形上变化的矩阵值函数 (x \mapsto G(x))。正是这种变化编码了空间的曲率。

例子：球面上的度量
用经纬度坐标(θ, φ)参数化单位球面。其上的黎曼度量矩阵为：
[
G(\theta, \phi) = \begin{bmatrix}
1 & 0 \
0 & \sin^2(\theta)
\end{bmatrix}
]
这个矩阵在极点（θ=0或π）处会退化（sin(θ)=0），这反映了在标准经纬度坐标下，极点处的奇异性。

梯度与“音乐同构”

在优化问题中，我们经常需要计算函数的梯度以进行梯度上升/下降。在欧几里得空间中，梯度向量grad f是函数微分df（一个行向量，或称余向量）的转置。

然而，在配备了黎曼度量G的流形上，从微分df到梯度grad f的转换更为复杂。这涉及到所谓的“音乐同构”：

  • 降号(♭)：将向量v映射为余向量v♭，定义为 (v♭(w) = \langle v, w \rangle)。
  • 升号(♯)：是降号的逆运算，将余向量α映射为向量α♯。

具体公式如下：

  • 向量 (u) 对应的余向量：(u^♭ = u^T G)
  • 余向量 (p) 对应的向量：(p^♯ = G^{-1} p^T)

函数的梯度则定义为其微分的升号：
[
\text{grad } f = (df)^♯
]
在标准欧几里得度量下（G=I），这简化为普通的转置操作。

应用：梯度流与测地线

黎曼几何的一个核心应用是定义和计算流形上的梯度流。如果我们想在流形M上最大化一个函数F，我们可以沿着其梯度的方向移动：
[
\dot{x} = \text{grad } F(x)
]
这个微分方程的解曲线给出了上升最快的路径。在图像配准等问题中，我们需要在SE(3)这样的矩阵群上优化一个目标函数（如点云重叠度），梯度流提供了求解的数值方法。

另一个重要概念是测地线，即流形上的“直线”（局部最短路径）。拉格朗日力学中的运动方程，在特定条件下（动能由黎曼度量定义，且无外力），描述的正是测地线流。

例如，对于之前给出的球面度量，将其代入欧拉-拉格朗日方程，得到的解就是球面上的大圆（测地线）。

总结

本节课中我们一起学习了黎曼几何的基础知识。我们从流形和切空间的定义出发，理解了在弯曲空间上进行几何测量的核心在于定义每一点切空间上的内积（即黎曼度量）。我们学习了如何通过度量矩阵计算长度、角度以及函数的梯度。最后，我们看到了黎曼几何在梯度优化算法和描述物理系统（如拉格朗日力学）中的关键应用。掌握这些概念，是理解后续在矩阵群上进行高级算法（如图像配准）的基础。

024：混合感知问题的因子图表示

概述

在本节课中，我们将学习如何利用因子图表示法来解决混合感知问题。混合感知问题涉及同时推理连续状态（如机器人位姿）和离散状态（如数据关联、语义类别）。我们将探讨因子图如何为这类问题提供优雅的建模框架，并介绍一种高效的交替优化求解算法。

研究背景与动机

上一节我们概述了课程内容，本节中我们来看看本次讲座的研究背景。本次讲座由麻省理工学院的Kevin Doherty博士主讲，他的研究兴趣集中在机器人感知、状态估计、导航和机器学习，尤其关注野外机器人和海洋机器人应用。

他的博士研究工作主要受到以下问题启发：假设一个机器人在一个未知环境中探索，它需要知道自己在环境中的位置（定位）、环境中物体的位置（建图），以及这些物体是什么（语义理解）。这在环境监测、基础设施检查、自动驾驶和家用机器人等场景中都有广泛应用。

数学上，这可以表述为一个语义SLAM问题，即从一组噪声测量中推断机器人的轨迹、环境地标的位置以及每个地标的语义类别。由于测量存在噪声，我们将其表述为一个贝叶斯推断问题。

混合感知问题的核心挑战

在介绍了问题背景后，本节我们来看看解决该问题的核心挑战。主要有两点：

  1. 将语义信息整合到传统几何估计中：用于获取语义信息的学习模型（如目标检测器）在实践中可能不可靠，尤其是在部署环境与训练数据不匹配时。它们的预测可能存在误差或歧义。
  2. 处理异常值和错误的数据关联：不可靠的语义预测会导致测量异常或错误的数据关联（即无法确定测量对应哪个地标）。这可能导致SLAM系统发生灾难性故障。处理这些问题需要解决离散估计问题。

这两个挑战都涉及对离散不确定性源的推理。因此，核心问题是如何表示离散状态与连续状态之间的耦合关系，并找到最可能的未知状态赋值。

一个自然的方法是将问题建模为在给定测量Z的情况下，最大化连续变量C和离散变量D的联合后验概率：
argmax_{C,D} P(C, D | Z)

因子图：统一的建模框架

面对上述挑战，我们需要一个强大的建模工具。因子图恰好提供了这样一个框架。

因子图是一种概率图模型，它允许我们将复杂的联合分布分解为更简单的因子乘积。混合因子图特指那些同时包含连续状态（用蓝色表示）和离散状态（用红色表示）的因子图。

以下是两个例子：

  • 切换系统：估计行人在图像中的连续运动，其运动模式受离散决策（如左转/右转）影响。
  • 鲁棒SLAM：估计机器人轨迹（连续状态），同时引入离散变量来控制是否将闭环检测测量视为内点（保留）或外点（剔除）。

因子图使我们能够清晰地写出这些问题的模型。然而，尽管GTSAM等现有库能很好地处理连续因子图，却缺乏直接解决这类混合问题的通用优化器。

DCSAM：混合因子图的求解库

既然现有工具存在不足，本节我们来看看为解决此问题而开发的工具：DCSAM库。

DCSAM（离散连续平滑与建图）是一个C++库，它将GTSAM扩展到了混合因子图设置。它包含一个默认的优化器，并易于扩展以建模新问题或集成新的优化方法。

其核心求解算法的关键思想是：虽然对混合因子图进行精确推断通常是计算困难的（离散状态组合数随规模指数增长），但这些问题通常可以分解为易于求解的子问题。具体来说，当固定连续状态时，对剩余离散状态的推断通常会解耦或形成结构简单的图（如链式结构），从而可以用维特比算法等高效方法求解。反之，固定离散状态后，连续状态的优化则是一个标准的非线性最小二乘问题，可用高斯-牛顿法等高效求解。

因此，DCSAM采用的算法是交替优化：

  1. 给定当前连续状态估计 C_i，求解最优的离散状态赋值 D_{i+1}。
  2. 固定离散状态为 D_{i+1}，对连续状态执行一步优化（如梯度下降），得到 C_{i+1}。
  3. 重复迭代直至收敛。

这种方法的一个著名特例是迭代最近点算法，它可以被表示为对一个特定混合因子图进行交替优化。

该方法的优势在于，它通常能高效处理成千上万的离散变量，而无需对离散假设空间进行剪枝。在鲁棒SLAM任务中，它能在保持与先进方法相当精度的同时，实现更快的求解速度。

应用：基于DCSAM的语义SLAM系统

在了解了核心求解工具后，本节我们来看一个具体应用：利用DCSAM构建一个语义SLAM系统。

系统需要联合估计：

  • 机器人位姿（连续状态）
  • 语义地标：包含3D位置（连续）和语义类别（离散）
  • 数据关联变量（离散）：表示每个测量与地图中地标的对应关系

系统工作流程如下：

  1. 当新的物体检测到来时，计算其与现有地图中所有地标的测量似然。
  2. 如果最大似然低于阈值，则认为该检测对应一个新地标，将其加入地图。
  3. 如果高于阈值，则认为它对应一个已知地标。此时，我们维护多个最有可能的关联假设，而不是只选一个。
  4. 系统还允许“空假设”，以拒绝可能是误检的测量（即视为外点）。

一个重要的技巧是，我们可以通过边缘化数据关联变量，将问题隐式地表示为仅关于机器人位姿和地标（位置和类别）的优化问题，从而显著减少需要显式维护的离散变量数量。

实验表明，在模拟和真实数据集（如KITTI）上，这种能够考虑多假设数据关联的方法，比仅采用最大似然关联的基线方法更加鲁棒，能有效应对检测噪声和动态物体干扰。

总结与未来展望

本节课中，我们一起学习了如何利用因子图表示和求解混合感知问题。

核心总结如下：

  1. 问题：机器人语义感知等任务需要联合推理连续状态（几何）和离散状态（语义、数据关联、外点），这具有挑战性。
  2. 建模工具：混合因子图为这类问题提供了清晰、统一的建模框架。
  3. 求解方案：DCSAM库提供了建模和求解混合因子图的通用工具。其核心算法利用因子图的条件独立结构，通过交替优化连续和离散状态来实现高效推断。
  4. 应用验证：基于DCSAM构建的语义SLAM系统能够处理数据关联歧义和类别噪声，比传统方法更鲁棒。

未来可能的研究方向包括：

  • 开发更具表现力的混合模型，例如结合物体形状和类别的推理。
  • 将层次化和抽象化知识（如场景图）纳入因子图框架。
  • 探索如何将大语言模型与场景几何基础通过因子图进行结合。
  • 利用基于对象的SLAM系统生成多视图一致的数据，用于自监督或半监督学习，以微调感知模型。

问答环节摘要

  • 关于离散子图求解：当固定连续状态后，离散子图若为链式结构，则可使用高效的维特比算法。若子图连接复杂，则精确求解可能很慢，此时可考虑使用坐标下降等局部优化方法替代。
  • 关于传感器融合：从工程角度，结合多传感器信息（冗余）通常比依赖单一传感模态更可靠。
  • 关于离散变量：DCSAM中的离散变量不限于二值，可以是多类（如多种语义类别）。
  • 关于初始化：算法通常需要较好的连续状态初始猜测（如来自里程计）。离散状态则无需初始化，因为第一步就是求解给定连续状态下的最优离散赋值。在极端情况下（如多机器人无共同初始参考系），初始化可能很困难，可考虑结合其他全局方法进行引导。
  • 关于全局搜索：交替优化是一种局部方法。可以探索将其与分支定界等全局搜索策略结合，以降低对初始值的敏感度，但这通常会牺牲效率。MHT、MH-iSAM2等方法正在这个方向探索。
  • 关于语义SLAM中的地标表示：在所示系统中，地标位置简单地取自检测边界框的中心和中值深度。未来可以探索使用带语义标签的关键点或分割掩码等更精细的表示。

025：Guest Lecture - 机器人3D感知中的等变学习

在本节课中，我们将学习一种名为“等变学习”的机器学习框架，特别是在机器人3D感知领域的应用。我们将探讨等变模型如何帮助机器人更可靠、更高效地理解其周围的三维环境。

机器人应用与3D感知的重要性

我们身处移动机器人课程中，脑海中会浮现许多机器人应用场景。例如，物体操控，即使用机械臂或手爪抓取和操作物体；室内导航，即让机器人在室内环境中自主移动；以及自动驾驶，这可以看作机器人在复杂室外场景中的导航任务。

对于所有这些任务，3D感知都是至关重要的组成部分。它使机器人能够理解三维环境，更重要的是，为机器人与现实世界中的实体进行交互做好准备。

因此，机器人应用中的卓越感知具有两个非常重要的特性。第一是安全关键性，因为机器人被设计用于与现实世界交互，感知中的错误可能导致灾难性后果。第二是计算资源敏感性，机器人机载的计算能力通常有限，因此感知算法也必须高效，不能依赖需要数百个GPU的庞大模型。

这两个特性对感知算法提出了两个要求：一是需要算法具有可预测的行为，二是需要算法高效运行。

可预测行为与环境变化

首先，我们来探讨“可预测行为”的含义。具体来说，我们希望感知算法在各种环境变化下都能给出可预测的结果。

环境变化主要可以分为两类。第一类是“不同实体”，例如，对于识别椅子的算法，世界上有各种形状、样式的椅子，我们希望模型能稳健地处理它们。室内外环境的布局也千差万别。第二类是“同一实体的不同观测”，例如，从不同视角观察同一把椅子，其外观会截然不同；同一棵树或车辆，在不同距离或角度下观测，外观也会有很大变化。

传统方法与等变学习的优势

目前，大多数模型以相同的方式处理这些变化：使用足够大、设计良好的模型，去分别识别每一个不同的实体和每一次不同的观测。

然而，这里存在一个自然的问题：既然不同的观测通常来自同一物体，那么我们能否只为同一物体的不同观测学习一次？也就是说，如果一个模型能够识别物体的内在属性，那么无论其外观如何变化，模型都知道它是同一个物体。这样做会带来什么好处？我们又该如何实现？

答案是：利用对称性。如果模型能够捕捉同一物体所有不同观测背后的底层对称性，这将为我们带来巨大好处。将这种对称性嵌入模型，使用保持对称性的模型（即等变模型）来处理所有对称变换，可以帮助模型在所有观测下产生可预测的行为，因为模型知道无论外观如何变化，它都是同一个东西，从而增加了感知系统的鲁棒性。

另一方面，效率也是机器人感知算法的重要部分。既然我们现在可以将所有不同的观测视为同一个物体来处理，就可以节省模型容量，去学习真正不同的实体。这降低了对模型容量的需求，使其能更好地应对现实世界中的环境变化。

什么是等变模型？

接下来，我们简要概述什么是等变模型。等变性是描述函数性质的一个概念。如果一个函数对某个变换群 G 保持对称性，那么我们称这个函数对群 G 是等变的。换句话说，应用变换和应用函数的顺序可以互换，不影响最终结果。

最著名的等变学习例子是卷积神经网络，它们具有平移等变性。这意味着，在图像中移动一个物体然后进行分割，与先分割再移动结果，两者是等价的。这个特性使得模型能够泛化到图像中物体的平移变化，模型无需为每个位置单独学习。

然而，CNNs 只对平移等变，对旋转并不等变。例如，旋转一张城市鸟瞰图，CNN 输出的特征图不仅发生了旋转，其内容也发生了改变。这意味着网络无法从旋转图像中学习到可靠的表示。

这正是等变网络发挥作用的地方。我们可以开发对平面旋转群 SO(2) 等变的网络。使用等变模型时，输入图像发生旋转，输出特征图也会发生完全相同的旋转，且特征内容保持稳定。这使得网络能够从旋转图像中学习到可靠且鲁棒的表示。

等变网络将平移等变的思想推广到更一般的变换，例如 SE(2)（二维刚体变换）、SO(3)（三维旋转）和 SE(3)（三维刚体变换）。想象一下，如果我们有一个对 SE(3) 等变的模型，那么给定同一客厅的两个不同视角，网络会知道它们对应完全相同的布局，不会被观察者移动引起的姿态变化所迷惑，这对于导航等任务非常有用。

构建等变模型：MLP 与 CNN

等变模型可以从当前深度学习社区常用的各种网络架构构建，例如多层感知机、卷积神经网络、Transformer 和图神经网络。本节课我们将主要关注前两种，即 MLP 和 CNN，因为它们相对简单，适合教学。我们将介绍基于它们的简单等变模型示例，并展示在机器人领域的酷炫应用。

为了让数学部分保持简单易懂，我们不会深入复杂的理论细节，但会让大家了解这些模型的样子以及它们能做什么。

基于 MLP 的等变模型：Vector Neurons

首先，我们来看 MLP。我们将聚焦于一个近期具有代表性的工作——“Vector Neurons”，这是一个多层感知机架构，但它被设计为对三维旋转 SO(3) 等变。该工作可应用于点云特征学习，并已在多种机器人任务中使用，包括点云配准、物体操控和 SLAM 系统。

我们可以将 Vector Neurons 视为一个 SO(3) 等变 MLP。我们来比较一下它与传统 MLP 的区别。

对于一个传统的点云处理网络 PointNet，其输入是一个 n x 3 的点云矩阵，每一行是一个点。网络将每个点的三维特征向量映射到更高维的特征，最后通过池化层汇总所有点的特征，得到代表整个点云的单个特征向量。

Vector Neurons 看起来非常相似，但关键区别在于：传统网络中每个点的特征是一个 1 x C 的向量，而在 Vector Neurons 中，每个点的特征是一个 C x 3 的矩阵。也就是说，每个点的特征在末尾增加了一个大小为 3 的维度。

这使得输入点云（n x 3）和输出特征（C x 3）都可以被一个 3x3 的旋转矩阵作用。正是这一点，使得 Vector Neurons 在高层设计上具备了旋转等变的潜力。

下面我们逐步拆解，看看这是如何实现的。

第一步：特征初始化
对于传统 MLP，将点坐标 [x, y, z] 映射为特征非常简单，只需乘以一个 C x 3 的权重矩阵，得到一个 C 维向量。
对于 Vector Neurons，给定一个点 [x, y, z]，它首先考虑该点的邻域点，例如图中所示的五个邻域点。我们将这些邻域点的偏移向量堆叠起来，得到一个 m x 3 的矩阵。这样，每个点的特征就从向量提升为了矩阵。然后，类似传统 MLP，我们在左侧应用一个权重矩阵乘法，将其映射到某个隐藏特征维度，例如 C x 3。

线性层
传统 PointNet 的线性层是通过在左侧乘以矩阵，将特征向量映射到另一个维度。
Vector Neurons 的线性层操作形式完全相同，只是每个点的特征在末尾多了一个维度。由于权重乘法是在左侧进行的，因此这个额外的维度不受影响。可以验证，这样的层是旋转等变的。根据等变的定义，变换（旋转矩阵右乘）和函数（权重矩阵左乘）的顺序可交换。因为左乘和右乘互不影响，所以该层是 SO(3) 等变的。

非线性层
这里以最常用的 ReLU 激活函数为例。
传统 MLP 的 ReLU 是 max(输入, 0)。
Vector Neurons 使用的是向量化版本的 ReLU。它首先估计一个规范方向 K（一个 1x3 的向量），然后截断每个特征向量：与规范方向夹角小于 90 度的向量保持不变，夹角大于 90 度的向量被投影到法平面上。这相当于将截断操作提升到了三维空间，用一个超平面代表零点。可以证明，这个操作也是旋转等变的。

池化层与置换不变性
Vector Neurons 也具有置换不变性。这是因为最大池化或平均池化操作本身是置换不变的。由于每个点在 Vector Neurons 中都是独立处理的，因此点的堆叠顺序不影响最终的池化结果。

Vector Neurons 的应用：点云配准

拥有了这些特性，我们能做什么呢？这是一个非常优雅的网络架构。

考虑两个点云：点云 P 和点云 P‘。P’ 是 P 经过旋转和随机打乱顺序后的版本，其中 R 是旋转矩阵，M 是置换矩阵。我们不知道 P‘ 和 P 之间点的对应关系。

由于等变性和置换不变性，我们知道这个 Vector Neurons SO(3) 等变编码器的输出满足：F(P‘) = F(P) R。也就是说，两个特征只相差一个旋转矩阵 R。

这带来了一个非常好的性质：给定这两个通过 R 关联的度量（现在是深度特征），我们可以应用奇异值分解来求解最终的旋转。这样，我们就在深度特征空间中进行点云配准，而不是在欧几里得空间。

这个策略非常巧妙，因为我们不需要求解两个原始点云之间的点对应关系。现在，深度特征是自动对应的，它们只通过旋转 R 关联，没有置换矩阵 M 的干扰。因此，我们避免了寻找点对应关系的难题。

在实践中，P‘ 可能并不完全是 P 的旋转副本，可能带有噪声。因此，我们还会训练一个解码器（例如用于占据栅格预测）来使特征对噪声更鲁棒，并通过对 SVD 估计的旋转施加损失，鼓励网络找到与真实旋转一致的解。

这种策略由于无需对应点且 SVD 提供了闭式解，因此可以实现与初始化误差无关的点云配准。

其他应用

SO(3) 等变特征还有其他应用。

  • 物体操控：任务中，机器人学习如何以某种姿态抓取物体。在测试时，机器人需要抓取未见过的物体或处于新姿态的物体。SO(3) 等变特征可以帮助可靠地跟踪抓取点，因为特征会随着点云的旋转而旋转。
  • 物体 SLAM：在这项工作中，SLAM 构建的地图中的物体不是用点云表示，而是用 SO(3) 等变特征表示。由于这些特征可以经历与欧几里得空间点云相同的旋转，因此它们可以融合到 SLAM 的姿态图优化中。

基于 CNN 的等变模型：群卷积

接下来，我们看卷积神经网络如何实现等变。正如之前提到的，群卷积可以将等变性扩展到更一般的变换，例如 SE(3)，从而处理更复杂的任务。

在深入如何构建等变卷积之前，我们需要一点关于“群”概念的背景知识。

群的概念

在数学中，群是一个集合，配备了一种二元运算，并满足封闭性、结合律、单位元和逆元四个公理。在机器人学中，我们使用群来描述变换，所有的变换都属于某个群。

例如，整数或实数配备加法运算后构成平移群。SO(2)、SO(3)、SE(2)、SE(3) 分别是二维旋转、三维旋转、二维刚体变换和三维刚体变换群。

引入群的概念是为了将卷积神经网络的思想推广到等变版本。

从传统卷积到群卷积

传统卷积是定义在欧几里得空间 R^N 上的函数（特征图）与滤波器（核函数）之间的积分运算。由于 R^N 本身在加法运算下构成一个群，因此我们可以将 R^N 推广到任意群 G，从而得到群卷积。

在群卷积中，输入特征图、滤波器和输出特征图都是定义在群 G 上的函数，卷积运算是在群 G 上进行积分。

当 G 是 R^N 时，就回到了传统卷积。当 G 是 SO(2)、SE(2)、SO(3) 等时，我们就得到了对应变换群的等变卷积。

群卷积的应用与计算考量

例如，考虑 SE(2) 群卷积。我们将 SO(2) 旋转离散化为 4 个方向（0, π/2, π, 3π/2）。这样，特征图的定义域就从 R^2 提升到了 R^2 × C4，特征图变得更大，每个空间位置现在还关联了 4 个方向。

通过插入不同的群 G，可以实现不同类型的等变性。应用包括：

  • 图像分类：在旋转后的 MNIST 数字数据集上，等变网络能可靠识别数字。
  • 航空图像目标检测：等变性帮助网络可靠检测地面不同朝向的物体。
  • 图像关键点检测：等变网络能可靠检测并匹配旋转图像中的关键点。
  • 3D 点云应用：如物体姿态估计、重建、室内 RGB-D 物体检测等。等变网络能一致地检测旋转的物体并可靠估计其方向。

群卷积的效率问题与改进

尽管群卷积应用广泛且成功，但它存在一个主要限制：效率问题。

群卷积将定义域从 R^N 提升到了更大的群空间 G，这会导致计算成本增加。例如，在三维空间中，一个点不仅要与空间邻居通信，还要与邻居点的所有方向进行通信，连接数大大增加，尤其是当群较大时（如 SE(3)），计算开销会成为问题。

因此，我们实验室的一项工作就是提高群卷积的效率。我们的方法是开发“商空间卷积”。

商空间卷积

总体思想是：我们使用商空间 G/H 来代替完整的群 G 进行卷积。就像除法会使结果变小一样，G/H 是一个比 G 小得多的空间。在更小的空间上进行卷积，效率自然更高。

我们将其应用于点云处理。以 SO(3) 等变为例，我们取子群 H 为 SO(2)（平面旋转），则商空间 SO(3)/SO(2) 同构于球面 S^2。这相当于将绕同一轴的所有旋转归为一组，用球面上的一个点来代表。这样，我们将自由度从 6 降到了 5，空间变小了。

在实践中，我们使用一个多面体（如二十面体，12个顶点）来离散化球面。完整的 SO(3) 旋转群（对该多面体的对称旋转）有 60 个元素。而商空间（即顶点）只有 12 个元素。通过这种商操作，我们将特征图的维度从 60 降到了 12。

关键在于，尽管我们只在 12 维特征上操作，但我们仍然可以恢复 60 种旋转。这是因为，如果我们以某种方式排列这 12 个顶点成一个向量，那么多面体的每一种旋转都会导致该向量的一个特定排列。因此，12 个元素的特征图通过其排列可以表示 60 种旋转。

这种策略带来了巨大的计算成本节省。在物体姿态估计、分类、关键点匹配等任务上的实验表明，相比标准的 SE(3) 等变群卷积，商空间卷积能大幅降低内存消耗，并将训练和推理速度提升数倍。

高效等变模型的大规模应用

效率的提升使得我们可以将 E(3) 等变学习应用于大规模点云，例如激光雷达数据。

  • 地点识别：也称为回环检测。激光雷达基于的地点识别对光照和天气变化更鲁棒。我们应用高效的 SE(3) 点卷积来从不同姿态下的点云中提取可靠的全局描述符。等变模型能提取出在姿态变化下非常稳定的描述符。
  • 全景分割：这是一个结合了语义分割、实例分割和对象跟踪的任务。我们的实验表明，等变学习能够实现更准确的对象分割和跟踪，从而有益于整体的全景分割性能。

总结与未来展望

本节课我们一起学习了如何从典型的网络架构（MLP 和 CNN）中学习等变特征，并看到了它们在机器人领域的众多应用。

等变学习虽然是一个几何概念，但由于它能将几何变化（姿态变化）从学习中分离出来，因此不仅有利于几何任务（如配准、地点识别），也有利于语义任务（如分割、检测）。结合两者，可以带来更有趣的应用，如物体 SLAM 和抓取操作。

展望未来，一个自然的方向是扩展对称性保持模型所能处理的情况范围。这样，更多的环境变化可以通过原则性的方式处理，从而减少处理其他变化（如不同实体）所需的数据量和模型大小。

未来的探索方向可能包括：

  1. 其他群对称性：例如单应性变换，它描述了 3D 变换在 2D 图像上的投影。处理这类对称性可能实现更鲁棒的从图像到 3D 的感知。
  2. 近似等变性：数据通常被噪声和遮挡破坏，导致不完全服从等变性。如何在这种情况下利用近似对称性？
  3. 从数据中发现对称性：当前模型大多是我们预先定义想要的对称性。未来可能希望模型能从数据本身学习对称性。
  4. 在机器人应用中更深入地整合对称性：作为机器人研究者，有更多可能性去探索如何将对称性整合到机器人应用中。

本节课到此结束，希望这些内容能帮助你理解等变学习在机器人 3D 感知中的强大潜力。

posted @ 2026-03-26 13:10  布客飞龙IV  阅读(19)  评论(0)    收藏  举报