一、SLAM建圖

“SLAM”，英文全稱是“Simultaneous Localization And Mapping”，翻譯過來就是“即時定位與地圖構建”。SLAM最早由Smith、Self和Cheeseman于1988年提出，因為它重要的理論和應用價值，所以被認為是使機器人實現真正自主移動的重點。激光雷達的數據特點為雷達激光所在平面與障礙物相切所得的切面圖，即激光雷達數據的點陣形狀。

而激光雷達在環境中某一位置的掃描范圍具有局限性，只能掃描到對應位置的障礙物信息。但是又因為相鄰位置所掃描到的障礙物有一部分是重合的，所以可以將相鄰位置所掃描到的障礙物輪廓進行組合，即將連續的激光雷達掃描到的障礙物輪廓組合起來，這樣就形成了一個完整的環境地圖，這個地圖是根據激光雷達掃描平面上所有障礙物的輪廓得到的。系統根據構建地圖時障礙物輪廓的重合關系，可以反推出機器人對應的位置關系和機器人位于地圖中的位置，這樣就實現了機器人在進行地圖構建的同時進行自身實時定位的功能，這就是SLAM的由來。
ROS所支持的SLAM算法有很多，大多數用戶使用的是Hector SLAM和Gmapping，這兩者的區別為：Hector SLAM可以僅依靠激光雷達進行工作；Gmapping則因為需要多傳感器的數據進行融合使用，所以建圖質量優于Hector SLAM。本章中使用Gmapping進行地圖的構建

1 在仿真環境中實現SLAM建圖

（1）確保在前面已經對兩個開源項目完成了部署。

（2）啟動開源項目“wpr_simulation”中SLAM建圖的仿真場景，在終端程序中輸入如下指令。

roslaunch wpr_simulation wpb_gmapping.launch

指令運行后會彈出Gazebo仿真界面，以及Rviz工具界面

在仿真場景中用隔板將場景分為了4片區域，模擬了一個正常的家庭環境，將整場景分為廚房、餐廳、客廳和臥室，并在每個房間中放置了對應的家具。
從Rviz工具界面中可以看到，由激光雷達所掃描到的區域為灰白色，激光雷達掃描到的障礙物為黑色，未掃描到的區域為深灰色。
（3）控制機器人在環境中移動，以便于掃描完整地圖，一般情況下會使用遙控手柄控制機器人移動。啟動的launch文件中已經包含了手柄控制的節點，可以直接使用手柄遙控。
如果沒有遙控手柄，可以使用鍵盤控制的方式移動機器人，打開一個新的終端程序，輸入如下指令。

rosrun wpr_simulation keyboard_vel_ctrl

指令運行后，終端中會提示控制機器人移動所用的按鍵，需要注意的是，必須保證此終端為當前選中窗口，否則不能控制機器人移動，控制機器人移動的對應按鍵為加速制，即按下對應按鍵后，機器人速度為累加制，當需要轉換機器人運動方向時，需先停止，再按下對應方向按鍵。

控制機器人掃描整個場景之后，在系統中可以看到機器人所掃描的地圖

將地圖保存下來，打開一個新的終端程序，輸入如下指令。

rosrun map_server map_saver -f map

這條指令的意思是，啟動map_server包的map_saver程序，將當前SLAM建好的圖保存為名為“map”的地圖。按下“Enter”鍵，確認保存，此時系統會提示地圖保存完成信息，同時在系統的主文件夾中會出現兩個名為“map”的文件，一個名為“map.pgm”，另一個名為“map.yaml”。其中“map.pgm”為圖片格式，雙擊可以查看圖片內容，它的內容就是建好的地圖圖案。

二、Navigation自主導航

使用之前建好的地圖進行機器人導航，在進行實際操作之前，先來了解一下機器人導航的原理。在前面的實驗中，使用激光雷達SLAM建立的地圖為柵格地圖，這種地圖是室內移動機器人進行導航時常用的地圖格式，它是一種在二維空間上描述障礙物分布狀況的地圖形式。柵格地圖將整個環境狀況在一個切面上分割成一個橫豎排列的柵格小空間，每一個小空間用不同的顏色標記出這個空間的情況。

Rviz中的二維地圖是用ROS的SLAM算法建立的二維平面地圖在Rviz里的顯示，如果放大觀察，這個二維地圖的細節部分是由一個個正方形的柵格組成的。


白色表示該柵格空間內無障礙物，機器人可以通行；黑色表示該柵格空間內存在障礙物，機器人不能通行；灰色表示該柵格空間尚未探索，可通行情況不明

有了柵格地圖，機器人的地圖導航問題就變成在柵格地圖中尋找機器人能通過的空間區域并驅動機器人從起點移動到終點，這里面包含了兩個部分的任務。
（1）機器人定位。機器人需要知道自己當前在地圖中的位置，才能確定導航的起點在哪。機器人在移動過程中，也需要時刻確定自己的位置是否貼合規劃的路徑。
（2）路徑規劃與導航。路徑規劃算法就是在柵格地圖中尋找一條可通行的路徑，即連續的可通行柵格，其從機器人當前位置一直延伸到導航的目標終點

在實際的機器人路徑規劃中，除了考慮白色可通行柵格的連通情況，還需要考慮機器人本身體積所占用的空間，只有白色柵格空間大于機器人的底盤直徑，機器人才能安全通過。因此通常會在黑色障礙物域膨脹出一條碰撞安全邊界，這條邊界的寬度為機器人底盤半徑長度，它的意義是：當機器人位于這條安全邊界里時提示它會與障礙物發生碰撞。這樣，機器人的可通行路徑會避開這一條安全邊界，在剩余的可通行區域里規劃。
在ROS中啟動Navigation導航系統后，可以在Rviz里看到障礙物柵格周圍有一圈淡藍色的安全邊界。機器人的路徑規劃會避開黑色障礙物柵格和淡藍色的安全邊界，在剩余的白色可通行區域里尋找出一條最短路徑，圖中的曲線就是Navigation導航系統規劃出的路徑。

ROS的Navigation導航系統按照上述功能分為兩個主要部分。
（1）AMCL：英文全稱為“Adaptive Monte Carlo Localization”（蒙特卡洛自適應定位算法），這是一種使用概率理論在已知地圖中對機器人自定位置進行估計的方法。這種方法會先在機器人可能通過的位置周圍假設多個位置，然后在機器人行進過程中，依據激光雷達和電機碼盤里程計等設備輸出的信息對這些假設位置進行篩選，逐步剔除明顯不可信的假設位置，留下可信度較高的定位位置。比如在圖中，地面上那些分散的綠色箭頭就是機器人的假設位置，一開始的時候綠色箭頭很多、很分散，在機器人運動過程中，這些箭頭會逐漸收斂，最終匯聚成一個箭頭，它就是機器人最可信的定位位置。
（2）Move_Base：這是Navigation系統里扮演核心中樞的ROS包，它將機器人導航需要用到的地圖、坐標、路徑和行為規劃器連接到了一起，同時還提供了導航參數的設置接口。

具體如下。
（1）map_server在主題“/map”里提供全局地圖信息，global_costmap從這個主題里獲得全局地圖信息，再加上從sensor sources獲得的激光雷達和點云等的信息，融合成一個全局的柵格代價地圖。
（2）全局的柵格代價地圖交給global_planner進行全局路徑規劃，再結合外部節點發送到主題“/move_base_simple/goal”里的移動終點得出紫色的全局移動路徑。
（3）local_costmap從global_costmap中截取機器人周圍一定距離內的地圖，結合傳感器信息生成一個局部的代價地圖。
（4）local_planner從global_planner獲得全局規劃路徑，結合從local_costmap獲得的局部代價地圖及amcl提供的機器人位置信息，計算出機器人當前應該執行的速度，發送到主題“cmd_vel”里，驅動機器人沿著全局路徑進行移動。
從上面部分可以看出，ROS的Navigation系統已經包含了導航需要的大部分功能，只需要提供全局地圖和導航目的地即可進行Navigation導航。

（1）復制地圖文件

建立的兩個地圖文件復制到工作空間“~/catkin_ws/src/wpr_simulation/maps”內。

(2）在終端程序中輸入如下指令啟動仿真程序

roslaunch wpr_simulation wpb_navigation.launch

仿真程序運行后會彈出導航仿真場景，場景內的布局和建圖的仿真場景一致，只是機器人初始位置發生了改變。

啟動Gazebo的同時，Rviz也會啟動，可以在左側任務欄進行窗口切換，在Rviz界面中就可以看到前面創建好的地圖。

在這個界面中可以看到規劃器根據地圖固定障礙物膨脹出來的安全邊界。
（3）在Rviz剛啟動時，機器人的默認位置是地圖的起始點，就是建圖時機器人出發的地方。而在導航開始時，現實世界中機器人很有可能并不在之前建圖時機器人出發的位置。這一點也可以通過觀察Rviz中紅色激光雷達數據點和靜態障礙物輪廓是否貼合判斷出來。如果現實世界中的機器人位置和Rviz中顯示的位置有偏差，那么需要在導航前先糾正這個偏差，具體過程如下。
先單擊Rviz界面上方工具欄里的“2D Pose Estimate”按鈕，然后單擊Rviz界面里現實機器人所處的位置。這時，界面里會出現一個箭頭，代表的是機器人在初始位置的朝向。按住鼠標左鍵不放，在屏幕上畫圈，可以控制箭頭的朝向，選定方向后即可松開按鍵。

4）設置好機器人的初始位置后，就可以為機器人指定導航的目標地點。先單擊Rviz工具欄里的“2D Nav Goal”按鈕，然后單擊Rviz界面上的導航目標點（通常在白色區域里選擇一個地點）。此時會再次出現箭頭，和前面的操作一樣，按住鼠標左鍵不放在屏幕上拖動，設置機器人移動到終點后的朝向。
選擇完目標朝向后，松開鼠標左鍵即可看到路徑規劃，全局規劃器會自動規劃出一條路徑，機器人會沿著此路徑移動到目標點，并在調整朝向后停止

同時，我們在Gazebo仿真界面中也可以看到機器人在場景中移動到目標點位。接下來可以嘗試設置其他位置目標點，觀察機器人規劃的路徑及移動過程

來源: 部分來自網絡