作者：P´eter Fankhauser* ， Marco Hutter
原文：A Universal Grid Map Library: Implementation and Use Case for Rough TerrainNavigation
链接：https://www.researchgate.net/publication/284415855

摘要

在本研究章节中，我们介绍了我们在通用网格地图库上的工作，该库用作移动机器人的映射框架。它设计用于广泛的应用，例如在线表面重建和用于粗糙地形导航的地形解释。我们的软件具有多层地图，地图边界的计算效率高的重新定位以及与现有ROS地图消息类型的兼容性。数据存储基于线性代数库Eigen，提供了广泛的数据处理算法。本章概述了如何将网格地图库集成到读者自己的应用程序中。我们将解释这些概念，并提供代码示例来讨论该软件的各种功能。作为一个用例，我们介绍了使用有腿机器人进行在线高程映射的库的应用。网格地图库和以机器人为中心的高程映射框架可从以下网站获得开源elevation_mapping

简介

传统上，移动地面机器人被设计为在平坦的地形上移动，并且通常针对二维环境抽象开发其映射，规划和控制算法。当在崎岖的地形中导航时（例如使用履带车辆或有腿的机器人），必须扩展算法以考虑周围的所有三个维度。最流行的方法是构建环境的海拔图，其中水平面上的每个坐标都与海拔/高度值关联。为简单起见，通常将高程图存储和处理为网格图，可以将其视为2.5维表示，其中网格中的每个单元格都保留一个高度值。
在我们最近的工作中，我们开发了一个通用的栅格地图库，用作带有机器人操作系统（ROS）的移动机器人的通用映射框架。从我们的实现不限于任何特殊类型的输入数据或处理步骤的意义上说，该应用程序是通用的。该库支持多个数据层，例如适用于海拔，差异，颜色，表面法线，占用率等。基础数据存储实现为二维循环缓冲区。循环缓冲区的实现可实现地图位置的无损且计算有效的移位。例如，这在机器人在环境中移动时地图不断重新定位的应用中很重要（例如，以机器人为中心的地图[1]）。我们的软件通过提供一些帮助功能来简化地图数据的处理。例如，用于矩形，圆形和多边形区域的迭代器功能可实现对地图子区域的方便且内存安全的访问。所有栅格地图数据都以Eigen [2]（一种流行的C ++线性代数库）的数据类型存储。用户可以将可用的本征算法直接应用于地图数据，这为数据处理提供了通用而有效的工具。
costmap 2d [3，4]程序包也是一种流行的ROS程序包，它也可用于基于网格的地图表示形式。它是2D导航堆栈的一部分[5]，用于二维机器人导航。其功能是处理范围测量并建立环境的占用网格。占用率通常表示为“已占用”，“空闲”和“未知”等状态。在内部，成本图存储为无符号字符数组，其整数值范围为0–255。尽管足以处理成本图，但此数据格式在更一般的应用程序中可能会受到限制。为了克服这些缺陷，本工作中介绍的网格地图库将地图存储为float类型的矩阵。在处理诸如高度，方差，表面法线向量等物理类型时，这可以提供更高的精度和灵活性。为确保与现有ROS生态系统兼容，网格图库提供了将网格图转换为OccupancyGrid的转换器（用于2D导航）堆栈），GridCells和PointCloud2消息类型。将地图数据转换和发布为不同的消息类型也可以利用现有的RViz可视化插件。另一个相关的软件包是OctoMap库[6]及其关联的ROS接口[7]。 OctoMap将地图表示为具有占据和自由体素的三维结构。 OctoMap地图结构为八叉树，可以动态扩展，并可以包含具有不同分辨率的区域。与2.5维网格表示相比，OctoMap的数据结构非常适合表示完整的三维结构。当使用具有多个楼层，悬挑结构或机器人手臂运动计划任务的扩展地图时，这通常很有用。但是，由于必须执行对树节点的搜索，因此访问八叉树中的数据需要额外的计算成本[6]。取而代之的是，在后处理和数据解释步骤中，网格图的表示允许直接值访问和简化的数据管理。
网格地图库已充当多个应用程序的基础框架。在[1]中，建立了海拔图以计划有腿机器人穿过崎岖地形的运动（见图1a）。从机载Kinect深度传感器获取的距离测量值和机器人姿态估计值融合在环境的概率表示中。映射程序是从以机器人为中心的角度制定的，以明确说明机器人移动时姿势的漂移。在[8]中，网格图库已用于微型飞机（MAV）的自动着陆工作（见图1b）。根据车载惯性测量单元（IMU）和单眼相机的估计深度数据生成高程图。然后使用该地图自动找到车辆的安全着陆点。

图1.网格地图库已应用于各种地图绘制任务。 a）从机载Kinect深度传感器创建的高程图允许四足机器人StarlETH [9]在崎岖的地形[1]中导航。颜色表示高度估计的不确定性，红色表示不确定性较高，蓝色表示较确定性。 b）通过找到安全的着陆点来实现多直升机的自主着陆[8]。该地图是根据机载IMU和单眼摄像机的深度估算创建的。不安全的着陆区域用红色标记，安全的区域用蓝色标记，所选的着陆点用绿色标记。
在本章中，我们讨论了为各种应用程序实现我们的软件所需的步骤。在本章的其余部分，我们涵盖以下主题：
首先，我们演示如何下载栅格地图库并对其组成部分进行概述。基于一个简单的示例节点，我们介绍了集成库所需的基本步骤。
–其次，我们描述了库的主要功能。我们重点介绍了主要概念，并通过教程节点中的代码示例演示了它们的用法。
–第三，我们讨论[1]中介绍的带有高程贴图应用程序的软件用例。

总览

2.1先决条件和安装

在下文中，我们假定可以正常运行Ubuntu 14.04 LTS（Trusty Tahr）和ROS Indigo。 [10]中给出了在Ubuntu上安装ROS Indigo的安装说明。尽管我们将介绍这些版本的过程，但是Grid Map软件包也已针对ROS Jade进行了测试，并且可以在不进行任何改动的情况下与将来的版本一起使用。此外，我们假设已经按照[11]中的描述设置了一个柳絮工作区。除了标准安装中的ROS软件包（cmake -modules，roscpp，传感器msgs，nav_msgs等）之外，网格图库仅依赖于线性代数库Eigen [2]。如果您的系统尚不可用，请使用以下命令安装Eigen:

sudo apt-get install libeigen3-dev

要使用网格图库，请使用以下命令将关联的软件包克隆到catkin工作区的/ src文件夹中：

git clone git@github.com:ethz-asl/grid_map.git

通过构建您的catkin工作区来完成安装:

catkin_make

为了使性能最大化，请确保以发布模式进行构建。您可以通过设置来指定构建类型:

catkinmake -DCMAKEBUILDTYPE=Release

如果需要，您可以使用以下命令构建和运行相关的单元测试：

 catkin_make run_tests_grid_map_core run_tests_grid_map

请注意，我们的库使用了C ++ 11功能，例如列表初始化和基于范围的for循环。因此，CMake标志-std = c ++ 11被添加到CMakeLists.txt文件中。

2.2 软件组成

grid map库包括以下几部分组成：

grid_map_core
实现网格地图库的核心算法。它提供了GridMap类和几个帮助器类。该程序包的实现没有ROS依赖性。
grid_map
是使用网格图库的ROS依赖项目的主要软件包。它提供了将网格映射对象转换为多个ROS的接口
消息类型。
grid_map_msgs
网格图消息包含GridMap的ROS消息和服务定义消息类型。
grid_map_visualization
包含一个节点，该节点通过将其转换为标准ROS消息类型来可视化RViz中的GridMap消息。可视化类型和参数可通过ROS参数完全由用户配置。
grid_map_demos
包含几个用于演示目的的节点。简单演示节点是有关如何使用网格图库的简短示例。教程演示节点中提供了库功能的扩展演示。最后，迭代器演示和图像到网格图演示节点分别展示了网格图迭代器的用法以及图像到网格图的转换。
grid_map_filters
建立在ROS过滤器库[12]的基础上，以实现一系列用于栅格地图数据的过滤器。过滤器提供了标准化的API，用于根据运行时参数定义一系列过滤器。当编写软件来处理网格图作为一系列可配置的过滤器时，这提供了极大的灵活性。

2.3 一个简单的例子

下面，我们描述一个有关如何使用网格图库的简单示例，使用此代码来验证您是否安装了网格图包并开始使用自己的库。找到具有以下内容的文件grid_map_demos / src / simple_demo_node.cpp：

#include <ros/ros.h>
#include <grid_map/grid_map.hpp>
#include <grid_map_msgs/GridMap.h>
#include <cmath>using namespace grid_map;int main(int argc, char** argv)
{
// Initialize node and publisher.
ros::init(argc, argv, "grid_map_simple_demo");
ros::NodeHandle nh("~");
ros::Publisher publisher =
nh.advertise<grid_map_msgs::GridMap>("grid_map", 1, true);// Create grid map.
GridMap map({"elevation"});
map.setFrameId("map");
map.setGeometry(Length(1.2, 2.0), 0.03);
ROS_INFO("Created map with size %f x %f m (%i x %i cells).",
map.getLength().x(), map.getLength().y(),
map.getSize()(0), map.getSize()(1));// Work with grid map in a loop.
ros::Rate rate(30.0);
while (nh.ok()) {
// Add data to grid map.
ros::Time time = ros::Time::now();
for (GridMapIterator it(map); !it.isPastEnd(); ++it)
{Position position;map.getPosition(*it, position);map.at("elevation", *it) = -0.04 + 0.2 * std::sin(3.0 *time.toSec() + 5.0 * position.y()) * position.x();
}
// Publish grid map.
map.setTimestamp(time.toNSec());
grid_map_msgs::GridMap  message;
GridMapRosConverter::toMessage(map, message);
publisher.publish(message);
ROS_INFO_THROTTLE(1.0, "Grid map (timestamp %f) published.",message.info.header.stamp.toSec());
// Wait for next cycle.
rate.sleep();
}
return 0;
}

在此程序中，我们初始化一个ROS节点，该节点创建一个网格图，添加数据并发布它。该代码由几个代码块组成，我们将对其进行部分说明。

10 // Initialize node and publisher.
11 ros::init(argc, argv, "grid_map_simple_demo");
12 ros::NodeHandle nh("~");
13 ros::Publisher publisher =
14 nh.advertise<grid_map_msgs::GridMap>("grid_map", 1, true);

此部分使用名称网格映射简单演示（第11行）初始化节点，并设置私有节点句柄（第12行）。创建了类型为grid_map_msgs::GridMap的发布者，该发布者在主题grid_map上发布（第1行）。

15 // Create grid map.
16 GridMap map({"elevation"});
17 map.setFrameId("map");
18 map.setGeometry(Length(1.2, 2.0), 0.03);
19 ROS_INFO("Created map with size %f x %f m (%i x %i cells).",
20 map.getLength().x(), map.getLength().y(),
21 map.getSize()(0), map.getSize()(1));

我们创建一个类型为grid_map :: GridMap的变量映射（我们在第6行上使用命名空间grid_map进行设置）。栅格地图可以包含多个地图图层。
在这里，栅格地图由一层称为高程的图层构成。
指定帧ID，并将地图大小设置为1.2×2.0 m（边长沿地图框架的x和y轴），分辨率为0.03 m / cell。可选地，可以使用setGeometry（…）方法的第三个参数来设置地图（中心位置）的位置。打印输出显示有关生成的信息：

[INFO ][..]: Created map with size 1.200000 x 2.010000 m (40 x 67 cells）

请注意，请求的地图边长2.0 m已更改为2.01 m，这是自动完成的，以确保一致性，以使地图长为分辨率的倍数（0.03 m / cell）。

23 // Work with grid map in a loop.
24 ros::Rate rate(30.0);
25 while (nh.ok()) {

设置节点和网格图之后，我们将数据添加到该图并以30 Hz的循环发布它。

27  // Add data to grid map.
28  ros::Time time = ros::Time::now();
29  for (GridMapIterator it(map); !it.isPastEnd(); ++it) {
30  Position position;
31  map.getPosition(*it, position);
32  map.at("elevation", *it) = -0.04 + 0.2 * std::sin(3.0 *time.toSec() + 5.0 * position.y()) * position.x();
33  }

我们的目标是将数据添加到地图的高程层，其中每个像元的高度取决于像元位置和当前时间。 GridMapIterator允许迭代网格图的所有单元格（第29行）。使用迭代器上的*运算符，可检索当前单元格索引。它用于借助于网格图的getPosition（…）方法（第31行）来确定每个单元格的位置。当前时间（以秒为单位）存储在可变时间中。应用临时变量的位置和时间，计算出高度并将其存储在高程层的当前单元格中（第32行）。

35 // Publish grid map.
36 map.setTimestamp(time.toNSec());
37 grid_map_msgs::GridMap message;
38 GridMapRosConverter::toMessage(map, message);
39 publisher.publish(message);
40 ROS_INFO_THROTTLE(1.0, "Grid map (timestamp %f) published.",message.info.header.stamp.toSec()

我们更新地图的时间戳，然后使用GridMapRosConverter类将网格地图对象（类型为grid_map :: GridMap）转换为ROS网格地图消息（类型为grid_map_msgs :: GridMap，第38行）。最后，在先前定义的发布者（第39行）的帮助下，将messagis广播到ROS。

rosrun grid_map_demos simple_demo

这将运行节点简单演示并在主题grid_map_simple_demo / grid_map下发布生成的网格图。在下一步中，我们显示可视化网格图数据的步骤。网格图可视化软件包提供了一个简单的工具来可视化ROS网格
在RViz中以各种形式映射消息。它通过将网格地图消息转换为消息格式（例如PointCloud2，OccupancyGrid，Marker等）来利用现有的RViz插件。我们在grid_map_demos / launch / simple_demo.launch下创建具有以下内容的启动文件

1 <launch>
2       <!-- Launch the grid map simple demo node -->
3       <node pkg="grid_map_demos" type="simple_demo"name="grid_map_simple_demo" output="screen" />
4       <!-- Launch the grid map visualizer -->
5       <node pkg="grid_map_visualization" type="grid_map_visualization"name="grid_map_visualization" output="screen">
6       <rosparam command="load" file="$(findgrid_map_demos)/config/simple_demo.yaml" />
7       </node>
8       <!-- Launch RViz with the demo configuration -->
9       <node name="rviz" pkg="rviz" type="rviz" args="-d $(findgrid_map_demos)/rviz/grid_map_demo.rviz" />
10 </launch>

这将启动简单的演示（第3行），网格图可视化（第5行）和RViz（第9行）。网格图可视化节点通过以下配置从grid_map_demos / config / simple_demo.yaml中加载参数:

1    grid_map_topic: /grid_map_simple_demo/grid_map
2   grid_map_visualizations:
3        - name: elevation_points
4           type: point_cloud
5           params:
6           layer: elevation
7        - name: elevation_grid
8           type: occupancy_grid
9           params:
10      layer: elevation
11      data_min: 0.1
12      data_max: -0.18

这将通过grid_map_topic参数（第1行）将网格地图可视化与简单的演示节点相连，并为高程层添加PointCloud2（第3行）和OccupancyGrid可视化（第7行）。使用以下命令运行启动文件:

roslaunch grid_map_demos simple_demo.launch

如果一切设置正确，您将在RVizas中看到生成的网格图，如图2所示。

图2。启动simple_demo.launch文件将运行简单的演示和网格地图可视化节点并打开RViz。生成的网格图可视化为点云。

3 工具包描述

本节概述了栅格地图库的功能。我们描述了该API，并通过位于grid_map_demos / src /tutorial_demo_node.cpp中的tuial文件中的示例代码演示了其用法。本教程通过使高程图层变质并带有噪点和外层（地图中的孔）来扩展简单的演示。应用平均滤波步骤重建原始数据，并分析剩余误差。您可以运行包括演示的教程演示：

roslaunch grid_map_demos tutorial_demo.launch

图3.网格图库使用多层网格图来存储不同类型信息的数据。

3.1添加，访问和删除图层

在使用移动机器人地图时，我们经常计算其他信息来解释数据并指导导航算法。网格地图库使用图层来存储不同类型数据的信息。图3说明了多层网格图的概念，其中每个单元的数据都存储在一致的层上。
可以将栅格地图类初始化为空，也可以直接使用诸如

18 GridMap map({"elevation", "normal_x", "normal_y", "normal_z"}；

可以使用void add（const std :: string＆layer，const float value）将新图层添加到现有地图，其中，参数值确定用于填充新图层的值。另外，也可以添加新图层的数据

45 map.add("noise", Matrix::Random(map.getSize()(0), map.getSize()(1))；

如果添加的图层已存在于地图中，则将其替换为新数据。可以使用exist（…）方法检查地图中图层的可用性。通过get（…）方法及其简短形式的运算符[…]可以访问图层数据：

46 map.add("elevation_noisy", map.get("elevation") + map["noise"]；

第3.7节中提供了将加法运算符+与网格图配合使用的更多详细信息。可以使用 erase(…）从地图上移除图层。

3.2 设置几何形状和位置

为了一致地表示数据，我们定义了网格图的几何特性，如图4所示。该图是在网格图框架中指定的，该图与该网格图对齐。地图的位置定义为指定框架中矩形地图的中心。地图的基本几何形状（如边长，分辨率和位置（请参见图4））通过setGeometry（…）方法进行设置。当使用不同的坐标系时，指定网格图的框架非常重要：

19 map.setFrameId("map");
20 map.setGeometry(Length(1.2, 2.0), 0.03, Position(0.0, -0.1)；

图4.网格图被定义为与其网格图框对齐。将地图的位置指定为地图相对于其框架的中心。
在添加任何数据之前设置几何非常重要，因为在重新设置网格图时会清除所有像元数据。

3.3 访问单元格

如图4所示，可以通过以下两种方式来访问网格地图层的各个单元格：通过使用
float& at(const std::string& layer, const grid_map::Index& index)
进行直接索引访问或通过引用以下位置底层单元格带有
float＆atPosition（const std :: string＆layer，const grid_map :: Position＆position）
位置与相应单元格索引之间的转换（反之亦然）由getIndex（…）和getPosition（…）方法给出。这些转换版本处理循环缓冲区存储所涉及的底层算法，并且是处理单元格索引/位置的推荐方式。用法示例如下：

51 if (map.isInside(randomPosition))
52 map.atPosition("elevation_noisy", randomPosition) = ..；

图5.栅格地图库提供了move（…）方法，以有效地改变栅格地图的位置，并且计算效率高且无损。网格图作为二维圆形缓冲区的底层实现允许在不分配新内存的情况下移动地图。
在这里，辅助方法isInside（…）用于确保所请求的位置在地图的范围内。

3.4 移动地图

栅格地图库提供了一种计算有效且无损的方法，无需移动即可更改栅格地图的位置
void move(const grid_map::Position& position)
参数position指定了栅格地图的新绝对位置。网格图框（请参阅第3.2节）。该方法将网格重新定位，以使网格在先前位置和新位置之间对齐（图5）。位置更改前后的重叠区域中的数据仍将被存储。落在地图新位置之外的数据将被丢弃。清空先前未知区域的细胞（设置为nan）。数据存储被实现为二维循环缓冲区，这意味着映射的重叠区域中的数据不会在内存中移动。这样可最大程度减少移动地图时需要更改的数据量。

3.5 基本层

对于某些应用程序，使用setBasicLayers（…）定义一组基本层很有用。检查网格地图单元格的有效性时会考虑这些层。如果该单元格在所有基本层中均具有有效（有限）值，则该方法将其声明为有效单元格

图6.迭代器是处理属于某些几何区域的单元的便捷方法。 a）GridMapIterator用于迭代网格图的所有单元格。 b）SubmapIterator访问属于地图矩形区域的像元。 c）CircleIterator遍历由中心和半径指定的圆形区域。 d）PolygonIterator由n个顶点定义，并覆盖所有inlyingcells。 e）LineIterator使用Bresenham的线算法[13]迭代由起点和终点定义的线。有关如何使用网格地图演示程序包的迭代器演示节点中给出的网格地图迭代器的示例代码。
bool isValid(const grid_map::Index& index) const.
同样，方法clearBasic（）将仅清除基本层，并且比clearAll（）清除所有层的效率更高。默认情况下，基本层列表为空。

3.6 遍历单元格

通常，人们想要遍历网格图中几何形状内的多个单元，例如检查机器人足迹所覆盖的单元。网格图库为各种区域提供了迭代器，例如矩形子图，圆形和多边形区域以及直线（请参见图6）。可以使用形式为for的循环来实现迭代

32 for (GridMapIterator it(map); !isPastEnd(); ++it) {

取消引用运算符*用于检索迭代器的当前单元格索引，例如以下示例：

35 map.at("elevation", *it) = ...;

67 Position currentPosition;68 map.getPosition(*it, currentPosition);

76 if (!map.isValid(*circleIt, "elevation_noisy")) continue;

这些迭代器可在地图边界使用，而无需担心，因为它们在内部确保仅访问地图边界内的像元。

3.7使用Eigen函数
由于网格图的每一层都作为Eigen矩阵在内部存储，因此Eigen库[2]提供的所有功能都可以直接应用。以下示例说明了这一点:

46 map.add("elevation_noisy", map.get("elevation") + map["noise"];

在这里，两层标高和噪声的像元值会进行算术求和，结果存储在新的层标高噪声中。或者，可以直接使用+ =运算符将噪声添加到高程层：

map.get("elevation") += 0.015 * Matrix::Random(map.getSize()(0),
map.getSize()(1));

一个更高级的示例演示了通过使用各种Eigen函数所提供的简单性：

91 map.add("error", (map.get("elevation_filtered") -map.get("elevation")).cwiseAbs());
92 unsigned int nCells = map.getSize().prod();
93 double rmse = sqrt(map["error"].array().pow(2).sum() / nCells

在此，计算两层之间的绝对误差（第91行），并将其用于计算均方根误差（RMSE）（第93行）:

其中ci表示原始高程图层的像元i的值，表示已过滤的高程过滤图层的像元值的个数，n表示网格图的像元数。

3.8 创建子图

可以使用以下方法生成栅格地图中零件的副本：

GridMap getSubmap(const grid_map::Position& position, const grid_map::Length& length, bool& isSuccess) .

返回值为GridMap类型，所有描述的方法也适用于检索到的子图。检索子图时，将对数据进行深拷贝，并且更改原始图或子图中的数据不会影响其副本。如果只有部分数据需要进一步处理，则使用子图通常有助于最大程度地减少计算负荷和数据传输。
要访问或操作子地图区域中的原始数据而不进行复制，请参阅第3.6节中描述的SubmapIterator。

3.9 转换ROS数据类型
GridMap类可以通过GridMapRosConverterclas的Message（…）和Message（…）中的方法在ROS Grid Map消息之间进行转换。

97 grid_map_msgs::GridMap message;
98 GridMapRosConverter::toMessage(map, message);

此外，可以使用saveToBag（…）和loadFromBag（…）方法将网格图保存到ROS bag文件中并从中进行加载。为了兼容性和可视化目的，还可以将网格图转换为PointCloud2，Occu pancyGrid和GridCells消息类型。例如，使用这种方法：

static void toPointCloud(const grid_map::GridMap& gridMap,const std::vector<std::string>& layers,const std::string& pointLayer,sensor_msgs::PointCloud2& pointCloud) ,

网格图将转换为PointCloud2消息。在这里，第二个自变量层确定将哪些层转换为点云。第三个参数pointLayer中的图层指定将这些图层中的哪一个用作点的z值（x和y由单元格位置给出）。所有其他层作为附加字段添加到点云消息中。这些附加字段可在RViz中用于确定点的颜色。这是一种可视化地图特征（例如不确定性，地形质量，材质等）的便捷方法。
图7显示了本教程演示的网格图可视化节点所生成的不同ROS消息类型。此示例的网格图可视化设置存储在grid_map_demos / config / tutorial_demo.yaml参数文件中。

3.10 从图像添加数据

网格图库允许从图像加载数据。第一步，可以使用GridMapRosConverter :: initializeFromImage（…）方法将网格图的几何形状初始化为图像的大小。提供了两种不同的方法来将图像中的数据添加为网格地图中的图层。要添加数据作为标量值（通常来自灰度图像），可以使用方法addLayerFromImage（）。这要求为图像的相应黑白像素指定下限值和上限值。图8a显示了一个示例，其中使用了图像编辑软件来绘制地形。要将图像中的数据添加为颜色信息，可以使用addColorLayerFromImage（）。例如，这可以用于将来自照相机的颜色信息添加到网格图，如图8所示。

图7.可以将网格图转换为几种ROS消息类型，以便在RViz中进行可视化。 a）可视化为PointCloud2消息类型，显示了教程演示过滤步骤的结果。颜色表示噪声破坏之前的滤波结果与原始数据之间的绝对误差。 b）向量（这里是表面法线）可以在标记消息类型的帮助下可视化。 c）将栅格地图的单层（此处为高程层）表示为OccupancyGrid。 d）相同的图层显示为GridCells类型，其中阈值内的单元是可视化的。

4用例：高程图

基于网格地图库，我们开发了一个ROS软件包，用于使用自主机器人进行本地地形映射。本节介绍了映射过程的技术背景，讨论了程序包的实现和用法，并提供了来自实际实验的结果。

4.1 背景

要使机器人能够在以前看不见的崎岖地形中导航，就需要在机器人穿越环境时重新构建地形。在这项工作中，我们假设现有的机器人姿态估计和机载距离测量传感器。对于许多自主机器人而言，姿势估计易于漂移。如果出现姿势估计漂移，则将新扫描与以前的数据拼接在一起会导致地图不一致。为了解决这个问题，我们从机器人为中心的角度制定了概率高程映射过程。在这种方法中，生成的地图是从局部角度估计地形的形状。我们在图9中说明了以机器人为中心的高程映射过程。我们的映射方法包括三个主要步骤：
1.测量更新：距离传感器的新测量值与地图中的现有数据融合在一起。通过对距离传感器的三维噪声分布建模，我们可以将深度测量误差传播到相应的高程不确定性。借助卡尔曼滤波器，新的测量值将与地图中的现有数据融合。
2.地图更新：对于以机器人为中心的公式，需要在机器人移动时更新高程图。我们将姿势协方差矩阵的变化传播到网格图中单元格的空间不确定性。这反映了海拔图中姿态估计的误差（例如漂移）。在此步骤中，将分别处理每个像元的不确定性以最大程度地减少计算量。
3.地图融合：在计划步骤中需要进一步处理地图数据时，将计算像元高度的估计值。这需要从其周围的单元格推断出每个单元格的高度和方差。
应用我们的映射方法，在考虑到距离传感器误差的情况下重建地形，并且机器人会带来不确定性。网格图中的每个像元都保存有关高度估计和相应方差的信息。机器人前方的区域通常具有最高的精度，因为它会根据前视距离传感器的新测量值不断更新。由于机器人相对姿态估计值的漂移，不在传感器视野范围内的区域（在机器人下方或后面）的确定性降低了。应用概率方法，运动/轨迹规划算法可以利用地图数据的可用确定性估计。

图8.图像数据可用于填充网格图。 a）在图像编辑软件中绘制地形并用于生成高度场网格地图图层。 b）网格图中的颜色层由机器人前后两个广角摄像机的投影视频流更新。在 grid_map_demos程序包的image_to_gridmap demo节点中，给出了有关如何从图像向网格地图添加数据的示例代码。

4.2实施

我们已将以机器人为中心的高程映射过程实现为ROS包。节点订阅PointCloud2深度测量和PoseWithCovarianceStamped类型的机器人姿势估计。对于深度测量，我们提供传感器处理器，这些传感器处理器根据设备的噪声模型生成测量方差。我们支持多种距离测量设备，例如Kinect型传感器[14]，激光距离传感器和立体声相机。

图9.高海拔映射映射过程是从以机器人为中心的角度制定的。
在机器人周围构建本地地图，该地图的位置会不断更新，以覆盖机器人周围的区域。这可以通过使用网格地图库提供的move（…）方法以最小的计算开销来实现（请参见第3.4节）。
高程贴图节点将生成的地图作为GridMap消息发布（包含高程，方差等图层），涉及两个主题。未融合的高程图（仅考虑第4.1节中的处理步骤1和2）在主题levation_map_raw下连续发布，并可用于监视映射过程。如果调用了ROS服务trigger_fusion，则将计算融合的海拔图（第4.1节中处理步骤3的结果）并将其发布在height_map下。可以通过get_submap服务调用来请求部分（融合）映射。该节点将地图更新和融合步骤实现为多线程过程，以实现连续的测量更新。
使用网格图可视化节点，可以通过多种方式在RViz中可视化高程图。将地图显示为点云并用来自不同图层的数据为点着色很方便。例如，在图1中，点用地图的方差数据着色，在图10中，点用RGB摄像机的颜色着色。使用单独的节点进行可视化是一种将计算负荷分散到不同系统并限制从机器人到操作员计算机的数据传输的好方法。

4.3 结论

图10.四足机器人StarlETH [9]通过使用高程映射节点在其当前位置周围绘制环境来越过障碍物。 Kinect型距离传感器与机器人姿态估计结合使用以生成地形的概率重建。
我们已经在四足机器人StarlETH [9]上实现了海拔标测软件（见图10）。朝下的PrimeSense Carmine 1.09结构光传感器作为距离传感器安装在机器人的正面。状态估计基于运动学和惯性测量的随机融合[15]，其中位置和偏航角通常不可观察，因此容易发生漂移。我们生成了一个高度图，尺寸为2.5×2.5 m，分辨率为1 cm / cell，并在2.60 GHz机载Intel Core i3上以20 Hz的距离测量和姿势估计对其进行了更新。该映射软件可以处理数据以足够的速度将地图用于实时导航。由于距离传感器的高分辨率和更新率，地图保存了密集的信息，仅包含很少的没有高度信息的像元。这是碰撞检查和立足点选择算法的重要先决条件。得益于数据的概率融合，可以准确地捕获真实的地形结构，并有效地抑制离群值和虚假数据。在另一种设置中，我们还使用了海拔图数据来查找穿过环境的可穿越路径。图11描绘了机器人在走廊地图中规划无碰撞路径的过程。机器人前部的旋转Hokuyo激光测距传感器用于生成6×6 m的高程图，分辨率为3 cm / cell。在这项工作中，对海拔信息进行处理以估计地形的可穿越性。多个自定义网格地图过滤器（请参阅第2.2节）用于根据坡度，台阶高度和地形的粗糙度来解释数据。基于RRT的[16]规划器为水平平移x和y以及偏航角ψ寻找到目标姿势的可穿越路径。在搜索树的每次扩展中，借助于PolygonIterator来检查机器人足迹中包含的所有单元的可遍历性（请参见第3.6节）。

图11.根据获取的高程图判断地形的可穿越性。可穿越性估算考虑了诸如坡度，步长和地形粗糙度等因素。使用基于RRT的[16]规划器可以找到机器人姿势的无碰撞路径。

5 总结和结论

在本章中，我们介绍了一个网格地图库，该库是为使用移动机器人绘制应用程序而开发的。基于一个简单的示例，介绍了将库集成到现有或新框架中的第一步。我们重点介绍了该软件的某些功能，并通过教程示例中的代码示例说明了它们的用法。最后，展示了栅格地图库在高程地图中的应用，从而讨论了背景，用途和结果。
在我们当前的工作中，我们在诸如多机器人映射，立足点质量评估，运动计划的碰撞检查以及通过颜色信息进行地形属性预测等应用程序中使用网格地图库（与海拔映射程序包结合使用）。我们希望我们的软件对其他从事使用移动机器人进行地形地形绘制和导航的研究人员有用。

[地图]通用网格地图库：粗糙地形导航的实现和用例相关推荐

【开源项目分享】用于机器人崎岖地形导航的高程图创建及Gazebo仿真
简介该项目是由苏黎世联邦理工学院(自治系统实验室和机器人系统实验室)开发,用于创建以机器人为中心的高程图,以实现崎岖地形的自主导航任务. 项目演示 https://www.bilibili.com/ ...
Unity的使用（四）：预制体，创建地形和地形导航
前面介绍了Unity游戏引擎的基础功能,现在终于要进入到游戏开发中了.那么,一款游戏开发要有资源,这个一般是由美术提供的,我们只需要负责程序方面的事.那么,怎么将获得的资源应用起来呢? 一. 导入资源 ...
百度步行导航加poi搜索android,【百度地图】带地图显示控件、导航控件、POI查找控件...
标题: [完全开源]百度地图Web service API C#.NET版,带地图显示控件.导航控件.POI查找控件概述: 源代码主要包含三个项目,BMap.NET.BMap.NET.Windows ...
1. 普通地图（建筑蓝图、手机导航地图）能直接用于移动机器人的定位导航吗？《移动机器人导航杂谈》系列
普通地图(建筑蓝图.手机导航地图)能直接用于移动机器人的定位导航吗? 对于做SLAM.VIO的同学来讲,这个问题的答案是一个非常基本的常识,答案是不能. 但对于一些初入移动机器人导航领域或者非相关方向 ...
IOS-百度地图API用点生成线路、导航、自定义标注 2013年11月更新
IOS百度地图API开发自定义气泡,点击气泡自动生成路线,以及拖拽 IOS百度地图开发POISearch搜索附近停车场,附近加油站 IOS百度地图视角跳到用户当前位置 IOS百度地图开发实时路况 IO ...
html5tab页高德地图,高德地图系列web篇——目的地公交导航
在web开发中经常会有地图模块,经常有一些公交以及POI搜索,下面是一个实际的例子 Paste_Image.png 最终导航效果图: Paste_Image.png 所有代码,注意要换掉你的key b ...
小程序里显示店铺地址，可在地图上查看，可点击导航到店铺
老规矩,先看效果图可以在地图上显示店铺位置,地址,联系方式点击位置可以调起导航功能第一步,获取经纬度因为小程序内置的是腾讯地图,所以你需要到腾讯地图上查询经纬度. 首先,你要明确一个事情,任何 ...
mc服务器小地图不显示玩家,我的世界旅行地图小地图不显示只显示地形 | 手游网游页游攻略大全...
发布时间:2016-04-03 我的世界游戏中是可以旅游的哦,在旅行的时候地图是很关键的,今天为大家整理了几款经典的辅助工具,想要的朋友们可以看看下面的怎么下载我的世界旅行地图我的世界各种旅行者必备 ...
立体栅格地图_网格Mesh-3D立体图形-教程-地图 JS API | 高德地图API
Mesh指的是所有用三角形面组合成的三维物体,JS API 目前提供的 Mesh 类型有:Mesh.MeshAcceptLights.Prism.MeshLine 四种,MeshLine表现为带状线条 ...
python怎么画地图空间分异图_基于地形梯度的赣南地区生态系统服务价值对人为干扰的空间响应...
南方山地丘陵是我国主体功能区划中"两屏三带"生态安全战略格局的重要组成部分, 是我国华南和西南地区的生态屏障带, 是长江流域与珠江流域的分水岭及源头区, 对于区域乃至全球的气候调节 ...

[地图]通用网格地图库：粗糙地形导航的实现和用例

摘要

简介

总览