1. 功能說(shuō)明

? ? 本文我們將利用鍵盤(pán)控制三輪全向底盤(pán)（R313b）小車(chē)運(yùn)動(dòng)完成slam建圖，并能在已建好的地圖里進(jìn)行自主導(dǎo)航。slam導(dǎo)航可以拆分為三步：

? ? 第一步：能用鍵盤(pán)控制底盤(pán)的運(yùn)動(dòng)；

? ? 第二步：基于實(shí)際場(chǎng)景，用鍵盤(pán)控制底盤(pán)小車(chē)進(jìn)行slam建圖；

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

2. 電子硬件

本實(shí)驗(yàn)中采用了以下硬件：

按下圖進(jìn)行電路連接：

第一步：先將步進(jìn)電機(jī)與主板的擴(kuò)展板進(jìn)行連接（提示：側(cè)重看x、y、z的接線位置，裝有雷達(dá)的一側(cè)代表底盤(pán)小車(chē)的車(chē)頭。雷達(dá)左側(cè)的步進(jìn)電機(jī) ?

控制小車(chē)的X軸方向；雷達(dá)的右側(cè)的步進(jìn)電機(jī)控制小車(chē)的Y軸方向；雷達(dá)對(duì)側(cè)的步進(jìn)電機(jī)控制小車(chē)的Z軸方向，具體的接線引腳見(jiàn)下圖）

第二步：將主控板的擴(kuò)展板跟樹(shù)莓派連接起來(lái)（見(jiàn)圖中藍(lán)色的線），并用鋰電池進(jìn)行供電。

第三步：將鼠標(biāo)、鍵盤(pán)、顯示屏與樹(shù)莓派進(jìn)行連接（見(jiàn)下圖）。其中顯示屏和樹(shù)莓派用充電寶供電，鼠標(biāo)、鍵盤(pán)這里用的是無(wú)線的。

第四步：將雷達(dá)與樹(shù)莓派進(jìn)行連接（建議把雷達(dá)連接在樹(shù)莓派的usb3.0上，見(jiàn)下圖）。

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

3. 功能實(shí)現(xiàn)

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

• 仿真系統(tǒng)：基于開(kāi)源機(jī)器人操作系統(tǒng)ROS melodic和開(kāi)源軟件平臺(tái)Arduino開(kāi)發(fā)，上位機(jī)采用ROS melodic，基于Rviz完成全向移動(dòng)底盤(pán)小車(chē)slam導(dǎo)航運(yùn)動(dòng)規(guī)劃，采用gazebo完成全向移動(dòng)底盤(pán)小車(chē)物理運(yùn)動(dòng)仿真；下位機(jī)采用Arduino實(shí)現(xiàn)對(duì)全向移動(dòng)底盤(pán)小車(chē)運(yùn)動(dòng)的控制。

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

3.1 鍵盤(pán)控制底盤(pán)小車(chē)運(yùn)動(dòng)

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

? ? ? 當(dāng)按下鍵盤(pán)上指定的鍵時(shí)，可以實(shí)現(xiàn)三輪全向底盤(pán)小車(chē)前進(jìn)、后退、轉(zhuǎn)向、平移；還可以靈活的設(shè)置底盤(pán)小車(chē)的角速度、線速度來(lái)調(diào)整底盤(pán)小車(chē)的運(yùn)動(dòng)。下面是本實(shí)驗(yàn)中規(guī)劃的控制底盤(pán)小車(chē)的鍵盤(pán)命令（見(jiàn)下表），可以先熟悉一下，后續(xù)在控制底盤(pán)小車(chē)時(shí)會(huì)用到。

（2）操作步驟

① 打開(kāi)參考例程文件夾（src\Arduino_Programing\Control_FuLaiLun_Car），下載程序Control_FuLaiLun_Car.ino：

② 啟動(dòng)雷達(dá)、鍵盤(pán)、rosserial程序包。打開(kāi)終端,輸入roslaunch robot_navigation_control robot.launch命令（見(jiàn)下圖），等待程序的運(yùn)行啟動(dòng)界面。

成功啟動(dòng)后，可以在終端看到底盤(pán)小車(chē)的當(dāng)前speed（線速度）為0.04，turn（角速度）為0.08?！咀⒁猓簊peed指的是線速度的大?。ò较颍?，turn指的是角速度的大小（包含方向）】

啟動(dòng)后的界面（見(jiàn)下圖）:

③ 嘗試按下鍵盤(pán)命令，控制底盤(pán)小車(chē)的運(yùn)動(dòng)。【注意：一、請(qǐng)先保證終端是激活狀態(tài)，見(jiàn)上圖；二、按鍵盤(pán)時(shí)，稍微有點(diǎn)間隔，這樣能給底盤(pán)小車(chē)轉(zhuǎn)動(dòng)的時(shí)間；三、一定要打開(kāi)擴(kuò)展板上的電源開(kāi)關(guān)，否則會(huì)發(fā)現(xiàn)小車(chē)位置在rviz中位置雖然變化，但車(chē)不會(huì)運(yùn)動(dòng)】

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

更多命令請(qǐng)自行嘗試。

3.2 底盤(pán)小車(chē)同步定位與建圖-SLAM

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

? ? ? 利用Gmapping算法對(duì)底盤(pán)小車(chē)所在的未知環(huán)境進(jìn)行建立地圖、同步定位、最后保存此地圖。此地圖可供后續(xù)底盤(pán)小車(chē)導(dǎo)航使用。

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

位于中心的是我們運(yùn)行的slam_gmapping節(jié)點(diǎn)，這個(gè)節(jié)點(diǎn)負(fù)責(zé)整個(gè)gmapping SLAM的工作。它的輸入需要有兩個(gè)：

輸入

? ? ?/tf以及/tf_static：坐標(biāo)變換，類(lèi)型為第一代的tf/tfMessage或第二代的 tf2_msgs/TFMessage 其中一定得提供的有兩個(gè)tf，一個(gè)是base_frame 與 laser_frame 之間的tf，即機(jī)器人底盤(pán)和激光雷達(dá)之間的變換；一個(gè)是base_frame 與 odom_frame之間的tf，即底盤(pán)和里程計(jì)原點(diǎn)之間的坐標(biāo)變換。odom_frame可以理解為里程計(jì)原點(diǎn)所在的坐標(biāo)系。

????? /scan：激光雷達(dá)數(shù)據(jù)，類(lèi)型為sensor_msgs/LaserScan

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

輸出

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

? ? ? /slam_gmapping/entropy：std_msgs/Float64 類(lèi)型，反映了機(jī)器人位姿估計(jì)的分 散程度

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

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

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

（2）操作步驟

① 先搭建一個(gè)實(shí)際場(chǎng)景，便于建立地圖。【注意：為了方便識(shí)別，搭建的實(shí)際測(cè)試地圖最好是不規(guī)則的區(qū)域。如下圖測(cè)試的場(chǎng)地，在長(zhǎng)方形區(qū)域的一邊是不規(guī)則的，這樣便于識(shí)別建立的仿真地圖】

② 啟動(dòng)雷達(dá)、鍵盤(pán)、rosserial；打開(kāi)終端并輸入命令：roslaunch robot_navigation_control robot_car.launch （見(jiàn)下圖）

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

界面啟動(dòng)后，在rviz中選擇“map”，可以在可視化界面看到有地圖出現(xiàn)。下面根據(jù)實(shí)際環(huán)境進(jìn)行建圖：

【注意：一、請(qǐng)先保證鍵盤(pán)控制的終端是激活狀態(tài)（請(qǐng)用鼠標(biāo)點(diǎn)擊一下此終端）；二、按鍵盤(pán)時(shí)，稍微有點(diǎn)間隔，這樣能給底盤(pán)小車(chē)轉(zhuǎn)動(dòng)的時(shí)間】

按下鍵盤(pán)相關(guān)指令（鍵盤(pán)指令請(qǐng)參考3.1）來(lái)控制底盤(pán)小車(chē)在場(chǎng)地內(nèi)運(yùn)行，直至在Rviz內(nèi)可以看到構(gòu)建地圖的輪廓，如下圖所示（注意：地圖環(huán)境會(huì)根據(jù)實(shí)際場(chǎng)景建立）。

④ 為了后續(xù)在已知環(huán)境中運(yùn)行底盤(pán)小車(chē)，需把此次建立的地圖進(jìn)行保存。需要先進(jìn)入保存地圖的文件夾，然后給地圖命名，保存即可。例如我們把此次建立的地圖命名為your_map_name。【提示：your_map_name為自定義的名稱，您也可以按自己的需求進(jìn)行命名】

下面是本實(shí)驗(yàn)中的具體操作

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

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

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

查看生成的地圖文件：

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

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

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

打開(kāi)your_map_name.pgm，見(jiàn)下圖：

your_map_name.yaml ，各個(gè)參數(shù)見(jiàn)下圖。

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

3.3 底盤(pán)小車(chē)導(dǎo)航-Navigation

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

? ? ? 在上述3.2搭建好的實(shí)際場(chǎng)景中，實(shí)現(xiàn)三輪全向底盤(pán)小車(chē)的導(dǎo)航效果。? ? ?

• 具體思路：加載上述3.2建好的地圖，設(shè)置底盤(pán)小車(chē)的初始位置和方向，設(shè)定目標(biāo)位置及方向，底盤(pán)小車(chē)就可以實(shí)現(xiàn)導(dǎo)航效果。? ? ?

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

在ROS中進(jìn)行導(dǎo)航需要使用到的三個(gè)包是：

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

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

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

基于上圖，我們來(lái)看看底盤(pán)簡(jiǎn)單對(duì)應(yīng)關(guān)系。

（2）操作步驟

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

這里我們需要修改four_macnum_navigation.launch文件，把上述3.2的your_map_name.yaml 添加到此launch文件中。步驟如下：

打開(kāi)終端并輸入：cd fourmacnum_car_ws/src/four_macnum_navigation/launch

之后再輸入：gedit four_macnum_navigation.launch

② 啟動(dòng)雷達(dá)、鍵盤(pán)、rosserial。

在該終端繼續(xù)輸入：roslaunch robot_navigation_control robot_car.launch（見(jiàn)下圖）

③ 按下（ctrl+shift+T）打開(kāi)第二個(gè)終端并輸入：roslaunch four_macnum_navigation four_macnum_navigation.launch（見(jiàn)下圖）

啟動(dòng)界面后，可以看到許多紅色的箭頭，代表底盤(pán)小車(chē)的姿態(tài)估計(jì)還不準(zhǔn)確。

④ 開(kāi)始調(diào)整底盤(pán)小車(chē)的姿態(tài)。

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

第二步：使用2D Pose Estimate 標(biāo)定底盤(pán)小車(chē)位于地圖中的初始位置及車(chē)頭指向（見(jiàn)下圖）。

第三步：鼠標(biāo)點(diǎn)擊有roslaunch robot_navigation_control robot_car.launch的終端后，嘗試按下鍵盤(pán)命令控制底盤(pán)小車(chē)運(yùn)動(dòng)（鍵盤(pán)命令請(qǐng)參考3.1），最好盡可能多的消除地圖中的箭頭【這一步實(shí)際是確定底盤(pán)小車(chē)在地圖中的實(shí)際位置以及車(chē)頭指向】。下圖是多次按下鍵盤(pán)命令控制底盤(pán)小車(chē)運(yùn)動(dòng)后，箭頭逐漸減少的效果。

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

嘗試給定三輪全向底盤(pán)小車(chē)目標(biāo)位置。使用2D Nav Goal，這里的目標(biāo)位置包含了目標(biāo)點(diǎn)及車(chē)頭指向，在可視化界面點(diǎn)擊且轉(zhuǎn)動(dòng)箭頭防線，機(jī)器人會(huì)移動(dòng)到指定點(diǎn)的位置。

4. 資料下載

資料內(nèi)容：

①slam導(dǎo)航-例程源代碼

②slam導(dǎo)航-樣機(jī)3D文件

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

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