基于ROS和視覺SLAM的室內(nèi)導航機器人研究

方育鑫 林盛鑫 史禮帆 任斌

(東莞理工學院 國際微電子學院,廣東東莞 523808)

隨著科技的不斷發(fā)展和人工智能的迅速進步,智能化機器人技術應用在生活中的逐漸普及,自主導航與環(huán)境感知成為機器人領域的重要研究方向。自主導航能夠使機器人具備在未知環(huán)境中自主移動和完成任務的能力,廣泛應用于無人車、無人機、服務機器人等領域。移動機器人的實時定位和地圖構建是實現(xiàn)自主導航的關鍵。通過感知和理解環(huán)境信息,機器人可以進行路徑規(guī)劃、障礙物避障等操作。因此,研究自主導航與環(huán)境感知對于推動機器人技術發(fā)展具有重要的科學意義和應用價值。

國內(nèi)外在自主導航與環(huán)境感知方面的研究取得了一系列重要成果。在自主導航方面,國外的ROS(Robot Operating System)系統(tǒng)成為廣泛使用的開源平臺,提供了強大的功能和工具支持。國內(nèi)的相關研究也取得了許多進展,如無人車、智能家居等領域的應用逐漸增多。在環(huán)境感知方面,深度相機、激光雷達等傳感器設備的發(fā)展和進步使得機器人能夠獲取更加準確和豐富的環(huán)境信息。

本研究旨在基于ROS系統(tǒng)和深度相機,實現(xiàn)小車的自主導航與環(huán)境感知功能。具體研究內(nèi)容包括視覺SLAM(Simultaneous Localization and Mapping)技術、路徑規(guī)劃算法和定位算法。通過研究這些技術,設計合理的系統(tǒng)架構,實現(xiàn)小車在室內(nèi)環(huán)境中的自主導航和定位。

1 系統(tǒng)設計

1.1 ROS介紹

ROS是一個開源的機器人操作系統(tǒng),提供了一套功能豐富的工具和庫,用于構建機器人的控制系統(tǒng)。ROS采用模塊化的設計理念,利用ROS基于節(jié)點的通信模型和消息傳遞機制,使得不同模塊之間可以進行高效的數(shù)據(jù)交換和協(xié)同工作。這為將視覺SLAM算法與移動機器人平臺的集成提供良好的基礎。在本研究中,選擇ROS作為軟件開發(fā)平臺,以便快速開發(fā)和集成各個模塊,并與深度相機進行數(shù)據(jù)交互和控制。

1.2 深度相機原理與選擇

視覺傳感器成為機器人系統(tǒng)中被廣泛使用的感知設備,相比于激光雷達,具有獲取豐富、高維度信息的優(yōu)勢。在本研究中,使用RGB-D相機作為主要的感知設備,RGB-D能夠獲取環(huán)境的RGB圖像和圖像深度信息。RGB-D獲取深度信息的主要原理是利用結(jié)構光技術,相較于雙目深度相機,RGB-D對計算機的計算性能要求較小。它具有功耗低、采集視角廣的特點,支持2 m內(nèi)的環(huán)境深度數(shù)據(jù)采集和最高1 280*960精度的RGB圖像采集性能,基本滿足室內(nèi)測試要求。

1.3 硬件平臺搭建

小車由三層金屬板構成基本框架,上層通過固定件安裝深度相機和激光雷達,以便更好地獲取環(huán)境。中層裝有樹莓派4B、STM32、里程計、IMU傳感器等核心器件,其中樹莓派4B安裝ROS系統(tǒng)作為控制核心,將樹莓派安裝于中層有利于和各個傳感器進行連接,確保數(shù)據(jù)交互的穩(wěn)定。小車底盤采用兩輪差速驅(qū)動結(jié)構,該結(jié)構由兩個驅(qū)動輪和一個支撐輪組成,其中兩個驅(qū)動輪可以實現(xiàn)前進、后退和旋轉(zhuǎn)功能,具有尺寸小巧、機動性高等優(yōu)點,適應多樣化的環(huán)境和任務需求。通過單獨控制左右兩個驅(qū)動輪的轉(zhuǎn)速差異,可以實現(xiàn)精確的方向控制。

1.4 系統(tǒng)結(jié)構

移動機器人軟件系統(tǒng)結(jié)構由三大模塊組成,分別是控制模塊、感知模塊和驅(qū)動模塊,系統(tǒng)結(jié)構圖如圖1所示?；赗OS的通訊機制——一種節(jié)點的分布式框架,每一個節(jié)點都可以獨立運行,節(jié)點可以分布于主機和各個從機中,從而分散計算壓力。各個模塊之間可以獨立工作,并通過消息傳遞進行數(shù)據(jù)交互。

圖1 系統(tǒng)結(jié)構圖

控制模塊由樹莓派4B和PC構成,樹莓派4B作為主機,PC作為從機,在分布式通訊下進行通信,利用地圖構建算法、路徑規(guī)劃算法和定位算法處理感知模塊數(shù)據(jù),并進行決策和控制小車的運動。感知模塊由深度相機、激光雷達和里程計等傳感器組成,負責獲取環(huán)境信息,每一個傳感器作為一個節(jié)點,節(jié)點和主機之間可以進行數(shù)據(jù)交互,為后續(xù)機器人實現(xiàn)自主導航和定位提供數(shù)據(jù)。執(zhí)行模塊驅(qū)動底盤,負責將控制模塊的命令執(zhí)行。

通過良好的軟件系統(tǒng)架構設計,實現(xiàn)了各個模塊的協(xié)同工作和功能整合。

2 RTAB-MAP算法

RTAB-Map(Real-Time Appearance-Based Mapping)算法是一種實時的基于外觀的SLAM算法,是視覺SLAM算法的一種,主要使用RGB-D相機數(shù)據(jù)來實時定位和構建三維稠密地圖。視覺SLAM基礎框架包括:視覺里程計、位姿優(yōu)化、閉環(huán)檢測、制圖,RTAB-MAP算法針對性地解決了視覺SLAM中閉環(huán)檢測出現(xiàn)的問題。RTAB-MAP算法在2019年進行擴展升級的基于內(nèi)存管理的閉環(huán)檢測技術[1],該技術針對SLAM的實時性將定位點數(shù)據(jù)分為3個部分:STM(Short-Term Memory)、LTM(Long-Term Memory)、WM(Work Memory),在構建實時地圖和同步定位有更高的精確性和實時性。

2.1 RTAB-MAP算法閉環(huán)檢測技術原理

SLAM要求有實時性,但視覺傳感器在實際獲取數(shù)據(jù)時是源源不斷的,將地圖全部定位點數(shù)據(jù)稱為“全部定位點”,將“全部定位點”進行閉環(huán)檢測與匹配定位所需時間過長,不能滿足實時性,所以在閉環(huán)檢測時只使用“全部定位點”中的“部分定位點”。但有時需要對“全部定位點”進行訪問,這時就需要對定位點數(shù)據(jù)進行管理,RTAB-MAP將這些數(shù)據(jù)分為3個部分,即LTM、STM、WM, RTAB-MAP會將那些不太可能形成閉環(huán)的定位點從WM轉(zhuǎn)移到LTM中,這些轉(zhuǎn)移的點將不參與下次閉環(huán)檢測,因此選擇哪些WM中的定位點轉(zhuǎn)移到LTM中非常重要。RTAB-Map的思想是假設頻繁訪問的定位點比其他定位點更容易形成閉環(huán),因此,一個定位點被連續(xù)訪問的次數(shù)可以用來衡量它形成閉環(huán)的權重。當WM轉(zhuǎn)移定位點到LTM時,優(yōu)先選擇具有最低權重的定位點,如果具有最低權重的定位點有多個,則選擇存儲時間最長的那個。對于具有形成閉環(huán)概率最高的那個定位點,將它的那些沒有在WM中的鄰接定位點重新從LTM中取出放回到WM中,通過“取回”和轉(zhuǎn)移這兩個操作靈活地管理不同數(shù)據(jù)庫中存儲的定位點[2]。

RTAB-Map閉環(huán)檢測時不使用STM中的定位點。當STM定位點達到一定數(shù)量時,STM中存儲時間最長的定位點移動到WM中。RTAB-Map使用離散貝葉斯濾波器估計閉環(huán)形成概率,將新的定位點與WM中的定位點進行比較。發(fā)現(xiàn)新舊定位點之間有高概率形成閉環(huán)時,就檢測到一個閉環(huán),并將它們連接在一起。

2.2 RTAB-MAP算法閉環(huán)檢測技術基本流程

1)創(chuàng)建定位點:使用詞袋法創(chuàng)建圖像的簽名,利用詞袋法提取從相機取得的圖像特征的視覺詞匯向量[3]。詞袋法具體步驟:首先提取圖像特征,然后通過聚類算法(如K均值聚類)對大量圖像特征描述子進行聚類,構建視覺詞典,最后通過詞典中的單詞的關鍵點數(shù)量形成圖像簽名,以在線增量式的方法創(chuàng)建定位點。

2)權重更新:通過相似度s來更新定位點權重,相似度由公式(1)表示

(1)

Npair表示匹配定位點簽名間的單詞對個數(shù),Nzt和Nzc分別表示簽名Zt和Zc的總單詞數(shù)目。

3)貝葉斯過濾器更新:估計當前定位點Lt和存儲在WM中的定位點形成閉環(huán)的概率來記錄閉環(huán)假設。

4)閉環(huán)假設選擇:當一個閉環(huán)假設成立,新的定位點Lt則和舊的定位點建立起了閉環(huán),并對定位點Lt的權重進行更新。

5)取回:對于具有形成閉環(huán)概率最高的那個定位點,將它的那些沒有在WM中的鄰接定位點,從LTM中取出放回到WM中。

6)轉(zhuǎn)移:當圖像數(shù)據(jù)處理時間超出一定閾值時,將WM中權重最低且存儲時間最長的定位點轉(zhuǎn)移至LTM。

2.3 地圖構建

經(jīng)過閉環(huán)檢測后,RTAB-Map算法將構成閉環(huán)的定位點采用GTSAM(GeorgiaTech Smoothing and Mapping)地圖優(yōu)化策略進行處理[4]。這一步驟的目的是將局部地圖拼接成全局地圖,提升整體建圖的準確性和一致性。在閉環(huán)重構過程中,每次產(chǎn)生回環(huán)時都會對全局地圖進行重構,也就是將閉環(huán)中的幀和新的特征點集成到全局地圖中,以便不斷完善和更新整個地圖。這樣的重構過程將持續(xù)進行直到不再檢測到新的圖像幀時停止。

通過這樣的閉環(huán)處理和全局地圖拼接,RTAB-Map算法能夠?qū)崿F(xiàn)更準確、一致且完整的地圖構建,適用于各種機器人應用場景,提供更可靠的導航和定位服務。

3 路徑規(guī)劃與定位

ROS小車的路徑規(guī)劃和定位是基于ROS的move_base和amcl功能包來實現(xiàn)的[5]。

move_base是ROS中一個重要的導航功能包,它通過全局路徑規(guī)劃和局部路徑規(guī)劃的組合,實現(xiàn)了機器人的自主導航和避障功能。

定位是指通過傳感器獲取機器人當前的位置,使機器人能夠知道自己在哪里[6]。在ROS中,常用的定位算法是AMCL(Adaptive Monte Carlo Localization)。

路徑規(guī)劃和定位在ROS中是緊密結(jié)合的,路徑規(guī)劃需要依賴定位信息來獲取機器人的當前位置,而定位則需要路徑規(guī)劃提供的地圖等信息來更準確地估計位置。通過結(jié)合路徑規(guī)劃和定位,ROS小車可以在已知環(huán)境中進行導航和避障,實現(xiàn)自主移動功能。

3.1 路徑規(guī)劃算法

3.1.1 A*算法

A*(A-star)算法是一種有效的尋路算法,它結(jié)合了Dijkstra算法和BFS算法的算法優(yōu)勢[7]。有效地找到起始節(jié)點和目標節(jié)點之間的最短路徑。原理圖如圖2所示,A*算法的基本流程如下:

圖2 A*算法原理

1)初始化:從定義一個起始節(jié)點和一個目標節(jié)點開始,給起始節(jié)點賦一個0的代價值,創(chuàng)建OpenList和CloseList,并將起始點放入OpenList。

2)選定節(jié)點:從OpenList中選擇總代價最小的節(jié)點,如果這個點是終點,那么得到最短路徑;如果不是終點,將選定的節(jié)點從OpenList移動到CloseList,表明它已經(jīng)被訪問過。

3)擴展節(jié)點:生成當前選定節(jié)點的鄰居節(jié)點,并計算它們的當前代價,如果鄰居節(jié)點不在OpenList中或者其當前代價小于當前節(jié)點的當前代價,就將這個鄰居節(jié)點加入OpenList。

4)重復步驟2)、3):繼續(xù)選擇總代價最小的節(jié)點,擴展其鄰居節(jié)點,更新代價,將鄰居節(jié)點添加到OpenList的過程,直到找到目標節(jié)點或OpenList中沒有更多節(jié)點。

5)回溯路徑:一旦到達目標節(jié)點,使用存儲在每個節(jié)點中的父節(jié)點信息,每個節(jié)點都有一個父節(jié)點,表示當前節(jié)點是從哪個節(jié)點擴展而來的。回溯從目標節(jié)點到起始節(jié)點的路徑,這條路徑表示從開始節(jié)點到目標節(jié)點的最短路徑。

A*算法背后的關鍵思想是使用啟發(fā)式函數(shù)來估計從每個節(jié)點到目標的代價。代價計算公式為

f=g+h,

(2)

其中f為總代價、g為當前代價、h為預估代價。

這種啟發(fā)式算法通過優(yōu)先考慮估計代價較低的節(jié)點來引導搜索,使算法首先探索最有希望的路徑。通過同時考慮實際成本和啟發(fā)式成本,A*在尋找最優(yōu)路徑和計算效率之間取得了平衡。

3.1.2 DWA

DWA(Dynamic Window Approach)算法是一種用于移動機器人路徑規(guī)劃和局部避障的經(jīng)典算法。DWA算法是在速度空間中采樣多組數(shù)據(jù),并模擬移動機器人以這些速度在下一時間段內(nèi)的軌跡并對軌跡進行評價,選取最優(yōu)軌跡對應的速度來驅(qū)動機器人運動[8]。DWA算法通過動態(tài)窗口的選擇和評估函數(shù)的計算,實現(xiàn)了移動機器人在復雜環(huán)境中的安全導航和路徑規(guī)劃。

1)機器人模型:假設v(t)和w(t)分別表示機器人在世界坐標系下t時刻的線速度和角速度,Δt表示采樣周期,機器人在Δt內(nèi),近似作直線運動。機器人運動模型如式(3)

(3)

其中x(t)、y(t)、θ(t)表示t時刻機器人在世界坐標系下的位姿。

2)速度采樣:動態(tài)窗口法將避障問題描述為速度空間中帶約束的優(yōu)化問題[9],根據(jù)環(huán)境因素和硬件結(jié)構的速度空間中的多組速度進行約束。

機器人最大速度和最小速度的約束為

(4)

加減速度約束:受電機性能的約束,模擬機器人采樣周期內(nèi)的速度被約束在一定范圍內(nèi)[10]。

(5)

其中vc、wc表示當前速度,vb、wb表示最大減速度,va、wa表示最大加速度。

機器人安全距離約束:機器人在移動過程中需要考慮當前速度以最大減速度減速時是否會與障礙物發(fā)生碰撞,在這一背景下對機器人移動速度存在一定約束。

(6)

其中dist(v,w)是機器人以速度,(v,w)估計時離障礙物的最短距離。

3)評價函數(shù):如果在速度空間中存在多組可行的采樣速度,則需要使用適當?shù)脑u價函數(shù)進行細致地評價以確定最優(yōu)軌跡。DWA算法中采用的評價函數(shù)指標綜合了多種因素,包括偏置角、線速度、仿真軌跡終點與遇到障礙物的最近距離。該算法通過對潛在軌跡的評估函數(shù),識別出最適合機器人運動的路徑,保證了機器人在復雜環(huán)境下的導航和移動的安全性。

評價函數(shù)為

G(v,w)=σ(αhead(v,w)+

γuel(v,w)+βdist(v,w)),

(7)

其中α、γ、β表示加權系數(shù),head(v,w)表示軌跡終點和目標點的方向角偏差,uel(v,w)用于評價當前速度,dist(v,w)表示與當前路線障礙物的距離的最短距離。

3.2 全局定位

在地圖中實現(xiàn)機器人的全局定位可以采用AMCL功能包。AMCL是一個在ROS中廣泛使用的定位功能包,它基于自適應蒙特卡洛定位算法,能夠?qū)崿F(xiàn)機器人在地圖中的定位。它結(jié)合了機器人傳感器數(shù)據(jù)和已知地圖信息,通過生成一組粒子來表示機器人在地圖上的可能位置,并根據(jù)傳感器數(shù)據(jù)的更新對這些粒子進行重采樣,以提高定位的精確性和穩(wěn)定性[11]。

使用AMCL功能包可以方便地在ROS中實現(xiàn)機器人的定位功能。它提供了參數(shù)配置、消息發(fā)布和訂閱等接口,可以與地圖構建、路徑規(guī)劃等其他功能包進行集成,實現(xiàn)完整的機器人導航系統(tǒng)。

4 實驗結(jié)果與分析

4.1 室內(nèi)建圖實驗

在一個室內(nèi)場景中分別測試移動機器人用激光雷達和用深度相機進行建圖。首先確保小車和PC連接到同一個TP-link wifi下,在PC端通過小車IP配置分布式通訊;然后啟動雷達SLAM的launch文件,深度相機SLAM則啟動rtabmap的launch文件,再啟動鍵盤控制節(jié)點和rviz可視化界面,控制小車遍歷實驗場景完成建圖,如圖3所示。

圖3 激光雷達建圖

4.2 室內(nèi)導航實驗

分別測試移動機器人用激光雷達和用深度相機進行導航。利用上一個實驗完成的建圖進行導航,分別啟動激光雷達和深度相機導航的launch文件,這里需要注意的是相機只有前向視野,所以在進行深度相機導航時只允許小車進行前向運動。在rviz可視化界面為小車指定目標點,小車進行自主導航順利到達目標點,如圖4所示。

圖4 深度相機建圖

4.3 實驗分析

基于ROS系統(tǒng),在室內(nèi)建圖實驗中,激光雷達的建圖(如圖5所示)在精確度上優(yōu)于用深度相機建圖(如圖6所示),深度相機所構建的三維深度圖在地圖輪廓上準確性。深度相機的優(yōu)勢在于它能夠提供豐富的空間地圖維度信息,包括物體的形狀、紋理和顏色等。在室內(nèi)導航實驗中,運用激光雷達和深度相機進行導航移動機器人都能合理規(guī)劃路徑實現(xiàn)自主導航和避障,激光雷達導航得益于其建圖的精確性,更適用于二維平面場景的使用。但是,如果對于空間維度信息的豐富性和感知能力更為重要,深度相機則提供了更多優(yōu)勢和可能性。

圖5 激光雷達導航

圖6 深度相機導航

5 結(jié)語

介紹了ROS作為一個靈活、強大的機器人開發(fā)框架的優(yōu)勢,并解釋了其在導航系統(tǒng)中的重要性;然后,介紹了深度相機的工作原理和應用;接著討論了建圖算法和路徑規(guī)劃算法的原理與實現(xiàn),通過將導航算法與深度相機數(shù)據(jù)結(jié)合起來,能夠?qū)崿F(xiàn)自主導航和動態(tài)避障;最后,進行了實驗驗證。實驗結(jié)果表明,基于ROS和深度相機的導航小車能夠完成導航和避障,具有廣泛的應用潛力。

結(jié)合ROS的強大功能和深度相機的感知能力,隨著技術的不斷進步,這將為機器人導航領域帶來更多可能性和創(chuàng)新;期待未來能夠看到更多基于ROS和深度相機的智能導航解決方案的出現(xiàn)。