第27章 机器人学

摘要：本章系统讲解机器人学的核心理论与算法。机器人是能感知、计算并对物理世界施加作用的自主体，其核心架构是"感知—规划—执行"闭环。本章首先引入**位形空间（C-space）*这一关键抽象，将带形状的机器人在工作空间中的避障运动转化为 C-space 中一个点的路径搜索问题；随后介绍运动规划的两大类方法——基于图搜索的 A 与基于采样的 RRT/PRM，并讨论势场法与轨迹规划；在控制层面，讲解 PID 反馈控制及其进阶方法（计算力矩控制、阻抗控制、MPC）；在感知层面，系统阐述贝叶斯滤波框架下的定位（粒子滤波/MCL）与 SLAM（EKF-SLAM、FastSLAM、图优化 SLAM）；最后介绍感知—规划—执行闭环的三种经典机器人体系结构（分层式、反应式、混合式），并延伸讨论 Moravec 悖论、具身智能的数据瓶颈，以及 LLM 作为机器人“高层大脑”的机遇与安全风险。

对应《人工智能：现代方法（第4版）》Chapter 27 — Robotics
机器人学是 AI 的“集大成”章节：它把感知（第26章）、推理、规划（第3–11章）、不确定性下的决策（第12–17章）与学习（第19–22章）整合进一个必须与物理世界实时交互的完整系统。

---

1. 章节概述

机器人（robot）是能感知环境、进行计算、并对物理世界施加作用的自主体（agent）。相比纯软件 AI，机器人面临三重额外挑战：

1. 连续状态与动作空间：位置、速度、力矩都是实数，无法枚举。经典的离散搜索（A*、逻辑推理）必须重新设计。
2. 不确定性无处不在：传感器有噪声、执行器有误差、环境模型不精确、世界会变化。部分可观测是常态。
3. 实时性与不可逆性：机器人不能“想清楚再动”——控制回路通常要求毫秒级响应；而且撞坏的东西无法撤销（与软件的“重跑一次”根本不同）。

本章的核心架构是 感知—规划—执行闭环（sense–plan–act loop）：

                    ┌──────────────────────────────────┐
                    ↓                                  │
   传感器 ──► 感知/状态估计 ──► 规划/决策 ──► 控制 ──► 执行器 ──► 物理世界
   (相机/LiDAR/     (滤波、SLAM、    (运动规划、   (PID、    (电机、      │
    IMU/编码器/      定位、目标识别)   任务规划)    阻抗控制)  夹爪)       │
    力传感器)                                                            │
        ↑────────────────────────────────────────────────────────────────┘
                              环境反馈

三个层次的时间尺度截然不同：

• 控制层（100 Hz–1 kHz）：PID、力控，反应式，无模型或简单模型
• 规划层（1–10 Hz）：路径规划、轨迹生成，需要地图与目标
• 任务层（0.1 Hz 或更慢）：任务分解、符号规划、人机交互

本章的核心概念工具是 位形空间（configuration space, C-space）——它把“带形状的机器人在工作空间中避障运动”这一几何问题，转化为“C-space 中的一个点做路径搜索”的标准搜索问题，从而让第 3 章的搜索算法得以复用。这是全章最重要的抽象。

---

2. 关键概念与定义

2.1 硬件与运动学

概念	定义
执行器（effector / actuator）	对环境施加物理作用的部件：电机、液压缸、夹爪
自由度（degrees of freedom, DOF）	独立可控的运动维度。刚体在三维空间有 6 DOF（3 平移 + 3 旋转）
可控自由度 vs 有效自由度	汽车有 2 个可控 DOF（油门、方向盘）但 3 个有效 DOF（x, y, θ）→ 非完整约束
非完整约束（nonholonomic）	对速度的约束，不可积分为位形约束。如汽车不能横向平移：$\dot{x}\sin \theta -\dot{y}\cos \theta =0$
运动学（kinematics）	只考虑几何，不考虑力
动力学（dynamics）	考虑质量、惯性、力矩：$M(q)\ddot{q}+C(q,\dot{q})\dot{q}+G(q)=\tau$
正运动学（forward kinematics）	关节角 → 末端位姿，$\mathbf{x}=f(\mathbf{q})$，唯一解
逆运动学（inverse kinematics, IK）	末端位姿 → 关节角，$\mathbf{q}=f^{-1}(\mathbf{x})$，可能无解、多解或无穷解
雅可比矩阵（Jacobian）	$J(\mathbf{q})=\partial f/\partial \mathbf{q}$，联系关节速度与末端速度：$\dot{\mathbf{x}}=J\dot{\mathbf{q}}$
奇异位形（singularity）	$J$ 降秩处，某些方向瞬时失去运动能力（如手臂完全伸直）
冗余机械臂（redundant）	DOF > 任务维数（如 7 DOF 臂做 6 DOF 任务），有零空间可用于避障/优化
末端执行器（end effector）	手爪、吸盘、工具

传感器分类：

• 本体感受（proprioceptive）：编码器、IMU、力矩传感器 — 测机器人自身状态
• 外部感受（exteroceptive）：相机、LiDAR、声呐、触觉 — 测环境
• 主动 vs 被动：LiDAR/声呐主动发射能量；相机被动接收

2.2 位形空间与规划

概念	定义
位形（configuration）$q$	完全确定机器人所有点位置的最小参数集合
位形空间 $\mathcal{C}$	所有位形构成的空间。平面移动机器人 $\mathcal{C}={\mathbb{R}}^2\times S^1$；$n$ 关节臂 $\mathcal{C}=(S^1{)}^n$（$n$ 维环面）
${\mathcal{C}}_{obs}$	与障碍物碰撞的位形集合（障碍在 C-space 中膨胀）
${\mathcal{C}}_{free}$	$\mathcal{C}\setminus {\mathcal{C}}_{obs}$，自由位形空间
路径（path）vs 轨迹（trajectory）	路径是 ${\mathcal{C}}_{free}$ 中的几何曲线；轨迹是带时间参数化的路径 $q(t)$
完备性（completeness）	有解必能找到、无解必能报告
概率完备（probabilistically complete）	样本数 → ∞ 时找到解的概率 → 1（RRT、PRM）
分辨率完备（resolution complete）	网格足够细时完备（栅格 A*）
工作空间（workspace）	末端执行器能到达的物理三维区域

2.3 不确定性、定位与控制

概念	定义
定位（localization）	已知地图，估计机器人位姿
建图（mapping）	已知位姿，构建环境地图
SLAM	同时定位与建图，两者都未知，循环依赖问题
置信状态（belief state）	位姿的概率分布 $bel(x_t)=P(x_t\mid z_{1:t},u_{1:t})$
运动模型	$P(x_t\mid x_{t-1},u_t)$，含执行噪声
观测模型	$P(z_t\mid x_t,m)$，含传感噪声
里程计（odometry）	由轮编码器积分推算位移；误差随时间无界累积（drift）
回环检测（loop closure）	识别"回到曾经来过的地方"，用于消除累积误差
数据关联（data association）	判断当前观测对应地图中哪个地标；错误关联会导致灾难性发散
控制律（control law）	$u=\pi (x)$ 或 $u=\pi (bel)$，从状态到控制量的映射
参考轨迹与误差	$e(t)=q_{ref}(t)-q(t)$
稳定性 / BIBO	有界输入产生有界输出；李雅普诺夫稳定性

---

3. 核心理论与算法

3.1 机器人硬件与运动学基础

3.1.1 运动学链与位姿表示

刚体位姿用齐次变换矩阵表示：
$$T=\left[\begin{array}{cc}R& \mathbf{t}\\ 0^{\top }& 1\end{array}\right]\in SE(3),R\in SO(3),\mathbf{t}\in {\mathbb{R}}^3$$

串联机械臂的正运动学是变换矩阵的连乘（DH 参数法）：
$$T_0^n(\mathbf{q})=T_0^1(q_1)T_1^2(q_2)\cdots T_{n-1}^n(q_n)$$

旋转的表示对比：

表示	参数数	优点	缺点
旋转矩阵	9（6 约束）	无奇异、易复合	冗余、数值漂移需正交化
欧拉角	3	直观	万向锁（gimbal lock）、不唯一
轴角	3	紧凑	π 附近奇异
四元数	4（1 约束）	无奇异、插值平滑（SLERP）、数值稳定	双覆盖（$q$ 与 $-q$ 同一旋转）

实践中：内部用四元数或旋转矩阵，界面用欧拉角。

3.1.2 逆运动学求解

• 解析解：满足 Pieper 条件（最后三轴交于一点）的 6-DOF 臂有闭式解，快且能枚举所有解（通常 8 组）。

• 数值解（雅可比迭代）：
  $$\Delta \mathbf{q}=J^{†}\Delta \mathbf{x},J^{†}=J^{\top }(J{J}^{\top }{)}^{-1}\text{(伪逆)}$$ 奇异点附近 $J^{†}$ 爆炸，用阻尼最小二乘（DLS / Levenberg–Marquardt）：
  $$\Delta \mathbf{q}=J^{\top }(J{J}^{\top }+{\lambda }^{2}I{)}^{-1}\Delta \mathbf{x}$$
  牺牲少量精度换取数值稳定。

• 冗余机械臂的零空间投影：
  $$\dot{\mathbf{q}}=J^{†}\dot{\mathbf{x}}+(I-J^{†}J){\dot{\mathbf{q}}}_0$$
  第二项在不影响末端运动的前提下优化次要目标（避关节限位、避障、最大化可操作度）。

局限性：IK 只解几何，不考虑动力学与碰撞；多解选择需额外准则（最小关节移动、避免构型突变）。

---

3.2 位形空间（Configuration Space）

核心思想：把“一个有形状、有朝向的机器人在有障碍的工作空间中运动”，转化为“C-space 中的一个点在 ${\mathcal{C}}_{free}$ 中运动”。

关键转换：障碍物膨胀
在工作空间中，机器人是一个多边形/多面体，碰撞检测很复杂。在 C-space 中，把障碍物按机器人形状做 Minkowski 和膨胀：
Cobs={q:A(q)∩O≠∅}=O⊕(−A(0))\mathcal{C}_{obs} = \{q : \mathcal{A}(q)\cap \mathcal{O} \ne \emptyset\} = \mathcal{O}\oplus(-\mathcal{A}(0))Cobs​={q:A(q)∩O=∅}=O⊕(−A(0))
（对平移机器人成立）。膨胀后机器人退化为一个点，规划变成纯粹的"点在自由空间中找路径"。

例：二连杆平面机械臂

工作空间 (物理)                  位形空间 C = S¹ × S¹ (环面)
   ┌──────────────┐              θ2 ↑
   │      ███     │             2π ┌──────────────┐
   │   ╱          │                │ ▓▓▓    ▓▓▓▓  │ ← 障碍在 C-space
   │  ╱  ███████  │                │  ▓▓        ▓ │   中变成不规则区域
   │ ╱            │                │      ▓▓▓▓▓   │
   │●──── 基座     │              0 └──────────────┘→ θ1
   └──────────────┘                 0            2π
   两根连杆，关节角 (θ1,θ2)         每个点 = 一个位形

要点（这是本章最需要理解的图）：

1. 工作空间中简单的凸障碍，在 C-space 中会变成形状复杂、可能非连通的区域。
2. C-space 的拓扑很重要：转动关节使 $\mathcal{C}$ 成为环面（$\theta =0$ 与 $\theta =2\pi$ 是同一点），路径可以“绕出去”。
3. 维数灾难：$n$ 个关节 → $n$ 维 C-space。7 DOF 臂若每维离散 100 格，就是 $10^{14}$ 个格子——显式构造 C-space 完全不可行。这直接推动了基于采样的规划（§3.3.2）。
4. C-space 通常不显式计算，只提供一个“碰撞检测器” CollisionFree(q)→{true,false}\text{CollisionFree}(q)\to\{\text{true},\text{false}\}CollisionFree(q)→{true,false} 作为黑盒查询。

非完整约束的处理：汽车的 $\mathcal{C}={\mathbb{R}}^2\times S^1$ 是 3 维，但瞬时只能沿 2 维运动（前进/转向）。${\mathcal{C}}_{free}$ 中的直线连接未必可行——必须用 Dubins 曲线、Reeds-Shepp 曲线或运动学积分（kinodynamic planning）来连接两个位形。

---

3.3 运动规划（Motion Planning）

3.3.1 基于图搜索的方法：A*

把 ${\mathcal{C}}_{free}$ 离散化（栅格、可视图、Voronoi 图、cell decomposition），再用第3章的 A*。

$$f(n)=g(n)+h(n)$$

• $g(n)$：起点到 $n$ 的实际代价
• $h(n)$：$n$ 到目标的启发式估计，需可采纳（admissible） $h(n)\le h^{\ast }(n)$ 且一致（consistent） $h(n)\le c(n,n^{\prime })+h(n^{\prime })$
• 常用 $h$：欧氏距离（8-邻域用对角距离，4-邻域用曼哈顿距离）

function A-STAR(q_start, q_goal, grid) returns 路径 or failure
  open   <- 优先队列, 按 f 排序; PUSH(q_start, f = h(q_start))
  g[q_start] <- 0;  came_from <- {}
  closed <- {}
  while open 非空:
      n <- POP-MIN(open)
      if n == q_goal: return RECONSTRUCT-PATH(came_from, n)
      closed.add(n)
      for each neighbor m of n:
          if m ∈ closed or COLLIDES(m): continue
          tentative_g <- g[n] + cost(n, m)
          if tentative_g < g[m] (或 m 未访问):
              g[m] <- tentative_g
              came_from[m] <- n
              PUSH/UPDATE(open, m, f = g[m] + h(m))
  return failure

变体：

• Dijkstra：$h\equiv 0$，无方向偏好，探索面积大。
• 加权 A（$f=g+\epsilon h$，$\epsilon >1$）*：牺牲最优性（解不超过 $\epsilon$ 倍最优）换取巨大加速。
• D / D Lite**：环境动态变化时增量重规划，只修复受影响部分，是火星车等实际系统的标准选择。
• Field D / Theta**：允许任意角度路径，消除栅格路径的"锯齿"。
• Hybrid A*：状态含朝向，扩展时用车辆运动学模型生成后继，用于自动驾驶泊车。

适用场景：2D/3D 低维空间、需要最优性保证、地图已知且相对静态。
局限性：

1. 维数灾难：状态数 $O(k^d)$，超过 4–5 维不可行 → 机械臂规划基本不用栅格 A*。
2. 栅格分辨率与最优性、内存的三难权衡。
3. 路径贴着障碍走（栅格最短路径），需后处理平滑与安全裕度（膨胀障碍）。

3.3.2 基于采样的方法：RRT 与 PRM

核心洞察：既然 ${\mathcal{C}}_{free}$ 无法显式表示，就随机采样 + 碰撞检测，用一张稀疏的图/树来“近似”连通结构。牺牲完备性与最优性，换取对高维空间的可扩展性。

RRT（Rapidly-exploring Random Tree）

function RRT(q_init, q_goal, K, step_size δ) returns 路径 or failure
  T <- 以 q_init 为根的树
  for k = 1 to K:
      # 1. 采样（以概率 p≈0.05~0.1 直接采目标，做 goal bias）
      q_rand <- (random() < p) ? q_goal : SAMPLE-FREE(C)

      # 2. 找树上最近节点
      q_near <- NEAREST(T, q_rand)                 # 用 KD-tree 加速

      # 3. 沿方向前进一小步
      q_new  <- STEER(q_near, q_rand, δ)           # 非完整系统在此积分运动模型

      # 4. 碰撞检测（离散检查 q_near→q_new 线段）
      if COLLISION-FREE(q_near, q_new):
          T.add_vertex(q_new); T.add_edge(q_near, q_new)
          if DIST(q_new, q_goal) < δ and COLLISION-FREE(q_new, q_goal):
              return PATH(T, q_init, q_goal)
  return failure

为什么“快速探索”？ NEAREST 使得采样点落在任何区域时，最近的树节点往往位于树的边界上。因此树天然朝着“尚未探索的大片空白区域”生长——树的顶点分布会收敛到采样分布（Voronoi 偏置）。

重要变体：

• RRT-Connect / 双向 RRT：从起点和终点同时生长两棵树，尝试连接。实践中最快，工业界常用。
• RRT*：渐进最优。加入两步改进：
  1. choose parent：在 $q_{new}$ 的半径 $r$ 邻域内选使 $\text{cost}(q_{new})$ 最小的父节点，而非最近节点；
  2. rewire：检查邻域内其他节点能否经 $q_{new}$ 获得更低代价，若能则改接。
     邻域半径按 r=min⁡{γ(log⁡n/n)1/d, δ}r=\min\{\gamma(\log n / n)^{1/d},\ \delta\}r=min{γ(logn/n)1/d, δ} 收缩，保证 $n\to \infty$ 时代价收敛到最优。
• Informed RRT*：找到初解后，只在以起点终点为焦点的超椭球内采样（该椭球是所有可能改进解的集合），收敛显著加快。
• Kinodynamic RRT：STEER 用动力学积分，直接在状态空间（含速度）规划。

PRM（Probabilistic Roadmap）：多查询方法。

学习阶段: 随机采 N 个无碰撞位形作为节点；
          每个节点与其 k 近邻尝试用局部规划器（直线）连接；
          得到一张覆盖 C_free 的路线图 (roadmap)。
查询阶段: 把 q_start, q_goal 连入路线图，在图上跑 A*/Dijkstra。

RRT vs PRM 对比：

	RRT	PRM
查询类型	单查询（single-query）	多查询（multi-query），一次建图反复用
结构	树（有向，从起点长出）	无向图
适合	动态环境、每次目标不同、有微分约束	静态环境、同一环境大量查询
弱点	路径曲折需后处理平滑；非最优	建图开销大；窄通道采样困难

共同局限性：

1. 窄通道问题（narrow passage）：细长通道被随机采到的概率极低，是采样方法的致命弱点。缓解：桥测试（bridge test）、障碍表面采样、高斯采样。
2. 只是概率完备——无解时永远不会终止，只能靠迭代上限判定失败。
3. 路径质量差，需后处理：短路平滑（shortcutting，随机取两点尝试直连）、样条拟合。
4. 碰撞检测占总时间的 90%+，是主要瓶颈。

3.3.3 势场法（Potential Field）

构造势函数：目标产生引力，障碍产生斥力，机器人沿负梯度下降。
$$U(q)=U_{att}(q)+U_{rep}(q),\mathbf{F}(q)=-\nabla U(q)$$
$$U_{att}=\frac{1}{2}{k}_a\Vert q-q_{goal}{\Vert }^2,U_{rep}=\{\begin{array}{ll}\frac{1}{2}{k}_r{\left(\frac{1}{\rho (q)}-\frac{1}{{\rho }_0}\right)}^2& \rho (q)\le {\rho }_0\\ 0& \rho (q)>{\rho }_0\end{array}$$ 其中 $\rho (q)$ 为到最近障碍的距离。

优点：计算极快、天然反应式、可用于实时避障（与全局规划器配合，作为局部规划器）。
致命缺陷：局部极小值——机器人可能卡在凹形障碍前来回震荡。解决：随机扰动、导航函数（navigation function，构造无局部极小的势场，但计算昂贵）、或只作为局部层配合全局规划。

3.3.4 轨迹规划（Trajectory Generation）

路径规划输出几何路径，轨迹规划为其分配时间参数化 $q(t)$，需满足：

• 运动学约束：$|\dot{q}|\le v_{max}$，$|\ddot{q}|\le a_{max}$，$|\stackrel{...}{q}|\le j_{max}$（jerk 影响舒适度与机械磨损）
• 动力学约束：$|\tau |\le {\tau }_{max}$
• 平滑性：位置、速度、加速度连续（$C^2$ 连续）

常用方法：

1. 多项式插值。五次多项式可满足起终点的位置/速度/加速度共 6 个条件：
   $$q(t)=a_0+a_{1}t+a_2{t}^2+a_3{t}^3+a_4{t}^4+a_5{t}^5$$

2. 梯形速度剖面（trapezoidal profile）：匀加速—匀速—匀减速，时间最优（在加速度约束下），但加速度不连续（jerk 无穷）。改进为 S 曲线（S-curve），限制 jerk。

3. 样条曲线：三次 B 样条、Bezier 曲线，保证连续性且具局部可调性。

4. 最小化 snap/jerk 的优化：无人机航迹常用
   $$\underset{q(t)}{\min }{\int }_0^T{\Vert \frac{d^{4}q}{d{t}^4}\Vert }^{2}dt\text{s.t. 途经点约束、走廊约束}$$
   这是二次规划（QP），可高效求解。

5. 时间最优路径参数化（TOPP）：给定几何路径，求沿路径的最快时间律，是凸优化问题。

现代做法：轨迹优化直接替代"路径+参数化"两步

• CHOMP：用协变梯度下降优化含障碍代价与平滑代价的泛函
  $$\mathcal{U}[\xi ]=\underset{\text{平滑项}}{\underbrace{\int \Vert \ddot{\xi }(t){\Vert }^{2}dt}}+\lambda \underset{\text{障碍代价，用有符号距离场}}{\underbrace{\int c(\xi (t))\Vert \dot{\xi }(t)\Vert dt}}$$
• TrajOpt：序列凸优化 + 连续碰撞检测（防止"穿墙"）
• MPC（模型预测控制）：每个控制周期在有限时域内求解优化问题，只执行第一步，然后重新规划。这是规划与控制的统一框架，在自动驾驶和足式机器人中已成主流。
  $$\underset{u_{0:H-1}}{\min }\sum _{k=0}^{H-1}\ell (x_k,u_k)+{\ell }_f(x_H)\text{s.t. }{x}_{k+1}=f(x_k,u_k),x_k\in \mathcal{X},u_k\in \mathcal{U}$$

---

3.4 反馈控制与 PID

开环 vs. 闭环：开环控制（预先算好指令直接执行）在有模型误差、外部扰动时必然失败——机器人必须用传感器反馈不断修正。

PID 控制器

误差 $e(t)=q_{ref}(t)-q(t)$，控制量：
$$\boxed{u(t)=K_{p}e(t)+K_i{\int }_0^{t}e(\tau )d\tau +K_d\frac{de(t)}{dt}}$$

三项的物理意义：

项	作用	增大的效果	过大的后果
P（比例）$K_p$	对当前误差成比例响应	响应更快，稳态误差减小	超调、振荡、失稳
I（积分）$K_i$	累积历史误差，消除稳态误差	消除常值扰动（如重力、摩擦）造成的偏差	积分饱和（anti-windup）、相位滞后、振荡
D（微分）$K_d$	预测误差趋势，提供阻尼	抑制超调、加快稳定	放大传感器噪声

为什么需要 I 项？ 纯 P 控制在存在恒定负载（如机械臂对抗重力）时，必须保持一个非零误差才能产生足够的控制量，即稳态误差恒存在。I 项通过累积误差持续增大输出，直到误差归零。

为什么 D 项能减振？ P 控制类似弹簧（$F=-kx$），系统是无阻尼振荡器；D 项提供阻尼力（$F=-c\dot{x}$），相当于加装阻尼器。目标是达到临界阻尼：最快无超调地收敛。二阶系统的行为由阻尼比决定：
$$\zeta <1\text{ 欠阻尼(振荡)},\zeta =1\text{ 临界阻尼(最优)},\zeta >1\text{ 过阻尼(缓慢)}$$

function PID-CONTROL(setpoint_fn, measure_fn, Kp, Ki, Kd, dt, u_min, u_max)
  integral <- 0
  prev_error <- 0
  loop every dt seconds:                        # 固定周期，如 1 kHz
      y   <- measure_fn()
      e   <- setpoint_fn() - y

      P   <- Kp * e

      # 积分项（含抗饱和）
      integral <- integral + e * dt
      I <- Ki * integral

      # 微分项（对测量值微分而非误差，避免 setpoint 突变引起微分冲击）
      D <- -Kd * (y - prev_y) / dt
      # 对 D 做低通滤波以抑制噪声： D <- α*D + (1-α)*D_prev

      u_raw <- P + I + D
      u     <- CLAMP(u_raw, u_min, u_max)       # 执行器饱和

      # 抗积分饱和 (anti-windup): 输出饱和时停止/回退积分
      if u != u_raw:
          integral <- integral - e * dt         # 条件积分法

      APPLY(u)
      prev_y <- y

调参方法：

• Ziegler–Nichols：增大 $K_p$ 直到持续等幅振荡，记临界增益 $K_u$ 与周期 $T_u$，则 $K_p=0.6K_u$，$K_i=1.2K_u/T_u$，$K_d=0.075K_u{T}_u$。快速但通常偏激进。
• 手动：先 P（调到略有振荡），再 D（消振荡），最后 I（消稳态误差）。

适用场景：单输入单输出（SISO）、近似线性、模型未知或简单的回路。工业界 90% 以上的控制回路仍是 PID，因其无需模型、参数少、鲁棒、易理解。

局限性：

1. 本质是线性、单变量控制。多关节机械臂存在强耦合（一个关节动会给另一个施加惯性力矩），独立 PID 会互相干扰。
2. 不处理约束（关节限位、力矩上限）、不做前瞻。
3. 对非线性（摩擦、重力随位形变化、柔性）适应差。

进阶控制方法：

• 计算力矩控制（computed torque / 逆动力学控制）：用动力学模型抵消非线性，再用 PD 控制残差
  $$\tau =M(q)({\ddot{q}}_{ref}+K_d\dot{e}+K_{p}e)+C(q,\dot{q})\dot{q}+G(q)$$ 若模型精确，误差动力学变为线性 $\ddot{e}+K_d\dot{e}+K_{p}e=0$，可任意配置极点。
• 阻抗/导纳控制：不控制位置或力，而控制两者的动态关系 $F=M_d\ddot{e}+B_d\dot{e}+K_{d}e$，使机器人表现得像一个可调的弹簧-阻尼系统。这是**人机安全交互与接触任务（打磨、装配）**的关键。
• LQR：线性系统二次代价的最优状态反馈 $u=-Kx$，$K$ 由 Riccati 方程解出。
• MPC：显式处理约束与前瞻，见 §3.3.4。

---

3.5 定位与粒子滤波

3.5.1 贝叶斯滤波框架

所有定位算法都是递归贝叶斯滤波的实例（与第 14 章完全一致）：

$$\underset{\text{预测（运动更新）：不确定性增大}}{\underbrace{\overline{bel}(x_t)=\int P(x_t\mid u_t,x_{t-1})bel(x_{t-1})d{x}_{t-1}}}$$
$$\underset{\text{校正（观测更新）：不确定性减小}}{\underbrace{bel(x_t)=\eta P(z_t\mid x_t)\overline{bel}(x_t)}}$$

不同实现的区别只在于如何表示 $bel$：

滤波器	置信表示	假设	优缺点
卡尔曼滤波（KF）	高斯 $(\mu ,\Sigma )$	线性 + 高斯	最优、$O(d^3)$、单峰
扩展卡尔曼滤波（EKF）	高斯	一阶泰勒线性化	通用；强非线性时发散
无迹卡尔曼滤波（UKF）	高斯	sigma 点确定性采样	无需雅可比，精度更高
直方图滤波	离散栅格	无	可多峰；维度诅咒
粒子滤波（PF）	加权样本集	任意分布、任意非线性	可多峰、易实现；粒子退化、高维需大量粒子

3.5.2 蒙特卡洛定位（MCL / 粒子滤波定位）

用 $M$ 个带权粒子 $\{(x_t^{[i]},w_t^{[i]}){\}}_{i=1}^M$ 近似后验分布。

function MONTE-CARLO-LOCALIZATION(X_{t-1}, u_t, z_t, map m, M) returns X_t
  X̄_t <- ∅ ;  X_t <- ∅

  # ---- 1. 预测：按运动模型传播每个粒子（含噪声）----
  for i = 1 to M:
      x_t^[i] <- SAMPLE-MOTION-MODEL(u_t, x_{t-1}^[i])   # 采样，不是取均值
      # ---- 2. 校正：用观测似然作为重要性权重 ----
      w_t^[i] <- MEASUREMENT-MODEL(z_t, x_t^[i], m)      # P(z_t | x_t, m)
      X̄_t.add( (x_t^[i], w_t^[i]) )

  归一化权重使 Σ w = 1

  # ---- 3. 重采样：按权重有放回抽取 M 个粒子 ----
  # 只在有效样本数 N_eff = 1 / Σ(w^[i])² 低于阈值(如 M/2)时执行，减少方差
  if N_eff < M/2:
      X_t <- LOW-VARIANCE-RESAMPLE(X̄_t, M)   # 权重归一为 1/M
  else:
      X_t <- X̄_t
  return X_t

低方差重采样（systematic resampling）：只抽一个随机数 $r\sim U[0,1/M)$，然后按 $r+(i-1)/M$ 等间隔"梳"过累积权重分布。相比独立抽 $M$ 次，它 $O(M)$ 且方差更小，是标准做法。

运动模型（差速轮机器人的里程计模型）：把里程计读数分解为 旋转₁ → 平移 → 旋转₂，每项加入与自身及其他项成比例的噪声：
$${\hat{\delta }}_{rot1}={\delta }_{rot1}-\mathcal{N}(0,{\alpha }_1{\delta }_{rot1}^2+{\alpha }_2{\delta }_{trans}^2)$$
$${\hat{\delta }}_{trans}={\delta }_{trans}-\mathcal{N}(0,{\alpha }_3{\delta }_{trans}^2+{\alpha }_4({\delta }_{rot1}^2+{\delta }_{rot2}^2))$$

观测模型（激光雷达的 beam model）是四个分量的混合：
$$P(z\mid x,m)=z_{hit}{p}_{hit}+z_{short}{p}_{short}+z_{max}{p}_{max}+z_{rand}{p}_{rand}$$

• $p_{hit}$：以射线投射（ray casting）的期望距离为中心的高斯 — 正确测量
• $p_{short}$：指数分布 — 未建模的动态障碍（行人）
• $p_{max}$：$z_{max}$ 处的尖峰 — 未击中任何物体
• $p_{rand}$：均匀分布 — 随机噪声
  这个混合模型使定位对动态环境与异常值鲁棒，是 MCL 实用化的关键。likelihood field 模型是更快的近似。

MCL 的三大能力：

1. 位姿跟踪（position tracking）：初始已知，粒子集中。
2. 全局定位（global localization）：初始完全未知，粒子均匀撒满地图，会先呈多峰（对称走廊中有多个假设），随观测积累坍缩到单峰。这是 KF 做不到的。
3. 绑架机器人问题（kidnapped robot）：机器人被突然搬走。标准 MCL 会失败（正确位姿附近已无粒子）。解决：Augmented MCL — 监控短期与长期平均权重之比，若观测持续意外则注入随机粒子。

粒子数自适应：KLD 采样 — 根据粒子占据的栅格数动态决定 $M$，全局定位时用几万个，收敛后降到几百个，大幅节省算力。

局限性：

1. 粒子退化/耗尽（particle depletion）：重采样会丢弃低权重粒子，若真实位姿附近的粒子被误删则无法恢复。
2. 维度诅咒：所需粒子数随状态维度指数增长。适合 3 DOF 定位，不适合直接做高维 SLAM（需 Rao-Blackwellization）。
3. 观测模型过于"尖锐"（高精度传感器）时，几乎所有粒子权重为 0 → 需要更多粒子或膨胀噪声。
4. 计算量随粒子数线性增长。

---

3.6 SLAM（同时定位与建图）

问题：既无地图也不知位姿，需同时估计
$$P(x_{1:t},m\mid z_{1:t},u_{1:t})$$
这是“鸡生蛋”：定位需要地图，建图需要位姿。关键：两者的误差是相关的，必须联合估计。

3.6.1 EKF-SLAM（在线 SLAM）

状态向量把机器人位姿与所有地标坐标拼在一起：
$${\mathbf{y}}_t=(\underset{\text{位姿}}{\underbrace{x,y,\theta }},\underset{N\text{个地标}}{\underbrace{m_{1,x},m_{1,y},\dots ,m_{N,x},m_{N,y}}}{)}^{\top }$$ 用 EKF 维护 $({\mathbf{\mu }}_t,{\Sigma }_t)$。

核心洞察：协方差矩阵 $\Sigma$ 的非对角块编码了地标间的相关性。当机器人重新观测到一个旧地标时，通过这些相关性，整张地图的所有地标位置都会被同时修正——这正是回环检测能消除累积误差的数学机制。

局限性：$\Sigma$ 是 $(3+2N)\times (3+2N)$ 矩阵，更新为 $O(N^2)$，地标数上千即不可行；线性化误差累积导致不一致；对数据关联错误极度敏感（一次错误关联可能使滤波器永久发散）。

3.6.2 FastSLAM（Rao-Blackwellized 粒子滤波）

关键因式分解：
$$P(x_{1:t},m\mid z_{1:t},u_{1:t})=P(x_{1:t}\mid z_{1:t},u_{1:t})\prod _{n=1}^{N}P(m_n\mid x_{1:t},z_{1:t})$$

含义：给定完整的机器人轨迹，各个地标是条件独立的（因为地标之间的相关性完全来自共同的位姿不确定性）。

因此：

• 用粒子滤波采样轨迹（每个粒子代表一条完整轨迹假设）；
• 每个粒子各自携带 $N$ 个独立的小 EKF（每个地标一个 2×2 的 EKF）。

复杂度从 $O(N^2)$ 降为 $O(M\log N)$（用平衡树共享地图结构）。且每个粒子有自己的数据关联假设，天然对关联错误鲁棒。

GMapping 是其栅格地图版本，是 ROS 生态中最广泛使用的 2D 激光 SLAM。

3.6.3 图优化 SLAM（GraphSLAM / 现代主流）

把 SLAM 建模为**因子图（factor graph）**上的最大后验估计：

• 节点：机器人各时刻位姿 $x_i$（与地标 $m_j$）
• 边（约束/因子）：里程计约束（相邻位姿）、观测约束（位姿-地标）、回环约束（时间上遥远但空间上相邻的两个位姿）

求解非线性最小二乘：
$${\mathbf{x}}^{\ast }=\arg \underset{\mathbf{x}}{\min }\sum _{⟨i,j⟩\in \mathcal{E}}{\mathbf{e}}_{ij}({\mathbf{x}}_i,{\mathbf{x}}_j{)}^{\top }{\Omega }_{ij}{\mathbf{e}}_{ij}({\mathbf{x}}_i,{\mathbf{x}}_j)$$
其中 ${\mathbf{e}}_{ij}$ 是预测与实测的差，${\Omega }_{ij}$ 是信息矩阵（协方差的逆，表示该约束的置信度）。用 Gauss-Newton 或 Levenberg-Marquardt 迭代求解，信息矩阵天然稀疏（每个位姿只与少数其他位姿有约束），用稀疏 Cholesky 分解可高效求解。

优于滤波方法之处：保留全部历史，可反复重线性化（消除线性化误差），可用鲁棒核函数（Huber、Cauchy）抑制错误回环的影响。代表系统：g2o、GTSAM、Ceres、ORB-SLAM、Cartographer、LIO-SAM。

3.6.4 现代 SLAM 系统架构

┌─────────────────── 前端 (Front-end) ───────────────────┐
│ 特征提取 (ORB/SIFT，见第26章)                            │
│ 帧间匹配 / 光流跟踪 → 视觉里程计 (VO)                     │
│ RANSAC 剔除外点 → 初始位姿估计                           │
│ 关键帧选择                                              │
└─────────────────────────┬──────────────────────────────┘
                          ↓
┌─────────────────── 后端 (Back-end) ────────────────────┐
│ 局部 BA (bundle adjustment): 联合优化局部位姿与三维点    │
│ 全局位姿图优化                                          │
└─────────────────────────┬──────────────────────────────┘
                          ↓
┌──────────── 回环检测 (Loop Closure) ───────────────────┐
│ 词袋模型 (DBoW) / 深度学习描述子 → 候选帧检索            │
│ 几何验证 (PnP + RANSAC)                                 │
│ 添加回环约束 → 触发全局优化，消除累积漂移                 │
└────────────────────────────────────────────────────────┘

多传感器融合：视觉（纹理丰富、尺度未知、怕光照）+ IMU（高频、短期精确、长期漂移）+ LiDAR（精确测距、怕退化环境如长走廊）+ GPS（全局无漂移、室内失效）。VIO/LIO 系统通过紧耦合优化互补各自弱点。

开放难题：

• 动态环境：SLAM 通常假设静态世界，行人车辆会污染地图（→ 动态物体剔除、多体 SLAM）。
• 长期自主（lifelong SLAM）：地图随季节、光照、家具移动而变化，需持续更新与遗忘机制。
• 退化场景：长走廊、白墙、隧道中几何约束不足。
• 语义 SLAM：把第 26 章的目标检测/分割结果融入地图，构建“这是椅子、那是门”的语义地图，是通向任务级自主的必要一步。

---

3.7 感知—规划—执行闭环与机器人体系结构

3.7.1 三种经典体系结构

1. 分层/审慎式（deliberative, sense-plan-act）

感知 → 建模 → 规划 → 执行  （串行，一次完整循环）

优点：能处理复杂长程任务、可解释。缺点：慢，环境变化时规划已过时（Shakey 机器人的教训）。

2. 反应式/包容架构（reactive, Brooks’ subsumption）

Layer 3: 探索                ─┐
Layer 2: 漫游                 ├─► 高层可抑制(suppress)低层输出
Layer 1: 避障                 │   每层直接感知→动作，无中央世界模型
Layer 0: 保持行走            ─┘

Brooks 的名言：“The world is its own best model.”
优点：极快、鲁棒、无需精确模型。缺点：无法处理需要前瞻的任务（不能"绕远路去拿钥匙再开门"）。

3. 混合式（hybrid / three-layer）—— 现代主流

┌─────────────────────────────────────────────┐
│ 审慎层 (Deliberative)  ~0.1 Hz              │
│   任务规划、符号推理、长期目标、LLM 任务分解  │
├─────────────────────────────────────────────┤
│ 执行层 (Executive/Sequencing)  ~1-10 Hz     │
│   行为树 / 有限状态机、监控执行、异常处理     │
│   运动规划 (RRT/A*)、重规划                  │
├─────────────────────────────────────────────┤
│ 反应层 (Reactive/Control)  ~100-1000 Hz     │
│   PID / MPC、紧急避障、安全停止              │
└─────────────────────────────────────────────┘

核心设计原则：时间尺度分离。越低层越快、越简单、越可靠（安全关键功能必须放在最低层，如碰撞后立即停机）；越高层越慢、越智能、越灵活。

反应式架构的深层局限：

1. 无法处理需要全局知识的任务：每层只依赖局部感知与即时动作，没有中央世界模型，因此无法进行需要全局视野的推理——例如"先探索整个房间、记住钥匙位置、再规划一条绕开障碍的取钥匙路线"。这类任务要求对环境的全局表征与跨时间推理，而反应式架构本质上"活在当下"。
2. 行为冲突难以设计：当多个行为同时被触发（如"避障"要求左转、"漫游"要求直行）时，subsumption 用固定的优先级仲裁，但优先级设计是手工的、脆弱的。复杂环境中行为间的交互可能产生不可预期的涌现行为，且难以调试与验证。

混合式架构如何结合两者优点：

混合式架构的核心洞察是：不同任务对"反应速度"与"推理深度"的需求不同，因此把两者放在不同时间尺度上并行运行，而非二选一。

• 审慎层（Deliberative）：负责需要全局知识与长期视野的任务——任务分解、符号规划、语义推理。它运行在 ~0.1 Hz，可以"慢慢想"，输出的是子目标序列而非具体动作。
• 执行层（Executive）：作为审慎层与反应层之间的翻译器与监督者。它把高层子目标转化为可执行的运动规划（RRT/A*），同时监控执行过程、处理异常（如规划失败、目标被遮挡），必要时触发重规划。运行在 1–10 Hz。
• 反应层（Reactive）：负责毫秒级的安全关键行为——PID 跟踪、紧急避障、力矩限制。它不依赖任何模型，直接感知→动作，保证系统在任何时刻都不会撞坏自己或伤人。

关键设计原则：时间尺度分离 + 安全下沉。越低层越快、越简单、越可靠；安全关键功能（碰撞停机、力矩限制）必须放在最低层，且拥有对上层输出的绝对否决权。这样既保留了反应式的鲁棒性，又获得了审慎式的全局智能。

实际系统示例：ROS 的 move_base 框架

ROS 的 move_base 是混合式架构最经典的工程实现，其内部正是三层结构：

┌─────────────────────────────────────────────────────┐
│ 审慎层 (全局规划器)  ~0.1-1 Hz                       │
│   global_planner: Dijkstra / A* / NavFn              │
│   在静态地图 (costmap) 上规划全局路径                 │
├─────────────────────────────────────────────────────┤
│ 执行层 (行为状态机)  ~1-10 Hz                        │
│   move_base 状态机:                                  │
│   全局规划 → 局部规划 → 执行 → 监控/重规划           │
│   检测到障碍阻塞时触发全局重规划                      │
├─────────────────────────────────────────────────────┤
│ 反应层 (局部规划器)  ~10-50 Hz                       │
│   base_local_planner: DWA / TEB / MPC                │
│   实时避障、速度约束、跟踪全局路径                    │
└─────────────────────────────────────────────────────┘

• 审慎层：global_planner 在静态地图上运行 A*/Dijkstra，输出一条全局路径。它"想得慢"，但能看到全局。
• 执行层：move_base 的状态机协调全局与局部规划器，监控执行进度；当局部规划器报告"前方被动态障碍堵死"时，触发全局重规划。
• 反应层：base_local_planner（如 DWA）以 10–50 Hz 实时计算速度指令，避开动态障碍、遵守速度与加速度约束，同时尽量跟踪全局路径。

这个框架完美体现了混合式架构的价值：全局规划保证任务可达，局部规划保证实时安全——两者通过执行层解耦，互不干扰。这也是为什么 move_base 成为移动机器人导航的事实标准。

3.7.2 不确定性下的闭环

理论上正确的做法是求解 POMDP（第 17 章）：在置信状态空间上做决策，$\pi :\mathcal{B}\to \mathcal{A}$。这能自动产生"信息搜集行为"（先转头看一眼再动）。但 POMDP 精确求解是 PSPACE-hard，实际机器人只能用近似：

• QMDP：假设下一步后完全可观测，$Q(b,a)={\sum }_{s}b(s)Q_{MDP}(s,a)$。快但不会主动探索。
• POMCP / DESPOT：基于蒙特卡洛树搜索的在线求解。
• 最常见的工程做法：确定性等价（certainty equivalence） — 用 $\hat{x}=\arg \max bel(x)$（或均值）替代分布，按确定性问题规划，靠高频重规划来补偿。这在实践中出奇地有效，但会系统性低估风险。

闭环的关键工程要素：

• 重规划频率：环境变化越快，重规划越频繁；D* Lite 等增量算法使其可行。
• 执行监控（execution monitoring）：持续检查"实际是否按预期发生"，检测到偏差（动作失败、物体滑落）时触发恢复行为。
• 安全层：独立于主控的硬件级急停、力矩限制、速度限制、虚拟围栏。功能安全（ISO 10218/15066）要求这一层可形式化验证。
• 实时性保证：控制回路必须硬实时（错过截止时间 = 失败），需实时操作系统与确定性调度。

3.7.3 机器人学习

---

4. 本章总结

本章以感知—规划—执行闭环为统摄主线，系统梳理了机器人学的核心脉络。起点是位形空间（C-space）这一关键抽象——它把带形状的机器人在工作空间中的避障运动，转化为 C-space 中一个点的路径搜索问题，从而让经典搜索算法得以复用。在此基础上，运动规划形成了两条互补路线：基于图搜索的 A（完备、最优、低维高效）与基于采样的 RRT/PRM（概率完备、可扩展至高维），辅以势场法做实时避障、轨迹规划分配时间参数化。控制层面从 PID 反馈控制出发，针对其线性、单变量的局限，引入计算力矩控制、阻抗控制与 MPC 等进阶方法。感知层面在贝叶斯滤波框架下，用粒子滤波/MCL* 解决定位问题，进而通过 SLAM（EKF-SLAM、FastSLAM、图优化）实现"无地图、无位姿"条件下的同时定位与建图。最终，三种经典体系结构（分层式、反应式、混合式）以不同方式组织这些模块，混合式架构通过时间尺度分离兼顾了全局智能与实时安全。感知—规划—执行闭环贯穿始终，是理解机器人系统设计的统摄性框架。

---

5. 思考与练习

概念题

1. 为什么在 C-space 中障碍物会"膨胀"？

因为 C-space 把机器人抽象为一个点，障碍物必须按机器人的形状做 Minkowski 和膨胀（${\mathcal{C}}_{obs}=\mathcal{O}\oplus (-\mathcal{A}(0))$），才能保证"点"与膨胀后障碍不碰撞等价于"原机器人"与原始障碍不碰撞。膨胀量等于机器人的"半径"；对旋转机器人，膨胀量随朝向 $\theta$ 变化，因此三维 C-space 的每个 $\theta$ 切片形状不同。

2. 为什么纯 P 控制存在稳态误差？

纯 P 控制的输出 $u=K_{p}e$ 与误差成正比。当存在恒定负载（如机械臂对抗重力）时，要维持一个非零输出才能平衡负载，就必须保持一个非零误差——即稳态误差恒存在。I 项通过累积历史误差持续增大输出，直到误差归零，因此能消除稳态误差。

算法题

3. 手写 RRT 的 NEAREST 步骤伪代码。

def NEAREST(T, q_rand):
    """
    在树 T 中找出距离采样点 q_rand 最近的节点。
    朴素实现 O(|T|)；大规模时用 KD-tree 加速到 O(log |T|)。
    """
    q_near = None
    d_min = float('inf')
    for q in T.vertices:          # 遍历树中所有节点
        d = DIST(q, q_rand)       # 通常用欧氏距离（或流形上的测地距离）
        if d < d_min:
            d_min = d
            q_near = q
    return q_near

4. 推导计算力矩控制的误差动力学方程。

计算力矩控制律为：
$$\tau =M(q)({\ddot{q}}_{ref}+K_d\dot{e}+K_{p}e)+C(q,\dot{q})\dot{q}+G(q)$$

代入机器人动力学方程 $M(q)\ddot{q}+C(q,\dot{q})\dot{q}+G(q)=\tau$，得：
$$M(q)\ddot{q}+C\dot{q}+G=M(q)({\ddot{q}}_{ref}+K_d\dot{e}+K_{p}e)+C\dot{q}+G$$

消去 $C\dot{q}+G$，左乘 $M^{-1}(q)$：
$$\ddot{q}={\ddot{q}}_{ref}+K_d\dot{e}+K_{p}e$$

整理得误差动力学方程：
$$\ddot{e}+K_d\dot{e}+K_{p}e=0$$

若模型精确，这是一个线性二阶系统，可通过选择 $K_p$、$K_d$ 任意配置极点（如临界阻尼 $\zeta =1$）。

开放讨论题

5. LLM 作为机器人“高层大脑”的机遇与风险。

• 机遇：LLM 提供开放世界的常识（“鸡蛋易碎，要轻拿”）与灵活的任务分解能力（把“准备早餐”拆为可执行子目标），弥补了传统审慎层在开放语义理解上的短板（如 SayCan 的语义相关性 × 物理可行性双评估）。

• 风险：LLM 的幻觉在具身场景从“质量问题”升级为“安全问题”——错误输出可能导致物理性、不可逆的伤害。语言模糊性（第24章）+ 感知不确定性（第26章）+ 执行不可逆性（本章）三者叠加构成风险复合。

• 讨论方向：如何把 LLM 严格限制在审慎层、只允许调用已验证的技能库？可验证的安全过滤器（如控制屏障函数）能否让“不可信的智能”与“可信的安全”共存？端到端 VLA 模型打破分层后，我们是否失去了插入安全检查的接口？

• 模仿学习 / 行为克隆：从人类演示学 $\pi (a\mid o)$。问题是分布偏移——策略一旦偏离演示分布就无法恢复（复合误差）。DAgger 通过让专家标注策略访问过的状态来缓解。

• 强化学习：在仿真中训练（第22章）。核心难题是 sim-to-real gap。缓解手段：域随机化（domain randomization）（随机化摩擦、质量、光照、纹理，迫使策略对参数不敏感）、系统辨识、真实数据微调。

• 端到端 vs 模块化：端到端（像素 → 力矩）简洁且能学到难以手工设计的技能，但样本效率低、不可解释、难以验证安全性；模块化可测试可调试，但模块间误差累积、接口设计限制性能。

• VLA（Vision-Language-Action）模型：把第25、26章的预训练模型接到机器人上，用 Transformer 统一处理图像、语言指令与动作 token（RT-2、OpenVLA 等）。这代表了机器人学与基础模型的融合方向。

---

4. 关键图示 / 表格说明

4.1 位形空间转换图（本章最重要的图）

   工作空间 (Workspace)                 位形空间 (C-space)
   ┌──────────────────────┐            ┌──────────────────────┐
   │                      │            │  ░░░░░░░░░░░░░░░░░░  │ ← 边界膨胀
   │      ┏━━━━━┓         │            │  ░░ ▒▒▒▒▒▒▒▒▒ ░░░░░  │
   │      ┃障碍 ┃         │   映射     │  ░░ ▒▒█████▒▒ ░░░░░  │
   │      ┗━━━━━┛         │  ────►     │  ░░ ▒▒▒▒▒▒▒▒▒ ░░░░░  │
   │   ▲                  │            │  ░░              ░░  │
   │  ╱█╲ 机器人(三角形)   │            │  ░░   ● q  (点)  ░░  │
   └──────────────────────┘            └──────────────────────┘
      机器人有形状、需考虑              机器人是一个点、
      形状与朝向的碰撞                  障碍被 Minkowski 和膨胀

要点：

• 膨胀量 = 机器人的“半径”（对旋转机器人，膨胀量随朝向 $\theta$ 变化，所以三维 C-space 的每一个 $\theta$ 切片形状都不同）。
• 转换后，任意复杂形状的机器人都变成一个点，所有路径规划算法都可以统一处理。
• 代价：C-space 维度 = DOF 数，高维时无法显式构造，只能采样查询。

4.2 A* vs RRT 探索模式对比

      A* (栅格)                          RRT (采样)
  ┌─────────────────┐              ┌─────────────────┐
  │ ○○○○○●●●●       │              │      ╱─┐        │
  │ ○○○●●●●●●●      │              │    ╱   └─╲      │
  │ ○●●●●█████●     │              │  ╱  ┌─█████╲    │
  │ S●●●●█████●●► G │              │ S──┘ █████ └──►G│
  │ ○●●●●█████●     │              │  ╲    ████      │
  │ ○○●●●●●●●●      │              │   └──╲          │
  └─────────────────┘              └─────────────────┘
  系统性扩展，节点密集             稀疏树，快速向空白区伸展
  最优、完备（分辨率）             次优、概率完备
  低维好，高维爆炸                 高维仍可用

要点：A* 的探索是“波前式”的，代价随维度指数增长；RRT 的树顶点数与维度无关（但覆盖质量会随维度下降）。这解释了为什么移动机器人（2–3 DOF）用 A*，而机械臂（6–7 DOF）用 RRT。

4.3 PID 阶跃响应对比

输出 ↑
     │        ┌╮  ┌╮
 1.0 ├────────┼─╰──╰────────  ← 目标值 (setpoint)
     │       ╱                  (a) Kp 大，无 D → 超调 + 振荡
     │      ╱ ─────────────     (b) PD 临界阻尼 → 快速无超调 ✓
     │     ╱╱
     │   ╱╱  ················   (c) Kp 小，无 I → 稳态误差 (offset)
     │ ╱╱
   0 └────────────────────────► 时间
     ↑ 阶跃输入

要点：

• 曲线 (a)：只有 P（且过大）→ 振荡。加 D 项提供阻尼。
• 曲线 ©：P 太小或有恒定扰动 → 稳态误差永不消除。加 I 项。
• 曲线 (b)：整定良好的 PID → 上升时间短、超调小、稳态误差为零。
• 复习提示：能看图说出"缺哪一项"是本节的核心考点。

4.4 粒子滤波定位的演化过程

t=0  全局定位初始      t=1  第一次观测后      t=5  多次观测后
┌──────────────┐      ┌──────────────┐      ┌──────────────┐
│············· │      │  ···    ···  │      │              │
│············· │      │ ·····  ····· │      │      ▓▓      │
│············· │  ──► │        ·   · │  ──► │      ▓▓      │
│············· │      │ ····   ····  │      │              │
│············· │      │  ···    ··   │      │              │
└──────────────┘      └──────────────┘      └──────────────┘
 粒子均匀分布          多峰：走廊对称性        单峰：收敛到真实位姿
 完全不知道在哪        导致多个候选假设        不确定性大幅降低

要点：这张图展示了粒子滤波相对卡尔曼滤波的根本优势——能表示多峰分布。在对称环境（相似的走廊、相同的房间）中，中间阶段的多假设是物理上正确的；高斯滤波会强行把两个峰平均成一个错误的中间位置。观测积累打破对称后，错误假设的粒子权重下降、被重采样淘汰。

4.5 SLAM 方法对比表

方法	表示	复杂度	优点	缺点
EKF-SLAM	高斯（联合协方差）	$O(N^2)$/步	理论清晰、在线	地标数受限、线性化误差、关联脆弱
FastSLAM	粒子 + 每粒子独立 EKF	$O(M\log N)$	多假设关联、可处理非线性	粒子退化、闭环大时假设丢失
GraphSLAM / BA	因子图 + 稀疏优化	稀疏，可实时	精度最高、可重线性化、鲁棒核	需批量优化、内存随轨迹增长
视觉 SLAM (ORB-SLAM)	关键帧 + 地图点	实时	成本低（仅相机）	尺度模糊（单目）、怕光照/低纹理
LiDAR SLAM (Cartographer/LOAM)	栅格/点云 + 位姿图	实时	精度高、不怕光照	传感器贵、几何退化场景失败

4.6 机器人体系结构分层对照表

层	频率	输入	输出	典型算法	失效后果
审慎/任务层	~0.1 Hz	目标、语义地图	子任务序列	PDDL 规划、行为树、LLM	任务失败（可恢复）
执行/规划层	1–10 Hz	位姿、局部地图	轨迹	A*/RRT*/MPC	路径不佳、卡住
反应/控制层	100–1000 Hz	编码器、IMU、力	电机指令	PID、阻抗控制	物理损坏、人员伤害

---

5. 与其他章节的关联

关联章节	关联内容
第2章 智能体	机器人是"物理具身智能体"的典范：PEAS 描述、环境性质（部分可观测、随机、连续、动态）
第3–4章 搜索	A*、加权 A*、D* 直接用于路径规划；RRT/PRM 是连续空间中的随机搜索；启发式设计原理通用
第11章 自动规划	任务级规划（PDDL、HTN）与运动规划的结合是 TAMP（Task and Motion Planning） 这一活跃方向
第12–13章 概率推理	贝叶斯滤波的理论基础；SLAM 的因子图即概率图模型
第14章 时序概率模型	卡尔曼滤波、粒子滤波、HMM 直接来源；与第24章 POS 标注的 Viterbi 同源
第16–17章 决策与 POMDP	不确定性下的最优决策；MDP/POMDP 是机器人决策的理论框架
第22章 强化学习	机器人技能学习、sim-to-real、策略梯度、模仿学习
第24–25章 NLP	自然语言指令理解与符号接地（symbol grounding）：把"把红色杯子放到桌子上"映射到 C-space 中的目标位形与动作序列；LLM 做任务分解（SayCan、Code-as-Policies）
第26章 计算机视觉	视觉是主要外部传感器：特征提取（SIFT/ORB）与 RANSAC 是视觉 SLAM 前端核心；目标检测用于抓取位姿估计；深度估计用于避障
第28章 伦理与安全	物理自主性带来的安全责任、自动驾驶的事故归责、自主武器、劳动替代

---

6. 延伸思考

思考一：Moravec 悖论与"具身智能"的数据瓶颈

Moravec 悖论：对 AI 而言，高层推理（下棋、证明定理、写代码）所需的计算量惊人地少，而低层感知运动技能（识别物体、稳定行走、灵巧抓取）所需的计算量惊人地多。今天 LLM 能通过法律考试、写出可运行的复杂程序，但让机器人可靠地叠一件衬衫、拧开一个陌生的瓶盖，仍然极其困难。

为什么？ 一个关键解释是数据的性质与规模：

• 语言与代码：互联网上有数十万亿 token 的现成数据，采集成本近乎为零，且自监督目标（预测下一词）与任务高度对齐。
• 机器人操作：数据必须在物理世界中实时采集，每条轨迹需要真实的时间与硬件磨损；不同机器人本体（morphology）之间数据难以迁移；失败可能损坏设备；而且没有一个像"预测下一词"那样通用又廉价的自监督目标。

当前的三条突围路线各有代价：

1. 大规模仿真 + 域随机化：数据近乎免费，但 sim-to-real gap 在接触丰富的任务（摩擦、形变、流体）上极难跨越——因为我们缺乏可微且准确的接触动力学模型。
2. 跨本体数据聚合（Open X-Embodiment 等）：把不同机构、不同机器人的数据统一为通用格式联合训练。挑战在于动作空间的异构性。
3. 从人类视频学习：YouTube 上有海量人类操作视频，但缺少动作标签与本体对应关系（人手 ≠ 夹爪）。

开放性问题：机器人学是否会迎来自己的"ImageNet 时刻"或"GPT 时刻"？还是说，具身智能的瓶颈根本上不同于语言与视觉——因为它的数据不能被复制、不能被爬取，而必须被经历？如果是后者，那么"scaling law 解决一切"的信念在此处会失效，我们可能需要样本效率上的根本性算法突破（更好的世界模型、更强的先验、更高效的探索），而非单纯的规模扩张。

思考二：LLM 作为机器人的"高层大脑"——机遇与不可接受的风险

把第25章的 LLM 接入 §3.7 的三层架构最顶层，是当前最活跃的方向之一。LLM 提供了机器人长期缺失的两种能力：开放世界的常识（知道"鸡蛋易碎，要轻拿"）和灵活的任务分解（把"帮我准备早餐"拆解为可执行的子目标序列）。SayCan 的做法很有代表性：用 LLM 评估每个候选技能对目标的语义相关性 $P(\text{skill}\mid \text{instruction})$，用学到的价值函数评估其物理可行性 $P(\text{success}\mid \text{skill},\text{state})$，两者相乘后选择——把"想做什么"和"能做什么"分开建模。

但这个组合引入了一个质变的风险：LLM 的失败模式是幻觉（自信地输出错误内容），而机器人的失败后果是物理性、不可逆的。在纯软件场景，幻觉的代价是一段错误的文本；在具身场景，幻觉的代价可能是打翻的化学品、误伤的人。第25章讨论的"幻觉源于训练目标"这一诊断，在这里从"质量问题"升级为"安全问题"。

三个具体的技术张力：

1. 语义接地的验证难题。LLM 说"把杯子放到桌上"，但"桌上"对应 C-space 中哪个具体位形？这个映射必须由感知（第26章）提供，而感知本身有误差且可能被对抗攻击。语言的模糊性（第24章）+ 感知的不确定性（第26章）+ 执行的不可逆性（本章）三者叠加，构成了具身 AI 特有的风险复合。

2. 分层架构是安全的朋友还是敌人？ 好消息是，分层给了我们一个自然的防护结构：把 LLM 严格限制在审慎层，让它只能输出"经过验证的技能库"中的调用，而反应层的安全约束（力矩限制、速度限制、急停）用传统的、可形式化验证的方法实现，且拥有对上层的绝对否决权。这本质上是"能力与权限分离"的安全工程原则。坏消息是，随着端到端 VLA 模型的兴起，这个清晰的分层正在被打破——当一个 Transformer 直接从像素和语言输出电机指令时，我们失去了可以插入安全检查的接口。

3. 可解释性与事故归因。当一辆自动驾驶车或一台家用机器人造成伤害，我们需要回答"为什么"。模块化系统可以逐层追溯（是感知漏检？规划失误？还是控制超调？）；端到端神经策略则给不出任何可审计的因果链。这不仅是技术问题，也是法律与保险制度能否接受该技术的前提。

开放性问题：我们能否设计一种架构，既享受基础模型的开放世界泛化能力，又保留形式化的安全保证？几个候选方向：

• 可验证的安全过滤器（safety filter）：如控制屏障函数（Control Barrier Function），在任何策略输出之上叠加一层数学上可证明的约束投影，保证系统状态永不进入不安全集。这允许"不可信的智能"与"可信的安全"共存。
• 不确定性感知的自主性调节：让机器人知道自己"不确定"，并在不确定时主动降级——减速、请求人类确认、或退回到保守的默认行为。这要求模型有良好的校准（calibration），而这恰恰是当前 LLM 的弱项。
• 仿真中的形式化验证 + 真实世界的运行时监控结合。

这个问题的重要性在于：机器人学是 AI 对齐问题从抽象走向具体的第一个战场。在纯文本领域，"AI 造成伤害"还需要经过人类的中介；而一旦 AI 拥有执行器，因果链就直接连通了物理世界。本章讨论的所有技术——从 PID 的稳定性保证，到运动规划的完备性，到滤波器的不确定性量化——其价值在基础模型时代不但没有降低，反而因为它们提供了可证明的性质而变得更加珍贵。未来的关键，或许不在于用学习取代这些经典方法，而在于如何把不可预测的智能安全地包裹在可预测的保障之中。