張開樂，陳興文

(大連民族大學信息與通信工程學院,遼寧 大連 116600)

隨著智能領域的快速發(fā)展，激光SLAM(Simultaneous localization and mapping)技術具有穩(wěn)定、可靠等性能優(yōu)勢，把該技術融入到AGV小車中進行自主導航[1]，可以大大提升AGV小車工作的靈活性和配送效率?？梢娮灾鲗Ш郊夹g是提高AGV小車智能化程度的核心技術之一，其中的同步定位與地圖構建是解決其自主導航的關鍵問題。目前激光SLAM可分為兩大類：基于濾波器和基于圖優(yōu)化的SLAM算法[2]。其中濾波器算法是用粒子濾波來估算機器人的位姿，并根據(jù)估算的位姿構建地圖。但是該類算法存在兩個問題，一是在大場景下，若里程計累計誤差較大，則需要用較多的粒子來表示機器人位姿的后驗概率分布，嚴重消耗內存；二是隨著重采樣次數(shù)的增多，粒子多樣性散失，粒子耗散問題會嚴重影響地圖的構建。而基于圖優(yōu)化的算法依靠前端回環(huán)檢測和后端優(yōu)化過程，優(yōu)化所有的位姿能夠減小里程計帶來的累計誤差，使定位和建圖的精度較高。因此采用基于圖優(yōu)化的Cartographer算法[3]，融入里程計信息和激光雷達數(shù)據(jù)實現(xiàn)小車的定位和環(huán)境地圖的構建，可以更好的實現(xiàn)自主導航功能。

1 基于圖優(yōu)化的Cartographer算法原理

Cartographer是Google推出的一套基于圖優(yōu)化的SLAM算法，主要由前端和后端兩部分組成。Cartographer算法流程圖如圖1所示。

圖1 Cartographer算法流程圖

首先，傳感器的數(shù)據(jù)獲取，主要利用激光雷達數(shù)據(jù)、底盤里程計數(shù)據(jù)以及IMU數(shù)據(jù)，其中激光雷達數(shù)據(jù)是必須項。對獲取后的激光雷達數(shù)據(jù)使用體素濾波器篩選出指定范圍的點，并通過自適應計算獲得適合的采樣間隔，以減少點云數(shù)據(jù)量；濾波之后的激光雷達數(shù)據(jù)被加入到前端里面進行掃描匹配；同時對輸入的里程計和IMU數(shù)據(jù)加入到位姿插值器中并融入上一次前端匹配完成的位姿，以估算出下一次前端匹配的初始位姿，并給到掃描匹配中作為其初值。

其次，前端主要負責構建一系列的柵格子圖。子圖的構建就是一段時間內對一幀一幀激光掃描匹配對齊和子圖坐標迭代的過程。其思想就是通過變換矩陣Tξ將每一幀雷達掃描點觀測數(shù)據(jù)獲得位姿的平移量(ξx,ξy)和旋轉量ξθ，從當前雷達觀測值(Scan)的坐標系轉換到子圖坐標系，具體公式如(1)所示：

.

(1)

一幀Scan插入到子圖之前，要通過掃描匹配完成點云位姿的最優(yōu)化，確保Scan中的點最大概率地匹配到子圖上。通過相關性掃描匹配(realtimeCSM)對激光幀數(shù)據(jù)在子圖的柵格地圖上進行一次粗匹配，產生一個比較好的位姿值，之后將該位姿再一次與子圖進行優(yōu)化匹配進行精細的搜索。在這里主要采用自帶的Ceres非線性優(yōu)化庫通過最小二乘法進行最優(yōu)化求解。具體公式如(2)所示：

.

(2)

式中變換矩陣Tξ能把所有的Scan點hk轉換到子圖坐標系下，通過M函數(shù)用雙三次樣條插值使雷達點變成在子圖中概率值的平滑形式，式中M取概率值為0到1之間的數(shù)。最后根據(jù)運動濾波器檢測此次姿態(tài)運動的大小，當小于某閾值時進行數(shù)據(jù)去除不加入到子圖中，然后根據(jù)優(yōu)化好的位姿將點云幀插入到子圖中進行疊加，實現(xiàn)子圖的構建。子圖采用分辨率為5 cm的離散柵格地圖表示，每個2 d的小格子都有自己的概率值P位于Pmin和Pmax空間當中，并且每一幀的激光點都會生成一組稱為命中和未命中的柵格點，每一次Scan插入到子圖上，都要更新命中集和非命中集。當激光點打在一個小格子上時，被擊中的格子更新命中集(就是激光點打在障礙物上的概率),對每個命中的點與激光原點連線上的點更新其未命中集，而沒有進行命中或未命中的點為其自定義分配一個概率值，逐一對地圖中每個柵格進行概率更新。當子圖創(chuàng)建完成時，柵格的概率值小于Pmin時，表示這個點無障礙物，中間為不確定，大于Pmax表示有障礙物。

最后，后端目的是通過在大場景下創(chuàng)建小的子圖來抑制誤差的累計，短時間內連續(xù)的激光數(shù)據(jù)組成的子圖誤差累計很小，但是當大量的子圖被創(chuàng)建完成時，就會有誤差累計，這時引入后端回環(huán)檢測，優(yōu)化所有子圖的位姿。位姿圖的構建其實就是一個回環(huán)檢測匹配的過程。把所有創(chuàng)建完成的子圖和當前的Scan數(shù)據(jù)用作回環(huán)檢測的初值，在其附近一定范圍內的搜索窗口中搜索最優(yōu)匹配幀，如果檢測到當前的掃描幀Scan和已創(chuàng)建的子圖中的某個Scan的位姿在距離上足夠近，則將子圖和掃描幀進行匹配，若最優(yōu)的匹配幀符合要求，則認為是一個回環(huán)。為了提高在子圖中的搜索效率減少計算量，通過分支定界的優(yōu)化方法進行優(yōu)化搜索，計算各個Scan和子圖之間的粗略的初始相對位姿，如果找到好的匹配，則調用Ceres的非線性優(yōu)化對粗匹配的結果進行精細匹配調整，從而構建位姿圖。

(3)

2 AGV小車仿真與環(huán)境建模

2.1 ROS和Gazebo簡介

ROS是機器人操作系統(tǒng)的簡稱，提供了一套用于描述和建立小車模型的工具，并可進行數(shù)據(jù)記錄、分析等。ROS采用松散耦合的機制，運行時由多個松耦合的進程組成，每個進程稱之為節(jié)點，所有節(jié)點可以運行在一個處理器上，也可以分布式運行在多個處理器上。Gazebo是一款功能強大的三維物理仿真平臺，與ROS有較好的兼容性，可以加載具有傳感器數(shù)據(jù)的小車模型和三維可視化環(huán)境的仿真；可以使用Gazebo提供的命令行工具在終端實現(xiàn)仿真控制。

2.2 AGV小車模型建立

在ROS中通過URDF(United Robotics Description Format)對小車模型建立，并可以實現(xiàn)模型在Gazebo仿真環(huán)境中的建圖與分析。AGV小車模型的構建是通過多個連桿(link)和關節(jié)(joint)實現(xiàn)的，每個link之間通過joint連接，joint則通過parent-child關系把上下link聯(lián)系起來，同時添加link部分的外觀參數(shù)，實現(xiàn)小車外形的構建。小車的外觀形狀完成后，需要定義link所在的位置，由于各個link通過joint進行連接，那么link的位置就由joint確定，所以使用子標簽origin在joint內定義位置。在定義URDF建模時存在一些問題，會出現(xiàn)模型冗長重復內容過多，沒有參數(shù)計算功能等，這時可用Xacro文件來優(yōu)化URDF模型，精簡模型代碼，并調用Xacro文件解析器，解析成小車模型。URDF模型如圖2所示。

圖2 URDF模型

2.3 仿真環(huán)境下小車SLAM的實現(xiàn)

在Gazebo中完成仿真環(huán)境的搭建，并加載小車模型，然后用Cartographer_ros功能包進行環(huán)境地圖的構建。該功能包是基于ROS的通信機制并獲取各種傳感器的數(shù)據(jù)將其轉換成Cartographer算法中定義的格式傳遞給Cartographer進行處理，經過處理后又以消息的形式發(fā)布出去，便于調試和可視化。配置其中的Lua文件和Launch文件，通過roslaunch命令啟動鍵盤控制節(jié)點mbot_teleop控制小車運動進行建圖，并通過Rviz觀察地圖的建立，把已建好的地圖保存在Navigation功能包下為自主導航提供一張地圖。仿真地圖如圖3(a)所示，柵格地圖如圖3(b)所示。

圖3 仿真地圖和柵格地圖

3 已知地圖下的定位與導航的仿真

在Navigation功能包下的Launch文件中運行地圖服務器map_server節(jié)點將上面已經構建好的柵格地圖加載進來，啟動AMCL節(jié)點估算小車在地圖坐標系下的位姿信息，并利用move_base功能包完成全局和局部的路徑規(guī)劃，實現(xiàn)最終的導航功能[4]。

AMCL(即自適應蒙特卡洛定位)算法的實現(xiàn)主要是在已知環(huán)境地圖下獲取激光雷達數(shù)據(jù)、里程計數(shù)據(jù)，利用粒子濾波方法得到機器人在地圖中的位姿。首先在地圖空間上很均勻的撒一把粒子，并初始化所有粒子的方向和位置；其次，根據(jù)機器人的運動參數(shù)(里程計，速度等)對粒子的狀態(tài)進行更新；再次，根據(jù)傳感器的觀測數(shù)據(jù)，計算出每一個粒子的權重并更新其狀態(tài)；之后判斷是否進行隨機采樣，隨機采樣公式如(4)所示。

.

(4)

其中參數(shù)ωslow和ωfast分別是長期似然平均估計權重和短期似然平均估計權重。如果短期似然優(yōu)于長期似然，則不增加隨機采樣，粒子群進行重采樣操作，否則按兩者之比的比例增加隨機采樣，以這種方式可以消除瞬時傳感器噪聲帶來的定位誤差；最后對粒子群進行重采樣，去除權重小的粒子，保留權重大的粒子，進行不斷迭代，粒子群會慢慢收斂到一起并得到機器人的真實位姿，完成機器人定位。AMCL定位流程圖如圖4所示。

利用ROS中move_base導航功能包配置其中的yaml文件實現(xiàn)全局和局部的路徑規(guī)劃，通過訂閱激光雷達數(shù)據(jù)、柵格地圖以及AMCL定位數(shù)據(jù)，利用Dijkstra算法規(guī)劃出全局路徑和DWA算法規(guī)劃出局部路徑，再將路徑轉化為小車的速度信息，把控制命令Twist發(fā)布到速度控制cmd_vel主題上，其中Twist消息包括小車前進速度和角速度，最終實現(xiàn)對小車左右輪速度的控制。導航過程仿真如圖5所示。

圖5 導航過程仿真

4 結語

本文對Cartographer SLAM算法的構圖和定位原理進行分析，從軟件仿真的角度基本實現(xiàn)了AGV小車的地圖構建、定位以及自主導航功能的仿真設計，并對導航系統(tǒng)的避障功能進行仿真測試與分析。但是對于Cartographer算法在優(yōu)化過程中的定位、建圖精度以及魯棒性上還需要進一步提升性能，以確保復雜工作場景如顛簸路面、斜坡、長距離、等寬度、少特征巷道下的定位、建圖與導航的精度與適應性。