基于ROS和激光雷達(dá)的室內(nèi)移動(dòng)機(jī)器人定位和 導(dǎo)航系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)

，，，

(同濟(jì)大學(xué)電子與信息工程學(xué)院, 上海 200092)

0 引言

移動(dòng)機(jī)器人必須具備在環(huán)境中移動(dòng)的能力，即能夠使用自身搭載的傳感器感知環(huán)境信息，并據(jù)此實(shí)現(xiàn)定位和導(dǎo)航功能。其中，同步定位與地圖構(gòu)建SLAM(simultaneous localization and mapping)技術(shù)被認(rèn)為是機(jī)器人自主運(yùn)動(dòng)的關(guān)鍵[1-3]，具有重要的理論研究?jī)r(jià)值和實(shí)際應(yīng)用意義。

本文在研究和分析SLAM算法的基礎(chǔ)上，使用卡片式電腦樹(shù)莓派3B作為核心控制器，基于激光雷達(dá)搭建硬件平臺(tái)，在Linux操作系統(tǒng)下使用ROS分布式機(jī)器人操作系統(tǒng)框架進(jìn)行軟件開(kāi)發(fā)，設(shè)計(jì)并實(shí)現(xiàn)開(kāi)源、低成本、差速驅(qū)動(dòng)的室內(nèi)定位和導(dǎo)航系統(tǒng)。

1 同步定位與地圖構(gòu)建

SLAM模型如圖1a所示，數(shù)學(xué)表達(dá)式可描述為：給定機(jī)器人的控制序列u1:t={u1,u2,,ut}和觀測(cè)序列z1:t={z1,z2,,zt}，求環(huán)境地圖m和機(jī)器人的位姿x0:t={x0,x1,,xt}，其中，xt表示t時(shí)刻機(jī)器人的位姿；mi表示環(huán)境特征；ut表示t時(shí)刻給機(jī)器人的控制量；zt表示t時(shí)刻傳感器的觀測(cè)量。t時(shí)刻機(jī)器人的狀態(tài)xt只與t-1時(shí)刻的狀態(tài)xt-1，當(dāng)前時(shí)刻的控制量ut以及傳感器的觀測(cè)量zt有關(guān)。機(jī)器人從未知環(huán)境的未知地點(diǎn)出發(fā)，在運(yùn)動(dòng)過(guò)程中通過(guò)重復(fù)觀測(cè)到的環(huán)境特征，如墻角、立柱等，定位自身位置和姿態(tài)，再根據(jù)自身位置增量式地構(gòu)建地圖，從而達(dá)到同時(shí)定位和地圖構(gòu)建的目的。

由于SLAM過(guò)程具有不確定性，某一時(shí)刻機(jī)器人的位姿無(wú)法精確表示，需要使用分布進(jìn)行估計(jì)。傳感器對(duì)機(jī)器人相對(duì)運(yùn)動(dòng)的辨別存在固有的不確定性，加之外界環(huán)境的噪聲影響，在構(gòu)建地圖的每一次迭代中，附加到原有地圖上的增量會(huì)產(chǎn)生誤差，隨著迭代次數(shù)的增加，誤差會(huì)逐漸累積，從而對(duì)地圖構(gòu)建、機(jī)器人定位以及導(dǎo)航等的精確度產(chǎn)生極大影響。因此，需要使用圖像匹配法、環(huán)路閉合檢測(cè)法、卡爾曼濾波或粒子濾波等方法對(duì)累積誤差進(jìn)行補(bǔ)償[2-4]。同步定位與地圖構(gòu)建過(guò)程可以簡(jiǎn)化為“估計(jì)--觀測(cè)--校正”，其系統(tǒng)框圖如圖1b所示。

圖1 SLAM模型及其系統(tǒng)框圖

2 基于ROS和激光雷達(dá)的系統(tǒng)實(shí)現(xiàn)

基于ROS和激光雷達(dá)的室內(nèi)機(jī)器人定位和導(dǎo)航系統(tǒng)，實(shí)物照片如圖2a所示，可作為室內(nèi)移動(dòng)機(jī)器人SLAM、導(dǎo)航以及ROS框架的基礎(chǔ)實(shí)驗(yàn)平臺(tái)，主要硬件模塊有：樹(shù)莓派3B、RPLIDAR A1激光雷達(dá)、Arduino Pro Mini、A4950雙路電機(jī)驅(qū)動(dòng)模塊、減速電機(jī)以及降壓/穩(wěn)壓直流電源模塊，各模塊的硬件連接關(guān)系如圖2b所示，加入其他傳感器或執(zhí)行機(jī)構(gòu)后，可實(shí)現(xiàn)更多的功能。

2.1 硬件核心模塊

2.1.1 激光雷達(dá)

激光雷達(dá)LiDAR是基于激光的距離測(cè)量系統(tǒng)，它通過(guò)向目標(biāo)發(fā)射激光束，并接收目標(biāo)反射的光束來(lái)測(cè)量目標(biāo)的距離等參數(shù)，常見(jiàn)的激光傳感系統(tǒng)[5-6]有：對(duì)單個(gè)特征點(diǎn)進(jìn)行測(cè)距的距離傳感器、對(duì)平面進(jìn)行二維掃描的激光雷達(dá)以及三維立體掃描的激光雷達(dá)。本文所使用的RPLIDAR A1是由SLAMTEC公司開(kāi)發(fā)的低成本二維激光雷達(dá)解決方案，利用三角測(cè)距原理對(duì)二維平面6 m半徑范圍內(nèi)進(jìn)行激光測(cè)距掃描，生成平面點(diǎn)云地圖信息，在5.5 Hz的掃描頻率下，測(cè)距分辨率可達(dá)當(dāng)前距離值的0.2%。

2.1.2 控制器

室內(nèi)移動(dòng)機(jī)器人的核心控制器樹(shù)莓派3B是一款基于ARM架構(gòu)，運(yùn)行Linux操作系統(tǒng)的開(kāi)源卡片式電腦[7]。由于基于Linux內(nèi)核的Ubuntu Mate操作系統(tǒng)是分時(shí)系統(tǒng)而非實(shí)時(shí)系統(tǒng)，而移動(dòng)機(jī)器人的運(yùn)動(dòng)控制對(duì)實(shí)時(shí)性有一定要求，因此需要使用額外的控制器Arduino Pro Mini最小系統(tǒng)來(lái)控制機(jī)器人的運(yùn)動(dòng)。主控制器和驅(qū)動(dòng)控制器之間使用高速串口通信完成數(shù)據(jù)和命令的發(fā)送和接收。

2.1.3 其它模塊

移動(dòng)機(jī)器人平臺(tái)使用A4950雙路功率驅(qū)動(dòng)分別驅(qū)動(dòng)兩個(gè)減速比為1∶30、額定功率7 W、最大轉(zhuǎn)速500 r/min的減速電機(jī)，實(shí)現(xiàn)差速運(yùn)動(dòng)。電機(jī)自帶A/B相編碼器，可用作轉(zhuǎn)速反饋以及里程計(jì)。移動(dòng)機(jī)器人使用大電流輸出的12 V航模電池供電，而樹(shù)莓派、Arduino Pro Mini和傳感器模塊的工作電壓為5 V，因此需要對(duì)電池電壓進(jìn)行降壓穩(wěn)壓。需要注意的是，樹(shù)莓派的USB接口輸出電流有限制，當(dāng)激光雷達(dá)旋轉(zhuǎn)并開(kāi)始工作時(shí)，所需供電電流可能會(huì)超過(guò)樹(shù)莓派的輸出能力。為保證機(jī)器人穩(wěn)定運(yùn)行，激光雷達(dá)另使用獨(dú)立的USB集線器供電。

2.2 軟件核心模塊

2.2.1 機(jī)器人操作系統(tǒng)ROS

ROS是一套專為機(jī)器人軟件開(kāi)發(fā)所設(shè)計(jì)的開(kāi)源元級(jí)操作系統(tǒng)，集成了硬件描述、底層驅(qū)動(dòng)程序管理、程序間傳遞消息、程序發(fā)行包管理等一系列的服務(wù)。ROS采用分布式處理框架，使得不同架構(gòu)的處理平臺(tái)成為機(jī)器人系統(tǒng)的一部分，其核心使用通信模塊實(shí)現(xiàn)模塊間對(duì)等式網(wǎng)絡(luò)的解耦合和網(wǎng)絡(luò)連接的處理，包括基于服務(wù)的同步RPC(遠(yuǎn)程過(guò)程調(diào)用)通信和基于話題的異步數(shù)據(jù)流通信。ROS的分布式處理框架可以使每個(gè)可執(zhí)行程序被單獨(dú)設(shè)計(jì)，并且在運(yùn)行時(shí)松散耦合，按照功能包和功能包集的方式進(jìn)行分組，以便于共享和分發(fā)。自2010年正式發(fā)布后，由于其開(kāi)源和易用等特性，迅速得到機(jī)器人研究者的關(guān)注和應(yīng)用[8-9]。

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

得益于ROS的分布式架構(gòu)，根據(jù)移動(dòng)機(jī)器人的不同需求和功能，可對(duì)機(jī)器人的系統(tǒng)軟件進(jìn)行模塊化設(shè)計(jì)和開(kāi)發(fā)[9]，主要包括底盤(pán)運(yùn)動(dòng)控制、主控制器和驅(qū)動(dòng)控制器通信、機(jī)器人模型、鍵盤(pán)遠(yuǎn)程控制、地圖構(gòu)建、導(dǎo)航以及用戶界面等功能模塊。各功能模塊對(duì)應(yīng)ROS中的軟件包基本組織單元，定義為base_controller、rosserial、myrobot、slam、move_base以及amcl包等，每個(gè)包中又有多個(gè)不同的用于實(shí)現(xiàn)具體功能的節(jié)點(diǎn)。軟件包及模塊間的關(guān)系如圖3所示。其中myrobot包包含機(jī)器人的描述文件，機(jī)器人啟動(dòng)時(shí)，使用ROS中的工具加載，向系統(tǒng)發(fā)布機(jī)器人的坐標(biāo)變換關(guān)系，可用于實(shí)際系統(tǒng)，也可用于仿真測(cè)試；base_controller包負(fù)責(zé)機(jī)器人的運(yùn)動(dòng)控制，接受機(jī)器人的線速度和角速度運(yùn)動(dòng)指令，并通過(guò)計(jì)算轉(zhuǎn)換發(fā)送至驅(qū)動(dòng)控制器，同時(shí)接收驅(qū)動(dòng)控制器發(fā)布的驅(qū)動(dòng)輪速度消息，并轉(zhuǎn)換為里程信息和里程坐標(biāo)變換，發(fā)布給其他節(jié)點(diǎn)，該包還提供鍵盤(pán)遠(yuǎn)程控制機(jī)器人移動(dòng)的節(jié)點(diǎn)，可在構(gòu)建地圖階段通過(guò)Wi-Fi局域網(wǎng)遠(yuǎn)程控制機(jī)器人移動(dòng)；rplidar_ros包為RPLIDAR的ROS驅(qū)動(dòng)，負(fù)責(zé)和激光雷達(dá)通信，配置激光雷達(dá)參數(shù)，并讀取傳感器數(shù)據(jù)；rosserial包負(fù)責(zé)通過(guò)USB在主控制器和驅(qū)動(dòng)控制器之間進(jìn)行通信；slam、move_base和amcl包則分別負(fù)責(zé)實(shí)現(xiàn)機(jī)器人的SLAM地圖構(gòu)建、導(dǎo)航以及定位功能。

圖3 室內(nèi)移動(dòng)機(jī)器人ROS系統(tǒng)軟件架構(gòu)

2.2.3 驅(qū)動(dòng)控制算法

驅(qū)動(dòng)控制器主要實(shí)現(xiàn)的功能有：電機(jī)控制、電機(jī)測(cè)速以及和主控制器間的通信。為精確控制機(jī)器人的運(yùn)動(dòng)，同時(shí)保持較好的實(shí)時(shí)性，驅(qū)動(dòng)控制器的運(yùn)行周期為20 ms，采用回調(diào)方式協(xié)調(diào)實(shí)現(xiàn)多個(gè)任務(wù)，其流程如圖4所示。機(jī)器人底盤(pán)使用2個(gè)減速電機(jī)驅(qū)動(dòng)，通過(guò)輸出TTL電平以及PWM信號(hào)驅(qū)動(dòng)A4950功率電路分別控制電機(jī)的方向和轉(zhuǎn)速。電機(jī)控制使用增量式PID控制算法，其表達(dá)式為：

Δu(k) =u(k)-u(k-1)

=KPΔe(k) +KIe(k) +

KD[Δe(k)-Δe(k-1)]

(1)

Δu為控制量u的增量，即輸出電機(jī)驅(qū)動(dòng)PWM信號(hào)占空比的變化量。PID控制器的輸出與前一時(shí)刻控制量疊加，得到當(dāng)前時(shí)刻控制器的實(shí)際輸出值。

圖4 驅(qū)動(dòng)控制器運(yùn)行流程

2.2.4 SLAM功能實(shí)現(xiàn)

HectorSLAM算法基于掃描匹配和慣性傳感系統(tǒng)，得到激光雷達(dá)的掃描數(shù)據(jù)后，對(duì)其中的環(huán)境特征進(jìn)行處理，提取出特征點(diǎn)集，再使用高斯-牛頓方程求解得到最佳匹配激光束的剛性變換，實(shí)現(xiàn)對(duì)機(jī)器人位姿的校正和占用柵格地圖的構(gòu)建[5,10]，其流程、機(jī)器人各個(gè)節(jié)點(diǎn)的關(guān)系以及數(shù)據(jù)流則如圖5所示。

圖5 HectorSLAM功能實(shí)現(xiàn)

①加載URDF機(jī)器人描述文件，發(fā)布機(jī)器人的非固定關(guān)節(jié)的連接狀態(tài)以及各坐標(biāo)系間的變換關(guān)系；②啟動(dòng)驅(qū)動(dòng)控制器和base_controller、odom_publisher節(jié)點(diǎn)，用于控制命令解析、當(dāng)前速度獲取以及里程數(shù)據(jù)的獲取、轉(zhuǎn)換和發(fā)布，同時(shí)啟動(dòng)鍵盤(pán)遠(yuǎn)程控制節(jié)點(diǎn)；③按照預(yù)設(shè)工作參數(shù)配置并啟動(dòng)激光雷達(dá)開(kāi)始工作；④啟動(dòng)地圖構(gòu)建節(jié)點(diǎn)hector_mapping，用于HectorSLAM的具體實(shí)現(xiàn)；⑤啟動(dòng)Rviz界面，用于實(shí)時(shí)觀察地圖構(gòu)建和機(jī)器人的位姿估計(jì)情況，若使用仿真測(cè)試系統(tǒng)，則還需使用Gazebo加載虛擬環(huán)境地圖、機(jī)器人三維模型和機(jī)器人運(yùn)動(dòng)控制插件。

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

機(jī)器人導(dǎo)航功能的實(shí)現(xiàn)主要使用ROS中的move_base軟件包，主要功能包括全局和局部的路徑規(guī)劃、計(jì)算里程信息以及輸出控制命令等。配置好move_base包的節(jié)點(diǎn)、參數(shù)以及代價(jià)地圖，將其加入機(jī)器人導(dǎo)航功能的啟動(dòng)文件，即可實(shí)現(xiàn)導(dǎo)航功能，在已建立好的地圖中指定目標(biāo)位置和方向后，根據(jù)機(jī)器人的傳感器信息，規(guī)劃路徑并控制機(jī)器人到達(dá)目標(biāo)狀態(tài)。

move_base包運(yùn)行必需的參數(shù)有：機(jī)器人在環(huán)境中的期望位姿move_base_simple/goal；機(jī)器人各個(gè)坐標(biāo)系的變換關(guān)系tf；根據(jù)機(jī)器人左右驅(qū)動(dòng)輪推算出的里程信息odom(坐標(biāo)及航向角)；傳感器采集的環(huán)境數(shù)據(jù)sensor topics，即激光雷達(dá)發(fā)布的環(huán)境數(shù)據(jù)話題sensor_msgs/LaserScan。move_base包根據(jù)傳感器的實(shí)時(shí)信息和基于Dijkstra最優(yōu)路徑算法規(guī)劃的路徑，在cmd_vel話題上發(fā)布Twist消息，控制機(jī)器人的運(yùn)動(dòng)。此外導(dǎo)航包可選輸入：使用自適應(yīng)蒙特卡羅方法的移動(dòng)機(jī)器人二維環(huán)境下的概率定位系統(tǒng)amcl包，利用粒子濾波器跟蹤和預(yù)測(cè)機(jī)器人的位姿；map_server則提供先驗(yàn)地圖信息。

3 實(shí)驗(yàn)結(jié)果和討論

如前所述，基于ROS和激光雷達(dá)的室內(nèi)定位和導(dǎo)航系統(tǒng)，系統(tǒng)控制器模塊主要分為主控制器和驅(qū)動(dòng)控制器：主控制器運(yùn)行ROS和SLAM算法，獲取激光雷達(dá)等傳感器的數(shù)據(jù)，構(gòu)建地圖、估算位姿并輸出控制命令；驅(qū)動(dòng)控制器主要負(fù)責(zé)控制機(jī)器人的運(yùn)動(dòng)，通過(guò)串口接收主控制器的控制命令，經(jīng)PID控制算法轉(zhuǎn)換為PWM占空比控制信號(hào)，輸出控制驅(qū)動(dòng)電機(jī)轉(zhuǎn)速。系統(tǒng)軟件部分充分利用ROS分布式架構(gòu)，基于所要實(shí)現(xiàn)的不同功能將軟件分為不同模塊，包括坐標(biāo)變換、驅(qū)動(dòng)控制、地圖構(gòu)建等功能模塊，模塊之間采用ROS提供的話題發(fā)布和訂閱機(jī)制進(jìn)行通信，實(shí)現(xiàn)SLAM和導(dǎo)航定位功能。

3.1 仿真測(cè)試

仿真測(cè)試使用ROS中的Gazebo工具進(jìn)行。為使仿真環(huán)境與實(shí)際環(huán)境更加接近，建立機(jī)器人的物理模型與實(shí)際機(jī)器人平臺(tái)使用相同的URDF機(jī)器人描述文件，通過(guò)XML描述文件的方式添加Gazebo插件，修改機(jī)器人模型的顏色、摩擦力、驅(qū)動(dòng)輪扭矩等參數(shù)和實(shí)際一致。搭建虛擬的仿真環(huán)境如圖6a所示，對(duì)機(jī)器人的SLAM功能和導(dǎo)航功能進(jìn)行測(cè)試，其中SLAM功能選用HectorSLAM算法實(shí)現(xiàn)，使用鍵盤(pán)通過(guò)Wi-Fi局域網(wǎng)對(duì)機(jī)器人的運(yùn)動(dòng)進(jìn)行遠(yuǎn)程控制，讓其從起始位置(虛擬環(huán)境的中心)出發(fā)，完成環(huán)境的遍歷，實(shí)現(xiàn)同步定位和地圖構(gòu)建。根據(jù)最終的測(cè)試結(jié)果，HectorSLAM雖然對(duì)激光雷達(dá)的精度以及幀率要求較高，但是由于其采用基于優(yōu)化的算法來(lái)解決最小二乘問(wèn)題，計(jì)算量小，且不需要里程計(jì)，對(duì)環(huán)境適應(yīng)性良好，構(gòu)建出的地圖精度和準(zhǔn)確度也滿足要求，故本文使用HectorSLAM算法實(shí)現(xiàn)機(jī)器人室內(nèi)定位和導(dǎo)航系統(tǒng)，其在仿真環(huán)境下運(yùn)動(dòng)過(guò)程中地圖更新的大致過(guò)程如圖6b ～c所示，最后構(gòu)建的地圖如圖6d所示。

通過(guò)上述的地圖構(gòu)建過(guò)程，可以得到虛擬環(huán)境的先驗(yàn)地圖，配置move_base包的各節(jié)點(diǎn)關(guān)系以及代價(jià)地圖等參數(shù)，先指定機(jī)器人的起始位姿，再指定目標(biāo)位置和方向，即可啟動(dòng)機(jī)器人的導(dǎo)航。導(dǎo)航啟動(dòng)后，算法自動(dòng)規(guī)劃路徑，并控制機(jī)器人朝目標(biāo)位置運(yùn)動(dòng)，與此同時(shí)可開(kāi)啟機(jī)器人位姿預(yù)測(cè)的粒子云實(shí)時(shí)顯示，其過(guò)程和結(jié)果如圖6e ～f所示。

圖6 仿真測(cè)試過(guò)程(使用HectorSLAM算法)

3.2 實(shí)際環(huán)境測(cè)試

基于ROS和激光雷達(dá)搭建如前圖2中所示的室內(nèi)機(jī)器人原型系統(tǒng)，在真實(shí)環(huán)境下進(jìn)行定位與導(dǎo)航的測(cè)試，實(shí)際測(cè)試環(huán)境如下圖7a中所示，使用隔板圍起不規(guī)則長(zhǎng)條形密閉區(qū)域，內(nèi)部設(shè)置有障礙物，機(jī)器人使用HectorSLAM算法進(jìn)行地圖構(gòu)建和定位，其結(jié)果如圖7b～d中所示，最后構(gòu)建的地圖如圖7e所示，準(zhǔn)確度較高，且實(shí)測(cè)過(guò)程中沒(méi)有與障礙物發(fā)生碰撞。

圖7 實(shí)際測(cè)試環(huán)境及測(cè)試結(jié)果

4 結(jié)束語(yǔ)

室內(nèi)移動(dòng)機(jī)器人以樹(shù)莓派為核心控制器，利用RPLIADR A1激光雷達(dá)采集環(huán)境信息，在ROS分布式框架下進(jìn)行軟件算法的開(kāi)發(fā)，通過(guò)軟件仿真和實(shí)際實(shí)驗(yàn)測(cè)試機(jī)器人系統(tǒng)的性能和表現(xiàn)，結(jié)果表明，可實(shí)現(xiàn)基于掃描匹配算法的SLAM功能、路徑規(guī)劃以及基于粒子濾波的導(dǎo)航功能，具有成本低、模塊化、靈活性強(qiáng)、拓展方便的特點(diǎn)，可作為室內(nèi)機(jī)器人SLAM、導(dǎo)航及ROS框架的基礎(chǔ)實(shí)驗(yàn)平臺(tái)。