基于激光雷達的自主移動機器人實驗教學(xué)平臺設(shè)計

司 壘， 王忠賓， 劉 揚， 劉新華， 朱遠勝

（中國礦業(yè)大學(xué)機電工程學(xué)院，江蘇 徐州 221116）

0 引 言

在以新技術(shù)、新業(yè)態(tài)、新模式、新產(chǎn)業(yè)為代表的新經(jīng)濟背景下，新形勢與新戰(zhàn)略的快速發(fā)展對我國傳統(tǒng)工程人才造成了極大挑戰(zhàn)，迫切需要具備工程實踐能力強、創(chuàng)新能力強以及能適應(yīng)未來發(fā)展的高素質(zhì)復(fù)合型的“新工科”人才［1-3］。為主動適應(yīng)國家、區(qū)域及行業(yè)需求，充分運用大數(shù)據(jù)、云計算、人工智能等升級改造傳統(tǒng)優(yōu)勢專業(yè)，機器人工程、智能制造工程等專業(yè)的實踐類課程迫切需要設(shè)計專門的實驗教學(xué)平臺，系統(tǒng)開展機械結(jié)構(gòu)設(shè)計、嵌入式系統(tǒng)開發(fā)、智能傳感器應(yīng)用等實驗，鍛煉學(xué)生機械、電子、自動化等課程理論體系的綜合應(yīng)用能力［4-6］。

自主移動機器人是一種面向特種行業(yè)的可代替人工進行檢查、作業(yè)的移動裝置，一般由傳感器、數(shù)據(jù)處理和自動控制等模塊組成，能代替人工在危險或惡劣環(huán)境下進行作業(yè)［7-8］。為實現(xiàn)移動機器人自主移動和可靠避障，一般需借助激光雷達、高清攝像頭、紅外熱成像儀等智能傳感器來感知周邊環(huán)境信息［9-10］，國內(nèi)外學(xué)者已進行了大量的研究工作［11-13］。另外，還研究了路徑規(guī)劃、運動控制算法來實現(xiàn)機器人的精確運動［14-15］。高校仍缺乏相關(guān)的實驗教學(xué)平臺來指導(dǎo)學(xué)生進行實踐操作，難以滿足相關(guān)課程教學(xué)需要及培養(yǎng)高素質(zhì)復(fù)合型人才的實際需求。鑒于此，開發(fā)了基于激光雷達的自主移動機器人實驗教學(xué)平臺。該平臺可開展Rviz可視化界面開發(fā)、SLAM仿真與室外建圖、運動路徑規(guī)劃等一系列前沿性綜合實驗，有助于培養(yǎng)學(xué)生利用多學(xué)科交叉理論知識解決工程實際問題的能力。

1 自主移動機器人實驗教學(xué)平臺

常見的移動機器人行走機構(gòu)主要分為輪式、履帶式和足式等。輪式行走機構(gòu)機械結(jié)構(gòu)簡單、行進速率快、容易操控和靈活性高，本實驗裝置選用驅(qū)動輪前置的兩輪差速模型底盤布局方式。由此搭建的實驗教學(xué)平臺如圖1 所示。

圖1 自主移動機器人實驗教學(xué)平臺

自主移動機器人實驗教學(xué)平臺以嵌入式控制系統(tǒng)為核心，由搭載Ubuntu 和機器人操作系統(tǒng)（Robot Operating System，ROS）的小型計算機作為主控制器，激光雷達和慣性測量單元（Inertial Measurement Unit，IMU）提供地圖數(shù)據(jù)和機器人本體的位姿數(shù)據(jù)，實現(xiàn)機器人的環(huán)境感知、同步定位與地圖構(gòu)建和路徑規(guī)劃等功能。底層控制系統(tǒng)將路徑規(guī)劃模塊給出的最優(yōu)軌跡當作輸入，利用控制算法實現(xiàn)對機器人本體自主行走的有效控制。遠程操作系統(tǒng)主要在PC 實現(xiàn)對機器人的遠程操控和數(shù)據(jù)監(jiān)測。自主移動機器人實驗教學(xué)平臺的組成功能模塊：

（1）感知模塊。包括環(huán)境感知與本體感知。環(huán)境感知是通過機器人搭載的外部傳感器（如：深度相機、激光雷達等）采集到的數(shù)據(jù)，獲得周邊環(huán)境狀況的描述。本體感知是通過機器人本體自身搭載的傳感器（如：慣性測量單元、輪速計等）采集到的信息，獲得對機器人本體自身狀態(tài)的描述。

（2）同步定位與地圖構(gòu)建模塊。通過同時定位與地圖構(gòu)建（Simultaneous Localization and Mapping，SLAM）技術(shù)，利用感知模塊中傳感器所獲取周邊環(huán)境數(shù)據(jù)和機器人本體的自身狀態(tài)數(shù)據(jù)，確定機器人當前位置與位姿，并且構(gòu)建局部環(huán)境地圖以及全局環(huán)境地圖。

（3）路徑規(guī)劃模塊。包括全局路徑規(guī)劃和局部路徑規(guī)劃。全局路徑規(guī)劃屬于靜態(tài)路徑規(guī)劃，其是利用全局環(huán)境地圖和靜態(tài)可通行約束，規(guī)劃一條無碰撞的最優(yōu)路徑；局部路徑規(guī)劃屬于動態(tài)路徑規(guī)劃，利用傳感器實時感知外部環(huán)境，規(guī)劃一條效率最優(yōu)的局部路徑。

（4）運動控制模塊。將路徑規(guī)劃的最優(yōu)軌跡作為輸入，利用控制算法實現(xiàn)對機器人本體自主行走的有效控制。

2 實驗項目開發(fā)

2.1 Rviz可視化界面開發(fā)實驗

為使學(xué)生直觀感受采集到的激光雷達點云數(shù)據(jù)，ROS集成了多種可視化工具，其中，Rviz 可使用可擴展標記語言對機器人以及周圍物體的各種屬性進行表述，包括質(zhì)量、尺寸、位置、關(guān)節(jié)和材質(zhì)等，保證物體能在界面很好地顯示。此外，Rviz還可通過圖形化方式，實時顯示機器人傳感器的信息、機器人的運動狀態(tài)和周圍環(huán)境的變化等，如圖2 所示。Rviz根據(jù)ROS 定義的數(shù)據(jù)類型，針對性地開發(fā)了分門別類地顯示格式，對于數(shù)據(jù)類型為PointCloud2 的激光雷達數(shù)據(jù)，用三維空間控件進行點云顯示，通過顯示控制界面的參數(shù)調(diào)節(jié)，可對三維點云的顯示效果進行調(diào)節(jié)。

圖2 ROS的Rviz可視化界面

本實驗要求學(xué)生熟悉Qt 開發(fā)框架下的C ++編程，掌握在ROS 環(huán)境下利用C ++實現(xiàn)可視化上位機與機器人本體間遠程通信的原理，讀懂激光雷達可視化界面程序代碼，熟悉可視化上位機配置信息編寫方法。結(jié)合機器人定位建圖和路徑規(guī)劃實驗修改程序，在上位機Rviz 可視化界面加入拾取目標點控件及顯示不同類型數(shù)據(jù)等功能。通過上述基本實驗，學(xué)生掌握可視化上位機編寫方法，根據(jù)機器人在定位建圖和路徑規(guī)劃過程中的需要，自己設(shè)計和編寫程序進行機器人可視化上位機的開發(fā)，進一步提高學(xué)生的人機交互軟件設(shè)計能力和機器人定位導(dǎo)航控制能力。

3.2 基于IMU緊耦合的SLAM仿真實驗

激光雷達能獲得大量環(huán)境點云信息，機器人的非線性運動會引發(fā)點云畸變。這些問題的存在對數(shù)據(jù)的直接運用極為不利，本實驗利用連續(xù)時間軌跡的畸變校正方法來解決上述問題。

為提高點云匹配算法的運行效率，同時解決環(huán)境退化給點云特征提取帶來的問題，實驗設(shè)計了一種基于IMU緊耦合的融合點云匹配策略，如圖3 所示。

圖3 融合點云匹配原理

IMU 數(shù)據(jù)經(jīng)過處理后，生成關(guān)于時間的連續(xù)軌跡，通過對時間的查詢即可獲得該時刻的位姿變換信息；Lidar數(shù)據(jù)中的每一點都有一個采集時間戳，利用該采集時間戳，可在連續(xù)時間軌跡中查詢獲得該點采集時刻的真實坐標系，通過坐標變換將點進行逐一校正，最后得到校正后的點云。

圖4 為基于IMU 緊耦合的融合建圖方案。前端點云匹配得到位姿變換后，可將點云再一次投影到初始時刻，得到更為精確的點云。因為激光雷達本身觀測存在一定的誤差，點云匹配算法不可避免地產(chǎn)生累計誤差，隨著時間的推移，將會發(fā)生一定程度的漂移。這一過程將會以位姿圖的形式將關(guān)鍵幀形成約束，進行位姿優(yōu)化，消除累計誤差。局部的位姿圖優(yōu)化遠不能達到預(yù)期效果，閉環(huán)檢測可將經(jīng)過曾經(jīng)到達的點的位姿圖加入優(yōu)化過程，形成閉環(huán)優(yōu)化，進行調(diào)整全局運動軌跡。

圖4 基于IMU緊耦合的融合SLAM方案

Gazebo是ROS專為機器人設(shè)計和開發(fā)的3D物理仿真平臺，可用于實現(xiàn)機器人運動學(xué)、動力學(xué)的仿真、調(diào)試與可視化。

圖5 為本實驗在該平臺下建立的仿真模型及其建圖效果。可見，本實驗采用的建圖方法效果較好，沒有出現(xiàn)地圖漂移，滿足實驗需求。

圖5 SLAM仿真實驗結(jié)果

本實驗的主要目的是讓學(xué)生熟悉ROS 下物理仿真平臺的使用方法，了解當前主流SLAM 方法的基本原理及流程，理解IMU在激光雷達點云處理過程中的輔助作用，掌握SLAM的仿真分析過程。

3.3 室外建圖實驗

為測試SLAM的建圖效果，選取的室外實驗環(huán)境如圖6 所示。圖6（a）為處于兩棟教學(xué)樓之間的環(huán)形道路，全程長度約為200 m；圖6（b）為學(xué)院周圍的大場景道路，全程長度約為600 m。

圖6 實驗環(huán)境衛(wèi)星圖

實驗選取雷達慣性里程計緊耦合算法（Tightlycoupled Lidar Inertial Odometry via Smoothing and Mapping，LIO-SAM）進行對比分析，建圖效果如圖7、8所示，可見，LIO-SAM 算法在中間某處中出現(xiàn)了明顯偏轉(zhuǎn)，回環(huán)檢測沒能及時進行糾正，導(dǎo)致后續(xù)的地圖偏移了原坐標系，且出現(xiàn)了不同程度的點云重影現(xiàn)象；本實驗采用的融合SLAM 建圖結(jié)果基本實現(xiàn)了閉環(huán)，沒有出現(xiàn)明顯的重影與漂移。

圖7 環(huán)形道路的建圖效果

圖8 大場景道路的建圖效果

本實驗的目的是讓學(xué)生熟悉并直觀感受不同建圖方法在真實場景下的建圖效果，并能根據(jù)實際應(yīng)用場景分析建圖效果的優(yōu)劣。

3.4 運動路徑規(guī)劃實驗

路徑規(guī)劃的目的是為機器人提供一條從起點到終點、且能有效避障的最優(yōu)運動路線。常用路徑規(guī)劃算法包括：A*算法、Dijkstra算法、Floyd算法、快速擴展隨機樹（Rapidly-Exploring Random Tree，RRT）算法和人工勢場法等。本實驗探討RRT算法的應(yīng)用效果。

實驗的目的是讓學(xué)生熟悉RRT 算法的基本原理和實現(xiàn)步驟，了解RRT算法在路徑規(guī)劃中的優(yōu)越性及可行性，并能根據(jù)實際應(yīng)用案例對結(jié)果進行科學(xué)分析。

通過分析發(fā)現(xiàn)，RRT 算法具有較強的搜索能力，且路徑規(guī)劃速度快，但該算法隨機性較強，產(chǎn)生的冗余點多，每次規(guī)劃產(chǎn)生的路徑都是不一樣的非最優(yōu)路徑。鑒于此，實驗引入人工勢場法中對工作空間內(nèi)障礙物和目標點的分析思想，通過對工作空間的引力勢場布置，讓RRT算法將環(huán)境因素考慮進去，用勢場合力方向代替算法原本的隨機點偏向概率，讓隨機樹每次擴展的新節(jié)點有更大的可能性快速向目標點靠攏，用以抑制隨機樹擴展方向無序性的問題。改進原理見圖9。

圖9 改進RRT的原理圖

圖9 中，F(xiàn)att為目標點對最近點xnear生成的引力，F(xiàn)rep為障礙物對最近點xnear生成的斥力，兩個分力經(jīng)過受力合成，得到整個勢場對最近點xnear的總合力Ftotal，由合力方向為軸線生成一個角度為θ、半徑為r的扇形，作為隨機點的拾取區(qū)域，取ε 為固定步長，生成新節(jié)點xnew。

根據(jù)人工勢場法的相關(guān)定義，對xnear處的受力進行數(shù)學(xué)建模，最近點xnear受到的目標點xgoal引力

式中：α（xnear，xgoal）為最近點xnear到目標點xgoal的歐氏距離；λ為引力增益系數(shù)；d*為目標點引力距離閾值。

同理，最近點xnear受到的障礙物斥力

式中：

μ為斥力增益系數(shù)；α0為障礙物區(qū)域?qū)σ苿訖C器人運動產(chǎn)生影響的最大距離，可稱為安全距離；d1為最近點xnear到障礙物輪廓的最小歐式距離；k、n均為大于0的實數(shù)。

此時，最近點xnear受到的合力為

因此，沿合力Ftotal（xnear）的方向進行隨機點采樣，新節(jié)點xnew位置坐標為

圖10、11 分別為RRT 路徑規(guī)劃的仿真結(jié)果和實驗結(jié)果。在仿真分析時，設(shè)置了4 個障礙物，該實驗采用改進RRT路徑規(guī)劃算法可捕獲周圍環(huán)境中的障礙物以及目標點的信息因子，有效避開障礙物而到達終點，詳細對比結(jié)果見表1?？梢?，本文改進RRT算法在平均路徑長度、平均運行時間和平均生成節(jié)點數(shù)3 個指標均優(yōu)于原RRT 算法，表明本文改進的RRT 算法在移動機器人路徑規(guī)劃方面的優(yōu)越性和可靠性。實驗過程中，設(shè)置了不規(guī)則擺放的障礙物（椅子），通過激光雷達建立先驗環(huán)境地圖，為進一步突出顯示障礙物和墻體特征，將地圖分辨率設(shè)置為0.35 m，使得地圖整體平整規(guī)則，并將底部點云去除，用灰色作為底色，得到如圖11（b）所示的地圖俯視圖。在此基礎(chǔ)上，利用RRT算法生成無碰撞行走路徑，如圖11（c）、（d）所示。詳細對比結(jié)果見表2，實驗結(jié)果表明，本實驗平臺在SLAM建圖的基礎(chǔ)上，采用改進RRT算法得到的移動機器人路徑規(guī)劃效果良好，其平均路徑更短、所用時間更少，滿足實驗平臺對移動機器人的性能要求。

表1 路徑規(guī)劃仿真結(jié)果對比

表2 室內(nèi)場景的路徑規(guī)劃實驗結(jié)果對比

圖10 路徑規(guī)劃仿真結(jié)果

圖11 路徑規(guī)劃實驗結(jié)果

4 結(jié) 語

針對國家高素質(zhì)復(fù)合型人才培養(yǎng)需求及相關(guān)課程的實驗教學(xué)要求，開發(fā)了基于激光雷達的自主移動機器人實驗教學(xué)平臺，實現(xiàn)了地圖構(gòu)建和路徑規(guī)劃等功能，并通過理論分析和實驗測試證明了該平臺的可行性和有效性。

基于該平臺開展的實驗教學(xué)項目均屬于綜合性實驗，涉及機械設(shè)計、機器人操作系統(tǒng)開發(fā)、SLAM 建圖、路徑規(guī)劃等相關(guān)理論知識。通過開展相關(guān)實驗項目，提高學(xué)生的創(chuàng)新實踐能力，為學(xué)生參加學(xué)科競賽和創(chuàng)新實踐項目奠定基礎(chǔ)。同時，學(xué)生在實驗過程中，可對實驗教學(xué)平臺進一步開展結(jié)構(gòu)創(chuàng)新設(shè)計，優(yōu)化相關(guān)算法，拓寬平臺功能，實現(xiàn)教學(xué)和科研的協(xié)同發(fā)展。