徐 興，劉 瓊，黃開坤

動態(tài)場景下倉儲機(jī)器人的視覺定位與建圖

徐 興，劉 瓊，黃開坤

（南華大學(xué) 機(jī)械工程學(xué)院，湖南 衡陽 421200）

針對目前大多數(shù)室內(nèi)倉儲機(jī)器人的視覺定位與建圖算法（SLAM）是假設(shè)機(jī)器人處在靜態(tài)的環(huán)境，但是當(dāng)場景中出現(xiàn)移動的物體時，機(jī)器人自身定位的準(zhǔn)確性和穩(wěn)定性易受到巨大影響，而其他室內(nèi)定位技術(shù)比如超寬帶、藍(lán)牙等必須在無線信號覆蓋的條件下工作的問題，提出一種面向室內(nèi)倉儲機(jī)器人在動態(tài)場景下的視覺SLAM算法：在基于旋轉(zhuǎn)不變特征點(diǎn)的定位與建圖改進(jìn)算法（ORB-SLAM2）基礎(chǔ)上，用幾何對應(yīng)神經(jīng)網(wǎng)絡(luò)（GCNv2）來替換基于圖像金字塔的特征點(diǎn)提取算法；添加目標(biāo)檢測(YOLOv4)的語義線程，并且使用光流法來追蹤特征點(diǎn)；然后通過運(yùn)動一致性檢測來識別圖像中潛在的動態(tài)物體；最后剔除動態(tài)特征點(diǎn)后進(jìn)行位姿估計。實(shí)驗(yàn)結(jié)果表明，在高度動態(tài)的室內(nèi)場景下,提出的算法相對于ORB-SLAM2算法的絕對軌跡誤差可減小95.56%～98.21%，能夠有效解決ORB-SLAM2在動態(tài)場景下定位不準(zhǔn)確的問題。

倉儲機(jī)器人；動態(tài)場景；幾何對應(yīng)神經(jīng)網(wǎng)絡(luò)（GCNv2）；目標(biāo)檢測；基于旋轉(zhuǎn)不變特征點(diǎn)的定位與建圖改進(jìn)算法（ORB-SLAM2）

0 引言

目前我國對移動機(jī)器人的研究正處在快速發(fā)展階段，尤其是在物流行業(yè)中，人們需要倉儲機(jī)器人來調(diào)配貨物，這就需要機(jī)器人具備能夠適應(yīng)陌生環(huán)境的能力[1]。因此，對于倉儲機(jī)器人來說，定位與建圖（simultaneous localization and mapping，SLAM）[2]技術(shù)可以幫助其在陌生環(huán)境中完成自身的定位與建圖。SLAM可以分為基于激光雷達(dá)的激光SLAM和基于相機(jī)的視覺SLAM 2種類型。雖然激光SLAM技術(shù)已經(jīng)成熟，但其成本過高，因此越來越多的研究人員尋求可以降低成本的方法。相比之下，基于相機(jī)的視覺SLAM具有價格低廉，易于融合其他傳感器和可獲取豐富的環(huán)境信息等優(yōu)點(diǎn)，成為相關(guān)領(lǐng)域主要研究方向之一[3]。

目前已有許多出色的視覺SLAM算法被研究出來，比如：基于旋轉(zhuǎn)不變特征點(diǎn)（oriented fast and rotated brief，ORB）的定位與建圖算法ORB-SLAM[4]、半直接視覺里程計（semi-direct visual odometry，SVO）[5]、大規(guī)模直接單目 SLAM（large scale direct monocular SLAM，LSD-SLAM）[6]等等。然而這些算法大多是基于靜態(tài)環(huán)境的假設(shè)，如果環(huán)境內(nèi)存在動態(tài)物體，動態(tài)特征點(diǎn)就會影響相機(jī)的位姿估計。因?yàn)檫@些算法會從圖像中提取一些具有代表性的像素作為特征點(diǎn)，當(dāng)相機(jī)運(yùn)動時，根據(jù)這些像素出現(xiàn)在圖像的不同位置來估計相機(jī)的運(yùn)動軌跡，如果圖片中的物體發(fā)生運(yùn)動，圖像間的像素匹配可能會出現(xiàn)錯誤，這就導(dǎo)致算法計算出來的相機(jī)位姿不準(zhǔn)確，甚至定位失敗[7]。近年來，隨著目標(biāo)檢測、語義分割等技術(shù)的發(fā)展，越來越多的研究人員嘗試著將這些技術(shù)應(yīng)用到視覺SLAM中。文獻(xiàn)[8]在基于旋轉(zhuǎn)不變特征點(diǎn)的定位與建圖改進(jìn)算法（ORB-SLAM2）的基礎(chǔ)上提出了動態(tài)場景中的跟蹤、映射和修復(fù)（tracking, mapping and inpainting in dynamic scenes，DynaSLAM）算法，該算法使用多視圖幾何的方法來判斷環(huán)境中的動態(tài)特征點(diǎn)。還有基于ORB-SLAM2算法的面向動態(tài)環(huán)境的語義視覺SLAM（semantic visual SLAM towards dynamic environments，DS-SLAM）算法[9]，通過加入基于語義分割網(wǎng)絡(luò)（segmentation network，SegNet）的語義分割部分來避免環(huán)境中動態(tài)物體對相機(jī)姿態(tài)估計的影響。但是這些算法存在著占用過大的計算資源以及對物體的邊緣部分分類準(zhǔn)確度不足的問題。

鑒于現(xiàn)有的視覺SLAM算法存在在動態(tài)環(huán)境下魯棒性較差并且相機(jī)位姿估計不夠準(zhǔn)確等問題，本文針對ORB-SLAM2算法進(jìn)行改進(jìn)：

1）在特征點(diǎn)提取階段，替換ORB-SLAM2基于圖像金字塔的特征點(diǎn)提取算法，采用幾何對應(yīng)神經(jīng)網(wǎng)絡(luò)（geometric correspondence network version 2，GCNv2）[10]來提取圖像的特征點(diǎn)。雖然基于旋轉(zhuǎn)不變特征點(diǎn)的定位與建圖[11]算法相對于加速穩(wěn)健特征（speeded up robust features，SURF）[12]和尺度不變特征變換（scale-invariant feature transform，SIFT）[13]算法有更快的計算速度[14]，但是仍然存在著特征點(diǎn)誤匹配的問題，并且提取的特征點(diǎn)不夠穩(wěn)定，在紋理不足的場景很容易跟丟[15]。在某些ORB-SLAM2無法跟蹤的極端場景下，GCNv2神經(jīng)網(wǎng)絡(luò)算法有著更好的表現(xiàn)。

2）在ORB-SLAM2算法的基礎(chǔ)上，添加了目標(biāo)檢測線程。該線程會使用目標(biāo)檢測（you only look once version 4，YOLOv4）[16]算法對傳輸過來的圖片進(jìn)行目標(biāo)檢測，并對圖片中的物體添加語義信息。最后將結(jié)果傳入到跟蹤線程。

3）本文算法使用光流法[17]跟蹤圖像中的特征點(diǎn)，對于跟蹤到的特征點(diǎn)進(jìn)行運(yùn)動一致性檢測，根據(jù)特征點(diǎn)到極線的距離判斷是否為動態(tài)特征點(diǎn)。然后結(jié)合YOLOv4檢測到的物體框，如果物體框內(nèi)的動態(tài)特征點(diǎn)與總特征點(diǎn)的比值大于一定閾值，就將該物體框設(shè)置為動態(tài)物體框，反之設(shè)置為靜態(tài)物體框。由于YOLOv4的目標(biāo)檢測框過大，會出現(xiàn)動態(tài)物體框和靜態(tài)物體框重疊的情況，如果直接刪除動態(tài)框內(nèi)的特征點(diǎn)，會損失大量的靜態(tài)特征點(diǎn)，反而會影響算法的精度。對于動態(tài)物體框內(nèi)的特征點(diǎn)，只刪除不在靜態(tài)物體框的特征點(diǎn)。這個方法能有效檢測出潛在的運(yùn)動物體，并盡可能地減小動態(tài)物體框所占的圖像面積，然后在后續(xù)的操作中會刪除動態(tài)區(qū)域里的特征點(diǎn)，降低動態(tài)特征點(diǎn)對算法精度的影響。

4）通過測試本文算法在慕尼黑工業(yè)大學(xué)（Technical University of Munich，TUM）提供的數(shù)據(jù)集下的表現(xiàn)情況，并與ORB-SLAM2算法進(jìn)行對比，判斷本文算法的優(yōu)化程度。

1 本文算法

1.1 算法框架

本文算法是基于ORB-SLAM2，在原本算法的跟蹤、局部建圖和回環(huán)檢測這3個主要線程[18]中添加了目標(biāo)檢測線程。整體算法框架如圖1所示，圖中RGB-D指彩色-深度（Red Green Blue-Depth）數(shù)據(jù)集。當(dāng)開始對系統(tǒng)輸入圖像數(shù)據(jù)的時候，這些圖像會同時傳入跟蹤線程和目標(biāo)檢測線程進(jìn)行處理。首先，在跟蹤線程中，特征點(diǎn)提取部分使用GCNv2神經(jīng)網(wǎng)絡(luò)來提取特征點(diǎn)，隨后通過對網(wǎng)絡(luò)輸出進(jìn)行非極大值抑制和雙線性采樣來獲取特征點(diǎn)和描述子。目標(biāo)檢測線程通過YOLOv4對輸入圖像進(jìn)行目標(biāo)檢測，得到每個物體的語義標(biāo)簽。對圖像的特征點(diǎn)進(jìn)行光流跟蹤，通過運(yùn)動一致性檢測判斷跟蹤成功的特征點(diǎn)是否為動態(tài)特征點(diǎn)。當(dāng)一個物體檢測框里的動態(tài)特征點(diǎn)所占比例達(dá)到一定閾值的時候，該物體即是潛在的運(yùn)動物體，將該物體框設(shè)置為動態(tài)物體框，反之設(shè)置為靜態(tài)物體框。最后保留處于動態(tài)物體框內(nèi)的靜態(tài)特征點(diǎn)，刪除剩余的動態(tài)特征點(diǎn)，用這些優(yōu)化后的靜態(tài)特征點(diǎn)來進(jìn)行相機(jī)的位姿估計。

圖1 算法框架

1.2 特征提取

在基于特征點(diǎn)法的視覺SLAM中，特征點(diǎn)提取算法的優(yōu)劣影響著后續(xù)對相機(jī)的位姿估計以及系統(tǒng)的穩(wěn)定性。ORB算法對于加速分割測試特征（features from accelerated segment test，F(xiàn)AST）角點(diǎn)的改進(jìn)以及添加二進(jìn)制的描述子，很大程度上提升了特征點(diǎn)的準(zhǔn)確度和計算速度。雖然ORB算法采用了四叉樹對圖像中的特征點(diǎn)進(jìn)行均勻分配，但是仍然會出現(xiàn)特征點(diǎn)重疊、分布不均勻的現(xiàn)象，從而影響系統(tǒng)的穩(wěn)定性[19]。

為了解決在動態(tài)場景下ORB算法提取的特征點(diǎn)分布不均勻以及計算復(fù)雜的問題，使用GCNv2神經(jīng)網(wǎng)絡(luò)來替換ORB算法特征點(diǎn)提取部分，獲得更為準(zhǔn)確的關(guān)鍵點(diǎn)和描述子。GCNv2是對GCN的改進(jìn)，是一種能夠計算圖像關(guān)鍵點(diǎn)和描述子的深度神經(jīng)網(wǎng)絡(luò)，可用來作為實(shí)時SLAM的前端。GCNv2采用了和ORB算法相同格式的二進(jìn)制描述子，該網(wǎng)絡(luò)通過卷積的方法，由原始灰度圖獲得低像素、多通道的概率圖和特征圖。再通過上采樣改變概率圖的像素大小，并根據(jù)變化后的概率圖中各點(diǎn)的概率值確定圖中的特征點(diǎn)位置。最后以特征點(diǎn)位置為基礎(chǔ)，通過特征圖中的對應(yīng)值和一個二值化網(wǎng)絡(luò)層提取出每個特征點(diǎn)對應(yīng)的二值化描述子。如圖2所示為GCNv2的神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)，圖中代表輸入圖像的分辨率。

圖2 GCNv2神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)

ORB提取的特征點(diǎn)分布如圖3（a）所示，GCNv2提取的特征點(diǎn)分布如圖3（b）所示。對于室內(nèi)場景，ORB提取的特征點(diǎn)分布并不均勻，并且有些區(qū)域的特征點(diǎn)沒有檢測出來；而GCNv2神經(jīng)網(wǎng)絡(luò)提取的特征點(diǎn)分布得更加均勻，并且沒有出現(xiàn)特征點(diǎn)重疊的現(xiàn)象，這是因?yàn)镚CNv2在訓(xùn)練的時候使用了非極大值抑制。當(dāng)相機(jī)快速運(yùn)動的時候，ORB特征跟蹤有時候會出現(xiàn)數(shù)據(jù)丟失的情況，但是GCNv2提取的特征點(diǎn)能很好地完成跟蹤，以此可見，GCNv2提取的特征點(diǎn)穩(wěn)定性更高。

圖3 特征點(diǎn)提取

1.3 目標(biāo)檢測

目標(biāo)檢測是機(jī)器人視覺研究領(lǐng)域的主要內(nèi)容之一，是從圖片中分辨出所有感興趣的目標(biāo)，并且精準(zhǔn)識別出目標(biāo)類型和位置[20]。目前，目標(biāo)檢測算法主要分為2類，即單階網(wǎng)絡(luò)和雙階網(wǎng)絡(luò)。單階網(wǎng)絡(luò)可以從網(wǎng)絡(luò)中直接提取特征來進(jìn)行物體種類的預(yù)測以及位置的計算，這些算法的優(yōu)點(diǎn)是通過適當(dāng)?shù)亟档湍繕?biāo)識別精度來大幅提升算法的計算速度。雙階網(wǎng)絡(luò)的第一階段是生成預(yù)定義的候選包圍框；第二階段通過卷積神經(jīng)網(wǎng)絡(luò)對包圍框內(nèi)的物體進(jìn)行分類，該算法識別精度高但計算用時長。

由于本文算法對物體識別精度要求不是很高，為了提升算法的計算速度，采用單階網(wǎng)絡(luò)YOLOv4來對圖像進(jìn)行目標(biāo)檢測。如圖4所示為使用YOLOv4對室內(nèi)環(huán)境進(jìn)行目標(biāo)檢測的結(jié)果，可以很清楚地看到，YOLOv4算法能夠很好地將圖片中各個物體檢測出來。

圖4 目標(biāo)檢測結(jié)果

1.4 動態(tài)物體特征點(diǎn)刪除

通過YOLOv4目標(biāo)檢測，獲得了邊界框。由于YOLOv4的目標(biāo)檢測框過大，會出現(xiàn)動態(tài)物體框和靜態(tài)物體框部分重疊的情況。如果直接刪除動態(tài)框內(nèi)的關(guān)鍵點(diǎn)，會損失大量的靜態(tài)特征點(diǎn)，反而影響算法的精度，造成定位失敗的后果。所以本文提出的算法將目標(biāo)檢測框分為動態(tài)框及靜態(tài)框，只有當(dāng)物體處在動態(tài)框而不處于靜態(tài)框內(nèi)的時候，才會將其刪除。

對于潛在的運(yùn)動物體，比如大部分時間處于靜止但又能被人輕易移動的書本、椅子等物體，通過光流法追蹤其特征點(diǎn)，并用運(yùn)動一致性檢測來判斷其是否為動態(tài)物體。

1.4.1 光流法

光流法是一種計算圖像中特征點(diǎn)運(yùn)動的方法。它計算了圖像中的每一個特征點(diǎn)的運(yùn)動方向和速度，從而實(shí)現(xiàn)對目標(biāo)運(yùn)動的跟蹤。光流算法利用圖像間的像素位移來計算物體的運(yùn)動信息，是一種非常有效的物體運(yùn)動估計方法，在計算機(jī)視覺、機(jī)器人、目標(biāo)跟蹤等領(lǐng)域有著廣泛的應(yīng)用。目前光流法主要分為2種，即稠密光流和稀疏光流。稠密光流計算了圖像中所有像素的光流向量，它的優(yōu)點(diǎn)是精度高，對于細(xì)微的運(yùn)動表示效果較好。但是，稠密光流的計算量很大，不適用于實(shí)時處理。相比之下，稀疏光流只對圖像中的一些關(guān)鍵點(diǎn)計算光流，從而減少計算量。為了盡可能減少算法的運(yùn)行時間，提高實(shí)時性，本文采用稀疏光流追蹤特征點(diǎn)的位置。假設(shè)物體在2幀圖像中亮度一樣，并且在時刻處于圖像(,)處像素的灰度值為()，當(dāng)?shù)?d時刻時，該點(diǎn)像素運(yùn)動到(d,d,d)，即有

將等式左邊進(jìn)行一階泰勒展開得

設(shè)dd，dd，寫成矩陣形式即

由于該等式存在2個未知數(shù)，所以還需要添加一個額外的約束來求解、。假設(shè)在一個大小為×的窗口中，像素有相同的運(yùn)動速度，則有

式（5）即為的形式，由此可求得光流()的最小二乘解為

1.4.2 運(yùn)動一致性檢測

通過使用YOLOv4可以有效地檢測出輸入圖像中的動態(tài)對象，但是并不能很好地識別潛在的動態(tài)對象。為了解決這個問題，采用DS-SLAM提出的基于稀疏光流的動態(tài)點(diǎn)檢測方法，即運(yùn)動一致性檢測。首先，通過計算光流金字塔，得到當(dāng)前幀中匹配的特征點(diǎn)，并刪除一些太接近圖像邊緣的匹配特征點(diǎn)及周邊像素差異過大的匹配特征點(diǎn)。然后通過匹配的特征點(diǎn)計算出基本矩陣，再利用基本矩陣計算出當(dāng)前幀中的極線。最后，計算出匹配點(diǎn)到極線的距離。如果距離大于設(shè)定的閾值，則將其設(shè)定為動態(tài)特征點(diǎn)，反之則認(rèn)為是靜態(tài)特征點(diǎn)。該方法的具體原理如圖5所示。

圖5 移動一致性檢測

圖5中：2個平行四邊形1、2表示的是2個不同位置相機(jī)的成像平面；1、2是相機(jī)的光心；是空間點(diǎn)，對應(yīng)在2個成像平面的像素坐標(biāo)是1、2，設(shè)其坐標(biāo)為1=(111)，2=(221)；點(diǎn)1、2組成的平面與2個相機(jī)的成像平面的交線即為極線1、2。極線1可以通過式（7）計算出來，即

式中：為基本矩陣；、、為極線1的方程參數(shù)，即極線1在平面的方程為0。對級約束描述了一幅圖像的一個點(diǎn)到另一幅圖像中的對應(yīng)極線的映射，映射關(guān)系可以通過式(8)描述，即

通過目標(biāo)檢測算法可以得到潛在運(yùn)動物體的物體框，在計算物體框內(nèi)動態(tài)點(diǎn)占所有特征點(diǎn)的比例時，當(dāng)比例超過預(yù)設(shè)閾值，就將該物體框視為動態(tài)物體框，并將動態(tài)物體框內(nèi)處于靜態(tài)物體框之外的特征點(diǎn)全部刪除，最后使用剩余的靜態(tài)特征點(diǎn)進(jìn)行位姿估計。

2 實(shí)驗(yàn)與結(jié)果分析

本文改進(jìn)算法的測試平臺是臺式電腦，硬件配置是Intel i5-11400H，主頻2.7 GHz，內(nèi)存為16 G，顯卡為RTX3060，搭載系統(tǒng)為Ubuntu18.04。這些硬件和軟件配置足以支持本文的算法實(shí)驗(yàn)，并且能夠提供充足的計算資源來進(jìn)行測試和評估。

2.1 實(shí)驗(yàn)數(shù)據(jù)集

為了評估本文提出的在室內(nèi)動態(tài)場景下倉儲機(jī)器人定位的精確性和魯棒性，采用德國慕尼黑工業(yè)大學(xué)提供的TUM RGB-D（Red Green Blue Depth）數(shù)據(jù)集，該數(shù)據(jù)集是開源數(shù)據(jù)集，由39組不同室內(nèi)環(huán)境的圖像序列組成。采集數(shù)據(jù)的設(shè)備是微軟公司的RGB-D Kinect相機(jī)，錄制頻率為 30 Hz，包含彩色圖像、深度圖像和相機(jī)位姿真值3個部分。其中的動態(tài)數(shù)據(jù)集分為2個部分，分別是“sitting”序列和“walking”序列。“sitting”序列平均時長為31 s左右，涵蓋了2個人坐在桌子前的場景，包含人物的肢體動作和靜態(tài)物體的移動?！皐alking”序列平均時長為30 s左右，涵蓋了2個人圍著桌子移動的場景，動態(tài)場景占據(jù)該序列一半以上的時間。除此之外，相機(jī)的運(yùn)動分為以下4種：1）halfsphere（半圓，即相機(jī)按照直徑為1 m的半圓形軌跡運(yùn)動)；2）rpy（相機(jī)按照翻滾—俯仰—偏擺即roll—pitch—yaw的順序進(jìn)行翻轉(zhuǎn)運(yùn)動）；3）static（靜止，即相機(jī)基本保持不動）；4）（相機(jī)按照軸的順序移動）。該數(shù)據(jù)集適合評估在復(fù)雜動態(tài)室內(nèi)環(huán)境下所提出的算法性能。

本文算法的評價指標(biāo)為相對位姿誤差（relative pose error，RPE）和絕對軌跡誤差（absolute trajectory error，ATE）及變化幅度，其中相對姿態(tài)誤差是指在SLAM系統(tǒng)中，相鄰幀間姿態(tài)的差異。這是由多種因素導(dǎo)致的，包括傳感器誤差、計算誤差和環(huán)境干擾。相對姿態(tài)誤差會影響SLAM系統(tǒng)的性能。因?yàn)槿绻噜弾g姿態(tài)誤差過大，它們就不能正確地匹配，進(jìn)而影響地圖的精確性。相機(jī)第幀的相對位姿誤差計算公式為：

絕對軌跡誤差是指SLAM系統(tǒng)中機(jī)器人的實(shí)際軌跡與預(yù)測軌跡的偏差。它可以用來評估SLAM系統(tǒng)的性能。第幀的絕對軌跡誤差的計算公式為：

式中為轉(zhuǎn)換矩陣。

變化幅度的計算公式為

式中：為ORB-SLAM2的運(yùn)行結(jié)果；為本文算法的運(yùn)行結(jié)果。

2.2 特征點(diǎn)提取實(shí)驗(yàn)

為了驗(yàn)證本文算法提取特征點(diǎn)的有效性，首先顯示ORB-SLAM2特征點(diǎn)的分布以及本文算法的特征點(diǎn)分布，通過觀察場景中人身上是否有特征點(diǎn)來判斷算法是否成功。并且比較2個算法在靜態(tài)環(huán)境下的絕對軌跡誤差和相對軌跡誤差。為了實(shí)驗(yàn)公平，數(shù)據(jù)集選擇運(yùn)動幅度較小的sitting序列來進(jìn)行測試。如圖6（a）所示為ORB-SLAM2算法提取的特征點(diǎn)，可以看到，人的身上分布著大量的特征點(diǎn)。如圖6（b）所示為改進(jìn)后的算法，可知有效地剔除了動態(tài)物體“人”身上的特征點(diǎn)。

圖6 算法提取特征點(diǎn)

如表1所示為GCNv2和ORB-SLAM2在特征點(diǎn)提取方面的對比?？芍陟o態(tài)環(huán)境或者微動態(tài)環(huán)境下，GCNv2提取的特征點(diǎn)相比于ORB-SLAM2提取的特征點(diǎn)，準(zhǔn)確性上都更好一點(diǎn)。

表1 ORB-SLAM2和本文算法的絕對軌跡誤差和相對軌跡誤差

2.3 定位誤差實(shí)驗(yàn)

為了驗(yàn)證本文算法在高度動態(tài)場景下的穩(wěn)定性及準(zhǔn)確度，采用walking序列的數(shù)據(jù)集，測試本文算法及ORB-SLAM2算法的相機(jī)絕對軌跡誤差，如表2所示，其中Mean和Median分別表示數(shù)據(jù)的平均值和中位數(shù)。

表2 ORB-SLAM2和本文算法的絕對軌跡誤差

對表2中數(shù)據(jù)進(jìn)行分析，可以得到改進(jìn)算法的提升程度。在高度動態(tài)場景下，移動的物體對傳統(tǒng)ORB-SLAM2算法的精度影響很大。本文提出的算法有效地剔除了動態(tài)特征點(diǎn)，絕對軌跡誤差明顯減小，減小的幅度在95.56%～98.21%之間。如圖7和圖8所示為ORB-SLAM2和本文算法對數(shù)據(jù)集walking_、walking_halfsphere相機(jī)的運(yùn)行軌跡估計值和運(yùn)行軌跡真實(shí)值在、、軸上的可視化曲線以及RPE誤差。

在圖7（a）、圖7（b）、圖8（a）、圖8（b）中，黑線代表相機(jī)的運(yùn)行軌跡真值，藍(lán)線代表算法的估計軌跡值，紅線代表二者的誤差。由可視化曲線可以很明顯地看出來，本文算法有效地減小了ORB-SLAM2算法對相機(jī)位姿估計的誤差。圖7（c）、圖7（d）、圖8（c）、圖8（d）描述了隨著時間的變化，估計算法軌跡與真實(shí)軌跡之間的平移誤差變化。圖7（c）、圖7（d）中間部分誤差較小，原因是此處數(shù)據(jù)集中相機(jī)運(yùn)動相對穩(wěn)定，場景中動態(tài)物體運(yùn)動幅度較小?？芍疚乃惴ㄜ壽E的平移量比ORB-SLAM2算法的軌跡平移量要小很多。

圖7 ORB-SLAM2估計的軌跡以及平移誤差

圖8 本文算法估計的軌跡以及平移誤差

3 結(jié)束語

本文提出了倉儲機(jī)器人在室內(nèi)動態(tài)場景下的視覺SLAM算法，這個算法在ORB-SLAM2算法的基礎(chǔ)上，用GCNv2來提取特征點(diǎn)，用YOLOv4來進(jìn)行目標(biāo)檢測。通過對特征點(diǎn)進(jìn)行光流跟蹤以及移動一致性檢測，成功地將場景中的動態(tài)特征點(diǎn)剔除，大幅提升了系統(tǒng)在動態(tài)場景下的準(zhǔn)確性和魯棒性。實(shí)驗(yàn)結(jié)果表明，本文算法明顯地提升了ORB-SLAM2算法在動態(tài)場景下的穩(wěn)定性與準(zhǔn)確性。本文的算法仍有以下不足之處：1）YOLOv4算法可能出現(xiàn)檢測出錯的情況，后續(xù)可以優(yōu)化網(wǎng)絡(luò)或者使用精度更好的目標(biāo)檢測算法來解決這個問題。2）本文算法在數(shù)據(jù)集中得到了較好的驗(yàn)證，但尚未應(yīng)用在實(shí)時場景，后續(xù)可對其在實(shí)際場景下進(jìn)行測試。

[1] 葛研. AGV自動導(dǎo)引車在國內(nèi)的發(fā)展前景探討[J]. 設(shè)備管理與維修, 2021, 489(Z1): 157-159. DOI:10.16621/j.cnki.issn1001-0599.2021.02.81.

[2] 吳凡, 宗艷桃, 湯霞清. 視覺SLAM的研究現(xiàn)狀與展望[J/OL]. 計算機(jī)應(yīng)用研究, 2019: 1-9[2023-04-19]. https://doi.org/10.19734/j.issn.1001-3695.2019.02.0035.

[3] 任偉建, 高強(qiáng), 康朝海, 等. 移動機(jī)器人同步定位與建圖技術(shù)綜述[J]. 計算機(jī)測量與控制, 2022, 30(2): 1-10,37. DOI:10.16526/j.cnki.11-4762/tp.2022.02.001.

[4] MUR-ARTAL R, MONTIEL JMM, TARDóS D J. Orb-SLAM: A versatile and accurate monocular SLAM system[J]. CoRR, 2015(5): 1147-1163.

[5] Forster C, Pizzoli M, D Scaramuzza. SVO: Fast semi-direct monocular visual odometry[C]// IEEE International Conference on Robotics & Automation. IEEE, 2014.

[6] Engel J, Schps T, Cremers D. LSD-SLAM: Large-scale direct monocular SLAM[C]// European Conference on Computer Vision. Springer, Cham, 2014.

[7] 張樂添,趙冬青, 賈曉雪, 等. 面向動態(tài)環(huán)境的雙目/慣性SLAM系統(tǒng)[J]. 導(dǎo)航定位學(xué)報, 2022, 10(6): 144-150. DOI:10.16547/j.cnki.10-1096.20220619.

[8] Berta B, José M F, Javier C, et al. Dynaslam: Tracking, mapping and inpainting in dynamic scenes[J]. IEEE Robot Autom Lett, 2018, 3(4): 4076-4083.

[9] Yu C, Liu Z, Liu X, et al. DS-SLAM: A semantic visual SLAM towards dynamic environments[C]// 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018.

[10] TANG Jiexiong, Ericson L, Folkesson J, et al. Gcnv2: Efficient correspondence prediction for real-time SLAM[J]. IEEE Robot Autom Lett, 2019, 4(4): 3505-3512.

[11] RUBLEE E, RABAUD V, KONOLIGE K, et al. ORB:An efficient alternative to SIFT or SURF[C]// International Conference on Computer Vision. IEEE, 2012: 2564-2571.

[12] BAY H, ESS A, TUYTELAARS T, et al. Speeded-Up robust features(SURF)[J]. Computer Vision & Image Understanding, 2008, 110(3): 346-359.

[13] LOWE D G. Distinctive image features from scale-invariant keypoints[J]. International Journal of Computer Vision, 2004, 60(2): 91-110.

[14] 索春寶, 楊東清, 劉云鵬. 多種角度比較SIFT、SURF、BRISK、ORB、FREAK算法[J]. 北京測繪, 2014(4): 23-26,22. DOI:10.19580/j.cnki.1007-3000.2014.04.006.

[15] 付元辰, 馬鳴庸, 丁龍陽. 面向室內(nèi)視覺定位的點(diǎn)特征提取算法比較[J]. 導(dǎo)航定位學(xué)報, 2022, 10(1): 36-40.

[16] Redmon J, Divvala S, Girshick R, et al. You only look once: Unified, real-time object detection[C]// Computer Vision & Pattern Recognition. IEEE, 2016.

[17] BEAUCHEMIN S S, BARRON J L. The computation of optical flow[J]. ACM Computing Surveys (CSUR), 1995, 27(3): 433-466.

[18] Mur-Artal R, Tardós J D. ORB-SLAM2:An open-source SLAM system for monocular,stereo and RGB-D cameras[J]. IEEE Transactions on Robotics, 2017, 33(5): 1255-1262.

[19] 成怡, 佟曉宇. 基于改進(jìn)ORB算法的移動機(jī)器人視覺SLAM方法研究[J]. 電子技術(shù)應(yīng)用, 2019 ,45(1): 10-13,18. DOI:10.16157/j.issn.0258-7998.183046.

[20] 尹宏鵬, 陳波, 柴毅, 等. 基于視覺的目標(biāo)檢測與跟蹤綜述[J]. 自動化學(xué)報, 2016, 42(10): 1466-1489. DOI:10.16383/j.aas.2016.c150823.

Vision SLAM of storage robot in dynamic scene

XU Xing, LIU Qiong, HUANG Kaikun

（School of Mechanical Engineering, University of South China, Hengyang, Hunan 421200, China）

Aiming at the problems that the current majority of visual simultaneous localization and mapping (SLAM) algorithms utilized by indoor storage robots are predicated upon the assumption that the robot be operating within a static environment, however, the presence of mobile objects within the visual field can greatly impair the accuracy and stability of the robot's inherent positioning capabilities, while other indoor positioning technologies such as ultra wide band (UWB) and bluetooth must work under the condition of wireless signal coverage, the paper proposed a vision-based SLAM algorithm tailored specifically to indoor storage robots operating within dynamic scenes: the geometric correspondence network version 2 (GCNv2) was leveraged to supplant the traditional oriented fast and rotated brief (ORB) feature point extraction algorithm based on image pyramids based on the ORB-SLAM2 framework; and the semantic threads of target detection technique you only look once version 4 (YOLOv4) were added in tandem with optical flow to track feature points; then the potential dynamic objects were identified in the image through motion consistency detection; finally, after the removal of dynamic feature points, the position and attitude estimation was performed. Experimental result showed that in highly dynamic indoor scenes, the absolute trajectory error of the proposed algorithm, compared to the ORB-SLAM2 algorithm, would be reduced by 95.56%～98.21%, which indicates that the proposed method could effectively solve the problem of inaccurate positioning of ORB-SLAM2 in dynamic scenarios.

storage robot; dynamic scene; geometric correspondence network version 2 (GCNv2); object detection; oriented fast and rotated brief (ORB)-simultaneous localization and mapping (SLAM) 2

徐興，劉瓊，黃開坤. 動態(tài)場景下倉儲機(jī)器人的視覺定位與建圖[J]. 導(dǎo)航定位學(xué)報, 2023, 11(6): 110-118.（XU Xing, LIU Qiong, HUANG Kaikun. Vision SLAM of storage robot in dynamic scene[J]. Journal of Navigation and Positioning, 2023, 11(6): 110-118.）DOI:10.16547/j.cnki.10-1096.20230614.

P228

A

2095-4999(2023)06-0110-09

2023-03-23

國家重點(diǎn)研發(fā)計劃項(xiàng)目（2023YFC3010900）；南華大學(xué)教學(xué)改革項(xiàng)目（2019ZD-XJG09）。

徐興（1996—），男，安徽宣城人，碩士研究生，研究方向?yàn)槭覂?nèi)定位。

劉瓊（1980—），男，湖南衡陽人，博士，副教授，研究方向?yàn)闄C(jī)械設(shè)計及理論。