规划是机器人从"知道自己在哪"走向"知道怎么动"的那一步。它经常被混成一个词,实际至少分三层:路径规划决定走哪条路,轨迹规划决定以什么速度、加速度走,局部控制决定每一步怎么躲开新出现的障碍。三层的算法、约束和失败模式完全不同。本文用 Ubuntu 24.04 + ROS 2 Jazzy 的环境,讲清 A*、RRT、轨迹参数化和 Nav2 / MoveIt 2 的分工,代码可以直接跑。
一、三层规划:路、轨迹、控制
在配置空间(configuration space)里看,机器人是一个点,障碍物是膨胀后的区域。路径规划只关心几何连通性,输出一串位姿;轨迹规划给这条路径加上时间信息,满足速度、加速度甚至 jerk 约束;控制器则根据当前状态和参考轨迹闭环输出指令。三层之间是"参考"关系,不是"命令"关系。
| 层级 | 输入 | 输出 | 典型算法 |
|---|---|---|---|
| 路径规划 | 地图、起点、终点、机器人形状 | 无时间的几何路径 | A*、Dijkstra、RRT、PRM |
| 轨迹规划 | 路径、速度/加速度/时间约束 | 带时间戳的参考轨迹 | 梯形速度、多项式、优化法 |
| 局部控制 | 参考轨迹、实时障碍、当前状态 | 速度或力矩指令 | DWA、MPPI、PID、MPC |
这三个层级对应三种"正确":路径正确(连通、无碰撞)、轨迹正确(动力学可行)、跟踪正确(实际执行误差可控)。很多项目失败在只验证了第一层——路径看起来没问题,控制器根本跟不上。
二、图搜索:Dijkstra 与 A*
把平面栅格化成网格后,规划就变成图搜索。Dijkstra 按累计代价向外扩展,能找到最短路径但会搜很多无关区域;A* 在此基础上加一个启发式函数 h,优先扩展"看起来更接近终点"的节点。只要 h 不高估真实代价(可采纳),A* 就能保证找到最优解。
import heapq
import math
def astar(grid, start, goal):
rows, cols = len(grid), len(grid[0])
def h(p):
return math.hypot(p[0] - goal[0], p[1] - goal[1])
moves = [(-1, 0, 1.0), (1, 0, 1.0), (0, -1, 1.0), (0, 1, 1.0),
(-1, -1, math.sqrt(2)), (-1, 1, math.sqrt(2)),
(1, -1, math.sqrt(2)), (1, 1, math.sqrt(2))]
open_heap = [(h(start), 0.0, start)]
came_from = {start: None}
best = {start: 0.0}
while open_heap:
_, cost, cur = heapq.heappop(open_heap)
if cur == goal:
path = []
while cur is not None:
path.append(cur)
cur = came_from[cur]
return path[::-1]
for dr, dc, step in moves:
nxt = (cur[0] + dr, cur[1] + dc)
if not (0 <= nxt[0] < rows and 0 <= nxt[1] < cols):
continue
if grid[nxt[0]][nxt[1]] == 1:
continue
new_cost = cost + step
if new_cost < best.get(nxt, float('inf')):
best[nxt] = new_cost
came_from[nxt] = cur
heapq.heappush(open_heap, (new_cost + h(nxt), new_cost, nxt))
return None
if __name__ == '__main__':
grid = [[0] * 10 for _ in range(6)]
for c in (3, 4, 5):
grid[2][c] = 1
print(astar(grid, (0, 0), (5, 9)))
Nav2 的默认全局规划器 NavFn 本质上是这类栅格搜索,Smac 系列则提供 A* 变体并支持运动学约束。真实系统里代价不是 0/1:膨胀层让靠近障碍的格子代价更高,规划器会自然选择"离墙远一点"的路线。这也是为什么地图分辨率、膨胀半径一改,路径形状就全变。
三、采样规划:RRT 家族与高维空间
机械臂有 6 到 7 个关节,网格化会维度爆炸,这时用采样式规划:在配置空间里随机采样,尝试连接,逐步长出一棵树。RRT 快但不最优,RRT* 通过重连逐渐逼近最优,PRM 先建路网再查询。它们都是概率完备的:给足够时间,找到解的概率趋近 1,但窄通道里可能非常慢。
MoveIt 2 默认用 OMPL 做机械臂规划,也支持 CHOMP、STOMP、Pilz 等规划器与多种规划管线。对大多数抓取任务,先用默认 OMPL 跑通,再根据"规划太慢"还是"轨迹太抖"换规划器。安装与启动(版本不同 launch 参数会变,以官方 Getting Started 为准):
sudo apt install ros-jazzy-moveit -y
# 若你的发行版没有对应二进制包,按官方 Getting Started 从源码构建
ros2 pkg list | grep moveit
# 官方教程提供 demo launch,装好 moveit2_tutorials 后按教程启动
# 拖动交互标记设定目标,观察规划路径与执行
采样规划的输出通常还要经过后处理:短切、平滑、时间参数化。RRT 路径往往有多余拐点,直接执行会让机械臂走走停停,所以 MoveIt 的规划管线里有一串适配器做这些事。
四、轨迹与避障:把路径变成可执行指令
最直观的时间参数化是梯形速度:先以最大加速度加速到限速,匀速,再减速到停。若距离太短,就退化成三角形速度曲线。这个函数能直接用来检查"这条路径在给定约束下需要多久":
import math
def trapezoid(distance, v_max=0.5, a_max=0.5):
d_acc = v_max * v_max / (2 * a_max)
if 2 * d_acc >= distance: # 距离太短,三角形曲线
v_peak = math.sqrt(a_max * distance)
t_acc = v_peak / a_max
return {'v_peak': v_peak, 't_total': 2 * t_acc}
t_acc = v_max / a_max
t_cruise = (distance - 2 * d_acc) / v_max
return {'v_peak': v_max, 't_acc': t_acc,
't_cruise': t_cruise, 't_total': 2 * t_acc + t_cruise}
if __name__ == '__main__':
for d in (0.2, 1.0, 5.0):
print(d, trapezoid(d))
移动底盘的局部避障常用 DWA:在当前速度的"动态窗口"里采样多组速度,预测短时轨迹并按代价打分;Nav2 的 MPPI 控制器则用采样加滚动优化,能处理更复杂的代价项。势场法看起来优雅,但遇到局部极小值会把机器人卡在凹形障碍前,工程上不要单独依赖它。
ros2 launch nav2_bringup tb3_simulation_launch.py headless:=False
ros2 param get /planner_server planner_plugins
ros2 param get /controller_server controller_plugins
ros2 param get /controller_server FollowPath.critics
上面三条命令能让你看到当前用的是哪个规划器、哪个控制器,以及控制器里有哪些评分项。调参前先确认插件,再改参数,否则改的是根本没加载的配置。
五、Nav2 与 MoveIt 2 的分工
两者都叫"规划",但解决的问题不同。Nav2 面向移动底盘的平面导航,状态空间基本是 (x, y, yaw),核心是代价地图和实时避障;MoveIt 2 面向机械臂的关节空间规划,核心是碰撞场景、逆运动学和轨迹执行。移动抓取任务需要两者协作:Nav2 把底盘开到桌边,MoveIt 2 规划手臂抓取,中间靠 TF 把基座、世界和物体统一到同一坐标系。
| Nav2 | MoveIt 2 | |
|---|---|---|
| 状态空间 | 平面位姿 + 速度 | 多关节角度 / 末端位姿 |
| 主要输入 | 代价地图、激光、目标点 | 点云、规划场景、目标位姿 |
| 规划算法 | NavFn、Smac + MPPI/DWB | OMPL、CHOMP、Pilz 等 |
| 执行接口 | /cmd_vel | ros2_control 轨迹控制器 |
协作的难点不在算法,而在坐标与时机:底盘定位误差会让机械臂的目标位姿偏移,机械臂展开后又会改变移动底盘的碰撞体积。先分别跑通,再做"停车后抓取",最后才尝试边走边抓。
六、常见坑与项目清单
- 坐标系搞错:在
odom里规划却发布到map,目标点永远对不上。 - 代价地图分辨率太粗,窄门在地图里直接消失;太细则规划耗时上升。
- 膨胀半径过大,物理上能过的门算法认为过不去。
- 规划器忽略运动学:为非完整约束底盘规划出直角转弯,控制器只能原地磨。
- 路径可行但控制器跟不上:速度、加速度、转弯半径超出底盘能力。
- 动态障碍不重规划:只规划一次,撞上去才发现环境变了。
- 机械臂场景没更新:物体被搬走了,碰撞场景里还在,规划一直失败。
- 只做仿真验证:仿真里没有延迟、没有打滑、没有真实的碰撞风险。
| 任务 | 验收标准 |
|---|---|
| 在栅格上实现 A* 并与 Dijkstra 对比 | 能说出扩展节点数量差异及其原因 |
| 用 Nav2 规划一条窄通道路径 | 能解释膨胀半径与代价地图对路径的影响 |
| 给路径做梯形时间参数化 | 速度曲线连续、起停符合加速度限制 |
| 在仿真中制造动态障碍 | 机器人能绕行或触发恢复,而不是直接撞上 |
| 写一份规划参数说明 | 记录插件、分辨率、膨胀半径、速度限制及理由 |
规划的功夫不在背算法名,而在把"约束"翻译成可执行的参数和验证方法。先让路径在仿真里安全地走通,再逐步提高速度和复杂度,每一步都留下能复现的日志。
参考与延伸阅读
- OMPL 官方文档(采样式运动规划)
- MoveIt 2 文档
- nav2_mppi_controller 包文档(Jazzy)
- LaValle:RRT 与采样式规划资料
- The Dynamic Window Approach to Collision Avoidance(DWA 原论文)
- Nav2 官方文档
外链内容由对应网站负责,访问以实际可用性为准。