基于激光雷達的自主移動機器人實驗教學平臺設計

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

（中國礦業大學機電工程學院，江蘇 徐州 221116）

0 引 言

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

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

1 自主移動機器人實驗教學平臺

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

圖1 自主移動機器人實驗教學平臺

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

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

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

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

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

2 實驗項目開發

2.1 Rviz可視化界面開發實驗

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

圖2 ROS的Rviz可視化界面

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

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

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

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

圖3 融合點云匹配原理

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

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

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

Gazebo是ROS專為機器人設計和開發的3D物理仿真平臺，可用于實現機器人運動學、動力學的仿真、調試與可視化。

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

圖5 SLAM仿真實驗結果

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

3.3 室外建圖實驗

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

圖6 實驗環境衛星圖

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

圖7 環形道路的建圖效果

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

本實驗的目的是讓學生熟悉并直觀感受不同建圖方法在真實場景下的建圖效果，并能根據實際應用場景分析建圖效果的優劣。

3.4 運動路徑規劃實驗

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

實驗的目的是讓學生熟悉RRT 算法的基本原理和實現步驟，了解RRT算法在路徑規劃中的優越性及可行性，并能根據實際應用案例對結果進行科學分析。

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

圖9 改進RRT的原理圖

圖9 中，Fatt為目標點對最近點xnear生成的引力，Frep為障礙物對最近點xnear生成的斥力，兩個分力經過受力合成，得到整個勢場對最近點xnear的總合力Ftotal，由合力方向為軸線生成一個角度為θ、半徑為r的扇形，作為隨機點的拾取區域，取ε 為固定步長，生成新節點xnew。

根據人工勢場法的相關定義，對xnear處的受力進行數學建模，最近點xnear受到的目標點xgoal引力

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

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

式中：

μ為斥力增益系數；α0為障礙物區域對移動機器人運動產生影響的最大距離，可稱為安全距離；d1為最近點xnear到障礙物輪廓的最小歐式距離；k、n均為大于0的實數。

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

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

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

表1 路徑規劃仿真結果對比

表2 室內場景的路徑規劃實驗結果對比

圖10 路徑規劃仿真結果

圖11 路徑規劃實驗結果

4 結 語

針對國家高素質復合型人才培養需求及相關課程的實驗教學要求，開發了基于激光雷達的自主移動機器人實驗教學平臺，實現了地圖構建和路徑規劃等功能，并通過理論分析和實驗測試證明了該平臺的可行性和有效性。

基于該平臺開展的實驗教學項目均屬于綜合性實驗，涉及機械設計、機器人操作系統開發、SLAM 建圖、路徑規劃等相關理論知識。通過開展相關實驗項目，提高學生的創新實踐能力，為學生參加學科競賽和創新實踐項目奠定基礎。同時，學生在實驗過程中，可對實驗教學平臺進一步開展結構創新設計，優化相關算法，拓寬平臺功能，實現教學和科研的協同發展。