掃地機(jī)器人增強(qiáng)位姿融合的Cartographer 算法及系統(tǒng)實現(xiàn)?

張 亮 , 劉智宇 , 曹晶瑛 , 沈沛意 , 蔣得志 , 梅 林 , 朱光明, 苗啟廣

1(西安電子科技大學(xué) 計算機(jī)科學(xué)與技術(shù)學(xué)院,陜西 西安 710071)

2(北京石頭世紀(jì)科技有限公司,北京 100039)

3(上海寬帶技術(shù)及應(yīng)用工程研究中心,上海 200436)

同步定位與地圖構(gòu)建(simultaneous localization and mapping,簡稱SLAM)產(chǎn)生于機(jī)器人領(lǐng)域[1],它指的是:機(jī)器人從未知環(huán)境的未知地點出發(fā),在運動過程中,通過重復(fù)觀測到的各傳感器數(shù)據(jù)定位自身位置,再根據(jù)自身位置構(gòu)建周圍環(huán)境的增量式地圖,從而達(dá)到同時定位和地圖構(gòu)建的目的.服務(wù)機(jī)器人為實現(xiàn)高智能化,在復(fù)雜的非結(jié)構(gòu)場景完成任務(wù),SLAM 技術(shù)是必要條件[2,3].

目前廣泛應(yīng)用于SLAM 上的傳感器分為攝像頭和激光雷達(dá)兩種,對應(yīng)視覺SLAM[4]和激光SLAM.在實際使用中,單個傳感器難以適應(yīng)復(fù)雜多變的環(huán)境要求,需要慣性測量單元以及里程計等傳感器輔助,增強(qiáng)環(huán)境的適應(yīng)性.視覺SLAM 的優(yōu)點是具有語義信息、成本低,但易受光照影響,同時計算量較大.激光SLAM 具有可靠性高、精度高、地圖可用于路徑規(guī)劃等優(yōu)點,是目前最主流的定位導(dǎo)航方法,缺點是無語義信息且成本較高.

SLAM 本質(zhì)是個系統(tǒng)狀態(tài)估計問題,即:在給定系統(tǒng)輸入的條件下,估計出機(jī)器人的位姿和地圖點的坐標(biāo),延伸出基于貝葉斯濾波[5]以及基于圖優(yōu)化[6]的兩類SLAM 算法.

基于貝葉斯濾波的方法是一種貝葉斯迭代狀態(tài)估計理論,即先對機(jī)器人運動進(jìn)行建模,構(gòu)造出貼合物理場景的運動方程和觀測方程,進(jìn)行狀態(tài)預(yù)測和測量更新.濾波類算法最大的問題是無法處理大尺度場景的建圖.由于濾波類算法是基于遞歸計算的,下一時刻的估計值依賴于上一時刻的估計,因而在大尺度場景下,由于系統(tǒng)參數(shù)和傳感器觀測數(shù)據(jù)的不準(zhǔn)確性,會造成誤差慢慢累積,一旦當(dāng)前時刻的估計出現(xiàn)偏差,無法修正該誤差,最終將無法獲得一致性的地圖.基于圖優(yōu)化SLAM 的出現(xiàn),解決了大尺度場景建圖的問題.圖優(yōu)化SLAM 分為前端和后端兩個模塊:前端先從里程計獲得初始的位姿,然后通過激光雷達(dá)數(shù)據(jù)連續(xù)幀匹配,求得激光約束后的位姿;后端采用回環(huán)檢測,構(gòu)造閉環(huán)約束,通過最小化觀測和估計殘差求得優(yōu)化后的位姿[7].在前端中會產(chǎn)生由估計所引入的誤差,隨著運動范圍的擴(kuò)大,該誤差會逐漸累積,導(dǎo)致最后產(chǎn)生錯誤的結(jié)果.為解決該問題,在后端使用閉環(huán)檢測進(jìn)行全局優(yōu)化,通過判斷機(jī)器人是否回到歷史中的某一點,添加約束較強(qiáng)的閉環(huán)約束,消除誤差累積.

圖優(yōu)化的SLAM 核心是構(gòu)建位姿圖,圖中每個節(jié)點表示不同時刻機(jī)器人的位姿,邊由各個節(jié)點間的變換關(guān)系構(gòu)成.其中:節(jié)點是優(yōu)化變量;邊是誤差項,隨著時間的累積,這個誤差會越來越大.圖構(gòu)建完成后,需要調(diào)整機(jī)器人位姿去滿足邊構(gòu)成的約束,最終目的是求解機(jī)器人位姿使目標(biāo)函數(shù)即誤差平方函數(shù)最小,可表示為

其中,xi,xj表示機(jī)器人位姿,zi,j表示測量值,Ωi,j即權(quán)重矩陣.

本文的研究內(nèi)容包括:

(1) 以應(yīng)用日趨廣泛的掃地機(jī)器人為研究對象,依托于低成本硬件嵌入式掃地機(jī)器人,以Cartographer 算法[8]為核心,在Player 系統(tǒng)平臺上,融合激光雷達(dá)、慣性導(dǎo)航儀以及里程計等多種傳感器數(shù)據(jù)來感知環(huán)境、構(gòu)建地圖,并在各種實際場景進(jìn)行實驗;

(2) 針對Cartographer 中,位姿融合采用UKF(unscented kalman filter)濾波方法帶來計算量較大且提供下一時刻位姿估計不夠準(zhǔn)確的問題,本文提出了一個基于位姿增量的多傳感器位姿融合方法的增強(qiáng)Cartographer 算法;

(3) 基于掃地機(jī)器人Player 平臺,實現(xiàn)了一個基于增強(qiáng)Cartographer 算法的SLAM 系統(tǒng),系統(tǒng)分為6 個模塊:系統(tǒng)配置模塊完成初始配置,其中,接口通信模塊完成與平臺上其他系統(tǒng)的交互,Player 傳感器數(shù)據(jù)交互模塊完成與Player 平臺的交互,數(shù)據(jù)管理及分發(fā)模塊完成與算法交互,算法模塊完成建圖,數(shù)據(jù)處理算法模塊進(jìn)行數(shù)據(jù)處理.

1 增強(qiáng)位姿融合的Cartographer 算法

本節(jié)在Cartographer 算法的基礎(chǔ)上設(shè)計了基于增強(qiáng)型的位姿融合方法:首先給出增強(qiáng)型位姿融合的SLAM算法框架,該算法包含局部SLAM(前端)和全局SLAM(后端)兩部分;然后,對局部SLAM 的組成和基于位姿增量的多傳感器位姿融合進(jìn)行分析設(shè)計;最后分析了全局優(yōu)化過程中基于分支定界法加速回環(huán)檢測的算法.

1.1 Cartographer算法架構(gòu)

Cartographer 是一個實時的室內(nèi)建圖算法,能生成分辨率r=5cm 的柵格地圖[9,10].在前端將最新的激光雷達(dá)掃描數(shù)據(jù)在相鄰的子圖上(整個地圖的一小塊)完成掃描匹配,得到一個在短時間內(nèi)準(zhǔn)確的最佳插入位置(位姿)后,將掃描插入到子圖中.掃描匹配中,位姿估計的誤差會在整個地圖中隨時間逐漸累積,在后端中,通過回環(huán)檢測加約束進(jìn)行優(yōu)化消除誤差.

圖1 為Cartographer 算法框架圖.整個框架分為數(shù)據(jù)源、局部SLAM 以及全局SLAM 以及數(shù)據(jù)融合等部分,其中,紅虛線框表示本文設(shè)計的位姿融合方法所處位置,紅框表示融合的數(shù)據(jù).

Cartographer 是基于圖優(yōu)化的SLAM,采用誤差累積小、計算成本低的Scan to Map 匹配方式,而不是會造成誤差快速累積以及計算成本高的Scan to Scan 匹配方式[11,12].整個算法分為局部SLAM 和全局SLAM 兩部分,兩部分均會對激光雷達(dá)觀測的位姿(即掃描)進(jìn)行優(yōu)化.

· 在局部SLAM 中,使用非線性優(yōu)化將每個連續(xù)的掃描與子圖進(jìn)行匹配,優(yōu)化將掃描與子圖對齊,也就是掃描匹配,生成較好的子圖.在這部分中,設(shè)計了基于位姿增量的多傳感器位姿融合方法,更快速、更準(zhǔn)確地為下一時刻提供估計位姿;

· 而在全局SLAM 中,將局部SLAM 累積的誤差消除,將不同的子圖以最匹配的位姿融合.

1.2 局部SLAM的組成與位姿優(yōu)化

一幀掃描即一次激光雷達(dá)采集的數(shù)據(jù),子圖由多個掃描構(gòu)成,構(gòu)建子圖是一個重復(fù)對齊掃描和子圖坐標(biāo)幀的迭代過程.多個掃描構(gòu)成的子圖由概率網(wǎng)格表示,將給定分辨率的離散網(wǎng)格點映射為值,這些值是網(wǎng)格點被占用的可能.對于每個網(wǎng)格點,定義對應(yīng)的像素以包含最接近這個網(wǎng)格點的所有點.

在將一個掃描插入子圖之前,使用基于Ceres[13]的掃描匹配器對插入的掃描位姿相對于當(dāng)前匹配的子圖進(jìn)行局部優(yōu)化.掃描器的目標(biāo)是在子圖中找到使插入掃描中掃描點概率最大的掃描位姿,可以將此問題轉(zhuǎn)變?yōu)橐粋€非線性最小二乘問題:

其中,Tξ表示掃描轉(zhuǎn)換到對應(yīng)子圖幀中的位姿變換,位姿變換將掃描點hk從掃描轉(zhuǎn)換到子圖幀中,平滑函數(shù)Msmooth:R2→R將每個掃描點概率值平滑映射到局部子圖中,目標(biāo)是使插入掃描的所有掃描點概率值最大,構(gòu)建非線性最小二乘目標(biāo)函數(shù).

1.3 基于位姿增量的多傳感器位姿融合

局部SLAM 的目標(biāo)是使用除了激光雷達(dá)之外的傳感器數(shù)據(jù)來預(yù)測下一個掃描應(yīng)該被插入到子圖的位置.在原本的位姿融合估計部分中,Cartographer 算法利用IMU(inertial measurement unit)構(gòu)建預(yù)測模型,掃描匹配器與里程計構(gòu)建觀測模型,采取無損濾波器進(jìn)行運動預(yù)測.當(dāng)掃描匹配器估計的位姿或者里程計數(shù)據(jù)到來時,利用觀測值與系統(tǒng)狀態(tài)變量均值的矢量差修正狀態(tài)變量(只有IMU 數(shù)據(jù)到來時,只進(jìn)行預(yù)測不進(jìn)行更新)及其協(xié)方差.這種方法中存在的問題有:對各數(shù)據(jù)源的時間戳對準(zhǔn)不夠精確;IMU 位姿的融合過程只有在掃描匹配器估計的位姿或里程計數(shù)據(jù)到來時才能做狀態(tài)更新,這樣會造成位姿估計較為滯后;沒有通過線速度而是直接使用里程計數(shù)據(jù)估計相鄰時刻位姿增量,精度較低.

針對基于UKF 方法[14]中位姿融合存在的問題,本文設(shè)計了一種基于位姿增量的多傳感器位姿融合方法.圖2 給出了該方法的具體描述.

該方法基于時間戳對齊數(shù)據(jù),可以在IMU 以及里程計等輔助傳感器的幫助下提供更精確的下一時刻包含旋轉(zhuǎn)和平移的位姿估計.在位姿融合中,因為IMU 和里程計的采集速率比激光雷達(dá)傳感器高很多,因而在一定時間段內(nèi)可能沒有激光掃描匹配估計的位姿數(shù)據(jù),這時需要通過IMU 和里程計數(shù)據(jù)進(jìn)行推斷;同時,因為這兩個傳感器存在相對激光掃描較大的累積誤差,所以在有估計位姿的時候?qū)烧哌M(jìn)行一些更新和初始化消除累積誤差;同時,在估計位姿的旋轉(zhuǎn)量時,可以在沒有IMU 數(shù)據(jù)時利用之前的位姿或者里程計估算出的速度進(jìn)行模擬,在一定程度上可以保持位姿數(shù)據(jù)的穩(wěn)定性.本方法將所有數(shù)據(jù)進(jìn)行緩存,放在雙端隊列中,便于新數(shù)據(jù)的添加和舊數(shù)據(jù)的刪除.算法不斷輪詢處理來自掃描匹配器優(yōu)化的位姿、IMU 數(shù)據(jù)以及里程計數(shù)據(jù).在獲取最后估計位姿以及處理IMU 和里程計兩種傳感器數(shù)據(jù)期間,位姿的變化由3 個跟蹤器表示,基于IMU 數(shù)據(jù),對這3 個跟蹤器進(jìn)行更新.最后獲取融合完成的位姿,提供給掃描匹配器作為初值.

對各數(shù)據(jù)源的數(shù)據(jù)進(jìn)行了嚴(yán)格的時間對齊后,能夠為后續(xù)的非線性優(yōu)化提供一個較為準(zhǔn)確的估計初值.同時,在使用該位姿融合方法后,需要在算法外部配合激光雷達(dá)的傳感器發(fā)布速率,同時添加一個發(fā)布局部位姿優(yōu)化的模塊,將局部優(yōu)化的位姿類似于傳感器數(shù)據(jù)一樣作為一個外部數(shù)據(jù)源.

1.4 基于回環(huán)檢測的全局優(yōu)化

由于掃描僅與時間相近的子圖匹配,其包含掃描數(shù)量少,局部掃描匹配方法會逐漸累積誤差.在短時間的連續(xù)掃描中,累積的誤差很小,在局部便可進(jìn)行優(yōu)化;而長期建圖累積的誤差,則需要通過檢測回環(huán)通過稀疏位姿調(diào)整[15]完成全局優(yōu)化來消除.

在算法中,被插入掃描的相對位姿存儲在內(nèi)存中,用于在回環(huán)閉合時進(jìn)行優(yōu)化.掃描匹配器一直在后端運行,如果找到一個高于一定閾值的匹配,就將相應(yīng)的相對位姿添加到優(yōu)化約束中.與掃描匹配一樣,全局優(yōu)化也被轉(zhuǎn)化為一個非線性最小二乘問題,這樣,在有其他數(shù)據(jù)時能夠較容易地添加殘差,殘差即約束與真實值之差.約束被添加后,每隔一段時間,通過優(yōu)化計算全局位姿最優(yōu)解.

在回環(huán)檢測中,最優(yōu)的匹配是精確到像素的匹配.原始算法以一定的角度步長遍歷搜索窗口內(nèi)的每個像素,但在目前的搜索量級下,運行速度非常慢.

Cartographer 中采用基于DFS(depth first search)的分支定界法進(jìn)行回環(huán)的檢測,這種方法首次在混合整數(shù)線性規(guī)劃中提出,在Land 和Clausen 的論文中[16,17]有相關(guān)概述.該方法的主要思想是:將可能性子集表示為一棵樹中的節(jié)點,其中:根節(jié)點表示所有可能的解,每個節(jié)點的子節(jié)點是其父節(jié)點的分區(qū),一起表示同一組的可能性;葉節(jié)點表示單獨個體,每個表示一個可行解.只要內(nèi)部節(jié)點c的得分是其所有子節(jié)點的得分上限,這樣的算法得到的解與原始算法相同.在這種情況下,每當(dāng)節(jié)點有界時,在該子樹中不存在比目前已知解更好的解.算法獲得目標(biāo)解的效率取決于搜索樹被修剪的部分是否占大多數(shù),而這又依賴于一個好的上界以及一個好的當(dāng)前解,DFS 可以快速評估大量葉節(jié)點獲得較好的當(dāng)前解來降低計算量.同時,因為不希望將較差的匹配作為閉環(huán)的約束,在匹配中引入一個分?jǐn)?shù)閾值,低于這個閾值則不將其作為解.引入分?jǐn)?shù)閾值后,在一定程度上減少了對節(jié)點選擇以及尋找啟發(fā)式解的依賴.DFS 算法中,訪問一個節(jié)點的子節(jié)點順序是先計算每個子節(jié)點的上界,然后訪問具有最大上界也就是最有可能的子節(jié)點.

2 基于掃地機(jī)器人Player 平臺的SLAM 系統(tǒng)實現(xiàn)

本節(jié)中,在掃地機(jī)器人Player 平臺[18,19]上實現(xiàn)了基于增強(qiáng)Cartographer 算法的SLAM 系統(tǒng).整個系統(tǒng)分為系統(tǒng)配置、Player 傳感器數(shù)據(jù)交互、接口通信、數(shù)據(jù)管理及分發(fā)、數(shù)據(jù)處理以及增強(qiáng)的Cartographer 算法這6個模塊.

2.1 基于增強(qiáng)Cartographer算法的SLAM系統(tǒng)設(shè)計

2.1.1 基于Player 平臺的SLAM 整體設(shè)計

Cartographer 中原有的接口是基于ROS[20]的,ROS 可以提供硬件抽象、設(shè)備驅(qū)動程序、庫、可視化器、消息傳遞以及包管理等功能.但因為ROS 集成各種功能,它顯得過于龐大而冗余,無法部署在成本較低的嵌入式設(shè)備上.而本文中所使用的Player 平臺是一個輕量級的中間件,具有小而快的優(yōu)點,同時最大化實用性和適用性,能部署在低配置的嵌入式設(shè)備上.

本文中實現(xiàn)的SLAM 系統(tǒng)以驅(qū)動的方式合入到整個Player 平臺中,易于修改替換,通過訂閱相應(yīng)傳感器接收其數(shù)據(jù)進(jìn)行處理.整個SLAM 系統(tǒng)分為6 個模塊,如圖3 所示:系統(tǒng)配置模塊中完成初始化,Player 傳感器數(shù)據(jù)交互模塊接收平臺的傳感器數(shù)據(jù),接口通信模塊對外提供接口與其他系統(tǒng)交互,數(shù)據(jù)管理及分發(fā)模塊負(fù)責(zé)與算法進(jìn)行數(shù)據(jù)交互,數(shù)據(jù)處理算法模塊完成數(shù)據(jù)處理矯正.在數(shù)據(jù)流向中,Player 傳感器數(shù)據(jù)交互模塊接收來自各傳感器的數(shù)據(jù)傳遞給數(shù)據(jù)管理及分發(fā)模塊,數(shù)據(jù)管理及分發(fā)模塊調(diào)用數(shù)據(jù)處理算法模塊對各種傳感器數(shù)據(jù)處理后傳遞給算法,Player 傳感器數(shù)據(jù)交互模塊通過數(shù)據(jù)管理及分發(fā)模塊獲取算法由激光雷達(dá)數(shù)據(jù)計算出的局部優(yōu)化位姿,再經(jīng)過數(shù)據(jù)管理及分發(fā)模塊傳到算法中進(jìn)行位姿融合.

2.1.2 基于多傳感器的系統(tǒng)配置

系統(tǒng)配置模塊中,整個系統(tǒng)首先需要向Player 平臺注冊,以獨特的名字標(biāo)識,同時說明本系統(tǒng)需要的傳感器;然后對傳感器進(jìn)行訂閱,加載配置文件,完成算法初始化;最后注冊傳感器數(shù)據(jù)處理的回調(diào)函數(shù).

系統(tǒng)使用Lua[21]進(jìn)行配置文件的加載,其小巧而速度快的特點適合應(yīng)用于嵌入式系統(tǒng)的配置文件.在系統(tǒng)停止建圖處理完所有傳感器數(shù)據(jù)后,結(jié)束當(dāng)前軌跡,完成對當(dāng)前優(yōu)化圖中子圖位姿、掃描位姿以及掃描在子圖坐標(biāo)系中匹配位姿的最終優(yōu)化.最后,將當(dāng)前優(yōu)化好的地圖信息通過Protobuf[22]進(jìn)行序列化保存到文件中,可在PC 端對保存的地圖信息進(jìn)行可視化.

2.2 基于數(shù)據(jù)交互的通信模型

2.2.1 Player 傳感器數(shù)據(jù)交互

Player 傳感器數(shù)據(jù)交互模塊主要完成傳感器數(shù)據(jù)及局部優(yōu)化位姿的發(fā)送.

在Player 傳感器數(shù)據(jù)交互模塊中,首先根據(jù)系統(tǒng)配置模塊加載的參數(shù)調(diào)用數(shù)據(jù)管理及分發(fā)模塊中的軌跡構(gòu)建函數(shù),并完成位姿融合函數(shù)的初始化.在建圖過程中,會不斷接收到來自Player 平臺的各種傳感器數(shù)據(jù),Player 傳感器數(shù)據(jù)交互模塊檢查其采樣頻率是否高于配置中的設(shè)置:高于則丟棄;否則將其發(fā)送到數(shù)據(jù)管理及分發(fā)模塊.同時,按照激光數(shù)據(jù)的采樣頻率從數(shù)據(jù)管理及分發(fā)模塊中獲取局部優(yōu)化后的位姿,將其再轉(zhuǎn)發(fā)到數(shù)據(jù)管理及分發(fā)模塊,作為位姿融合的一個數(shù)據(jù)源.Player 傳感器數(shù)據(jù)交互模塊作為一個中間件模塊,轉(zhuǎn)發(fā)來自接口通信模塊或數(shù)據(jù)管理及分發(fā)模塊的數(shù)據(jù).本模塊也是在系統(tǒng)啟動后唯一與Player 平臺交互的模塊,作為整個SLAM 系統(tǒng)的數(shù)據(jù)發(fā)布源.

在建圖終止后,通過數(shù)據(jù)管理及分發(fā)模塊結(jié)束軌跡完成最終全局優(yōu)化以及序列化地圖信息保存到文件中.

2.2.2 數(shù)據(jù)管理及分發(fā)

數(shù)據(jù)管理及分發(fā)模塊與Player 傳感器數(shù)據(jù)交互模塊相同,也是一個中間件模塊.模塊首先調(diào)用算法中的軌跡添加函數(shù)進(jìn)行配置,并初始化數(shù)據(jù)處理算法模塊.在建圖過程中,循環(huán)調(diào)用數(shù)據(jù)處理算法模塊對來自Player 傳感器數(shù)據(jù)交互模塊的傳感器數(shù)據(jù)進(jìn)行處理,將處理后的數(shù)據(jù)傳遞給算法.同時,將從Player 傳感器數(shù)據(jù)交互模塊發(fā)送的局部優(yōu)化位姿傳遞給算法,算法將數(shù)據(jù)融合后得到下一時刻估計的位姿.

2.2.3 接口通信

SLAM 系統(tǒng)提供的接口分為輸入和輸出兩部分.

· 輸入部分中包含:設(shè)置暫停地圖,地圖將暫停更新,停止對傳感器數(shù)據(jù)的處理,重設(shè)置后繼續(xù);重定位功能,在搬動機(jī)器人后,從當(dāng)前已有地圖上重定位出其位姿,用當(dāng)前一幀或幾幀激光雷達(dá)掃描數(shù)據(jù)在地圖中找到最佳匹配,類似于Cartographer 中對閉環(huán)的檢測;加載地圖,導(dǎo)入之前已經(jīng)建好的地圖,對當(dāng)前地圖信息進(jìn)行覆蓋;設(shè)置機(jī)器人位姿,將當(dāng)前機(jī)器人位姿設(shè)置為參數(shù)值,比如在重定位后,將得到的位姿設(shè)置為正確值.

· 在輸出部分中包含以下接口:獲取當(dāng)前完整地圖、已使用地圖以及增量更新地圖;獲取機(jī)器人位姿;保存地圖,通過Protobuf 將當(dāng)前地圖序列化保存為文件.

在初始時,地圖空間按照預(yù)定義尺寸建圖(如1000×1000,精度為5cm),地圖中心為坐標(biāo)原點.獲取完整地圖是獲得整個預(yù)定義好的地圖,其中,網(wǎng)格表示等同于柵格地圖,未知表示為-1,非障礙表示為0,障礙表示為1.已使用地圖表示整體地圖的已使用區(qū)域,返回整體地圖中已知區(qū)域的最大矩形.增量更新地圖則是每幀激光雷達(dá)掃描數(shù)據(jù)對地圖的更新量.

2.3 數(shù)據(jù)矯正及適配算法的研究

2.3.1 激光雷達(dá)數(shù)據(jù)矯正

機(jī)器人建圖主要依靠激光雷達(dá)傳感器采集的點云數(shù)據(jù),通過基于激光雷達(dá)的SLAM 算法實現(xiàn)建圖與定位,而建圖與定位的準(zhǔn)確度非常依賴于激光雷達(dá)提供掃描數(shù)據(jù)的準(zhǔn)確度.

在機(jī)器人運動中,有兩種會對傳感器掃描數(shù)據(jù)準(zhǔn)確度產(chǎn)生明顯影響的特殊情況:一種是傳感器發(fā)射激光束的可旋轉(zhuǎn)部分與機(jī)器人具有相反的旋轉(zhuǎn)方向,且旋轉(zhuǎn)的角速度相等時,激光雷達(dá)傳感器自身并不能探測到其采集的掃描數(shù)據(jù)是對同一個位置的多次掃描;另一種情況是機(jī)器人的前進(jìn)方向垂直于一面墻,激光雷達(dá)在前進(jìn)中不斷進(jìn)行掃描,在掃描過程中,機(jī)器人從遠(yuǎn)處逐漸靠近墻體,此時激光雷達(dá)采集到與機(jī)器人直面的墻面掃描數(shù)據(jù)將不再與機(jī)器人的前進(jìn)方向所垂直,其他方向的墻面數(shù)據(jù)也與實際情況不同,只是誤差相比垂直情況較小.

只有激光雷達(dá)的旋轉(zhuǎn)部分進(jìn)行旋轉(zhuǎn),而機(jī)器人的其他部分沒有產(chǎn)生相對旋轉(zhuǎn)部分的運動時,傳感器得到的掃描數(shù)據(jù)才最接近于真實環(huán)境的情況,這時的誤差只有傳感器自身所帶來的測量誤差.而在其他情況下,傳感器采集的掃描數(shù)據(jù)不僅僅包含激光雷達(dá)旋轉(zhuǎn)部分的運動,還包括機(jī)器人除旋轉(zhuǎn)部分外其他部分所產(chǎn)生的運動[23].因而需要考慮激光雷達(dá)在這些運動情況下采集的掃描數(shù)據(jù),根據(jù)掃描周期進(jìn)行矯正,使掃描數(shù)據(jù)更符合所掃描的真實場景情況,減小誤差,是進(jìn)行高質(zhì)量建圖的基礎(chǔ).

2.3.2 IMU 和里程計數(shù)據(jù)轉(zhuǎn)換及處理算法

在Player 中,由平臺傳輸過來的IMU 數(shù)據(jù)和里程計數(shù)據(jù)同時得到,耦合在同一個數(shù)據(jù)結(jié)構(gòu)里,原始數(shù)據(jù)無法直接提供給算法使用,需要進(jìn)行轉(zhuǎn)換處理后傳遞給Cartographer.

其中,IMU 由陀螺儀和加速度計兩部分構(gòu)成:陀螺儀提供各軸的角速度,加速度計提供三軸的加速度大小.其中,重力加速度是地表垂直方向的加速度.Cartographer 中要求陀螺儀提供的是角速度,但在Player 中提供的是角度,這就需要對數(shù)據(jù)進(jìn)行轉(zhuǎn)化.IMU 的采集頻率(30Hz)較高,角速度可以通過角度差除以時間差得到,時間差通過記錄每次數(shù)據(jù)中附帶的時間戳獲取.這樣,第一次的IMU 數(shù)據(jù)并不發(fā)送給算法,只是進(jìn)行一個記錄.

而加速度計部分,需要將其由四元數(shù)轉(zhuǎn)化為歐拉角給算法使用.四元數(shù)和歐拉角都可以表示機(jī)器人的位姿:四元數(shù)可以避免萬向節(jié)鎖現(xiàn)象,提供平滑插值,但是表示不直觀;歐拉角表示很直觀,只用到3 個數(shù),但是會造成萬向節(jié)鎖現(xiàn)象[24].實際使用中,直接利用Eigen 庫構(gòu)造四元數(shù)和歐拉角,調(diào)用庫函數(shù)完成轉(zhuǎn)換.

里程計中的數(shù)據(jù)——機(jī)器人前進(jìn)速度以及角速度，是通過對左右輪的速度計算得到,通過機(jī)器人在兩個相鄰時刻的位置變換,根據(jù)幾何關(guān)系得到偏航角等于機(jī)器人繞一段圓弧運動角度得到角速度.

3 實驗測試和分析評估

本節(jié)首先在PC 端對增強(qiáng)的Cartographer 建圖質(zhì)量進(jìn)行測試,基于數(shù)據(jù)集對進(jìn)行定量誤差分析;然后基于掃地機(jī)器人平臺,在環(huán)境復(fù)雜度以及尺度不同的實際場景使用不同組合的傳感器,對SLAM 系統(tǒng)進(jìn)行評估測試.

3.1 基于標(biāo)準(zhǔn)數(shù)據(jù)集的測試與分析

PC 端測試平臺使用的系統(tǒng)為Ubuntu 16.04.4,其內(nèi)核為4.4.0-139-generic x86_64.

在測試中,基于ROS kinetic 平臺進(jìn)行數(shù)據(jù)發(fā)送.

圖4 表示兩個由2D 激光雷達(dá)背包和Revo LDS 激光雷達(dá)[25]數(shù)據(jù)建圖得到的柵格地圖,其中,德意志博物館數(shù)據(jù)集包含來自IMU 的數(shù)據(jù)、水平和豎直激光雷達(dá)的數(shù)據(jù),整個數(shù)據(jù)集時間長1913s,數(shù)據(jù)集覆蓋范圍約為128m*270m,地圖軌跡比較復(fù)雜.Revo LDS 數(shù)據(jù)集則只包含低成本激光雷達(dá)采集的點云數(shù)據(jù),數(shù)據(jù)集時間長度為990s,覆蓋范圍約為33m*32m.兩個數(shù)據(jù)集包含大量的閉環(huán),能有效檢測算法的回環(huán)性能.

驗證算法的建圖質(zhì)量評估公式如下:

文獻(xiàn)[26]中提出的方法能夠評估SLAM 算法的準(zhǔn)確度,這種方法基于機(jī)器人位姿間的相對位移,其中,N是相對關(guān)系的數(shù)量,trans(?)和rot(?)分別用于分離和加權(quán)平移和旋轉(zhuǎn)分量,δ和δ*分別表示估計值和真實值(ground truth),δi,j=xiΘxj表示從節(jié)點xi到節(jié)點xj的相對變換關(guān)系,Θ表示標(biāo)準(zhǔn)運動合成的逆運算符.

表1 是增強(qiáng)Cartographer 算法在兩個數(shù)據(jù)集測試后,利用上述公式將結(jié)果與ground truth 比較分析后所得.

Table 1 Dataset test quantitative analysis表1 數(shù)據(jù)集測試定量分析

表1 中包含的誤差分為絕對平移和絕對旋轉(zhuǎn)以及均方平移和均方旋轉(zhuǎn)等4 種,能夠較全面地評估算法.基于兩個數(shù)據(jù)集,在原算法以及GMapping 也完成相同測試.從表中的對比數(shù)據(jù)可以看出:改進(jìn)后的算法在具有多傳感器數(shù)據(jù)效果有一定的提升,說明改進(jìn)后的位姿融合對多傳感器數(shù)據(jù)建圖的質(zhì)量具有較明顯效果,其中,絕對平移誤差降低了53%,絕對旋轉(zhuǎn)誤差降低了50%;而在只有一種傳感器數(shù)據(jù)時,結(jié)果沒有得到有效改善.

3.2 基于掃地機(jī)器人的實際場景測試分析

掃地機(jī)器人為米家掃地機(jī)器人二代,機(jī)器人平臺使用的系統(tǒng)為Ubuntu 14.04.3,其內(nèi)核為3.4.39 armv7l,基于Player 平臺進(jìn)行傳感器數(shù)據(jù)采集和發(fā)送.配備的是自研LDS 激光雷達(dá),最大和最小探測距離分別為6m 和15cm,發(fā)布頻率為5Hz,配有SLAM 中使用的IMU 和里程計傳感器.

基于掃地機(jī)器人平臺,在兩種完全不同的場景對算法進(jìn)行測試,檢測其效果.兩個場景如圖5 所示.

其中,實驗室場景范圍約為15m×8m,這個場景信息豐富,能產(chǎn)生較多的回環(huán),局部地圖不會產(chǎn)生重復(fù),場景大小與復(fù)雜度比較符合家用掃地機(jī)器人實際應(yīng)用的場景.長走廊場景范圍約為12m×63m,這個場景較大而信息簡單,在走廊部分局部信息重復(fù),用于檢測評估算法在特殊場景的建圖質(zhì)量.

掃地機(jī)器人在真實場景中的測試,因為現(xiàn)有條件無法測得ground truth,因而使用掃描匹配中約束構(gòu)建的分?jǐn)?shù)進(jìn)行評估,評估指標(biāo)分為最大值、最小值以及平均值這3 項.

3.2.1 實驗室場景測試與分析

圖6 是掃地機(jī)器人在實驗室場景掃描建圖,保存地圖信息后,在PC 端通過rviz 軟件可視化的2D 柵格地圖.

圖6(a)是只含有激光雷達(dá)數(shù)據(jù)的地圖,圖6(b)是含有激光雷達(dá)與IMU 數(shù)據(jù)的地圖,圖6(c)是含有激光雷達(dá)與里程計數(shù)據(jù)的地圖,圖6(d)是含有所有傳感器數(shù)據(jù)的地圖.從柵格地圖顯示的結(jié)果可以看出:圖6(c)(含有激光雷達(dá)和里程計數(shù)據(jù))的結(jié)果中建圖質(zhì)量最差,其余3 圖結(jié)果中無較大差異.

表2 是在實驗室場景對各種傳感器組合的建圖測試,因為對真實場景掃描不可能像數(shù)據(jù)集一樣做到每次一致,因而匹配分?jǐn)?shù)是在進(jìn)行5 次測試后取Mean 的中位值為代表.

Table 2 Matching scores of sensor combinations in the laboratory scene表2 實驗室場景各傳感器組合的匹配分?jǐn)?shù)

從表中可以得到:在含有3 種傳感器數(shù)據(jù)時建圖質(zhì)量是最高的,擁有最高的平均匹配分?jǐn)?shù),IMU 和里程計數(shù)據(jù)對激光雷達(dá)數(shù)據(jù)起到了較好的輔助作用.只有激光數(shù)據(jù)時的建圖質(zhì)量高于有兩種傳感器的建圖質(zhì)量,其中,只包含里程計數(shù)據(jù)作為輔助數(shù)據(jù)的建圖質(zhì)量是最低的.說明里程計數(shù)據(jù)對建圖的干擾相比IMU 數(shù)據(jù)稍大,在除激光雷達(dá)外只有一種傳感器的情況下,這種傳感器數(shù)據(jù)因為誤差較大,沒有其余傳感器進(jìn)行輔助矯正,對建圖起到負(fù)面影響.因而在實際應(yīng)用中,需要使用所有傳感器數(shù)據(jù)才能得到最佳的建圖質(zhì)量.

3.2.2 長走廊場景測試與分析

圖7 是在長走廊場景基于不同傳感器建圖得到的柵格地圖.

圖7(a)是只含有激光雷達(dá)傳感器數(shù)據(jù)的地圖,可以看到,圖中出現(xiàn)了比較明顯的兩處錯誤(兩個標(biāo)出的紅框1 和紅框2):紅框1 出現(xiàn)局部地圖匹配錯誤,紅框2 中尺度與實際場景差異較大.圖7(b)是含有激光雷達(dá)和IMU數(shù)據(jù)的地圖,紅框部分的地圖與實際環(huán)境的尺度差異仍然存在,IMU 數(shù)據(jù)并不能處理尺度問題.圖7(c)是含有激光雷達(dá)和里程計數(shù)據(jù)的地圖,因為從里程計數(shù)據(jù)得到較為準(zhǔn)確的線速度,紅框部分的尺度問題得到解決,但還是存在匹配錯誤問題.圖7(d)是包含所有傳感器數(shù)據(jù)的地圖,局部地圖的匹配以及走廊部分的尺度不一致有明顯改善.

在長走廊場景的圖7(a)～圖7(c)中出現(xiàn)明顯建圖錯誤,是因為此場景的局部環(huán)境基本重復(fù)無變化,信息較為簡單.在此場景中僅依靠激光雷達(dá)傳感器數(shù)據(jù),無法探測到場景實際已發(fā)生改變.加入IMU 數(shù)據(jù)后也無法幫助正確建圖,僅能幫助改善局部的地圖匹配;而在里程計數(shù)據(jù)的輔助下雖然能正確感知環(huán)境的改變,但無法解決局部匹配錯誤的問題.只有在3 種傳感器的共同作用下,才能得到一個理想的建圖結(jié)果.

在這個場景的測試說明:掃地機(jī)器人在一些特殊場景的建圖,對IMU 和里程計等傳感器數(shù)據(jù)有較大的依賴.IMU 數(shù)據(jù)能提供角度增量,得到較為準(zhǔn)確的位姿估計;里程計數(shù)據(jù)能提供較為準(zhǔn)確的線速度進(jìn)行位置估計,在激光雷達(dá)數(shù)據(jù)的基礎(chǔ)上得到較好的地圖.

3.2.3 機(jī)器人碰撞的數(shù)據(jù)處理

在掃地機(jī)器人實際應(yīng)用的場景中,碰撞不可避免,有時為了清掃障礙物邊緣的灰塵,往往還會主動碰撞障礙物.在測試中發(fā)現(xiàn):產(chǎn)生碰撞會對里程計數(shù)據(jù)產(chǎn)生較大的干擾,最后生成的柵格地圖具有明顯的誤差.而在沒有里程計數(shù)據(jù)的情況下,只有激光雷達(dá)數(shù)據(jù)或者激光和IMU 數(shù)據(jù)都不會因為發(fā)生碰撞而干擾建圖.而在前文的測試中得到,里程計對于探測場景變換以及提升建圖質(zhì)量都是不可缺少的,這時需要在發(fā)生碰撞時對里程計數(shù)據(jù)進(jìn)行處理.

在掃地機(jī)器人上,前置有3 個感知碰撞的傳感器,在SLAM 模塊中,加入對這部分傳感器的訂閱.當(dāng)發(fā)生碰撞,也就是接收到感知碰撞傳感器的數(shù)據(jù)時,SLAM 模塊停止發(fā)送里程計數(shù)據(jù),在碰撞結(jié)束后(一定時間內(nèi)不再接收到感知碰撞傳感器的數(shù)據(jù)),再繼續(xù)發(fā)送.算法中需要清空緩存隊列中的里程計數(shù)據(jù),防止之后利用這部分?jǐn)?shù)據(jù)進(jìn)行角速度與線速度的計算;同時,在位姿融合部分保存之前通過里程計數(shù)據(jù)計算的線速度與角速度,需要加入時間戳,在發(fā)生碰撞期間不再使用這些值,這樣能避免里程計數(shù)據(jù)不準(zhǔn)確造成的錯誤影響.

圖8 是在實驗室場景產(chǎn)生碰撞后的柵格地圖(紅圈處為發(fā)生碰撞處,圖8(a)中發(fā)生1 次,圖8(b)中為4 次).

圖8(a)中,在紅圈處僅發(fā)生一次碰撞,此測試沒有加入里程計數(shù)據(jù)的處理;圖8(b)中,在紅圈處發(fā)生了4 次碰撞,這個測試中加入了對里程計數(shù)據(jù)的處理.可以看出:在加入里程計數(shù)據(jù)處理后,即使碰撞多次,建圖質(zhì)量仍未下降;而在沒有里程計數(shù)據(jù)處理的情況下,僅發(fā)生一次碰撞,也會使地圖產(chǎn)生較大的誤差.

表3 是在上述兩種情況下的匹配分?jǐn)?shù)對比圖.從表中可以看出:相較于有里程計數(shù)據(jù)處理的測試,未加入處理的測試中,建圖匹配分?jǐn)?shù)有明顯的降低.另外,當(dāng)碰撞發(fā)生時,未加入對里程計數(shù)據(jù)處理的測試中,建圖匹配分?jǐn)?shù)有明顯的下降;對比有里程計數(shù)據(jù)處理的測試,建圖匹配分?jǐn)?shù)并未下降.說明在碰撞中對里程計數(shù)據(jù)的處理是不可缺少的,否則會極大影響建圖質(zhì)量.

Table 3 Match scores with or without odometer data processing collision表3 有無里程計數(shù)據(jù)處理發(fā)生碰撞的匹配分?jǐn)?shù)

4 總結(jié)

本文主要對當(dāng)前的激光SLAM 算法進(jìn)行了分析和研究,在Cartographer 的基礎(chǔ)上設(shè)計了基于位姿增量的多傳感器位姿融合算法,在Player 平臺設(shè)計實現(xiàn)了基于增強(qiáng)Cartographer 算法的SLAM 系統(tǒng),并在實際場景生成2D 柵格地圖.

5 展 望

提高掃地機(jī)器人智能化,是未來服務(wù)機(jī)器人應(yīng)用的一個重要方向,之后的研究還可以從以下幾個方面進(jìn)行.(1) 目前使用的傳感器無法探測語義信息[27],從而限制機(jī)器人的智能化.將來可以研究加入視覺傳感器,在提供更精確位姿的同時,能夠與深度學(xué)習(xí)結(jié)合,在不同場景使用不同的清掃方式,增強(qiáng)機(jī)器人的智能化程度;(2) 目前,在掃地機(jī)器人上實現(xiàn)的是二維環(huán)境下的SLAM 算法,在今后的實際發(fā)展中,三維環(huán)境的SLAM 算法具有更廣泛的應(yīng)用場景;(3)究在復(fù)雜場景對動態(tài)物體的檢測以及進(jìn)行軌跡的預(yù)測,實現(xiàn)規(guī)避,讓機(jī)器人更安全、更方便.