CABINET / Machines in Motion / 具身智能研究
规划与决策
规划与决策层的核心任务是将感知层输出的环境表征转化为可执行的动作序列。这一层回答两个问题：做什么（任务规划）和怎么做（运动规划）。在具身智能系统中，规划层处于感知（Layer 5）与控制（Layer 3）之间，接收场景理解结果，输出轨迹或动作指令。
┌─────────────────────────────────────────────────────────┐
│ Layer 7: 学习层 │
└─────────────────────────┬───────────────────────────────┘
│ learned policies / value functions
┌─────────────────────────▼───────────────────────────────┐
│ Layer 6: 规划与决策层 │
│ │
│ ┌───────────┐ ┌───────────┐ ┌───────────────┐ │
│ │ 任务规划 │───▶│ 运动规划 │───▶│ 抓取规划 │ │
│ │(Task Plan) │ │(Motion │ │(Grasp Plan) │ │
│ │ │ │ Planning) │ │ │ │
│ └─────┬─────┘ └─────┬─────┘ └───────┬───────┘ │
│ │ │ │ │
│ ▼ ▼ ▼ │
│ ┌───────────────────────────────────────────────┐ │
│ │ MPC / Trajectory Execution │ │
│ └───────────────────────────────────────────────┘ │
│ │
└─────────────────────────┬───────────────────────────────┘
│ joint trajectories / torques
┌─────────────────────────▼───────────────────────────────┐
│ Layer 3: 底层控制与动力学层 │
└─────────────────────────────────────────────────────────┘规划层的输入输出接口：
| 方向 | 数据类型 | 来源/去向 |
|---|---|---|
| 输入 | 语义任务指令（自然语言或符号） | 用户 / 高层策略 |
| 输入 | 场景图（Scene Graph） | 感知层 Layer 5 |
| 输入 | 点云 / 占据栅格（Occupancy Grid） | 感知层 Layer 5 |
| 输入 | 机器人当前关节状态 q | 状态估计 Layer 4 |
| 输出 | 关节空间轨迹 q(t) | 控制层 Layer 3 |
| 输出 | 笛卡尔空间轨迹 x(t) | 控制层 Layer 3 |
| 输出 | 抓取位姿（6-DoF grasp pose） | 末端执行器控制 |
1. 任务规划（Task Planning）
任务规划将高层目标分解为有序的子任务序列。其核心在于离散动作空间中的搜索与约束满足。
1.1 经典符号规划：PDDL 与 STRIPS
STRIPS（Stanford Research Institute Problem Solver） 是最早的自动规划形式化框架。每个动作（Action）由三个部分定义：
- 前置条件（Preconditions）：动作执行前必须为真的命题集合
- 添加效果（Add Effects）：动作执行后变为真的命题
- 删除效果（Delete Effects）：动作执行后变为假的命题
PDDL（Planning Domain Definition Language） 是 STRIPS 的标准化扩展，将规划问题分为两个文件：
Domain 文件：定义类型、谓词、动作模板
(define (domain pick-and-place)
(:requirements :strips :typing)
(:types object location gripper)
(:predicates
(at ?obj - object ?loc - location)
(holding ?g - gripper ?obj - object)
(gripper-free ?g - gripper)
(reachable ?loc - location)
)
(:action pick
:parameters (?obj - object ?loc - location ?g - gripper)
:precondition (and
(at ?obj ?loc)
(gripper-free ?g)
(reachable ?loc)
)
:effect (and
(holding ?g ?obj)
(not (at ?obj ?loc))
(not (gripper-free ?g))
)
)
(:action place
:parameters (?obj - object ?loc - location ?g - gripper)
:precondition (and
(holding ?g ?obj)
(reachable ?loc)
)
:effect (and
(at ?obj ?loc)
(gripper-free ?g)
(not (holding ?g ?obj))
)
)
)Problem 文件：定义初始状态与目标状态
(define (problem sort-objects)
(:domain pick-and-place)
(:objects
cup plate - object
table shelf - location
left-gripper - gripper
)
(:init
(at cup table)
(at plate table)
(gripper-free left-gripper)
(reachable table)
(reachable shelf)
)
(:goal (and
(at cup shelf)
(at plate shelf)
))
)常用 PDDL 求解器：Fast Downward、FF Planner、LAMA。这些求解器使用启发式搜索（Heuristic Search），通过松弛问题（Relaxed Problem）估计目标距离。
PDDL 的局限性在于：需要人工定义完整的动作模型；难以处理连续物理量；符号接地问题（Symbol Grounding Problem）使其难以直接连接感知输出。
1.2 行为树（Behavior Tree）
行为树是一种层次化的任务执行架构，通过树状结构组织动作的条件判断与执行流程。
节点类型：
| 节点类型 | 符号 | 行为 | 子节点返回 Success | 子节点返回 Failure | 子节点返回 Running |
|---|---|---|---|---|---|
| Sequence | → | 依次执行所有子节点 | 继续下一个子节点 | 立即返回 Failure | 返回 Running |
| Selector (Fallback) | ? | 尝试子节点直到一个成功 | 立即返回 Success | 继续下一个子节点 | 返回 Running |
| Condition | ◯ | 检查条件是否为真 | N/A | N/A | N/A |
| Action | □ | 执行具体动作 | N/A | N/A | N/A |
| Decorator | ◇ | 修饰子节点行为（重试、取反、超时） | 取决于装饰类型 | 取决于装饰类型 | 取决于装饰类型 |
行为树示例：机器人取物任务
[→] Root Sequence
/ | \
[?] Get [→] Navigate [→] Pick
Object to Target Object
/ \ | \ | \
[◯] [□] [□] [◯] [□] [□]
Have Locate Move At Open Close
Obj? Object Base Goal? Grip Grip行为树与有限状态机（FSM）对比：
| 特性 | 行为树 (BT) | 有限状态机 (FSM) |
|---|---|---|
| 结构 | 树形层次结构 | 图结构（节点+边） |
| 可扩展性 | 高：添加子树即可扩展 | 低：状态增加导致转移边指数增长 |
| 模块化 | 天然支持子树复用 | 状态间耦合度高 |
| 响应性 | 每个 tick 从根节点重新评估 | 需要显式定义转移条件 |
| 调试 | 树结构直观可视化 | 复杂状态图难以追踪 |
| 典型问题 | tick 频率设定、大树性能开销 | 状态爆炸（State Explosion） |
实现框架：
- BehaviorTree.CPP：C++ 实现，ROS 2 Nav2 的默认任务执行引擎，支持 XML 格式定义行为树，内置 Groot 可视化编辑器
- PyTrees：Python 实现，灵活性高，适合快速原型开发
- SMACH（State Machine for ROS）：ROS 1 时代的标准状态机框架，通过 actionlib 接口调用底层能力
BehaviorTree.CPP 的 XML 定义示例：
<root main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<Sequence>
<Condition ID="IsObjectDetected" object="{target}"/>
<Action ID="ComputeGraspPose" object="{target}" pose="{grasp_pose}"/>
<Action ID="MoveToPreGrasp" goal="{grasp_pose}"/>
<Action ID="CloseGripper"/>
<Action ID="MoveToPlacement" goal="{place_pose}"/>
<Action ID="OpenGripper"/>
</Sequence>
</BehaviorTree>
</root>1.3 基于大语言模型的任务规划
2022 年起，大语言模型（LLM）被引入机器人任务规划，利用其自然语言理解与常识推理能力进行任务分解。
SayCan（Google Research, 2022）
核心思想：将 LLM 的语言评分（Language Score）与机器人的可供性评分（Affordance Score）相乘，确保生成的计划在物理上可执行。
P(action | instruction, state) = P_LLM(action | instruction) × P_affordance(action | state)- P_LLM：LLM 对该动作与指令相关性的评分
- P_affordance：该动作在当前状态下成功执行的概率，由预训练的 value function 提供
这种设计解决了 LLM 缺乏物理世界知识的问题。LLM 可能提议”飞到桌子上方”，但 affordance model 会给出接近 0 的分数将其过滤。
Code as Policies（Google Research, 2022）
将 LLM 的输出从自然语言动作序列变为可执行代码。LLM 接收 API 文档和示例，直接生成调用机器人 primitive 的 Python 代码。
# LLM 生成的代码示例
def stack_blocks_by_color():
red_blocks = detect_objects("red block")
blue_blocks = detect_objects("blue block")
# 按从大到小排序
red_blocks.sort(key=lambda b: b.size, reverse=True)
target_pos = get_position("table_center")
for i, block in enumerate(red_blocks):
place_height = i * BLOCK_HEIGHT
pick(block)
place(target_pos + [0, 0, place_height])优势：代码具有精确语义，支持循环、条件判断、变量绑定，比自然语言动作列表表达力强。
Inner Monologue（Google, 2022）
引入闭环反馈机制。机器人执行每一步后，将感知结果（成功/失败、物体状态变化）以自然语言形式反馈给 LLM，LLM 据此调整后续计划。这使得规划过程从开环变为闭环。
ReKep（Relational Keypoint Constraints, 2024）
将操作任务表示为关键点（Keypoint）之间的空间关系约束。LLM 不直接输出动作序列，而是输出关键点约束（如”杯子口部的关键点必须位于水龙头出水口正下方”），由底层优化器将约束转化为轨迹。
# ReKep 约束定义示例
def pour_water_constraint(kp_cup_rim, kp_faucet_outlet):
"""杯口在水龙头出水口正下方"""
cost = 0
cost += (kp_cup_rim[0] - kp_faucet_outlet[0])**2 # x 对齐
cost += (kp_cup_rim[1] - kp_faucet_outlet[1])**2 # y 对齐
cost += max(0, kp_cup_rim[2] - kp_faucet_outlet[2]) # 杯口低于出水口
return cost幻觉风险与物理执行
LLM 在物理任务规划中的核心风险：
| 风险类型 | 具体表现 | 缓解方案 |
|---|---|---|
| 物理不可行 | 提议超出工作空间的动作 | Affordance grounding (SayCan) |
| 顺序错误 | 先放置后抓取 | 验证前置条件 |
| 遗漏步骤 | 跳过打开抽屉直接取物 | 物理模拟器验证 |
| 过度简化 | 忽略碰撞约束 | 运动规划器可行性检查 |
| 感知假设错误 | 假设物体在某位置但实际不在 | Inner Monologue 闭环反馈 |
| 数值幻觉 | 给出错误的坐标或尺寸 | 从感知系统获取实际数值 |
当前工程实践中，LLM 通常作为高层任务分解器，其输出必须经过下游运动规划器的可行性验证才能执行。
2. 运动规划（Motion Planning）
运动规划的目标是在避免碰撞的前提下，计算从起始构型（Configuration）到目标构型的连续路径或轨迹。
2.1 构型空间（Configuration Space, C-space）
机器人的状态由其所有关节角度组成的向量 q = [q1, q2, …, qn] 描述。对于 n 自由度机器人，其构型空间是 n 维空间。
C-space 分解：
C = C_free ∪ C_obs
C_free: 无碰撞构型集合（机器人可安全到达的所有构型）
C_obs: 碰撞构型集合（机器人与障碍物或自身碰撞的构型）运动规划的本质是在 C_free 中寻找连接 q_start 和 q_goal 的连续路径。
C-space 的维度示例：
| 机器人类型 | DoF | C-space 维度 |
|---|---|---|
| 平面移动机器人 | 3 | R^2 × SO(1) |
| 6-DoF 工业臂 | 6 | T^6（6维环面） |
| 7-DoF 协作臂（如 Franka） | 7 | T^7 |
| 双臂机器人 | 14 | T^14 |
| 人形机器人全身 | 30+ | T^30+ |
C-space 维度越高，计算碰撞检测和路径搜索的计算量指数增长，这就是运动规划的维度灾难（Curse of Dimensionality）。
2.2 采样规划算法（Sampling-based Planning）
采样规划算法通过在 C-space 中随机采样来探索自由空间，避免了显式构建 C_obs 的计算开销。
RRT（Rapidly-exploring Random Tree）
Algorithm: RRT(q_start, q_goal, max_iter)
─────────────────────────────────────────
Input: q_start 起始构型
q_goal 目标构型
max_iter 最大迭代次数
Output: path 从 q_start 到 q_goal 的路径，或 FAILURE
1: T.init(q_start) // 初始化树，根节点为起始构型
2: for i = 1 to max_iter do
3: q_rand ← SampleRandom(C) // 在 C-space 中均匀随机采样
4: q_near ← Nearest(T, q_rand) // 找到树中距 q_rand 最近的节点
5: q_new ← Steer(q_near, q_rand, δ) // 从 q_near 向 q_rand 方向延伸 δ 步长
6: if CollisionFree(q_near, q_new) then // 检查路径段是否无碰撞
7: T.addNode(q_new)
8: T.addEdge(q_near, q_new)
9: if Distance(q_new, q_goal) < ε then // 到达目标邻域
10: return ExtractPath(T, q_start, q_new)
11: end if
12: end if
13: end for
14: return FAILURERRT 的关键性质：
- 概率完备（Probabilistically Complete）：如果路径存在，随着迭代次数趋近无穷，找到路径的概率趋近 1
- 非最优：找到的路径通常不是最短路径
- 对目标的偏置采样（Goal Bias）：以一定概率（通常 5% 至 10%）直接将 q_goal 作为 q_rand，加速收敛
RRT*（Optimal RRT）
RRT* 在 RRT 基础上增加了重连（Rewiring）操作，使其渐近最优（Asymptotically Optimal）。
Algorithm: RRT*(q_start, q_goal, max_iter)
─────────────────────────────────────────
（在 RRT 基础上，步骤 6-8 替换为以下逻辑）
6: if CollisionFree(q_near, q_new) then
7: // 在 q_new 的邻域内寻找所有节点
8: Q_near ← NearNodes(T, q_new, radius)
9:
10: // 选择最优父节点（Choose Best Parent）
11: q_min ← q_near
12: c_min ← Cost(q_near) + LineCost(q_near, q_new)
13: for each q_candidate in Q_near do
14: c_candidate ← Cost(q_candidate) + LineCost(q_candidate, q_new)
15: if c_candidate < c_min AND CollisionFree(q_candidate, q_new) then
16: q_min ← q_candidate
17: c_min ← c_candidate
18: end if
19: end for
20: T.addNode(q_new, parent=q_min)
21:
22: // 重连（Rewiring）
23: for each q_candidate in Q_near do
24: c_new_path ← Cost(q_new) + LineCost(q_new, q_candidate)
25: if c_new_path < Cost(q_candidate) AND CollisionFree(q_new, q_candidate) then
26: T.changeParent(q_candidate, new_parent=q_new)
27: end if
28: end for
29: end ifRRT* 的邻域半径通常设为：
radius = γ * (log(n) / n)^(1/d)
其中：
γ: 与 C-space 体积相关的常数
n: 当前树中节点数
d: C-space 维度Bi-RRT（双向 RRT）
从 q_start 和 q_goal 同时生长两棵树，交替扩展。当两棵树的节点足够接近时连接。在窄通道（Narrow Passage）场景中效率显著高于单向 RRT。
Algorithm: BiRRT(q_start, q_goal, max_iter)
────────────────────────────────────────────
1: T_a.init(q_start)
2: T_b.init(q_goal)
3: for i = 1 to max_iter do
4: q_rand ← SampleRandom(C)
5: if Extend(T_a, q_rand) ≠ TRAPPED then
6: if Connect(T_b, q_new_a) = REACHED then
7: return MergePaths(T_a, T_b)
8: end if
9: end if
10: Swap(T_a, T_b) // 交替扩展两棵树
11: end for
12: return FAILUREPRM（Probabilistic Roadmap）
PRM 是多查询（Multi-query）算法，适用于环境不变但起止点频繁变化的场景。
两阶段：
- 构建阶段（Construction Phase）：在 C_free 中采样大量节点，对每个节点尝试连接其 k 近邻，碰撞检测通过则加入路线图
- 查询阶段（Query Phase）：将 q_start 和 q_goal 连接到路线图，然后使用 A* 或 Dijkstra 在图中搜索路径
Construction Phase:
──────────────────
1: V ← ∅, E ← ∅
2: for i = 1 to N do
3: q_sample ← SampleFree(C)
4: V ← V ∪ {q_sample}
5: neighbors ← KNearest(V, q_sample, k)
6: for each q_n in neighbors do
7: if CollisionFree(q_sample, q_n) then
8: E ← E ∪ {(q_sample, q_n)}
9: end if
10: end for
11: end for
12: return Graph(V, E)OMPL（Open Motion Planning Library）
OMPL 是运动规划算法的标准开源库，提供 RRT、RRT*、PRM、BiRRT、KPIECE、BIT* 等 50+ 种规划算法的统一接口。
关键设计：OMPL 本身不包含碰撞检测或机器人模型，通过回调接口（StateValidityChecker）接入外部碰撞检测库。MoveIt 2 通过 OMPL 插件接口调用这些算法。
2.3 轨迹优化（Trajectory Optimization）
采样规划算法输出的路径通常锯齿状且非最优。轨迹优化在初始路径基础上，通过数值优化获得平滑、高效、满足约束的轨迹。
CHOMP（Covariant Hamiltonian Optimization for Motion Planning）
CHOMP 将轨迹优化形式化为函数空间中的梯度下降：
目标函数：
U[ξ] = f_smooth(ξ) + λ * f_obs(ξ)
其中：
ξ = [q_1, q_2, ..., q_T] 轨迹（T 个路径点的序列）
f_smooth = (1/2) * ξ^T A ξ 平滑性代价（A 为有限差分矩阵）
f_obs = Σ_t ||ξ'_t|| * c(ξ_t) 障碍物代价（带速度加权）
c(x): 距离场（Signed Distance Field）的代价函数
λ: 权重系数
更新规则（协变梯度下降）：
ξ ← ξ - (1/η) * A^{-1} * ∇U[ξ]
A^{-1} 的使用使得更新在函数空间中是协变的（Covariant），
即结果不依赖于路径点的参数化方式。CHOMP 需要预计算环境的符号距离场（Signed Distance Field, SDF），用于快速查询任意点到最近障碍物的距离及梯度方向。
TrajOpt（Sequential Convex Optimization）
TrajOpt 使用序列凸优化（Sequential Convex Programming, SCP）求解：
原始问题（非凸）：
minimize f(ξ) // 轨迹代价（平滑性、时间最短等）
subject to g_i(ξ) ≤ 0 // 碰撞约束（非凸）
h_j(ξ) = 0 // 等式约束（起终点等）
SCP 迭代：
在当前解 ξ_k 处将非凸约束线性化：
g_i(ξ) ≈ g_i(ξ_k) + ∇g_i(ξ_k)^T (ξ - ξ_k)
求解凸子问题获得 ξ_{k+1}，反复迭代直至收敛。TrajOpt 的碰撞约束使用连续碰撞检测（Continuous Collision Detection），检测两个相邻路径点之间的扫掠体（Swept Volume）是否与障碍物相交，避免了离散检测遗漏碰撞的问题。
MPPI（Model Predictive Path Integral）
MPPI 是一种基于采样的轨迹优化方法，特别适合 GPU 并行计算：
Algorithm: MPPI(x_current, u_prev, K_samples)
─────────────────────────────────────────────
Input: x_current 当前状态
u_prev 上一时刻的控制序列 [u_0, u_1, ..., u_{H-1}]
K_samples 采样数量（通常 1000 至 10000）
Output: u_optimal 优化后的控制序列
1: for k = 1 to K do // 可 GPU 并行
2: ε_k ← SampleNoise(H, dim_u) // 采样 H 步噪声序列
3: u_k ← u_prev + ε_k // 扰动控制序列
4: x_k ← Rollout(x_current, u_k) // 前向模拟得到状态序列
5: S_k ← ComputeCost(x_k, u_k) // 计算轨迹代价
6: end for
7:
8: // 计算权重（指数加权）
9: β ← min(S_1, ..., S_K)
10: for k = 1 to K do
11: w_k ← exp(-(1/λ) * (S_k - β))
12: end for
13: w ← w / Σ w_k // 归一化权重
14:
15: // 加权平均获得最优控制
16: u_optimal ← Σ_k w_k * u_k
17:
18: return u_optimalMPPI 的核心优势：无需计算梯度，可处理非光滑代价函数；前向模拟高度并行化，GPU 上可同时模拟数千条轨迹。NVIDIA Isaac Lab 中广泛使用 MPPI 进行实时运动规划。
2.4 MoveIt 2 框架架构
MoveIt 2 是 ROS 2 生态中的标准运动规划框架，集成了规划、碰撞检测、运动学求解等功能。
┌─────────────────────────────────────────────────────────────┐
│ MoveIt 2 架构 │
├─────────────────────────────────────────────────────────────┤
│ │
│ ┌─────────────┐ ┌──────────────┐ ┌──────────────┐ │
│ │ Move Group │ │ Planning │ │ Perception │ │
│ │ Interface │───▶│ Pipeline │◀───│ Pipeline │ │
│ │ (用户API) │ │ │ │ │ │
│ └─────────────┘ └──────┬───────┘ └──────────────┘ │
│ │ │
│ ┌──────────────────┼──────────────────┐ │
│ ▼ ▼ ▼ │
│ ┌─────────────┐ ┌──────────────┐ ┌──────────────┐ │
│ │ OMPL │ │ Pilz │ │ STOMP / │ │
│ │ (Sampling) │ │ (Cartesian) │ │ CHOMP │ │
│ └─────────────┘ └──────────────┘ └──────────────┘ │
│ │
│ ┌─────────────────────────────────────────────────────┐ │
│ │ Planning Scene │ │
│ │ ┌───────────────┐ ┌────────────────────────┐ │ │
│ │ │ Robot Model │ │ World (Collision Obj) │ │ │
│ │ │ (URDF/SRDF) │ │ (OctoMap, Meshes) │ │ │
│ │ └───────────────┘ └────────────────────────┘ │ │
│ └─────────────────────────────────────────────────────┘ │
│ │
│ ┌──────────────────┐ ┌─────────────────────────────┐ │
│ │ Kinematics │ │ Collision Detection │ │
│ │ (KDL/IKFast/ │ │ (FCL / Bullet) │ │
│ │ TRAC-IK) │ │ │ │
│ └──────────────────┘ └─────────────────────────────┘ │
│ │
│ ┌─────────────────────────────────────────────────────┐ │
│ │ Trajectory Execution (Controller Interface) │ │
│ │ → FollowJointTrajectory action → ros2_control │ │
│ └─────────────────────────────────────────────────────┘ │
└─────────────────────────────────────────────────────────────┘MoveIt 2 核心组件：
| 组件 | 功能 | 关键接口 |
|---|---|---|
| Planning Scene | 维护机器人模型+世界模型的状态 | PlanningSceneInterface |
| Motion Planner | 调用 OMPL/CHOMP/STOMP 计算路径 | PlanningPipeline |
| Kinematics Solver | 正/逆运动学求解 | KinematicsBase |
| Collision Detection | 检测碰撞 | CollisionDetection (FCL) |
| Controller Manager | 执行轨迹 | MoveItControllerManager |
| Perception Pipeline | 接入点云构建 OctoMap | OccupancyMapUpdater |
2.5 碰撞检测
碰撞检测是运动规划中计算开销最大的环节，通常占规划总时间的 80% 以上。
FCL（Flexible Collision Library）：
- MoveIt 2 的默认碰撞检测后端
- 支持几何原语（球体、圆柱、盒子）和三角网格
- 使用 BVH（Bounding Volume Hierarchy）加速：AABB 树、OBB 树
- 支持距离查询（Closest Points）和穿透深度（Penetration Depth）
Bullet Physics：
- 物理仿真引擎中的碰撞检测模块
- MoveIt 2 支持 Bullet 作为替代后端
- 连续碰撞检测（CCD）能力强于 FCL
- 适合动态环境中的实时碰撞检查
碰撞检测加速策略：
层次化检测流程：
1. Broad Phase: AABB 包围盒快速剔除不可能碰撞的物体对
→ 算法: SAP (Sweep and Prune), 空间哈希 (Spatial Hashing)
2. Narrow Phase: 对 Broad Phase 筛选出的物体对做精确检测
→ 算法: GJK (Gilbert-Johnson-Keerthi), EPA (Expanding Polytope)
3. 自碰撞过滤: SRDF 中定义的 ACM (Allowed Collision Matrix)
→ 预先标记永远不会碰撞的 link 对，跳过检测3. 抓取规划（Grasp Planning）
抓取规划的目标是为目标物体计算一个 6-DoF 抓取位姿（position + orientation），使得末端执行器能稳定抓取物体。
3.1 解析式抓取规划（Analytical Grasp Planning）
基于力学分析的经典方法，核心概念：
力闭合（Force Closure）：
接触力的凸锥（Contact Wrench Space）包含原点，即抓取能抵抗任意方向的外力和力矩。
力闭合条件：
给定 n 个接触点，每个接触点 i 施加力 f_i（在摩擦锥 FC_i 内），
抓取力闭合当且仅当：
∃ f_1, ..., f_n 使得:
Σ_i G_i * f_i = 0 (力平衡)
f_i ∈ FC_i (摩擦锥约束)
其中 G_i 是接触点 i 的 Grasp Map（将接触力映射到物体重心的力旋量）形闭合（Form Closure）：
纯几何约束使物体无法运动，不依赖于摩擦。比力闭合更强的条件。
摩擦锥（Friction Cone）：
点接触 + 库仑摩擦模型：
接触力 f 必须在摩擦锥内：
f_n > 0 (法向力为正，不拉物体)
||f_t|| ≤ μ * f_n (切向力不超过摩擦力)
其中：
f_n: 法向分量
f_t: 切向分量
μ: 摩擦系数
摩擦锥的近似：用 m 面多面体锥（Polyhedral Cone）近似，
将非线性约束线性化。抓取质量度量（Grasp Quality Metrics）：
| 度量 | 定义 | 直觉含义 |
|---|---|---|
| ε-metric | 力旋量空间中最大内接球半径 | 抓取能抵抗的最小外力 |
| Volume metric | 凸包体积 | 力旋量空间覆盖范围 |
| 最小奇异值 | Grasp Matrix 的最小奇异值 | 力传递的最差方向效率 |
| 任务相关度量 | 在任务力旋量子空间中的覆盖 | 对特定任务的抓取质量 |
3.2 数据驱动抓取规划
GraspNet-1Billion（2020）
- 数据集：190 个场景，88 个物体，超过 10 亿个标注抓取位姿
- 输入：单帧 RGB-D 点云
- 输出：场景中所有可见物体的抓取位姿集合及质量评分
- 网络架构：PointNet++ backbone + Grasp Proposal + Grasp Evaluation
AnyGrasp（2023）
- GraspNet 的工程化部署版本
- 支持实时推理（约 50ms per frame）
- 处理透明物体和镜面物体
- 提供 ROS 接口和 Python SDK
Contact-GraspNet（2021）
- 将抓取建模为接触图（Contact Map）：对点云中每个点预测该点是否为接触点，以及从该接触点出发的抓取方向
- 6-DoF 抓取姿态直接从点云回归
- 可处理密集堆叠场景（Cluttered Scene）
3.3 抓取规划的关键约束
一个有效的抓取位姿必须同时满足：
约束检查流程：
1. 力闭合/形闭合检查
→ 抓取在力学上是否稳定？
2. 碰撞检查（Collision-Free）
→ 末端执行器在抓取位姿处是否与环境碰撞？
→ 接近路径（Approach Direction）是否畅通？
3. 运动学可达性（Kinematically Feasible）
→ 逆运动学是否有解？
→ 该解是否在关节限位内？
4. 运动规划可行性
→ 从当前构型到抓取前构型是否存在无碰撞路径？
5. 任务兼容性
→ 抓取后能否完成后续操作？
（例如：抓杯子需要保持杯口向上）抓取规划与运动规划的耦合是当前系统集成的难点。通常采用”采样、排序、验证”（Sample, Rank, Validate）的流水线：
Grasp Planning Pipeline:
────────────────────────
1. 生成 N 个候选抓取位姿（数据驱动方法）
2. 按抓取质量评分排序
3. 对 top-K 候选逐一验证：
a. IK 求解（逆运动学）
b. 碰撞检测（抓取位姿处）
c. 接近方向碰撞检测
d. 运动规划验证（可选，计算开销大）
4. 返回第一个通过所有验证的抓取4. MPC（Model Predictive Control，模型预测控制）
MPC 是一种在线优化控制方法，在每个控制周期内求解有限时域（Finite Horizon）最优控制问题，执行第一步控制量后滚动重复。
4.1 滚动时域公式（Receding Horizon Formulation）
在每个时刻 t，求解：
minimize Σ_{k=0}^{H-1} l(x_k, u_k) + l_f(x_H)
subject to x_{k+1} = f(x_k, u_k) // 动力学模型
x_min ≤ x_k ≤ x_max // 状态约束
u_min ≤ u_k ≤ u_max // 输入约束
g(x_k, u_k) ≤ 0 // 其他约束（碰撞等）
x_0 = x(t) // 初始条件为当前实际状态
其中：
H: 预测时域（Prediction Horizon）
l: 阶段代价（Stage Cost）
l_f: 终端代价（Terminal Cost）
f: 系统动力学模型
x_k: 第 k 步预测状态
u_k: 第 k 步控制输入
仅执行 u_0*（最优序列的第一步），然后在下一时刻重新求解。4.2 线性 MPC vs 非线性 MPC（NMPC）
| 特性 | 线性 MPC | 非线性 MPC |
|---|---|---|
| 动力学模型 | x_{k+1} = Ax_k + Bu_k | x_{k+1} = f(x_k, u_k)（任意非线性） |
| 优化问题 | 二次规划（QP） | 非线性规划（NLP） |
| 求解器 | qpOASES, OSQP | IPOPT, CasADi, acados |
| 求解时间 | 亚毫秒至毫秒级 | 毫秒至秒级 |
| 全局最优保证 | 是（凸问题） | 否（局部最优） |
| 适用场景 | 线性化后的系统、小角度运动 | 全身动力学、接触模式切换 |
线性 MPC 的 QP 形式：
minimize (1/2) * U^T H U + g^T U
subject to A_ineq * U ≤ b_ineq
A_eq * U = b_eq
其中 U = [u_0, u_1, ..., u_{H-1}] 为决策变量（控制序列的拼接）
通过将动力学约束代入代价函数，消去状态变量 x_k，得到仅关于 U 的 QP。4.3 MPPI 在 MPC 框架中的位置
MPPI（详见 2.3 节）本质上是一种无梯度（Derivative-free）的 MPC 求解方法：
- 不需要对动力学模型求导
- 通过大量并行采样 + 前向模拟代替梯度计算
- 适合动力学模型难以解析求导的场景（接触、柔性体）
- GPU 并行化后实时性优于基于梯度的 NMPC
4.4 MPC 在足式机器人中的应用
MIT Cheetah 系列：
MIT Mini Cheetah 使用线性 MPC 进行步态生成：
- 将足式机器人简化为单刚体模型（Single Rigid Body Model）
- 控制输入：四条腿的地面反力（12维，每条腿 3 维）
- 约束：摩擦锥约束（力在摩擦锥内）、正向力约束（只推不拉）
- 求解频率：500 Hz（QP 求解时间约 0.5ms）
MIT Cheetah MPC 简化模型：
──────────────────────────
状态 x = [p, θ, ṗ, ω]^T ∈ R^12
p: 质心位置 (3)
θ: 欧拉角 (3)
ṗ: 质心速度 (3)
ω: 角速度 (3)
控制 u = [f_1, f_2, f_3, f_4]^T ∈ R^12
f_i: 第 i 条腿的地面反力 (3)
约束：
f_i,z > 0 (腿只能推地面)
|f_i,x| ≤ μ*f_i,z (x方向摩擦锥)
|f_i,y| ≤ μ*f_i,z (y方向摩擦锥)
f_i = 0 if leg i in swing phase (摆动腿无力)ANYmal（ETH Zurich / ANYbotics）：
使用 NMPC + 全身动力学（Whole-Body Dynamics），通过 acados 框架求解非线性优化问题。支持在崎岖地形上的自适应步态规划。
4.5 规划方法对比表
| 方法 | 最优性 | 完备性 | 典型求解时间 | 适用维度 | 动态环境 |
|---|---|---|---|---|---|
| RRT | 非最优 | 概率完备 | 10ms 至 1s | 中高维(6-14) | 需重规划 |
| RRT* | 渐近最优 | 概率完备 | 100ms 至 10s | 中维(6-7) | 需重规划 |
| PRM | 非最优(图搜索最优) | 概率完备 | 构建慢,查询快 | 中维 | 不适合 |
| CHOMP | 局部最优 | 不完备 | 100ms 至 1s | 中维 | 需重优化 |
| TrajOpt | 局部最优 | 不完备 | 50ms 至 500ms | 中维 | 需重优化 |
| MPPI | 局部最优(采样) | 不完备 | 1ms 至 50ms (GPU) | 低中维 | 实时适应 |
| 线性 MPC | QP 全局最优 | 依模型 | 0.1ms 至 5ms | 低维(线性化) | 实时适应 |
| NMPC | 局部最优 | 依模型 | 5ms 至 100ms | 中维 | 实时适应 |
| cuRobo | 局部最优(并行) | 不完备 | 1ms 至 50ms (GPU) | 中维(7) | 实时适应 |
5. 场景图与空间推理（Scene Graphs and Spatial Reasoning）
5.1 场景图的基本结构
场景图（Scene Graph）是一种图结构的环境表征，节点表示物体，边表示物体间的空间关系或语义关系。
场景图 G = (V, E)
节点 V 中的每个节点 v_i 包含：
- 物体类别（Object Class）: "cup", "table", "drawer"
- 位姿（Pose）: SE(3) 中的位置与朝向
- 几何属性: 边界框（Bounding Box）/ 网格（Mesh）/ 点云
- 语义属性: 材质、颜色、功能标签
- 状态属性: open/closed, full/empty
边 E 中的每条边 e_{ij} 包含：
- 空间关系: on, in, next_to, above, below, inside
- 功能关系: supports, contains, attached_to
- 距离: 物体间的度量距离场景图示例：
┌──────────┐
│ Room │
└────┬─────┘
┌──────────┼──────────┐
▼ ▼ ▼
┌──────────┐ ┌────────┐ ┌────────┐
│ Table │ │ Shelf │ │ Chair │
└────┬─────┘ └───┬────┘ └────────┘
┌───┼───┐ │
▼ ▼ ▼ ▼
┌───┐┌───┐┌───┐ ┌────┐
│Cup││Plate│Book│ │Box │
└───┘└───┘└───┘ └──┬─┘
▼
┌─────┐
│Ball │
└─────┘
边的语义标注：
Table --supports--> Cup
Table --supports--> Plate
Table --supports--> Book
Shelf --supports--> Box
Box --contains--> Ball
Chair --next_to--> Table5.2 3D 场景图（3D Scene Graphs）
3D 场景图在传统场景图基础上引入层次化的空间结构：
层次结构：
Level 4: Building（建筑）
Level 3: Room（房间）
Level 2: Place（局部区域，由导航图连接）
Level 1: Object（物体，含 3D 边界框和语义标签）
Level 0: Metric-Semantic Mesh（底层几何+语义网格）代表工作：
- Hydra（MIT SPARK Lab, 2022）：实时构建分层 3D 场景图，从视觉 SLAM 的网格中自动提取物体、房间、建筑的层次结构
- ConceptGraphs（2023）：利用开放词汇检测（Open-Vocabulary Detection）和 LLM 构建语义丰富的 3D 场景图
5.3 ConceptFusion（2023）
ConceptFusion 将预训练视觉语言模型（如 CLIP）的特征融合到 3D 重建的每个点上：
处理流程：
1. RGB-D 序列 → 3D 重建（TSDF / Neural Radiance Field）
2. 每帧 RGB 通过 CLIP Image Encoder → 像素级特征
3. 利用深度信息将 2D 特征投影到 3D 点上
4. 多帧特征在 3D 空间中融合平均
5. 查询时：文本经 CLIP Text Encoder 编码，与 3D 点特征计算余弦相似度
结果：任何自然语言查询都能在 3D 空间中定位对应区域5.4 SayPlan（2023）
SayPlan 将 3D 场景图作为 LLM 任务规划的结构化输入：
SayPlan 架构：
┌──────────────────────────────────────────┐
│ LLM Task Planner │
│ 输入: 自然语言指令 + 场景图文本描述 │
│ 输出: 高层动作序列 │
└─────────────────┬────────────────────────┘
│
┌─────────────────▼────────────────────────┐
│ 3D Scene Graph │
│ 提供: 物体列表、空间关系、房间结构 │
│ 支持: 图搜索定位目标物体 │
└─────────────────┬────────────────────────┘
│
┌─────────────────▼────────────────────────┐
│ Scene Graph Simulator │
│ 验证: 动作执行后的状态变化是否合理 │
│ 反馈: 不可行动作返回给 LLM 重新规划 │
└──────────────────────────────────────────┘SayPlan 的关键贡献：LLM 无需处理原始点云或图像，场景图将物理世界压缩为符号化的图结构，显著降低了 LLM 的推理负担。同时，场景图的图搜索能力使得 LLM 能高效处理大规模环境（多房间、多楼层）中的长距离导航与操作任务。
5.5 场景图在规划体系中的位置
感知层输出 规划层使用方式
────────── ──────────────
点云 + 分割 ─────┐
├───▶ 场景图构建 ───▶ 任务规划器输入
语义标签 ─────┤ (物体可操作性查询)
│
空间关系推理 ─────┘ ───▶ 运动规划器输入
(碰撞物体位姿)
───▶ 抓取规划器输入
(目标物体位姿+形状)场景图的核心价值：将连续的感知数据（点云、图像）转化为离散的符号表征（物体、关系），使得上层任务规划能在符号空间中高效搜索，同时保留足够的几何信息供运动规划使用。
6. NVIDIA cuRobo
6.1 设计动机
传统运动规划（如 OMPL 中的 RRT/RRT*）在 CPU 上运行，对于 7-DoF 机械臂的典型场景，规划时间在 100ms 至数秒之间。这在工业生产的高节拍（High Takt Time）场景和需要实时重规划的动态环境中不可接受。cuRobo 将运动规划的核心计算（碰撞检测、轨迹优化）迁移到 GPU，实现毫秒级规划。
6.2 技术架构
┌─────────────────────────────────────────────────────────┐
│ cuRobo 架构 │
├─────────────────────────────────────────────────────────┤
│ │
│ ┌───────────────────────────────────────────────────┐ │
│ │ Motion Generation │ │
│ │ 输入: 当前 q, 目标位姿/关节角 │ │
│ │ 输出: 时间最优轨迹 q(t) │ │
│ └───────────────────────┬───────────────────────────┘ │
│ │ │
│ ┌───────────────────────▼───────────────────────────┐ │
│ │ GPU-Parallel Trajectory Optimization │ │
│ │ │ │
│ │ ┌─────────┐ ┌─────────────┐ ┌─────────────┐ │ │
│ │ │ L-BFGS │ │ Particle │ │ Graph-based │ │ │
│ │ │ (local) │ │ Optimization│ │ Warm Start │ │ │
│ │ └─────────┘ └─────────────┘ └─────────────┘ │ │
│ └───────────────────────┬───────────────────────────┘ │
│ │ │
│ ┌───────────────────────▼───────────────────────────┐ │
│ │ GPU Collision Checker │ │
│ │ │ │
│ │ 世界表征: Cuboids, Meshes, NVBLOX (TSDF/ESDF) │ │
│ │ 机器人表征: Sphere decomposition │ │
│ │ 并行度: 数千条轨迹同时检测碰撞 │ │
│ └───────────────────────────────────────────────────┘ │
│ │
│ ┌───────────────────────────────────────────────────┐ │
│ │ CUDA Robotics Kernels │ │
│ │ 正运动学 (FK)、雅可比计算 (Jacobian)、 │ │
│ │ SDF 查询、插值 │ │
│ └───────────────────────────────────────────────────┘ │
│ │
└─────────────────────────────────────────────────────────┘6.3 核心技术点
球体分解（Sphere Decomposition）：
将机器人的每个 link 近似为一组球体。球与环境的碰撞检测只需计算球心到障碍物的距离是否小于球半径，计算简单且可高度并行化。
碰撞检测计算：
对于机器人 link 上的球体 S(center=c, radius=r)
与环境 SDF 值 d = SDF(c):
碰撞状态 = d < r
穿透深度 = r - d (当 d < r)
梯度方向 = ∇SDF(c) (用于优化时推离障碍物)并行多起点优化（Parallel Multi-start Optimization）：
cuRobo 同时从数百个不同初始轨迹出发进行优化（使用 L-BFGS），取代价最低的结果。这在 GPU 上的开销几乎与单次优化相同。
Graph-based Warm Start：
预计算一个 C-space 路线图（类似 PRM），对新的规划请求先在图上搜索粗略路径作为初始解，再进行轨迹优化。显著减少优化迭代次数。
6.4 性能数据
| 指标 | cuRobo (GPU) | MoveIt 2 + OMPL (CPU) | 加速比 |
|---|---|---|---|
| 单次规划时间（Franka 7-DoF） | 5ms 至 50ms | 100ms 至 2000ms | 20x 至 100x |
| 批量规划（1000 问题） | 数秒 | 数十分钟 | 100x+ |
| 碰撞检测（每条轨迹） | 亚毫秒 | 数毫秒 | 10x+ |
| 支持动态环境重规划 | 是（实时 ESDF 更新） | 有限 | N/A |
6.5 与 Isaac Sim 的集成
cuRobo 与 NVIDIA Isaac Sim 深度集成：
- Isaac Sim 提供高保真物理仿真环境
- NVBLOX 实时生成 ESDF（Euclidean Signed Distance Field），作为 cuRobo 的碰撞检测输入
- Isaac Sim 中的 Replicator 可生成合成数据用于训练感知模型
- 端到端流程：感知（Isaac Sim 传感器） → 场景理解（NVBLOX ESDF） → 运动规划（cuRobo） → 控制执行
6.6 局限性
- 需要 NVIDIA GPU（CUDA 依赖），不适用于 CPU-only 嵌入式平台
- 球体分解是几何近似，对薄板状物体精度有限
- 当前主要优化关节空间轨迹，笛卡尔空间约束支持有限
- 局部优化方法，在高度约束的窄通道场景可能陷入局部最优
7. 公司与技术格局
7.1 规划与决策层核心玩家
| 公司/组织 | 核心技术/产品 | 定位 | 关键特点 |
|---|---|---|---|
| Open Robotics | MoveIt 2 | 开源运动规划框架 | ROS 2 生态标准，OMPL 集成，社区驱动 |
| NVIDIA | cuRobo, Isaac Sim | GPU 加速规划 | 毫秒级规划，Isaac 生态闭环 |
| Intrinsic (Google/Alphabet) | Flowstate | 工业机器人编程平台 | 简化部署，收购 Vicarious/Everyday Robots 技术 |
| Covariant | Covariant Brain | AI-driven 仓储拣选 | 端到端学习 + 规划，商业化部署 |
| DeepMind | MuJoCo, 研究发表 | 物理仿真与控制研究 | MuJoCo 开源，MPPI/RL 研究前沿 |
| ETH Zurich (ANYbotics) | ANYmal MPC stack | 足式运动规划 | NMPC + 全身控制，acados 框架 |
| MIT CSAIL/SPARK | Drake, Hydra | 优化控制与场景理解 | Drake 数学优化框架，3D 场景图 |
| Realtime Robotics | RapidPlan | FPGA 加速运动规划 | 硬件加速，工业部署 |
| PickNik Robotics | MoveIt Pro | MoveIt 商业版 | 企业支持，高级功能 |
7.2 开源生态
| 项目 | 语言 | 用途 | 许可证 |
|---|---|---|---|
| MoveIt 2 | C++ | 运动规划框架 | BSD |
| OMPL | C++ | 采样规划算法库 | BSD |
| Drake | C++ | 多体动力学 + 优化控制 | BSD |
| cuRobo | Python/CUDA | GPU 加速规划 | NVIDIA License |
| PyBullet | Python/C++ | 物理仿真 + 规划 | Zlib |
| CasADi | C++/Python | 符号化数值优化 | LGPL |
| acados | C | 实时最优控制 | BSD |
| BehaviorTree.CPP | C++ | 行为树执行引擎 | MIT |
| Fast Downward | C++ | PDDL 求解器 | GPL |
| FCL | C++ | 碰撞检测 | BSD |
7.3 技术路线对比
当前规划与决策层存在两条主要技术路线：
路线 A：经典规划 + 优化
─────────────────────────
任务层: PDDL / 行为树 / 手工设计的状态机
运动层: OMPL (RRT/PRM) + 轨迹优化 (CHOMP/TrajOpt)
控制层: MPC (线性/非线性)
优势: 可解释性强，安全保证明确，工业成熟度高
劣势: 需要精确模型，泛化能力有限，难以处理开放世界
路线 B：LLM/VLM + 学习策略
─────────────────────────
任务层: LLM (SayCan/Code as Policies) + 场景图
运动层: 学习的策略 (Diffusion Policy / ACT) 或 cuRobo
控制层: RL-based 全身控制
优势: 开放词汇理解，泛化能力强，无需手动建模
劣势: 安全性难以保证，幻觉风险，可解释性差
路线 C（趋势）：混合架构
─────────────────────────
任务层: LLM 做高层分解 + 经典规划做子任务求解
运动层: cuRobo (GPU 优化) + 学习的抓取规划
控制层: MPC + RL residual
安全层: 运动规划做可行性验证，MPC 做约束保证7.4 投资视角的关键判断点
| 判断维度 | 关键问题 |
|---|---|
| 实时性 | 规划是否能在控制周期（1ms 至 10ms）内完成？GPU 加速是否为刚需？ |
| 泛化性 | 系统是否能处理未见过的物体和场景？是否依赖大量手工建模？ |
| 安全性 | 是否有碰撞避免保证？LLM 幻觉是否会导致物理危险？ |
| 部署成本 | 是否需要 GPU 集群？嵌入式部署是否可行？ |
| 数据飞轮 | 部署后是否能持续改进规划质量？数据壁垒是否存在？ |
参考文献与资源
经典论文
- LaValle, S.M. (1998). “Rapidly-Exploring Random Trees: A New Tool for Path Planning.” TR 98-11.
- Karaman, S. & Frazzoli, E. (2011). “Sampling-based Algorithms for Optimal Motion Planning.” IJRR.
- Ratliff, N. et al. (2009). “CHOMP: Gradient Optimization Techniques for Efficient Motion Planning.” ICRA.
- Schulman, J. et al. (2014). “Motion Planning with Sequential Convex Optimization and Convex Collision Checking.” IJRR.
- Williams, G. et al. (2017). “Model Predictive Path Integral Control: From Theory to Parallel Computation.” JGCD.
- Di Carlo, J. et al. (2018). “Dynamic Locomotion in the MIT Cheetah 3 Through Convex Model-Predictive Control.” IROS.
LLM-based Planning
- Ahn, M. et al. (2022). “Do As I Can, Not As I Say: Grounding Language in Robotic Affordances.” (SayCan)
- Liang, J. et al. (2022). “Code as Policies: Language Model Programs for Embodied Control.”
- Huang, W. et al. (2022). “Inner Monologue: Embodied Reasoning through Planning with Language Models.”
- Huang, W. et al. (2024). “ReKep: Spatio-Temporal Reasoning of Relational Keypoint Constraints for Robotic Manipulation.”
抓取与场景理解
- Fang, H. et al. (2020). “GraspNet-1Billion: A Large-Scale Benchmark for General Object Grasping.” CVPR.
- Fang, H. et al. (2023). “AnyGrasp: Robust and Efficient Grasp Perception in Spatial and Temporal Domains.” T-RO.
- Sundermeyer, M. et al. (2021). “Contact-GraspNet: Efficient 6-DoF Grasp Generation in Cluttered Scenes.” ICRA.
- Rosinol, A. et al. (2020). “3D Dynamic Scene Graphs: Actionable Spatial Perception with Places, Objects, and Humans.” RSS.
- Jatavallabhula, K.M. et al. (2023). “ConceptFusion: Open-set Multimodal 3D Mapping.” RSS.
- Rana, K. et al. (2023). “SayPlan: Grounding Large Language Models using 3D Scene Graphs for Scalable Robot Task Planning.”
系统与框架
- Sucan, I.A. et al. (2012). “The Open Motion Planning Library.” IEEE Robotics & Automation Magazine. (OMPL)
- Coleman, D. et al. (2014). “Reducing the Barrier to Entry of Complex Robotic Software: a MoveIt! Case Study.” (MoveIt)
- Sundaralingam, B. et al. (2023). “cuRobo: Parallelized Collision-Free Robot Motion Generation.” ICRA.
教材
- LaValle, S.M. (2006). “Planning Algorithms.” Cambridge University Press.
- Lynch, K.M. & Park, F.C. (2017). “Modern Robotics: Mechanics, Planning, and Control.”
- Siciliano, B. et al. (2009). “Robotics: Modelling, Planning and Control.” Springer.