Part 4: Algorithms · The robot's brain
Chapter 11
MoveIt 2 - 机器人手臂的“大脑”
你站在冰箱前面，想拿最高层架子上的一个杯子。你的手从身体侧面出发，绕过半开的冰箱门，避开旁边的调料瓶，最后精确地握住杯子的把手。整个过程大概两秒，你甚至没怎么想。
但你的身体其实做了一件非常复杂的事：肩关节、肘关节、腕关节同时在多个方向上旋转，彼此配合，让你的手沿着一条不碰任何东西的路径到达目标。你的大脑在实时计算关节角度、避免碰到冰箱门和调料瓶、同时保持手的朝向适合抓握。
这就是机器人手臂面对的核心问题：怎么让末端执行器（比如夹爪）从当前位置移动到目标位置，同时不撞到自己也不撞到环境中的任何东西。 Nav2 解决了底盘在平面上的导航，MoveIt 2 解决的是手臂在三维空间中的运动规划。如果说 Nav2 是机器人的“高德地图”，MoveIt 2 就是手臂的“大脑” - 它告诉每个关节该转多少度、什么时候转、按什么顺序转。
两种描述手臂状态的语言
在聊运动规划之前，有一个基础概念必须先搞清楚，因为后面所有东西都建立在它之上。
描述一台机械臂“在哪”有两种完全不同的方式。
关节空间（Joint Space） 用每个关节的角度来描述手臂状态。一个 6 自由度的机械臂有 6 个关节，每个关节一个角度值，所以它的状态就是一个 6 维向量：[q1, q2, q3, q4, q5, q6]。比如 [0, -45, 90, 0, 45, 0] 度 - 每个数字对应一个关节转了多少。这是对电机最友好的表达方式，因为你可以直接把这些角度值发给各个关节的伺服驱动器。
笛卡尔空间（Cartesian Space / Task Space） 用末端执行器在三维世界中的位置和朝向来描述。一个完整的末端姿态需要 6 个值：三个平移 [x, y, z] 加三个旋转 [roll, pitch, yaw]。比如“夹爪在桌面上方 30 厘米处，开口朝下” - 这是对人类最直观的表达方式。
这两种描述之间的转换，就是运动学要解决的事情。
正运动学和逆运动学 - FK 和 IK
正运动学（Forward Kinematics, FK） 的问题是：已知每个关节的角度，手臂末端在哪？
这是一个纯数学问题，而且有唯一解。你把每个连杆的长度和关节角度代入一系列变换矩阵相乘，就能算出末端的位置和朝向。虽然矩阵乘法看起来吓人，但对计算机来说不过是几次矩阵运算，微秒级就能出结果。
逆运动学（Inverse Kinematics, IK） 是反过来的问题：我希望夹爪到达空间中的某个位置和朝向，每个关节应该转多少度？
这个问题难得多。难在三个地方：
第一，可能无解。目标位置也许超出了手臂的工作范围 - 比如你让一台臂展 50 厘米的手臂去够 1 米外的物体，物理上就不可能。或者目标位置在工作范围内，但要求的朝向做不到 - 比如要求夹爪在某个位置朝上，但关节的旋转极限不允许。
第二，可能有多个解。这才是更常见的麻烦。想象你要碰自己的后脑勺 - 你可以从头顶绕过去，也可以从耳朵旁边绕过去，两种姿势都能到。对 6 自由度手臂来说，一个目标位置可能有 8 组甚至更多的关节角度组合都能到达。选哪一组？离当前姿态最近的？关节运动量最小的？避开障碍物的？这是一个需要策略的选择。
第三，数值稳定性。实际工程中大多数 IK 求解器用的是数值迭代方法（不是解析解），所以存在收敛速度和精度的 trade-off。MoveIt 2 默认使用的 IK 求解器是 KDL（来自 Orocos 项目），但很多团队会换成 TRAC-IK - 它结合了数值法和一个基于 SQP 的优化方法，在处理关节极限约束时表现明显更好，失败率比 KDL 低很多。
运动规划 - 在高维迷宫里找路
知道起点（当前关节角度）和终点（IK 算出的目标关节角度）还不够。手臂不能“瞬间跳”到目标姿态 - 它需要沿着一条路径连续地移动过去。而这条路径上的每一个中间姿态，都不能让手臂撞到任何东西。
这就是运动规划（Motion Planning）要做的事。
配置空间（C-Space）
理解运动规划的关键概念是 配置空间（Configuration Space, C-Space）。
一个 6 自由度手臂的配置空间是一个 6 维空间，每个维度对应一个关节的角度范围。手臂的每一个可能的姿态对应这个 6 维空间中的一个点。现在，把真实世界中的障碍物“投影”到这个空间里 - 所有会导致手臂碰撞的关节角度组合变成 C-Space 中的“禁区”。
运动规划的本质就变成了：在一个 6 维空间里，从一个点走到另一个点，绕过所有禁区。
问题在于，这个空间维度太高了。你没法像 Nav2 那样画一张 2D 地图然后跑 A* 搜索。6 维空间要离散化成网格的话，计算量随维度指数爆炸。所以机器人手臂的运动规划用的是一种完全不同的策略：基于采样的规划（Sampling-based Planning）。
OMPL 和它的算法家族
MoveIt 2 的默认运动规划后端是 OMPL（Open Motion Planning Library） - 一个开源的运动规划算法库，里面实现了几十种采样式规划算法。
先给一个实用建议：绝大多数场景用 RRT-Connect 就够了。 它是 MoveIt 2 的默认规划器，从起点和终点同时长两棵随机树，中间碰头就找到路径了。速度快，开箱即用，不需要你理解太多理论。如果你刚开始接触 MoveIt 2，不用纠结算法选择 - 用默认的就行。
那什么时候需要换？两个典型信号：
路径质量不够好时，考虑 RRT*。RRT-Connect 找到的路径是”能用”的，但往往弯弯绕绕，因为它是随机探索出来的。如果你的场景对路径平滑度有要求 - 比如手臂端着一杯水，抖一下就洒了 - RRT* 会在找到初始路径后持续优化，重新连线让路径更短更光滑。代价是更慢：你通常要给它一个时间预算（比如 5 秒），让它在这段时间内尽量优化。适合离线规划或对实时性要求不高的任务。
狭窄通道问题严重时，考虑 PRM 或 KPIECE。手臂要伸进两个货架之间拿东西，可行路径在 C-Space 中只是一条很窄的”缝隙”，RRT-Connect 的随机采样可能怎么撒都撒不到那条缝里。PRM 换了一种思路：先在整个 C-Space 中大量撒点，把不碰撞的点留下来互相连线形成一个”路网”。路网建好之后，不管你查询什么起终点，直接在路网上搜就行。如果环境是静态的（工厂产线上固定的工位），PRM 特别划算 - 建一次路网反复用。但环境动态变化时（有人在手臂旁走来走去），路网要不断重建，优势就没了。
核心判断逻辑很简单：默认 RRT-Connect；路径太丑换 RRT*；狭窄空间频繁超时换 PRM 或 KPIECE；环境静态且需要反复规划时 PRM 最划算。
碰撞检测 - 规划器的计算瓶颈
运动规划里每撒一个采样点、每延伸一步，都要回答一个问题：这个姿态会碰撞吗？一次规划可能采样几千上万个点，每个点都要做碰撞检测。所以碰撞检测的速度直接决定了规划的速度。
碰撞检测配错了是什么后果？一个真实场景：你的机械臂在桌上抓取物体，Planning Scene 里忘了添加桌面的碰撞模型。规划器找到一条”完美”的路径 - 穿过桌面。在仿真里看起来手臂像幽灵一样穿桌而过，在真实硬件上就是手臂全速撞向桌面。反过来，如果碰撞模型过度膨胀 - 比如把桌面建成了一个比实际大 10 厘米的盒子 - 规划器会认为桌面附近全是禁区，怎么都找不到抓取路径，反复报 PLANNING_FAILED。碰撞检测的核心挑战就是在”漏检导致撞击”和”误检导致无路可走”之间找平衡。
碰撞分两类，处理方式不同：
自碰撞（Self-collision） - 手臂的某个连杆打到了自己的其他部分，或者打到了机器人的躯干。MoveIt 2 用 ACM（Allowed Collision Matrix） 来加速：这个矩阵记录了哪些连杆对之间”永远不可能碰到”或者”碰了也没关系”（比如两个相邻连杆的连接处），检测时直接跳过这些对。ACM 配置不当的坑很隐蔽 - 如果你给机器人换了一个更大的夹爪但没更新 ACM，MoveIt 2 可能还在用旧的”这两个部件碰不到”假设，结果新夹爪在某些姿态下撞到了前臂。MoveIt 2 的 Setup Assistant 可以自动生成初始 ACM，但换了末端工具后一定要重新检查。
环境碰撞 - 手臂碰到了外部障碍物。你需要告诉 MoveIt 2 环境里有什么东西，通过 Planning Scene 这个数据结构来管理。两种常见做法：对于已知的固定物体（桌面、墙壁、机器人底座），手动添加基本几何体（盒子、圆柱）到 Planning Scene 里，简单可靠；对于动态或形状复杂的环境，接入深度相机的点云数据，用 OctoMap（八叉树结构的 3D 占据地图）自动构建环境模型。底层的几何碰撞计算由 FCL（Flexible Collision Library） 完成。
关键的工程权衡在碰撞模型的精细程度上。用手臂的精确 mesh 模型做碰撞检测很准确，但一次规划要检查上万个采样点，每个点都做精确 mesh 碰撞，规划时间可能从 1 秒变成 10 秒。实际项目中常见的做法是：手臂本体用胶囊体（capsule）近似 - 速度快得多，代价是稍微”过度保守”，会把一些实际能过的缝隙判定为碰撞。如果发现规划器在明显有空间的地方找不到路，检查碰撞模型是不是膨胀过度了。
抓取规划 - 从“到那里”到“拿起来”
运动规划解决了“手臂怎么到达目标位置”，但到了还不够 - 你还得把东西拿起来。
抓取规划要回答两个问题：从哪个方向接近物体？夹多紧？
抓取姿态（Grasp Pose） 的选择取决于物体的形状和任务需求。拿一个水杯，你可能从侧面水平接近握住杯身；拿一支笔，你可能从上方垂直捏住。每个物体可能有几十种合理的抓取姿态。MoveIt 2 本身不做智能的抓取姿态生成 - 这通常由上游的感知模块（比如基于 GPD 或 GraspNet 的抓取检测网络）提供一组候选姿态，MoveIt 2 负责选一个能规划出可行路径的。
位控和力控的切换是抓取中一个关键的工程细节。接近物体的过程用 位置控制（Position Control） - 你需要手臂精确地到达指定位置。但真正抓握的瞬间要切换到 力控制（Force Control） - 你不再关心位置精确到毫米，而是关心施加的力是否合适。抓鸡蛋用 2 牛顿，抓铁块用 20 牛顿，抓错了就是碎鸡蛋或者掉铁块。
很多工业场景不需要这么灵活。如果你永远只抓同一种规格的盒子，硬编码一个固定的抓取姿态和夹持力就行了 - 简单、可靠、好调试。灵活的抓取规划是在面对不确定物体时才真正需要的能力。
开发者踩坑指南
奇异点（Singularity）
这是机械臂开发者最头疼的问题之一。奇异点是手臂在某些特定姿态下，末端执行器“丢失”一个或多个自由度的现象。
最常见的例子：手臂完全伸直时。这时候你让它沿着伸直方向再往前走一点点，关节需要的角速度趋近于无穷大 - 实际表现就是手臂突然剧烈抖动或者直接报错停下来。类比：你把手臂伸直指向前方，然后试着让指尖再往前移动 1 厘米但手肘不能弯 - 你做不到，因为伸直方向上的运动自由度没了。
MoveIt 2 在规划时会检测奇异点并尝试绕过，但在笛卡尔空间的直线运动中（computeCartesianPath），如果路径恰好穿过奇异点，规划会失败。解决方法通常是调整路径的 waypoints，绕开奇异区域。
规划超时
OMPL 是基于采样的，不保证一定能找到路径。在狭窄空间里（比如手臂要伸进两个货架之间拿东西），可行路径本身就是 C-Space 中一条很窄的“通道”，随机采样碰到这条通道的概率很低。
实际表现：你给 MoveIt 2 一个目标，等了 5 秒（默认超时），回来一个“PLANNING_FAILED”。
常见的应对策略：增加规划时间（但客户可能不接受等 30 秒）；换更高效的规划器（比如在狭窄空间用 BiTRRT 或者 KPIECE）；简化碰撞模型让采样成功率提高；或者把一次困难的规划拆成多段简单的。
IK 多解的选择
前面说了 IK 可能有多组解。如果你不指定偏好，MoveIt 2 会选一个“最近”的解 - 离当前关节角度最近。但“最近”不一定“最好”。有时候“最近”的解会让手臂从桌子底下绕过去，而一个“更远”的解反而是从上方简单地挥过去。
一个常见的做法是给 IK 求解器提供多个 seed（起始猜测值），得到多组解，然后用运动规划器分别对这些解做规划，选一个规划耗时最短或路径最光滑的。这增加了一些计算量，但能避免很多诡异的运动轨迹。
夹爪打滑
力控参数没调好是最常见的原因。但还有一些不那么明显的坑：夹爪的接触面材质（光滑的金属夹头抓光滑的瓶子 - 物理上就抓不住）、物体的重心偏移（抓一个装了半瓶水的瓶子，重心位置随水的晃动在变）、抓取点的选择（抓瓶身 vs 抓瓶盖，受力方式完全不同）。
这些问题往往不在软件层面解决，而是要回到硬件 - 换有摩擦力纹理的夹爪指尖垫，或者用吸盘代替夹爪。在机器人开发中，“问题不在你的代码里”这种情况比软件开发中常见得多。
MoveIt 2 的输出喂给了谁
MoveIt 2 的最终输出是一条 关节轨迹（JointTrajectory） - 一系列带时间戳的关节角度序列。这条轨迹通过 ROS 2 的 FollowJointTrajectory action 接口发送给手臂的关节控制器，控制器负责让真实的电机按照这条轨迹运动。
在整个系统中，MoveIt 2 的上游是感知模块（告诉它“要去哪”和“环境里有什么障碍物”），下游是关节控制器（执行它规划出的轨迹）。而编排这一切 - 什么时候做感知、什么时候规划、什么时候抓取 - 是行为树的工作，也就是下一个话题之后会聊到的内容。