Author: P'eter Fankhauser *, Marco Hutter
original: A Universal Grid Map Library: Implementation and Use Case for Rough TerrainNavigation
link: https://www.researchgate.net/publication/284415855
Summary
In this study section, we introduce our work on a common grid map library, the library used as a mobile robot mapping framework. It is designed for a wide range of applications, such as online and surface reconstruction for rough terrain navigation terrain explanation. Our software has multi-level map, a map with high computational efficiency boundary repositioning and compatibility with existing ROS map message type. Data storage is based on linear algebra library Eigen, it offers a wide range of data processing algorithms. This chapter provides an overview of how the grid will be integrated into the reader's own library applications. We will explain these concepts, and provide code examples to discuss the various features of the software. As a use case, we have introduced the use of robotic leg elevation maps online application library. Grid map library and robot-centric elevation mapping framework can be obtained from the following website open source elevation_mapping
Brief introduction
Traditionally, mobile ground robots are designed to move on flat terrain, and usually abstract map for the development of its two-dimensional environment, planning and control algorithms. When navigating rough terrain (e.g., using a robot with a tracked vehicle or leg), the algorithm must be extended to account for all three dimensions around. The most popular method is to build FIG altitude environment, wherein each coordinate in the horizontal plane is associated with elevation / height value. For simplicity, FIG elevation typically stored and processed as a grid map, it can be viewed as 2.5D, where each cell in the grid have a height retention value.
In our recent work, we developed a common grid map library, as a general purpose mapping framework mobile robot with a robot operating system (ROS) of. From our implementation is not limited to the sense of the input data or the processing steps in any particular type of said application it is generic. The library supports a plurality of data layers, for example, suitable for elevation difference, color, surface normal, occupancy rate. The underlying data storage is implemented as a two-dimensional circular buffer. Circular buffer may be implemented to achieve a map and computationally efficient lossless displacement. For example, when the robot moves in this application in the environment map continually repositioned very important (for example, the robot-centered map [1]). Our software to simplify the processing of map data by providing some help function. For example, a rectangular, circular and polygonal area iterator functions can be implemented easily map memory sub-regions and secure access. All raster map data to Eigen [2] (a popular linear algebra C ++ libraries) data types are stored. The user may be an intrinsic algorithm may be applied directly to the map data, which provides a versatile and effective tool for data processing.
costmap 2d [3,4] is a popular package ROS package, it can also be used mesh-based representation of the map. It is part of the navigation stack 2D [5], a two-dimensional robot navigation. Its function is to deal with the range of measurement and the establishment of occupancy grid environment. Occupancy rate is usually expressed as "occupied", "free" and "unknown" and other states. Internally, the cost FIG stored as an unsigned character array, which is an integer value in the range of 0-255. Although FIG sufficient processing costs, but the more general data format of the application may be limited. To overcome these drawbacks, the present work describes the grid library map is stored as a matrix of type float. When the type of a physical process such as height, variance, surface normal vector and the like, which may provide greater accuracy and flexibility. To ensure compatibility with existing ROS ecosystem grid library provides a trellis diagram is converted to OccupancyGrid converter (for navigation 2D) stack), GridCells PointCloud2 and message types. And the map data is converted to publish different message types may be visualized using a conventional RViz plug. Another related packages are OctoMap library [6] and its associated ROS interface [7]. OctoMap map represented as a three-dimensional structure has occupied and free voxels. OctoMap map octree structure, dynamic expansion, and may contain areas with different resolutions. 2.5D compared with the mesh representation, the data structure is very suitable OctoMap represent a complete three-dimensional structure. When using a multiple floors to expand map cantilevered structure or arm robot motion planning task, which is often useful. However, since it is necessary to perform a search tree node, and therefore access to data octree require additional computational cost [6]. Instead, after the processing, and data interpretation step in the trellis diagram showing values allows direct access and simplify data management.
Grid Gallery has to serve as a basic framework for multiple applications. In [1], the establishment of programs to FIG elevation legged moving robot through rough terrain (see FIG. 1a). Distance measurements and pose estimation values acquired from onboard robot Kinect depth sensor fusion probability environment representation. Mapper was developed from robot-centric perspective to explicitly posture when the robot moves drift. In [8], the grid library has been used for micro-aircraft (the MAV) autoland operation (see Figure 1b). FIG elevation depth data generated according to the estimated vehicle inertial measurement unit (IMU) and the monocular camera. The map is then used to automatically find safe landing point of the vehicle.
Figure 1. Grid Gallery has been applied to a variety of mapping tasks. a) From the elevation map created onboard Kinect depth sensor allows quadruped robot StarlETH [9] in rugged terrain [1] to navigate. Color represents the height estimation uncertainty, high uncertainty red, blue represents the more uncertainty. b) to achieve independent multi helicopter landing site by finding safe landing [8]. The map is created based on depth estimation onboard IMU and monocular camera. Unsafe landing zone, the security of the region marked in blue with a red mark, landing the selected point marked in green.
In this chapter, we discuss the steps needed for a variety of applications to implement our software. In the remainder of this chapter, we cover the following topics:
First, we demonstrate how to download a grid map gallery and an overview of its components. Based on a simple example of a node, we introduce the basic steps required to integrate the library.
- Second, we describe the main functions of the library. We focus on the main concepts and demonstrates their use through code examples tutorial nodes.
- Third, we discuss the application software with an elevation map of [1] described use cases.
Overview
2.1 Prerequisites and Installation
In the following, we assume that you can run Ubuntu 14.04 LTS (Trusty Tahr) and ROS Indigo normal. [10] gives ROS Indigo is installed on Ubuntu installation instructions. Although we will introduce these versions of the process, but Grid Map packages have also been tested for ROS Jade, and can not be used in future versions together under any circumstances changes. In addition, we have set up a hypothesis catkins workspaces in [11] is described. In addition to the standard installation package ROS (cmake -modules, roscpp, sensor msgs, nav_msgs etc.), the grid library depends only on linear algebra library Eigen [2]. If your system is not yet available, use the following command to install Eigen:
sudo apt-get install libeigen3-dev
To use the grid library, please use the following command to associate the package was cloned into the catkin Workspace / src folder:
git clone [email protected]:ethz-asl/grid_map.git
To complete the installation by building your catkin workspace:
catkin_make
To maximize performance, be sure to build in release mode. You can specify the type constructed by setting:
catkinmake -DCMAKEBUILDTYPE=Release
If desired, you can build and run unit tests using the following command:
catkin_make run_tests_grid_map_core run_tests_grid_map
Please note that our library uses C ++ 11 features, such as a list of initialization and range-based for loops. Therefore, CMake flag -std = c ++ 11 CMakeLists.txt be added to the file.
2.2 software components
grid map library includes the following components:
- grid_map_core
achieve core algorithm grid map library. It provides GridMap classes and several helper classes. The program implements the package is not dependent ROS. - grid_map
is the main software packages using the grid library of ROS-dependent projects. It provides an object to convert the grid map into a plurality of ROS interface
message type. - grid_map_msgs
trellis diagram comprising GridMap of ROS message service message and the message type defined. - grid_map_visualization
comprises a node that GridMap message by converting a standard ROS message type of visualization RViz. Visualization types and parameters may be configured by the user through ROS entirely parameters. - grid_map_demos
contains several nodes for demonstration purposes. A simple demonstration of the node is a brief example of how to use the grid gallery. Tutorial demonstrates node provides an extended demonstration of library functions. Finally, the iterator and the image presentation to the grid figure shows a trellis diagram shows each node iterator usage, and an image converter to the grid of FIG. - grid_map_filters
based ROS filter library [12], based on a series of filters used to implement the raster map data. Filter provides a standardized API, a series of filters according to the parameters defined in the runtime. When writing software to process the trellis diagram can be configured as a series of filters, which provides great flexibility.
A simple example 2.3
Here, we describe a simple example of how to use the grid library, use this code to verify that you have installed the grid map pack and start using their own libraries. Grid_map_demos find the file with the following content / 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;
}
In this program, we initialize a ROS node, create a grid map that node, add data and publish it. The code consists of several blocks of code, we will be partially described.
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);
This section uses a simple demonstration of the grid map names (line 11) to initialize the node, and the private node handle is provided (at line 12). Creating a type grid_map_msgs :: GridMap publisher, the publisher released (Line 1) in the subject grid_map.
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));
We create a type of variable grid_map :: GridMap maps (we use the namespace on line 6 grid_map set). Grid map can contain multiple map layers.
Here, a grid map by a layer called the elevation of layers composed.
Specifies the frame ID, and the map size is set to 1.2 × 2.0 m (x side length along the y-axis and the map of the frame), a resolution of 0.03 m / cell. Alternatively, you can use setGeometry (...) method of setting the third parameter to map the location (the center position). Printout generated display information about:
[INFO ][..]: Created map with size 1.200000 x 2.010000 m (40 x 67 cells）
Note that, the map request side length 2.0 m was changed to 2.01 m, which is done automatically, to ensure consistency, so long as the resolution map multiple (0.03 m / cell).
23 // Work with grid map in a loop.
24 ros::Rate rate(30.0);
25 while (nh.ok()) {
After setting the node and grid map, we will add to the data published it and FIG. 30 Hz of cycles.
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 }
Our goal is to add data to a map layer of elevation, where each element depends on the image height pixel location and the current time. Iterative trellis diagram GridMapIterator allows all cells (line 29). * Use the iterator operator can retrieve the current cell index. It is used by means of a trellis diagram of the getPosition (...) method (line 31) to determine the position of each cell. Current time (in seconds) is stored in the variable time. Application of temporary variables and time position is calculated and the height of the cell (line 32) currently stored in the layer height.
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()
We update the map of a time stamp, and then use GridMapRosConverter class grid map object (of type grid_map :: GridMap) is converted into ROS grid map message (of type grid_map_msgs :: GridMap, line 38). Finally, with the help of publisher previously defined (line 39) will messagis broadcast to the ROS.
rosrun grid_map_demos simple_demo
This will run a simple demonstration of the node grid map generated and published under the theme grid_map_simple_demo / grid_map. In the next step, we step of displaying visual grid map data. FIG grid visualization package offers a simple tool to visualize ROS grid
map information in various forms in RViz. It grid map by converting the message to the message format (e.g. PointCloud2, OccupancyGrid, Marker, etc.) using existing RViz plug. We create a startup file with the following content under 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>
This simple demonstration starts (line 3), grid map visualization (line 5) and RViz (line 9). FIG mesh node loads parameters from the visualization grid_map_demos / config / simple_demo.yaml by the following configuration:
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
This parameter through grid_map_topic (line 1) will be connected to the grid map visualization simple demonstration node, and adds PointCloud2 (row 3) and for the elevation layer OccupancyGrid visualization (line 7). Run the startup file using the following command:
roslaunch grid_map_demos simple_demo.launch
If everything is set up properly, you will see that in a trellis diagram generated in RVizas, as shown in FIG.
figure 2. Start simple_demo.launch file will run simple demonstration and grid map visualization node and open RViz. The generated mesh point cloud visualized as FIG.
3 Kit Description
This section outlines the grid map library functions. We describe the API, and sample code tuial file located grid_map_demos / src /tutorial_demo_node.cpp in the demonstration of its usage. This tutorial elevation layers by deterioration and noise and with an outer layer (the hole map) to extend a simple presentation. Application of the step of averaging filter to reconstruct the original data, and analyzing the residual error. You can run the demo includes the tutorial demo:
roslaunch grid_map_demos tutorial_demo.launch
FIG. 3. FIG mesh grid library using a multilayer store data of different types of information.
3.1 add, access, and remove layers
在使用移动机器人地图时，我们经常计算其他信息来解释数据并指导导航算法。 网格地图库使用图层来存储不同类型数据的信息。 图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节）。
5 总结和结论
In this chapter, we introduce a grid map library, the library is the rendering application developed using a mobile robot. Based on a simple example, will be introduced to integrate with the existing or a new frame of the first step. We've highlighted some of the features of the software, code examples and tutorials by examples illustrate their use. Finally, the application shows a grid map in the Gallery at elevation, which discusses the background, purpose and results.
In our current work, we in such multi-robot mapping, quality assessment standpoint, collision checking exercise program as well as terrain properties predicted by the color information and other applications using the grid map library (used in combination with altitude mapping package) . We want our software useful for other researchers engaged in the use of mobile robots terrain terrain rendering and navigation.
