基于ROS的應急監(jiān)測機器人導航系統(tǒng)設計與實現(xiàn)

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

（南華大學 機械工程學院，衡陽421000）

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

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

1 基于ROS 的室內建圖和導航系統(tǒng)實現(xiàn)

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

圖1 應急監(jiān)測機器人實物圖Fig.1 Physical map of emergency monitoring robot

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

2 系統(tǒng)軟硬件設計

2.1 系統(tǒng)硬件設計

2.1.1 機械結構設計

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

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

2.1.2 控制處理器

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

圖4 Jetson TX2Fig.4 Jetson TX2

2.1.3 激光雷達

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

圖5 激光雷達Fig.5 Lidar

2.2 系統(tǒng)軟件設計

2.2.1 ROS 系統(tǒng)

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

2.2.2 ROS 分布式主從通信部署

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

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

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

主機：

export ROS_HOSTNAME=jetson-0421518035271

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

從機：

export ROS_HOSTNAME=LAPTOP

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

（3）測試。在主機啟動turtlesim_node 節(jié)點，從機中啟動turtlesim turtle_teleop_key，看是否能在從機上控制主機上的節(jié)點。

3 室內建圖與導航功能實現(xiàn)

3.1 SLAM 算法

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

圖6 SLAM 模型Fig.6 SLAM model

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

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

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

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

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

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

3.2 導航功能實現(xiàn)

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

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

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

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

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

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

4 仿真與實驗

4.1 仿真

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

圖9 仿真測試過程Fig.9 Simulation test process

4.2 實際環(huán)境

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

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

圖11 測試結果Fig.11 Test result

5 結語

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