作者：最愛喝獸奶 | 來源：3DCV

在公眾號(hào)「3DCV」后臺(tái)，回復(fù)「原論文」可獲取論文pdf和代碼鏈接

添加微信：dddvision，備注：NeRF SLAM，拉你入群。文末附行業(yè)細(xì)分群

1、導(dǎo)讀

在多機(jī)器人系統(tǒng)中，準(zhǔn)確的定位和建圖對(duì)于機(jī)器人之間的協(xié)作和任務(wù)執(zhí)行至關(guān)重要。然而，由于環(huán)境的復(fù)雜性和傳感器的限制，單個(gè)機(jī)器人的定位和建圖可能存在誤差。因此，本文提出了一種名為CoVOR-SLAM的方法，通過結(jié)合視覺里程計(jì)和距離測(cè)量數(shù)據(jù)，實(shí)現(xiàn)了多機(jī)器人系統(tǒng)的協(xié)作定位和建圖。該方法通過數(shù)據(jù)融合和優(yōu)化算法，提高了定位和建圖的準(zhǔn)確性，并降低了計(jì)算復(fù)雜度。通過實(shí)驗(yàn)驗(yàn)證，CoVOR-SLAM在減小視覺里程計(jì)誤差和實(shí)現(xiàn)高精度定位方面取得了顯著的效果。

2、研究思路

本文的研究思路是通過合作式同時(shí)定位與地圖構(gòu)建（Cooperative SLAM）方法，利用視覺里程計(jì)（Visual Odometry）和距離測(cè)量數(shù)據(jù)來解決多機(jī)器人系統(tǒng)中的定位和地圖構(gòu)建問題。具體而言，本研究提出了一種名為CoVOR-SLAM的方法，該方法利用視覺里程計(jì)和距離測(cè)量數(shù)據(jù)進(jìn)行數(shù)據(jù)融合，以提高定位的準(zhǔn)確性和地圖的完整性。

3、貢獻(xiàn)

包括以下幾點(diǎn)：

• 引入了距離測(cè)量數(shù)據(jù)：傳統(tǒng)的視覺里程計(jì)方法在定位時(shí)存在尺度模糊的問題，而本研究通過引入距離測(cè)量數(shù)據(jù)，可以減小尺度模糊的影響，提高定位的準(zhǔn)確性。

• 利用多機(jī)器人的合作：本研究通過多機(jī)器人之間的合作，利用彼此之間的距離測(cè)量數(shù)據(jù)進(jìn)行數(shù)據(jù)融合，從而提高整個(gè)系統(tǒng)的定位和地圖構(gòu)建的準(zhǔn)確性。

• 降低計(jì)算和通信負(fù)載：本研究的方法在數(shù)據(jù)融合過程中需要的計(jì)算和通信負(fù)載相對(duì)較低，適用于計(jì)算和通信能力有限的多機(jī)器人系統(tǒng)。

4、研究內(nèi)容

本篇文章的研究內(nèi)容是關(guān)于多機(jī)器人系統(tǒng)中的協(xié)作SLAM（同時(shí)定位與地圖構(gòu)建）問題。文章提出了一種名為CoVOR-SLAM的方法，該方法利用視覺里程計(jì)和距離測(cè)量數(shù)據(jù)來實(shí)現(xiàn)多機(jī)器人系統(tǒng)的定位和地圖構(gòu)建。CoVOR-SLAM通過融合視覺里程計(jì)和距離測(cè)量數(shù)據(jù)，可以減少視覺里程計(jì)的尺度模糊問題，并且在計(jì)算復(fù)雜度上具有較低的要求。文章通過實(shí)驗(yàn)數(shù)據(jù)驗(yàn)證了CoVOR-SLAM方法的有效性，并與其他方法進(jìn)行了比較分析。在這里推薦徹底搞懂基于LOAM框架的3D激光SLAM: 源碼剖析到算法優(yōu)化

5、創(chuàng)新性

本研究的創(chuàng)新性主要體現(xiàn)在以下幾個(gè)方面：

• 提出了一種基于視覺里程計(jì)和距離測(cè)量的多機(jī)器人協(xié)作SLAM方法。傳統(tǒng)的SLAM方法通常只使用視覺信息或者距離測(cè)量信息，而本研究將兩者結(jié)合起來，通過多機(jī)器人之間的協(xié)作，實(shí)現(xiàn)更準(zhǔn)確和魯棒的環(huán)境建模和定位。

• 引入了基于范圍測(cè)量的協(xié)作因子圖優(yōu)化。通過使用范圍測(cè)量信息，可以更好地約束機(jī)器人之間的相對(duì)位置關(guān)系，從而提高整個(gè)系統(tǒng)的定位精度和魯棒性。

• 提出了一種增量式的姿態(tài)估計(jì)方法。通過將估計(jì)的姿態(tài)增量轉(zhuǎn)換為矩陣形式，并使用Levenberg-Marquardt算法進(jìn)行優(yōu)化，可以實(shí)現(xiàn)姿態(tài)的增量更新，從而提高定位的準(zhǔn)確性和效率。

• 在實(shí)驗(yàn)數(shù)據(jù)上驗(yàn)證了CoVOR-SLAM的性能。通過在實(shí)際場(chǎng)景中進(jìn)行實(shí)驗(yàn)，本研究證明了CoVOR-SLAM相比于傳統(tǒng)的視覺里程計(jì)方法具有更高的定位精度和更低的計(jì)算復(fù)雜度。

6、CoVOR-SLAM

CoVOR-SLAM是一種使用視覺里程計(jì)和測(cè)距數(shù)據(jù)的協(xié)作式SLAM方法。它可以用于多機(jī)器人系統(tǒng)中，每個(gè)機(jī)器人通過視覺里程計(jì)估計(jì)自己的姿態(tài)和地圖點(diǎn)，并通過交換消息來進(jìn)行數(shù)據(jù)融合。CoVOR-SLAM的框架如圖所示。

每個(gè)機(jī)器人使用視覺里程計(jì)在本地參考框架中估計(jì)自己的姿態(tài)和地圖點(diǎn)。數(shù)據(jù)融合模塊通過交換三種類型的消息（KeyframeMsg、InterRangeMsg和AnchorRangeMsg）進(jìn)行多機(jī)器人數(shù)據(jù)融合。這些消息包含機(jī)器人的姿態(tài)和測(cè)距數(shù)據(jù)。

6.1、系統(tǒng)模型

多智能體系統(tǒng)模型如圖所示，圖中使用了兩個(gè)智能體來解釋模型。智能體的導(dǎo)航坐標(biāo)系被表示為，它是一個(gè)固定的坐標(biāo)系，智能體用它來表示自己的軌跡。使用視覺里程計(jì)（VO），每個(gè)機(jī)器人逐步估計(jì)其機(jī)載相機(jī)的6自由度姿態(tài)（3D旋轉(zhuǎn)和平移）在其本地參考坐標(biāo)系中。時(shí)間時(shí)刻在坐標(biāo)系中的無誤差相機(jī)姿態(tài)表示為：

其中?。

對(duì)于搭載單目相機(jī)的智能體，由于視覺里程計(jì)存在全局尺度模糊，平移向量只能以全局尺度模糊的方式進(jìn)行估計(jì)。因此，在坐標(biāo)系中的姿態(tài)也是按比例縮放的。在這個(gè)示例設(shè)置中，將智能體1的參考坐標(biāo)系設(shè)置為全局參考坐標(biāo)系（即）。因此，智能體1的帶有尺度模糊的6自由度本地姿態(tài)可以通過簡單地添加一個(gè)尺度因子轉(zhuǎn)換為相機(jī)狀態(tài)在全局坐標(biāo)系中的7自由度表示，這個(gè)轉(zhuǎn)換可以用矩陣形式表示：

當(dāng)我們使用單目相機(jī)估計(jì)每個(gè)機(jī)器人的姿態(tài)時(shí)，表示單目相機(jī)軌跡估計(jì)的絕對(duì)尺度。當(dāng)使用立體相機(jī)估計(jì)姿態(tài)時(shí)，我們可以簡單地初始化=1，因?yàn)榱Ⅲw視覺里程計(jì)沒有全局尺度模糊（使用已知的立體相機(jī)基線長度估計(jì)軌跡的絕對(duì)尺度）。然而，立體相機(jī)的相對(duì)尺度估計(jì)誤差也會(huì)隨時(shí)間累積。

在全局坐標(biāo)系G中，智能體2的姿態(tài)（使用Sim(3)中的7自由度矩陣參數(shù)化）可以從其本地姿態(tài)（帶有尺度模糊）轉(zhuǎn)換得到：

其中表示兩個(gè)本地參考坐標(biāo)系之間的相似變換，將姿態(tài)從坐標(biāo)系轉(zhuǎn)換到全局坐標(biāo)系，如之前方程中所定義。可以使用已知模式（如ArUco標(biāo)記和棋盤格）在任務(wù)初始化階段估計(jì)。如果兩個(gè)機(jī)器人在初始化期間在同一個(gè)2D平面上移動(dòng)，還可以使用其他方法估計(jì)。在公共全局坐標(biāo)系G中，所有K個(gè)智能體的7自由度姿態(tài)被定義為要估計(jì)的系統(tǒng)狀態(tài)變量：

不同智能體的時(shí)間戳{}可以是異步的。

6.2、測(cè)量模型

測(cè)量模型包括兩種類型的測(cè)量：機(jī)器人之間的測(cè)距和機(jī)器人與錨點(diǎn)之間的測(cè)距。

機(jī)器人之間的測(cè)距（Inter-agent range measurements）：這些測(cè)量是通過機(jī)器人上的測(cè)距傳感器（天線）測(cè)量的，表示為:

機(jī)器人與錨點(diǎn)之間的測(cè)距（Anchor-to-agent range measurements）：這些測(cè)量是通過機(jī)器人上的測(cè)距標(biāo)簽?zāi)K與錨點(diǎn)之間的真實(shí)距離加上一個(gè)誤差項(xiàng)表示的，表示為:

6.3、多機(jī)器人數(shù)據(jù)融合

CoVOR-SLAM是一種使用視覺里程計(jì)和距離測(cè)量進(jìn)行多機(jī)器人系統(tǒng)的協(xié)作SLAM方法。它通過融合視覺里程計(jì)和距離測(cè)量來估計(jì)多個(gè)機(jī)器人的姿態(tài)和地圖點(diǎn)。在CoVOR-SLAM中，每個(gè)機(jī)器人使用視覺里程計(jì)來估計(jì)其相對(duì)于本地參考框架的姿態(tài)和地圖點(diǎn)。為了實(shí)現(xiàn)多機(jī)器人數(shù)據(jù)融合，系統(tǒng)通過三種類型的消息進(jìn)行信息交換：關(guān)鍵幀消息(KeyframeMsg)、機(jī)器人間距離消息(InterRangeMsg)和錨點(diǎn)到機(jī)器人的距離消息(AnchorRangeMsg)。關(guān)鍵幀消息包含機(jī)器人的姿態(tài)信息，用于在數(shù)據(jù)融合模塊中進(jìn)行姿態(tài)估計(jì)。機(jī)器人間距離消息和錨點(diǎn)到機(jī)器人的距離消息用于融合距離測(cè)量，進(jìn)一步提高姿態(tài)估計(jì)的準(zhǔn)確性。CoVOR-SLAM的數(shù)據(jù)融合模塊可以在每個(gè)機(jī)器人的處理器上實(shí)現(xiàn)，使得系統(tǒng)可以以分散的方式運(yùn)行。通過融合視覺里程計(jì)和距離測(cè)量，CoVOR-SLAM能夠準(zhǔn)確估計(jì)機(jī)器人的姿態(tài)，即使在機(jī)器人之間的通信能力有限的情況下也能夠工作。總之，CoVOR-SLAM通過視覺里程計(jì)和距離測(cè)量的協(xié)作，實(shí)現(xiàn)了多機(jī)器人系統(tǒng)的SLAM。它通過數(shù)據(jù)融合來提高姿態(tài)估計(jì)的準(zhǔn)確性，并且可以在通信能力有限的情況下工作。

7、CoVOR-SLAM的優(yōu)勢(shì)

• 使用稀疏的測(cè)距數(shù)據(jù)可以準(zhǔn)確估計(jì)多個(gè)機(jī)器人的姿態(tài)，即使只使用單目相機(jī)（不使用其他傳感器，如IMU和激光雷達(dá)）。其他傳感器可以很容易地集成到基于圖優(yōu)化的框架中。

• 在CoVOR-SLAM的框架中，我們將機(jī)器人的運(yùn)動(dòng)參數(shù)化為7自由度（DoF）的相似變換，因此可以在視覺-測(cè)距數(shù)據(jù)融合過程中考慮視覺定位的尺度誤差（并因此減小融合中的尺度誤差）。

• 它需要比機(jī)器人之間的環(huán)路閉合更少的通信能力，在通信能力有限時(shí)更有用。此外，該算法可以在每個(gè)機(jī)器人上運(yùn)行，因?yàn)樗恍枰罅康挠?jì)算能力。因此，它對(duì)于分散式群體智能系統(tǒng)可能更有用。

• 它不限制機(jī)器人的移動(dòng)方式，只要可以測(cè)量到距離，即使在兩個(gè)模塊之間使用的是無線電信號(hào)，也通?？梢栽谳^大距離上進(jìn)行測(cè)量。

• 機(jī)器人的平臺(tái)選擇可以靈活，因?yàn)樵?code>CoVOR-SLAM中，每個(gè)群體元素不需要具有相似的視角來檢測(cè)機(jī)器人之間的環(huán)路。

• 該算法不需要同時(shí)連接到多個(gè)錨定站。即使其中一個(gè)漫游器只連接到一個(gè)單獨(dú)的錨定站，它也可以顯著減小群體系統(tǒng)的姿態(tài)估計(jì)誤差。CoVOR-SLAM已經(jīng)在仿真和實(shí)驗(yàn)數(shù)據(jù)上進(jìn)行了系統(tǒng)測(cè)試，并取得了良好的結(jié)果。具體的測(cè)試結(jié)果可以在論文中找到。

8、實(shí)驗(yàn)

本研究采用了實(shí)驗(yàn)方法來驗(yàn)證CoVOR-SLAM算法的性能。實(shí)驗(yàn)分為兩個(gè)部分，分別在足球場(chǎng)和碎石場(chǎng)進(jìn)行。在足球場(chǎng)實(shí)驗(yàn)中，使用了由德國航空航天中心通信與導(dǎo)航研究所開發(fā)的兩個(gè)機(jī)器人。其中一個(gè)機(jī)器人（Rover-1）配備了前置攝像頭、UWB傳感器、GNSS RTK和慣性導(dǎo)航系統(tǒng)，用于獲取真實(shí)位置信息。另一個(gè)機(jī)器人（Rover-2）作為錨點(diǎn)固定在一個(gè)位置。通過UWB傳感器獲取機(jī)器人之間的測(cè)距數(shù)據(jù)。實(shí)驗(yàn)數(shù)據(jù)包括在足球場(chǎng)上的行駛時(shí)間、行駛距離和軌跡維度等信息。在碎石場(chǎng)實(shí)驗(yàn)中，同樣使用了上述的兩個(gè)機(jī)器人。實(shí)驗(yàn)數(shù)據(jù)包括在碎石場(chǎng)上的行駛時(shí)間、行駛距離和軌跡維度等信息。通過這些實(shí)驗(yàn)數(shù)據(jù)，研究人員驗(yàn)證了CoVOR-SLAM算法在減小視覺里程計(jì)估計(jì)誤差、提高定位精度和降低計(jì)算復(fù)雜度方面的有效性。同時(shí)，還對(duì)UWB測(cè)距數(shù)據(jù)的誤差模型進(jìn)行了建模，并通過實(shí)驗(yàn)數(shù)據(jù)生成了測(cè)距數(shù)據(jù)。通過對(duì)比實(shí)驗(yàn)結(jié)果和地面真實(shí)軌跡，驗(yàn)證了CoVOR-SLAM算法的性能。

9、結(jié)論

本文提出了一種名為CoVOR-SLAM的協(xié)作式同時(shí)定位與地圖構(gòu)建方法，用于多機(jī)器人系統(tǒng)中的相對(duì)定位。該方法通過交換機(jī)器人之間的姿態(tài)估計(jì)、估計(jì)不確定性和相互測(cè)距信息來實(shí)現(xiàn)定位。與傳統(tǒng)的基于視覺特征的方法相比，CoVOR-SLAM不需要進(jìn)行復(fù)雜的特征匹配和地圖融合，從而大大減少了計(jì)算和通信負(fù)載。實(shí)驗(yàn)結(jié)果表明，CoVOR-SLAM能夠準(zhǔn)確估計(jì)機(jī)器人的姿態(tài)，并且所需的計(jì)算和通信資源較少。因此，CoVOR-SLAM在資源受限的環(huán)境下具有很大的應(yīng)用潛力。