CMU - FarPlanning 代码速读

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_;		// 基于轮廓构造的局部图，并与全局图匹配
1
2
3
4

主要线程

• 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 是轮廓上的点

DynamicGraph::UpdateNavGraph

• near_nav_graph_->extend_match_nodes_ 包含 边界点 和 active 点
• new_nodes_当前帧检测到的 vertex，且没有 Global 匹配项；

GraphPlanner::UpdateGraphTraverability 生成两套代价地图

GraphPlanner::PathToGoal

Refs

• 轮廓检测算法
  • Simplify Polylines with the Douglas Peucker Algorithm
• far_planner 源码阅读笔记
• 机器人自主导航从零开始第六步——FAR Planner全局路径规划算法
• https://www.cmu-exploration.com/development-environment