ROS2 多机器人系统

path_planner的图模型,BFS和route拼接

ROS2

Posted by Bruce Lee on 2025-03-15

path_planner的图模型,BFS和route拼接

给fleet_manager一个足够简单,足够稳定,并且足够可解释的route结果,让任务分配真正闭环

也就是说,当前planner首先要解决的不是"最先进",而是"最小正确".

这篇只分析三个问题:

  1. 为什么当前用的是离散waypoint图模型
  2. 为什么当前算法选BFS而不是更复杂的方法
  3. 为什么route要拆成start -> pickup和pickup -> dropoff两段再拼接

当前阶段目标

当前path_planner在系统里的目标其实很克制:

  1. 接收起点waypoint
  2. 接收pickup waypoint
  3. 接收dropoff waypoint
  4. 在当前配置图上给出一条可走的waypoint序列

它不是在做连续空间轨迹规划,也不是在做动态避障,更不是在做多机器人全局联合规划.

所以如果把这个节点放回整个项目背景里看,就会发现它当前的任务本质是:把任务语义翻译成离散路径语义,而不是把机器人控制问题一次性做完.

先看接口: planner到底承诺了什么

当前服务接口是PlanRoute.srv:

1
2
3
4
5
6
7
string start_waypoint
string pickup_waypoint
string dropoff_waypoint
---
bool success
string message
string[] route_waypoints

这个接口非常值得注意,因为它说明planner当前不是通用图搜索接口,而是明显带有任务语义的接口.

它不是只问:

  1. 从A到B怎么走

而是直接问:

  1. 从当前起点怎么到pickup
  2. 再从pickup怎么到dropoff

也就是说,当前planner并不是一个抽象"路径库",它更像是一个为transport task服务的路线生成器.

这个设计有两个直接含义:

  1. planner理解当前任务模型
  2. manager不需要自己拆任务路径结构

这说明当前分层方式是明确的: manager负责决定谁接任务, planner负责把任务转成route.

为什么这里先采用离散图模型

当前demo图配置在demo_map.yaml里,本质上由两部分组成:

  1. waypoint列表
  2. edge列表

例如现在的waypoint有:

  1. dock_a
  2. mid_1
  3. pickup_zone
  4. mid_2
  5. dropoff_zone
  6. dock_c

边则像这样:

  1. dock_a:mid_1
  2. mid_1:pickup_zone
  3. mid_1:mid_2
  4. mid_2:dropoff_zone
  5. mid_2:dock_c

这类建模方式说明当前系统把环境先抽象成了一个无权离散图.

为什么这一步是合理的

因为当前项目真正想验证的是车队协调主链路,不是地图表达能力本身.

对于当前阶段,manager真正关心的是:

  1. 机器人现在在哪个离散点
  2. pickup在哪个离散点
  3. dropoff在哪个离散点
  4. 是否存在一条图上的连通路径

只要这些语义成立,系统就已经能做出有意义的分配和执行模拟.这意味着项目当前实际上主动回避了几个高复杂度问题:

  1. 连续空间建图
  2. 局部避障
  3. 轨迹时间参数化
  4. 运动学约束
  5. 更复杂地图格式接入

这不是能力缺失,而是阶段控制.当前系统先把环境压缩成离散可达关系,是为了先验证任务协调,不是为了永久停留在简单地图上

参数化图配置这件事,其实比看起来更重要

path_planner启动时并不是把图硬编码死在逻辑里面,而是先声明参数:

  1. graph_waypoints
  2. graph_edges

然后通过load_graph_from_parameters()把参数装入内部的邻接表.这个设计的价值在于,它把"规划逻辑"和"地图内容"拆开了.

换句话说,当前planner真正硬编码的不是地图,而只是:

  1. 图是由waypoint和edge组成
  2. edge格式用from:to表示
  3. 图内部以邻接表保存

至于具体有多少节点,节点怎么连,则交给launch配置控制.

这个分离对当前项目很重要,因为它让系统具备了以下性质:

  1. 不改planner代码也能替换demo图
  2. 同一套planner逻辑可以服务不同地图
  3. blog和文档可以把"算法行为"和"场景配置"分开分析

遗留问题和现象1: 当前边配置默认被视为双向边

代码里读取一条from:to后,会同时写入:

  1. graph_[from].push_back(to)
  2. graph_[to].push_back(from)

这说明当前图模型是无向图.

这对demo阶段是合理的,但后续如果出现单向通道,禁行方向,不同边代价,这个模型就需要升级.

为什么当前算法选BFS

很多人会下意识问:

  1. 为什么不是Dijkstra
  2. 为什么不是A*
  3. 为什么不是更接近真实导航的方案

但当前这个项目阶段里,BFS恰恰是非常合理的.

因为现在图的假设是:

  1. 节点是离散waypoint
  2. 边没有权重差别
  3. 当前只需要最少边数意义上的可达路径

在这种前提下,BFS的性质正好匹配需求:

  1. 实现简单
  2. 行为稳定
  3. 结果可解释
  4. 调试成本低
  5. 足够支持当前V1闭环

也就是说,这里算法选择不是在比拼上限,而是在追求:在当前问题模型下,选择最小且正确的算法

这点很关键. 因为项目当前还处在"系统链路验证期",此时如果一开始就引入更复杂算法,会同时引入更多调试面.

BFS在这里到底做了什么

当前shortest_path(start, goal)的逻辑很标准:

  1. 先检查起点终点是否存在于图中
  2. 用队列做广度优先遍历
  3. 用parent记录每个访问节点来自哪里
  4. 一旦到达goal,结束搜索
  5. 再从goal沿parent反向回溯得到完整路径
  6. 最后reverse成正向route

这里真正值得注意的不是算法教材本身,而是它在工程上的可控性.

第一, 它天然给出一条最少跳数路径

在当前图边等价的前提下,这已经足够形成一个合理demo route.

第二, 它很容易判断失败

如果最后parent里没有goal,直接返回空路径.

这种失败语义非常清楚,manager也很好处理.

第三, 它不引入额外启发式假设

也就是说,当前路径结果只由图结构决定,不由额外heuristic决定. 这会让系统行为更容易预测.

对于当前阶段来说,这一点比所谓"更聪明"更重要.

为什么route不是直接做一次 start -> dropoff

这是当前planner里最值得单独拿出来写的一点.它当前不是直接求:

start_waypoint -> dropoff_waypoint

而是先求两段:

  1. start_waypoint -> pickup_waypoint
  2. pickup_waypoint -> dropoff_waypoint

然后把第二段去掉第一个节点后拼接到第一段后面.这个设计非常贴合transport task语义.

为什么必须拆成两段

因为取货点不是一个可有可无的中间点,它是任务语义的一部分.

如果你只求start -> dropoff, 那么只要图上存在一条更短路线绕过pickup, planner就会给出一条"几何上可走,任务上错误"的路径.

所以在当前任务模型里,pickup不是路径优化提示,而是硬约束.

也就是说:当前route不是单目标路径问题,而是一个带中间必经点的任务路径问题

这也是为什么当前接口直接把pickup和dropoff都放进service请求里.

为什么拼接时要跳过第二段的起点

因为第二段是从pickup开始的. 如果直接全量拼接,那么pickup节点会重复一次.

所以代码里使用:

std::next(to_dropoff.begin())

本质上就是在做:

  1. 保留第一段完整路径
  2. 跳过第二段里重复的pickup起点
  3. 只接上pickup之后的后续部分

这个处理虽然小,但是说明当前planner返回的route已经在为agent执行做格式清理,而不是把拼接负担丢给下游.

这类planner的真正价值,不是复杂,而是可解释

当前planner最大的优点之一,其实不是它会搜图,而是它非常容易解释.

比如给出一条route:

dock_a -> mid_1 -> pickup_zone -> mid_1 -> mid_2 -> dropoff_zone

读者,开发者,甚至后续做日志分析的人,都能非常直接地回答:

  1. 它先去pickup
  2. 然后再去dropoff
  3. 它为什么经过这些中间点
  4. 图上是不是确实这么连的

这对于当前blog连载也很重要. 因为可解释的系统才容易被逐篇记录和分析.

如果一开始就上太重的规划系统,博客写作本身也会失去抓手.

当前V1里,这个planner没有解决什么

理解一个模块,不仅要看它做了什么,还要看它故意没做什么.当前path_planner还没有处理这些问题:

1. 没有边权重

所以当前最短路只是最少跳数,不是最小时间/最小代价.

2. 没有冲突感知

planner只考虑单机器人在图上的可达性,并不考虑两个机器人会不会在同一节点或边上冲突.

3. 没有动态重规划

当前更接近一次性给route,不是执行过程中根据状态变化不断调整.

4. 没有方向约束

边现在被当作双向,因此还不能表达单向路径.

5. 没有连续空间语义

它只负责离散waypoint序列,不负责真正的轨迹生成和底层导航.遗留问题和现象2: 当前planner已经足以支撑任务闭环,但它支撑的是"离散协调层正确性",不是"真实导航层完整性"

可以会延伸出去的问题方向

分析完path_planner,后续自然会分成两条延伸方向.

第一延伸出去问题方向是:

  1. robot_agent如何消费route
  2. waypoint推进为什么构成一个简化执行状态机
  3. route sequence如何最终变成idle/executing/completed

第二个问题方向是回到更高层:

  1. 如果未来引入冲突预约,planner接口要不要升级
  2. 如果未来引入边权重,是不是要从BFS转向Dijkstra/A*
  3. 如果未来接Nav2,当前planner应当保留在哪一层

也就是说,当前这个planner虽然很小,但它刚好卡在一个很好的位置:它足够简单,能把当前阶段讲清楚; 也足够关键,能自然把文章继续带到下一阶段

最后

path_planner现在做的不是复杂路径规划,而是把transport task稳定翻译成一条离散,可解释,可执行的waypoint route,从而让整个V1系统的任务闭环真正落到图结构上

robot_agent又是做什么的呢: 它如何把route_waypoints转成执行过程,以及为什么它本质上是一个简化任务执行状态机.

If you like this blog or find it useful for you, you are welcome to comment on it. You are also welcome to share this blog, so that more people can participate in it. All the images used in the blog are my original works or AI works, if you want to take it,don't hesitate. Thank you !

特色标签
ROS2 多机器人系统
链友
ESC