Grid map 的应用

1. 前言

在前面我们主要集中在TEB算法的学习。并且，在学习中，我已经把TEB算法从ROS的框架中移植出来，使用IPC中的shm替代ROS的socket通信，替代的过程可以查看我的队友lxz的blog。

其实，TEB的移植本身很好做，但是，因为它本身对ROS没有太严重的依赖，算法所需要的变量类型在TEB本身中已经定义完备，不需要依赖ROS所定义的类型。

但是，TEB算法所规划的路径是软约束，有可能产生很不可能通行的路径，因此，不能直接使用TEB算法所产生的路径。
需要对路径进行一定的判断和检验后，才能使用。
这里可以参考TEB算法中的teb_local_planner_ros.cpp的computeVelocityCommands函数。

在函数中，有很重要的一个检验：判断路径是否碰撞到障碍物。这里需要用到ROS的Navigation中的costmap。

但因为我的模块需要脱离ROS环境，因此，我这里选择使用了功能更加强大、方便和通用的grid map进行替代。

之后的所有路径可通行性判断，TEB算法的障碍物获取等功能的使用，都将基于Grid Map实现。
Grid Map项目地址为：https://github.com/ANYbotics/grid_map

2. Grid Map 简介

这是一个带有ROS接口的C++库，用于管理具有多个数据层的二维网格地图。它是为移动机器人测绘设计的，用于存储数据，如高程、方差、颜色、摩擦系数、立足点质量、表面法线、可穿越性等。

具体的介绍可以查看官网：http://wiki.ros.org/grid_map

3. Grid Map 使用

对于本项目，我只需要使用 Grid Map 的 Grid map core，而这一部分，完全脱离ROS实现，可以很方便的移植到自己的项目。

本次项目中，Grid Map 是一个很方便用来存储地图的容器。它可以无拷贝高效地实现地图中心跟随着小车移动，从而实现只记录小车周围障碍物地图的作用。

3.1 写入Grid Map

这一部分由我的队友yxs完成。但我这里可以大致说一下流程：

1. 输入：
   1. 世界坐标系下的lidar点云数据
   2. 小车当前位置的精确IMU信息
2. 使用滤波器筛选数据，滤除不关心的数据，比如：地面，超过小车6m的点。
3. 只考虑每个点的x, y坐标信息，使用占据栅格地图构建算法，往Grid Map中写入数据。通过这样的方法可以只保留静止障碍物的信息。点云地图内的行人走过的痕迹，会随着时间在地图中被逐渐擦除。
4. 使用move操作，更新 Grid Map 中心在世界坐标系下的位置，让地图紧跟小车当前在世界坐标系中的位置。

3.2 使用 Grid Map

当我们获取到通过占据栅格构建算法得到的 Grid Map 后，在本项目中，主要可以用来识别障碍物和判断TEB算法生成的轨迹是否碰到障碍物。

3.2.1 识别障碍物

识别障碍物的工作我是参考了costmap_convertor。然后基于 Grid Map 写了一个 convertor 用来从 Grid Map 识别障碍物并转换到 TEB 的障碍物类别。

具体流程如下：

1. 使用 DBSCAN算法进行聚类。从而获得到一个个的点集。
2. 使用 monotoneChain 对每一个点集，做凸包，得到对应的多边形的顶点的有序点集。
3. 将第2个步骤得到的有序点集转换成TEB算法定义的障碍物。

3.2.2 判断轨迹可通行性

这个部分主要是为了复现 TEB 算法的 ROS 接口中判断轨迹是否是可通行的函数 isTrajectoryFeasible 。

在原本的TEB算法中， isTrajectoryFeasible 是通过调用 navigation 中的一个模块 costamap ，让 costmap 判断给定的位姿下，小车是否有碰到障碍物。

我这里采用的方法和 costmap 类似。对于判断路径上的一个点是否有碰到障碍物，我选择通过使用 Grid Map 的迭代器，去查询在当前位姿点下，小车的足迹模型内是否存在障碍物。