作者：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 [email protected]: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 總結和結論
在本章中，我們介紹了一個網格地圖庫，該庫是爲使用移動機器人繪製應用程序而開發的。 基於一個簡單的示例，介紹了將庫集成到現有或新框架中的第一步。 我們重點介紹了該軟件的某些功能，並通過教程示例中的代碼示例說明了它們的用法。 最後，展示了柵格地圖庫在高程地圖中的應用，從而討論了背景，用途和結果。
在我們當前的工作中，我們在諸如多機器人映射，立足點質量評估，運動計劃的碰撞檢查以及通過顏色信息進行地形屬性預測等應用程序中使用網格地圖庫（與海拔映射程序包結合使用）。 我們希望我們的軟件對其他從事使用移動機器人進行地形地形繪製和導航的研究人員有用。