基于激光SLAM實(shí)現(xiàn)AGV自主導(dǎo)航技術(shù)的仿真

張開樂，陳興文

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

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

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

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

圖1 Cartographer算法流程圖

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

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

.

(1)

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

.

(2)

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

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

(3)

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

2.1 ROS和Gazebo簡介

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

2.2 AGV小車模型建立

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

圖2 URDF模型

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

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

圖3 仿真地圖和柵格地圖

3 已知地圖下的定位與導(dǎo)航的仿真

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

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

.

(4)

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

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

圖5 導(dǎo)航過程仿真

4 結(jié)語

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