1. 功能描述

? ?本文將利用鍵盤控制輪腿運動完成slam建圖，并能在已建好的地圖里進行自主導航。slam導航可以拆分為三步：

? ?第一步：能用鍵盤控制輪腿機器人運動；

? ?第二步：基于實際場景，用鍵盤控制輪腿機器人進行slam建圖；

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

2. 功能實現(xià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)對全向移動輪腿運動的控制。

? ?編程環(huán)境：Arduino 1.8.19

2.1 鍵盤控制輪腿運動

（1）實現(xiàn)思路：按下鍵盤上指定的鍵時，可以實現(xiàn)輪腿前進、后退、轉(zhuǎn)向、平移；還可以靈活的設(shè)置輪腿的角速度、線速度來調(diào)整輪腿的運動。下面是規(guī)劃的控制輪腿的鍵盤命令（見下表）?？梢韵仁煜ひ幌拢罄m(xù)在控制輪腿機器人時會用到。

（2）操作步驟：

① 打開參考例程文件夾（fourmacnum_car_ws\src\arduino_programming\Ros_Arduino_Translation_Programming），下載控制輪腿的程序

? ?Ros_Arduino_Translation_Programming.ino：

② 啟動雷達、鍵盤、rosserial程序包。打開終端,輸入roslaunch robot_navigation_control robot.launch命令（見下圖），等待程序的運行啟動界面。

成功啟動后，可以在終端看到輪腿的當前speed（線速度）為0.04，turn（角速度）為0.08。

注意：speed指的是線速度的大?。ò较颍?，turn指的是角速度的大小（包含方向）。

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

③ 嘗試按下鍵盤命令，控制輪腿的運動。 (注意：一、請先保證終端是激活狀態(tài)，見上圖；二是按鍵盤時，稍微有點間隔，這樣好給輪腿轉(zhuǎn)動的時間)。

? ?假如現(xiàn)在希望先增加輪腿的最大速度，再降低輪腿的最大速度，則分別按下鍵盤上q、z命令，可以觀察到終端的結(jié)果（見下圖）。

更多命令可自行嘗試。

2.2 輪腿同步定位與建圖-SLAM ?

（1）實現(xiàn)思路：利用Gmapping算法對輪腿所在的未知環(huán)境進行建立地圖、同步定位、最后保存此地圖。此地圖可供后續(xù)輪腿導航使用。Gmapping算法是目前基于激光雷達和里程計方案里面比較可靠和成熟的一個算法， 它基于粒子濾波，采用RBPF的方法，效果穩(wěn)定。本次輪腿機器人slam建圖采用的是gmapping_slam包。下面介紹Gmapping SLAM計算圖。Gmapping的作用是根據(jù)激光雷達和里程計（Odometry）的信息，對環(huán)境地圖進行構(gòu)建，并且對自身狀態(tài)進行估計。因此它得輸入應當包括激光雷達和里程計的數(shù)據(jù)，而輸出應當有自身位置和地圖。下面我們從計算圖（消息的流向）的角度來看看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）操作步驟：

① 啟動雷達、鍵盤、rosserial。打開終端并輸入命令：roslaunch robot_navigation_control robot_car.launch（見下圖）。

② 啟動構(gòu)建地圖服務(wù)。重新打開新終端并輸入：roslaunch four_macnum_slam four_macnum_slam.launch（見下圖）。

界面啟動后，在rviz中選擇“map”，可以在可視化界面看到有地圖出現(xiàn)；

下面根據(jù)實際環(huán)境進行建圖：按下鍵盤相關(guān)指令（鍵盤指令請參考上面2.1內(nèi)容）來控制輪腿在場地內(nèi)運行，直至在Rviz內(nèi)可以看到構(gòu)建地圖的輪廓，如下圖所示（注意實際地圖環(huán)境不同，出現(xiàn)的輪廓不同）。

③ 為了后續(xù)在已知環(huán)境中運行輪腿，需把此次建立的地圖進行保存。需要先進入保存地圖的文件夾，然后給地圖命名保存即可（見下圖）。

下面是以保存your_map_name地圖為例來介紹地圖的保存過程：

重新打開新終端（ctrl+shift+t）并輸入：cd fourmacnum_car_ws/src/four_macnum_navigation/maps

之后輸入：rosrun map_server map_saver -f ./your_map_name

返回下圖紅色框的信息，代表成功保存地圖。

查看生成的地圖文件：

? ? ?上面我們用map_server來保存地圖。這里我們來簡單了解和查看一下剛才保存的地圖文件（見下圖）。其中map_server是一個和地圖相關(guān)的功能包，它可以將已知地圖發(fā)布出來，供導航和其他功能使用，也可以保存SLAM建立的地圖。

? ? ?地圖文件，通常為pgm格式；

? ? ?地圖的描述文件，通常為yaml格式。

打開your_map_name.pgm，見下圖：

your_map_name.yaml ，各個參數(shù)見下圖。

其中占據(jù)的概率 occ = (255-color_avg)/255.0，color_avg為RGB三個通道的平均值。

2.3 輪腿導航—Navigation

（1）實現(xiàn)思路：加載上述2.2建好的地圖，設(shè)置輪腿機器人的初始位置和方向，設(shè)定目標位置及方向，輪腿就可以實現(xiàn)導航效果。

Navigation與機器人底盤關(guān)系：ROS的二維導航功能包，簡單來說，就是根據(jù)輸入的里程計等傳感器的信息流和機器人的全局位置，通過導航算法，計算得出安全可靠的機器人速度控制指令。但是如何在特定的機器人上實現(xiàn)導航功能包的功能，卻是一件較為復雜的工程，作為導航功能包使用的必要先決條件，機器人必須運行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)有的地圖進行定位。

（2）操作步驟：

① 將創(chuàng)建的地圖應用到導航程序中。

? ? 這里我們需要修改four_macnum_navigation.launch文件,把上述2.2的zhanhui_mapping_test.yaml 添加到此launch文件中。

步驟如下：

打開終端并輸入：cd fourmacnum_car_ws/src/four_macnum_navigation/launch

之后再輸入：gedit four_macnum_navigation.launch

② 啟動雷達、鍵盤、rosserial。在該終端繼續(xù)輸入：roslaunch robot_navigation_control robot_car.launch。

③打開新終端并輸入：roslaunch four_macnum_navigation four_macnum_navigation.launch（見下圖）。

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

④ 開始調(diào)整輪腿的姿態(tài)。使用2D Pose Estimate 標定輪腿位于地圖中的初始位置及車頭指向（見下圖）。

⑤ 鼠標點擊有roslaunch robot_navigation_control robot_car.launch的終端后，嘗試按下鍵盤命令控制輪腿運動（鍵盤命令請參考上面2.1內(nèi)容），最好盡可能多的消除地圖中的箭頭（這一步實際是確定輪腿在地圖中的實際位置以及車頭指向）。下面是多次按下鍵盤命令控制輪腿運動后，箭頭逐漸減少的效果。

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

⑥ 嘗試給定輪腿目標位置（使用2D Nav Goal），這里的目標位置包含了目標點及車頭指向，在可視化界面點擊且轉(zhuǎn)動箭頭防線，機器人會移動到指定點的位置。?

3. 資料下載

資料內(nèi)容：slam導航-例程源代碼

資料下載地址：http://www.robotway.com/h-col-196.html

想了解更多機器人開源項目資料請關(guān)注 機器譜網(wǎng)站 https://www.robotway.com