无人车路径规划算法---（2）地图

上一篇博客 "无人车路径规划算法—（1）算法体系综述 " 中介绍了无人车路径规划通常采用的"前端生成路径->后端平滑优化"的pipeline，本文将为大家介绍路径规划算法常用地图格式，文章主要分为以下三个模块：

• 常用地图格式
• 栅格地图
• 拓扑地图与高精度地图

生成一个随机二维栅格地图方法的代码将开源在博主github主页

---

地图（MAP）

无人车的规划模块是联通感知模块，定位模块与控制模块的中间模块（架构图见图一），因此，基于搜索的规划算法通常是在能够包含无人车自身定位信息以及周边环境信息的数据结构体中进行运算，这种能够体现无人车周边环境信息的数据载体统称为地图（MAP），在介绍搜索具体的搜索算法之前，需要先引入搜索地图的概念以及常用的地图类型。

本部分将重点说明以下四种常见形式：

• Point Cloud Map（点云地图）
• Metric Map(度量地图)
• Topological Map(拓扑地图)
• Semantic Map(语义地图)

1. Point Cloud Map（点云地图）

说到自动驾驶的地图，大家常最先想起的就是激光雷达给出的点云地图，但是原始点云地图通常不会被拿来进行路径规划使用，主要是由于以下原因：

• 点云地图通常规模很大，需要大量的存储空间，同时点云地图提供了很多不必要的细节
• 处理多个点云重叠处的效果不好

因此，需要利用costmap或者octomap进一步的处理（见第二部分）。

2. Metric Map（度量地图）

度量地图框架是最常见的框架，它考虑的是目标对象的二维空间。用精确的坐标表示目标对象以及各个目标之间的相对关系。这种表示方法非常有效，但其对噪声敏感，并且很难计算距离精度。譬如在地图中，北京可以用固定的经纬度来表示，上海可以用另一个固定的经纬度来表示，两者之间的距离可以通过各自的经纬度推算出来。
通常我们用稀疏（Sparse）与稠密（Dense）对它们进行分类：1

• 稀疏地图：对地图信息进行一定程度的抽象，只保留需要关注的信息，对次要信息进行简化
• 稠密地图：对所有地图中所有见到的信息进行描述，对于无人车的路径规划模块来说，稠密地图是十分必要的，通常我们将所有的有效信息通过设定的分辨率，分割到每个固定的栅格中（二维的栅格称为Grid,三维的栅格称为Voxel）。每个栅格由占有，空闲，与未知三种状态构成以表述对应栅格的障碍物占据情况。但是我们也看到，这种地图需要存储每一个格点的状态，耗费大量的存储空间，而且多数情况下地图的许多细节部分是无用的。

在度量地图中，与无人车规划模块或机器人导航模块应用最广泛的则是占据栅格地图（Occupancy Grid Map）与代价地图（CostMap）

2.1 Occupancy Grid Map (占据栅格地图)

占据栅格地图是把环境划分成一系列栅格，其中每一栅格给定一个可能值，表示该栅格被占据的概率。占据栅格的构建主要是基于二值贝叶斯滤波原理利用多传感器（激光雷达，相机等）的输入判断地图内每个栅格的障碍物占据概率。
由于占据栅格地图通常是单层结构，更适用于简单的路径规划功能，当机器人或无人车对于环境信息精确度要求不高时可以利用此地图进行规划，当规划模块对精度与安全性要求较高时，更好的选择是利用代价地图（CostMap）进行规划。

2.2 CostMap (代价地图)

Costmap是无人车或者机器人融合多传感器信息后建立维护的二维或三维栅格地图，在地图中障碍物信息会被转换到对应的栅格中并且利用相应的规则对障碍物进行膨胀。ROS的官方Navigation库中最早利用costmap进行障碍物的位置判断以及在此基础上利用DWA（原理可参见我之前的系列博客）算法进行了局部规划，后来在日本的开源框架Autoware中也基于costmap实现了乘用车的局部规划功能（HybridAstar算法）。
ROS官方Navigation库中对于costmap说明文档如下：http://wiki.ros.org/costmap_2d/
官方的说明文档中有以下两幅图需要重点关注：

• 地图结构说明图 （见图一）
• 障碍物膨胀（inflation）与栅格分数关系图（见图二）

下图（图一）可以大致描述出costmap地图中的三个主要组成要素：

1. Footprint （红色多边形）
2. 障碍物的膨胀层 （蓝色栅格）
3. 障碍物位置 （红色栅格）

下图（图二）详细的解释了障碍物膨胀区栅格分数的取值依据，其中红色多边形为车辆轮廓：

1. Lethal：机器人的中心与障碍物所占据栅格重合，此时机器人必然与障碍物冲突
2. Inscribed：障碍物所占据栅格与机器人footprint内切圆重合，此时机器人必然与障碍物冲突
3. Possibly circumscribed：机器人外接圆与障碍物占据栅格相切或接近，因此是否碰撞取决于机器人的朝向等因素
4. Freespace：自由空间，无碰撞风险
5. Unknow：未知区域
6. 其他的栅格的分数可以根据与Lethal以及Freespace的距离来计算，也可以根据自身产品设计相应的衰减函数来推算

介绍完costmap自身构建原理之后，另外一个值得注意的重点则是栅格地图坐标系与world坐标系转换问题：

单层地图由许多个cell组成，一般可以认为单层地图有两个坐标系：位置坐标系(以m为单位)和索引坐标系(以cell为单位)。在地图构造完成后，会有索引坐标系到位置坐标系的转换关系，所以在定位某个cell的时候可以使用任意坐标系，不存在本质性的区别。

转换关系如下图所示2：

• 地图坐标系（map frame，坐标系原点即map center，其在world中的位置就是mapPosition，X正向上，Y正向左)
• 内存坐标系（原点为内存块的左上角，即第一个存储的cell，可以近似理解为图像坐标系，X正向下，Y正向右）

样例代码片段如下：

/**
*@ This Section Is For Transform pose from world frame to index frame
*@ This Is The First Method
*/
gemoetometry_msgs::Pose transformPose(const geometry_msgs::Pose& pose, const tf::Transform& tf)
{
    // convert ROS pose to TF pose
    tf::Pose tf_pose;
    tf::poseMsgToTF(pose, tf_pose);
    
    // transform pose
    tf_pose = tf * tf_pose;
    
    // convert TF pose to ROS pose
    geometry_msgs::Pose ros_pose;
    tf::poseTFToMsg(tf_pose, ros_pose);
    
    return ros_pose;
}

void poseToIndex(const geometry_msgs::Pose& pose, int* index_x, int* index_y) 
{
    tf::Transform orig_tf;
    tf::poseMsgToTF(costmap_info.origin, orig_tf);
    geometry_msgs::Pose pose_new = transformPose(pose, orig_tf.inverse());
    
    *index_x = pose_new.position.x / costmap_info.resolution;
    *index_y = pose_new.position.y / costmap_info.resolution;
}

/**
*@ This Section Is For Transform pose from world frame to index frame
*@ This Is The Second Method
*/
void poseToIndex(const geometry_msgs::Pose& pose, int* index_x, int* index_y)
{
    *index_x = (pose.position.x - costmap_.info.origin.position.x) / costmap_.info.resolution;
    *index_y = (pose.position.y - costmap_.info.origin.position.y) / costmap_.info.resolution;
}

/**
*@ This Section Is For Transform pose from index frame to world frame
*/
void indexToPose(int index_x , int index_y , geometry_msgs::Pose& pose) 
{
  double px, py;
  px = costmap_.info.origin.x + (*index_x + 0.5) * costmap_.info.resolution;
  py = costmap_.info.origin.y + (*index_y + 0.5) * costmap_.info.resolution;
  pose.position.x = px;
  pose.position.y = py;
}

2.3 开源库分享

grid_map 开源库：

1. grid_map库是设计有ROS接口的C++库，用于管理具有多个数据层的二维栅格地图。它是为移动机器人地图设计的，用于存储(海拔)高度、方差、颜色、摩擦系数、可通过性等数据。它用于为粗糙地形导航设计的以机器人为中心的高程地图包中。
2. 链接：https://github.com/ANYbotics/grid_map

Octomap 开源库：

1. Octomap是SLAM领域经常用到的地图结构，它以八叉树数据结构来存储三维环境的概率占据地图，采用此结构可以节省大量的存储空间同时可以实现地图的压缩与更新，并且地图的分辨率可调。
2. 链接：https://github.com/OctoMap/octomap

2.4 式例代码

编写了一个随机生成障碍物的二维地图 (跳票中。。。。最近活太多。。。正在努力填坑中，后面也会找时间把这段时间做的东西整理一下分享出来)

3 Topological Map （拓扑地图）

拓扑地图则更强调地图元素之间的关系。拓扑地图是一个图（Graph），由节点和边组成，只考虑节点间的连通性。它放松了地图对精确位置的需要，去掉地图的细节问题，是一种更为紧凑的表达方式。这种地图更适用于低纬度的路径规划，因此接触的不多，有经验的网友欢迎补充。

4 Semantic Map （语义地图）

语义地图个人理解可以分为ADAS MAP和高精度地图（HD MAP）,ADAS地图是级别低于高精度地图的一种包含语义信息的地图，我们这里还是主要介绍一下高精度地图。
高精度地图主要服务与自动驾驶车辆（用于机器人太奢侈，高精度地图成本太高），通过一套独特的导航体系，帮助自动驾驶解决系统性能问题，扩展传感器检测边界。它可以帮助车辆找到合适的行车空间，还可以帮助Planner确定不同的路线选择。如APOLLO中，高精度地图可以帮助车辆识别准确的道路中心线，这样在规划时就可以让车辆尽可能沿道路中心线行驶。同时在十字路口等交叉路口也可以帮助车辆缩小规划范围，以便节省计算成本与时间，在最有范围内选择出最佳路线。

参考文献：
1《视觉SLAM十四讲：从理论到实践》
2 https://github.com/FleverX/grid_map