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

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

中圖分類號(hào)：TP391"" 文獻(xiàn)標(biāo)志碼：A

文章編號(hào)：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)境信息并估計(jì)自身位姿[1]，在機(jī)器人、自動(dòng)駕駛、三維重建等領(lǐng)域得到了廣泛的研究和發(fā)展[2]，成為了眾多平臺(tái)實(shí)現(xiàn)自主導(dǎo)航與定位的關(guān)鍵。

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

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

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

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

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

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

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

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

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

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

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

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

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

2 本文算法

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

2.1 RS算法

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

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

寫(xiě)為更加緊湊的形式，即

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

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

同理可知，最小化相對(duì)測(cè)量位置誤差項(xiàng)也是一個(gè)線性最小二乘問(wèn)題，并且具有封閉解：

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

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

2.2 RW-RLSPGO算法

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

2.2.1殘差聚類

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

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

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

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

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

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

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

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

算法 RW-RLSPGO

3 實(shí)驗(yàn)結(jié)果與分析

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

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

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

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

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

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

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

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

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

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

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

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

4 結(jié)束語(yǔ)

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

參考文獻(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]劉浩敏， 章國(guó)鋒， 鮑虎軍. 基于單目視覺(jué)的同時(shí)定位與地圖構(gòu)建方法綜述[J]. 計(jì)算機(jī)輔助設(shè)計(jì)與圖形學(xué)學(xué)報(bào)， 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é)報(bào)， 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á)， 朱曉康， 等. 融合語(yǔ)義信息的實(shí)時(shí)動(dòng)態(tài)視覺(jué)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]李雨潔， 魏國(guó)亮， 蔡潔， 等. 基于特征分解的快速位姿圖優(yōu)化算法[J]. 計(jì)算機(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的點(diǎn)云智能配準(zhǔn)與位姿圖優(yōu)化方法[J]. 宇航學(xué)報(bào)， 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é)報(bào)， 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]簡(jiǎn)單， 魏國(guó)亮， 蔡潔， 等. 基于嵌套剖分的位姿圖分層優(yōu)化算法[J]. 計(jì)算機(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]王苗苗， 魏國(guó)亮， 蔡潔，等. 基于偏差矩陣的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.）