Robotic Embodied Intelligence - From Zero to Hero

6.3.1 关节空间轨迹 vs 末端位姿轨迹

在上一节中，我们已经从动力学和低层控制的角度讨论了机器人“怎么动得稳”。本节开始从任务层 出发，讨论“应该规划一条什么样的动作轨迹”，以及这些轨迹如何被编码为适合学习和控制的表示。

从本质上看，轨迹规划可以在两种空间中进行：

• 关节空间（joint space）：直接对各个关节角度随时间的变化进行规划；
• 笛卡尔空间 / 操作空间（Cartesian / task space）：对末端执行器的空间位姿进行规划。(mclab-me.pusan.ac.kr)

理解这两种描述方式及它们的折中，是后续设计 VLA 模型动作表示的基础。

6.3.1.1 关节空间轨迹：直接在关节角度上规划

定义与表示形式 设机器人有 \(\displaystyle n\) 个关节，关节向量记为

\[ q(t) = [q_1(t), q_2(t), \dots, q_n(t)]^\top \]

关节空间轨迹就是在给定时间区间内，为每个关节规划一条随时间变化的函数 \(q_i(t)\)，通常要求这些轨迹在位置、速度、加速度上都是连续且平滑的（例如多项式插值、样条插值）。(哥伦比亚大学计算机学院)

优点：天然满足关节约束与动力学约束

• 易于施加关节限制：关节速度、加速度、力矩等限制天然定义在关节空间中，因此通过限制 \(q(t)、\dot q(t)、\ddot q(t)\) 的边界可以直接保证不超限。
• 与低层控制匹配：大多数工业机器人伺服控制器的内部接口本来就是“期望关节角/关节速度”，因此规划出的关节轨迹可以几乎直接下发。(首尔科技大学)
• 数值稳定：在关节空间进行插值，可以避免在接近奇异位形时末端轨迹出现过大的速度或加速度。

缺点：在任务空间不直观

• 路径形状“不可见”：给定两个关节配置 \(q_\text{start}\) 和 \(q_\text{goal}\)，在关节空间做一条看似平滑的插值，映射到末端的路径可能会绕远路、穿过障碍物，甚至在某些姿态下关节严重弯折，这在规划时并不直观。(ResearchGate)
• 难以精确约束末端行为：比如“末端保持竖直、沿直线从 A 移到 B”，关节空间轨迹很难直接表达这样的几何约束。

典型使用场景

• 无人协作、环境比较空旷的工业场景（如点到点搬运），只要末端在起点和终点准确即可，中间路径没那么重要；
• 对动力学和关节力矩有严格要求的高速运动（如喷涂、高速搬运），需要在轨迹规划阶段就考虑关节加速度和力矩极限。

【图 6-3-1 占位：二维平面 2 自由度机械臂，从同一个起点到终点，展示“关节空间线性插值”和“末端空间直线轨迹”在工作空间中的路径差异。】

6.3.1.2 末端位姿轨迹：在笛卡尔空间中规划路径

定义与表示形式 末端执行器的位姿通常记为

\[ x(t) = {p(t), R(t)} \]

其中 \(p(t)\) 为位置，\(R(t)\) 为姿态（可用旋转矩阵、欧拉角或四元数等表示）。笛卡尔空间轨迹就是为 \((p(t), R(t))\) 直接规划一条路径，例如“末端沿直线或圆弧移动，同时保持某个方向”。(diag.uniroma1.it)

规划完成后，需要通过逆运动学（IK） 将每个时间步的末端位姿 \(x(t)\) 转换为关节配置 \(q(t)\)，再交由低层控制器执行（对应 6.1.4 中的 IK 内容在任务层的应用）。

优点：直观且易于表达几何约束

• 路径可视化清晰：工程师可以直接在工作空间中画出“末端从桌面上方某点沿直线插入盒子内”的轨迹，便于人类理解与调试。
• 易于避障与安全约束：障碍物、禁行区域等都自然定义在工作空间中，在笛卡尔空间规划路径更便于保证末端不穿透桌面、不碰撞墙体。
• 方便与视觉/语言对齐：视觉识别得到的是物体在相机或世界坐标系下的位置，语言指令也常以“把某物移动到某处”表述，从末端位姿出发可以减少表示上的跨度，这一点与后续 VLA 模型中的“操作空间动作 token”有很好呼应。

缺点：需要逆运动学与奇异性处理

• 需要 IK 求解：对于高自由度或构型复杂的机械臂，解析 IK 不一定存在，只能用数值迭代，可能出现收敛失败或多解选择问题。(doc.aldebaran.com)
• 奇异位形与关节极限问题：规划在笛卡尔空间是连续的，但映射到关节空间时可能经过奇异位形或导致某些关节角接近极限，因此必须在 IK 与轨迹生成中额外检查。
• 动力学约束不显式：末端路径看起来很好，但对应的关节加速度和力矩不一定在安全范围内，通常需要额外的时间尺度调整或再优化。

典型使用场景

• 需要严格控制末端路径形状的任务：例如“沿直线插入孔内”、“沿焊缝轨迹焊接”、“沿边缘擦拭”。
• 对“对人安全”和“可解释路径”有要求的人机协作场景：操作员希望看到末端的动作是可理解、可预期的。

6.3.1.3 混合规划：在笛卡尔空间设关键节点，在关节空间生成可执行轨迹

实际系统中，常用的并不是“纯关节空间”或“纯笛卡尔空间”两极，而是两者的混合策略：

1. 在笛卡尔空间指定关键路径点（waypoints）
   • 例如抓取任务中：
     • 远离物体的“预抓取位姿”；
     • 靠近物体表面的“接近位姿”；
     • 真正抓取时的“抓取位姿”；
     • 抬起后的“撤离位姿”。(MoveIt)
   • 这些关键位姿由感知模块（视觉、力觉）和任务规划模块共同产生，与 8 章中的高层规划输出一致。
2. 对关键点之间进行插值
   • 在笛卡尔空间做位置和姿态插值（直线、样条），得到一串更密的末端位姿样本。
3. 调用 IK 生成关节轨迹
   • 对每个插值后的末端位姿调用 IK 求解得关节角；
   • 在关节空间对这些离散点再次进行平滑（如时间重分配、多项式拟合），保证速度、加速度连续，最终获得可执行的关节轨迹。(哥伦比亚大学计算机学院)

这种“笛卡尔空间规划 + 关节空间执行”的混合方式，既保留了末端路径的直观性，又充分利用了关节级限幅与动力学约束的优势，是工程实践和现代机器人学习系统里最常见的做法。后续在讨论“动作原语”和“抓取模式”时，可以把每个原语理解为：在笛卡尔空间定义了一些关键位姿和运动形状，再自动转化为底层关节轨迹。

【图 6-3-2 占位：展示一个抓取任务的关键笛卡尔路径点（预抓取、接近、抓取、撤离）以及这些点通过 IK 映射到关节空间轨迹的示意图。】

6.3.2 Delta Pose / Velocity 命令的含义

在具身智能与 VLA 模型中，一个常见的设计是让策略网络直接输出“增量位姿”或“末端速度”命令，而不是最终轨迹。这类命令与 6.2 中的低层控制接口紧密相连：高层在较低频率上输出 delta/速度，低层控制器在高频率上进行插值和闭环控制。

6.3.2.1 增量位姿命令（Delta Pose）

基本形式 若当前末端位姿为 \((x_t = {p_t, R_t})\)，高层策略输出一个增量位姿

\[ \Delta x_t = {\Delta p_t, \Delta R_t} \]

则下一步目标位姿为

\[ x_{t+1} = x_t \oplus \Delta x_t \]

其中 \(\oplus\) 表示位姿的组合（位置相加、姿态用旋转复合，例如四元数相乘）。

在实现上，这通常表现为：

• 高层每隔 \(20 \sim 100\) ms 计算一次较小的位姿增量（单位可以是 m、rad 或归一化后的步长）；
• 低层控制器将 \((x_{t+1})\) 作为当前位置的短距离目标，通过 IK + 关节控制使末端快速靠近该目标；
• 连续叠加这些小增量，就形成了一条平滑轨迹。

优点与直觉

• 相对命令，更适合闭环调整：策略只需要“往前挪一点”、“向左偏一点”，对环境变化的响应更自然；
• 数值平稳：每一步增量较小，降低了策略输出突然大幅跳变的风险，对强化学习和模仿学习都更友好；
• 和视觉伺服自然结合：可将视觉误差（例如末端与目标的偏差）直接映射为下一步的增量位姿，实现“看到一点，改一点”的控制模式。(PMC)

在 VLA 实现中，Delta Pose 往往被离散化成若干 token（例如“x 方向 +1 单位格”、“z 方向 -1 单位格”、“绕 z 轴旋转 +5°”），从而可以由 Transformer 等序列模型输出。

6.3.2.2 速度命令（Velocity Command）

另一种常见方式是直接在操作空间输出末端线速度 + 角速度：

\[ v_t = [v_x, v_y, v_z, \omega_x, \omega_y, \omega_z]^\top \]

在时间步长 \(\Delta t\) 下，末端位姿的更新可近似为

\[ p_{t+1} \approx p_t + v_{lin,t} \cdot \Delta t,\quad R_{t+1} \approx R_t \cdot \exp(\hat\omega_t \Delta t) \]

低层控制器在更高频率下根据这些速度设定关节速度，积分得到真实轨迹。

特点

• 连续控制感更强：在时间上看，机器人“持续地朝某个方向运动”，非常适合描述平滑运动、推拽、擦拭等操作；
• 方便施加速度上限：在接口层限制 \((|v_t|)\) 即可保证安全，不会出现瞬时大位移；
• 与动力学控制兼容：在力控或阻抗控制框架下，期望速度往往是控制方程中的显式变量，用速度命令更易结合。

速度命令在工业控制与视觉伺服中应用广泛，例如通过图像误差生成运动速度，确保末端在摄像机视野中按照一定速率收敛到目标。(doc.aldebaran.com)

6.3.2.3 相对命令 vs 绝对命令：何时用哪一种？

从抽象上看，可以把控制命令分成两类：

• 绝对命令（absolute command）
  • 直接给出目标关节位置 \(q^{*}\) 或目标末端位姿 \(x^{*}\)；
  • 控制器的任务是“尽快把机器人从当前状态拉到目标”，典型于点到点快速定位、搬运。
• 相对命令（relative command）
  • Delta Pose、速度命令都属于“相对控制”：每一步都在当前状态的基础上做一个小改动；
  • 更适合连续跟踪、细致操作，以及在执行过程中不断根据传感反馈修正轨迹。

如何选择？可以粗略遵循以下原则：

• 任务几何结构简单、只需要把物体“从 A 搬到 B”，且环境基本静态 → 用绝对命令更简单，和关节空间轨迹规划相似；
• 任务需要持续闭环调整，例如抓取时对准物体、旋转瓶盖、插销入孔等 → 用相对命令更合适，控制策略学到的是“误差→修正”的局部规则；
• 在 VLA 中，绝对命令更像“规划一个目标位姿”，相对命令更像“一步一步执行策略”；两者在 8 章中会分别对应“高层子目标输出”和“低层连续控制输出”。

6.3.3 动作原语（skill primitives / motion primitives）

有了轨迹与命令的基本概念，下一步是建立更高层的动作抽象 。 如果把“Delta Pose / Velocity 命令”看作机器人控制的“字母”，那么动作原语 就对应“单词”或“短语”：在一个较长时间窗口内实现一个相对完整的小目标，例如“抓起桌上的杯子”、“沿边缘推一段距离”。(arXiv)

6.3.3.1 概念：常用子动作的封装

动作原语（motion / skill primitive） 通常具有以下几个特征：

1. 时间上有限、空间上局部
   • 原语持续一段有限时间（例如 0.5–3 秒），在一个局部区域内完成特定动作。
2. 内部包含固定的控制逻辑或轨迹模板
   • 例如“抓取”原语内部可能包含：接近、闭合手爪、检测抓取成功、微调姿态等一整套控制流程；
   • 这些流程可以直接由规则 + 控制算法手写，也可以由学习得到的策略来实现。(CMU Robotics Institute)
3. 参数化
   • 一个原语往往带有少量参数，如目标位置、目标朝向、接近方向等；
   • 同一个“抓取”原语，通过不同参数化就能抓取桌上不同位置、不同朝向的物体。(arXiv)

典型原语包括：抓取（grasp） 、放置（place） 、推动（push） 、拉动（pull） 、擦拭（wipe） 、开门（open-door） 等。很多近期的机器人强化学习和终身学习工作，都是在这些原语上进行组合和参数学习。(CVF开放获取)

6.3.3.2 组合复杂任务：从“原语库”到“任务脚本”

引入动作原语的直接收益，是能够从离散层次 上拼装复杂任务：

• 对于“收拾桌面”这样的复杂任务，可以粗略写成：
  • 重复执行：{找到一个物体 → 抓取 → 搬运 → 放置到收纳盒}；
• 对于“装配任务”，可以组合：
  • {抓起零件 → 插入槽位 → 施加压力到底 → 松开 → 撤离}。

在系统架构上，常见做法是：

1. 高层任务规划器（symbolic planner / LLM / VLA 高层）

   • 在符号或语言层面决定原语序列：

     \[ \text{Plan} = [\text{Grasp}(obj_1),\ \text{Place}(obj_1,\ box),\ \text{Grasp}(obj_2), \dots] \]

2. 原语调度与参数选择

   • 对每个原语，基于当前视觉和状态选择参数：目标位置、方向、力阈值等。

3. 原语内部控制与反馈

   • 原语内部运行自己的轨迹规划与闭环控制，一旦完成或失败，向上层报告结果。

这种层次结构使得系统更易解释、调试和扩展：

• 如果“抓取”失败，多半可以定位到“抓取原语”的实现或参数，而不是整条长链条；
• 对 VLA 模型而言，也可以设计成“预测下一步应该调用哪个原语，以及原语参数”，而不是直接在连续控制空间中长时间滚动控制。

6.3.3.3 学习动作原语：从示教到强化学习

动作原语可以由工程师手工设计，但随着任务增多和环境复杂，完全手工定义会变得困难，因此研究界大量工作关注从数据中学习动作原语 。(dfki.de)

常见思路包括：

1. 示教学习（Learning from Demonstration）
   • 从人类示范中分割出若干“重复出现的子动作片段”，通过聚类或序列建模发现潜在原语；
   • 为每个原语学习一个条件策略：输入局部观测 → 输出 Delta Pose / 速度命令；
   • 原语的参数来自于示范中的“接触点、目标位姿”等。
2. 基于强化学习的原语优化
   • 先指定一个粗略的原语模板（例如“向目标点接近→闭手爪→抬起”），再用强化学习在模拟或现实中微调内部控制策略，以提高成功率、鲁棒性和效率。(arXiv)
3. 多任务共享的原语
   • 不同技能（“抓香蕉”、“放锅”、“推门”）在空间上往往共享一些底层动作模式（如“从侧面接近并闭手爪”），可以用共享原语库 建模，再针对不同任务学习高层组合和参数调节。(CVF开放获取)

在 VLA 框架中，这些学得的原语可以进一步被“语言化”：

• 用自然语言描述原语的语义（如“轻轻抓起杯子”、“沿边缘推到尽头”），
• 让大语言模型理解和调用这些原语，为“从语言到动作”的桥接提供更结构化的中间层。

6.3.4 抓取与操作任务的典型动作模式

最后，我们用“抓取”和“放置”等常见操作任务，具体展示一个动作从任务层描述 → 轨迹表示 → 控制执行 的大致模版。这些典型模式也可以直接对应到上一节的动作原语。

6.3.4.1 抓取动作分解：接近、对准、抓紧、提起

一个看似简单的“抓起物体”动作，实际可以拆分为以下阶段：(MoveIt)

1. 接近（approach）
   • 从安全距离上的“预抓取位姿”开始，沿着设计好的路径接近物体附近；
   • 通常要求路径避开障碍，并保持手爪朝向大致合理，以便后续微调。
2. 对准（align）
   • 利用视觉或力觉反馈，逐渐修正末端位姿，使手爪与物体表面或主轴线对齐；(MDPI)
   • 对准过程往往使用 Delta Pose 或速度命令实现小幅调整，可能包含：
     • 平移到物体中心上方或侧面；
     • 旋转使夹爪面与物体抓取面平行；
     • 通过轻触桌面、检测力变化确认高度。
3. 抓紧（grasp）
   • 在对准后缓慢闭合夹爪，监控力传感器或夹爪开合位置，确认物体已被稳定夹持；
   • 一些研究提出顺应性抓取原语，通过控制手指与支撑面的柔顺接触，显著提高抓取成功率。(CMU Robotics Institute)
4. 提起（lift）
   • 在确认抓取成功后，沿着安全方向（通常是竖直向上）抬起物体，避免与周围物体碰撞；
   • 这一阶段结束时，机器人进入“携带物体”的状态，为后续放置或操作做好准备。

在具身智能系统中，每个阶段都可以被视作一个原语（“ApproachPrimitive”、“AlignPrimitive”、“GraspPrimitive”、“LiftPrimitive”），高层策略只需决定何时切换阶段及其参数。

【图 6-3-3 占位：抓取动作四阶段序列示意图，每个阶段配一帧图片（接近、对准、闭合、提起），并用箭头标注末端运动方向。】

6.3.4.2 放置动作分解：移动、接触、松开、撤离

与抓取相对的“放置（place）”动作，同样可以拆成多个阶段，且要求在接触与松开过程中保持物体的稳定和姿态正确。(MoveIt)

1. 移动到目标位置附近
   • 根据任务指令（如“将杯子放到托盘中央”）与感知信息，规划一条轨迹，将物体移动到目标区域上方一定高度的“预放置位姿”；
   • 此阶段仍然以末端位姿轨迹规划和避障为主。
2. 接触（place down）
   • 缓慢沿法向方向下降末端，使物体与目标表面接触；
   • 可以通过力传感器检测接触力是否在合理范围内，避免砸下去或悬空。
3. 松开夹爪
   • 在物体已被支撑面稳定支撑的前提下，缓慢松开夹爪，并监控物体在图像或力觉上的反馈，确保未被带动滑动或翻倒；
   • 某些精密装配任务中，会引入“推压/微调”原语，在松开前先用小力将物体推至定位边缘。(MDPI)
4. 撤离（retreat）
   • 松开后沿着安全路径撤回末端，避免带动周围物体或重新碰撞刚刚放置的物体；
   • 撤离路径常设计为先沿法向抬高，再在远离目标的方向移动。

在 MoveIt 等机器人软件中，完整的 “pick-and-place” pipeline 通常就是：

• 抓取阶段：approach → grasp → lift；
• 放置阶段：move → place down → release → retreat。(MoveIt)

6.3.4.3 其他操作模式：旋转、插入与高要求操作

除了抓取与放置，还有一类对对准和力控要求更高的操作任务，例如：

1. 旋转（如旋开瓶盖、拧螺丝、扭门把）
   • 通常需要：
     • 抓取并固定物体本体（瓶身、螺母等）；
     • 控制末端绕特定轴施加旋转位姿或扭矩；
     • 监控旋转角度与力矩变化，判断是否已经旋开或拧紧。
   • 这里，轨迹表示不仅包括位置和姿态，还需要显式地考虑力/扭矩限幅与顺应性控制。(CMU Robotics Institute)
2. 插入（如插销入孔、插拔插头、装配轴–孔结构）
   • 对几何对准精度 要求高，误差容易导致卡死；
   • 常见模式是：
     • 在远处通过视觉对准，规划大致插入方向；
     • 靠近后使用小步长 Delta Pose + 力控，边轻触边调整姿态；
     • 在检测到插入深度或力反馈满足条件后，进入完全插入阶段。(MDPI)
3. 推 / 拉 / 调整位置
   • 与抓取不同，推/拉往往利用环境约束（桌面边缘、槽口等）辅助定位，如通过推到边缘确保物体位置准确；
   • 典型做法是设计“推拽原语”，内部控制末端施加持续速度和限定力，并用视觉或位姿反馈判断是否到达目标。(MDPI)

这些高要求操作的共同特点是：

• 对轨迹局部形状 和接触力 非常敏感；
• 通常配合相对命令（Delta Pose / 速度） 和阻抗/顺应控制 实现细颗粒度的调整；
• 很适合作为独立的动作原语，通过示教和强化学习进行专门训练。

【图 6-3-4 占位：展示三类操作模式的典型示意： （a）旋开瓶盖：夹持瓶身 + 绕 z 轴旋转； （b）插销入孔：先对准、再沿孔轴缓慢插入； （c）利用环境约束的推拽：沿桌面边缘推动物体到定位位置。】

本小节到此为止，你可以把 6.3.1–6.3.4 看作是从“连续控制信号”上升到“任务动作单元”的第一层抽象：

• 关节空间 vs 末端位姿轨迹明确了“在哪个空间里描述运动”；
• Delta Pose / 速度命令是“步进式”的控制接口；
• 动作原语和典型抓取/操作模式则为后续 VLA 模型提供了“结构化动作词汇表”。

在第 8 章中，我们会进一步看到，这些概念如何被编码为 token、序列和模块，使得视觉–语言–动作模型能在统一的框架下学习和调用这些动作能力。