第一次正经用 MoveIt 2 给六轴机械臂做避障规划时，我差点被一个“绕路”问题搞到崩溃：明明障碍物在右边，规划出来的轨迹却先往左画了一个大弧线，再绕回来夹取目标。后来查了底层的规划日志，才发现是默认的 OMPL 配置选中了 RRTConnect，并且没有设置合适的规划时间预算。从那一刻起我意识到，想用好 MoveIt 2，光会拖 UI 界面远远不够，还是得把 RRT、A* 这类运动规划算法的脾气摸清楚。
这篇文章是我在实际项目中反复调试运动规划模块后的整理，内容包括 A*、RRT/RRT* 的原理拆解、它们在 MoveIt 2 中的接入方式、以及算法选型时的判断依据。适合刚接触机器人路径规划的开发者，也适合已经在用 MoveIt 但经常被“规划失败”“规划路径反人类”困扰的工程师。我会尽量把算法背后的“为什么这么说”讲透，再结合实操里常见的坑一起聊。
1. 运动规划到底卡在哪：从“直线能不能到”到“构型空间敢不敢走”
很多第一次接触运动规划的人，会把它误当成“在二维地图上画一条不撞墙的线”。但机器人尤其是机械臂的运动规划，真正的麻烦在于一个概念：构型空间（Configuration Space，简称 C-space）。你可以把机械臂的每个关节角度看成坐标轴上的一个维度，六个关节就是六维空间里的一个点。机械臂从姿态 A 运动到姿态 B，本质上是在这个六维空间里找一条从起点到目标点的连续曲线，并且曲线上每个点对应的机械臂姿态都不能与障碍物碰撞。
这里有个反直觉的地方：工作空间里看似宽敞的路径，在构型空间里可能根本不是一个正常几何体。比如一个长杆状的机械臂，在工作空间里要绕过桌子边缘，需要在构型空间里同时协调多个关节转角。你在地面上走迷宫只需要关心 x、y 两个方向，而机械臂需要同时担心每个关节是否超出限位、连杆是否扫过障碍。这种维度上的暴涨，让很多在二维地图上表现优秀的算法，到了高维空间里立刻变得力不从心。
碰撞检测又是另一层成本。假设你规划出一条 200 个点组成的路径，规划器每评估一个点是否合法，就要把机械臂各连杆的包围盒和场景物体的包围盒做相交测试。如果场景里有几十个物体，200 个点乘几十对碰撞对，一次完整检查就是上万次计算。这也是为什么很多规划器会把大部分时间耗在“验证路径可不可行”上，而不是“找路径”上。
所以运动规划的本质，实际是在回答两个问题：第一，构型空间里哪块区域是能自由运动的；第二，怎么在庞大的自由空间里高效找到一条满足运动学约束和最优性目标的轨迹。A* 擅长解决低维且离散化的搜索问题，RRT 家族则擅长高维连续空间的概率探索，而 MoveIt 2 的价值在于把这两类方案统一封装成可配置的规划管线。理解这条主线，后面看算法细节就不会觉得头晕。
2. A* 不是过时玩具：启发式搜索在现代机器人里的正确位置
2.1 从网格地图到代价函数：A* 究竟在算些什么
A* 的核心思想一句话就能说清：每次都优先扩展“当前代价 + 预估代价”最小的节点。当前代价指的是从起点走过来的实际路径长度，预估代价通常用启发函数估算，比如欧氏距离或曼哈顿距离。用一个简单 Python 伪代码能看得很清楚：
def a_star(start, goal, grid): open_set = {start} came_from = {} g = {start: 0} f = {start: heuristic(start, goal)} while open_set: current = min(open_set, key=lambda x: f[x]) if current == goal: return reconstruct_path(came_from, current) open_set.remove(current) for neighbor in get_neighbors(current, grid): tentative_g = g[current] + cost(current, neighbor) if tentative_g < g.get(neighbor, float('inf')): came_from[neighbor] = current g[neighbor] = tentative_g f[neighbor] = g[neighbor] + heuristic(neighbor, goal) open_set.add(neighbor) return None代码本身不复杂，但有两个容易翻车的点。第一，启发函数必须保持“可采纳性”，也就是它给出的估计值不能高于实际代价。如果你为了让搜索快一点而把启发值调得过大，就可能得不到最短路径。第二，网格邻域的选择会影响路径形态。四邻域只能走上下左右，路径会明显“僵硬”；八邻域允许斜向运动，路径更自然，但代价函数的设置也要跟上，否则会出现斜穿墙角的视觉 bug。
很多人在二维栅格上反复调 A* 的参数，却忘了它在机器人领域里真正的价值并不只在二维。MoveIt 2 的很多底层规划器其实也会用类似“离散采样 + 启发式扩展”的思路来处理低维问题，比如用 A* 在代价地图上做全局路径生成，再交给局部规划器平滑。所以理解 A* 不只是为了对付面试，而是帮助你理解“路径代价”这套语言。
2.2 启发函数、邻域扩展和地图分辨率之间的三角关系
用 A* 做实际项目时，最容易被忽略的是地图分辨率。假设你要让一台移动机器人在 20 米 x 20 米的房间定位，栅格分辨率是 0.05 米/格，那么地图就是 400x400，一共 16 万个节点。A* 在这种规模上完全够用。但如果你把分辨率提到 0.01 米/格，节点数直接变成 400 万，搜索速度会爆炸式变慢。所以要先想清楚：你的“最短路径”需要精确到厘米级，还是误差十几厘米完全可以接受？
另一个经验是不要把 A* 的目标点设成单个栅格中心。真实场景下机器人有体积，终点如果贴着墙壁，单点搜索很容易失败。更稳妥的做法是把目标区域膨胀成一个小集合，只要搜索到其中任意一个节点就算到达。这一步在实际部署时非常常见，但几乎没人写进教科书。
再提一个 A* 在机械臂领域的冷门用途：它可以作为采样规划器的高效“局部引导器”。比如在二维平面上先跑一遍 A*，把粗路径的拐点作为候选稀疏里程碑，再把这些点映射到构型空间里做局部连接。这种混合方案我已经在好几个原型项目里用过，能显著减少纯 RRT 的随机性，让规划结果更稳定、更符合直觉。
3. RRT 与 RRT*：用随机撒点绕开“高维空间的诅咒”
3.1 RRT 为什么能在高维空间里“碰运气式”找路
快速扩展随机树（RRT）的思路特别朴素：从起点开始，反复在地图里随机采样一个点，然后从已有的树上找到离它最近的节点，沿着两点连线方向拓展一小步。如果这一小步没有碰到障碍，就把它加进树里。重复几千次之后，树会像植物的根须一样逐渐蔓延到可行空间，直到某个新节点足够接近目标点。
def rrt(start, goal, scene, max_iterations=2000, step_size=0.1): tree = [start] for _ in range(max_iterations): sample = random_sample(scene) nearest = nearest_neighbor(tree, sample) new_node = steer(nearest, sample, step_size) if not collides(new_node, scene): tree.append(new_node) if distance(new_node, goal) < goal_tolerance: return extract_path(tree) return None这里有个关键点：“随机采样”并不是真的盲目乱撒。工程上常用目标偏向采样，也就是一小部分概率直接把目标点当作采样点，从而帮助树更快朝终点生长。否则光靠均匀随机采样，在大空间里让树“偶然”长到目标附近，耗时非常酸爽。步长（step_size）也要仔细选：太小则树生长慢，迭代次数要加很多；太大则容易“跨过”窄缝，导致窄通道场景里一直找不到合法扩展。
RRT 最迷人的地方在于它不需要像 A* 那样去离散化整个空间，因此高维构型空间对它来说只是“每个维度随机取个值”而已。这在六轴、七轴机械臂上非常实用。但代价是 RRT 得到的路径通常非常绕，并且带有很强的随机性——同一次规划跑两次，结果往往长得完全不同，路径长度也谈不上最优。
3.2 RRT* 的 rewiring 机制到底改进了什么
RRT* 在 RRT 基础上加了两步操作：重新选择父节点（choose parent）和重布树（rewire）。每一步新增节点后，它会检查附近半径内的已有树节点，看能否把新节点作为这些节点的父节点，从而降低它们到起点的总代价。如果可行，就把原来的父节点换成更优的新节点。这个机制不断修正树的结构，让最终路径长度在迭代足够多次后逐步收敛到最优解附近。
代价是计算量上涨。RRT* 每插入一个节点都要搜索邻域，并且做多次碰撞检测。在时间受限的在线规划场景里，RRT* 未必能跑出明显优于 RRT 的结果，因为它还没来得及利用 rewiring 把树“拧顺”。这就是为什么很多项目里宁可采用 RRTConnect 或者 RRT* 的低配版本，把时间预算留给快速求解，而不是追求渐进最优。
实操建议是：如果任务只需“找到一条可行路径”，且环境比较空旷，直接用 RRT 就可以，速度快得让人感动；如果环境复杂且有反复规划的需求，则用 RRT* 并给足迭代时间。在 MoveIt 2 里，两者都集成在 OMPL 中，切换成本几乎为零，所以稳稳地把两类算法的特性搞清楚才是重点。
4. MoveIt 2 把算法变成了工程：架构、OMPL 接入与配置细节
4.1 MoveIt 2 的规划管线里到底发生了什么
MoveIt 2 是机器人操作系统 ROS 2 生态下的运动规划框架，它对上层提供“输入当前状态 + 目标姿态 + 约束”，对下层调用规划器，再把结果交给轨迹执行模块。这中间最核心的组件叫规划管线（Planning Pipeline），它由三个环节串成：规划请求适配器（Planning Request Adapter）、规划器（Planner）和规划响应适配器（Planning Response Adapter）。
很多人第一次配置 MoveIt 时，以为只要装好包就能高枕无忧。实际上一个标准的规划请求要经历重重改造：假设你给的目标是笛卡尔空间坐标，运动学解算会先转成关节空间目标；然后规划器开始搜索；搜索完成后，响应适配器还可能做路径平滑、时间参数化、碰撞再验算。如果只盯着最后结果看，就会把问题归咎到“随机算法不给力”，但实际上前面某个环节已经悄悄引入了一个不合理的约束。
MoveIt 2 自带一组默认规划器，底层是OMPL（Open Motion Planning Library），里面包含 RRT、RRT*、PRM、EST、LBKPIECE 等一堆算法。你可以通过修改 ompl_planning.yaml 文件来指定每个规划组（Planning Group）使用哪种算法、最大规划时间、目标容差等参数。在 MoveIt 1 时代这些配置就已经成型，MoveIt 2 主要把它们迁移到了 ROS 2 的参数服务器模型中，因此看老教程时要注意话题名和参数名可能变了。
4.2 一份 minial 的 OMPL 配置长什么样
以某六轴机械臂的规划组为例，典型的 YAML 配置大致如下：
planning_group: arm planner_id: RRTConnectkConfigDefault planning_time_limit: 5.0 max_iterations: 1000 max_planning_attempts: 10 goal_tolerance: 0.01这里planner_id不是随便填的，它对应 OMPL 内部的具体算法实现名称。RRTConnectkConfigDefault是 RRTConnect 的默认配置，双树策略通常比单树算得快，适合大多数避障场景。planning_time_limit是单次规划的时间上限，调得越大，找到可行路径的概率越高，但机器人停下来等的“发呆时间”也会变长。goal_tolerance描述末端执行器到达目标时允许的位置误差，设太大会让人感觉“怎么没到位也算成功”，设太小则规划器经常找不到解。
还有一个细节是 MoveIt 2 会加载机器人的URDF 和 SRDF。URDF 描述几何、质量、关节类型，SRDF 描述规划组、碰撞免检对等信息。很多人在面对“规划失败”时习惯调算法参数，但最后发现是 SRDF 里漏掉了一个不该免检的碰撞体，导致规划器“看不见”某个障碍。这类隐蔽问题只有把规划日志打开，一行行去追碰撞检测信息的时候才能揪出来。
4.3 OMPL 之外：运动学求解器和碰撞检测也决定了成败
拿到一条合法的关节空间路径后，MoveIt 2 还要把它转成机器人能执行的运动指令。这里面绕不开逆运动学求解。MoveIt 2 默认使用 KDL 求解器，但 KDL 在奇异位形附近经常求解失败，导致你明明看到规划器输出了一条路径，执行端却报 IK 无解。更成熟的做法是换用基于数值优化的求解器，或者为常见目标姿势预生成 IK 解，放进 IKFast 库中。
碰撞检测方面，MoveIt 2 默认使用 FCL 库。FCL 把机械臂连杆和场景物体抽象成几何包围盒或网格体，然后做分离轴检测。实际项目里有个常见坑：为了省算力，有人把连杆的碰撞体设置得比真实形状小很多，结果路径规划顺利完成，但真机一走就撞上了。反过来把碰撞体设太大，又会频繁出现“明明看起来没撞到，但规划器说无解”。
5. 选型不是玄学：一张表看清 A*、RRT、RRT*、PRM 的边界
我自己做项目时最怕的不是某个算法不会用，而是不知道怎么在时间预算内选对算法。这里分享一张我常用来和同事快速对齐的表：
| 算法 | 完备性 | 最优性 | 典型适用维度 | 主要代价 | 适合场景 |
|---|---|---|---|---|---|
| A* | 离散图完备 | 可最优（取决于启发函数） | 2~4 维离散空间 | 地图分辨率高时内存暴涨 | 二维移动机器人、网格导航 |
| RRT | 概率完备 | 否 | 3~10 维连续空间 | 路径随机性强、不够直 | 快速找一条可行路径 |
| RRT* | 概率完备 | 渐进最优 | 3~10 维连续空间 | rewiring 计算量大 | 有充足规划时间，追求质量 |
| PRM | 概率完备 | 可渐进最优（需后续查询） | 6~10 维 | 预处理耗时 | 多次查询同一环境 |
这张表只是用来做初步判断。真正落地的时候还要考虑：环境是静态还是动态？如果动态障碍物频繁出现，预处理型算法如 PRM 要不断重新构建道路图，并不划算；而 RRT 每次从零开始规划反而更快。目标点是否经常变化？变化频繁就用 RRTConnect 这类单次快速求解型；机器人需要反复经过同一片区域，则 PRM 的多次查询优势会被放大。
还有一个常常被忽略的维度是“可解释性”。甲方爸爸或操作员经常希望机器人的轨迹符合人类直觉——别绕远路、别做诡异转头。RRT 的随机性在这方面非常吃亏，你可能要反复运行多次才撞见一条顺眼的路径。这时可以考虑 RRT* 或者加一个后处理平滑模块，把折线转角拉直，代价是牺牲一点最优性换取视觉舒适度。
从工程角度看，我建议团队里至少跑通三套配置：一套 RRTConnect 用作默认快速规划，一套 RRT* 用于离线精调路径，再配合一个 A* 做二维导航或三自由度平面问题。这样面对大部分需求时，都能在一小时内找到可用的起点。
6. 实测中最容易踩的四个坑：从规划超时到机械臂突然“发疯”
6.1 规划超时不是算法慢，可能是碰撞检测模型太大
有一次我把一个 40 厘米高的柱状障碍物加进场景，却一连接到五次规划超时。检查日志发现，MoveIt 2 默认给物体包了一个轴对齐包围盒，而包围盒尺寸比柱体实际轮廓膨胀了一圈半。结果机械臂离柱子还有十厘米左右，碰撞检测就开始报警。解决办法是把柱体换成精确的网格文件，并把碰撞安全距离从默认 5 厘米降到 1 厘米。这一改，规划时间从 10 秒降到 1 秒以内。碰撞检测模型过保守，是“找不到路径”的头号隐性原因，排查时先看一眼场景物体的碰撞几何长什么样。
6.2 目标容差设置不当，导致反复规划都“差一点成功”
目标容差是 MoveIt 2 里很温顺但也很坑的参数。它控制的是“末端执行器距离目标点多少毫米以内算成功”。默认值往往是 0.01 米，对有些场景够用，但对高精度插装任务来说显然太松。表面看机器人已经停在目标点旁边，但夹具对不准插入孔，执行端反复调整又总是先退后进，非常折磨人。反过来如果你把容差调到 0.0001 米，规划器在数值求解时可能一直达不到这么高的收敛精度，于是你得到无穷无尽的“规划失败”弹窗。我的经验是先查真实任务要求的重复定位精度，再反推目标容差的合理范围。
6.3 关节限位在规划时不报错，执行时才突然“发疯”
有时候规划器开心地返回了一条路径，可你按下执行，机械臂却在中途像触了电一样猛甩一下。这种问题八成出在关节限位。很多 URDF 文件里的关节 limits 写的是理论值，但在高速运动下，实际电机和减速机允许的动态范围比静态限位窄得多。MoveIt 2 的规划器只按静态关节限位做检测，于是路径包含了一些“静态可达但动态不可达”的点位。解决办法有两类：要么在规划请求里额外添加速度加速度约束，要么在轨迹执行前对关节速度做二次校验，把超速点主动剔除。
6.4 局部极小和传感器噪声：实车与仿真最大的分水岭
仿真环境里跑得好好的 RRT，到了实车上一言不合就原地乱转，这种情况我也遇到过。仿真里障碍物位置是“上帝视角”精确已知的，而实车依赖传感器建图，地图本身就是带噪声的。规划器基于这张不准的地图找路，只会保证“在地图上不碰撞”，不保证“在真实世界中不碰撞”。智能一些的做法是给规划器输入膨胀后的地图，并配合实时局部避障模块，把“全局找路”和“局部避让”分开。千万不要只靠全局规划器一肩挑，那不是算法不够强，而是任务边界划错了。
最后再分享一个实际体会
如果你也是从 MoveIt 2 这个“大头”反过来学运动规划算法，我的建议是先不要急着调参，而是把每个底层规划器分别跑一遍同一组测试场景，记录下路径长度、规划时间、成功率三个指标。首选构建一个小工具，批量运行数十次规划，把每次结果保存下来，否则单凭肉眼判断“这条轨迹顺不顺”，很容易被一次特别顺眼的成功案例带偏判断。我当初就是因为被某次 RRT 的“超常发挥”迷惑，误以为它比 RRT* 更适合某个项目，测试了四小时后才发现成功率其实只有不到一半。用数据说话，比用直觉说事靠谱得多。
运动规划技术的深坑并不会因为你熟练了 A* 和 RRT 就彻底消失。它更像是和“空间几何 + 随机过程 + 工程约束”打持久战。希望这篇总结能帮你少走我走过的弯路，尤其是那些“看似规划成功、实则隐患重重”的边界情况，往往才是真正拉开专业差距的地方。
