基于ROS的應(yīng)急監(jiān)測(cè)機(jī)器人導(dǎo)航系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)

李貽文，鄒樹梁，張 德，陳宏斌

（南華大學(xué) 機(jī)械工程學(xué)院，衡陽(yáng)421000）

核與輻射等相關(guān)突發(fā)事件具有爆發(fā)突然、危害巨大且迅速、影響深遠(yuǎn)的特點(diǎn)，在核與輻射等相關(guān)環(huán)境下，救援先遣隊(duì)伍面臨先期的輻射、偵檢、環(huán)境偵察、指揮決策等一系列關(guān)鍵問題。為了讓救援先遣隊(duì)員減少輻射的傷害，在救援前能夠構(gòu)建好地圖就顯得尤為重要。隨著機(jī)器人技術(shù)的不斷發(fā)展，移動(dòng)機(jī)器人在安防、醫(yī)療、服務(wù)、軍事方面的應(yīng)用越來越廣泛。機(jī)器人代替人偵檢具有輻射的場(chǎng)所并得到精準(zhǔn)的環(huán)境地圖成為可能。其中，同步定位與地圖構(gòu)建SLAM（simultaneous localization and mapping）技術(shù)作為機(jī)器人自主運(yùn)動(dòng)的關(guān)鍵[1-3]之一，具有重要的理論研究?jī)r(jià)值和實(shí)際應(yīng)用意義。

本文在研究和分析SLAM 算法的基礎(chǔ)上，將設(shè)計(jì)實(shí)現(xiàn)室內(nèi)定位建圖以及導(dǎo)航的應(yīng)急監(jiān)測(cè)機(jī)器人，基于ROS 平臺(tái)利用激光雷達(dá)進(jìn)行周圍環(huán)境感知，利用Cartographer SLAM 算法，實(shí)現(xiàn)在環(huán)境中的自主定位并構(gòu)建出增量式環(huán)境地圖的方法，實(shí)時(shí)性好，為移動(dòng)機(jī)器人的自主導(dǎo)航提供基礎(chǔ)，同時(shí)應(yīng)用Move_base 導(dǎo)航包完成已知地圖的路徑規(guī)劃，設(shè)計(jì)并實(shí)現(xiàn)室內(nèi)定位和導(dǎo)航系統(tǒng)。

1 基于ROS 的室內(nèi)建圖和導(dǎo)航系統(tǒng)實(shí)現(xiàn)

基于ROS的應(yīng)急監(jiān)測(cè)機(jī)器人系統(tǒng)定位和導(dǎo)航系統(tǒng)實(shí)物照片如圖1所示，可作為室內(nèi)應(yīng)急監(jiān)測(cè)機(jī)器人SLAM、導(dǎo)航以及ROS 框架的基礎(chǔ)實(shí)驗(yàn)平臺(tái)，主要硬件模塊有STM32、ROBOSENSE-LIDAR-16 激光雷達(dá)、慣性計(jì)量單元（IMU）、電源模塊，各模塊的硬件連接關(guān)系如圖2所示，加入視覺傳感器或機(jī)械臂后，可實(shí)現(xiàn)更多的功能。

圖1 應(yīng)急監(jiān)測(cè)機(jī)器人實(shí)物圖Fig.1 Physical map of emergency monitoring robot

圖2 應(yīng)急監(jiān)測(cè)機(jī)器人系統(tǒng)總體框圖Fig.2 Overall block diagram of emergency monitoring robot system

2 系統(tǒng)軟硬件設(shè)計(jì)

2.1 系統(tǒng)硬件設(shè)計(jì)

2.1.1 機(jī)械結(jié)構(gòu)設(shè)計(jì)

應(yīng)急監(jiān)測(cè)機(jī)器人采用履帶式底盤車，如圖3所示。機(jī)器人的主體采用不銹鋼和合金板，不銹鋼和合金板具有良好的穩(wěn)定性和抗撞擊能力。采用四輪組平衡懸掛，可適應(yīng)重載避震，全車安裝6 枚避震器結(jié)臺(tái)獨(dú)特的撞擊吸能設(shè)計(jì)，可有效減少地形沖擊，特有張緊履帶連接，可適用于較為復(fù)雜的環(huán)境，在機(jī)器人內(nèi)部有留有足夠的空間可以作為搭載平臺(tái)。

圖3 應(yīng)急監(jiān)測(cè)機(jī)器人三維模型Fig.3 Three dimensional model of emergency monitoring robot

2.1.2 控制處理器

由于一般的工業(yè)機(jī)和普通的筆記本電腦的質(zhì)量和體積都較大，不適合在空間不大的應(yīng)急機(jī)器人上使用，因此本文選用NVIDIA 公司生產(chǎn)的Jetson TX2 的計(jì)算機(jī)模塊板作為控制處理器，可為嵌入式計(jì)算設(shè)備提供出色的速度與能效。這款超級(jí)計(jì)算機(jī)模塊配有NVIDIA PascalTM GPU、高達(dá)8 GB 內(nèi)存、59.7 GB/s 的顯存帶寬。其尺寸為50 mm×87 mm，是由雙核Denver 2 64 位CPU 和四核ARM A57 Complex，具備一個(gè)USB 接口，一個(gè)HDMI 接口。因此NVIDIA Jetson TX2 可以滿足SLAM 算法的實(shí)時(shí)運(yùn)算要求，也能滿足在應(yīng)急機(jī)器人平臺(tái)上體積小的要求，實(shí)物如圖4所示。

圖4 Jetson TX2Fig.4 Jetson TX2

2.1.3 激光雷達(dá)

本文采用的激光雷達(dá)是深圳市速騰聚創(chuàng)科技有限公司生產(chǎn)的RS-LIDAR-16 國(guó)產(chǎn)16 線激光雷達(dá)，如圖5所示。RS-LiDAR-16 通過16 個(gè)激光頭同時(shí)發(fā)射高頻率激光束對(duì)外界環(huán)境進(jìn)行持續(xù)性的掃描，出點(diǎn)數(shù)高達(dá)640000 點(diǎn)/s，測(cè)量距離達(dá)到150 m，達(dá)到目前世界頂級(jí)的測(cè)量精度2 cm 以內(nèi)。

圖5 激光雷達(dá)Fig.5 Lidar

2.2 系統(tǒng)軟件設(shè)計(jì)

2.2.1 ROS 系統(tǒng)

本文軟件平臺(tái)ROS 操作系統(tǒng)是基于LINUX 系統(tǒng)上的一個(gè)次級(jí)操作系統(tǒng)。ROS 軟件模塊已經(jīng)可以使用任何已開發(fā)了客戶端庫(kù)的語(yǔ)言來寫，包括C++、Pthon 等[4]。ROS 集成很多開源軟件包供開發(fā)者修改并使用，可用于建圖、導(dǎo)航、路徑規(guī)劃等功能的實(shí)現(xiàn)。且提供了RViz、Gazebo 等仿真軟件，通過仿真可以驗(yàn)證算法的有效性等，有助于快速完成機(jī)器人系統(tǒng)的開發(fā)[5]。ROS 是分布式進(jìn)程，可執(zhí)行進(jìn)程的最小單位（節(jié)點(diǎn)，Node）的形式進(jìn)行編程，每個(gè)進(jìn)程獨(dú)立運(yùn)行，并有機(jī)地收發(fā)數(shù)據(jù)。由于它以功能包的形式管理著多個(gè)具有相同目的的進(jìn)程，所以開發(fā)和使用起來很容易，并且很容易共享、修改和重新發(fā)布[4]。

2.2.2 ROS 分布式主從通信部署

在ROS 中，每一個(gè)進(jìn)程都以一個(gè)節(jié)點(diǎn)的形式進(jìn)行，可以分布于多個(gè)不同的主機(jī)。節(jié)點(diǎn)間的通信消息通過一個(gè)帶有發(fā)布和訂閱功能的RPC 傳輸系統(tǒng)，從發(fā)布節(jié)點(diǎn)傳送到接收節(jié)點(diǎn)。這種點(diǎn)對(duì)點(diǎn)的設(shè)計(jì)可以分散定位、導(dǎo)航等功能帶來的實(shí)時(shí)計(jì)算壓力。得益于這種點(diǎn)對(duì)點(diǎn)的設(shè)計(jì)，本文可以將即時(shí)定位與地圖構(gòu)建以及導(dǎo)航算法運(yùn)算需要處理的大量實(shí)時(shí)傳感器數(shù)據(jù)傳輸?shù)絇C 端，將實(shí)時(shí)生成地圖數(shù)據(jù)顯示在PC 端的屏幕上，具體的分布式主從通信部署如下：

（1）PC 端作為從機(jī)，控制器Jetson TX2 作為主機(jī)，在同一個(gè)WIFI 下分別設(shè)置固定IP，分別在主從機(jī)的終端輸入ifconfig 與hostname 得到各自的IP地址和計(jì)算機(jī)名，在到自己的系統(tǒng)的/etc/hosts 文件中添加對(duì)方的IP 地址與名字。

（2）設(shè)置ROS_MASTER_URI，ROS_MASTER 的位置可以用環(huán)境變量ROS_MASTER_URI 進(jìn)行定義，在主機(jī)和從機(jī)中的.bashr 文件的最后添加。

主機(jī)：

export ROS_HOSTNAME=jetson-0421518035271

export ROS_MASTER_URI=http：//jetson-042151 8035271：11311

從機(jī)：

export ROS_HOSTNAME=LAPTOP

export ROS_MASTER_URI=http：//jetson-042151 8035271：11311

（3）測(cè)試。在主機(jī)啟動(dòng)turtlesim_node 節(jié)點(diǎn)，從機(jī)中啟動(dòng)turtlesim turtle_teleop_key，看是否能在從機(jī)上控制主機(jī)上的節(jié)點(diǎn)。

3 室內(nèi)建圖與導(dǎo)航功能實(shí)現(xiàn)

3.1 SLAM 算法

SLAM 模型如圖6所示，已知機(jī)器人的控制數(shù)組c1：t=｛c1，c2，…ct｝和觀察數(shù)組e1：t=｛e1，e2，…et｝，求環(huán)境地圖mt=｛m1，m2，…mt｝和機(jī)器人的實(shí)際位姿x0：t=｛x0，x1，…xt｝，其中xt表示t時(shí)刻給機(jī)器人的位姿；mt表示環(huán)境特征；ct表示t時(shí)刻給機(jī)器人的控制量；et表示t時(shí)刻傳感器的預(yù)估值，機(jī)器人從未知環(huán)境的未知地點(diǎn)出發(fā)，在運(yùn)動(dòng)過程中通過反復(fù)觀測(cè)到的環(huán)境特征，定位自身位姿，再根據(jù)自身位姿增量式地構(gòu)建地圖，從而達(dá)到同時(shí)定位和地圖構(gòu)建的目的。

圖6 SLAM 模型Fig.6 SLAM model

主流的SLAM 算法中，包括傳感器獲取數(shù)據(jù)、前端配準(zhǔn)、后端優(yōu)化、地圖構(gòu)建4 個(gè)模塊，而針對(duì)后端優(yōu)化板塊可以分為兩類：一類是基于粒子濾波算法，另一類是基于圖優(yōu)化的算法。

（1）基于濾波器的Gmapping 算法

現(xiàn)在主流的基于濾波器的SLAM 算法是Gmapping 算法，其基本原理是RBPF 粒子濾波算法[6]，即將定位和建圖過程分離，先進(jìn)行定位再進(jìn)行建圖。Gmapping 算法可以實(shí)時(shí)構(gòu)建室內(nèi)地圖，且在較小的環(huán)境地圖中構(gòu)建出來的地圖精度與Cartographer 算法構(gòu)建出來的精度并無(wú)太多差別，計(jì)算量少于Cartographer，但隨著場(chǎng)景增大所需要的粒子數(shù)也隨之增多，而每一個(gè)粒子都相當(dāng)于一副地圖，在構(gòu)建大場(chǎng)景地圖時(shí)，所需要的計(jì)算內(nèi)存和計(jì)算量驟增。

（2）基于圖優(yōu)化的Cartographer 算法

Cartographer 算法采用基于圖優(yōu)化的方法，在前端部分接收激光雷達(dá)的數(shù)據(jù)后進(jìn)行匹配，匹配到一個(gè)雷達(dá)位姿以后，將當(dāng)前幀的激光插入到子圖當(dāng)中，當(dāng)子圖中的位姿沒有變化或者變化較緩慢，就會(huì)將其去除，將變化的激光位姿不斷更新從而得到子圖。后端部分采用分支定界法對(duì)地圖搜索進(jìn)行加速，構(gòu)建一個(gè)回環(huán)檢測(cè)，來消除子圖的匹配誤差[7-9]。

相比較Hector 和Gmapping 兩種算法，其魯棒性好，尤其在沒有IMU 的輔助下，應(yīng)對(duì)旋轉(zhuǎn)速度高的轉(zhuǎn)向，沒有出現(xiàn)建圖錯(cuò)誤，且采用了回環(huán)檢測(cè)，可以消除累計(jì)誤差，建圖效果比較穩(wěn)定[10]。本文采取的是Cartographer 算法作為SLAM 的算法。

3.2 導(dǎo)航功能實(shí)現(xiàn)

應(yīng)急監(jiān)測(cè)機(jī)器人的導(dǎo)航功能實(shí)現(xiàn)使用ROS 中的move_base 軟件包，move_base 包可分為全局路徑規(guī)劃，局部路徑規(guī)劃，里程計(jì)信息以及傳感器信息4個(gè)模塊。Move_base 軟件包的總體框架圖如圖7所示。

圖7 Move_base 框架圖Fig.7 Move_base frame diagram

全局路徑規(guī)劃：通過全局代價(jià)地圖進(jìn)行全局的路徑規(guī)劃，計(jì)算出機(jī)器人到目標(biāo)位姿的路線。這些功能通過Dijkstra 最短路徑的算法實(shí)現(xiàn)，或者是A*算法實(shí)現(xiàn)。Djikstar 和A*都能實(shí)現(xiàn)功能，但A*算法較Djikstar 消耗時(shí)間少，可以讓機(jī)器人在導(dǎo)航過程中連續(xù)的運(yùn)動(dòng)，本文采用的是A* 算法作為全局路徑規(guī)劃的算法。

局部路徑規(guī)劃：局部路徑規(guī)劃通過Local_planner 中的cost function 與全局路徑規(guī)劃中的參數(shù)連接，局部規(guī)劃的路徑會(huì)靠近全局規(guī)劃的路徑，并結(jié)合實(shí)際情況，對(duì)局部路徑進(jìn)行調(diào)整。局部路徑規(guī)劃的算法按速度采樣空間的不同，可以分為Dynamic Windows Approach（DWA）算法和T/rajectory Rollout 算法，兩者之間有細(xì)微的差別，但差別不大。

為了提高機(jī)器人的定位精度，本系統(tǒng)使用了AMCL 功能包。該功能包采用了蒙特卡洛定位方法，來幫助機(jī)器人進(jìn)行二維的定位，針對(duì)已有地圖使用粒子濾波器跟蹤一個(gè)機(jī)器人的姿態(tài)，AMCL 通過傳感器采集到的信息在地圖上的一個(gè)位姿估算，框架如圖8所示。

圖8 AMCL 框架圖Fig.8 AMCL frame diagram

4 仿真與實(shí)驗(yàn)

4.1 仿真

為提高實(shí)驗(yàn)的開發(fā)效率，本文使用ROS 中Gazebo工具進(jìn)行仿真實(shí)驗(yàn)，為了使仿真結(jié)果更加接近真實(shí)情況，在Gazebo 搭建物理仿真環(huán)境，首先配置機(jī)器人建立應(yīng)急機(jī)器人URDF 描述文件，為描述文件添加慣性參數(shù)，碰撞屬性，顏色，摩檫力等參數(shù)，同時(shí)添加傳動(dòng)裝置以及Gazebo 的控制器插件；其次創(chuàng)建仿真環(huán)境，利用Gazebo 中的Build editor 工具以及自帶的模型庫(kù)生仿真模型。最后啟動(dòng)之前配置好的Cartographer 算法launch 文件。通過上述的構(gòu)建地圖過程，可以得到仿真環(huán)境的先驗(yàn)地圖，配置move_base包德個(gè)節(jié)點(diǎn)關(guān)系，先指定機(jī)器人的起始位姿，再指定目標(biāo)點(diǎn)的位姿，最后啟動(dòng)機(jī)器人的導(dǎo)航。算法自動(dòng)規(guī)劃路徑，并實(shí)現(xiàn)局部避障。其導(dǎo)航避障過程與結(jié)果如圖9所示。

圖9 仿真測(cè)試過程Fig.9 Simulation test process

4.2 實(shí)際環(huán)境

基于ROS 和激光雷達(dá)搭建應(yīng)急機(jī)器人原型系統(tǒng)，在真實(shí)環(huán)境下進(jìn)行定位與建圖測(cè)試，實(shí)際測(cè)試環(huán)境如圖10所示，機(jī)器人使用Cartographer 算法進(jìn)行地圖構(gòu)建和定位，move_base 功能包進(jìn)行導(dǎo)航，其結(jié)果如圖11所示，在實(shí)驗(yàn)過程中沒有與障礙物發(fā)生碰撞。

圖10 實(shí)際測(cè)試環(huán)境Fig.10 Actual test environment

圖11 測(cè)試結(jié)果Fig.11 Test result

5 結(jié)語(yǔ)

本文以NVIDIA 公司的Jetson TX2 計(jì)算機(jī)模塊版作為控制器，通過Robosence 公司的RS-LiDAR-16 雷達(dá)以及IMU 采集環(huán)境信息，在ROS 分布式系統(tǒng)以及Cartographer 算法構(gòu)建了應(yīng)急檢測(cè)履帶式機(jī)器人的SLAM 系統(tǒng)。對(duì)其進(jìn)行了仿真實(shí)驗(yàn)以及實(shí)際環(huán)境測(cè)試結(jié)果表明，該應(yīng)急監(jiān)測(cè)機(jī)器人具有良好的建圖精度，具有模塊性化，拓展方便等優(yōu)點(diǎn)，可實(shí)現(xiàn)在室內(nèi)環(huán)境的自主導(dǎo)航的功能?？勺鳛閼?yīng)急監(jiān)測(cè)機(jī)器人SLAM、導(dǎo)航及ROS 框架的實(shí)驗(yàn)基礎(chǔ)平臺(tái)。