基于回環邊殘差聚焦權重模型的位姿圖優化算法

摘 要：基于圖優化的同時定位與建圖（SLAM）系統中含有大噪聲的回環邊，可能嚴重阻礙優化器迅速收斂到最優解，顯著降低定位精確性和地圖一致性。因此，針對大噪聲回環邊的優化算法的魯棒性至關重要。引入K-means聚類思想，對回環邊殘差值進行分類，進而建立了一種新的殘差閾值模型，自適應調整回環邊在優化時的權重，減少回環邊對優化的影響；然后，基于迭代重加權最小二乘的思想形成了RW-RLSPGO 算法（residual weighted enhancement for recursive least squares pose graph optimization algorithm，RW-RLSPGO）；最后，在模擬和真實的PGO數據集上進行蒙特卡羅實驗。實驗結果表明，RW-RLSPGO算法在準確性和魯棒性方面都取得了顯著的提高，驗證了其在大噪聲環境下的有效性。

關鍵詞：同時定位與建圖；位姿圖優化；回環邊；大噪聲；聚類

中圖分類號：TP391"" 文獻標志碼：A

文章編號：1001-3695（2025）01-021-0149-07

doi： 10.19734/j.issn.1001-3695.2024.04.0170

Pose graph optimization algorithm based on loop-closureedges residual focusing weight model

Abstract：In graph-based SLAM systems， loop-closure edges with large noise may severely impede the optimizer from rapidly converging to the optimal solution， leading to a noticeable decrease in localization accuracy and map consistency. Therefore， the objective of this paper was to investigate robust methods for handling loop-closure edges， which was crucial for optimization algorithms in the presence of large noise. Toward this aim， this paper introduced a new concept of K-means clustering to classify the residual values of loop-closure edges， thereby established a new residual threshold model. This model adaptively adjusted the weights of loop-closure edges during optimization to reduce their impact on the optimization process. Subsequently， the formulation of the residual weighted enhancement for recursive least squares pose graph optimization algorithm （RW-RLSPGO） was based on the iterative reweighted least squares principle. Finally， it conducted Monte Carlo experiments on both simulated and real pose graph optimization （PGO） datasets. The experimental results demonstrate a significant improvement in both accuracy and robustness with the RW-RLSPGO algorithm， validating its effectiveness in high-noise environments.

Key words：simultaneous localization and mapping（SLAM）; pose graph optimization; loop-closure edge; large noise; clustering

0 引言

SLAM，即在未知的環境下利用傳感器獲取環境信息并估計自身位姿[1]，在機器人、自動駕駛、三維重建等領域得到了廣泛的研究和發展[2]，成為了眾多平臺實現自主導航與定位的關鍵。

當前，SLAM系統框架主要分為前端里程計和后端非線性優化。前端里程計主要任務是從各種傳感器的測量數據中估計相鄰幀間的幾何信息。后端接收到前端不同時刻的位姿以及回環檢測的信息，通過最大后驗概率模型進行估計。前端傳向后端的信息包括節點、邊的測量信息以及表示邊不確定性的協方差矩陣。其中，邊分為由節點順序連接的里程計邊，以及由位置識別算法識別出來的處于同一位置的節點連接起來的回環邊[3]兩種。位姿圖優化（pose graph optimization， PGO）是后端非線性優化的重要方法之一，它的目標是最小化估計值與觀測值之間的誤差，求解不同時刻的位姿。在求解目標函數時，通常采用高斯-牛頓、列文伯格-馬夸爾特以及信任域方法等局部優化技術[4， 5]。然而，由于傳感器的測量誤差會使回環測量產生較大的噪聲，嚴重影響位姿估計的精度。大噪聲產生的原因有很多，主要包括傳感器誤差和錯誤匹配。一方面，由于相機、IMU等傳感器精度的限制以及環境對傳感器的性能影響，導致測量數據與實際數據差距較大[6]，尤其在回環邊的檢測中，傳感器的測量數據會受到時間上的不一致性和環境視角光暗等因素的干擾；另一方面，場景中的動態物體可能會導致特征點的錯誤匹配，誤將動態物體的特征點匹配到靜態物體上[7]，使得經典的特征提取與匹配算法的魯棒性、精確性不高，從而造成回環時出現錯誤匹配，加大了回環數據的噪聲（本文將此類數據稱為大噪聲數據集）。

針對傳感器誤差和錯誤匹配引起的大噪聲數據集，SLAM后端通常采用良好的初始估計，這有助于加速迭代優化的收斂，同時降低陷入局部最小值的風險。一般情況下，初始估計采用如里程計測量[8]和最小生成樹搜索[9]等啟發式初始化算法得到。它們擁有輕量的計算成本，但是提供的初始估計值效果較差，容易造成迭代優化陷入局部最小，因而無法獲得高精度的位姿，如圖1所示，里程計測量odometry和最小生成樹搜索spanning tree提供的初始值比較差。隨后，也有一些復雜的初始化算法，如柯西算法[10]、MASAT[11]等。這些算法在低噪聲的情況下可以提供較好的初始估計值，但是應對大噪聲卻無法達到理想效果。為解決這一問題，文獻[12]引入權重來降低大噪聲對優化結果的干擾。可信的觀測將被賦予更高的權重，以確保其對優化的影響顯著；反之，含大噪聲的觀測將被賦予較小的權重，以此來削弱它們對優化的影響。通過這樣的權重分配策略能使得優化更具有魯棒性和穩定性，從而得到更加精確的優化結果。

本文受文獻[12]的啟發，針對含有大噪聲的PGO數據集，提出一種新的聚類權重模型來減輕大噪聲對位姿估計的影響，降低迭代次數，在RS算法基礎上，形成了RW-RLSPGO算法。該算法可以作為大噪聲的PGO初始化方法，為PGO迭代求解器提供一個良好的初始值。

具體而言，本算法的貢獻主要體現在以下方面：

a）該算法受到K-means聚類思想的啟發，基于簇內誤差平方和的手肘法，隨著簇數k的變化情況，求取簇內誤差平方和的二階導的最大值，將其對應的簇數作為聚類的k值，對回環邊的殘差值聚類。

b）基于聚類中心和回環邊的聚類結果，構建了一種新的殘差聚焦權重模型。實驗證明，該模型通過對目標函數加權的方式，有效降低了大噪聲對位姿估計的影響。

c）本文算法在含有大噪聲的PGO公開數據集上進行蒙特卡羅實驗。實驗結果表明，RW-RLSPGO相較于RS在大噪聲的情況下，更加精確和魯棒。

1 PGO問題及相關工作

在SLAM理論算法的發展過程中，針對后端的位姿圖優化問題，涌現出了各種不同的解決方法。Lu等人[13]首次提出以所有幀間的空間關系為約束來全局優化各幀位姿，揭開了以非線性方法優化機器人位姿的序幕；Thrun等人[14]基于文獻[13]提出GraphSLAM，將后端優化問題轉換為圖形網絡，構建出以不同時刻的機器人位姿為節點、不同位置觀測產生的約束為邊的圖模型，再從全局角度對圖模型優化；類似地，Folkesson等人[15]在Graphical SLAM中使用了兩節點之間的關系來減少后端優化問題中的變量數量，降低了計算的復雜度。圖優化的研究為位姿圖優化的興起和發展提供了堅實的研究基礎。

具體來說，假設后端優化中給定一個有向圖G（V，E），V=v1，v2，v3，…，vn表示頂點集，E=e1，e2，e3，…，em表示空間中頂點間的m條相對測量邊，PGO問題是計算出每個位姿的全局位置和旋轉方向，以最小化相對測量的平移和旋轉誤差作為目標函數，即

其中：Rnoise表示相對旋轉方向測量的噪聲；dnoise表示相對位置測量的噪聲。

早期的研究中，Duckett 等人[16]使用高斯塞爾德松弛方法求解最小化殘差。Howard等人[17]將松弛方法與位姿圖的彈簧動力學相結合，不僅提高了位姿圖估計的準確性，同時也加快了收斂的速度，更快地求解全局最小值。之后的研究中松弛方法在位姿圖中的應用更加廣泛。Frese等人[18]將多級松弛算法應用到SLAM中;Carlone等人[19， 20]將拉格朗日對偶算法引入SLAM中。盡管松弛方法在位姿圖的優化中表現出較好的魯棒性和收斂速度，但是面對大規模的真實場景，計算的復雜度和資源需求限制了它在實際應用中的可行性[21]。因此研究者們開始尋找其他方法，提出了高斯-牛頓法[22]、列文伯格-馬夸爾特法[23]、信任域方法[24]、隨機梯度下降及其變體[25， 26]、平滑方法[27]、分層方法[28， 29]等多種迭代方法。

然而，Nasiri等人[30]表明，迭代算法受到初始值的影響，容易收斂到局部最小值，一個好的初始值不僅可以避免收斂到局部最小值，而且可以加快在最優解附近的收斂速度。文獻[31～33]提出的初始化算法認為PGO位姿圖優化的目標函數中旋轉方向測量相關部分與位置測量無關，可以獨立求解。這個方法可以引導迭代方法的良好初始化。進而，Carlone等人[34]在對幾種三維初始化算法的比較中得出結論，Chordal初始化算法在三維位姿圖優化中表現出優異的效果。然而，之前的位姿圖初始化算法適用于小噪聲，無法解決含有大噪聲的問題。Nasiri等人[30]結合之前學者的研究，提出了一種RS的初始化算法，即假設旋轉測量方向保持不變，當選擇一個初始的旋轉方向測量，可利用含有位置測量的最小二乘函數項求解出位置，并證明了RS算法在無噪聲和帶噪聲的情況下具有較好的收斂性，與Chordal初始化算法比較，RS可以提供更好的初始值。

2 本文算法

本文受到RS算法[30]的啟發，接下來將介紹RS算法。

2.1 RS算法

RS本質上將求解式（1）轉變成求解兩個線性最小二乘問題。具體而言，分別為最小化相對測量位置誤差項和最小化相對測量旋轉誤差項。在求解旋轉的最優解時，建立如下模型：

引理1 假設矩陣An×n為任意方陣，那么

寫為更加緊湊的形式，即

其中：A表示圖的關聯矩陣[29]。不難發現式（9）是一個最小二乘的形式，有封閉解，即

那么，可以根據（10）的封閉解求得旋轉方向R。

同理可知，最小化相對測量位置誤差項也是一個線性最小二乘問題，并且具有封閉解：

P*=ATΛA－1ATΛD（11）

其中：Pn×3=p1，…，pnT是位置矩陣，Am×n是關聯矩陣；Λm×m=diag（［λ1，…，λm］）是權重的對角矩陣；Dm×3是所有邊的相對測量位置的測量值的矩陣參考坐標系，其第k行是dTijRi。通過式（11），代入求得的旋轉方向R，最終求得頂點的位置P。

2.2 RW-RLSPGO算法

正因為噪聲的存在，每個回環邊的誤差受到了顯著影響。為了應對這一挑戰，本文采用了一種新穎的方法，通過動態調整每條邊殘差的權重來提高解決方案的自適應性。這個新的權重求解策略有效地提升了系統對于誤差的容忍度，使得在噪聲存在的情況下，仍然能夠更準確地處理每條邊的信息，從而提高整體的定位和回環檢測的精度。

2.2.1殘差聚類

為了實現回環邊的聚類，定義Erij為每條回環邊對應的誤差，即

由于手肘法應用廣泛，所以被選擇用作求解簇數。具體而言，通過計算每個簇數對應的簇內平方和，即誤差平方和（sum of squares due to error，SSE）

再對簇內平方和進行兩次微分，則最佳的簇數k為簇內平方和兩次微分最大值對應的簇數。

其中：k為聚類的簇數;Ci為對應簇的質心；x為對應簇內的回環邊殘差值；S′為簇內平方和的一次微分；S″為簇內平方和的二次微分。從數據集中隨機選擇k個回環邊殘差作為不同簇的聚類中心，對回環邊中的每個樣本，計算其到k個質心點的距離，并選擇距離最近質心點的類別作為回環邊所屬的簇。接著根據上一步重新指定的簇，計算每個簇的新的簇內質心點。最后，為每個回環邊重新分配距離其最近質心點的簇。

2.2.2 殘差聚焦權重模型

當聚類結束后，構建出了一種魯棒的殘差權重模型，即

通過殘差權重模型構建的目標函數，大大降低了含大噪聲的回環邊對模型優化產生的影響。

RW-RLSPGO通過對回環邊殘差聚類以及利用殘差聚焦權重模型求出每條回環邊權重，構建新的PGO目標函數，分別求解目標函數的最小化相對測量位置誤差項和最小化相對測量旋轉誤差項，得到最終估計的位姿，具體實現步驟如下所示。

算法 RW-RLSPGO

3 實驗結果與分析

本章將采用公共的PGO數據集對本文算法進行實驗分析，并驗證算法性能。首先，在公共數據集中加入多級噪聲，測試RW-RLSPGO、Odomentry、Spinning Tree、MASAT和RS幾個主流初始化算法的總損失函數值，比較這五個算法面對含噪聲數據集的性能；接著，進一步比較RS和RW-RLSPGO處理大噪聲數據集的具體情況，經過蒙特卡羅實驗，驗證了RW-RLSPGO在大噪聲的數據集下有著較強的魯棒性。本實驗主要是與RS算法進行對比實驗，兩者都為高魯棒性的初始化算法；最后，將大噪聲數據集分別經RW-RLSPGO和Odomentry、Spinning Tree、MASAT、RS算法得到的初始估計值加入SE-sync位姿圖迭代優化算法中，得到優化后軌跡圖，通過比較軌跡誤差，驗證RW-RLSPGO可以提供更佳的初始估計值。所有實驗均在AMD Ryzen 7 5800H with Radeon Graphics 3.2 GHz的CPU、16 GB內存的聯想拯救者R9000P筆記本電腦上進行，編程語言為MATLAB。

3.1 數據集

本文實驗中，PGO數據集參考文獻[34]的3DSLAM初始化技術的數據集，包括sphere-a、torus、cube、garage、cubicle和rim。數據集均由相機傳感器采集而來，包含了相機位姿以及完整的里程計和回環觀測數據，相機位姿采用ID編號、平移向量元素（tx、ty、tz）和旋轉的單位四元數（qx、qy、qz、qw）構成；里程計邊和回環邊觀測數據以兩個相機位姿ID、平移向量元素（tx、ty、tz）、旋轉的單位四元數（qx、qy、qz、qw）以及信息矩陣的右上角構成。公共PGO數據集中，sphere-a是g2o[4]中提供的一個球體數據集，torus和cube分別為環面和立方體數據集，以上三個數據集均為模擬數據集；garage為openslam中的vertigo提供的斯坦福停車場的3D地圖真實數據集;cubicle和rim為佐治亞理工學院RIM中心收集的真實數據集。這三個數據集為SLAM前端提供的位姿數據信息，僅含有少量噪聲，不包含異常值。

實驗中使用的公共3DSLAM數據集包含基本參數如表1所示，表中數據涵蓋了6個數據集的相對旋轉噪聲的均值mean、標準差STD、位姿節點數量N、相對測量邊的數量M以及回環邊的數量H（單位：個）。

為了測試RW-RLSPGO和Odomentry[8]、Spinning Tree、MASAT[11]、RS[30]算法面對大噪聲的優化效果，實驗分別在上述數據集的邊測量中添加均值μnoise=0，標準差σnoise分別為0.1 rad、0.2 rad、0.3 rad、0.4 rad、0.5 rad和1 rad的噪聲，通過比較算法之間的總損失函數值cost、旋轉損失函數值rcost、平移損失函數值tcost、算法運行時間（times，單位：s）以及迭代次數（Itr，單位：次）等指標情況，分析大噪聲情況下RW-RLSPGO、Odomentry、Spinning Tree、MASAT、RS五種初始化算法的魯棒性。

3.2 實驗結果

本文通過文獻[35]使用的添加噪聲的方式，向上述6個PGO數據集添加不同級別的噪聲。為了減輕噪聲添加的隨機性對實驗的影響，本文采用蒙特卡羅實驗，重復50次隨機產生噪聲取平均值。實驗記錄每種算法針對不同數據集的實驗結果，包括總損失函數值、旋轉損失函數值、平移損失函數值、算法運行時間以及迭代次數，分析RW-RLSPGO和其余四種算法在大噪聲情況下的魯棒性。圖2展示了Odomentry、Spinning Tree、MASAT、RS和RW-RLSPGO算法在不同級別噪聲下的公共PGO數據集上總損失函數值情況。

RW-RLSPGO中提出的回環邊殘差聚焦模型，顯著降低了大噪聲對位姿數據的干擾。具體來說，與其他四種初始化算法相比，RW-RLSPGO在處理噪聲數據方面表現出更優越的魯棒性。其獨特的加權機制，使得模型在優化過程中能夠更加聚焦于含有大噪聲的數據。隨著噪聲級別的增加，回環邊權重的修正力度也會相應增強，從而大大提升了回環邊對大噪聲的抗干擾能力。這種機制有效地抑制了噪聲的影響，確保了位姿估計的穩定性和可靠性。在針對各算法總損失函數值的實驗中，RW-RLSPGO的損失函數值明顯低于Odomentry、Spinning Tree、MASAT和RS初始化算法，尤其是在高噪聲條件下，這一優勢更加明顯。這表明RW-RLSPGO在噪聲環境下依然能夠保持較高的精度和性能，為迭代優化提供優質的初始值，從而有助于后續優化過程的順利進行。

為了進一步驗證RW-RLSPGO在位姿圖優化中的準確性和魯棒性，本文對比了RW-RLSPGO和同樣在大噪聲環境中表現出較強魯棒性的RS。針對總損失函數值、算法迭代時間和迭代次數三個指標進行分析，詳見表2。表中的加粗數據為實驗中表現相對較優的數據。結果顯示，所建立的回環邊殘差聚焦模型能夠有效降低大噪聲回環對位姿優化結果的影響，減少了迭代過程中陷入局部最優解的可能性，顯著降低了總損失函數值。相對于RS，RW-RLSPGO在魯棒性方面表現更為優越。由于RW-RLSPGO引入了回環邊殘差的聚焦權重，減少了因錯誤回環引起的不穩定性，從而有效減少了優化過程中不必要的迭代次數和運行時間。實驗結果表明，RW-RLSPGO在位姿圖優化的初始化階段表現出較強的綜合性能，特別是在處理大噪聲數據時表現出顯著的魯棒性，并且在迭代優化的精度、計算效率和收斂速度方面優于RS算法。

進一步分析RW-RLSPGO相較于RS在旋轉損失函數值rcost和平移損失函數值tcost上的提升，以標準差σnoise=0.5的大噪聲為例，計算提升比率，結果如表3所示。

在對sphere-a、cube、garage和torus數據集進行分析時不難發現，相較于平移損失函數值，明顯可見旋轉損失函數值在整體上表現出較為顯著的改善。在這種情況下，可以進一步表述：RW-RLSPGO算法對這些數據集產生顯著的改善可能源于sphere-a、cube、garage和torus數據集受旋轉噪聲的影響較為明顯；而另外兩個數據集受平移噪聲影響比較顯著，從而造成誤差權重更受平移誤差影響。

為了更直觀地對比在大噪聲情況下RW-RLSPGO算法相對于Odomentry、Spinning Tree、MASAT、RS四個初始化算法依然能提供優質的初始估計值，實驗選取torus和rim兩個數據集，經五個初始化算法與SE-sync位姿圖迭代優化算法結合，比較最終軌跡圖。圖3、4展現了兩個數據集torus和rim在大噪聲情況下的優化結果，圖3是一個模擬機器人在環型表面采集的數據集torus在噪聲值σnoise為0.5 rad和1 rad情況下分別用RW-RLSPGO、Odomentry、Spinning Tree、MASAT、RS結合當前常用的位姿圖優化算法SE-sync優化后的軌跡圖。圖4是佐治亞理工學院RIM中心收集的真實數據集rim在噪聲值σnoise為0.5 rad和1 rad情況下分別用RW-RLSPGO、Odomentry、Spinning Tree、MASAT、RS結合SE-sync優化后的軌跡圖。通過該實驗的結果可以看出，在大噪聲的情況下，本文RW-RLSPGO+SE-sync算法優化后的軌跡與無噪聲真實軌跡相似度更高，相比較于Odomentry+SE-sync、Spinning Tree+SE-sync、MASAT+SE-sync、RS+SE-sync的優化后軌跡更優，而且在實驗中，其他三個初始化算法Odomentry、Spinning Tree、MASAT提供的初始值容易使得SE-sync算法陷入局部最小，促使優化中斷，無法求解得到最優值。

4 結束語

本文提出了一種基于回環邊殘差聚焦的遞歸最小二乘位姿圖優化算法（RW-RLSPGO）。首先引入了K-means循環迭代的聚類算法，通過對回環邊殘差值的聚類劃分，與各自的聚類質心求取比重，構建殘差權重模型，大大降低大噪聲對初始化算法的影響，顯著提升定位精確性和地圖一致性。該算法在3DSLAM的公共PGO數據集中得到驗證，它相比于RS更具有魯棒性，可以適應大噪聲對優化的影響，有效避免了陷入局部最小值的風險。本文所提初始化方法可以與g2o等優化算法結合，使得SLAM位姿圖計算更加高效。由于K-means聚類算法在初始化時隨機選擇聚類中心，不同的初始聚類中心可能導致不同的聚類結果，且該算法容易陷入局部最優解，也會導致在不同運行中產生不同的聚類效果，進而會引起實驗效果的波動。在未來的工作中，需要將重點放到改進K-means聚類算法中，以提高其穩定性和魯棒性。例如，引入更加先進的初始聚類中心選擇方法等，或者采用全局優化技術和研究更高效的迭代策略，減少算法陷入局部最優解的可能性；與此同時，將把RW-RLSPGO的MATLAB編程語言轉為C++編程語言，增強算法的通用性和可移植性，將該算法融入到實時的SLAM實驗中，發揮它在實際應用中的優勢。

參考文獻：

[1]Davison A J， Reid I D， Molton N D，et al. MonoSLAM： real-time single camera SLAM[J]. IEEE Trans on Pattern Analysis and Machine Intelligence， 2007， 29（6）： 1052-1067.

[2]劉浩敏， 章國鋒， 鮑虎軍. 基于單目視覺的同時定位與地圖構建方法綜述[J]. 計算機輔助設計與圖形學學報， 2016， 28（6）： 855-868. （Liu Haomin， Zhang Guofeng， Bao Hujun. A survey of monocular simultaneous localization and mapping[J]. Journal of Computer Aided Design amp; Computer Graphics， 2016， 28（6）： 855-868.）

[3]Latif Y， Cadena C， Neira J. Robust loop closing over time for pose graph SLAM[J]. The International Journal of Robotics Research， 2013， 32（14）： 1611-1626.

[4]Kyummerle R， Grisetti G， Strasdat H，et al. G2o： a general framework for graph optimization[C]// Proc of IEEE International Confe-rence on Robotics and Automation. Piscataway， NJ： IEEE Press， 2011： 3607-3613.

[5]Ila V， Polok L， Solony M， et al. SLAM++： a highly efficient and temporally scalable incremental SLAM framework[J]. The International Journal of Robotics Research， 2017， 36（2）： 210-230.

[6]黎萍， 操超超. 適應于弱光環境的ORB-SLAM算法[J]. 北京郵電大學學報， 2024， 47（1）： 106-111. （Li Ping， Cao Chaochao. Vision SLAM algorithm for low light environment[J]. Journal of Beijing University of Posts and Telecommunications， 2024， 47（1）： 106-111.）

[7]鄒彪， 任偉達， 朱曉康， 等. 融合語義信息的實時動態視覺SLAM方法[J/OL]. 控制工程. （2024-01-05）. https：//link.cnki.net/doi/10.14107/j.cnki.kzgc.20231002. （Zou Biao， Ren Weida， Zhu Xiaokang， et al. Real-time dynamic visual SLAM approach to fusing semantic information[J/OL]. Control Engineering of China. （2024-01-05）. https：//link.cnki.net/doi/10.14107/j.cnki.kzgc.20231002.）

[8]Lowry S， Sunderhauf N， Newman P， et al. Visual place recognition： a survey[J]. IEEE Trans on Robotics， 2016， 32（1）： 1-19.

[9]Penrose， Mathew D. The longest edge of the random minimal spanning tree[J]. Annals of Applied Probability， 1997， 7（2）： 340-361.

[10]Hu G， Khosoussi K， Huang Shoudong. Towards a reliable slam back-end[C]// Proc of IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway， NJ： IEEE Press， 2013： 37-43.

[11]Harsányi K， Kiss A， Szirányi T，et al. MASAT： a fast and robust algorithm for pose-graph initialization[J]. Pattern Recognition Letters， 2020， 129： 131-136.

[12]Wu Fang， Beltrame G. Cluster-based penalty scaling for robust pose graph optimization[J]. IEEE Robotics and Automation Letters， 2020， 5（4）： 6193-6200.

[13]Lu F， Milios E. Globally consistent range scan alignment for environment mapping[J]. Autonomous Robots， 1997， 4（4）： 333-349.

[14]Thrun S， Montemerlo M. The graph SLAM algorithm with applications to large-scale mapping of urban structures[J]. The International Journal of Robotics Research， 2006， 25（5-6）： 403-429.

[15]Folkesson J， Christensen H. Graphical SLAM：a self-correcting map[C]//Proc of IEEE International Conference on Robotics and Automation. Piscataway， NJ： IEEE Press， 2004： 383-390.

[16]Duckett T， Marsland S， Shapiro J. Fast， on-line learning of globally consistent maps[J]. Autonomous Robots， 2002， 12（3）： 287-300.

[17]Howard A， Mataric M J， Sukhatme G. Relaxation on a mesh： a formalism for generalized localization [C]// Proc of IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway， NJ： IEEE Press， 2001： 1055-1060

[18]Frese U， Larsson P， Duckett T. A multilevel relaxation algorithm for simultaneous localization and mapping[J]. IEEE Trans on Robo-tics， 2005， 21（2）： 196-207.

[19]Carlone L， Calafiore G C， Tommolillo C， et al. Planar pose graph optimization： duality， optimal solutions， and verification[J]. IEEE Trans on Robotics， 2016， 32（3）： 1-21.

[20]Carlone L， Rosen D M， Calafiore G，et al. Lagrangian duality in 3D SLAM： verification techniques and optimal solutions [C]// Proc of IEEE International Conference on Intelligent Robots and Systems. Piscataway，NJ：IEEE Press， 2015： 125-132.

[21]李雨潔， 魏國亮， 蔡潔， 等. 基于特征分解的快速位姿圖優化算法[J]. 計算機應用研究， 2022， 39（10）： 3065-3070. （Li Yujie， Wei Guoliang， Cai Jie， et al. Fast pose graph optimization algorithm based on eigen decomposition[J]. Application Research of Computers， 2022， 39（10）： 3065-3070.）

[22]李榮華， 童欣基， 薛豪鵬， 等. 改進PointNetLK的點云智能配準與位姿圖優化方法[J]. 宇航學報， 2022， 43（11）： 1557-1565. （Li Ronghua， Dong Xinji， Xue Haopeng， et al. Improved pointNetLK method for point cloud intelligent registration and pose graph optimization[J]. Journal of Astronautics， 2022， 43（11）： 1557-1565.）

[23]Kaess M， Ranganathan A， Dellaert F. iSAM： incremental smoothing and mapping [J]. IEEE Trans on Robotics， 2008， 24（6）： 1365-1378.

[24]戴天虹， 李志成. 基于優化列文伯格-馬夸爾特法的SLAM圖優化[J]. 哈爾濱理工大學學報， 2021， 26（2）： 68-74. （Dai Tianhong， Li Zhicheng. Optimization of SLAM graph based on optimization Levenberg-Marquardt method[J]. Journal of Harbin University of Science and Technology， 2021， 26（2）： 68-74.）

[25]Rosen D M， Kaess M， Leonard J J. RISE： an incremental trust-region method for robust online sparse least-squares estimation[J]. IEEE Trans on Robotics， 2014， 30（5）： 1091-1108.

[26]Olson E， Leonard J， Teller S. Fast iterative alignment of pose graphs with poor initial estimates[C]// Proc of IEEE International Confe-rence on Robotics and Automation. Piscataway， NJ： IEEE Press， 2006： 2262-2269.

[27]Grisetti G， Stachniss C， Burgard W. Non-linear constraint network optimization for efficient map learning[J]. IEEE Trans on Intelligent Transportation Systems， 2009， 10（3）： 428-439.

[28]Guadagnino T， Di Giammarino L， Grisetti G. HiPE： hierarchical initialization for pose graphs[J]. IEEE Robotics and Automation Letters， 2021， 7（1）： 287-294.

[29]簡單， 魏國亮， 蔡潔， 等. 基于嵌套剖分的位姿圖分層優化算法[J]. 計算機應用研究， 2024， 41（6）： 1916-1920. （Jian Dan， Wei Guoliang， Cai Jie， et al. Hierarchical pose graph optimization algorithm based on nested dissection[J]. Application Research of Computers， 2024， 41（6）： 1916-1920.）

[30]Nasiri S M， Hosseini R， Moradi Hadi. A recursive least square method for 3D pose graph optimization problem [EB/OL]. （2018-06-01）. https：//arxiv.org/abs/1806.00281.

[31]Carlone L， Aragues R， Castellanos J A， et al. A fast and accurate approximation for planar pose graph optimization[J]. The International Journal of Robotics Research， 2014， 33（7）： 965-987.

[32]Carlone L， Censi A. From angular manifolds to the integer lattice： guaranteed orientation estimation with application to pose graph optimization[J]. IEEE Trans on Robotics， 2014， 30（2）： 475-492.

[33]Martinec D， Pajdla T. Robust rotation and translation estimation in multiview reconstruction[C]// Proc of IEEE Conference on Computer Vision and Pattern Recognition. Piscataway， NJ： IEEE Press， 2007： 1-8.

[34]Carlone L， Tron R， Daniilidis K， et al. Initialization techniques for 3D SLAM： a survey on rotation estimation and its use in pose graph optimization[C]// Proc of IEEE International Conference on Robotics and Automation. Piscataway， NJ： IEEE Press， 2015： 4597-4604.

[35]王苗苗， 魏國亮， 蔡潔，等. 基于偏差矩陣的3D SLAM位姿圖優化算法[J]. 信息與控制， 2023， 52（3）： 334-342. （Wang Miao-miao， Wei Guoliang， Cai Jie， et al. Deviation matrix based for 3D SLAM pose graph optimization[J]. Information and Control， 2023， 52（3）： 334-342.）