12機器人導航(模擬)

阿新 • • 發佈：2022-04-12

機器人導航(模擬)

導航是機器人系統中最重要的模組之一，比如現在較為流行的服務型室內機器人，就是依賴於機器人導航來實現室內自主移動的，本章主要就是介紹模擬環境下的導航實現，主要內容有:

導航相關概念
導航實現:機器人建圖(SLAM)、地圖服務、定位、路徑規劃....以視覺化操作為主。
導航訊息:瞭解地圖、里程計、雷達、攝像頭等相關訊息格式。

預期達成的學習目標:

瞭解導航模組中的組成部分以及相關概念
能夠在模擬環境下獨立完成機器人導航

案例演示:

SLAM建圖

概述

概念

在ROS中機器人導航(Navigation)由多個功能包組合實現，ROS 中又稱之為導航功能包集，關於導航模組，官方介紹如下:

一個二維導航堆疊，它接收來自里程計、感測器流和目標姿態的資訊，並輸出傳送到移動底盤的安全速度命令。

更通俗的講: 導航其實就是機器人自主的從 A 點移動到 B 點的過程。

作用

秉著"不重複發明輪子"的原則，ROS 中導航相關的功能包集為機器人導航提供了一套通用的實現，開發者不再需要關注於導航演算法、硬體互動... 等偏複雜、偏底層的實現，這些實現都由更專業的研發人員管理、迭代和維護，開發者可以更專注於上層功能，而對於導航功能的呼叫，只需要根據自身機器人相關引數合理設定各模組的配置檔案即可，當然，如果有必要，也可以基於現有的功能包二次開發實現一些定製化需求，這樣可以大大提高研發效率，縮短產品落地時間。總而言之，對於一般開發者而言，ROS 的導航功能包集優勢如下:

安全: 由專業團隊開發和維護
功能: 功能更穩定且全面
高效: 解放開發者，讓開發者更專注於上層功能實現

另請參考:

http://wiki.ros.org/navigation

導航模組簡介

機器人是如何實現導航的呢？或換言之，機器人是如何從 A 點移動到 B 點呢？ROS 官方為了提供了一張導航功能包集的圖示,該圖中囊括了 ROS 導航的一些關鍵技術:

假定我們已經以特定方式配置機器人，導航功能包集將使其可以運動。上圖概述了這種配置方式。白色的部分是必須且已實現的元件，灰色的部分是可選且已實現的元件，藍色的部分是必須為每一個機器人平臺建立的元件。

總結下來，涉及的關鍵技術有如下五點:

全域性地圖
自身定位
路徑規劃
運動控制
環境感知

機器人導航實現與無人駕駛類似，關鍵技術也是由上述五點組成，只是無人駕駛是基於室外的，而我們當前介紹的機器人導航更多是基於室內的。

全域性地圖

在現實生活中，當我們需要實現導航時，可能會首先參考一張全域性性質的地圖，然後根據地圖來確定自身的位置、目的地位置，並且也會根據地圖顯示來規劃一條大致的路線.... 對於機器人導航而言，也是如此，在機器人導航中地圖是一個重要的組成元素，當然如果要使用地圖，首先需要繪製地圖。關於地圖建模技術不斷湧現，這其中有一門稱之為 SLAM 的理論脫穎而出:

SLAM(simultaneous localization and mapping),也稱為CML (Concurrent Mapping and Localization), 即時定位與地圖構建，或併發建圖與定位。SLAM問題可以描述為: 機器人在未知環境中從一個未知位置開始移動,在移動過程中根據位置估計和地圖進行自身定位，同時在自身定位的基礎上建造增量式地圖，以繪製出外部環境的完全地圖。
在 ROS 中，較為常用的 SLAM 實現也比較多，比如: gmapping、hector_slam、cartographer、rgbdslam、ORB_SLAM ....
當然如果要完成 SLAM ，機器人必須要具備感知外界環境的能力，尤其是要具備獲取周圍環境深度資訊的能力。感知的實現需要依賴於感測器，比如: 鐳射雷達、攝像頭、RGB-D攝像頭...
SLAM 可以用於地圖生成，而生成的地圖還需要被儲存以待後續使用，在 ROS 中儲存地圖的功能包是 map_server

另外注意: SLAM 雖然是機器人導航的重要技術之一，但是二者並不等價，確切的講，SLAM 只是實現地圖構建和即時定位。

自身定位

導航伊始和導航過程中，機器人都需要確定當前自身的位置，如果在室外，那麼 GPS 是一個不錯的選擇，而如果室內、隧道、地下或一些特殊的遮蔽 GPS 訊號的區域，由於 GPS 訊號弱化甚至完全不可用，那麼就必須另闢蹊徑了，比如前面的 SLAM 就可以實現自身定位，除此之外，ROS 中還提供了一個用於定位的功能包: amcl

amcl(adaptiveMonteCarloLocalization)自適應的蒙特卡洛定位,是用於2D移動機器人的概率定位系統。它實現了自適應（或KLD取樣）蒙特卡洛定位方法，該方法使用粒子過濾器根據已知地圖跟蹤機器人的姿態。

路徑規劃

導航就是機器人從A點運動至B點的過程，在這一過程中，機器人需要根據目標位置計算全域性運動路線，並且在運動過程中，還需要時時根據出現的一些動態障礙物調整運動路線，直至到達目標點，該過程就稱之為路徑規劃。在 ROS 中提供了 move_base 包來實現路徑規則,該功能包主要由兩大規劃器組成:

全域性路徑規劃(gloable_planner)

根據給定的目標點和全域性地圖實現總體的路徑規劃，使用 Dijkstra 或 A* 演算法進行全域性路徑規劃，計算最優路線，作為全域性路線
本地時時規劃(local_planner)

在實際導航過程中，機器人可能無法按照給定的全域性最優路線執行，比如:機器人在執行中，可能會隨時出現一定的障礙物... 本地規劃的作用就是使用一定演算法(Dynamic Window Approaches) 來實現障礙物的規避，並選取當前最優路徑以儘量符合全域性最優路徑

全域性路徑規劃與本地路徑規劃是相對的，全域性路徑規劃側重於全域性、巨集觀實現，而本地路徑規劃側重與當前、微觀實現。

運動控制

導航功能包集假定它可以通過話題"cmd_vel"釋出geometry_msgs/Twist型別的訊息，這個訊息基於機器人的基座座標系，它傳遞的是運動命令。這意味著必須有一個節點訂閱"cmd_vel"話題，將該話題上的速度命令轉換為電機命令併發送。

環境感知

感知周圍環境資訊，比如: 攝像頭、鐳射雷達、編碼器...，攝像頭、鐳射雷達可以用於感知外界環境的深度資訊，編碼器可以感知電機的轉速資訊，進而可以獲取速度資訊並生成里程計資訊。

在導航功能包集中，環境感知也是一重要模組實現，它為其他模組提供了支援。其他模組諸如: SLAM、amcl、move_base 都需要依賴於環境感知。

導航之座標系

簡介

定位是導航中的重要實現之一，所謂定位，就是參考某個座標系(比如:以機器人的出發點為原點建立座標系)在該座標系中標註機器人。定位原理看似簡單，但是這個這個座標系不是客觀存在的，我們也無法以上帝視角確定機器人的位姿，定位實現需要依賴於機器人自身，機器人需要逆向推導參考系原點並計算座標系相對關係，該過程實現常用方式有兩種:

通過里程計定位:時時收集機器人的速度資訊計算併發布機器人座標系與父級參考系的相對關係。
通過感測器定位:通過感測器收集外界環境資訊通過匹配計算併發布機器人座標系與父級參考系的相對關係。

兩種方式在導航中都會經常使用。

特點

兩種定位方式都有各自的優缺點。

里程計定位:

優點:里程計定位資訊是連續的，沒有離散的跳躍。
缺點:里程計存在累計誤差，不利於長距離或長期定位。

感測器定位:

優點:比里程計定位更精準；
缺點:感測器定位會出現跳變的情況，且感測器定位在標誌物較少的環境下，其定位精度會大打折扣。

兩種定位方式優缺點互補，應用時一般二者結合使用。

座標系變換

上述兩種定位實現中，機器人座標系一般使用機器人模型中的根座標系(base_link 或 base_footprint)，里程計定位時，父級座標系一般稱之為 odom，如果通過感測器定位，父級參考系一般稱之為 map。當二者結合使用時，map 和 odom 都是機器人模型根座標系的父級，這是不符合座標變換中"單繼承"的原則的，所以，一般會將轉換關係設定為: map -> doom -> base_link 或 base_footprint。

另請參考:

https://www.ros.org/reps/rep-0105.html

導航條件說明

導航實現，在硬體和軟體方面是由一定要求的，需要提前準備。

硬體

雖然導航功能包集被設計成儘可能的通用，在使用時仍然有三個主要的硬體限制：

它是為差速驅動的輪式機器人設計的。它假設底盤受到理想的運動命令的控制並可實現預期的結果，命令的格式為：x速度分量，y速度分量，角速度(theta)分量。
它需要在底盤上安裝一個單線鐳射雷達。這個鐳射雷達用於構建地圖和定位。
導航功能包集是為正方形的機器人開發的，所以方形或圓形的機器人將是效能最好的。它也可以工作在任意形狀和大小的機器人上，但是較大的機器人將很難通過狹窄的空間。

軟體

導航功能實現之前，需要搭建一些軟體環境:

毋庸置疑的，必須先要安裝 ROS
當前導航基於模擬環境，先保證上一章的機器人系統模擬可以正常執行

在模擬環境下，機器人可以正常接收 /cmd_vel 訊息，併發布里程計訊息，感測器訊息釋出也正常，也即導航模組中的運動控制和環境感知實現完畢

後續導航實現中，我們主要關注於: 使用 SLAM 繪製地圖、地圖服務、自身定位與路徑規劃。

導航實現

本節內容主要介紹導航的完整性實現，旨在掌握機器人導航的基本流程，該章涉及的主要內容如下:

SLAM建圖(選用較為常見的gmapping)
地圖服務(可以儲存和重現地圖)
機器人定位
路徑規劃
上述流程介紹完畢，還會對功能進一步整合實現探索式的SLAM建圖。

準備工作

請先安裝相關的ROS功能包:

安裝 gmapping 包(用於構建地圖):sudo apt install ros-<ROS版本>-gmapping
安裝地圖服務包(用於儲存與讀取地圖):sudo apt install ros-<ROS版本>-map-server
安裝 navigation 包(用於定位以及路徑規劃):sudo apt install ros-<ROS版本>-navigation

新建功能包，並匯入依賴: gmapping map_server amcl move_base

導航實現01_SLAM建圖

SLAM演算法有多種，當前我們選用gmapping，後續會再介紹其他幾種常用的SLAM實現。

gmapping簡介

gmapping 是ROS開源社群中較為常用且比較成熟的SLAM演算法之一，gmapping可以根據移動機器人里程計資料和鐳射雷達資料來繪製二維的柵格地圖，對應的，gmapping對硬體也有一定的要求:

該移動機器人可以釋出里程計訊息
機器人需要釋出雷達訊息(該訊息可以通過水平固定安裝的雷達釋出，或者也可以將深度相機訊息轉換成雷達訊息)

關於里程計與雷達資料，模擬環境中可以正常獲取的，不再贅述，柵格地圖如案例所示。

gmapping 安裝前面也有介紹，命令如下:

sudo apt install ros-<ROS版本>-gmapping

gmapping節點說明

gmapping 功能包中的核心節點是:slam_gmapping。為了方便呼叫，需要先了解該節點訂閱的話題、釋出的話題、服務以及相關引數。

訂閱的Topic

tf (tf/tfMessage)

用於雷達、底盤與里程計之間的座標變換訊息。

scan(sensor_msgs/LaserScan)

SLAM所需的雷達資訊。

釋出的Topic

map_metadata(nav_msgs/MapMetaData)

地圖元資料，包括地圖的寬度、高度、解析度等，該訊息會固定更新。

map(nav_msgs/OccupancyGrid)

地圖柵格資料，一般會在rviz中以圖形化的方式顯示。

~entropy(std_msgs/Float64)

機器人姿態分佈熵估計(值越大，不確定性越大)。

服務

dynamic_map(nav_msgs/GetMap)

用於獲取地圖資料。

引數

~base_frame(string, default:"base_link")

機器人基座標系。

~map_frame(string, default:"map")

地圖座標系。

~odom_frame(string, default:"odom")

里程計座標系。

~map_update_interval(float, default: 5.0)

地圖更新頻率，根據指定的值設計更新間隔。

~maxUrange(float, default: 80.0)

鐳射探測的最大可用範圍(超出此閾值，被截斷)。

~maxRange(float)

鐳射探測的最大範圍。

.... 引數較多，上述是幾個較為常用的引數，其他引數介紹可參考官網。

所需的座標變換

雷達座標系→基座標系

一般由 robot_state_publisher 或 static_transform_publisher 釋出。

基座標系→里程計座標系

一般由里程計節點發布。

釋出的座標變換

地圖座標系→里程計座標系

地圖到里程計座標系之間的變換。

gmapping使用

3.1編寫gmapping節點相關launch檔案

launch檔案編寫可以參考 github 的演示 launch檔案：https://github.com/ros-perception/slam_gmapping/blob/melodic-devel/gmapping/launch/slam_gmapping_pr2.launch

複製並修改如下:

<launch>

    <!-- 模擬環境下，將引數設定為true -->
    <param name="use_sim_time" value="true"/>
    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
        <!-- 設定雷達話題 -->
        <remap from="scan" to="scan"/>
        <!-- 關鍵引數：座標系 -->
        <param name="base_frame" value="base_footprint"/><!--底盤座標系-->
        <param name="odom_frame" value="odom"/> <!--里程計座標系-->
        <param name="map_frame" value="map"/> <!--地圖座標系-->
        <param name="map_update_interval" value="5.0"/>
        <param name="maxUrange" value="16.0"/>
        <param name="sigma" value="0.05"/>
        <param name="kernelSize" value="1"/>
        <param name="lstep" value="0.05"/>
        <param name="astep" value="0.05"/>
        <param name="iterations" value="5"/>
        <param name="lsigma" value="0.075"/>
        <param name="ogain" value="3.0"/>
        <param name="lskip" value="0"/>
        <param name="srr" value="0.1"/>
        <param name="srt" value="0.2"/>
        <param name="str" value="0.1"/>
        <param name="stt" value="0.2"/>
        <param name="linearUpdate" value="1.0"/>
        <param name="angularUpdate" value="0.5"/>
        <param name="temporalUpdate" value="3.0"/>
        <param name="resampleThreshold" value="0.5"/>
        <param name="particles" value="30"/>
        <param name="xmin" value="-50.0"/>
        <param name="ymin" value="-50.0"/>
        <param name="xmax" value="50.0"/>
        <param name="ymax" value="50.0"/>
        <param name="delta" value="0.05"/>
        <param name="llsamplerange" value="0.01"/>
        <param name="llsamplestep" value="0.01"/>
        <param name="lasamplerange" value="0.005"/>
        <param name="lasamplestep" value="0.005"/>
    </node>
    <node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher" />
    <node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher" />

    <node pkg="rviz" type="rviz" name="rviz" />
    <!-- 可以儲存 rviz 配置並後期直接使用-->
    <!--
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find my_nav_sum)/rviz/gmapping.rviz"/>
    -->
</launch>

關鍵程式碼解釋：

<remap from="scan" to="scan"/><!-- 雷達話題 -->
<param name="base_frame" value="base_footprint"/><!--底盤座標系-->
<param name="odom_frame" value="odom"/> <!--里程計座標系-->

執行

1.先啟動 Gazebo 模擬環境(此過程略)

2.然後再啟動地圖繪製的 launch 檔案:

roslaunch 包名 launch檔名

3.啟動鍵盤鍵盤控制節點，用於控制機器人運動建圖

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

4.在 rviz 中新增元件，顯示柵格地圖

最後，就可以通過鍵盤控制gazebo中的機器人運動，同時，在rviz中可以顯示gmapping釋出的柵格地圖資料了，下一步，還需要將地圖單獨儲存。

另請參考:

http://wiki.ros.org/gmapping

導航實現02_地圖服務

上一節我們已經實現通過gmapping的構建地圖並在rviz中顯示了地圖，不過，上一節中地圖資料是儲存在記憶體中的，當節點關閉時，資料也會被一併釋放，我們需要將柵格地圖序列化到的磁碟以持久化儲存，後期還要通過反序列化讀取磁碟的地圖資料再執行後續操作。在ROS中，地圖資料的序列化與反序列化可以通過 map_server 功能包實現。

map_server簡介

map_server功能包中提供了兩個節點: map_saver 和 map_server，前者用於將柵格地圖儲存到磁碟，後者讀取磁碟的柵格地圖並以服務的方式提供出去。

map_server安裝前面也有介紹，命令如下:

sudo apt install ros-<ROS版本>-map-server

map_server使用之地圖儲存節點(map_saver)

map_saver節點說明

訂閱的topic:

map(nav_msgs/OccupancyGrid)

訂閱此話題用於生成地圖檔案。

地圖儲存launch檔案

地圖儲存的語法比較簡單，編寫一個launch檔案，內容如下:

<launch>
    <arg name="filename" value="$(find nav_demo)/map/nav" />
    <node name="map_save" pkg="map_server" type="map_saver" args="-f $(arg filename)" />
</launch>

其中 mymap 是指地圖的儲存路徑以及儲存的檔名稱。

SLAM建圖完畢後，執行該launch檔案即可。

測試:

首先，參考上一節，依次啟動模擬環境，鍵盤控制節點與SLAM節點；

然後，通過鍵盤控制機器人運動並繪圖；

最後，通過上述地圖儲存方式儲存地圖。

結果：在指定路徑下會生成兩個檔案，xxx.pgm 與 xxx.yaml

儲存結果解釋

xxx.pgm 本質是一張圖片，直接使用圖片檢視程式即可開啟。

xxx.yaml 儲存的是地圖的元資料資訊，用於描述圖片，內容格式如下:

image: /home/zjh/ros_demo/demo05_ws/src/nav_demo/map/nav.pgm
resolution: 0.050000
origin: [-50.000000, -50.000000, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

解釋:

image:被描述的圖片資源路徑，可以是絕對路徑也可以是相對路徑。
resolution: 圖片分片率(單位: m/畫素)。
origin: 地圖中左下畫素的二維姿勢，為（x，y，偏航），偏航為逆時針旋轉（偏航= 0表示無旋轉）。
occupied_thresh: 佔用概率大於此閾值的畫素被視為完全佔用(即障礙物)。
free_thresh: 佔用率小於此閾值的畫素被視為完全空閒(即無障礙，可順利通行)。
negate: 是否應該顛倒白色/黑色自由/佔用的語義。

map_server 中障礙物計算規則:

地圖中的每一個畫素取值在 [0,255] 之間，白色為 255，黑色為 0，該值設為 x；
map_server 會將畫素值作為判斷是否是障礙物的依據，首先計算比例: p = (255 - x) / 255.0，白色為0，黑色為1(negate為true，則p = x / 255.0)；
根據步驟2計算的比例判斷是否是障礙物，如果 p > occupied_thresh 那麼視為障礙物，如果 p < free_thresh 那麼視為無物。

備註:

圖片也可以根據需求編輯。

map_server使用之地圖服務(map_server)

map_server節點說明

釋出的話題

map_metadata（nav_msgs / MapMetaData）

釋出地圖元資料。

map（nav_msgs / OccupancyGrid）

地圖資料。

服務

static_map（nav_msgs / GetMap）

通過此服務獲取地圖。

引數

〜frame_id（字串，預設值：“map”）

地圖座標系。

圖讀取

通過 map_server 的 map_server 節點可以讀取柵格地圖資料，編寫 launch 檔案如下:

<launch>
    <!-- 設定地圖的配置檔案 -->
    <arg name="map" default="nav.yaml" />
    <!-- 執行地圖伺服器，並且載入設定的地圖-->
    <node name="map_server" pkg="map_server" type="map_server" args="$(find nav_demo)/map/$(arg map)"/>
</launch>

其中引數是地圖描述檔案的資源路徑，執行該launch檔案，該節點會發布話題:map(nav_msgs/OccupancyGrid)

地圖顯示

在 rviz 中使用 map 元件可以顯示柵格地圖：

另請參考:

http://wiki.ros.org/map_server

導航實現03_定位

所謂定位就是推算機器人自身在全域性地圖中的位置，當然，SLAM中也包含定位演算法實現，不過SLAM的定位是用於構建全域性地圖的，是屬於導航開始之前的階段，而當前定位是用於導航中，導航中，機器人需要按照設定的路線運動，通過定位可以判斷機器人的實際軌跡是否符合預期。在ROS的導航功能包集navigation中提供了 amcl 功能包，用於實現導航中的機器人定位。

amcl簡介

AMCL(adaptive Monte Carlo Localization) 是用於2D移動機器人的概率定位系統，它實現了自適應（或KLD取樣）蒙特卡洛定位方法，可以根據已有地圖使用粒子濾波器推算機器人位置。

amcl已經被整合到了navigation包，navigation安裝前面也有介紹，命令如下:

sudo apt install ros-<ROS版本>-navigation

amcl節點說明

amcl 功能包中的核心節點是:amcl。為了方便呼叫，需要先了解該節點訂閱的話題、釋出的話題、服務以及相關引數。

訂閱的Topic

scan(sensor_msgs/LaserScan)

鐳射雷達資料。

tf(tf/tfMessage)

座標變換訊息。

initialpose(geometry_msgs/PoseWithCovarianceStamped)

用來初始化粒子濾波器的均值和協方差。

map(nav_msgs/OccupancyGrid)

獲取地圖資料。

釋出的Topic

amcl_pose(geometry_msgs/PoseWithCovarianceStamped)

機器人在地圖中的位姿估計。

particlecloud(geometry_msgs/PoseArray)

位姿估計集合，rviz中可以被 PoseArray 訂閱然後圖形化顯示機器人的位姿估計集合。

tf(tf/tfMessage)

釋出從 odom 到 map 的轉換。

服務

global_localization(std_srvs/Empty)

初始化全域性定位的服務。

request_nomotion_update(std_srvs/Empty)

手動執行更新和釋出更新的粒子的服務。

set_map(nav_msgs/SetMap)

手動設定新地圖和姿態的服務。

呼叫的服務

static_map(nav_msgs/GetMap)

呼叫此服務獲取地圖資料。

引數

~odom_model_type(string, default:"diff")

里程計模型選擇: "diff","omni","diff-corrected","omni-corrected" (diff 差速、omni 全向輪)

~odom_frame_id(string, default:"odom")

里程計座標系。

~base_frame_id(string, default:"base_link")

機器人極座標系。

~global_frame_id(string, default:"map")

地圖座標系。

.... 引數較多，上述是幾個較為常用的引數，其他引數介紹可參考官網。

座標變換

里程計本身也是可以協助機器人定位的，不過里程計存在累計誤差且一些特殊情況時(車輪打滑)會出現定位錯誤的情況，amcl 則可以通過估算機器人在地圖座標系下的姿態，再結合里程計提高定位準確度。

里程計定位:只是通過里程計資料實現 /odom_frame 與 /base_frame 之間的座標變換。
amcl定位: 可以提供 /map_frame 、/odom_frame 與 /base_frame 之間的座標變換。

amcl使用

3.1編寫amcl節點相關的launch檔案

關於launch檔案的實現，在amcl功能包下的example目錄已經給出了示例，可以作為參考，具體實現:

roscd amcl
ls examples

該目錄下會列出兩個檔案: amcl_diff.launch 和 amcl_omni.launch 檔案，前者適用於差分移動機器人，後者適用於全向移動機器人，可以按需選擇，此處參考前者，新建 launch 檔案，複製 amcl_diff.launch 檔案內容並修改如下:

<launch>
    <node pkg="amcl" type="amcl" name="amcl" output="screen">
        <!-- Publish scans from best pose at a max of 10 Hz -->
        <param name="odom_model_type" value="diff"/>
        <param name="odom_alpha5" value="0.1"/>
        <param name="transform_tolerance" value="0.2" />
        <param name="gui_publish_rate" value="10.0"/>
        <param name="laser_max_beams" value="30"/>
        <param name="min_particles" value="500"/>
        <param name="max_particles" value="5000"/>
        <param name="kld_err" value="0.05"/>
        <param name="kld_z" value="0.99"/>
        <param name="odom_alpha1" value="0.2"/>
        <param name="odom_alpha2" value="0.2"/>
        <!-- translation std dev, m -->
        <param name="odom_alpha3" value="0.8"/>
        <param name="odom_alpha4" value="0.2"/>
        <param name="laser_z_hit" value="0.5"/>
        <param name="laser_z_short" value="0.05"/>
        <param name="laser_z_max" value="0.05"/>
        <param name="laser_z_rand" value="0.5"/>
        <param name="laser_sigma_hit" value="0.2"/>
        <param name="laser_lambda_short" value="0.1"/>
        <param name="laser_lambda_short" value="0.1"/>
        <param name="laser_model_type" value="likelihood_field"/>
        <!-- <param name="laser_model_type" value="beam"/> -->
        <param name="laser_likelihood_max_dist" value="2.0"/>
        <param name="update_min_d" value="0.2"/>
        <param name="update_min_a" value="0.5"/>
        
        <param name="odom_frame_id" value="odom"/><!-- 里程計座標系 -->
        <param name="base_frame_id" value="base_footprint"/><!-- 新增機器人基座標系 -->
        <param name="global_frame_id" value="map"/><!-- 新增地圖座標系 -->

        <param name="resample_interval" value="1"/>
        <param name="transform_tolerance" value="0.1"/>
        <param name="recovery_alpha_slow" value="0.0"/>
        <param name="recovery_alpha_fast" value="0.0"/>
    </node>
</launch>

編寫測試launch檔案

amcl節點是不可以單獨執行的，執行 amcl 節點之前，需要先載入全域性地圖，然後啟動 rviz 顯示定位結果，上述節點可以整合進launch檔案，內容示例如下:

<!-- 測試檔案 -->
<launch>
    <!-- 啟動rviz -->
    <node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher" />
    <node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher" />

    <node pkg="rviz" type="rviz" name="rviz" />

    <!-- 載入地圖服務 -->
    <include file="$(find nav_demo)/launch/nav03_map_server.launch" />

    <!-- amcl檔案 -->
    <include file="$(find nav_demo)/launch/nav04_amcl.launch" />


</launch>

當然，launch檔案中地圖服務節點和amcl節點中的包名、檔名需要根據自己的設定修改。

執行

1.先啟動 Gazebo 模擬環境(此過程略)；

2.啟動鍵盤控制節點：

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

3.啟動上一步中整合地圖服務、amcl 與 rviz 的 launch 檔案；

4.在啟動的 rviz 中，新增RobotModel、Map元件，分別顯示機器人模型與地圖，新增 posearray 外掛，設定topic為particlecloud來顯示 amcl 預估的當前機器人的位姿，箭頭越是密集，說明當前機器人處於此位置的概率越高；

5.通過鍵盤控制機器人運動，會發現 posearray 也隨之而改變。

導航實現04_路徑規劃

毋庸置疑的，路徑規劃是導航中的核心功能之一，在ROS的導航功能包集navigation中提供了 move_base 功能包，用於實現此功能。

move_base簡介

move_base 功能包提供了基於動作(action)的路徑規劃實現，move_base 可以根據給定的目標點，控制機器人底盤運動至目標位置，並且在運動過程中會連續反饋機器人自身的姿態與目標點的狀態資訊。如前所述(7.1)move_base主要由全域性路徑規劃與本地路徑規劃組成。

move_base已經被整合到了navigation包，navigation安裝前面也有介紹，命令如下:

sudo apt install ros-<ROS版本>-navigation

move_base節點說明

move_base功能包中的核心節點是:move_base。為了方便呼叫，需要先了解該節點action、訂閱的話題、釋出的話題、服務以及相關引數。

動作

動作訂閱

move_base/goal(move_base_msgs/MoveBaseActionGoal)

move_base 的運動規劃目標。

move_base/cancel(actionlib_msgs/GoalID)

取消目標。

動作釋出

move_base/feedback(move_base_msgs/MoveBaseActionFeedback)

連續反饋的資訊，包含機器人底盤座標。

move_base/status(actionlib_msgs/GoalStatusArray)

傳送到move_base的目標狀態資訊。

move_base/result(move_base_msgs/MoveBaseActionResult)

操作結果(此處為空)。

訂閱的Topic

move_base_simple/goal(geometry_msgs/PoseStamped)

運動規劃目標(與action相比，沒有連續反饋，無法追蹤機器人執行狀態)。

釋出的Topic

cmd_vel(geometry_msgs/Twist)

輸出到機器人底盤的運動控制訊息。

服務

~make_plan(nav_msgs/GetPlan)

請求該服務，可以獲取給定目標的規劃路徑，但是並不執行該路徑規劃。

~clear_unknown_space(std_srvs/Empty)

允許使用者直接清除機器人周圍的未知空間。

~clear_costmaps(std_srvs/Empty)

允許清除代價地圖中的障礙物，可能會導致機器人與障礙物碰撞，請慎用。

引數

請參考官網。

move_base與代價地圖

概念

機器人導航(尤其是路徑規劃模組)是依賴於地圖的，地圖在SLAM時已經有所介紹了，ROS中的地圖其實就是一張圖片，這張圖片有寬度、高度、解析度等元資料，在圖片中使用灰度值來表示障礙物存在的概率。不過SLAM構建的地圖在導航中是不可以直接使用的，因為：

SLAM構建的地圖是靜態地圖，而導航過程中，障礙物資訊是可變的，可能障礙物被移走了，也可能添加了新的障礙物，導航中需要時時的獲取障礙物資訊；
在靠近障礙物邊緣時，雖然此處是空閒區域，但是機器人在進入該區域後可能由於其他一些因素，比如：慣性、或者不規則形體的機器人轉彎時可能會與障礙物產生碰撞，安全起見，最好在地圖的障礙物邊緣設定警戒區，儘量禁止機器人進入...

所以，靜態地圖無法直接應用於導航，其基礎之上需要新增一些輔助資訊的地圖，比如時時獲取的障礙物資料，基於靜態地圖新增的膨脹區等資料。

組成

代價地圖有兩張:global_costmap(全域性代價地圖) 和 local_costmap(本地代價地圖)，前者用於全域性路徑規劃，後者用於本地路徑規劃。

兩張代價地圖都可以多層疊加,一般有以下層級:

Static Map Layer：靜態地圖層，SLAM構建的靜態地圖。
Obstacle Map Layer：障礙地圖層，感測器感知的障礙物資訊。
Inflation Layer：膨脹層，在以上兩層地圖上進行膨脹（向外擴張），以避免機器人的外殼會撞上障礙物。
Other Layers：自定義costmap。

多個layer可以按需自由搭配。

碰撞演算法

在ROS中，如何計算代價值呢？請看下圖:

上圖中，橫軸是距離機器人中心的距離，縱軸是代價地圖中柵格的灰度值。

致命障礙:柵格值為254，此時障礙物與機器人中心重疊，必然發生碰撞；
內切障礙:柵格值為253，此時障礙物處於機器人的內切圓內，必然發生碰撞；
外切障礙:柵格值為[128,252]，此時障礙物處於其機器人的外切圓內，處於碰撞臨界，不一定發生碰撞；
非自由空間:柵格值為(0,127]，此時機器人處於障礙物附近，屬於危險警戒區，進入此區域，將來可能會發生碰撞；
自由區域:柵格值為0，此處機器人可以自由通過；
未知區域:柵格值為255，還沒探明是否有障礙物。

膨脹空間的設定可以參考非自由空間。

move_base使用

路徑規劃演算法在move_base功能包的move_base節點中已經封裝完畢了，但是還不可以直接呼叫，因為演算法雖然已經封裝了，但是該功能包面向的是各種型別支援ROS的機器人，不同型別機器人可能大小尺寸不同，感測器不同，速度不同，應用場景不同....最後可能會導致不同的路徑規劃結果，那麼在呼叫路徑規劃節點之前，我們還需要配置機器人蔘數。具體實現如下:

先編寫launch檔案模板
編寫配置檔案
整合導航相關的launch檔案
測試

launch檔案

關於move_base節點的呼叫，模板如下:

<launch>

    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
        <rosparam file="$(find 功能包)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
        <rosparam file="$(find 功能包)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
        <rosparam file="$(find 功能包)/param/local_costmap_params.yaml" command="load" />
        <rosparam file="$(find 功能包)/param/global_costmap_params.yaml" command="load" />
        <rosparam file="$(find 功能包)/param/base_local_planner_params.yaml" command="load" />
    </node>

</launch>

launch檔案解釋:

啟動了 move_base 功能包下的 move_base 節點，respawn 為 false，意味著該節點關閉後，不會被重啟；clear_params 為 true，意味著每次啟動該節點都要清空私有引數然後重新載入；通過 rosparam 會載入若干 yaml 檔案用於配置引數，這些yaml檔案的配置以及作用詳見下一小節內容。

配置檔案

關於配置檔案的編寫，可以參考一些成熟的機器人的路徑規劃實現，比如: turtlebot3，github連結：https://github.com/ROBOTIS-GIT/turtlebot3/tree/master/turtlebot3_navigation/param，先下載這些配置檔案備用。

在功能包下新建 param 目錄，複製下載的檔案到此目錄: costmap_common_params_burger.yaml、local_costmap_params.yaml、global_costmap_params.yaml、base_local_planner_params.yaml，並將costmap_common_params_burger.yaml 重新命名為:costmap_common_params.yaml。

配置檔案修改以及解釋:

costmap_common_params.yaml

該檔案是move_base 在全域性路徑規劃與本地路徑規劃時呼叫的通用引數，包括:機器人的尺寸、距離障礙物的安全距離、感測器資訊等。配置參考如下:

#機器人幾何參，如果機器人是圓形，設定 robot_radius,如果是其他形狀設定 footprint
robot_radius: 0.12 #圓形
# footprint: [[-0.12, -0.12], [-0.12, 0.12], [0.12, 0.12], [0.12, -0.12]] #其他形狀

obstacle_range: 3.0 # 用於障礙物探測，比如: 值為 3.0，意味著檢測到距離小於 3 米的障礙物時，就會引入代價地圖
raytrace_range: 3.5 # 用於清除障礙物，比如：值為 3.5，意味著清除代價地圖中 3.5 米以外的障礙物


#膨脹半徑，擴充套件在碰撞區域以外的代價區域，使得機器人規劃路徑避開障礙物
inflation_radius: 0.2
#代價比例係數，越大則代價值越小
cost_scaling_factor: 3.0

#地圖型別
map_type: costmap
#導航包所需要的感測器
observation_sources: scan
#對感測器的座標系和資料進行配置。這個也會用於代價地圖新增和清除障礙物。例如，你可以用鐳射雷達感測器用於在代價地圖新增障礙物，再新增kinect用於導航和清除障礙物。
scan: {sensor_frame: laser, data_type: LaserScan, topic: scan, marking: true, clearing: true}

global_costmap_params.yaml

該檔案用於全域性代價地圖引數設定:

global_costmap:
  global_frame: map #地圖座標系
  robot_base_frame: base_footprint #機器人座標系
  # 以此實現座標變換

  update_frequency: 1.0 #代價地圖更新頻率
  publish_frequency: 1.0 #代價地圖的釋出頻率
  transform_tolerance: 0.5 #等待座標變換髮布資訊的超時時間

  static_map: true # 是否使用一個地圖或者地圖伺服器來初始化全域性代價地圖，如果不使用靜態地圖，這個引數為false.

local_costmap_params.yaml

該檔案用於區域性代價地圖引數設定:

local_costmap:
  global_frame: odom #里程計座標系
  robot_base_frame: base_footprint #機器人座標系

  update_frequency: 10.0 #代價地圖更新頻率
  publish_frequency: 10.0 #代價地圖的釋出頻率
  transform_tolerance: 0.5 #等待座標變換髮布資訊的超時時間

  static_map: false  #不需要靜態地圖，可以提升導航效果
  rolling_window: true #是否使用動態視窗，預設為false，在靜態的全域性地圖中，地圖不會變化
  width: 3 # 區域性地圖寬度 單位是 m
  height: 3 # 區域性地圖高度 單位是 m
  resolution: 0.05 # 區域性地圖解析度 單位是 m，一般與靜態地圖解析度保持一致

base_local_planner_params

基本的區域性規劃器引數配置，這個配置檔案設定了機器人的最大和最小速度限制值，也設定了加速度的閾值。

TrajectoryPlannerROS:

# Robot Configuration Parameters
  max_vel_x: 0.5 # X 方向最大速度
  min_vel_x: 0.1 # X 方向最小速速

  max_vel_theta:  1.0 # 
  min_vel_theta: -1.0
  min_in_place_vel_theta: 1.0

  acc_lim_x: 1.0 # X 加速限制
  acc_lim_y: 0.0 # Y 加速限制
  acc_lim_theta: 0.6 # 角速度加速限制

# Goal Tolerance Parameters，目標公差
  xy_goal_tolerance: 0.10
  yaw_goal_tolerance: 0.05

# Differential-drive robot configuration
# 是否是全向移動機器人
  holonomic_robot: false

# Forward Simulation Parameters，前進模擬引數
  sim_time: 0.8
  vx_samples: 18
  vtheta_samples: 20
  sim_granularity: 0.05

引數配置技巧

以上配置在實操中，可能會出現機器人在本地路徑規劃時與全域性路徑規劃不符而進入膨脹區域出現假死的情況，如何儘量避免這種情形呢？

全域性路徑規劃與本地路徑規劃雖然設定的引數是一樣的，但是二者路徑規劃和避障的職能不同，可以採用不同的引數設定策略:

全域性代價地圖可以將膨脹半徑和障礙物係數設定的偏大一些；

本地代價地圖可以將膨脹半徑和障礙物係數設定的偏小一些。

這樣，在全域性路徑規劃時，規劃的路徑會盡量遠離障礙物，而本地路徑規劃時，機器人即便偏離全域性路徑也會和障礙物之間保留更大的自由空間，從而避免了陷入“假死”的情形。

launch檔案整合

如果要實現導航，需要整合地圖服務、amcl 、move_base 與 Rviz 等，整合示例如下:

<launch>
    <!-- 地圖服務 -->
    <include file="$(find nav_demo)/launch/nav03_map_server.launch" />
    <!-- amcl -->
    <include file="$(find nav_demo)/launch/nav04_amcl.launch" />
    <!-- move_base -->
    <include file="$(find nav_demo)/launch/nav05_path.launch" />
    <!-- rviz -->
        <node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher" />
    <node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher" />

    <node pkg="rviz" type="rviz" name="rviz" />
</launch>

測試

1.先啟動 Gazebo 模擬環境(此過程略)；

2.啟動導航相關的 launch 檔案；

3.新增Rviz元件(參考演示結果),可以將配置資料儲存，後期直接呼叫；

全域性代價地圖與本地代價地圖元件配置如下:

全域性路徑規劃與本地路徑規劃元件配置如下:

4.通過Rviz工具欄的 2D Nav Goal設定目的地實現導航。

5.也可以在導航過程中，新增新的障礙物，機器人也可以自動躲避障礙物。

另請參考:

http://wiki.ros.org/move_base

導航與SLAM建圖

場景:在 7.2.1 導航實現01_SLAM建圖中，我們是通過鍵盤控制機器人移動實現建圖的，而後續又介紹了機器人的自主移動實現，那麼可不可以將二者結合，實現機器人自主移動的SLAM建圖呢？

上述需求是可行的。雖然可能會有疑問，導航時需要地圖資訊，之前導航實現時，是通過 map_server 包的 map_server 節點來發布地圖資訊的，如果不先通過SLAM建圖，那麼如何釋出地圖資訊呢？SLAM建圖過程中本身就會時時釋出地圖資訊，所以無需再使用map_server，SLAM已經發布了話題為 /map 的地圖訊息了，且導航需要定位模組，SLAM本身也是可以實現定位的。

該過程實現比較簡單，步驟如下:

編寫launch檔案，整合SLAM與move_base相關節點；
執行launch檔案並測試。

編寫launc檔案

當前launch檔案實現，無需呼叫map_server的相關節點，只需要啟動SLAM節點與move_base節點，示例內容如下:

<launch>
    <!-- 啟動SLAM節點 -->
    <!-- nav01_slam.launch已經集成了rviz -->
    <include file="$(find nav_demo)/launch/nav01_slam.launch" />
    <!-- 執行move_base節點 -->
    <include file="$(find nav_demo)/launch/nav05_path.launch" />
 
</launch>

測試

1.首先執行gazebo模擬環境；

2.然後執行launch檔案；

3.在rviz中通過2D Nav Goal設定目標點，機器人開始自主移動並建圖了；

4.最後可以使用 map_server 儲存地圖。

導航相關訊息

在導航功能包集中包含了諸多節點，毋庸置疑的，不同節點之間的通訊使用到了訊息中介軟體(資料載體)，在上一節的實現中，這些訊息已經在rviz中做了視覺化處理，比如:地圖、雷達、攝像頭、里程計、路徑規劃...的相關訊息在rviz中提供了相關元件，本節主要介紹這些訊息的具體格式。

導航之地圖

地圖相關的訊息主要有兩個:

nav_msgs/MapMetaData

地圖元資料，包括地圖的寬度、高度、解析度等。

nav_msgs/OccupancyGrid

地圖柵格資料，一般會在rviz中以圖形化的方式顯示。

nav_msgs/MapMetaData

呼叫rosmsg info nav_msgs/MapMetaData顯示訊息內容如下:

time map_load_time
float32 resolution #地圖解析度
uint32 width #地圖寬度
uint32 height #地圖高度
geometry_msgs/Pose origin #地圖位姿資料
  geometry_msgs/Point position
    float64 x
    float64 y
    float64 z
  geometry_msgs/Quaternion orientation
    float64 x
    float64 y
    float64 z
    float64 w

nav_msgs/OccupancyGrid

呼叫 rosmsg info nav_msgs/OccupancyGrid顯示訊息內容如下:

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
#--- 地圖元資料
nav_msgs/MapMetaData info
  time map_load_time
  float32 resolution
  uint32 width
  uint32 height
  geometry_msgs/Pose origin
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w
#--- 地圖內容資料，陣列長度 = width * height
int8[] data

導航之里程計

里程計相關訊息是:nav_msgs/Odometry，呼叫rosmsg info nav_msgs/Odometry 顯示訊息內容如下:

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
string child_frame_id
geometry_msgs/PoseWithCovariance pose
  geometry_msgs/Pose pose #里程計位姿
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w
  float64[36] covariance
geometry_msgs/TwistWithCovariance twist
  geometry_msgs/Twist twist #速度
    geometry_msgs/Vector3 linear
      float64 x
      float64 y
      float64 z
    geometry_msgs/Vector3 angular
      float64 x
      float64 y
      float64 z    
  # 協方差矩陣
  float64[36] covariance

導航之座標變換

座標變換相關訊息是: tf/tfMessage，呼叫rosmsg info tf/tfMessage 顯示訊息內容如下:

geometry_msgs/TransformStamped[] transforms #包含了多個座標系相對關係資料的陣列
  std_msgs/Header header
    uint32 seq
    time stamp
    string frame_id
  string child_frame_id
  geometry_msgs/Transform transform
    geometry_msgs/Vector3 translation
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion rotation
      float64 x
      float64 y
      float64 z
      float64 w

導航之定位

定位相關訊息是:geometry_msgs/PoseArray，呼叫rosmsg info geometry_msgs/PoseArray顯示訊息內容如下:

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/Pose[] poses #預估的點位姿組成的陣列
  geometry_msgs/Point position
    float64 x
    float64 y
    float64 z
  geometry_msgs/Quaternion orientation
    float64 x
    float64 y
    float64 z
    float64 w

導航之目標點與路徑規劃

目標點相關訊息是:move_base_msgs/MoveBaseActionGoal，呼叫rosmsg info move_base_msgs/MoveBaseActionGoal顯示訊息內容如下:

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
actionlib_msgs/GoalID goal_id
  time stamp
  string id
move_base_msgs/MoveBaseGoal goal
  geometry_msgs/PoseStamped target_pose
    std_msgs/Header header
      uint32 seq
      time stamp
      string frame_id
    geometry_msgs/Pose pose #目標點位姿
      geometry_msgs/Point position
        float64 x
        float64 y
        float64 z
      geometry_msgs/Quaternion orientation
        float64 x
        float64 y
        float64 z
        float64 w

路徑規劃相關訊息是:nav_msgs/Path，呼叫rosmsg info nav_msgs/Path顯示訊息內容如下:

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/PoseStamped[] poses #由一系列點組成的陣列
  std_msgs/Header header
    uint32 seq
    time stamp
    string frame_id
  geometry_msgs/Pose pose
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w

導航之鐳射雷達

鐳射雷達相關訊息是:sensor_msgs/LaserScan，呼叫rosmsg info sensor_msgs/LaserScan顯示訊息內容如下:

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
float32 angle_min #起始掃描角度(rad)
float32 angle_max #終止掃描角度(rad)
float32 angle_increment #測量值之間的角距離(rad)
float32 time_increment #測量間隔時間(s)
float32 scan_time #掃描間隔時間(s)
float32 range_min #最小有效距離值(m)
float32 range_max #最大有效距離值(m)
float32[] ranges #一個週期的掃描資料
float32[] intensities #掃描強度資料，如果裝置不支援強度資料，該陣列為空

導航之相機

深度相機相關訊息有:sensor_msgs/Image、sensor_msgs/CompressedImage、sensor_msgs/PointCloud2

sensor_msgs/Image 對應的一般的影象資料，sensor_msgs/CompressedImage 對應壓縮後的影象資料，sensor_msgs/PointCloud2 對應的是點雲資料(帶有深度資訊的影象資料)。

呼叫rosmsg info sensor_msgs/Image顯示訊息內容如下:

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
uint32 height #高度
uint32 width  #寬度
string encoding #編碼格式:RGB、YUV等
uint8 is_bigendian #影象大小端儲存模式
uint32 step #一行影象資料的位元組數，作為步進引數
uint8[] data #影象資料，長度等於 step * height

呼叫rosmsg info sensor_msgs/CompressedImage顯示訊息內容如下:

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
string format #壓縮編碼格式(jpeg、png、bmp)
uint8[] data #壓縮後的資料

呼叫rosmsg info sensor_msgs/PointCloud2顯示訊息內容如下:

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
uint32 height #高度
uint32 width  #寬度
sensor_msgs/PointField[] fields #每個點的資料型別
  uint8 INT8=1
  uint8 UINT8=2
  uint8 INT16=3
  uint8 UINT16=4
  uint8 INT32=5
  uint8 UINT32=6
  uint8 FLOAT32=7
  uint8 FLOAT64=8
  string name
  uint32 offset
  uint8 datatype
  uint32 count
bool is_bigendian #影象大小端儲存模式
uint32 point_step #單點的資料位元組步長
uint32 row_step   #一行資料的位元組步長
uint8[] data      #儲存點雲的陣列，總長度為 row_step * height
bool is_dense     #是否有無效點

深度影象轉鐳射資料

本節介紹ROS中的一個功能包:depthimage_to_laserscan，顧名思義，該功能包可以將深度影象資訊轉換成鐳射雷達資訊，應用場景如下:

在諸多SLAM演算法中，一般都需要訂閱鐳射雷達資料用於構建地圖，因為鐳射雷達可以感知周圍環境的深度資訊，而深度相機也具備感知深度資訊的功能，且最初鐳射雷達價格比價比較昂貴，那麼在感測器選型上可以選用深度相機代替鐳射雷達嗎？

答案是可以的，不過二者釋出的訊息型別是完全不同的，如果想要實現感測器的置換，那麼就需要將深度相機發布的三維的圖形資訊轉換成二維的鐳射雷達資訊，這一功能就是通過depthimage_to_laserscan來實現的。

depthimage_to_laserscan簡介

原理

depthimage_to_laserscan將實現深度影象與雷達資料轉換的原理比較簡單，雷達資料是二維的、平面的，深度影象是三維的，是若干二維(水平)資料的縱向疊加，如果將三維的資料轉換成二維資料，只需要取深度圖的某一層即可，為了方面理解，請看官方示例:

圖一:深度相機與外部環境(實物圖)

圖二:深度相機發布的圖片資訊，圖中彩線對應的是要轉換成雷達資訊的資料

圖三:將圖二以點雲的方式顯示更為直觀，圖中彩線對應的仍然是要轉換成雷達資訊的資料

圖四:轉換之後的結果圖(俯視)

優缺點

優點:深度相機的成本一般低於鐳射雷達，可以降低硬體成本；

缺點: 深度相機較之於鐳射雷達無論是檢測範圍還是精度都有不小的差距，SLAM效果可能不如鐳射雷達理想。

安裝

使用之前請先安裝,命令如下:

sudo apt-get install ros-melodic-depthimage-to-laserscan

epthimage_to_laserscan節點說明

depthimage_to_laserscan 功能包的核心節點是:depthimage_to_laserscan ，為了方便呼叫，需要先了解該節點訂閱的話題、釋出的話題以及相關引數。

訂閱的Topic

image(sensor_msgs/Image)

輸入影象資訊。

camera_info(sensor_msgs/CameraInfo)

關聯影象的相機資訊。通常不需要重新對映，因為camera_info將從與image相同的名稱空間中進行訂閱。

釋出的Topic

scan(sensor_msgs/LaserScan)

釋出轉換成的鐳射雷達型別資料。

引數

該節點引數較少，只有如下幾個，一般需要設定的是: output_frame_id。

~scan_height(int, default: 1 pixel)

設定用於生成鐳射雷達資訊的象素行數。

~scan_time(double, default: 1/30.0Hz (0.033s))

兩次掃描的時間間隔。

~range_min(double, default: 0.45m)

返回的最小範圍。結合range_max使用，只會獲取 range_min 與 range_max 之間的資料。

~range_max(double, default: 10.0m)

返回的最大範圍。結合range_min使用，只會獲取 range_min 與 range_max 之間的資料。

~output_frame_id(str, default: camera_depth_frame)

鐳射資訊的ID。

depthimage_to_laserscan使用

編寫launch檔案

編寫launch檔案執行，將深度資訊轉換成雷達資訊

<launch>
    <node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan">
        <remap from="image" to="/camera/depth/image_raw" />
        <param name="output_frame_id" value="camera"  />
    </node>
</launch>

訂閱的話題需要根據深度相機發布的話題設定，output_frame_id需要與深度相機的座標系一致。

修改URDF檔案

經過資訊轉換之後，深度相機也將釋出雷達資料，為了不產生混淆，可以註釋掉 xacro 檔案中的關於鐳射雷達的部分內容。

執行

1.啟動gazebo模擬環境

2.啟動rviz並新增相關元件(image、LaserScan)，結果如下:

SLAM應用

現在我們已經實現並測試通過深度影象資訊轉換成鐳射雷達資訊了，接下來是實踐階段，通過深度相機實現SLAM，流程如下:

1.先啟動 Gazebo 模擬環境；

2.啟動轉換節點；

3.再啟動地圖繪製的 launch 檔案；

4.啟動鍵盤鍵盤控制節點，用於控制機器人運動建圖；

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

5.在 rviz 中新增元件，顯示柵格地圖最後，就可以通過鍵盤控制gazebo中的機器人運動，同時，在rviz中可以顯示gmapping釋出的柵格地圖資料了，但是，前面也介紹了，由於精度和檢測範圍的原因，尤其再加之環境的特徵點偏少，建圖效果可能並不理想，建圖中甚至會出現地圖偏移的情況。

本章小結

本章介紹了在模擬環境下的機器人導航實現，主要內容如下:

導航概念以及架構設計
SLAM概念以及gmapping實現
地圖的序列化與反序列化
定位實現
路徑規劃實現
導航中涉及的訊息解釋

導航整體設計架構中，包含地圖、定位、路徑規劃、感知以及控制等實現，感知與控制模組在上一章機器人系統模擬中已經實現了，因此沒有做過多介紹，其他部分，當前也是基於模擬環境實現的，後續，我們將搭建一臺實體機器人並實現導航功能。