最新圖優(yōu)化框架,全面提升SLAM定位精度

0. 這篇文章干了啥？

同時(shí)定位與地圖構(gòu)建（SLAM）是一項(xiàng)關(guān)鍵技術(shù)，允許移動(dòng)機(jī)器人在部分或完全未知的環(huán)境中自主導(dǎo)航。它包括使用機(jī)載傳感器同時(shí)估計(jì)機(jī)器人狀態(tài)和構(gòu)建傳感器檢測(cè)到的環(huán)境地圖。SLAM可以根據(jù)傳感器和地圖構(gòu)建技術(shù)的類別進(jìn)行分類，如視覺SLAM、激光SLAM、慣性SLAM等。

解決SLAM問題的經(jīng)典方法可以分為基于濾波的方法和基于圖的方法。在1986年提出SLAM問題的前二十年里，基于概率公式的濾波方法已經(jīng)實(shí)現(xiàn)了準(zhǔn)確的估計(jì)。然而，在大規(guī)模問題中更新協(xié)方差矩陣在計(jì)算上是昂貴的。基于圖的方法最早由Lu和Milios在1997年引入，隨著圖的增長(zhǎng)，計(jì)算成本較低。隨著計(jì)算能力的提高，基于圖的SLAM的優(yōu)化算法相比經(jīng)典的濾波方法（如擴(kuò)展卡爾曼濾波、Rao-Blackwellized粒子濾波和信息濾波）獲得了廣泛關(guān)注。Wilbers等人展示了基于圖的方法在定位方面比粒子濾波具有更高的精度。

姿態(tài)圖優(yōu)化（PGO）可以建模為一個(gè)非凸優(yōu)化問題，是基于圖的SLAM的基礎(chǔ)，其中它將每個(gè)姿態(tài)與一個(gè)頂點(diǎn)關(guān)聯(lián)，將每個(gè)測(cè)量與圖的一條邊關(guān)聯(lián)，需要從有噪聲的相對(duì)測(cè)量中估計(jì)多個(gè)未知姿態(tài)。在三維空間中的姿態(tài)通常包括旋轉(zhuǎn)和平移，旋轉(zhuǎn)可以使用歐拉角、軸角（so(3)）、特殊正交群（SO(3)）或四元數(shù)（Q）表示，平移由一個(gè)三維向量t指定。此外，整體姿態(tài)還可以使用特殊歐氏群（SE(3)）、李代數(shù)（se(3）或雙四元數(shù)（DQ）表示。不同的建模方法會(huì)產(chǎn)生不同的約束，如在se(3)中沒有約束，在SE(3)中有矩陣正交和行列式約束，或在Q中有球面約束。選擇與問題結(jié)構(gòu)兼容的簡(jiǎn)單表示將導(dǎo)致一個(gè)更容易解決和更準(zhǔn)確的模型。

在過去的二十年中，許多模型已經(jīng)根據(jù)噪聲的不同統(tǒng)計(jì)分布和姿態(tài)表示方法得到了發(fā)展。同時(shí)，也提出了許多高效的優(yōu)化算法來解決這些模型。從模型的角度來看，旋轉(zhuǎn)噪聲的統(tǒng)計(jì)分布通常分為高斯分布或各向同性的馮米塞斯-費(fèi)舍爾（vMF）分布，而平移噪聲統(tǒng)一表現(xiàn)為高斯噪聲?；谧畲笏迫还烙?jì)，在se(3)上的高斯噪聲可以直接導(dǎo)出一個(gè)無約束的非線性最小二乘模型。同樣，Cheng等人建立了基于單位雙四元數(shù)的最小二乘模型，并提出了一種更有效的方法來計(jì)算雅可比矩陣。通過消除兩個(gè)變量，他們的模型也是無約束的。另一種建模方法使用SO(3)表示旋轉(zhuǎn)，假定其服從vMF分布，并導(dǎo)出具有正交和行列式約束的模型。由于se(3)需要轉(zhuǎn)換來描述運(yùn)動(dòng)過程，用SO(3)或Q和一個(gè)三維向量表示的目標(biāo)函數(shù)的表達(dá)式相比無約束模型更簡(jiǎn)潔；然而，約束的引入增加了挑戰(zhàn)。

從算法的角度來看，提出了幾種高效且準(zhǔn)確的方法來解決SLAM中的大規(guī)模問題。諸如隨機(jī)梯度下降等一階優(yōu)化方法可以減少梯度計(jì)算的復(fù)雜性，并有效地解決無約束優(yōu)化問題。收斂速度更快的算法，如高斯-牛頓方法、Levenberg--Marquardt方法、信賴域方法也被引入來解決該問題。與計(jì)算矩陣逆不同，使用QR或Cholesky分解等矩陣分解技術(shù)來降低復(fù)雜性，并提出了增量版本。Grisetti等人和Wagner等人提出了基于流形的高斯-牛頓算法，其中雅可比矩陣具有稀疏結(jié)構(gòu)，更新過程避免了大規(guī)模線性方程系統(tǒng)的昂貴存儲(chǔ)。

然而，二階算法僅在局部區(qū)域具有快速收斂率，對(duì)于非凸問題通常返回局部極小值。后來的工作集中于找到更好的初始點(diǎn)并確認(rèn)解的最優(yōu)性。Rosen等人提出了一種基于Powell的Dog-Leg信賴域方法的穩(wěn)健增量最小二乘估計(jì)，并提高了數(shù)值穩(wěn)定性。Carlone等人通過檢查對(duì)偶間隙推導(dǎo)了一個(gè)帶約束的二次規(guī)劃并驗(yàn)證了最優(yōu)解。通過擴(kuò)展可行集到其凸閉包，一種凸松弛方法有效地克服了非凸問題初始點(diǎn)選擇的難題。此外，Rosen等人將模型松弛為一個(gè)半定規(guī)劃，并證明了只要噪聲低于某個(gè)臨界閾值，其松弛的最小化結(jié)果提供了一個(gè)精確的最大似然估計(jì)。Fan和Murphey提出了PGO的一個(gè)上界，并通過廣義近端方法解決它，該方法可以收斂到一階臨界點(diǎn)且不依賴于黎曼梯度。另一種找到更好局部極小值或全局極小值的方法依賴于初始化技術(shù)。他們指出非凸旋轉(zhuǎn)估計(jì)是SLAM困難的真正原因，平移對(duì)旋轉(zhuǎn)估計(jì)影響較小。因此，計(jì)算一個(gè)好的旋轉(zhuǎn)估計(jì)將提高算法的性能。

我們提出了一種非凸姿態(tài)圖優(yōu)化的近端線性化黎曼交替方向乘子法（PieADMM），它使用最新的部分信息更新其他變量。我們的子問題不僅具有閉式解，而且可以并行計(jì)算，從而使每次更新的時(shí)間復(fù)雜性較低。這一優(yōu)勢(shì)在大規(guī)模數(shù)值實(shí)驗(yàn)中得到了驗(yàn)證。從理論上講，收斂性分析補(bǔ)充了我們的發(fā)現(xiàn)。

下面一起來閱讀一下這項(xiàng)工作~

1. 論文信息

標(biāo)題：Non-convex Pose Graph Optimization in SLAM via Proximal Linearized Riemannian ADMM

作者：Xin Chen, Chunfeng Cui, Deren Han, Liqun Qi

機(jī)構(gòu)：北京航空航天大學(xué)

原文鏈接：https://arxiv.org/abs/2404.18560

2. 摘要

位姿圖優(yōu)化 (PGO) 是解決基于位姿的同時(shí)定位與地圖構(gòu)建 (SLAM) 問題的一種著名技術(shù)。在本文中，我們使用單位四元數(shù)和三維向量表示旋轉(zhuǎn)和平移，并提出了一種基于馮·米塞斯-費(fèi)舍爾分布的新型 PGO 模型。從單位四元數(shù)導(dǎo)出的約束是球面流形，投影到這些約束上可以通過歸一化來計(jì)算。然后，我們開發(fā)了一種近端線性化黎曼交替方向乘子法 (PieADMM) 來解決所提出的模型，該方法不僅具有低內(nèi)存需求，而且可以并行更新位姿。此外，我們建立了 PieADMM 以 O(1/?2) 的迭代復(fù)雜度找到我們模型的 ?-駐點(diǎn)解。通過對(duì)兩個(gè)合成數(shù)據(jù)集和四個(gè) 3D SLAM 基準(zhǔn)數(shù)據(jù)集的數(shù)值實(shí)驗(yàn)，展示了我們所提出算法的效率。

3. 效果展示

首先，我們使用不同的算法測(cè)試n = 100，m = 100的環(huán)形數(shù)據(jù)集。圖2顯示了當(dāng)σr = 0.01，σt = 0.05，并采用弦初始化時(shí)的俯視軌跡，三種方法在視覺上收斂于相同的解。我們還測(cè)試了里程計(jì)猜測(cè)初始化技術(shù)。由于恢復(fù)的軌跡幾乎重疊，并且很難觀察出差異，我們將它們省略了。

相反，我們?cè)趫D3中報(bào)告了優(yōu)化過程，記錄了在不同方法和初始化技術(shù)下，Rel.Err和NRMSE的下降趨勢(shì)以及CPU時(shí)間。由于我們的PieADMM能夠針對(duì)每個(gè)頂點(diǎn)并行更新，因此它可以比其他方法更快地收斂。此外，弦初始化可以在旋轉(zhuǎn)更新后給出平移的估計(jì)，這提供了比其他方法更準(zhǔn)確的初始點(diǎn)。在此初始化下，我們的PieADMM可以收斂到具有較低相對(duì)誤差的解。與里程計(jì)猜測(cè)初始化相比，我們的PieADMM通常不如mG-N的前幾步準(zhǔn)確，但隨著迭代的進(jìn)行，它可以實(shí)現(xiàn)略微更好的性能。因此，我們將弦初始化作為下一步實(shí)驗(yàn)中的標(biāo)準(zhǔn)初始化技術(shù)。

4. 主要貢獻(xiàn)

(i) 我們提出了一種基于增強(qiáng)單位四元數(shù)和vMF分布的非凸姿態(tài)圖優(yōu)化模型，其中數(shù)據(jù)存儲(chǔ)成本低，單位四元數(shù)的投影可以通過歸一化計(jì)算。

(ii) 我們提出了一種PieADMM，其子問題具有閉式解，并且可以并行更新。

(iii) 基于流形上的一階最優(yōu)條件，我們定義了模型的一個(gè)?-駐點(diǎn)解。然后，我們建立了PieADMM在找到?-駐點(diǎn)解時(shí)的迭代復(fù)雜度O(1/?2)。

(iv) 我們?cè)趦蓚€(gè)不同數(shù)據(jù)規(guī)模的合成數(shù)據(jù)集和四個(gè)三維SLAM基準(zhǔn)數(shù)據(jù)集上測(cè)試了我們的算法。數(shù)值實(shí)驗(yàn)驗(yàn)證了我們方法的有效性。

5. 基本原理是啥？

6. 實(shí)驗(yàn)結(jié)果

我們?cè)陬~外的噪聲水平下比較這些算法，并在表II中列出了關(guān)于Rel.Err、NRMSE和CPU時(shí)間的數(shù)值結(jié)果。我們發(fā)現(xiàn)PieADMM花費(fèi)更少的時(shí)間并且獲得更好的結(jié)果。

我們還測(cè)試了姿態(tài)數(shù)量n的影響。實(shí)際上，由于我們限制了機(jī)器人軌跡的范圍，同等級(jí)別的噪聲將在頂點(diǎn)數(shù)量增加時(shí)產(chǎn)生更大的影響。因此，在比較不同n的數(shù)據(jù)大小的影響時(shí)，我們使用相對(duì)噪聲水平作為統(tǒng)一標(biāo)準(zhǔn)，這意味著σr = 100 × σrelr / n和σt = 100 × σrelt / n。結(jié)果如圖4所示。圖4a和4b顯示了PieADMM的性能平穩(wěn)，有時(shí)略優(yōu)于其他兩種方法。然而，PieADMM的運(yùn)行時(shí)間增加速度比它們慢得多，參見圖4c。這是因?yàn)閚的規(guī)模幾乎不影響旋轉(zhuǎn)子問題的成本，它可以并行計(jì)算。此外，平移子問題僅涉及矩陣乘法，并且不依賴于矩陣的逆。

對(duì)于立方體數(shù)據(jù)集，讓?duì)襱 = σrelt /?n，其中σrelt表示平移的相對(duì)噪聲水平。我們首先考慮了?n = 5或8，σr = 0.1，σrelt = 0.1和pcube = 0.3的兩個(gè)例子。圖5a和5d顯示了真實(shí)軌跡，其中藍(lán)線由運(yùn)動(dòng)產(chǎn)生，紅色虛線由觀測(cè)產(chǎn)生。圖5b、5c和5e、5f分別是對(duì)應(yīng)于不同?n的嘈雜和恢復(fù)的軌跡。圖6顯示了Rel.Err隨著CPU時(shí)間的下降趨勢(shì)，其中我們省略了圖像的上半部分以突出顯示細(xì)節(jié)。由于PGO模型是非凸的，而PieADMM是非單調(diào)的算法，曲線可能會(huì)振蕩。然而，它總是在更短的時(shí)間內(nèi)收斂到更高精度的解。

我們還從2到10選擇?n，并在表III中展示了數(shù)值結(jié)果。圖7a顯示了立方體數(shù)據(jù)集的邊緣和頂點(diǎn)數(shù)量之間的關(guān)系，圖7b和7c說明了速度隨著?n的增加而上升的趨勢(shì)。mG-N和mL-M的成本增長(zhǎng)都是立方的，而PieADMM的增長(zhǎng)速度較慢。

我們測(cè)試了一些流行的3D SLAM數(shù)據(jù)集。車庫數(shù)據(jù)集是一個(gè)大規(guī)模的真實(shí)世界示例，另外三個(gè)（球1、球2和環(huán)面）是用來比較性能的常見數(shù)據(jù)集。與球1數(shù)據(jù)集不同，球2數(shù)據(jù)集添加了更大的噪聲。我們還使用弦初始化技術(shù)為所有方法計(jì)算了一個(gè)初始點(diǎn)。圖8顯示了軌跡的視覺結(jié)果，相應(yīng)的數(shù)值結(jié)果列在表IV中。值得注意的是，我們的旋轉(zhuǎn)模型是基于vMF分布而不是傳統(tǒng)的高斯分布，因此恢復(fù)的解不相同，并且比較目標(biāo)函數(shù)值或梯度是沒有意義的。我們?cè)诒碇酗@示了CPU時(shí)間，表明PieADMM收斂速度比mG-N和mL-M快。

7. 總結(jié)

在SLAM中的位姿圖優(yōu)化是一種特殊的非凸優(yōu)化，其中變量通常位于se(3)中，具有非線性目標(biāo)函數(shù)，或在具有正交約束的特殊歐幾里得群中。復(fù)雜的模型使得找到全局解變得困難。本文提出了一種基于增強(qiáng)單位四元數(shù)和馮米塞斯-費(fèi)舍爾分布的新非凸位姿圖優(yōu)化模型，這是一個(gè)在單位球面上的大規(guī)模四次多項(xiàng)式優(yōu)化。通過引入輔助變量，我們將其重新表述為多二次多項(xiàng)式優(yōu)化、多線性最小二乘問題。然后，我們引入了一個(gè)針對(duì)PGO模型的近端線性化黎曼ADMM，其中子問題是簡(jiǎn)單的投影問題，并且可以根據(jù)有向圖的結(jié)構(gòu)并行解決，從而大大提高了效率。然后，基于我們PGO模型滿足的Lipschitz梯度連續(xù)性假設(shè)和流形上的一階最優(yōu)性條件，我們建立了找到ε駐點(diǎn)解的迭代復(fù)雜度為O(1/?2)。在兩個(gè)具有不同數(shù)據(jù)規(guī)模和噪聲水平的合成數(shù)據(jù)集以及四個(gè)3D SLAM基準(zhǔn)數(shù)據(jù)集上的數(shù)值實(shí)驗(yàn)驗(yàn)證了我們方法的有效性。