基于回環(huán)邊殘差聚焦權(quán)重模型的位姿圖優(yōu)化算法

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

關(guān)鍵詞：同時定位與建圖；位姿圖優(yōu)化；回環(huán)邊；大噪聲；聚類

中圖分類號：TP391"" 文獻(xiàn)標(biāo)志碼：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，即在未知的環(huán)境下利用傳感器獲取環(huán)境信息并估計自身位姿[1]，在機器人、自動駕駛、三維重建等領(lǐng)域得到了廣泛的研究和發(fā)展[2]，成為了眾多平臺實現(xiàn)自主導(dǎo)航與定位的關(guān)鍵。

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

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

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

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

a）該算法受到K-means聚類思想的啟發(fā)，基于簇內(nèi)誤差平方和的手肘法，隨著簇數(shù)k的變化情況，求取簇內(nèi)誤差平方和的二階導(dǎo)的最大值，將其對應(yīng)的簇數(shù)作為聚類的k值，對回環(huán)邊的殘差值聚類。

b）基于聚類中心和回環(huán)邊的聚類結(jié)果，構(gòu)建了一種新的殘差聚焦權(quán)重模型。實驗證明，該模型通過對目標(biāo)函數(shù)加權(quán)的方式，有效降低了大噪聲對位姿估計的影響。

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

1 PGO問題及相關(guān)工作

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

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

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

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

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

2 本文算法

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

2.1 RS算法

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

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

寫為更加緊湊的形式，即

其中：A表示圖的關(guān)聯(lián)矩陣[29]。不難發(fā)現(xiàn)式（9）是一個最小二乘的形式，有封閉解，即

那么，可以根據(jù)（10）的封閉解求得旋轉(zhuǎn)方向R。

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

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

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

2.2 RW-RLSPGO算法

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

2.2.1殘差聚類

為了實現(xiàn)回環(huán)邊的聚類，定義Erij為每條回環(huán)邊對應(yīng)的誤差，即

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

再對簇內(nèi)平方和進(jìn)行兩次微分，則最佳的簇數(shù)k為簇內(nèi)平方和兩次微分最大值對應(yīng)的簇數(shù)。

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

2.2.2 殘差聚焦權(quán)重模型

當(dāng)聚類結(jié)束后，構(gòu)建出了一種魯棒的殘差權(quán)重模型，即

通過殘差權(quán)重模型構(gòu)建的目標(biāo)函數(shù)，大大降低了含大噪聲的回環(huán)邊對模型優(yōu)化產(chǎn)生的影響。

RW-RLSPGO通過對回環(huán)邊殘差聚類以及利用殘差聚焦權(quán)重模型求出每條回環(huán)邊權(quán)重，構(gòu)建新的PGO目標(biāo)函數(shù)，分別求解目標(biāo)函數(shù)的最小化相對測量位置誤差項和最小化相對測量旋轉(zhuǎn)誤差項，得到最終估計的位姿，具體實現(xiàn)步驟如下所示。

算法 RW-RLSPGO

3 實驗結(jié)果與分析

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

3.1 數(shù)據(jù)集

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

實驗中使用的公共3DSLAM數(shù)據(jù)集包含基本參數(shù)如表1所示，表中數(shù)據(jù)涵蓋了6個數(shù)據(jù)集的相對旋轉(zhuǎn)噪聲的均值mean、標(biāo)準(zhǔn)差STD、位姿節(jié)點數(shù)量N、相對測量邊的數(shù)量M以及回環(huán)邊的數(shù)量H（單位：個）。

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

3.2 實驗結(jié)果

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

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

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

進(jìn)一步分析RW-RLSPGO相較于RS在旋轉(zhuǎn)損失函數(shù)值rcost和平移損失函數(shù)值tcost上的提升，以標(biāo)準(zhǔn)差σnoise=0.5的大噪聲為例，計算提升比率，結(jié)果如表3所示。

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

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

4 結(jié)束語

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

參考文獻(xiàn)：

[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]劉浩敏， 章國鋒， 鮑虎軍. 基于單目視覺的同時定位與地圖構(gòu)建方法綜述[J]. 計算機輔助設(shè)計與圖形學(xué)學(xué)報， 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]黎萍， 操超超. 適應(yīng)于弱光環(huán)境的ORB-SLAM算法[J]. 北京郵電大學(xué)學(xué)報， 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]鄒彪， 任偉達(dá)， 朱曉康， 等. 融合語義信息的實時動態(tài)視覺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]李雨潔， 魏國亮， 蔡潔， 等. 基于特征分解的快速位姿圖優(yōu)化算法[J]. 計算機應(yīng)用研究， 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]李榮華， 童欣基， 薛豪鵬， 等. 改進(jìn)PointNetLK的點云智能配準(zhǔn)與位姿圖優(yōu)化方法[J]. 宇航學(xué)報， 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]戴天虹， 李志成. 基于優(yōu)化列文伯格-馬夸爾特法的SLAM圖優(yōu)化[J]. 哈爾濱理工大學(xué)學(xué)報， 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]簡單， 魏國亮， 蔡潔， 等. 基于嵌套剖分的位姿圖分層優(yōu)化算法[J]. 計算機應(yīng)用研究， 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位姿圖優(yōu)化算法[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.）