Wheel-SLAM：低成本輪式IMU的SLAM系統(tǒng)

摘要

移動(dòng)機(jī)器人需要一種對(duì)環(huán)境干擾具有魯棒性的可靠位姿估計(jì)器，為此，慣性測量單元（IMU）發(fā)揮了重要作用，因?yàn)樗鼈兛梢元?dú)立感知車輛的全運(yùn)動(dòng)狀態(tài)。然而，由于固有的噪聲和偏置不穩(wěn)定性，它會(huì)出現(xiàn)累積誤差，尤其是對(duì)于低成本傳感器。在我們之前對(duì) Wheel-INS的研究中，本文提出通過在機(jī)器人的輪子上安裝 IMU 以利用旋轉(zhuǎn)調(diào)制來限制純慣性導(dǎo)航系統(tǒng) (INS) 的漂移誤差。但是，由于缺乏外部校正信號(hào)，它仍然在很長一段時(shí)間內(nèi)漂移。

在這篇文章中利用 Wheel-INS 的環(huán)境感知能力來實(shí)現(xiàn)僅使用一個(gè) IMU 的同時(shí)定位和建圖 (SLAM)。具體來說，我們使用路堤角度作為地形特征，以使用 Rao-Blackwellized 粒子濾波器實(shí)現(xiàn)閉環(huán)，根據(jù)粒子維護(hù)的網(wǎng)格圖中的機(jī)器人位置，對(duì)路堤角度進(jìn)行采樣和存儲(chǔ)，根據(jù)當(dāng)前估計(jì)的滾動(dòng)序列與地形圖之間的差異更新粒子的權(quán)重。實(shí)驗(yàn)表明，使用機(jī)器人滾動(dòng)角估計(jì)在 WheelINS 中執(zhí)行 SLAM 的想法是可行的。此外，定位精度比 Wheel-INS 顯著提高（超過 30%）

圖1. 我們之前的工作[1]中的車輛實(shí)驗(yàn)中的車輛橫滾估計(jì)和測試軌跡。在標(biāo)記的區(qū)域內(nèi)，車輛一直往返盤旋?？梢杂^察到相應(yīng)的機(jī)器人橫滾角估計(jì)（表示道路傾斜角）顯示出一個(gè)可以利用的重復(fù)模式，可以用來進(jìn)行環(huán)路閉合檢測和修正。

主要貢獻(xiàn)

提出并實(shí)現(xiàn)了一種Wheel-IMU 使用地形特征（由 Wheel-IMU 測量）的 SLAM 系統(tǒng)。

通過廣泛的現(xiàn)場實(shí)驗(yàn)說明了利用機(jī)器人滾動(dòng)角估計(jì)來實(shí)現(xiàn)閉環(huán)以有效限制Wheel-INS中的誤差漂移的可行性。

據(jù)我們所知，這是文獻(xiàn)中第一個(gè)只為輪式機(jī)器人使用一個(gè)低成本輪式 IMU 的 SLAM 系統(tǒng)。

相關(guān)工作

背景

Wheel-INS [1] 是 Wheel-SLAM 的基礎(chǔ)，用于提供機(jī)器人里程計(jì)和橫滾角估計(jì)，Wheel-INS 有兩個(gè)主要優(yōu)點(diǎn)。首先，輪速可以通過陀螺儀輸出和輪半徑計(jì)算，實(shí)現(xiàn)與只使用一個(gè) IMU（無其他傳感器）的 ODO/INS 相同的信息融合。其次，它可以利用旋轉(zhuǎn)調(diào)制來限制 INS 的誤差漂移。由于空間限制，我們?cè)谶@里僅概述 Wheel-INS 的算法。圖2描述了 Wheel-IMU 的安裝和相關(guān)坐標(biāo)系的定義。



圖2. Wheel-IMU的安裝方案以及車輛框架(v-frame)、車輪框架(w-frame)和IMU本體框架(b-frame)的定義

Wheel-INS 的系統(tǒng)概述如圖3所示，首先，執(zhí)行前向 INS 機(jī)械化來預(yù)測機(jī)器人狀態(tài)，同時(shí)，使用 Wheel-IMU x 軸的陀螺儀輸出計(jì)算輪速，然后，將此車輛速度視為帶有非完整約束（NHC）的外部觀測，并通過誤差狀態(tài)擴(kuò)展卡爾曼濾波器（EKF）來更新狀態(tài)。



圖3. Wheel-INS的概述，ω和f分別代表Wheel-IMU測量的角速度和特定力；PVA表示W(wǎng)heel-IMU的位置、速度和姿態(tài)。我們使用Wheel-IMU的輸出來進(jìn)行INS運(yùn)算以預(yù)測機(jī)器人狀態(tài)（PVA）。在Wheel-IMU的x軸上測量的角速度和輪半徑被用來計(jì)算前進(jìn)速度。這個(gè)速度被作為一個(gè)帶有非完整約束的三維速度觀測值，通過EKF來更新機(jī)器人狀態(tài)以及校正慣性傳感器的誤差，例如陀螺儀偏差。

動(dòng)態(tài)貝葉斯網(wǎng)絡(luò)

粒子濾波是一種序列蒙特卡羅方法，其基本思想是使用重要性抽樣和離散隨機(jī)測量近似概率分布的概念，遞歸計(jì)算相關(guān)概率分布。在粒子濾波中，機(jī)器人狀態(tài)的后驗(yàn)分布由一組粒子表示，這些粒子隨著新信息的融合而遞歸地演化?；赗ao-Blackwellization技術(shù)，Wheel-SLAM將SLAM問題分解為機(jī)器人定位問題和地形映射問題，其取決于機(jī)器人位姿估計(jì)。Wheel-SLAM算法包括四個(gè)主要步驟： 1）通過運(yùn)動(dòng)模型對(duì)機(jī)器人狀態(tài)進(jìn)行采樣； 2）更新地形圖； 3）一旦確定了閉環(huán)，則更新粒子權(quán)重； 4）在必要時(shí)對(duì)粒子進(jìn)行重采樣。 算法1概述了Wheel-SLAM的流程。



網(wǎng)格地形圖的構(gòu)建

與FootSLAM中使用的六邊形網(wǎng)格地圖相比，由于機(jī)器人的運(yùn)動(dòng)模式相對(duì)簡單，我們將網(wǎng)格簡化為正方形，由于我們假設(shè)車輛在水平平面上移動(dòng)，因此我們構(gòu)建一個(gè)二維網(wǎng)格地圖，每個(gè)網(wǎng)格包含Wheel-INS在該位置估計(jì)的相應(yīng)道路傾斜角度，圖4說明了構(gòu)建的網(wǎng)格地圖及機(jī)器人位姿演化。



圖4. 網(wǎng)格地圖的構(gòu)建和回訪識(shí)別的示意圖，不同顏色的曲線表示由不同粒子采樣的機(jī)器人路徑，灰色網(wǎng)格已經(jīng)被機(jī)器人訪問過，因此它們具有道路側(cè)傾角度估計(jì)，一旦一個(gè)粒子檢測到機(jī)器人已經(jīng)連續(xù)返回訪問過的網(wǎng)格（藍(lán)色），則會(huì)報(bào)告一個(gè)潛在的回環(huán)，并進(jìn)行進(jìn)一步的檢查。

粒子權(quán)重更新

一開始，所有粒子都被賦予相同的權(quán)重，當(dāng)機(jī)器人移動(dòng)時(shí)，每個(gè)粒子都有不同的軌跡和地形圖，為了確保環(huán)路閉合的可靠性并減少異常值的影響，我們?cè)O(shè)定了三個(gè)標(biāo)準(zhǔn)。首先，環(huán)路閉合需要在長度為 Nr 的窗口內(nèi)被機(jī)器人位置連續(xù)檢測到。其次，我們使用 Pearson 相關(guān)系數(shù) [19] 計(jì)算 Nr 個(gè)滾動(dòng)序列匹配分?jǐn)?shù)，并將它們與閾值 Cthr 進(jìn)行比較。在這個(gè) Nr 窗口中，至少需要有 Nthr個(gè)系數(shù)大于 Cthr。第三，當(dāng)前位置的相關(guān)系數(shù)需要大于閾值。如果滿足所有三個(gè)要求，我們認(rèn)為它是一個(gè)真正的環(huán)路閉合，隨后按一定的方式更新粒子權(quán)重。

實(shí)驗(yàn)描述

為了證明所提出的Wheel-SLAM系統(tǒng)的可行性和有效性，我們?cè)谖錆h大學(xué)校園內(nèi)使用一輛汽車進(jìn)行了五組現(xiàn)場測試。該汽車裝有一個(gè)Wheel-IMU和參考系統(tǒng)，以提供車輛姿態(tài)的真實(shí)值，如圖5所示。測試中車輛運(yùn)動(dòng)的特性如表I所示。



兩個(gè)MEMS IMU和高端IMU的主要技術(shù)參數(shù)列在表II中，其中ARW表示角隨機(jī)游走；Acc.表示加速度計(jì)；VRW表示速度隨機(jī)游走。



圖6展示了五個(gè)實(shí)驗(yàn)軌跡。汽車開始移動(dòng)之前的靜態(tài)IMU數(shù)據(jù)用于獲得Wheel-IMU的初始橫滾和俯仰角以及陀螺儀偏差的初始值，其他慣性傳感器誤差設(shè)為零。



圖6. 實(shí)驗(yàn)軌跡。Seq. 1、Seq. 2 和 Seq. 3 是環(huán)形軌跡，車輛在一個(gè)方向上移動(dòng)了多次，而 Seq. 4 和 Seq. 5 是更為復(fù)雜的軌跡，在大規(guī)模環(huán)境中車輛不僅沿著同一車道同一方向行駛，還沿著同一條道路相反的方向行駛。

實(shí)驗(yàn)中設(shè)置的Wheel-SLAM的關(guān)鍵參數(shù)列在表III中，標(biāo)準(zhǔn)差在表III中表示為STD。



性能比較與分析

1）性能比較：圖7分別比較了五個(gè)實(shí)驗(yàn)中Wheel-SLAM和Wheel-INS的定位誤差和航向誤差。



圖7. 在所有五個(gè)實(shí)驗(yàn)中，Wheel-INS和Wheel-SLAM的估計(jì)軌跡及相應(yīng)的水平位置誤差和航向誤差的比較。

圖8展示了Wheel-SLAM在Seq. 1，Seq. 2和Seq. 4中構(gòu)建的地形圖，由于車輛輪胎直接接觸地面，所以制圖不受車輛懸架系統(tǒng)的影響，特別是當(dāng)車輛機(jī)動(dòng)性大時(shí)，這種影響尤為明顯，這也是在車體上安裝IMU時(shí)的情況。此外，這些地圖可以用于提供有價(jià)值的信息，以監(jiān)測道路的變形和劣化情況。



圖8. Wheel-SLAM在Seq. 1、Seq. 2和Seq. 4中估計(jì)的地形地圖。顏色表示道路傾角的值。道路傾角越大，顏色越淺。

2）Wheel-SLAM特性分析：為了進(jìn)一步評(píng)估Wheel-SLAM的性能和穩(wěn)定性，我們?cè)O(shè)置不同的粒子數(shù)來比較其定位性能。該算法每個(gè)配置運(yùn)行100次。圖9顯示了結(jié)果。



圖9. 在Seq. 1中，使用不同粒子數(shù)的Wheel-SLAM的定位RMSE。

討論

Wheel-SLAM的核心原則可以總結(jié)如下：

1）粒子擴(kuò)散以對(duì)機(jī)器人的可能狀態(tài)進(jìn)行采樣，并通過每個(gè)粒子維護(hù)的軌跡檢測環(huán)路閉合；

2）道路坡度角序列匹配結(jié)果用于更新粒子權(quán)重，以挑選出最可信的粒子。

在Wheel-SLAM中發(fā)揮核心作用的是滾動(dòng)序列匹配策略。它必須足夠穩(wěn)健，以保留突出的粒子同時(shí)過濾掉虛警。因此，我們采用了相當(dāng)嚴(yán)格的環(huán)路閉合檢測標(biāo)準(zhǔn)，使環(huán)路閉合檢測具有魯棒性。

然而，可以意識(shí)到，在應(yīng)用Wheel-SLAM時(shí)存在兩個(gè)主要限制。首先，機(jī)器人必須嚴(yán)格重新訪問以前的地方，并具有一定的長度，這不像基于視覺的SLAM，其中車輛通過使用外部感知傳感器（例如相機(jī)和LiDAR）具有遠(yuǎn)程感知能力，在Wheel-SLAM中，Wheel-IMU用于提取地形特征，這些特征只能通過機(jī)器人的精確到達(dá)來獲取，其次，環(huán)路閉合的成功取決于道路坡度角序列的匹配，如果機(jī)器人在極其平滑的道路上行駛，沒有任何坡度角的波動(dòng)，那么很難檢測到環(huán)路閉合。

總結(jié)

在本研究中，我們提出了一種利用單個(gè)輪式慣性測量單元（Wheel-IMU）執(zhí)行同時(shí)定位與地圖構(gòu)建（SLAM）的方法，通過利用Wheel-IMU的環(huán)境感知能力，具體而言，我們通過從機(jī)器人的橫滾角估計(jì)中提取地形特征來啟用環(huán)路閉合檢測，從而將我們之前的Wheel-INS方法擴(kuò)展為Wheel-SLAM。系統(tǒng)采用了Rao-Blackwellized粒子濾波器，每個(gè)粒子維護(hù)其自己的機(jī)器人狀態(tài)和柵格地圖。實(shí)驗(yàn)結(jié)果表明，所提出的方法能夠有效抑制Wheel-INS的誤差漂移，相對(duì)于Wheel-INS，定位和方向精度平均提高了52.6％和53.2％。

然而，Wheel-SLAM有兩個(gè)主要限制，首先，需要一定程度的道路沿角變化。其次，機(jī)器人必須準(zhǔn)確回到同一地點(diǎn)，Wheel-SLAM適用于那些在給定區(qū)域內(nèi)重復(fù)移動(dòng)的機(jī)器人，例如清掃機(jī)器人和在受限區(qū)域內(nèi)巡邏的機(jī)器人， 未來研究方向包括將Wheel-SLAM與其他外部感知傳感器（例如相機(jī)和LiDAR）集成，以提高機(jī)器人導(dǎo)航系統(tǒng)的穩(wěn)健性和適用性。

審核編輯：劉清