SLAM+語音機器人DIY系列：（六）SLAM建圖與自主避障導航——3.ros-navigation機器人自主避障導航

時間 2019-11-10

標籤 slam 語音機器人 diy 系列自主導航 3.ros ros navigation 简体版

原文原文鏈接

摘要

經過前面的基礎學習，本章進入最爲激動的機器人自主導航的學習。在前面的學習鋪墊後，終於迎來了最大樂趣的時刻，就是賦予咱們的miiboo機器人能自由行走的生命。本章將圍繞機器人SLAM建圖、導航避障、巡航、監控等內容展開。本章內容：html

1.在機器人上使用傳感器python

2.google-cartographer機器人SLAM建圖nginx

3.ros-navigation機器人自主避障導航git

4.多目標點導航及任務調度github

5.機器人巡航與現場監控算法

3.ros-navigation機器人自主避障導航

前面的學習教程打好了必須的基礎，如今就正式開始探討ROS系統最強大的特性之一，讓咱們的機器人能自主導航和避障。這都得益於開源社區和共享代碼，使ROS擁有大量可用的導航算法。將這些導航算法集大成者，即是ros-navigation導航功能包集。瞭解更多ros-navigation的信息，請參考官方wiki教程：http://wiki.ros.org/navigation/。django

3.1.機器人自主避障導航原理分析

要分析導航功能包的原理，確定須要先參考下面這張ros-navigation官方給出的系統框圖，如圖30。最中心的是move_base節點，是導航過程運動控制的最終執行機構，move_base訂閱用戶發佈的導航目標move_base_simple/goal，並將實時運動控制信號cmd_vel下發給底盤以實現最終運動控制，move_base中的各類導航算法模塊都是以插件的形式進行調用的，這樣能夠很方便的替換不一樣的算法以適應不一樣的應用，其中global_planner用於全局路徑規劃、local_planner用於局部路徑規劃、golobal_costmap是全局代價地圖用於描述全局環境信息、local_costmap是局部代價地圖用於描述局部環境信息、recovery_behaviors是恢復策略用於機器人碰到障礙後自動進行逃離恢復。而後是amcl節點，amcl節點利用粒子濾波算法實現機器人的全局定位，爲機器人導航提供全局位置信息。再而後是map_server節點，map_server節點經過調用前面SLAM建圖獲得的地圖爲導航提供環境地圖信息。最後就是要提供機器人模型相關的tf信息、里程計odom信息、激光雷達信息scan。ubuntu

（圖30）ros-navigation導航功能包集系統框圖centos

機器人全局定位amcl粒子濾波算法：bash

amcl的全稱是自適應蒙特卡洛粒子濾波，這裏經過講解粒子濾波、重要性採樣、機器人綁架、自適應蒙特卡洛這幾個概念來講明機器人全局定位的原理。因爲amcl的數學理論比較複雜，限於篇幅這裏不展開講解，感興趣的朋友能夠參考《Probabilistic Robotics》這本書，裏面有詳細的推導過程，如圖31。

（圖31）機率機器人

粒子濾波，是一種思想，好比要計算一個矩形裏面一個不規則形狀的面積，這個問題很差直接計算，可是能夠拿一把豆子均勻撒到矩形中，統計落在不規則形狀中豆子的佔比就能算出其面積了。在機器人定位問題中，咱們在地圖的任意位置撒上許多粒子點，而後經過傳感器觀測數據按照必定的評價方法對每一個粒子點進行打分，評分高的粒子點表示機器人有更大的可能在此位置；在下一輪撒點時，就在評分高的粒子點附近多撒一些點，這樣經過不斷的迭代，粒子點就會聚攏到一個地方。這個粒子點彙集的地方，就是機器人位置的最優估計點。如圖32，紅色的粒子點慢慢聚攏到一團。

（圖32）粒子濾波

重要性採樣，在粒子濾波的迭代過程當中，評分高的粒子點會被下一輪迭代時更加看重，這樣不斷迭代真實估計值附近的粒子點會愈來愈多。

機器人綁架，當機器人被忽然從一個地方抱走到另外一個地方，這個時候前一輪迭代獲得的粒子點徹底不能在新的位置上試用，這樣繼續迭代下去就會發生位置估計的錯誤。

自適應蒙特卡洛，自適應主要體如今兩個方面。經過判斷粒子點的平均分突變來識別機器人綁架問題，並在全局從新撒點來解決機器人綁架問題；經過判斷粒子點的彙集程度來肯定位置估計是否準確，在估計比較準確的時候下降須要維護的粒子點數目，這樣來下降算法的計算開銷。

代價地圖costmap：

代價地圖用於描述環境中的障礙物信息，代價地圖利用激光雷達、聲吶、紅外測距等探測傳感器的數據來生成，大體的原理是經過創建不一樣的圖層Layer而後疊在一塊兒，被填充的柵格點表示有障礙物，如圖33，想要了解代價地圖的更多細節能夠閱讀《Layered Costmaps for Context-Sensitive Navigation》這篇論文。機器人導航中用到了兩個代價地圖，全局代價地圖global_costmap和局部代價地圖local_costmap。

（圖33）代價地圖

路徑規劃：

機器人導航中核心的就是路徑規劃，路徑規劃是經過利用環境障礙物信息找到一條到達目標而且開銷小的路勁。導航中會用到兩種路徑規劃，即全局路徑規劃global_planner和局部路徑規劃local_planner。全局路徑規劃更像是一種戰略性策略，須要考慮全局，規劃處一條儘可能短而且易於執行的路徑。在全局路徑的指導下，機器人在實際行走時還須要考慮周圍實時的障礙物並制定避讓策略，這就是局部路徑規劃要完成的事，能夠說機器人的自主導航最終是由局部路徑規劃一步步完成的。ROS中推薦的全局路徑規劃器是global_planner，基於A*算法，如圖34。

（圖34）基於A*算法的全局路徑規劃

因爲ROS中的局部路徑規劃器比較老了，因此我推薦你們使用比較新的局部路徑規劃器teb_local_planner。teb_local_planner是基於彈性時間帶碰撞約束的算法，算法將動態障礙物、運行時效、路徑平滑性等約束作綜合考慮，在複雜環境下有更優秀的表現，如圖35。

（圖35）基於彈性時間帶碰撞算法的局部路徑規劃

3.2.ros-navigation安裝

ros-navigation的安裝有兩種方法，方法一是直接經過apt-get安裝編譯好的ros-navigation庫到系統中，方法二是下載ros-navigation源碼手動編譯安裝。因爲後續可能須要對ros-navigation中的算法作修改和改進，因此我採用方法二進行安裝。

這裏將創建一個叫catkin_ws_nav的ROS工做空間，專門用於存放機器人導航相關的功能包。關於建立ROS工做空間的操做，請參考前面相應部份內容，這裏就不作講解。

安裝命令很簡單，執行下面的命令編譯安裝就好了。

首先前往https://github.com/ros-planning/navigation，將分支切換到kinetic-devel而後下載源碼到本地，將navigation-kinetic-devel.zip解壓備用。將解壓好的navigation-kinetic-devel文件夾拷貝到~/catkin_ws_nav/src/後，就可編譯安裝了。

#編譯過程可能會遇到找不到Bullet等依賴問題，解決這些依賴簡便辦法
#先apt-get安裝ros-navigation包，這樣依賴會被自動裝上 
sudo apt-get install ros-kinetic-navigation* 
#再apt-get卸載掉ros-navigation包 
sudo apt-get remove ros-kinetic-navigation ros-kinetic-navigation-experimental

cd  ~/catkin_ws_nav/
catkin_make

而後須要安裝teb_local_planner，前往https://github.com/rst-tu-dortmund/teb_local_planner，將分支切換到kinetic-devel而後下載源碼到本地，並解壓源碼包到~/catkin_ws_nav/src/後，就能夠編譯安裝了。

#安裝依賴
source ~/catkin_ws_nav/devel/setup.bash 
rosdep install teb_local_planner

cd  ~/catkin_ws_nav/
catkin_make  -DCATKIN_WHITELIST_PACKAGES="teb_local_planner"

值得一提的是，teb_local_planner源碼中關於plugin的配置文件均已寫好，直接編譯源碼就能完成plugin的註冊及插入，很是方便。

3.3.ros-navigation使用

ros-navigation功能包集是一個很強大的導航框架，支持幾乎全部的移動機器人。咱們只須要按照適當的配置就能夠將ros-navigation應用到咱們本身的機器人。爲了更好的管理配置文件和啓動節點，咱們須要先爲miiboo機器人創建一個功能包miiboo_nav，用於專門存放咱們機器人的導航配置與啓動文件。miiboo_nav功能包文件結構，如圖36。不難發現，miiboo_nav是一個不包含任何可執行源碼的功能包，裏面只包含config和launch。config中存放導航中各算法模塊的參數配置，launch中存放啓動導航所需的各類節點的啓動文件。下面依次介紹這些文件的做用。

（圖36）miiboo_nav功能包文件結構

機器人全局定位AMCL算法配置amcl.launch.xml：

導航中用到的全局定位AMCL算法功能包包含不少能夠配置的參數，這裏創建配置文件amcl.launch.xml對這些參數進行配置，內容如圖37。配置參數分爲3類：粒子濾波參數、雷達模型參數、里程計模型參數。因爲參數比較多，關於參數配置的具體講解就不展開，請直接參考wiki官方教程：http://wiki.ros.org/amcl。

（圖37）全局定位AMCL算法配置amcl.launch.xml

代價地圖公有參數配置costmap_common_params.yaml：

全局代價地圖和局部代價地圖公有的參數被放置在costmap_common_params.yaml這裏面，這樣只須要載入這個配置文件就能完成共有參數的配置，避免了在全局代價地圖和局部代價地圖配置文件中重複配置一些相同參數的冗餘，內容如圖38。配置參數分爲2類：機器人形狀和代價地圖各Layer圖層。機器人形狀能夠用多邊形或圓形描述，咱們的miiboo機器人是矩形的，因此選多邊形描述。代價地圖各Layer圖層包括：靜態層static_layer（由SLAM創建獲得的地圖提供數據）、障礙層obstacle_layer（由激光雷達等障礙掃描傳感器提供實時數據）、全局膨脹層global_inflation_layer（爲全局代價地圖提供膨脹效果）、局部膨脹層global_inflation_layer（爲局部代價地圖提供膨脹效果）。關於參數配置的具體講解就不展開，請直接參考wiki官方教程：http://wiki.ros.org/costmap_2d。

（圖38）代價地圖共有參數配置costmap_common_params.yaml

全局代價地圖參數配置global_costmap_params.yaml：

全局代價地圖是以插件的形式組建的，在costmap_common_params.yaml中定義的各Layer圖層均可以經過插件的形式放入全局代價地圖中，各Layer圖層的組合能夠根據需求自由選擇，如圖39。咱們的miiboo機器人的全局代價地圖只用了static_layer和global_inflation_layer兩個圖層。

（圖39）全局代價地圖參數配置global_costmap_params.yaml

局部代價地圖參數配置global_costmap_params.yaml：

局部代價地圖和全局代價地圖相似，就不展開講解了，如圖40。咱們的miiboo機器人的局部代價地圖只用了obstacle_layer和local_inflation_layer兩個圖層。

（圖40）局部代價地圖參數配置local_costmap_params.yaml

全局路徑規劃器參數配置global_planner_params.yaml：

全局路徑規劃器也是以插件的形式被使用，這裏主要對全局路徑規劃器GlobalPlanner插件及參數進行申明，在後面的move_base_params.yaml中將會調用這個插件。配置文件內容如圖41。關於參數配置的具體講解就不展開，請直接參考wiki官方教程：

http://wiki.ros.org/global_planner。

（圖41）全局路徑規劃器參數配置global_planner_params.yaml

局部路徑規劃器參數配置teb_local_planner_params.yaml：

局部路徑規劃器也是以插件的形式被使用，這裏主要對局部路徑規劃器TebLocalPlannerROS插件及參數進行申明，在後面的move_base_params.yaml中將會調用這個插件。配置文件內容如圖42。關於參數配置的具體講解就不展開，請直接參考wiki官方教程：

http://wiki.ros.org/teb_local_planner。

（圖42）局部路徑規劃器參數配置teb_local_planner_params.yaml

導航核心控制參數配置move_base_params.yaml：

導航核心控制由move_base節點最終實現，move_base在整個導航框架中處於核心地位，全局代價地圖、局部代價地圖、全局路徑規劃、局部路徑規劃、恢復策略等等都將在這裏獲得具體實現的調用。配置內容如圖43。不難發現，最開始兩行便對調用的全局路徑規劃器和局部路徑規劃器進行了申明，而後就是一些運動控制方面的參數。關於參數配置的具體講解就不展開，請直接參考wiki官方教程：http://wiki.ros.org/move_base。

（圖43）導航核心控制參數配置move_base_params.yaml

啓動導航所需的各類節點的啓動文件miiboo_nav.launch：

在config目錄中將各個導航模塊的參數配置好後，就能夠在一個啓動文件中將導航所需的各類節點進行啓動，並載入相關配置。啓動文件miiboo_nav.launch就是來完成這個任務的，如圖44。不難發現，第一個啓動項是map_server，將SLAM建圖中獲得的地圖進行載入後發佈到ROS中供導航中須要的模塊使用；第二個啓動項是amcl，amcl開始運行後將爲導航中的模塊提供機器人在地圖中的全局位置；第三個啓動項是move_base，在這裏將載入代價地圖公有參數、全局代價地圖參數、局部代價地圖參數、move_base參數、全局路徑規劃參數、局部路徑規劃參數。

（圖44）啓動導航所需的各類節點的啓動文件miiboo_nav.launch

到這裏，咱們已經準備好了導航的各個配置文件和啓動文件，如今就正式來進行導航。這裏須要特別提醒，咱們假設已經在SLAM建圖過程獲得了一張比較好的環境地圖，並保存到了正確的路徑。啓動導航分爲3步：啓動機器人上全部傳感器、啓動導航所需各個節點、發送導航目標點。

首先，啓動機器人上全部傳感器，打開終端，經過下面的命令直接啓動就好了。

source ~/catkin_ws/devel/setup.bash 
roslaunch miiboo_bringup miiboo_all_sensor.launch

而後，啓動導航所需各個節點，打開終端，經過下面的命令直接啓動就好了。

source ~/catkin_ws_nav/devel/setup.bash 
roslaunch miiboo_nav miiboo_nav.launch

最後，發送導航目標點。這裏有兩種方式來下發導航目標點，方法一是經過rviz交互界面，方法二是經過手機APP交互界面。下面對兩種方法分別介紹。

方法一：

在PC端打開rviz，打開一個新終端，運行rviz啓動命令。

rosrun rviz rviz

訂閱/map、/scan、/tf等信息，並觀察機器人的初始位置是否正確，若是機器人的初始位置不正確，須要用[2D Pose Estimate]按鈕手動給定一個正確的初始位置，如圖45。操做方法很簡單，先點擊[2D Pose Estimate]按鈕，而後將鼠標放置到機器人在地圖中實際應該的位置，最後按住鼠標並拖動鼠標來完成機器人朝向的設置。

（圖45）在rviz中手動給定機器人初始位置

初始位置設置正確後，就能夠用[2D Nav Goal]按鈕手動指定導航目標點了，如圖46。操做方法很簡單，先點擊[2D Nav Goal]按鈕，而後將鼠標放置到地圖中任意的想讓機器人到達的空白位置，最後按住鼠標並拖動鼠標來完成機器人朝向的設置。這樣機器人就會開始規劃路徑並自動導航到該指定目標點。

（圖46）在rviz中手動指定導航目標點

方法二：

直接打開手機上的miiboo機器人APP，就能夠看到地圖和機器人在地圖中的位置了。和方法一中的同樣，須要觀察機器人的初始位置是否正確，若是機器人的初始位置不正確，用[Set Pose]按鈕手動給定一個正確的初始位置，如圖47。操做過程和方法一相似，就不展開了。

（圖47）在miiboo機器人APP中手動給定機器人初始位置

初始位置設置正確後，就能夠用[Set Goal]按鈕手動指定導航目標點了，如圖48。操做過程和方法一相似，就不展開了。

（圖48）在miiboo機器人APP中手動指定導航目標點