摘要

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

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

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

主要貢獻

提出并實現了一種Wheel-IMU 使用地形特征（由 Wheel-IMU 測量）的 SLAM 系統。

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

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

相關工作

背景

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



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

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



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

動態貝葉斯網絡

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



網格地形圖的構建

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



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

粒子權重更新

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

實驗描述

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



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



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



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

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



性能比較與分析

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



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

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



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

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



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

討論

Wheel-SLAM的核心原則可以總結如下：

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

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

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

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

總結

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

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

審核編輯：劉清