CMU - FarPlanning 代码速读

news2025/10/31 9:44:36

https://github.com/MichaelFYang/far_planner
https://www.cmu-exploration.com/

系统结构

Far Planner 属于 High-level planning module，进行全局规划，找到可行路径；
将 way_point发布给 Local planner和 path following

KeyPoint

Local-layer

当前帧点云 ➡️ edge points ➡️ enclosed polygons ➡️ visibility graph
动态障碍物
- 将被遮挡的 visibility edges 删除；
- 动态障碍物消失后，重新连接；
点云提取多边形
- 将点云映射到图像（鸟瞰图）上，黑色-可通行，白色-障碍物
  - 白色像素使用机器人尺寸进行膨胀，均值滤波平滑
  - OpenCV 拓扑边缘点提取，输出 enclosed polygons ：Topological structural analysis of digitized binary images by border following
  - 封闭多边形顶点稀疏化：Algorithms for the reduction of the number of points required to represent a digitized line or its caricature
    - 顶点间内角用于量化障碍物曲率，内角太小的抛掉

Global-layer

和 Local-layer 匹配，获得增量式更新
- Local-layer 构建
  - 之前得到了封闭多边形，对 Edge 超过阈值的进行可视角度筛选；Edge 太短的角度噪声太大，没法选；
- Local-Global 关联
  - 遍历 vertex，若两地图间的 vertex 位置重叠，则将其关联；
  - 关联后，更新 Global 中 vertex 的位置；Modern Methods for Robust Regression
    - 在 vertex 对应历史帧位置中挑选 inlier，然后对 inlier 求均值
  - Global 中长期未被关联的 vertex 根据投票删除
  - Local 中未被关联的 vertex 直接添加到 Global
  - Edge 更新
    - 若 Global 两点之间存在 Edge，则更新
    - 若不存在，则新增
  - 可视图更新
  - Talk 图

Planning on V-Graph

在 Global-Layer 中添加 [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-Rsy5uQxf-1684922016766)(null#card=math&code=P_{robot}&id=ID5nJ)]和 [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-dlYkMZ9t-1684922016693)(null#card=math&code=P_{goal}&id=Q9KPL)] 两个 vertex，并根据可视性连接 Edge；
在 Global-Layer 中使用宽度优先搜索，找 robot到 goal的可行最短路径
- 与 [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-okJ8FdWV-1684922016708)(null#card=math&code=P_{robot}&id=zpeCD)]能构成可视 Edge 的 vertex 构成 free space
  - 沿途所有 robot能看到的 vertex 均组成 free space
- 其余 Global vertex 构成 unknown space【未探索区域】
保存 V-Graph
- 将带有 free space标识的 V-Graph 保存
- 再次加载时，可选择是否
  - attemptable planning：在 free space+ unknown space 中导航
  - non-attemptable planning：仅在 free space 中导航

Multi-layer

用于飞行器场景，在 3D 空间中建立多个水平切片，并在每层切片构建多边形，跨层构建 3D 可见性；

Terrain traversability analysis module

分割障碍物（obs）、地面（free）

Modules

外围模块

FARMaster

ContourDetector contour_detector_;	// 轮廓检测 - 处理当前帧点云
DynamicGraph graph_manager_;		// 全局图 nav_graph_
GraphPlanner graph_planner_;		// 可视图上的 Planning 模块
ContourGraph contour_graph_;		// 基于轮廓构造的局部图，并与全局图匹配

主要线程

PlanningCallBack()
- 定时器线程，约 2.5 Hz
- 在接收 Goal 任务之后，周期性执行可视图路径规划
Loop()
- ROS 定时线程，2.5 Hz
- 主要用于 V-Graph 的维护

NavNode
- is_odom表示机器人当前位姿插入图中的 Node
  - 会随机器人位姿移动，属于 free node
  - 每次更新均为重新初始化
- is_navpoint表示局部导航点，可探测范围内的一个临时目标点
  - 属于 free node
- is_active表示与机器人很近的点、或邻近点【有点像 frontier】
  - is_nearby，距离小于车身直径
  - 在 odom_node 的邻近点（connect_nodes）栈内
  - 当前点的邻近点都是 is_active
- is_contour_match 是轮廓上的点