王一波 梁偉鄯

柳州工學(xué)院

0 引言

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

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

1.1 基本功能框架

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

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

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

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

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

環(huán)境控制節(jié)點(diǎn)主要負(fù)責(zé)環(huán)境參數(shù)的收集，包括無(wú)人系統(tǒng)大致位置、時(shí)區(qū)、時(shí)間、天氣及室內(nèi)或者室外等。無(wú)人系統(tǒng)通過(guò)連接互聯(lián)網(wǎng)實(shí)時(shí)獲取，主要用于為SLAM系統(tǒng)環(huán)境建模和姿態(tài)估計(jì)提供信息用以輔助決策，以提高視覺(jué)SLAM系統(tǒng)的魯棒性?；驹砣缦拢侯惐热酥饕@取信息的主要渠道是視覺(jué)，但視覺(jué)會(huì)遵循環(huán)境光照強(qiáng)度變化做出相應(yīng)的調(diào)節(jié)，而對(duì)于視覺(jué)SLAM系統(tǒng)也是相同的，視覺(jué)傳感器對(duì)于天候及環(huán)境也將有著不同的表現(xiàn)，比如在雨天、霧天和雪天，會(huì)嚴(yán)重影響成像，甚至?xí)斐梢曈X(jué)傳感器失效，如果不做調(diào)整，會(huì)為無(wú)人系統(tǒng)的應(yīng)用帶來(lái)?yè)p失。綜合各種環(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ù)的運(yùn)行和中止，以此提高系統(tǒng)對(duì)環(huán)境的適應(yīng)性。

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

表2 決策說(shuō)明

1.3 動(dòng)態(tài)檢測(cè)節(jié)點(diǎn)

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

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 視覺(jué)融合SLAM技術(shù)架構(gòu)設(shè)計(jì)

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

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

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

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

2.2 節(jié)點(diǎn)通信設(shè)計(jì)

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

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

3 實(shí)驗(yàn)及分析

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

圖4 實(shí)驗(yàn)環(huán)境

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

圖5 實(shí)驗(yàn)路線

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

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

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

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

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

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

?

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

4 結(jié)束語(yǔ)

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