王一波 梁偉鄯

柳州工學(xué)院

0 引言

同時定位與地圖構(gòu)建（Simultaneous Localization And Mapping，簡稱SLAM），通常是指在機(jī)器人或者其他載體上，通過對各種傳感器數(shù)據(jù)進(jìn)行采集和計算，生成對其自身位置姿態(tài)的定位和場景地圖信息的系統(tǒng)，其對于智能機(jī)器人走向應(yīng)用發(fā)揮著關(guān)鍵作用。當(dāng)前，主流SLAM技術(shù)分為激光和視覺兩大類。激光SLAM算法簡單，但成本相對較高，應(yīng)用具有一定局限性；視覺SLAM利用攝像機(jī)（單目、雙目或者RGBD）可以從環(huán)境中獲取海量的、冗余的紋理信息，擁有超強(qiáng)的場景辨識能力，將成為今后技術(shù)發(fā)展的主要趨勢，但其算法復(fù)雜，魯棒性較差。以視覺為主要信息收集手段，融合IMU、編碼器等多種傳感器，構(gòu)成視覺融合SLAM框架，可以從根本上克服單視覺SLAM對光照敏感的問題，更具魯棒特性。而作為成熟的機(jī)器人操作系統(tǒng)ROS，由于支持眾多硬件的軟件端口的同時掛載，在操作系統(tǒng)層和應(yīng)用層之間架起了一座可靠的技術(shù)橋梁，而日漸得到廣大研究人員的青睞，以其為基礎(chǔ)構(gòu)建SLAM的通用平臺，可低耦合度的分布式進(jìn)程，控制網(wǎng)絡(luò)邏輯清晰，掛載方便，十分便于驗證SLAM框架性能。本文將構(gòu)建一個以維持系統(tǒng)運行穩(wěn)定為主的框架，以提高SLAM技術(shù)的魯棒性，增強(qiáng)其在不同環(huán)境和狀況下的適應(yīng)能力。

1 視覺融合SLAM功能架構(gòu)設(shè)計

1.1 基本功能框架

視覺SLAM利用攝像機(jī)（單目、雙目或者 RGBD）具有重定位、場景分類上無可比擬的巨大優(yōu)勢，但目前受限于硬件技術(shù)性能及軟件算法，尚未取得關(guān)鍵性突破。而慣性傳感器（IMU）具有不受場景和運動速度限制、實時準(zhǔn)確測量無人系統(tǒng)位姿變化的優(yōu)點，缺點是長時間運行會產(chǎn)生累加誤差。本文將以視覺傳感器為主、IMU為輔構(gòu)建視覺慣性融合SLAM，充分利用圖像信息的長時間穩(wěn)定性和IMU數(shù)據(jù)的短時間準(zhǔn)確性，從而得到更加魯棒的狀態(tài)估計結(jié)果。視覺傳感器與IMU的耦合方式可分為剛性連接和非剛性連接，前者即IMU相對視覺傳感器的空間位置固定，后者是兩類傳感器位置無約束關(guān)系?？紤]IMU僅僅作為視覺傳感器的輔助機(jī)構(gòu)，避免增加問題的復(fù)雜性，本文選擇剛性連接，即未出現(xiàn)異常情況下，兩傳感器經(jīng)參數(shù)標(biāo)定后的位姿轉(zhuǎn)換關(guān)系不會發(fā)生改變。

本文的設(shè)計目的除SLAM系統(tǒng)常規(guī)任務(wù)定位和建圖外，還包括魯棒性設(shè)計，主要通過增加相應(yīng)環(huán)節(jié)提升SLAM系統(tǒng)的環(huán)境性，降低誤報概率，從而避免無人系統(tǒng)關(guān)鍵應(yīng)用的損失。為了更好地描述需求細(xì)節(jié)，通過分節(jié)點解構(gòu)視覺SLAM的任務(wù)，同時理清數(shù)據(jù)流向，整體設(shè)計如圖1所示。

圖1 視覺SLAM功能結(jié)構(gòu)圖

圖1 中視覺傳感器采集外部環(huán)境數(shù)據(jù)，IMU采集無人系統(tǒng)本體數(shù)據(jù)（包括加速度、角速度和地磁方向），這與高等動物融合視覺和立體感覺來推測自身姿態(tài)的方法十分相似。校準(zhǔn)初始化節(jié)點主要用于兩類傳感器的同步，確保各自狀態(tài)估計不會錯位；失效檢測主要用于視覺傳感器的即時失效檢測，起到數(shù)據(jù)過濾功能，確保質(zhì)量低下圖像被濾除；濾波節(jié)點主要用于估計系統(tǒng)狀態(tài)；魯棒融合節(jié)點則對應(yīng)CI融合魯棒穩(wěn)態(tài)卡爾曼濾波器，用于融合視覺SLAM估計和里程計估計的結(jié)果，形成對于較大范圍環(huán)境的穩(wěn)定輸出。在此，重點說明環(huán)境控制節(jié)點和動態(tài)檢測節(jié)點。

1.2 環(huán)境控制節(jié)點

環(huán)境控制節(jié)點主要負(fù)責(zé)環(huán)境參數(shù)的收集，包括無人系統(tǒng)大致位置、時區(qū)、時間、天氣及室內(nèi)或者室外等。無人系統(tǒng)通過連接互聯(lián)網(wǎng)實時獲取，主要用于為SLAM系統(tǒng)環(huán)境建模和姿態(tài)估計提供信息用以輔助決策，以提高視覺SLAM系統(tǒng)的魯棒性。基本原理如下：類比人主要獲取信息的主要渠道是視覺，但視覺會遵循環(huán)境光照強(qiáng)度變化做出相應(yīng)的調(diào)節(jié)，而對于視覺SLAM系統(tǒng)也是相同的，視覺傳感器對于天候及環(huán)境也將有著不同的表現(xiàn)，比如在雨天、霧天和雪天，會嚴(yán)重影響成像，甚至?xí)斐梢曈X傳感器失效，如果不做調(diào)整，會為無人系統(tǒng)的應(yīng)用帶來損失。綜合各種環(huán)境參數(shù)，可以換算為環(huán)境的光照亮度范圍，以此作為決策的依據(jù)。具體決策如表1和表2所示。決策依據(jù)光照亮度分為A、B、C三類，分別進(jìn)行數(shù)據(jù)傳輸與任務(wù)決策，用以決定是否進(jìn)行數(shù)據(jù)轉(zhuǎn)發(fā)以及任務(wù)的運行和中止，以此提高系統(tǒng)對環(huán)境的適應(yīng)性。

表1 環(huán)境影響決策

表2 決策說明

1.3 動態(tài)檢測節(jié)點

根據(jù)已有經(jīng)驗，視覺SLAM在靜態(tài)環(huán)境下表現(xiàn)更為魯棒，而當(dāng)場景中出現(xiàn)運動物體，則會造成不同程度的影響，而且運動物體越多，影響將越大。為此，可將場景分為靜態(tài)和動態(tài)兩部分，前者是相對穩(wěn)定不變的背景部分，后者是前景移動的目標(biāo)部分，在視覺SLAM應(yīng)用中被認(rèn)為是干擾因素。為降低動態(tài)部分的影響，本文設(shè)計了專門的檢測節(jié)點，用以剔除場景中的動態(tài)部分。通過訓(xùn)練可實時運行的You only look once（YOLO）網(wǎng)絡(luò)，檢測包括植物、動物、交通工具、路標(biāo)等各種室外環(huán)境中的目標(biāo)，將其從圖像中分離出來。

Scene={（c_i，n_i，P_i ）│i=1，2，…，K} （1）

式中，c_i表示第i類別目標(biāo)，n_i表示第i類別目標(biāo)數(shù)量，P_i表示目標(biāo)的位置（包括x、y坐標(biāo)及目標(biāo)高度、寬度）。

2 視覺融合SLAM技術(shù)架構(gòu)設(shè)計

2.1 基于ROS的硬件框架設(shè)計

從學(xué)科上看，SLAM屬于機(jī)器人學(xué)的范疇，具有一定的復(fù)雜度，要使其穩(wěn)定可靠運行，必須提升技術(shù)架構(gòu)的性能。類比智能手機(jī)目前常用的操作系統(tǒng)Android和iOS的底層分別是開源的GNU/Linux和Darwin，目前移動機(jī)器人的開發(fā)多是建立在ROS（Robot operating system）上，其能為復(fù)雜任務(wù)計算節(jié)約空間和算力，確保各程序在適當(dāng)時間被執(zhí)行，同時可以為多進(jìn)程通信提供良好的底層支持。ROS是由Open SourceRobotics Foundation（OSRF）提供的開源的機(jī)器人學(xué)中間件，用于編寫機(jī)器人軟件部分的靈活框架，其集成了各種工具、庫和規(guī)范，旨在簡化在各種機(jī)器人平臺上控制機(jī)器人實現(xiàn)復(fù)雜、強(qiáng)大行為的工作。

本文驗證平臺的總體方案是“微型計算機(jī)上位機(jī)+單片機(jī)下位機(jī)”。上位機(jī)承載ROS，負(fù)責(zé)任務(wù)視覺SLAM與控制，下位機(jī)作為掛載于ROS中的節(jié)點存在，控制具體的控制對象（如電機(jī)、傳感器等），通用平臺技術(shù)框架如圖2所示。

圖2 通用平臺技術(shù)框架

2.2 節(jié)點通信設(shè)計

相較于ROS1，ROS2對通信機(jī)制進(jìn)行了改進(jìn)，提出了基于Data Distribution Service（DDS）通信，實現(xiàn)了無主節(jié)點的去中心化通信及指定分布式訂閱和發(fā)布規(guī)范，廣泛應(yīng)用于醫(yī)療、航空和船舶等對穩(wěn)定性要求高的領(lǐng)域。本文構(gòu)建了圖3所示的基于DDS（Data Distribution Service）的ROS 2系統(tǒng)的通信技術(shù)架構(gòu)，通過將所有節(jié)點數(shù)據(jù)發(fā)布到所在全局?jǐn)?shù)據(jù)域（Global Data Space），實現(xiàn)了所有節(jié)點也將從全局?jǐn)?shù)據(jù)域訂閱其它節(jié)點數(shù)據(jù)。同時DDS層也為二次開發(fā)提供了多編程語言的統(tǒng)一接口，可以滿足使用C++、Java和Python在不同操作系統(tǒng)實現(xiàn)無阻礙的去中心化數(shù)據(jù)交流。圖3中，操作系統(tǒng)層用橙色標(biāo)識，中間件層包含DDS和ROS 2，分別采用綠色和藍(lán)色標(biāo)識，應(yīng)用層即維穩(wěn)框架（SafeguardFramework），用黃色標(biāo)識。

圖3 DDS通信技術(shù)結(jié)構(gòu)圖

3 實驗及分析

選取封閉環(huán)境作為機(jī)器人掃描實驗的環(huán)境，為增加機(jī)器人掃描的完整性，實驗環(huán)境為某校教室，長12米寬9米，現(xiàn)對封閉環(huán)境進(jìn)行簡單的搭建與設(shè)置，實驗的主要研究對象是6把椅子和1張講臺，同時在實驗環(huán)境中還有一些其他的障礙物。實驗環(huán)境如圖4所示。

圖4 實驗環(huán)境

因?qū)嶋H場景中，難以獲得機(jī)器人的真實軌跡，但場景中物體尺寸可事先測量得到，為此設(shè)計如下實驗：不同光照條件下點云掃描精度實驗、不同運動速度條件下點云掃描精度實驗，比較本文提出的視覺SLAM融合框架與單視覺SLAM。為排除干擾因素，實驗路線如圖5所示，統(tǒng)一為直線行進(jìn)。

圖5 實驗路線

本次實驗分別選擇上午光照均勻、中午光照較亮及傍晚光照較暗三種自然光照條件下進(jìn)行，為避免速度的影響，實驗過程在速度較慢的條件下完成。

由圖6、圖7可以看出，視覺融合SLAM算法相比單視覺SLAM更穩(wěn)定，二者在光照均勻條件下，掃描點云都比較完整，而在光照較暗或者較亮的條件下，點云都有明顯缺失，但單視覺SLAM更為明顯，這是由于光照變化劇烈時，部分幀特征點缺失，造成建圖丟失。根據(jù)點云數(shù)據(jù)進(jìn)一步對場景中的物體尺寸進(jìn)行測量，結(jié)果如表3和表4所示。

圖6 單視覺SLAM不同光照條件下掃描結(jié)果圖

圖7 視覺融合SLAM不同光照條件下掃描結(jié)果圖

表3 單視覺SLAM不同光照測量結(jié)果

表4 融合視覺SLAM不同光照測量結(jié)果

?

由上表可以看出，單視覺SLAM在光照均勻條件下，最大誤差0.14m，光照較暗、光照較亮條件下最大誤差分別為0.26m和0.17m；融合視覺SLAM在光照均勻條件下，最大誤差0.15m，光照較暗、光照較亮條件下最大誤差分別為0.16m和0.15m?？梢姽庹談×易兓瘯r，單視覺SLAM受到較大影響，魯棒性較差，融合視覺SLAM在均勻光照條件下，測量精度沒有明顯提高，但對光照變化的魯棒性發(fā)生了顯著增加，所以視覺融合SLAM框架比單視覺SLAM表現(xiàn)更穩(wěn)定。

4 結(jié)束語

本文提出了一種基于融合視覺的SLAM框架，為提高SLAM魯棒性，從功能設(shè)計階段充分考慮了環(huán)境及動態(tài)過程可能給定位和建圖帶來的影響，專門設(shè)計了環(huán)境監(jiān)測節(jié)點和動態(tài)檢測節(jié)點。在技術(shù)實現(xiàn)階段，充分利用了ROS2通信的優(yōu)越性，采用DDS方式將上位機(jī)掛載的節(jié)點有效融合在了一起。實驗表明，融合多種傳感器的視覺SLAM框架比單視覺SLAM框架，無論在定位精度還是魯棒性上，都有了一定的提升高，同時也發(fā)現(xiàn)深度融合多傳感器數(shù)據(jù)是解決機(jī)器人定位的重要方向，今后將圍繞如何在機(jī)理層面實現(xiàn)融合開展進(jìn)一步的研究。