TARE-Planner自动探索算法源码学习笔记

tare_planner自动探索算法特点在于将探索路径的规划分为两层，一层是局部路径规划（在local planner horizon内），第二层是全局路径规划（在local planner horizon外的状态为'探索中'的单元格中），每次生成新的局部路径与全局路径后，在根据他们计算预瞄点发送给导航模块。

一、主要环境管理对象介绍

/// 关键位姿图，论文中的  sparse random roadmap，用来规划全局路径
std::unique_ptr<keypose_graph_ns::KeyposeGraph> keypose_graph_; 
/// 规划环境管理对象，主要用来管理边界点及空间占用情况
std::unique_ptr<planning_env_ns::PlanningEnv> planning_env_;
/// 视点管理对象，用于管理各位置视点的覆盖（观测）情况及连通情况
std::shared_ptr<viewpoint_manager_ns::ViewPointManager> viewpoint_manager_; 
/// 局部规划器，用于生成局部路径
std::unique_ptr<local_coverage_planner_ns::LocalCoveragePlanner> local_coverage_planner_;
/// 环境网格，用于管理生成全局路径
std::unique_ptr<grid_world_ns::GridWorld> grid_world_;

下面分别介绍。

1、keypose_graph_

keypose_graph_是一张位姿图，图中的节点分为关键节点及非关键节点，其中关键节点是机器人所途径的关键帧位姿，非关键节点则是在每次规划前，计算的当前机器人单元格到其前后左右上下6个近邻单元格的路径点。除节点外，还有节点之间的连接边。

std::vector<KeyposeNode> nodes_;      /// 节点
std::vector<std::vector<int>> graph_; /// 边

'单元格'是指 grid_world_ 中的 cell ，当前机器人单元格到其近邻单元格的路径点则是由局部规划区域内的viewpoint_manager_中的连通视点生成，具体实现位于grid_world.cpp中的GridWorld::AddPathsInBetweenCells

函数。

因此，keypose_graph_是一张机器人途径范围内的稀疏地图，也就是论文中的 'sparse random roadmap'，在全局路径规划中用于计算状态为'EXPLORING'的局部规划区域外的任意两单元格间的最短路径，以求解TSP问题，然后根据TSP求解顺序计算单元格主路径点间的普通路径点以构成全局路径，具体实现位于 grid_world.cpp 中的 GridWorld::SolveGlobalTSP 。

2、planning_env_

planning_env_ 规划环境管理对象主要用来管理规划环境中的点云情况及空间占用情况，主要由两个网格管理器及三个点云容器进行管理：

/// 全局关键帧垂直点云网格管理器
std::unique_ptr<pointcloud_manager_ns::PointCloudManager> pointcloud_manager_;
/// 滚动占用栅格管理器
std::unique_ptr<rolling_occupancy_grid_ns::RollingOccupancyGrid> rolling_occupancy_grid_;
/// 规划点云容器，来自pointcloud_manager_中的pointcloud_grid_中的局部区域网格
std::unique_ptr<pointcloud_utils_ns::PCLCloud<PlannerCloudPointType>> planner_cloud_;
/// 边界点云容器，来自rolling_occupancy_grid_中的occupancy_array_中的边界栅格点
std::unique_ptr<pointcloud_utils_ns::PCLCloud<pcl::PointXYZI>> filtered_frontier_cloud_;
/// 碰撞点云容器
pcl::PointCloud<pcl::PointXYZI>::Ptr collision_cloud_;

算法接收到的点云数据（配准点云及地面分析点云）都经planning_env_处理用以更新环境各属性或对象的状态，例如视点的碰撞性、连通性等，下面分别介绍pointcloud_manager_ 及 rolling_occupancy_grid_;

2.1 pointcloud_manager_

pointcloud_manager_ 使用如下两个对象管理垂直点云（地面点云外的点，也是碰撞点云）及局部环境外的占栅格点云：

/// 全局点云网格，用于保存关键帧点云中的垂直点云
std::unique_ptr<grid_ns::Grid<PCLCloudTypePtr>> pointcloud_grid_;
/// 存放局部环境外的占用栅格点
std::unique_ptr<grid_ns::Grid<pcl::PointCloud<pcl::PointXYZI>::Ptr>> occupancy_cloud_grid_;

pointcloud_grid_ 的管理范围为全局空间，并且将全局空间分为尺寸为50 * 50 * 30，分辨率为18m * 18m * 1.8m的三维网格，每个网格中都存储着对应位置的点云，这些点云来自关键帧点云中的垂直点，在每次执行规划任务前加入 pointcloud_manager_，新加入的点的g值为0，在UpdateCoveredAreas函数中会将机器人视线范围内的planner_cloud_点的g值设置为255；也就是说通过g属性值来区分点是否被观测过。

其中局部环境（机器人附近55 5网格）内的点云作为 planner_cloud_;

后面选择候选视点时，有一个条件就是视点视角覆盖的planner_cloud_中未观测点(g <255)的数量

occupancy_cloud_grid_ 中的点云为局部环境外的占栅格点，随着机器人的移动会对其进行更新，也就是当检测到随着机器人移动rolling_occupancy_grid_网格发生滚动后，将rolling_occupancy_grid_->occupancy_array_中滚出局部环境的占栅格点存放到occu_pancy_cloud_grid_中；并且如果occupancy_cloud_grid_中的占栅格点滚进局部环境中也会根据其状态来更新rolling_occupancy_grid_->occupancy_array_中对应的栅格状态。

2.2 rolling_occupancy_grid_

rolling_occupancy_grid_是滚动占用栅格管理器，主要用来管理局部环境的空间占用情况，其管理一个尺寸为300300 300，分辨率为0.3m0.3m 0.3m的网格，每个栅格的维护着一个空间状态{UNKNOWN = 0, OCCUPIED = 1, FREE = 2}。

rolling_occupancy_grid_中最重要的三个成员变量为：

/// 滚动管理网格
std::unique_ptr<rolling_grid_ns::RollingGrid> rolling_grid_;
/// 局部空间网格
std::unique_ptr<grid_ns::Grid<CellState>> occupancy_array_;
/// 网格发生rolling时滚出局部环境的占栅格点
pcl::PointCloud<pcl::PointXYZI>::Ptr occupancy_cloud_;

occupancy_array_ 中管理着局部空间中每个栅格的占用状态，状态变化的几种情况：

（1）随着近邻网格滚动，新增的近邻栅格状态设置为 UNKNOWN。

（2）根据重新滚进局部环境中的占栅格点（也就是重新进入局部环境中的pointcloud_manager_->occupancy_cloud_grid_中的点）属性修改其对应空间位置的栅格状态。

（3）新增点云对应的栅格点状态设置为 OCCUPIED。

rolling_grid_ 则管理网格中栅格实际对应空间状态与栅格索引的对应关系（这是由于随着机器人移动，网格表示的区域会发生滚动），也就是说，occupancy_array_中栅格索引与其栅格内容不是对应的，栅格内的状态并不是表示它的索引对应的空间的状态。这是因为随着机器人位置的移动，局部环境中的网格对应的空间位置也会发生滚动，而滚动后，occupancy_array_ 单元格内的状态就无法表示现在的空间的状态了，表示的是滚动前空间位置的状态，因此，rolling_grid_的作用就是将occupancy_array_中栅格索引与其栅格内容对应起来，从而保证机器人运动过程中，occupancy_array_中栅格的状态能正确地反映出机器人运动过程中，该栅格的空间状态。

因此要得到一个单元格的状态，需要先计算该单元格的坐标或索引，然后根据该索引从rolling_grid_中得到对应的occupancy_array_单元格的索引，然后再从occupancy_array_中得到索引对应的状态，根据实际的栅格位置，找到存储该栅格状态的 occupancy_array_ 索引。

rolling_grid_ 中有两个Grid对象，grid1_,grid_0，在网格发生滚动时交替更新这两个网格来实现occupancy_array_的索引管理。

occupancy_cloud_ 是当前滚出局部环境的占栅格点云，会将其添加到pointcloud_manager_->occupancy_cloud_grid_中。

边界点云就是从occupancy_array_中选取的满足条件的栅格点，具体条件为：

（1）栅格状态未知；

（2）水平方向有状态为 FREE 的邻近栅格；

（3）上下栅格状态不能是 FREE.

3、viewpoint_manager_

4、local_coverage_planner_

5、grid_world_