4D多普勒雷達和激光雷達里程計傳感器的研發(fā)越來越流行，這些傳感器生成的三維點云包含有關(guān)每個點相對于傳感器的徑向速度的信息,。特別是4D雷達在低能見度條件（如灰塵,、煙霧）下進行目標(biāo)感知和導(dǎo)航變得越來越有趣，而在這些條件下激光雷達和攝像頭通常會失效,。隨著高分辨率多普勒雷達的出現(xiàn),，有望通過單幀點云估計里程計，從而避免在特征稀疏的場地環(huán)境中易出錯的掃描配準(zhǔn)過程,。

我們比較了幾種里程計估計方法,，從多普勒/IMU數(shù)據(jù)的直接積分和卡爾曼濾波傳感器融合到三維點云間的掃描幀對掃描幀和掃描幀對地圖的配準(zhǔn)。使用兩個最新的4D雷達和兩個IMU的三個數(shù)據(jù)集進行了實驗,。令人驚訝的是,，我們的結(jié)果顯示僅使用多普勒和IMU數(shù)據(jù)得到的里程計結(jié)果與3D點云配準(zhǔn)相比，要么相似,，要么更好。在實驗中,，平均位置誤差可以低至1.8和4.5公里行程中的0.3%,。這使得在特征稀疏的礦山環(huán)境等低能見度條件下,，也能夠準(zhǔn)確估計6自由度的自身運動。這些結(jié)果對于在特征稀疏和低能見度條件下的資源受限機器人平臺導(dǎo)航的應(yīng)用非常有用,，例如在采礦,、建筑和搜救行動中。

近年來,，出現(xiàn)了幾種毫米波雷達里程計和同時定位與地圖構(gòu)建（SLAM）的方法,。受到在地下采礦環(huán)境中開發(fā)SLAM系統(tǒng)的問題的啟發(fā)，我們比較了幾種代表性的毫米波雷達里程計估計方法,。為此將它們部署到包含兩種不同的現(xiàn)代成像雷達的三個數(shù)據(jù)集中,。兩個數(shù)據(jù)集是使用我們的移動傳感器裝置記錄的：一個在地下礦山（圖1）中，另一個在用于大型輪式裝載機的室外測試場地（圖3和圖4）中,。第三個數(shù)據(jù)集由Zhang等人發(fā)布,，代表了一個結(jié)構(gòu)化的城市環(huán)境。令人驚訝的是,，使用直接融合基于多普勒的雷達自身速度和由慣性測量單元（IMU）提供的方向的最簡單方法,，我們發(fā)現(xiàn)這個實驗結(jié)果對于設(shè)計上述應(yīng)用的定位和地圖構(gòu)建系統(tǒng)是有用的，并值得在機器人學(xué)界進一步研究,。此外將我們的數(shù)據(jù)集公開發(fā)布在https://github.com/kubelvla/mine-and-forest-radar-dataset,。

圖1：Kvarntorp礦山環(huán)境的細節(jié)，由兩種傳感器模式（激光雷達和雷達）捕獲,。毫米波雷達模式受到有限的視場（FOV）,，較低的分辨率和較少的返回的影響。然而在采礦中預(yù)期的低能見度條件下,，毫米波雷達更為適用,。

在眾多毫米波雷達里程計方法中，我們選擇了一組代表性的方法,，這些方法是開源的,、適用于我們的傳感器，并覆蓋了從簡單的傳感器融合到先進的掃描匹配的全方位內(nèi)容,。

多普勒速度和IMU

在這項工作中測試的最簡單的姿態(tài)估計方法利用了由IMU提供的方向和由多普勒雷達傳感器測量的自我速度,。首先根據(jù)IMU的方向?qū)⒆晕宜俣葟囊苿悠脚_的坐標(biāo)框架轉(zhuǎn)換為基于IMU方向的世界坐標(biāo)框架。然后假設(shè)在連續(xù)雷達掃描之間速度保持不變,，對其進行數(shù)值積分,。這樣就在世界坐標(biāo)框架中生成了一個軌跡。我們將這種方法稱為IMU+Doppler,，由于毫米波雷達沒有直接提供自身速度測量,，而是提供其檢測到的目標(biāo)速度的徑向分量，因此有必要對這些信息進行魯棒處理，以估計雷達的自身速度,。為此,，我們采用了Doer和Trommer的方法和代碼。他們的3-點RANSAC-LSQ自我運動估計方法將RANSAC應(yīng)用于底層最小二乘優(yōu)化問題,。這個算法非常高效,，在我們的數(shù)據(jù)集中，每個雷達掃描幀的平均處理時間為10毫秒,。

擴展卡爾曼濾波器融合

與直接多普勒+IMU融合相比,，使用EKF可以更加原則地處理傳感器測量中的噪聲，并提供姿態(tài)置信度估計,，在使用Doer和Trommer的代碼,，該實現(xiàn)將他們的3-點RANSAC-LSQ自身運動估計與慣性和氣壓測量結(jié)合在一起。傳感器測量以EKF的寬松耦合方式進行融合,。有幾個算法擴展可用,。我們選擇原始的ekf-rio版本，因為它不需要精確的雷達觸發(fā)信號,，不幸的是我們無法從雷達中獲取該信號,。在這種情況下，該算法以大約100毫秒的延遲應(yīng)用傳入的自身運動測量,，這可能會影響狀態(tài)估計的質(zhì)量,，特別是在高度動態(tài)運動過程中。

此外由于我們的傳感器裝置缺乏氣壓計測量,，我們省略了氣壓計測量,。因此在這里獲得的結(jié)果代表了濾波方法可實現(xiàn)的里程計質(zhì)量的下限。值得注意的是,，Michalczyk等人的研究通過使用緊耦合的EKF濾波進行雷達慣性里程計,，能夠?qū)崿F(xiàn)低于1%的局部漂移。緊耦合算法如何處理成千上萬個目標(biāo)的雷達掃描幀仍然是一個有趣的問題,。

圖4：從上方視圖看黃色輪式裝載機行駛在森林道路上,。從其傳感器套件中，顯示了激光雷達,、雷達和前置攝像頭,。深黑色的點表示活動激光掃描，與有色的前置雷達掃描形成對比,。

點到面迭代最近點法與局部地圖

被測試的毫米波雷達具有很高的分辨率,，這使我們能夠嘗試最初為激光雷達點云配準(zhǔn)而開發(fā)的方法。這里選擇了norlab-icp-mappe這個開源且高度可配置的工具,，它支持多種迭代最近點（ICP）算法變體,。我們特別選擇了點到面的變體方法,，因為它通常在結(jié)構(gòu)化和半結(jié)構(gòu)化環(huán)境中表現(xiàn)良好。該建圖方案不支持通過閉環(huán)識別進行地圖優(yōu)化,，而是構(gòu)建一個整體地圖,，因此在行為上類似于激光雷達里程計方法,。

在實驗中,，我們配置建圖方法以向地圖中添加新點，直到達到由最小點之間的最小距離定義的最大密度,，該最小距離在我們的實驗中為0.1米,。點到面ICP還需要基于地圖中每個點周圍的局部幾何形狀估計法線向量。在我們的實驗中,，使用了15個最近的點,。值得注意的是，初步測試表明,，當(dāng)該建圖方案在雷達數(shù)據(jù)上進行部署時,，需要先前的運動估計。因此在所有實驗中將多普勒+IMU的姿態(tài)作為先驗提供,。建圖方案中的ICP算法提供完整的6自由度（DOF）姿態(tài)估計,，或者是受限的4-DOF姿態(tài)估計。在4-DOF變體中,，點云配準(zhǔn)僅優(yōu)化位置和航向,，另外兩個DOF則直接采用建圖方法從IMU提供的方向中獲取，在這項工作中,，我們測試了這兩個變體,，并將它們分別稱為ICP和ICP 4DOF。

圖5：汽車停車場環(huán)境的組合雷達地圖（左側(cè)）,，由黑色點表示,，帶有實時雷達掃描（紅色點）。傳感器裝置（右下角）是手動沿軌跡推動的,。

掃描幀匹配變體方法

在這項工作中測試的雷達里程計變體的最后一組采用了掃描對掃描匹配,，這經(jīng)常用于較大SLAM框架的前端模塊中。Zhang等人成功地在現(xiàn)代成像雷達（Oculii Eagle）的SLAM框架中應(yīng)用了這種方法,。由于他們的SLAM框架實現(xiàn)是開源的,，在這里將其包含在內(nèi)以測試我們的雷達數(shù)據(jù)集的雷達里程計。此外提供了他們數(shù)據(jù)集能夠使用Oculii Eagle雷達測試所有其他方法,。他們的雷達里程計前端是高度可配置的,，允許用戶從幾種其他掃描匹配算法中進行選擇，我們選擇測試它們的自適應(yīng)概率分布-GICP（APDGICP）變體的GICP,。雖然該掃描幀匹配方法可以在沒有先前運動估計的情況下工作,，但我們修改了代碼以包含使用多普勒+IMU測程先驗的選項,，以使其與與子地圖匹配的變體進行公平比較。在提供先驗時,，我們將該方法稱為APDGICP IMU Prior,，否則稱為APDGICP。我們還選擇測試他們的正態(tài)分布變換（NDT）掃描匹配算法,，因為它經(jīng)常用于激光雷達里程計解決方案,。

環(huán)境和傳感器設(shè)置

受到在惡劣環(huán)境條件下進行SLAM研究的啟發(fā)，記錄了兩個現(xiàn)場數(shù)據(jù)集：一個在瑞典厄勒布魯附近的Kvarntorp研究礦井中,，另一個在瑞典埃斯基爾斯圖納的沃爾沃建筑設(shè)備輪式裝載機和自卸車戶外測試場地,。Kvarntorp測試礦提供了一個用于地下采礦行業(yè)應(yīng)用的模型環(huán)境。使用固定在皮卡車車頂上的傳感器裝置記錄了一次長達4500米的運行,，如圖2所示,。

圖2：穿越礦山的皮卡車，車頂安裝有多傳感器裝置（上圖）,，傳感器裝置的詳細信息（下圖）

Eskilstuna戶外測試場地由沃爾沃CE用于其產(chǎn)品的開發(fā)和測試,，包括如圖3所示的大型輪式裝載機。

圖3：森林環(huán)境的俯視圖（左）和裝備有傳感器裝置的沃爾沃CE輪式裝載機（右）

里程計性能評估

APE與軌跡圖一起提供了有關(guān)給定感知和環(huán)境組合的里程計變體行為的初始,、一般性的想法,，但易受到累積的姿態(tài)誤差的乘性、非線性影響,。RPE對這一指標(biāo)進行了補充,，指示誤差累積的速率。

圖6：使用Sensrad Hugin雷達記錄的礦井環(huán)境,。為了清晰起見,，省略了4DOF ICP在這個圖中的顯示，與紅色顯示的標(biāo)準(zhǔn)ICP相比,，其垂直漂移會受到限制,。同樣出于其快速發(fā)散的原因，未顯示掃描幀對掃描幀匹配的里程計,。

圖6和圖7展示了在礦井實驗中討論的雷達里程計法的性能,。掃描幀到掃描幀匹配的APDGICP變體以及NDT不適用于Hugin雷達提供的輸出類型。

圖7：在礦井中討論的所有里程計變體方法的APE平移分量,。

圖8：在森林場景中討論的所有里程計變體的APE平移分量,。

圖9顯示，依賴多普勒速度（IMU+多普勒和EKF）的簡單方法在垂直和航向漂移方面存在問題,。我們認為這主要是由于在記錄數(shù)據(jù)集時使用的IMU類型,。事實上，ICP,、APDGICP和NDT的所有變體方法的性能都相似,，并在圖10中保持在10米內(nèi),。

圖 9: 使用Oculii Eagle雷達記錄的Car Park實驗。為了更好的清晰度,，僅顯示了選定的里程計變體,。

圖 10: Car Park環(huán)境中所有討論的雷達里程計變體的APE值。

使用RPE指標(biāo)在圖11中總結(jié)了兩個不同雷達的里程計方法的性能,。Car Park實驗表明,，依賴多普勒速度的方法（IMU+多普勒和EKF）在平移上更糟，在旋轉(zhuǎn)誤差中,，我們看到掃描幀匹配的限制效果,，這阻止了較大誤差的累積,，與IMU+多普勒和EKF相反,。

圖 11: 兩個不同傳感器設(shè)置的RPE值。每對小圖表示用于評估RPE的步長,，分別為1m和10m,。每個圖中直接顯示了中位數(shù)RPE值。

在這項工作中,，我們比較了在地下和室外環(huán)境中使用兩種不同的現(xiàn)代成像毫米波雷達記錄的三個數(shù)據(jù)集上的幾種雷達里程計估計方法,。在Oculii Eagle雷達中，掃描幀匹配方法的精度高于濾波方法,。另一方面得益于Sensrad Hugin雷達中高度準(zhǔn)確的多普勒速度測量,，最簡單的傳感器融合方法IMU+多普勒在礦山和森林實驗中僅達到0.3%的位置漂移。這使得該方法適用于在惡劣環(huán)境中運行的資源受限機器,，例如礦業(yè)中的重型機械,。在未來的工作中，我們將調(diào)查Eagle雷達中多普勒速度不準(zhǔn)確的原因,，并將雷達里程計擴展為完整的SLAM解決方案,。

參考代碼

1，https://github.com/christopherdoer/reve 

2,，https://github.com/christopherdoer/rio

3,，https://github.com/norlab-ulaval/norlab_icp_ mapper 

4，https://github.com/zhuge2333/4DRadarSLAM