摘要

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

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

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

主要貢獻

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

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

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

相關工作

背景

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



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

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



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

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

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



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

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



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

粒子權(quán)重更新

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

實驗描述

為了證明所提出的Wheel-SLAM系統(tǒng)的可行性和有效性，我們在武漢大學校園內(nèi)使用一輛汽車進行了五組現(xiàn)場測試。該汽車裝有一個Wheel-IMU和參考系統(tǒng)，以提供車輛姿態(tài)的真實值，如圖5所示。測試中車輛運動的特性如表I所示。



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



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



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

實驗中設置的Wheel-SLAM的關鍵參數(shù)列在表III中，標準差在表III中表示為STD。



性能比較與分析

1）性能比較：圖7分別比較了五個實驗中Wheel-SLAM和Wheel-INS的定位誤差和航向誤差。



圖7. 在所有五個實驗中，Wheel-INS和Wheel-SLAM的估計軌跡及相應的水平位置誤差和航向誤差的比較。

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



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

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



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

討論

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

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

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

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

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

總結(jié)

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

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

審核編輯：劉清