基于3D點云SLAM的AGV小車創(chuàng)新改進

徐宇航 吳靜靜 趙仕龍 王養(yǎng)坤 巴爾娜·巴拉提

（江南大學，江蘇 無錫 214122）

0 引言

隨著計算機技術和人工智能技術的發(fā)展，機器人的研究日益受到關注，智能移動機器人（Automated Guided Vehicle，AGV）成為機器人領域的一個主要研究方向。其中，AGV小車的定位和地圖創(chuàng)建（Simultaneous Localization and Mapping，SLAM）是AGV領域的熱點研究問題。目前現有的解決方案僅針對已知環(huán)境中的機器人自主定位問題和已知機器人位置的地圖創(chuàng)建問題。然而在大多數環(huán)境中機器人無法利用全局定位系統(tǒng)進行定位，而且事先獲取機器人工作環(huán)境的地圖較為困難，甚至無法實現。這時機器人需要在自身位置不確定的條件下，實時重建地圖，同時利用地圖進行自主定位和導航。這就是使用移動機器人的同時定位與地圖創(chuàng)建問題。

雖然目前關于SLAM的研究取得了一定的進展，并且相關研究已經廣泛應用于室內、水下以及室外等環(huán)境。陽映雯[1]等人利用激光雷達和深度相機，實現小車對環(huán)境數據的采集，通過ROS系統(tǒng)來對智能小車進行環(huán)境的模擬和仿真；張晨陽[2]等人提出了一種適用于動態(tài)場景的RGB-D SLAM算法，利用全新的跨平臺神經網絡深度學習框架檢測場景中的動態(tài)語義特征。但是目前市場上的AGV小車大多通過ROS一體控制，只能處理如激光數據之類的小量數據，而智能化的SLAM需要更加精確和大量的傳感器采集數據，對于硬件和處理算法的性能要求更高，單純依靠ROS系統(tǒng)無法滿足對于大量深度數據的處理需求。且單個深度相機的視野角小于180°，導致機器人的定位和姿態(tài)精度下降。

綜上所述，為了增大視野廣角，同時滿足對于大量深度數據處理與建模的需求，從而提高運動精度，本文對AGV小車整體系統(tǒng)進行了創(chuàng)新改進。并提出了一種基于3D點云SLAM的AGV小車創(chuàng)新改進方法。

1 系統(tǒng)整體方案設計

為了實現AGV小車在室內環(huán)境中的精確定位與環(huán)境地圖構建，對小車系統(tǒng)的硬件和軟件部分分別進行了改進。在硬件系統(tǒng)方面，為了擴大視角，使用兩個深度相機作為系統(tǒng)的數據采集設備。在軟件系統(tǒng)方面，對現有AGV小車的控制方式進行改進，設計PC-ROS主從控制系統(tǒng)，通過PC端完成大量點云數據集的處理與建模，再通過編寫PC-ROS的通信協議，完成PC端的數據傳輸到ROS端的工作，減輕了ROS的計算負擔。本文的系統(tǒng)框架圖如圖1所示。

圖1 系統(tǒng)框架圖

應用層主要為小車的操作系統(tǒng)和傳感器，實現人機交互和信息傳輸。導航層以移動庫為基礎，根據地圖服務器和小車狀態(tài)來規(guī)劃路徑發(fā)送控制信號，并通過反饋數據不斷調整。硬件驅動實時檢測并向導航層發(fā)送小車位置信號。硬件層通過Rosserial通信協議接受控制信號，并根據串口算法來驅動電機。

2 硬件系統(tǒng)改進

本文的硬件系統(tǒng)框架圖如圖2所示，2個RGBD相機采用的是Intel RealSense L515深度相機和樂視體感深度相機。其中Intel RealSense L515深度相機連接在主機上，AGV小車運行前通過SLAM系統(tǒng)對AGV小車的整體運行環(huán)境進行點云構圖，運行過程中對小車的位置進行實時監(jiān)控。AGV小車運行過程中，主機通過SLAM系統(tǒng)獲得小車的運動特征，并將此運動特征傳遞給從機以實現反饋調節(jié)，提高AGV小車初始姿態(tài)和運行過程精度。

圖2 硬件框架圖

3 軟件系統(tǒng)改進

3.1 SLAM框架介紹

如圖3所示為整體視覺SLAM框架，視覺SLAM框架包括傳感器數據模塊、前端視覺里程計模塊、后端非線性優(yōu)化模塊以及回環(huán)檢測模塊。

圖3 SLAM框架

傳感器數據模塊：包括數據采集模塊和數據預處理模塊，深度傳感器采集到深度圖像數據之后，將兩個不同傳感器采集到的深度圖像進行配準，然后將配準后的數據信息傳送到前端視覺里程計和回環(huán)檢測。

前端視覺里程計模塊：視覺里程計是視覺SLAM前端的主要構成部分，根據圖像幀的幀間關系還原相機的位姿變化，從而給后端模塊提供較好的初值。本文采用ORB特征點法。特征點由關鍵點和描述子組成，關鍵點稱為“Oriented FAST”，是一種改進的FAST角點，描述子稱為“BRIEF”。關于特征匹配，采用快速近似最近鄰算法，這種算法不僅可以滿足SLAM中的實時性需求，也適用于匹配點數極多的情況。

后端非線性優(yōu)化模塊：后端的任務為使用非線性誤差函數優(yōu)化位姿圖中相機的位姿信息。

回環(huán)檢測模塊：回環(huán)檢測的前提是相機運行軌跡構成回環(huán)，SLAM系統(tǒng)通過圖像幀之間的相似度來判斷是否產生回環(huán)，本框架中采用詞袋模型進行回環(huán)檢測，設一幅圖像為m，他所有單詞數為n，則該圖像的描述可如式（2）所有。其中為圖像中出現的單詞，為該單詞對應的權重。具體如圖4所示。

圖4 回環(huán)檢測示意圖

則圖像S1和S2之間的相似度可以記為：

后端非線性優(yōu)化模塊：接受不同時刻前端視覺里程計測量的相機位姿，以及回環(huán)檢測信息，得到全局一致的軌跡和地圖；處理SLAM中的噪聲問題，后端優(yōu)化要考慮的是如何從帶有噪聲的數據中，估計整個系統(tǒng)的狀態(tài)，以及這個狀態(tài)估計的不確定性有多大——這稱為最大后驗概率估計，這個狀態(tài)包括機器人自身軌跡也包含地圖。該塊主要是濾波與非線性優(yōu)化算法。

3.2 深度相機配準

近年來，隨著價格親民的Kinect、RealSense等深度相機的出現[3]，研究者們可以方便地獲得深度圖像數據，因此，基于深度信息的場景建模越來越得到廣泛的應用。本文所采用的深度相機為Intel RealSense L515深度相機[4]和樂視體感深度相機，通過深度相機獲取到的三維數據，可以準確得知圖像中各個像素點與相機之間的距離，從而獲取各點在三維空間上的位置坐標，最后通過三維坐標位置進行場景建模[5]，還原真實的小車環(huán)境。

由于兩個深度相機之間存在空間位置與角度的物理偏差，從而使得采集到的深度圖的像素位置不一致，深度圖像之間存在不對齊的現象，導致目標物的深度信息不準確。為了實現反饋調節(jié)，以達到更高的運動精度，需將兩個深度相機獲取的深度圖像進行像素級配準。

深度配準的算法可以選擇以其中一幅深度圖為目標。配準算法的原理圖如圖5所示，將深度圖一的圖像坐標系進行轉換與深度圖二的圖像坐標系進行統(tǒng)一。首先，將深度圖一的像素點映射到深度坐標系下，直接獲取深度圖一坐標系與深度圖二坐標系之間的歐式變換矩陣Td2c；其次，將深度圖二坐標系下的深度點投影到Z=1的平面上。具體算法如下式（3）所示：

圖5 深度圖配準原理圖

式中：Td2c表示深度圖一坐標系與深度圖二坐標系的轉換矩陣；Tw2c表示世界坐標系到深度圖二坐標系的轉換矩陣；Tw2d表示世界坐標系到深度坐標系的轉換矩陣。根據此公式可以將深度圖一與深度圖二進行配準，能夠得到更準確的深度數據，準確而有效地補償了深度圖一與深度圖二之間的像素偏移量。

3.3 主從機通信

對于軟件系統(tǒng)方面，主機通過SLAM系統(tǒng)得到小車的運動特征，將此運動特征傳遞給從機以實現反饋調節(jié)，從而提高AGV小車初始姿態(tài)和運行過程精度。主機與從機的之間通信功能是基于ROS系統(tǒng)的Rosserial模塊實現的。具體交互流程如圖6所示，本文使用Rosserial_server功能包。首先從機安裝Rosserial-Server和Rosserial-Windows，再生成與ROS主設備對話所需的代碼，在消息信息節(jié)點側啟動主服務器節(jié)點；然后在主機上編寫客戶端通訊代碼，使得ROS側與Windows端都可以看到訂閱話題中的信息。至此，主機和從機完成通信，ROS端可接收到Windows端發(fā)送的小車運動特征信號。主從機之間的通信協議如表1所示。

表1 PC-樹莓派之間的通訊協議

圖6 交互流程示意圖

4 實驗與結果分析

根據本文提出的改進方法，在實際應用場景下進行了仿真實驗，記錄并繪制了AGV小車的運行軌跡，軌跡圖如圖7所示。從圖7中可明顯看出小車運行精度較改進前總體上有了較大的提升，更加貼合預定軌跡。

圖7 仿真試驗結果圖

5 結語

本文所設計的基于3D點云SLAM的AGV小車系統(tǒng)具有良好的理論與應用價值。一般情況下RGBD相機的視野角小于180°，采用兩個RGBD相機不僅可以起到擴大觀察角和視野的作用，兩個RGBD相機獲取的小車姿態(tài)信息還可以相互配準，實現反饋調節(jié)，從而可以達到更高的運動精度。此外，本文使用的主從機控制方法可以規(guī)避ROS系統(tǒng)無法處理大量3D點云數據的弊端，利用主機可以更加快速地處理數據和建模，并將處理結果實時傳輸到ROS系統(tǒng)，從而提升定位精度。