? ? ? ?即時定位與地圖構(gòu)建（Simultaneous Localization and Mapping，SLAM）技術(shù)可以很精確地實現(xiàn)環(huán)境的地圖構(gòu)建、定位以及多點導航。目前SLAM技術(shù)可以分為激光SLAM（Lidar SLAM）和視覺SLAM（Visual SLAM），激光SLAM采用的傳感器為激光雷達，而視覺SLAM則采用深度攝像頭。激光SLAM技術(shù)較為成熟、誤差少，且足以滿足當前環(huán)境的使用。

? ? ? ?近年來，隨著計算機視覺和人工智能等高新技術(shù)的逐漸成熟，其被越來越廣泛地應(yīng)用于人們生活中的每個角落。室內(nèi)服務(wù)機器人將會是未來一個很大的發(fā)展方向，由于在室內(nèi)環(huán)境中GPS無法精準定位，所以SLAM技術(shù)應(yīng)運而生。SLAM（同步定位與地圖構(gòu)建），是指運動物體根據(jù)傳感器的信息，一邊計算自身位置、一邊構(gòu)建環(huán)境地圖的過程，解決機器人等在未知環(huán)境下運動時的定位與地圖構(gòu)建問題。

? ? ? ?目前，SLAM主要應(yīng)用于機器人、無人機、無人駕駛、AR、VR等領(lǐng)域。其用途包括傳感器自身的定位，以及后續(xù)的路徑規(guī)劃、運動性能、場景理解。當機器人有了SLAM的功能，就相當于機器人有了一雙眼睛，其可以掃描未知環(huán)境從而獲取地圖，以及實時獲取自己的當前位置；可以自行路徑規(guī)劃到達指定地圖上的地點。如智能掃地機就可以通過SLAM實現(xiàn)精準的地面避障遍歷，又自行回到充電地點。在無人駕駛以及無人機上亦同樣適用，其需要實現(xiàn)一個共同的目的：避障、定位、路徑規(guī)劃。為了讓機器人更加智能化，可在實現(xiàn)此項技術(shù)上再加上手機app的控制和語音識別控制。

? ? ? ??SLAM系統(tǒng)一般分為五個模塊，包括傳感器數(shù)據(jù)、視覺里程計、后端、建圖及回環(huán)檢測。

? ? ? ?傳感器數(shù)據(jù)：主要用于采集實際環(huán)境中的各類型原始數(shù)據(jù)。包括激光掃描數(shù)據(jù)、視頻圖像數(shù)據(jù)、點云數(shù)據(jù)等。

? ? ? ?視覺里程計：主要用于不同時刻間移動目標相對位置的估算。包括特征匹配、直接配準等算法的應(yīng)用。

? ? ? ?后端：主要用于優(yōu)化視覺里程計帶來的累計誤差。包括濾波器、圖優(yōu)化等算法應(yīng)用。

? ? ???建圖：用于三維地圖構(gòu)建。

? ? ???回環(huán)檢測：主要用于空間累積誤差消除。

? ? ? ?其工作流程大致為：傳感器讀取數(shù)據(jù)后，視覺里程計估計兩個時刻的相對運動（Ego-motion），后端處理視覺里程計估計結(jié)果的累積誤差，建圖則根據(jù)前端與后端得到的運動軌跡來建立地圖，回環(huán)檢測考慮了同一場景不同時刻的圖像，提供了空間上約束來消除累積誤差。

? ? ? ?激光SLAM是目前最穩(wěn)定、最主流的定位導航方法。目前室內(nèi)主要采用二維激光雷達，室外采用三維激光雷達。今天我們來分享一下2D激光雷達SLAM建圖定位與導航。

? ? ? ?該項目我們利用鍵盤控制福來輪底盤小車運動完成slam建圖，并能在已建好的地圖里進行自主導航。

? ? ? ?slam導航可以拆分為三步：

? ? ? ?第一步：能用鍵盤控制底盤的運動；

? ? ? ?第二步：基于實際場景，用鍵盤控制底盤小車進行slam建圖；

? ? ? ?第三步：基于已建好的地圖，模擬實現(xiàn)slam導航。

福來輪底盤小車

一、主要使用的器材

? ? ? ?樹莓派、雷達、樹莓派相關(guān)的實驗器材（顯示屏、鍵盤鼠標、充電寶/手機充電器）

二、電路連接

? ? ? ?根據(jù)slam導航的要求，下面將詳細介紹各個電子部件與slam導航車的連接過程。? ? ??

? ? ? ?第一步：先將步進電機與主板的擴展板進行連接（提示：側(cè)重看x、y、z的接線位置，裝有雷達的一側(cè)代表底盤小車的車頭。雷達左側(cè)的步進電機控制小車的X軸方向；雷達的右側(cè)的步進電機控制小車的Y軸方向；雷達對側(cè)的步進電機控制小車的Z軸方向，具體的接線引腳見下圖）

? ? ? ?第二步：將主控板的擴展板跟樹莓派連接起來（見圖中藍色的線），并用鋰電池進行供電。

? ? ? ?第三步：將鼠標、鍵盤、顯示屏與樹莓派進行連接（見下圖）。其中顯示屏和樹莓派用充電寶供電，鼠標、鍵盤這里用的是無線的。

? ? ? ?第四步：將雷達與樹莓派進行連接（建議把雷達連接在樹莓派的usb3.0上，見下圖）。

? ? ? ?經(jīng)過上邊幾步的電路連接后，就完成了相應(yīng)的電路連接（見下圖）。

一、軟件環(huán)境介紹

? ? ? ?在使用樹莓派之前，我們需要給樹莓派的SD卡里燒錄系統(tǒng)鏡像。而本次實驗中的SD卡已經(jīng)燒錄好系統(tǒng)鏡像，相關(guān)配置如下：

? ? ? ?操作系統(tǒng)：Ubuntu18.04系統(tǒng)，基于Debian GNU/Linux，支持x86、amd64（即x64）、ARM和ppc架構(gòu)。

? ? ? ?仿真系統(tǒng)：基于開源機器人操作系統(tǒng)ROS melodic和開源軟件平臺Arduino開發(fā)，上位機采用ROS melodic，基于Rviz完成全向移動底盤小車slam導航運動規(guī)劃，采用gazebo完成全向移動底盤小車物理運動仿真；下位機采用Arduino實現(xiàn)對全向移動底盤小車運動的控制。

二、程序編寫

（一）鍵盤控制底盤小車運動

? ? ? ?（1）實現(xiàn)思路

? ? ? ?按下鍵盤上指定的鍵，實現(xiàn)底盤小車前進、后退、轉(zhuǎn)向、平移；還可以靈活的設(shè)置底盤小車的角速度、線速度來調(diào)整底盤小車的運動。下面是規(guī)劃的控制底盤小車的鍵盤命令（見下圖），可以先熟悉一下，稍后在控制底盤小車時會用到。

? ? ? ?（2）編寫程序及調(diào)試

? ? ? ?①?將以下Arduino程序下載到主機中【詳細的程序源碼可加QQ交流群：221964861進行獲取】

? ? ? ?②?啟動雷達、鍵盤、rosserial程序包。打開終端,輸入roslaunch robot_navigation_control robot.launch 命令（見下圖），?等待程序的運行啟動界面。成功啟動后，可以在終端看到底盤小車的當前speed（線速度）為0.04；turn（角速度）為0.08。

? ? ? ?啟動后的界面（見下圖）:

? ? ? ?③?嘗試按下鍵盤命令，控制底盤小車的運動。(注意：1、請先保證終端是激活狀態(tài)；2、按鍵盤時，稍微有點間隔，這樣給底盤小車轉(zhuǎn)動的時間；3、一定要打開擴展版上的電源開關(guān)，否則會發(fā)現(xiàn)小車位置在rviz中位置變化，但車不運動）

（二）底盤小車同步定位與建圖-SLAM? ? ??

? ? ? ?（1）實現(xiàn)思路? ? ??

? ? ? ?利用Gmapping算法對底盤小車所在的未知環(huán)境進行建立地圖、同步定位、最后保存此地圖。此地圖可供底盤小車導航使用。?

? ? ? ?Gmapping算法是目前基于激光雷達和里程計方案里面比較可靠和成熟的一個算法， 它基于粒子濾波，采用RBPF的方法效果穩(wěn)定。本次底盤slam建圖采用的是 gmapping_slam包。下面介紹Gmapping SLAM計算圖。Gmapping的作用是根據(jù)激光雷達和里程計（Odometry）的信息，對環(huán)境地圖進行構(gòu)建，并且對自身狀態(tài)進行估計。因此它得輸入應(yīng)當包括激光雷達和里程計的數(shù)據(jù)，而輸出應(yīng)當有自身位置和地圖。下面我們從計算圖（消息的流向）的角度來看看gmapping算法的實際運行中的結(jié)構(gòu)：

? ? ? ?位于中心的是我們運行的slam_gmapping節(jié)點，這個節(jié)點負責整個gmapping SLAM的工作。它的輸入需要有兩個：

? ? ? ?輸入

? ? ? ?/tf 以及 /tf_static：坐標變換，類型為第一代的 tf/tfMessage 或第二代的 tf2_msgs/TFMessage 其中一定得提供的有兩個tf，一個是base_frame 與 laser_frame 之間的tf，即機器人底盤和激光雷達之間的變換；一個是base_frame 與 odom_frame之間的tf，即底盤和里程計原點之間的坐標變換。odom_frame可以理解為里程計原點所在的坐標系。

? ? ? ?/scan :激光雷達數(shù)據(jù)，類型為sensor_msgs/LaserScan?

? ? ? ?/scan很好理解，Gmapping SLAM所必須的激光雷達數(shù)據(jù)，而/tf 是一個比較容易忽 視的細節(jié)。盡管/tf 這個Topic聽起來很簡單，但它維護了整個ROS三維世界里的轉(zhuǎn)換關(guān)系，而 slam_gmapping要從中讀取的數(shù)據(jù)是base_frame 與 laser_frame 之間的tf，只有這樣才能夠把周圍障礙物變換到機器人坐標系下，更重要的是base_frame 與 odom_frame之間的tf，這個tf 反映了里程計（電機的光電碼盤、視覺里程計、IMU）的監(jiān)測數(shù)據(jù)，也就是機器人里程計測得走了多少距離，它會把這段變換發(fā)布到odom_frame 和 laser_frame 92之間。因此slam_gmapping 會從/tf 中獲得機器人里程計的數(shù)據(jù)。?

? ? ? ?輸出

? ? ? ?/tf：主要是輸出 map_frame 和 odom_frame 之間的變換

? ? ?? /slam_gmapping/entropy：std_msgs/Float64 類型，反映了機器人位姿估計的分 散程度

? ? ? ?/map：slam_gmapping 建立的地圖

? ? ?? /map_metadata：地圖的相關(guān)信息

? ? ? ?輸出的/tf 里又一個很重要的信息，就是map_frame和odom_frame之間的變換，這 其實就是對機器人的定位。通過連通map_frame和odom_frame，這樣map_frame與 base_frame 甚至與 laser_frame 都連通了。這樣便實現(xiàn)了機器人在地圖上的定位。

? ???? （2）編寫程序及調(diào)試? ? ??

? ? ? ?①?先搭建一個實際場景，便于建立地圖?！咀⒁猓簽榱朔奖阕R別，搭建的實際測試地圖最好是不規(guī)則的區(qū)域。如下圖測試的場地，在長方形區(qū)域的一邊是不規(guī)則的，這樣便于識別建立的仿真地圖】

? ? ? ?② 按照上述步驟，下載控制底盤小車的Arduino程序；啟動雷達、鍵盤、rosserial；打開終端并輸入命令：roslaunch robot_navigation_control robot_car.launch? ? ??

? ? ? ?③?啟動構(gòu)建地圖服務(wù)。按（ctrl+shift+T）打開第二個終端并輸入：roslaunch four_macnum_slam four_macnum_slam.launch（見下圖）。

? ? ? ?界面啟動后，在rviz中選擇“map”，可以在可視化界面看到有地圖出現(xiàn)。下面根據(jù)實際環(huán)境進行建圖：按下鍵盤相關(guān)指令（鍵盤指令請參考上述表格）來控制底盤小車在場地內(nèi)運行，直至在Rviz內(nèi)可以看到構(gòu)建地圖的輪廓，如下圖所示（注意：地圖環(huán)境會根據(jù)實際場景建立）

? ? ? ?為了后續(xù)在已知環(huán)境中運行底盤小車，需把此次建立的地圖進行保存。需要先進入保存地圖的文件夾，然后給地圖命名，保存即可。例如我們把此次建立的地圖命名為your_map_name。【詳細的操作步驟可加QQ交流群：221964861進行獲取】

（三）底盤小車導航—Navigation

? ? ? ?（1）實現(xiàn)思路

? ? ? ?在上述搭建好的實際場景中，實現(xiàn)底盤小車的導航效果。? ? ??

? ? ? ?具體思路：加載上節(jié)建好的地圖，設(shè)置底盤小車的初始位置和方向，設(shè)定目標位置及方向，底盤小車就可以實現(xiàn)導航效果。? ? ??

? ? ? ?Navigation與底盤關(guān)系：ROS的二維導航功能包，簡單來說，就是根據(jù)輸入的里程計等傳感器的信息流和機器 人的全局位置，通過導航算法，計算得出安全可靠的機器人速度控制指令。但是如何在特定的機器人上實現(xiàn)導航功能包的功能，卻是一件較為復(fù)雜的工程，作為導航功能包使用的必要先決條件，機器人必須運行ROS，發(fā)布tf變換樹，并發(fā)布使用ROS消息類型的傳感器數(shù)據(jù)。同時為了讓機器人更好的完成導航任務(wù)，開發(fā)者還要根據(jù)機器人的外形尺寸和性能， 配置導航功能包的一些參數(shù)。?? ? ??

? ? ? ?在ROS中進行導航需要使用到的三個包是：?? ? ??

? ? ? ?① move_base：根據(jù)參照的消息進行路徑規(guī)劃，使移動機器人到達指定的位置；? ? ??

? ? ? ?② gmapping：根據(jù)激光數(shù)據(jù)（或者深度數(shù)據(jù)模擬的激光數(shù)據(jù)）建立地圖；?? ? ??

? ? ? ?③ amcl：根據(jù)已經(jīng)有的地圖進行定位。

? ? ? ?基于上圖，我們來看看底盤簡單對應(yīng)關(guān)系。

? ? ? ?（2）編寫程序及調(diào)試? ? ??

? ? ? ?① 編寫程序或直接燒錄上述程序。啟動雷達、鍵盤、rosserial，在該終端并繼續(xù)輸入：roslaunch robot_navigation_control robot_car.launch（見下圖）

? ? ? ?按下（ctrl+shift+T）打開第二個終端并輸入：roslaunch four_macnum_navigation four_macnum_navigation.launch （見下圖）

? ? ? ?啟動界面后，可以看到許多紅色的箭頭，代表底盤小車的姿態(tài)估計還不準確。

? ? ? ?②?開始調(diào)整底盤小車的姿態(tài)

? ? ? ?第一步：請先旋轉(zhuǎn)地圖，確保地圖和實際場景的方向基本一致。

? ? ? ?第二步：使用2D Pose Estimate 標定底盤小車位于地圖中的初始位置及車頭指向（見下圖）

? ? ? ?第三步：鼠標點擊有 roslaunch robot_navigation_control robot_car.launch 的終端后，嘗試按下鍵盤命令控制底盤小車運動，最好盡可能多的消除地圖中的箭頭【這一步實際是確定底盤小車在地圖中的實際位置以及車頭指向】。? ? ??

? ? ? ?下面是多次按下鍵盤命令控制底盤小車運動后，箭頭逐漸減少的效果。

? ? ? ?下圖是最后調(diào)整完的底盤小車姿態(tài)（即底盤小車在地圖中的實際位置以及車頭指向）。

? ? ? ?③ 嘗試給定底盤小車目標位置【使用2D Nav Goal】，這里的目標位置包含了（目標點及車頭指向），在可視化界面點擊且轉(zhuǎn)動箭頭防線，機器人會移動到指定點的位置?！驹敿毜牟僮鞑襟E可加QQ交流群：221964861進行獲取】