基于3D點云SLAM的AGV小車創新改進

徐宇航 吳靜靜 趙仕龍 王養坤 巴爾娜·巴拉提

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

0 引言

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

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

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

1 系統整體方案設計

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

圖1 系統框架圖

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

2 硬件系統改進

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

圖2 硬件框架圖

3 軟件系統改進

3.1 SLAM框架介紹

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

圖3 SLAM框架

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

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

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

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

圖4 回環檢測示意圖

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

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

3.2 深度相機配準

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

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

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

圖5 深度圖配準原理圖

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

3.3 主從機通信

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

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

圖6 交互流程示意圖

4 實驗與結果分析

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

圖7 仿真試驗結果圖

5 結語

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