【自己動手做一臺SLAM導(dǎo)航機(jī)器人】第六章：SLAM建圖與自主避障導(dǎo)航
本專欄目錄
第六章：SLAM建圖與自主避障導(dǎo)航
第七章：語音交互與自然語言處理
附錄A：用于ROS機(jī)器人交互的Android手機(jī)APP開發(fā)
附錄B：用于ROS機(jī)器人管理調(diào)度的后臺服務(wù)器搭建
附錄C：如何選擇ROS機(jī)器人平臺進(jìn)行SLAM導(dǎo)航入門
視頻教程
https://www.bilibili.com/video/BV1jS4y1a7Lz
通過前面的基礎(chǔ)學(xué)習(xí)，本章進(jìn)入最為激動的機(jī)器人自主導(dǎo)航的學(xué)習(xí)。在前面的學(xué)習(xí)鋪墊后，終于迎來了最大樂趣的時刻，就是賦予我們的miiboo機(jī)器人能自由行走的生命。本章將圍繞機(jī)器人SLAM建圖、導(dǎo)航避障、巡航、監(jiān)控等內(nèi)容展開。本章內(nèi)容：
1.在機(jī)器人上使用傳感器
2.google-cartographer機(jī)器人SLAM建圖
3.ros-navigation機(jī)器人自主避障導(dǎo)航
4.多目標(biāo)點(diǎn)導(dǎo)航及任務(wù)調(diào)度
5.機(jī)器人巡航與現(xiàn)場監(jiān)控
下面這本書是本篇文章的參考文獻(xiàn)，大家有需要可以入手一本：
1.在機(jī)器人上使用傳感器
SLAM建圖需要用到底盤、激光雷達(dá)和IMU，所以這里詳細(xì)介紹如何在機(jī)器人上使用這些傳感器。要使用這些傳感器也很簡單，就是在機(jī)器人上開啟相應(yīng)傳感器的ROS驅(qū)動節(jié)點(diǎn)，在設(shè)置合適的可配參數(shù)就行了。
需要注意的是，底盤、激光雷達(dá)、IMU這三個傳感器都使用串口與樹莓派通信，為了防止每次開機(jī)這三個設(shè)備的串口號發(fā)生變動，需要將串口號進(jìn)行綁定與重映射，操作方法在前面已經(jīng)介紹過了，如果還沒有綁定直接前往前面相關(guān)內(nèi)容參考。
這里將建立一個叫catkin_ws的ROS工作空間，專門用于存放機(jī)器人傳感器相關(guān)的ROS驅(qū)動功能包。關(guān)于創(chuàng)建ROS工作空間的操作，請參考前面相應(yīng)部分內(nèi)容，這里就不做講解。
1.1.使用底盤
在機(jī)器人上只需要使用miiboo這個驅(qū)動包就可以驅(qū)動底盤了。將miiboo這個驅(qū)動包拷貝到~/catkin_ws/src/中，編譯后就可以使用了。miiboo驅(qū)動包文件結(jié)構(gòu)，如圖1。miiboo驅(qū)動包中含有兩個ROS功能包miiboo_bringup和miiboo_description，驅(qū)動miiboo底盤、底盤PID整定、里程計標(biāo)定這些功能包含在miiboo_bringup中，miiboo底盤urdf模型包含在miiboo_description中。
底盤控制可配參數(shù)：
關(guān)于底盤控制可配參數(shù)都放在miiboo_bringup/launch/minimal.launch中，如圖2。
參數(shù)com_port是底盤控制的串口號，由于前面已經(jīng)做了綁定，所以直接填入綁定好的名稱/dev/miiboo就行了；
參數(shù)speed_ratio是里程計走直線標(biāo)定值，這個值通過標(biāo)定得到。
參數(shù)wheel_distance是里程計轉(zhuǎn)角標(biāo)定值，這個值通過標(biāo)定得到。
其余參數(shù)一般不需要修改，如有需要可以結(jié)合閱讀源碼來了解參數(shù)含有和做相應(yīng)修改。
驅(qū)動miiboo底盤：
其實(shí)很簡單，一條命令啟動miiboo底盤控制。
底盤PID整定：
我們的miiboo機(jī)器人底盤的stm32控制板中已經(jīng)內(nèi)置了整定好的PID參數(shù)，如果選用我們提供的控制板和電機(jī)，一般情況下是不需要整定PID的。
對于想體驗(yàn)一下PID參數(shù)整定過程或?qū)⑽覀兊膍iiboo機(jī)器人底盤的stm32控制板應(yīng)用到其他地方的朋友，這里給出了整定PID的整個操作過程和思路，方便大家學(xué)習(xí)和更深層次的研究。這里主要講解PID整定的操作，關(guān)于原理性的東西可以參考前面相關(guān)內(nèi)容進(jìn)行了解。
由于底盤PID整定是非必須的功能，所以沒有對底盤PID整定的串口（DEBUG-uart1）做綁定，需要先手動插入該串口到樹莓派3，然后手動查看該串口的設(shè)備號，并修改該設(shè)備號的可讀寫權(quán)限。然后將該設(shè)備號填入miiboo_bringup/launch/pid_set.launch中的com_port參數(shù)中。然后，需要啟動底盤控制節(jié)點(diǎn)、底盤調(diào)試節(jié)點(diǎn)、鍵盤控制節(jié)點(diǎn)。鍵盤控制節(jié)點(diǎn)teleop_twist_keyboard需要通過apt-get命令來安裝，rqt_plot是ROS提供的繪圖工具。
ROS提供的繪圖工具rqt_plot用法很簡單，在rqt_plot界面中，在Topic欄輸入曲線數(shù)據(jù)來源，我們這里為左、右輪速度，然后點(diǎn)擊旁邊的“+”將曲線加入繪制界面，人如圖3。
然后，通過在啟動teleop_twist_keyboard節(jié)點(diǎn)的終端通過I/</J/L四個按鍵來控制底盤前進(jìn)/后退/左轉(zhuǎn)/右轉(zhuǎn)控制，并觀察速度曲線的變化，根據(jù)PID整定規(guī)則對PID參數(shù)進(jìn)行整定，在啟動 pid_set.launch的終端下按相應(yīng)提示輸入PID參數(shù)實(shí)現(xiàn)對整定參數(shù)的編輯。直到得到一個比較好的速度曲線，就可以結(jié)束整定過程了。實(shí)時速度曲線顯示，如圖4。
里程計標(biāo)定：
機(jī)器人底盤運(yùn)行的精度是衡量底盤的重要指標(biāo)。底盤精度受里程計的走直線誤差和轉(zhuǎn)角誤差影響。因此，需要對里程計的走直線和轉(zhuǎn)角進(jìn)行標(biāo)定，盡量減小誤差。miiboo機(jī)器人底盤的ROS驅(qū)動中已經(jīng)寫好了相應(yīng)的標(biāo)定程序，跟里程計標(biāo)定有關(guān)的文件主要有：
.../miiboo_bringup/launch/check_linear.launch為里程計走直線標(biāo)定啟動文件
.../miiboo_bringup/launch/check_angular.launch為里程計轉(zhuǎn)角標(biāo)定啟動文件
.../miiboo_bringup/launch/minimal.launch為設(shè)置標(biāo)定參數(shù)及底盤控制啟動文件
下面是標(biāo)定步驟過程。
由于標(biāo)定過程在前面已經(jīng)講解過了，請直接前往相應(yīng)內(nèi)容參考。
miiboo底盤urdf模型：
urdf模型描述了機(jī)器人底盤的形狀、傳感器之間的安裝關(guān)系、各個傳感器在tf tree中的關(guān)系。其實(shí)，miiboo底盤urdf模型的主要作用是提供各個傳感器在tf tree中的關(guān)系，這些關(guān)系將在SLAM和導(dǎo)航算法中被使用。
圖5是miiboo機(jī)器人底盤中各個傳感器tf關(guān)系，base_footprint是底盤的運(yùn)動中心，base_laser_link是激光雷達(dá)的中心，imu_link為IMU模塊的中心。以base_footprint為原點(diǎn)，建立機(jī)器人底盤的坐標(biāo)系，坐標(biāo)系為標(biāo)準(zhǔn)右手系，即底盤正前方為x軸、正左方為y軸、正上方為z軸、以x軸起始逆時針方向?yàn)閠heta軸。以base_footprint為父坐標(biāo)系，建立base_footprint->base_laser_link關(guān)系，建立base_footprint->imu_link關(guān)系，就實(shí)現(xiàn)了各個傳感器tf關(guān)系的構(gòu)建，構(gòu)建的具體實(shí)現(xiàn)在miiboo_description/urdf/miiboo.urdf中完成。如圖6，為miiboo.urdf的具體內(nèi)容。
要使用這個urdf模型就很簡單了，直接一句命令啟動。
1.2.使用激光雷達(dá)
在機(jī)器人上只需要使用ydlidar這個驅(qū)動包就可以驅(qū)動ydlidar-x4雷達(dá)了。將ydlidar這個驅(qū)動包拷貝到~/catkin_ws/src/中，編譯后就可以使用了。ydlidar驅(qū)動包文件結(jié)構(gòu)，如圖7。ydlidar驅(qū)動包中的其他文件我們不需要關(guān)心，這些都是由雷達(dá)廠商提供的標(biāo)準(zhǔn)驅(qū)動，只需要設(shè)置我們自己建立的ydlidar/launch/my_x4.launch文件，這個用于啟動雷達(dá)。
激光雷達(dá)數(shù)據(jù)可配參數(shù)：
關(guān)于激光雷達(dá)數(shù)據(jù)可配參數(shù)都放在ydlidar/launch/my_x4.launch中，如圖8。
參數(shù)port是激光雷達(dá)的串口號，由于前面已經(jīng)做了綁定，所以直接填入綁定好的名稱/dev/lidar就行了；
參數(shù)range_min和range_max是設(shè)置激光雷達(dá)數(shù)據(jù)的有效值區(qū)間。
其余參數(shù)一般不需要修改，如有需要可以結(jié)合閱讀源碼來了解參數(shù)含有和做相應(yīng)修改。
驅(qū)動ydlidar-x4激光雷達(dá)：
其實(shí)很簡單，一條命令啟動ydlidar-x4激光雷達(dá)。
激光雷達(dá)數(shù)據(jù)格式：
激光雷達(dá)采用右手坐標(biāo)系，雷達(dá)正前方為x軸、正左方為y軸、正上方為z軸、以x軸起始逆時針方向?yàn)閠heta軸。激光雷達(dá)的掃描數(shù)據(jù)以極坐標(biāo)的形式表示，雷達(dá)正前方是極坐標(biāo)0度方向、雷達(dá)正左方是極坐標(biāo)90度方向，紅色點(diǎn)為掃描到的數(shù)據(jù)點(diǎn)，如圖9所示。
激光雷達(dá)的數(shù)據(jù)在ROS中是以sensor_msgs/LaserScan消息類型進(jìn)行表示，如圖10，angle_increment表示激光數(shù)據(jù)點(diǎn)的極坐標(biāo)遞增角度，ranges數(shù)組存放實(shí)際的極坐標(biāo)點(diǎn)距離值。
1.3.使用IMU
在機(jī)器人上只需要使用miiboo_imu這個驅(qū)動包就可以驅(qū)動mpu9250模塊了。將miiboo_imu這個驅(qū)動包拷貝到~/catkin_ws/src/中，編譯后就可以使用了。miiboo_imu驅(qū)動包文件結(jié)構(gòu)，如圖11。ydlidar驅(qū)動包中的其他文件我們不需要關(guān)心，只需要設(shè)置ydlidar/launch/my_x4.launch文件，這個用于啟動IMU。
IMU數(shù)據(jù)可配參數(shù)：
關(guān)于IMU數(shù)據(jù)可配參數(shù)都放在miiboo_imu/launch/imu.launch中，如圖12。
參數(shù)come_port是IMU的串口號，由于前面已經(jīng)做了綁定，所以直接填入綁定好的名稱/dev/imu就行了；
其余參數(shù)一般不需要修改，如有需要可以結(jié)合閱讀源碼來了解參數(shù)含有和做相應(yīng)修改。
驅(qū)動IMU模塊：
其實(shí)很簡單，一條命令啟動IMU模塊。
IMU數(shù)據(jù)格式：
IMU模塊采用右手坐標(biāo)系，IMU模塊正前方為x軸、正左方為y軸、正上方為z軸。IMU模塊提供3軸加速度、3軸角速度、3軸磁力計、經(jīng)數(shù)據(jù)融合后用歐拉角表示的姿態(tài)。
IMU數(shù)據(jù)在ROS中是以sensor_msgs/Imu消息類型進(jìn)行表示，如圖13。
1.4.使用攝像頭
miiboo機(jī)器人上使用的是USB攝像頭，用ROS驅(qū)動USB攝像頭可以采用以下3中方法。
方法1：
使用usb_cam這個ROS包直接驅(qū)動
方法2：
使用gscam這個ROS包直接驅(qū)動
方法3：
自制OpenCV，cv_bridge，image_transport驅(qū)動ROS包
為了方便起見，我采用的是方法1，直接安裝usb_cam這個ROS包直接驅(qū)動。
usb_cam攝像頭驅(qū)動安裝：
將usb_cam下載到~/catkin_ws/src/中，直接編譯就行了。
攝像頭數(shù)據(jù)可配參數(shù)：
關(guān)于攝像頭數(shù)據(jù)可配參數(shù)都放在usb_cam/launch/usb_cam.launch中，如圖14。
參數(shù)video_device是攝像頭的設(shè)備號，由于直插了一個USB攝像頭，所以直接填入名稱/dev/video0就行了；
其余參數(shù)一般不需要修改，如有需要可以結(jié)合閱讀源碼來了解參數(shù)含有和做相應(yīng)修改。
驅(qū)動USB攝像頭：
其實(shí)很簡單，一條命令啟動USB攝像頭。
攝像頭遠(yuǎn)程顯示：
攝像頭數(shù)據(jù)遠(yuǎn)程顯示的方法有兩種，方法一是在PC端rviz中訂閱攝像頭發(fā)布的圖像topic，方法二是用Android手機(jī)上miiboo機(jī)器人APP直接顯示。
先說方法一，在PC端打開rviz，在rviz中添加需要顯示的Topic，這樣就可以看到圖像了。如圖15和16。
方法二，就很簡單了，只要Android手機(jī)上miiboo機(jī)器人APP連接到機(jī)器人端成功后，就能自動顯示圖像了。如圖17。
1.5.局域網(wǎng)內(nèi)廣播機(jī)器人自己的IP
這個很簡單，由broadcast_ip這個功能包實(shí)現(xiàn)，我已經(jīng)寫好放入~/catkin_ws/src/并編譯了。只需要一句命令啟動就行了。
2.google-cartographer機(jī)器人SLAM建
圖
主流的激光SLAM算法有hector、gmapping、karto、cartographer。
hector是一種結(jié)合了魯棒性較好的掃描匹方法2D_SLAM方法和使用慣性傳感系統(tǒng)的導(dǎo)航技術(shù)。傳感器的要求較高，高更新頻率小測量噪聲的激光掃描儀，不需要里程計。使空中無人機(jī)與地面小車在不平坦區(qū)域運(yùn)行存在運(yùn)用的可能性。作者利用現(xiàn)代激光雷達(dá)的高更新率和低距離測量噪聲，通過掃描匹配實(shí)時地對機(jī)器人運(yùn)動進(jìn)行估計。所以當(dāng)只有低更新率的激光傳感器時，即便測距估計很精確，對該系統(tǒng)都會出現(xiàn)一定的問題。
gmapping是一種基于粒子濾波的激光SLAM算法，它已經(jīng)集成在ROS中，是移動機(jī)器人中使用最多的SLAM算法?；诹Ｗ訛V波的算法用許多加權(quán)粒子表示路徑的后驗(yàn)概率，每個粒子都給出一個重要性因子。但是，它們通常需要大量的粒子才能獲得比較好的的結(jié)果，從而增加該算法的的計算復(fù)雜性。此外，與PF重采樣過程相關(guān)的粒子退化耗盡問題也降低了算法的準(zhǔn)確性。
karto是基于圖優(yōu)化的SLAM算法，用高度優(yōu)化和非迭代cholesky矩陣進(jìn)行稀疏系統(tǒng)解耦作為解。圖優(yōu)化方法利用圖的均值表示地圖，每個節(jié)點(diǎn)表示機(jī)器人軌跡的一個位置點(diǎn)和傳感器測量數(shù)據(jù)集，箭頭的指向的連接表示連續(xù)機(jī)器人位置點(diǎn)的運(yùn)動，每個新節(jié)點(diǎn)加入，地圖就會依據(jù)空間中的節(jié)點(diǎn)箭頭的約束進(jìn)行計算更新。路標(biāo)landmark越多,內(nèi)存需求越大,然而圖優(yōu)化方式相比其他方法在大環(huán)境下制圖優(yōu)勢更大。
cartographer是google開發(fā)的實(shí)時室內(nèi)SLAM項目，cartographer采用基于google自家開發(fā)的ceres非線性優(yōu)化的方法，cartographer的量點(diǎn)在于代碼規(guī)范與工程化，非常適合于商業(yè)應(yīng)用和再開發(fā)。并且cartographer基于submap子圖構(gòu)建全局地圖的思想，能有效的避免建圖過程中環(huán)境中移動物體的干擾。并且cartographer支持多傳感器數(shù)據(jù)（odometry、IMU、LaserScan等）建圖，支持2D_SLAM和3D_SLAM建圖。所以，我果斷采用cartographer來建圖，我的樹莓派3主板跑cartographer實(shí)時建圖是十分的流暢，這一點(diǎn)很欣慰^_^
2.1.google-cartographer建圖算法原理分析
cartographer采用的是主流的SLAM框架，也就是特征提取、閉環(huán)檢測、后端優(yōu)化的三段式。由一定數(shù)量的LaserScan組成一個submap子圖，一系列的submap子圖構(gòu)成了全局地圖。用LaserScan構(gòu)建submap的短時間過程累計誤差不大，但是用submap構(gòu)建全局地圖的長時間過程就會存在很大的累計誤差，所以需要利用閉環(huán)檢測來修正這些submap的位置，閉環(huán)檢測的基本單元是submap，閉環(huán)檢測采用scan_match策略。cartographer的重點(diǎn)內(nèi)容就是融合多傳感器數(shù)據(jù)（odometry、IMU、LaserScan等）的submap子圖創(chuàng)建以及用于閉環(huán)檢測的scan_match策略的實(shí)現(xiàn)。
2.2.cartographer_ros安裝
我們直接參考google-cartographer官方教程安裝就行，官方教程分為cartographer和cartographer_ros，其實(shí)cartographer就是核心算法層、cartographer_ros是核心算法層的ros調(diào)用層。官方教程如下：
https://google-cartographer.readthedocs.io/en/latest/index.html#
https://google-cartographer-ros.readthedocs.io/en/latest/index.html#
直接按照第二個鏈接cartographer_ros的安裝教程，就可將cartographer_ros、cartographer、以及各種依賴都安裝了。不過特別說明一點(diǎn)，為了解決從官網(wǎng)下載ceres-solver速度慢的問題，我將ceres-solver的下載地址換到了github源；我需要將官方教程中生成的src/.rosinstall替換成了自己的內(nèi)容，如圖19。其余安裝過程和官方教程一模一樣。
（1）安裝編譯工具
我來編譯cartographer_ros，我們需要用到wsool和rosdep。為了加快編譯，我們使用ninja工具進(jìn)行編譯。
（2）創(chuàng)建存放cartographer_ros的專門工作空間
特別說明，在執(zhí)行wstool update -t src之前，需要將src/.rosinstall文件修改成以下內(nèi)容,以解決ceres-solver下載不了的問題，如圖19。
（3）安裝依賴項
安裝cartographer_ros的依賴項proto3、deb包等。如果執(zhí)行sudo rosdep init
報錯，可以直接忽略。
（4）編譯和安裝
上面的配置和依賴都完成后，就可以開始編譯和安裝cartographer_ros整個項目工程了。
特別提醒，以后對cartographer_ros中的配置文件或源碼有改動時，都需要執(zhí)行這個編譯命令使修改生效。
2.3.cartographer_ros使用
cartographer_ros整體代碼結(jié)構(gòu)分析：
最頂層的是cartographer_ros，作為rosj接口調(diào)用層，通過調(diào)用cartographer核心算法，訂閱多傳感器數(shù)據(jù)（/scan、/imu、/odom等），并發(fā)布地圖、機(jī)器人位置信息（/map、/tf等）；其次是cartographer，作為SLAM算法的核心實(shí)現(xiàn)，特征提取、子圖構(gòu)建、閉環(huán)檢測、全局優(yōu)化都在這里實(shí)現(xiàn)，其中優(yōu)化過程需要調(diào)用ceres-solver非線性優(yōu)化庫；最后是ceres-solver，是非線性優(yōu)化庫，用于求解SLAM中的優(yōu)化問題。
在miiboo機(jī)器人上用cartographer_ros多傳感器建圖進(jìn)行配置：
經(jīng)過前面對cartographer_ros進(jìn)行安裝后，我們肯定迫不及待想在實(shí)際的miiboo機(jī)器人上使用cartographer_ros進(jìn)行SLAM建圖了。為了最大限度的提高SLAM建圖的性能，我們的miiboo機(jī)器人提供了激光雷達(dá)、IMU、輪式里程計（/scan、/imu、/odom）這三種傳感器的數(shù)據(jù)，所以我們需要先將cartographer_ros配置成對應(yīng)的工作模式。
cartographer算法是一個非常通用和適應(yīng)不同平臺的開放框架算法，所以支持多種配置與工作模式。我們就來看看cartographer_ros如何進(jìn)行配置。配置文件由*.lua書寫被放在路徑cartographer_ros/configuration_files/，我們需要建立一個我們自己的配置文件，取名就叫miiboo_mapbuild.lua吧，具體內(nèi)容如圖21。由于我們的miiboo機(jī)器人采用激光雷達(dá)、IMU、 輪式里程計三種傳感器融合建圖，所以以下參數(shù)一定要設(shè)置正確：
參數(shù)tracking_frame設(shè)置為imu_link，因?yàn)槲覀兪褂?imu的數(shù)據(jù)；
參數(shù)published_frame設(shè)置為odom，因?yàn)槲覀兪褂?odom的數(shù)據(jù)；
參數(shù)provide_odom_frame設(shè)置為false，因?yàn)槲覀兪褂猛獠?odom，所以這里不需要內(nèi)部提供；
參數(shù)use_odometry設(shè)置為true，因?yàn)槲覀兪褂猛獠?odom的數(shù)據(jù)；
參數(shù)use_imu_data設(shè)置為true，因?yàn)槲覀兪褂?imu的數(shù)據(jù)；
參數(shù)imu_gravity_time_constant設(shè)置為10，這個是IMU的重力加速度常數(shù)。
其余參數(shù)根據(jù)需要自行調(diào)整，由于cartographer是發(fā)展很迅速的算法，所以很多代碼和文檔一直在更新，所以參考官方文檔來解讀這些參數(shù)的含義是最好的選擇，官方文檔連接地址我貼在下面了。
https://google-cartographer-ros.readthedocs.io/en/latest/index.html
然后需要配置*.launch文件，我們給miiboo機(jī)器人建立啟動文件取名叫miiboo_mapbuild.launch，存放路徑在cartographer_ros/launch/里面，具體內(nèi)容如圖22。
不難發(fā)現(xiàn)launch文件中包含三個node啟動項，即urdf模型啟動項、cartographer_node啟動項、cartographer_occupancy_grid_node啟動項。
第一個啟動項是啟動urdf模型，這個接口是提供給那些只使用cartographer單獨(dú)建圖的應(yīng)用場景，由于我們miiboo機(jī)器人建立完地圖后還需要繼續(xù)進(jìn)行自動導(dǎo)航任務(wù)，所以我們使用miiboo底盤提供的urdf模型，而不使用這里的urdf模型，所以這個啟動項被注釋掉了，這樣建圖和導(dǎo)航就更容易管理。
第二個啟動項是啟動cartographer_node建圖節(jié)點(diǎn)，這個是SLAM建圖主節(jié)點(diǎn)，我們建立的配置miiboo_mapbuild.lua將在這里被載入，同時這里可以對建圖輸入數(shù)據(jù)scan、imu、odom的topic名稱做重映射。
第三個啟動項是啟動cartographer_occupancy_grid_node地圖格式轉(zhuǎn)換節(jié)點(diǎn)，由于cartographer_node建圖節(jié)點(diǎn)提供的地圖是submapList格式的，需要轉(zhuǎn)換成GridMap格式才能在ROS中顯示和使用。這里面有兩個可配參數(shù)，resolution用來設(shè)置GridMap地圖的分辨率，publish_period_sec用來設(shè)置GridMap地圖發(fā)布的頻率。
配置參數(shù)修改好后，不要忘了再編譯一次整個catkin_ws_carto工作空間，切換到catkin_ws_carto目錄，執(zhí)行下面的編譯命令。
啟動cartographer_ros建圖：
要在miiboo機(jī)器人上，啟動cartographer_ros建圖，分為這幾個步驟：啟動機(jī)器人上的各個傳感器、啟動cartographer_ros、在PC端啟動鍵盤控制機(jī)器人運(yùn)動并啟動rviz觀察地圖（或者在Android手機(jī)端用miiboo機(jī)器人APP控制機(jī)器人運(yùn)動和觀察地圖）。
首先，啟動機(jī)器人上的各個傳感器，為了操作方便，我已經(jīng)將要啟動的傳感器都寫入miiboo_bringup/launch/miiboo_all_sensor.launch這個啟動文件了，文件內(nèi)容如圖23。這個啟動文件包含機(jī)器人urdf啟動項、miiboo底盤啟動項、激光雷達(dá)啟動項、IMU啟動項、攝像頭啟動項、廣播IP啟動項。
打開終端，通過下面的命令直接啟動就行了。
然后，啟動cartographer_ros，由于前面已經(jīng)做好了相應(yīng)的配置，所以直接使用命令啟動就行了。
這里給個小提示，為了查看cartographer_ros建圖算法有沒有正常開始工作，我們可以用rosrun rqt_tf_tree rqt_tf_tree查看整個tf樹的結(jié)構(gòu)，正常的tf樹如圖24。map->odom之間的關(guān)系由cartographer建圖節(jié)點(diǎn)提供，odom->base_footprint之間的關(guān)系由miiboo底盤的輪式里程計提供，base_footprint->imu_link和base_link和base_laser_link之間的關(guān)系由miiboo機(jī)器人的urdf模型提供。從tf樹不難看出整個建圖過程中機(jī)器人定位的實(shí)現(xiàn)原理，cartographer建圖節(jié)點(diǎn)通過維護(hù)map->odom之間的關(guān)系最終實(shí)現(xiàn)全局定位，miiboo底盤的輪式里程計通過維護(hù)odom->base_footprint之間的關(guān)系來實(shí)現(xiàn)局部定位，傳感器之間的安裝關(guān)系由urdf模型提供，這個靜態(tài)關(guān)系主要用于多傳感器數(shù)據(jù)融合。
最后，在PC端啟動鍵盤控制機(jī)器人運(yùn)動并啟動rviz觀察地圖（或者在Android手機(jī)端用miiboo機(jī)器人APP控制機(jī)器人運(yùn)動和觀察地圖）。如果用PC端控制和觀察，啟動命令如下。
在PC端打開一個新終端，運(yùn)行rviz啟動命令。
在rviz窗口中添加訂閱/map，就可以看到建圖效果了，如圖25。
在PC端再打開一個新終端，運(yùn)行鍵盤控制啟動命令。
在該終端下，用鍵盤就可以控制機(jī)器人前進(jìn)、后退、左轉(zhuǎn)、右轉(zhuǎn)了。
如果是在Android手機(jī)端用miiboo機(jī)器人APP控制機(jī)器人運(yùn)動和觀察地圖，直接就能使用，如圖26。
保存cartographer_ros建圖結(jié)果：
當(dāng)我們在房間里面掃描一圈，地圖建立的差不多了，就可以將建圖結(jié)果保存下來了，cartographer_ros提供了將建圖結(jié)果保存為*.pbstream專門的方法，其實(shí)就是一條命令。
其實(shí)就是調(diào)用cartographer_ros提供的叫/write_state這個名字的服務(wù)，服務(wù)傳入?yún)?shù)/home/ubuntu/map/carto_map.pbstream為地圖的保存路徑。保存成功后，會返回相應(yīng)的狀態(tài)信息，如圖27。
地圖格式轉(zhuǎn)換：
由于用cartographer_ros提供的/write_state方法保存的地圖是*.pbstream的格式，而要在后續(xù)的自主導(dǎo)航中使用這個地圖，我們需要將其轉(zhuǎn)換為ROS中通用的GridMap格式。其實(shí)很簡單，cartographer_ros已經(jīng)跟我們提供了cartographer_pbstream_to_ros_map這個節(jié)點(diǎn)用于轉(zhuǎn)換的實(shí)現(xiàn)。所以，我們只需要寫一個啟動文件啟動這個節(jié)點(diǎn)就行了，我給這個啟動文件取名miiboo_pbstream2rosmap.launch，存放路徑是cartographer_ros/launch/，啟動文件的內(nèi)容如圖28。在使用這個啟動文件進(jìn)行啟動時，需要從外部傳入兩個參數(shù)，參數(shù)pbstream_filename為待轉(zhuǎn)換的*.pbstream文件路徑，參數(shù)map_filestem為轉(zhuǎn)換后存放結(jié)果的文件路徑。
配置參數(shù)修改好后，不要忘了再編譯一次整個catkin_ws_carto工作空間，切換到catkin_ws_carto目錄，執(zhí)行下面的編譯命令。
最后，就可以打開終端，使用啟動這個啟動文件，對地圖格式進(jìn)行轉(zhuǎn)換了，命令如下。
保存結(jié)束后，節(jié)點(diǎn)會自動退出，這時我們可以得到轉(zhuǎn)換后的地圖，轉(zhuǎn)換后的GridMap地圖由*.pgm和*.yaml兩部分構(gòu)成，這時標(biāo)準(zhǔn)的ROS格式地圖，可以被ROS導(dǎo)航框架中的map_server節(jié)點(diǎn)直接調(diào)用，轉(zhuǎn)換后的地圖結(jié)果如圖29。
3.ros-navigation機(jī)器人自主避障導(dǎo)航
前面的學(xué)習(xí)教程打好了必須的基礎(chǔ)，現(xiàn)在就正式開始探討ROS系統(tǒng)最強(qiáng)大的特性之一，讓我們的機(jī)器人能自主導(dǎo)航和避障。這都得益于開源社區(qū)和共享代碼，使ROS擁有大量可用的導(dǎo)航算法。將這些導(dǎo)航算法集大成者，便是ros-navigation導(dǎo)航功能包集。了解更多ros-navigation的信息，請參考官方wiki教程：http://wiki.ros.org/navigation/。
3.1.機(jī)器人自主避障導(dǎo)航原理分析
要分析導(dǎo)航功能包的原理，肯定需要先參考下面這張ros-navigation官方給出的系統(tǒng)框圖，如圖30。最中心的是move_base節(jié)點(diǎn)，是導(dǎo)航過程運(yùn)動控制的最終執(zhí)行機(jī)構(gòu)，move_base訂閱用戶發(fā)布的導(dǎo)航目標(biāo)move_base_simple/goal，并將實(shí)時運(yùn)動控制信號cmd_vel下發(fā)給底盤以實(shí)現(xiàn)最終運(yùn)動控制，move_base中的各種導(dǎo)航算法模塊都是以插件的形式進(jìn)行調(diào)用的，這樣可以很方便的替換不同的算法以適應(yīng)不同的應(yīng)用，其中g(shù)lobal_planner用于全局路徑規(guī)劃、local_planner用于局部路徑規(guī)劃、golobal_costmap是全局代價地圖用于描述全局環(huán)境信息、local_costmap是局部代價地圖用于描述局部環(huán)境信息、recovery_behaviors是恢復(fù)策略用于機(jī)器人碰到障礙后自動進(jìn)行逃離恢復(fù)。然后是amcl節(jié)點(diǎn)，amcl節(jié)點(diǎn)利用粒子濾波算法實(shí)現(xiàn)機(jī)器人的全局定位，為機(jī)器人導(dǎo)航提供全局位置信息。再然后是map_server節(jié)點(diǎn)，map_server節(jié)點(diǎn)通過調(diào)用前面SLAM建圖得到的地圖為導(dǎo)航提供環(huán)境地圖信息。最后就是要提供機(jī)器人模型相關(guān)的tf信息、里程計odom信息、激光雷達(dá)信息scan。
機(jī)器人全局定位amcl粒子濾波算法：
amcl的全稱是自適應(yīng)蒙特卡洛粒子濾波，這里通過講解粒子濾波、重要性采樣、機(jī)器人綁架、自適應(yīng)蒙特卡洛這幾個概念來說明機(jī)器人全局定位的原理。由于amcl的數(shù)學(xué)理論比較復(fù)雜，限于篇幅這里不展開講解，感興趣的朋友可以參考《Probabilistic Robotics》這本書，里面有詳細(xì)的推導(dǎo)過程，如圖31。
粒子濾波，是一種思想，比如要計算一個矩形里面一個不規(guī)則形狀的面積，這個問題不好直接計算，但是可以拿一把豆子均勻撒到矩形中，統(tǒng)計落在不規(guī)則形狀中豆子的占比就能算出其面積了。在機(jī)器人定位問題中，我們在地圖的任意位置撒上許多粒子點(diǎn)，然后通過傳感器觀測數(shù)據(jù)按照一定的評價方法對每個粒子點(diǎn)進(jìn)行打分，評分高的粒子點(diǎn)表示機(jī)器人有更大的可能在此位置；在下一輪撒點(diǎn)時，就在評分高的粒子點(diǎn)附近多撒一些點(diǎn)，這樣通過不斷的迭代，粒子點(diǎn)就會聚攏到一個地方。這個粒子點(diǎn)聚集的地方，就是機(jī)器人位置的最優(yōu)估計點(diǎn)。如圖32，紅色的粒子點(diǎn)慢慢聚攏到一團(tuán)。
重要性采樣，在粒子濾波的迭代過程中，評分高的粒子點(diǎn)會被下一輪迭代時更加看重，這樣不斷迭代真實(shí)估計值附近的粒子點(diǎn)會越來越多。
機(jī)器人綁架，當(dāng)機(jī)器人被突然從一個地方抱走到另一個地方，這個時候前一輪迭代得到的粒子點(diǎn)完全不能在新的位置上試用，這樣繼續(xù)迭代下去就會發(fā)生位置估計的錯誤。
自適應(yīng)蒙特卡洛，自適應(yīng)主要體現(xiàn)在兩個方面。通過判斷粒子點(diǎn)的平均分突變來識別機(jī)器人綁架問題，并在全局重新撒點(diǎn)來解決機(jī)器人綁架問題；通過判斷粒子點(diǎn)的聚集程度來確定位置估計是否準(zhǔn)確，在估計比較準(zhǔn)確的時候降低需要維護(hù)的粒子點(diǎn)數(shù)目，這樣來降低算法的計算開銷。
代價地圖costmap：
代價地圖用于描述環(huán)境中的障礙物信息，代價地圖利用激光雷達(dá)、聲吶、紅外測距等探測傳感器的數(shù)據(jù)來生成，大致的原理是通過建立不同的圖層Layer然后疊在一起，被填充的柵格點(diǎn)表示有障礙物，如圖33，想要了解代價地圖的更多細(xì)節(jié)可以閱讀《Layered Costmaps for Context-Sensitive Navigation》這篇論文。機(jī)器人導(dǎo)航中用到了兩個代價地圖，全局代價地圖global_costmap和局部代價地圖local_costmap。
路徑規(guī)劃：
機(jī)器人導(dǎo)航中核心的就是路徑規(guī)劃，路徑規(guī)劃是通過利用環(huán)境障礙物信息找到一條到達(dá)目標(biāo)并且開銷小的路勁。導(dǎo)航中會用到兩種路徑規(guī)劃，即全局路徑規(guī)劃global_planner和局部路徑規(guī)劃local_planner。全局路徑規(guī)劃更像是一種戰(zhàn)略性策略，需要考慮全局，規(guī)劃處一條盡量短并且易于執(zhí)行的路徑。在全局路徑的指導(dǎo)下，機(jī)器人在實(shí)際行走時還需要考慮周圍實(shí)時的障礙物并制定避讓策略，這就是局部路徑規(guī)劃要完成的事，可以說機(jī)器人的自主導(dǎo)航最終是由局部路徑規(guī)劃一步步完成的。ROS中推薦的全局路徑規(guī)劃器是global_planner，基于A*算法，如圖34。
由于ROS中的局部路徑規(guī)劃器比較老了，所以我推薦大家使用比較新的局部路徑規(guī)劃器teb_local_planner。teb_local_planner是基于彈性時間帶碰撞約束的算法，算法將動態(tài)障礙物、運(yùn)行時效、路徑平滑性等約束做綜合考慮，在復(fù)雜環(huán)境下有更優(yōu)秀的表現(xiàn)，如圖35。
3.2.ros-navigation安裝
ros-navigation的安裝有兩種方法，方法一是直接通過apt-get安裝編譯好的ros-navigation庫到系統(tǒng)中，方法二是下載ros-navigation源碼手動編譯安裝。由于后續(xù)可能需要對ros-navigation中的算法做修改和改進(jìn)，所以我采用方法二進(jìn)行安裝。
這里將建立一個叫catkin_ws_nav的ROS工作空間，專門用于存放機(jī)器人導(dǎo)航相關(guān)的功能包。關(guān)于創(chuàng)建ROS工作空間的操作，請參考前面相應(yīng)部分內(nèi)容，這里就不做講解。
安裝命令很簡單，執(zhí)行下面的命令編譯安裝就行了。
首先前往https://github.com/ros-planning/navigation，將分支切換到kinetic-devel然后下載源碼到本地，將navigation-kinetic-devel.zip解壓備用。將解壓好的navigation-kinetic-devel文件夾拷貝到~/catkin_ws_nav/src/后，就可編譯安裝了。
然后需要安裝teb_local_planner，前往https://github.com/rst-tu-dortmund/teb_local_planner，將分支切換到kinetic-devel然后下載源碼到本地，并解壓源碼包到~/catkin_ws_nav/src/后，就可以編譯安裝了。
?值得一提的是，teb_local_planner源碼中關(guān)于plugin的配置文件均已寫好，直接編譯源碼就能完成plugin的注冊及插入，非常方便。
3.3.ros-navigation使用
ros-navigation功能包集是一個很強(qiáng)大的導(dǎo)航框架，支持幾乎所有的移動機(jī)器人。我們只需要按照適當(dāng)?shù)呐渲镁涂梢詫os-navigation應(yīng)用到我們自己的機(jī)器人。為了更好的管理配置文件和啟動節(jié)點(diǎn)，我們需要先為miiboo機(jī)器人建立一個功能包miiboo_nav，用于專門存放我們機(jī)器人的導(dǎo)航配置與啟動文件。miiboo_nav功能包文件結(jié)構(gòu)，如圖36。不難發(fā)現(xiàn)，miiboo_nav是一個不包含任何可執(zhí)行源碼的功能包，里面只包含config和launch。config中存放導(dǎo)航中各算法模塊的參數(shù)配置，launch中存放啟動導(dǎo)航所需的各種節(jié)點(diǎn)的啟動文件。下面依次介紹這些文件的作用。
機(jī)器人全局定位AMCL算法配置amcl.launch.xml：
導(dǎo)航中用到的全局定位AMCL算法功能包包含很多可以配置的參數(shù)，這里建立配置文件amcl.launch.xml對這些參數(shù)進(jìn)行配置，內(nèi)容如圖37。配置參數(shù)分為3類：粒子濾波參數(shù)、雷達(dá)模型參數(shù)、里程計模型參數(shù)。由于參數(shù)比較多，關(guān)于參數(shù)配置的具體講解就不展開，請直接參考wiki官方教程：http://wiki.ros.org/amcl。
代價地圖公有參數(shù)配置costmap_common_params.yaml：
全局代價地圖和局部代價地圖公有的參數(shù)被放置在costmap_common_params.yaml這里面，這樣只需要載入這個配置文件就能完成共有參數(shù)的配置，避免了在全局代價地圖和局部代價地圖配置文件中重復(fù)配置一些相同參數(shù)的冗余，內(nèi)容如圖38。配置參數(shù)分為2類：機(jī)器人形狀和代價地圖各Layer圖層。機(jī)器人形狀可以用多邊形或圓形描述，我們的miiboo機(jī)器人是矩形的，所以選多邊形描述。代價地圖各Layer圖層包括：靜態(tài)層static_layer（由SLAM建立得到的地圖提供數(shù)據(jù)）、障礙層obstacle_layer（由激光雷達(dá)等障礙掃描傳感器提供實(shí)時數(shù)據(jù)）、全局膨脹層global_inflation_layer（為全局代價地圖提供膨脹效果）、局部膨脹層global_inflation_layer（為局部代價地圖提供膨脹效果）。關(guān)于參數(shù)配置的具體講解就不展開，請直接參考wiki官方教程：http://wiki.ros.org/costmap_2d。
全局代價地圖參數(shù)配置global_costmap_params.yaml：
全局代價地圖是以插件的形式組建的，在costmap_common_params.yaml中定義的各Layer圖層都可以通過插件的形式放入全局代價地圖中，各Layer圖層的組合可以根據(jù)需求自由選擇，如圖39。我們的miiboo機(jī)器人的全局代價地圖只用了static_layer和global_inflation_layer兩個圖層。
局部代價地圖參數(shù)配置global_costmap_params.yaml：
局部代價地圖和全局代價地圖類似，就不展開講解了，如圖40。我們的miiboo機(jī)器人的局部代價地圖只用了obstacle_layer和local_inflation_layer兩個圖層。
全局路徑規(guī)劃器參數(shù)配置global_planner_params.yaml：
全局路徑規(guī)劃器也是以插件的形式被使用，這里主要對全局路徑規(guī)劃器GlobalPlanner插件及參數(shù)進(jìn)行申明，在后面的move_base_params.yaml中將會調(diào)用這個插件。配置文件內(nèi)容如圖41。關(guān)于參數(shù)配置的具體講解就不展開，請直接參考wiki官方教程：
http://wiki.ros.org/global_planner。
局部路徑規(guī)劃器參數(shù)配置teb_local_planner_params.yaml：
局部路徑規(guī)劃器也是以插件的形式被使用，這里主要對局部路徑規(guī)劃器TebLocalPlannerROS插件及參數(shù)進(jìn)行申明，在后面的move_base_params.yaml中將會調(diào)用這個插件。配置文件內(nèi)容如圖42。關(guān)于參數(shù)配置的具體講解就不展開，請直接參考wiki官方教程：
http://wiki.ros.org/teb_local_planner。
導(dǎo)航核心控制參數(shù)配置move_base_params.yaml：
導(dǎo)航核心控制由move_base節(jié)點(diǎn)最終實(shí)現(xiàn)，move_base在整個導(dǎo)航框架中處于核心地位，全局代價地圖、局部代價地圖、全局路徑規(guī)劃、局部路徑規(guī)劃、恢復(fù)策略等等都將在這里得到具體實(shí)現(xiàn)的調(diào)用。配置內(nèi)容如圖43。不難發(fā)現(xiàn)，最開始兩行便對調(diào)用的全局路徑規(guī)劃器和局部路徑規(guī)劃器進(jìn)行了申明，然后就是一些運(yùn)動控制方面的參數(shù)。關(guān)于參數(shù)配置的具體講解就不展開，請直接參考wiki官方教程：http://wiki.ros.org/move_base。
啟動導(dǎo)航所需的各種節(jié)點(diǎn)的啟動文件miiboo_nav.launch：
在config目錄中將各個導(dǎo)航模塊的參數(shù)配置好后，就可以在一個啟動文件中將導(dǎo)航所需的各種節(jié)點(diǎn)進(jìn)行啟動，并載入相關(guān)配置。啟動文件miiboo_nav.launch就是來完成這個任務(wù)的，如圖44。不難發(fā)現(xiàn)，第一個啟動項是map_server，將SLAM建圖中得到的地圖進(jìn)行載入后發(fā)布到ROS中供導(dǎo)航中需要的模塊使用；第二個啟動項是amcl，amcl開始運(yùn)行后將為導(dǎo)航中的模塊提供機(jī)器人在地圖中的全局位置；第三個啟動項是move_base，在這里將載入代價地圖公有參數(shù)、全局代價地圖參數(shù)、局部代價地圖參數(shù)、move_base參數(shù)、全局路徑規(guī)劃參數(shù)、局部路徑規(guī)劃參數(shù)。
到這里，我們已經(jīng)準(zhǔn)備好了導(dǎo)航的各個配置文件和啟動文件，現(xiàn)在就正式來進(jìn)行導(dǎo)航。這里需要特別提醒，我們假設(shè)已經(jīng)在SLAM建圖過程得到了一張比較好的環(huán)境地圖，并保存到了正確的路徑。啟動導(dǎo)航分為3步：啟動機(jī)器人上所有傳感器、啟動導(dǎo)航所需各個節(jié)點(diǎn)、發(fā)送導(dǎo)航目標(biāo)點(diǎn)。
首先，啟動機(jī)器人上所有傳感器，打開終端，通過下面的命令直接啟動就行了。
然后，啟動導(dǎo)航所需各個節(jié)點(diǎn)，打開終端，通過下面的命令直接啟動就行了。
最后，發(fā)送導(dǎo)航目標(biāo)點(diǎn)。這里有兩種方式來下發(fā)導(dǎo)航目標(biāo)點(diǎn)，方法一是通過rviz交互界面，方法二是通過手機(jī)APP交互界面。下面對兩種方法分別介紹。
方法一：
在PC端打開rviz，打開一個新終端，運(yùn)行rviz啟動命令。
訂閱/map、/scan、/tf等信息，并觀察機(jī)器人的初始位置是否正確，如果機(jī)器人的初始位置不正確，需要用[2D Pose Estimate]按鈕手動給定一個正確的初始位置，如圖45。操作方法很簡單，先點(diǎn)擊[2D Pose Estimate]按鈕，然后將鼠標(biāo)放置到機(jī)器人在地圖中實(shí)際應(yīng)該的位置，最后按住鼠標(biāo)并拖動鼠標(biāo)來完成機(jī)器人朝向的設(shè)置。
初始位置設(shè)置正確后，就可以用[2D Nav Goal]按鈕手動指定導(dǎo)航目標(biāo)點(diǎn)了，如圖46。操作方法很簡單，先點(diǎn)擊[2D Nav Goal]按鈕，然后將鼠標(biāo)放置到地圖中任意的想讓機(jī)器人到達(dá)的空白位置，最后按住鼠標(biāo)并拖動鼠標(biāo)來完成機(jī)器人朝向的設(shè)置。這樣機(jī)器人就會開始規(guī)劃路徑并自動導(dǎo)航到該指定目標(biāo)點(diǎn)。
方法二：
直接打開手機(jī)上的miiboo機(jī)器人APP，就可以看到地圖和機(jī)器人在地圖中的位置了。和方法一中的一樣，需要觀察機(jī)器人的初始位置是否正確，如果機(jī)器人的初始位置不正確，用[Set Pose]按鈕手動給定一個正確的初始位置，如圖47。操作過程和方法一類似，就不展開了。
初始位置設(shè)置正確后，就可以用[Set Goal]按鈕手動指定導(dǎo)航目標(biāo)點(diǎn)了，如圖48。操作過程和方法一類似，就不展開了。
4.多目標(biāo)點(diǎn)導(dǎo)航及任務(wù)調(diào)度
通過前面的學(xué)習(xí)，我們已經(jīng)可以通過點(diǎn)擊地圖的方式來命令機(jī)器人運(yùn)動到目標(biāo)點(diǎn)。其實(shí)，ros-navigation導(dǎo)航框架就是為我們提供了一個最基本的機(jī)器人自動導(dǎo)航接口，即單點(diǎn)導(dǎo)航。然而，在實(shí)際的機(jī)器人應(yīng)用中，機(jī)器人往往要完成復(fù)雜的任務(wù)，這些復(fù)雜的任務(wù)都是由一個個基本的任務(wù)組合而成的。一般的，機(jī)器人通過狀態(tài)機(jī)的形式將一個個基本任務(wù)組合在一起來進(jìn)行復(fù)雜任務(wù)的調(diào)度實(shí)現(xiàn)。
4.1.狀態(tài)機(jī)
這里我們只討論有限狀態(tài)機(jī),也稱為FSM(Finite State Machine)，其在任意時刻都處于有限狀態(tài)集合中的某一狀態(tài)。當(dāng)其獲得一個輸入條件時，將從當(dāng)前狀態(tài)轉(zhuǎn)換到另一個狀態(tài)，或者仍然保持在當(dāng)前狀態(tài)。任何一個FSM都可以用狀態(tài)轉(zhuǎn)換圖來描述，圖中的節(jié)點(diǎn)表示FSM中的一個狀態(tài)，有向加權(quán)邊表示輸入條件時狀態(tài)的變化。如圖49，以一個上班族的生活場景來舉例說明狀態(tài)機(jī)的狀態(tài)轉(zhuǎn)換圖。矩形框表示FSM中的一個狀態(tài)，有向邊表示在輸入條件下的狀態(tài)轉(zhuǎn)換過程。
4.2.多目標(biāo)點(diǎn)巡航
機(jī)器人多目標(biāo)點(diǎn)巡航，特別是按特定巡邏路徑進(jìn)行巡航是很實(shí)用的功能。這里將利用前面學(xué)到的ros-navigation單點(diǎn)導(dǎo)航、狀態(tài)機(jī)、狀態(tài)機(jī)任務(wù)調(diào)度的知識。我們來編寫一個應(yīng)用功能包實(shí)現(xiàn)機(jī)器人多目標(biāo)點(diǎn)巡航。
到這里，我們慢慢清楚了miiboo機(jī)器人編程的框架思路，我們將傳感器相關(guān)的底層驅(qū)動包放在~/catkin_ws/工作空間統(tǒng)一管理，將基于google-cartographer的SLAM建圖程序包放在~/catkin_ws_carto/工作空間統(tǒng)一管理，將基于ros-navigation的導(dǎo)航程序包放在~/catkin_ws_nav/工作空間統(tǒng)一管理，將高層應(yīng)用功能包放在~/catkin_ws_apps/工作空間統(tǒng)一管理。miiboo機(jī)器人編程的框架思路，如圖50。
這里將建立一個叫catkin_ws_apps的ROS工作空間，專門用于放置日后開發(fā)的各種應(yīng)用層功能包。關(guān)于創(chuàng)建ROS工作空間的操作，請參考前面相應(yīng)部分內(nèi)容，這里就不做講解。在~/catkin_ws_apps/src/中建立一個叫patrol的功能包，建好后的patrol功能包文件結(jié)構(gòu)，如圖51。
關(guān)于功能包的文件結(jié)構(gòu)，大家已經(jīng)很熟悉了，就不啰嗦了。這里重點(diǎn)講解一下patrol_fsm.py這個文件，文件內(nèi)容如圖52。
這里采用python來編寫多目標(biāo)點(diǎn)巡航的邏輯，python開發(fā)ROS節(jié)點(diǎn)的優(yōu)點(diǎn)是簡潔高效。代碼中waypoints數(shù)組里面存放的是要巡航的各個目標(biāo)點(diǎn)，大家可以根據(jù)自己的需要進(jìn)行相應(yīng)的替換和增減；with patrol代碼塊里面實(shí)現(xiàn)狀態(tài)機(jī)的構(gòu)建；最后調(diào)用狀態(tài)機(jī)的執(zhí)行函數(shù)，狀態(tài)機(jī)就開始工作了，也就是開始執(zhí)行巡航了。
啟動多目標(biāo)點(diǎn)巡航分為3步：啟動機(jī)器人上所有傳感器、啟動導(dǎo)航所需各個節(jié)點(diǎn)、啟動多目標(biāo)點(diǎn)巡航節(jié)點(diǎn)。
首先，啟動機(jī)器人上所有傳感器，打開終端，通過下面的命令直接啟動就行了。
然后，啟動導(dǎo)航所需各個節(jié)點(diǎn)，打開終端，通過下面的命令直接啟動就行了。
最后，啟動多目標(biāo)點(diǎn)巡航節(jié)點(diǎn)，打開終端，通過下面的命令直接啟動就行了。
4.3.復(fù)雜多任務(wù)機(jī)器人未來展望
機(jī)器人可以進(jìn)行自動導(dǎo)航、人機(jī)對話、用機(jī)械臂抓取物體、物體識別等。將這些任務(wù)結(jié)合起來，利用機(jī)器人強(qiáng)大的大腦推理機(jī)制能完成更為復(fù)雜和智能化的任務(wù)。如果說基于狀態(tài)機(jī)的復(fù)雜任務(wù)調(diào)度是1.0版本的智能，那么基于大腦推理機(jī)制的復(fù)雜任務(wù)調(diào)度將是2.0版本的智能。
我的設(shè)想是利用強(qiáng)化學(xué)習(xí)神經(jīng)網(wǎng)絡(luò)作為大腦推理機(jī)制的實(shí)現(xiàn)實(shí)體，如圖53。自動導(dǎo)航、人機(jī)對話、用機(jī)械臂抓取物體、物體識別等任務(wù)組合的整體作為機(jī)器人與外界環(huán)境交互的動作空間，動作空間的的狀態(tài)分為兩種形態(tài)：執(zhí)行結(jié)果、執(zhí)行策略。執(zhí)行結(jié)果作為強(qiáng)化學(xué)習(xí)神經(jīng)網(wǎng)絡(luò)的輸入，而執(zhí)行策略作為強(qiáng)化學(xué)習(xí)神經(jīng)網(wǎng)絡(luò)的輸出。我們不斷通過各種復(fù)雜的實(shí)際場景的粒子來訓(xùn)練機(jī)器人，讓機(jī)器人能在復(fù)雜場景下能做正確的事情。比如說，當(dāng)機(jī)器人收到主人“我渴了”的語音信息后，自動導(dǎo)航到桌子邊，然后識別桌上的可樂，并用機(jī)械臂抓取，最后遞給主人，并提醒主人“你的可樂來了”。
哈哈！這樣的想法很炫酷，不過以目前的技術(shù)實(shí)現(xiàn)難度還比較大，所以作為未來展望分享給大家。希望和大家一起努力，在不遠(yuǎn)的將來能實(shí)現(xiàn)這個夢想。
5.機(jī)器人巡航與現(xiàn)場監(jiān)控
機(jī)器人在實(shí)際生產(chǎn)生活中能給人類帶來很大的幫助，比如一款能巡視和現(xiàn)場監(jiān)控的機(jī)器人。商場、機(jī)場、家里等需要監(jiān)控和安保的場景，拍一個機(jī)器人去是再合適不過的了。結(jié)合前面所學(xué)的知識，我們可以讓miiboo機(jī)器人執(zhí)行巡視與現(xiàn)場監(jiān)控的任務(wù)。這里介紹兩種工作方式：手動遙控式現(xiàn)場監(jiān)控、自動巡航式現(xiàn)場監(jiān)控。
5.1.手動遙控式現(xiàn)場監(jiān)控
首先，啟動機(jī)器人上所有傳感器，這樣機(jī)器人上的攝像頭就可以將圖像發(fā)布到ROS中，供遠(yuǎn)程訂閱和顯示。打開終端，通過下面的命令直接啟動就行了。
然后，啟動導(dǎo)航所需各個節(jié)點(diǎn)，這樣就可以實(shí)時查看機(jī)器人所在的具體位置。打開終端，通過下面的命令直接啟動就行了。
最后，在Android手機(jī)端用miiboo機(jī)器人APP控制機(jī)器人運(yùn)動和視頻監(jiān)控周圍環(huán)境，如圖54。
5.2.自動巡航式現(xiàn)場監(jiān)控
與手動遙控式現(xiàn)場監(jiān)控的區(qū)別是，自動巡航式現(xiàn)場監(jiān)控不需要人員值守和遙控機(jī)器人運(yùn)動。機(jī)器人能根據(jù)設(shè)定的巡邏路線巡航并實(shí)時回傳視頻監(jiān)控，結(jié)合人臉識別、事件識別等人工智能及視頻分析算法，可以對異常事件進(jìn)行自動的監(jiān)控。
首先，啟動機(jī)器人上所有傳感器，這樣機(jī)器人上的攝像頭就可以將圖像發(fā)布到ROS中，供遠(yuǎn)程訂閱和顯示。打開終端，通過下面的命令直接啟動就行了。
然后，啟動導(dǎo)航所需各個節(jié)點(diǎn)，這樣就可以實(shí)時查看機(jī)器人所在的具體位置。打開終端，通過下面的命令直接啟動就行了。
接下來，設(shè)定好巡邏路線后，啟動多目標(biāo)點(diǎn)巡航節(jié)點(diǎn)，打開終端，通過下面的命令直接啟動就行了。
最后，在Android手機(jī)端用miiboo機(jī)器人APP視頻監(jiān)控周圍環(huán)境，或者將訂閱的視頻放入人工智能及視頻分析算法，如圖55。
后記
為了防止后續(xù)大家找不到本篇文章，我同步制作了一份文章的pdf和本專欄涉及的例程代碼放在github和gitee方便大家下載，如果下面給出的github下載鏈接打不開，可以嘗試gitee下載鏈接：
github下載鏈接：https://github.com/xiihoo/DIY_A_SLAM_Navigation_Robot
gitee下載鏈接：https://gitee.com/xiihoo-robot/DIY_A_SLAM_Navigation_Robot
技術(shù)交流
QQ技術(shù)交流群：117698356
參考文獻(xiàn)
[1] 張虎，機(jī)器人SLAM導(dǎo)航核心技術(shù)與實(shí)戰(zhàn)[M]. 機(jī)械工業(yè)出版社，2022.
