一、SLAM建圖

“SLAM”，英文全稱是“Simultaneous Localization And Mapping”，翻譯過(guò)來(lái)就是“即時(shí)定位與地圖構(gòu)建”。SLAM最早由Smith、Self和Cheeseman于1988年提出，因?yàn)樗匾睦碚摵蛻?yīng)用價(jià)值，所以被認(rèn)為是使機(jī)器人實(shí)現(xiàn)真正自主移動(dòng)的重點(diǎn)。激光雷達(dá)的數(shù)據(jù)特點(diǎn)為雷達(dá)激光所在平面與障礙物相切所得的切面圖，即激光雷達(dá)數(shù)據(jù)的點(diǎn)陣形狀。

而激光雷達(dá)在環(huán)境中某一位置的掃描范圍具有局限性，只能掃描到對(duì)應(yīng)位置的障礙物信息。但是又因?yàn)橄噜徫恢盟鶔呙璧降恼系K物有一部分是重合的，所以可以將相鄰位置所掃描到的障礙物輪廓進(jìn)行組合，即將連續(xù)的激光雷達(dá)掃描到的障礙物輪廓組合起來(lái)，這樣就形成了一個(gè)完整的環(huán)境地圖，這個(gè)地圖是根據(jù)激光雷達(dá)掃描平面上所有障礙物的輪廓得到的。系統(tǒng)根據(jù)構(gòu)建地圖時(shí)障礙物輪廓的重合關(guān)系，可以反推出機(jī)器人對(duì)應(yīng)的位置關(guān)系和機(jī)器人位于地圖中的位置，這樣就實(shí)現(xiàn)了機(jī)器人在進(jìn)行地圖構(gòu)建的同時(shí)進(jìn)行自身實(shí)時(shí)定位的功能，這就是SLAM的由來(lái)。
ROS所支持的SLAM算法有很多，大多數(shù)用戶使用的是Hector SLAM和Gmapping，這兩者的區(qū)別為：Hector SLAM可以僅依靠激光雷達(dá)進(jìn)行工作；Gmapping則因?yàn)樾枰鄠鞲衅鞯臄?shù)據(jù)進(jìn)行融合使用，所以建圖質(zhì)量?jī)?yōu)于Hector SLAM。本章中使用Gmapping進(jìn)行地圖的構(gòu)建

1 在仿真環(huán)境中實(shí)現(xiàn)SLAM建圖

（1）確保在前面已經(jīng)對(duì)兩個(gè)開(kāi)源項(xiàng)目完成了部署。

（2）啟動(dòng)開(kāi)源項(xiàng)目“wpr_simulation”中SLAM建圖的仿真場(chǎng)景，在終端程序中輸入如下指令。

roslaunch wpr_simulation wpb_gmapping.launch

指令運(yùn)行后會(huì)彈出Gazebo仿真界面，以及Rviz工具界面

在仿真場(chǎng)景中用隔板將場(chǎng)景分為了4片區(qū)域，模擬了一個(gè)正常的家庭環(huán)境，將整場(chǎng)景分為廚房、餐廳、客廳和臥室，并在每個(gè)房間中放置了對(duì)應(yīng)的家具。
從Rviz工具界面中可以看到，由激光雷達(dá)所掃描到的區(qū)域?yàn)榛野咨す饫走_(dá)掃描到的障礙物為黑色，未掃描到的區(qū)域?yàn)樯罨疑?br /> （3）控制機(jī)器人在環(huán)境中移動(dòng)，以便于掃描完整地圖，一般情況下會(huì)使用遙控手柄控制機(jī)器人移動(dòng)。啟動(dòng)的launch文件中已經(jīng)包含了手柄控制的節(jié)點(diǎn)，可以直接使用手柄遙控。
如果沒(méi)有遙控手柄，可以使用鍵盤(pán)控制的方式移動(dòng)機(jī)器人，打開(kāi)一個(gè)新的終端程序，輸入如下指令。

rosrun wpr_simulation keyboard_vel_ctrl

指令運(yùn)行后，終端中會(huì)提示控制機(jī)器人移動(dòng)所用的按鍵，需要注意的是，必須保證此終端為當(dāng)前選中窗口，否則不能控制機(jī)器人移動(dòng)，控制機(jī)器人移動(dòng)的對(duì)應(yīng)按鍵為加速制，即按下對(duì)應(yīng)按鍵后，機(jī)器人速度為累加制，當(dāng)需要轉(zhuǎn)換機(jī)器人運(yùn)動(dòng)方向時(shí)，需先停止，再按下對(duì)應(yīng)方向按鍵。

控制機(jī)器人掃描整個(gè)場(chǎng)景之后，在系統(tǒng)中可以看到機(jī)器人所掃描的地圖

將地圖保存下來(lái)，打開(kāi)一個(gè)新的終端程序，輸入如下指令。

rosrun map_server map_saver -f map

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

二、Navigation自主導(dǎo)航

使用之前建好的地圖進(jìn)行機(jī)器人導(dǎo)航，在進(jìn)行實(shí)際操作之前，先來(lái)了解一下機(jī)器人導(dǎo)航的原理。在前面的實(shí)驗(yàn)中，使用激光雷達(dá)SLAM建立的地圖為柵格地圖，這種地圖是室內(nèi)移動(dòng)機(jī)器人進(jìn)行導(dǎo)航時(shí)常用的地圖格式，它是一種在二維空間上描述障礙物分布狀況的地圖形式。柵格地圖將整個(gè)環(huán)境狀況在一個(gè)切面上分割成一個(gè)橫豎排列的柵格小空間，每一個(gè)小空間用不同的顏色標(biāo)記出這個(gè)空間的情況。

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


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

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

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

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

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

（1）復(fù)制地圖文件

建立的兩個(gè)地圖文件復(fù)制到工作空間“~/catkin_ws/src/wpr_simulation/maps”內(nèi)。

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

roslaunch wpr_simulation wpb_navigation.launch

仿真程序運(yùn)行后會(huì)彈出導(dǎo)航仿真場(chǎng)景，場(chǎng)景內(nèi)的布局和建圖的仿真場(chǎng)景一致，只是機(jī)器人初始位置發(fā)生了改變。

啟動(dòng)Gazebo的同時(shí)，Rviz也會(huì)啟動(dòng)，可以在左側(cè)任務(wù)欄進(jìn)行窗口切換，在Rviz界面中就可以看到前面創(chuàng)建好的地圖。

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

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

同時(shí)，我們?cè)贕azebo仿真界面中也可以看到機(jī)器人在場(chǎng)景中移動(dòng)到目標(biāo)點(diǎn)位。接下來(lái)可以嘗試設(shè)置其他位置目標(biāo)點(diǎn)，觀察機(jī)器人規(guī)劃的路徑及移動(dòng)過(guò)程

來(lái)源: 部分來(lái)自網(wǎng)絡(luò)