基于特征分解的快速位姿圖優(yōu)化算法

摘要：位姿圖優(yōu)化（pose graph optimization，PGO）是計(jì)算機(jī)視覺(jué)領(lǐng)域中廣泛應(yīng)用的高維非凸優(yōu)化算法，很難直接求解，主要依賴于迭代技術(shù)，對(duì)初始值的質(zhì)量要求較高，在實(shí)踐中很難得到保證。針對(duì)位姿圖優(yōu)化問(wèn)題進(jìn)行了研究，提出了基于特征分解的位姿圖簡(jiǎn)單封閉解算法，該算法首先對(duì)PGO問(wèn)題的最大似然估計(jì)進(jìn)行半定松弛，然后將其轉(zhuǎn)換為特征分解問(wèn)題，并利用數(shù)據(jù)的稀疏性設(shè)計(jì)了改進(jìn)的模型降階方法進(jìn)行求解，進(jìn)一步提高了算法的計(jì)算速度。算法具有可伸縮性、計(jì)算成本低和精度高等優(yōu)點(diǎn)。最后，在模擬和真實(shí)的位姿圖數(shù)據(jù)集上進(jìn)行實(shí)驗(yàn)評(píng)估，結(jié)果表明在不影響精度的情況下，該算法可以快速地進(jìn)行位姿圖優(yōu)化。

關(guān)鍵詞：位姿圖優(yōu)化； 最大似然估計(jì)； 特征分解； 模型降階

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

文章編號(hào)：1001-3695（2022）10-028-3065-06

doi：10.19734/j.issn.1001-3695.2022.02.0122

Fast pose graph optimization algorithm based on eigen decomposition

Li Yujiea， Wei Guoliangb， Cai Jiea， Wang Miaomiaoa

（a.College of Science， b.Business School， University of Shanghai for Science amp; Technology， Shanghai 200093， China）

Abstract：Pose graph optimization （PGO） is a high dimensional non-convex optimization algorithm widely used in the field of computer vision. It is hard to solve directly and mainly relies on iterative techniques. It requires high quality of initial value which is difficult to be guaranteed in practice. This paper studied PGO problem and proposed a simple closed solution algorithm for pose graph based on eigen decomposition. This algorithm first developed the semidefinite relaxation of maximum-likelihood estimation （MLE） for PGO problems. Then it transformed MLE into eigen decomposition problem， and designed an improved model reduction method to solve the problem by using the sparsity of data， which further improved the computational speed of the algorithm. The algorithm had the advantages of scalability， low computational cost and high precision. Finally， experimental evaluation on simulated and real-world pose-graph datasets shows that the proposed algorithm can optimize the pose graph quickly without compromising accuracy.

Key words：pose graph optimization; maximum-likelihood estimation; eigen decomposition; model reduction

0引言

SLAM（simultaneous localization and mapping）是在機(jī)器人缺乏環(huán)境先驗(yàn)信息的情況下，通過(guò)傳感器的測(cè)量值在對(duì)自身運(yùn)動(dòng)進(jìn)行估計(jì)的同時(shí)建立環(huán)境模型［1］，這對(duì)在未知場(chǎng)景中進(jìn)行自主導(dǎo)航和探索至關(guān)重要。近年來(lái)，SLAM技術(shù)廣泛應(yīng)用于日常生活中，例如自動(dòng)駕駛［2］、搜索救援［3］、精密農(nóng)業(yè)［4］等，成為研究熱點(diǎn)。SLAM系統(tǒng)分為前端和后端，如圖1所示。前端視覺(jué)里程計(jì)（visula odometry，VO），是根據(jù)相鄰圖像估計(jì)相機(jī)之間的相對(duì)運(yùn)動(dòng)，但這個(gè)過(guò)程存在累積誤差，需要后端對(duì)前端輸出的結(jié)果進(jìn)行優(yōu)化，得到最優(yōu)的位姿估計(jì)和全局一致［5］的地圖，提高SLAM系統(tǒng)的精度。PGO是目前機(jī)器人領(lǐng)域中應(yīng)用最廣泛的SLAM后端優(yōu)化技術(shù)［6］，將SLAM問(wèn)題用圖進(jìn)行直觀表示，其中圖的頂點(diǎn)表示機(jī)器人的絕對(duì)位姿，邊表示相對(duì)測(cè)量值，然后對(duì)圖進(jìn)行優(yōu)化，得到更精確的結(jié)果。PGO的目標(biāo)問(wèn)題通常是高維的［7］，涉及很多變量和約束，所以對(duì)優(yōu)化算法的計(jì)算效率要求很高。此外由于目標(biāo)問(wèn)題的非凸性，在求解時(shí)容易陷入局部極小值使估計(jì)完全無(wú)用，所以在保證計(jì)算效率的基礎(chǔ)上，避免陷入局部極小值是PGO問(wèn)題的重中之重。

目前PGO問(wèn)題的求解算法主要?jiǎng)澐譃槿悾?］，第一類算法是利用問(wèn)題的全局結(jié)構(gòu)進(jìn)行迭代求解。Lu等人［9］首次提出用位姿圖表示SLAM問(wèn)題，并進(jìn)行迭代非線性優(yōu)化。Olson等人［10］提出用隨機(jī)梯度下降方法求解PGO問(wèn)題，該方法具有良好的穩(wěn)定性和可擴(kuò)展性，即使給定較差的初始估計(jì)，也可以快速優(yōu)化機(jī)器人的位姿。Grisetti等人［11］改進(jìn)了Olson等人的工作，使用樹(shù)狀結(jié)構(gòu)定義和更新局部區(qū)域，顯著提高了隨機(jī)梯度下降方法的收斂性。文獻(xiàn)［12～14］提出了增量平滑算法，該類算法可以更好地處理非線性測(cè)量模型，通過(guò)增量重新排序提高了優(yōu)化效率，可以實(shí)現(xiàn)大規(guī)模PGO問(wèn)題的更新。Rosen等人［15］研究了PGO問(wèn)題的最小二乘結(jié)構(gòu)，指出了降低其非線性和非凸性的可能性。Kümmerle等人［16］利用圖結(jié)構(gòu)的稀疏性，使用高斯—牛頓方法進(jìn)行求解。然而，上述所有的非線性優(yōu)化技術(shù)都是局部搜索方法，并不能保證解的最優(yōu)性。

第二類算法旨在尋找良好的初始值，以提高收斂速度并降低收斂到局部極小值的風(fēng)險(xiǎn)。Carlone等人［17］提出了LAGO算法，為2D PGO問(wèn)題提供了精確的初始估計(jì)，但是LAGO算法只能應(yīng)對(duì)中低噪聲數(shù)據(jù)集。之后文獻(xiàn)［18］對(duì)幾種先進(jìn)的初始化方法進(jìn)行了回顧和比較，結(jié)果表明，使用弦方法作為初始化器，GTSAM作為求解器是迭代求解的最佳組合。Nasiri等人［19］提出了RS算法求解PGO問(wèn)題，該算法在高噪聲下也能得到最優(yōu)解的精確近似，具有較好的魯棒性。

第三類算法是對(duì)PGO問(wèn)題進(jìn)行松弛，以克服目標(biāo)問(wèn)題的非凸性。Carlone等人對(duì)原問(wèn)題進(jìn)行半定松弛，利用拉格朗日對(duì)偶理論驗(yàn)證了2D［20］和3D［21］ SLAM中解的最優(yōu)性，并證明了當(dāng)對(duì)偶間隔為零時(shí)，可以檢索出原問(wèn)題的全局最優(yōu)解。Rosen等人［22］利用PGO問(wèn)題的低秩、幾何結(jié)構(gòu)，將其簡(jiǎn)化為低維黎曼流形上的等價(jià)優(yōu)化問(wèn)題，并設(shè)計(jì)了一種截?cái)嘈湃斡蚍椒ㄟM(jìn)行求解，比使用內(nèi)點(diǎn)法求解快幾個(gè)數(shù)量級(jí)。Eriksson等人［23］利用圖論對(duì)旋轉(zhuǎn)平均問(wèn)題進(jìn)行了理論分析，求解出了噪聲水平的誤差界，以確保解的全局最優(yōu)性。然而，因?yàn)檫@些解是通過(guò)松弛約束得到的，所以通常不在原問(wèn)題的可行域內(nèi)，需要重新投影。

綜上，目前PGO的相關(guān)算法可以收斂到全局最優(yōu)近似解，但是計(jì)算速度較慢，計(jì)算成本過(guò)高。為了加快求解效率，Singer［24］提出通過(guò)特征分解進(jìn)行角同步的計(jì)算，但是只能應(yīng)用于2D問(wèn)題，不能擴(kuò)展到更常用的3D問(wèn)題。之后文獻(xiàn)［25］將特征分解方法擴(kuò)展到了3D問(wèn)題，但是只計(jì)算了位姿的旋轉(zhuǎn)部分，忽略了平移部分的優(yōu)化。Arrigoni等人［26］提出用特征分解方法解決運(yùn)動(dòng)同步問(wèn)題，同時(shí)對(duì)旋轉(zhuǎn)和平移進(jìn)行優(yōu)化（即在線性群SE（3）上進(jìn)行優(yōu)化），但是增大了優(yōu)化矩陣的維度，隨著問(wèn)題規(guī)模的增加，求解速度會(huì)受到很大影響；并且為了最終得到滿足SE（3）形式的解，犧牲了估計(jì)的精度。

本文在上述問(wèn)題的基礎(chǔ)上提出了基于特征分解的快速位姿圖優(yōu)化算法，即EDPO算法。該算法分為兩個(gè)階段進(jìn)行優(yōu)化：第一階段使用改進(jìn)的特征分解算法優(yōu)化旋轉(zhuǎn)矩陣，第二階段使用最小二乘法求解平移部分。該算法具有可伸縮性、計(jì)算成本低和精度高等優(yōu)點(diǎn)。本文的主要貢獻(xiàn)如下：

a）提出了一種新穎的PGO算法，將原PGO問(wèn)題劃分為兩個(gè)子問(wèn)題。首先對(duì)旋轉(zhuǎn)矩陣的行列式約束進(jìn)行松弛，推導(dǎo)出了基于特征分解的封閉解，再將解投影到正交群上以滿足旋轉(zhuǎn)矩陣的性質(zhì)；然后使用最小二乘法優(yōu)化位姿的平移部分。

b）提出了一個(gè)簡(jiǎn)潔的矩陣公式，將原PGO問(wèn)題的公式改寫為可以進(jìn)行特征分解的形式，與對(duì)應(yīng)的圖結(jié)構(gòu)的Laplacian矩陣建立了聯(lián)系，以便進(jìn)行特征分解；并且該矩陣公式可以處理數(shù)據(jù)缺失的情況，計(jì)算更精確。

c）為了提高特征分解模型的計(jì)算性能，提出了一種改進(jìn)的模型降階方法，高效地解決了推導(dǎo)出的特征分解模型的高維問(wèn)題。

d）在模擬和真實(shí)的PGO數(shù)據(jù)集上的對(duì)比實(shí)驗(yàn)表明，在不影響精度的情況下，EDPO算法顯著提高了計(jì)算效率。

1位姿圖優(yōu)化理論基礎(chǔ)

1.1圖的理論基礎(chǔ)

圖結(jié)構(gòu)可以用G=（V，E）表示，其中V是非空集合稱為節(jié)點(diǎn)集，E是V中元素構(gòu)成的無(wú)序二元組的集合，稱為邊集，本文中節(jié)點(diǎn)數(shù)和邊數(shù)分別用n和m表示。若每對(duì)不同的頂點(diǎn)之間都恰有一條邊相連，則該圖為完全圖（如圖2）。

令A(yù)∈Euclid Math TwoRApn×n為G的鄰接矩陣，其元素為

Aij=1（i，j）∈E0（i，j）E

（1）

很顯然，A為對(duì)稱矩陣。令D∈Euclid Math TwoRApn×n為G的度矩陣，則D為對(duì)角陣，對(duì)角線上的元素為各個(gè)頂點(diǎn)的度，即Dii=∑jAij。令L∈Euclid Math TwoRApn×n為G的Laplacian矩陣，則有L=D－A。以圖2為例，該圖的鄰接矩陣A、度矩陣D和Laplacian矩陣L分別為

加權(quán)圖是由圖G=（V，E）和定義在其邊上的權(quán)重函數(shù)w組成的三聯(lián)體G=（V，E，w）。在一些應(yīng)用中，給出了反映測(cè)量值可靠性的非負(fù)權(quán)重wij，將其存儲(chǔ)在圖的鄰接矩陣A=［wij］中，因此加權(quán)圖的度矩陣D定義為Dii=∑jwij。

1.2位姿圖優(yōu)化

位姿圖是圖優(yōu)化的一種具體形式，其中圖的頂點(diǎn)表示機(jī)器人的絕對(duì)位姿，邊表示相對(duì)測(cè)量值，本文將其用于解決機(jī)器人的軌跡優(yōu)化問(wèn)題。PGO計(jì)算在給定m對(duì)相對(duì)測(cè)量值的條件下，n個(gè)機(jī)器人位姿x1，…，xn的最大似然估計(jì)。在三維問(wèn)題中，機(jī)器人的絕對(duì)位姿和相對(duì)測(cè)量值都是特殊歐氏群SE（3）中的元素SE（3）{（R，t）：R∈SO（3），t∈Euclid Math TwoRAp3}，旋轉(zhuǎn)矩陣需要同時(shí)滿足正交約束和行列式約束。本文使用符號(hào)xi=（Ri，ti）和ij=（ij，ij）分別表示絕對(duì)位姿和相對(duì)測(cè)量值。本節(jié)提出一個(gè)修改后的PGO公式，其中將使用弦距離來(lái)量化旋轉(zhuǎn)誤差，從測(cè)量值的生成模型開(kāi)始，然后推導(dǎo)出相應(yīng)的最大似然估計(jì)。

1.2.1測(cè)量值的生成模型

假設(shè)以下是相對(duì)測(cè)量值（ij，ij）的生成模型

ij=RTi（tj-ti）+ijij～Gaussian（03，ω2tI3）

ij=RiRTjijij～vonMises（I3，ω2R）（2）

其中：Gaussian（μ，Ω）表示均值為μ和協(xié)方差矩陣為Ω的高斯分布；vonMises（S，k）表示均值為S，參數(shù)為k的各向同性的on Mises-Fisher分布。

1.2.最大似然估計(jì)

求解位姿的最大似然估計(jì)（MLE）等價(jià)于最小化負(fù)對(duì)數(shù)似然：

f*MLE=min{xi∈SE（3）}∑（i，j）∈E－log L（ij|x）－log L（ij|x）（3）

－log L（ij|x）=k2ij2‖Ri－ijRj‖2F+const（4）

－log L（ij|x）=τ2ij‖tj－ti－Riij‖2+const（5）

其中：‖·‖2F表示矩陣的Frobenius范數(shù)。

將式（4）和（5）代入到式（3）中，得到最大似然估計(jì)：

f*MLE=min{ti∈R3}{Ri∈SO（3）}∑（i，j）∈Eτ2ij‖tj－ti－Riij‖2+k2ij2‖Ri－ijRj‖2F（6）

MLE問(wèn)題是高維、非凸的，假設(shè)R*i，i={1，…，n}是式（6）的最優(yōu)旋轉(zhuǎn)解，則平移的求解變成一個(gè)最小二乘問(wèn)題［19］，因此，將原優(yōu)化問(wèn)題劃分為兩個(gè)子問(wèn)題：

minRi∈SO（3）k2ij2‖Ri－ijRj‖2F（7）

minti∈Euclid Math TwoRAp3τ2ij‖tj－ti－Riij‖2（8）

接下來(lái)將先求解問(wèn)題式（7），然后用得到的最優(yōu)旋轉(zhuǎn)解來(lái)求解問(wèn)題式（8）。

2本文方法

2.1旋轉(zhuǎn)優(yōu)化模型

為了求解旋轉(zhuǎn)問(wèn)題式（7），下面證明通過(guò)特征分解可以找到一個(gè)全局最優(yōu)旋轉(zhuǎn)解R*。為方便進(jìn)行推導(dǎo)計(jì)算，在后面的推導(dǎo)中使用塊矩陣，將所有位姿的旋轉(zhuǎn)矩陣堆疊在R∈Euclid Math TwoRAp3n×3中，即

R=［RT1，…，RTn］TRi∈SO（3）（9）

首先考慮所有相對(duì)測(cè)量值都可用（即完全圖），并且相對(duì)測(cè)量值不含噪聲的情況，將旋轉(zhuǎn)測(cè)量值Rij堆疊在塊矩陣R∈Euclid Math TwoRAp3n×3n中，即

=I312…1n

21I3…2n

n1n2…I3（10）

基于ij=RiRTj，則有

=RRT

rank（）=3（11）

因?yàn)镽TR=nI3，可得到

R=nR（12）

這意味著在沒(méi)有噪聲的情況下，R的列是由的與特征值n對(duì)應(yīng)的特征向量構(gòu)成的，因?yàn)榈闹葹?，所以其余特征值均為零，n就是R的最大特征值。式（12）等價(jià)于式（13）。

（nI3－）R=0（13）

其次考慮相對(duì)運(yùn)動(dòng)測(cè)量被噪聲破壞的情況，式（12）對(duì)應(yīng)的優(yōu)化問(wèn)題為

minRi∈O（3）k2ij2‖R－nR‖2F（14）

此處放松了旋轉(zhuǎn)矩陣的行列式約束，即在O（3）中進(jìn)行優(yōu)化，O（3）{R∈Euclid Math TwoRAp3×3：RTR=I3}。然而在實(shí)際應(yīng)用中并非所有相對(duì)測(cè)量值都可用，所以下面考慮數(shù)據(jù)缺失的情況。缺失的相對(duì)運(yùn)動(dòng)對(duì)應(yīng)中的零塊，（A13）表示可用的相對(duì)運(yùn)動(dòng)信息，其中，表示Kronecker積，表示Hadamard積。鄰接矩陣A在與13進(jìn)行Kronecker乘積后被擴(kuò)維，以匹配的塊結(jié)構(gòu)。在這種情況下，式（12）可以推廣為

（（A13））R=（DI3）R（15）

等價(jià)于

（（DI3）－（A13））R=0（16）

觀察到DI3對(duì)應(yīng)于（D13），因?yàn)榈膶?duì)角線上的元素是單位陣，并且DI3是塊對(duì)角陣。所以式（16）可以再進(jìn)一步改寫為

（（L13））R=0（17）

當(dāng)噪聲存在時(shí)，上式不完全成立，對(duì)應(yīng)的優(yōu)化問(wèn)題為

minRi∈O（3）‖MR‖2F（18）

其中：M=（（L13）。

命題1問(wèn)題式（18）存在一個(gè)封閉解，由MTM的最小特征值相關(guān)的三個(gè)特征向量給出。

證明問(wèn)題式（18）等價(jià)于

min tr（RT（MTM）R）

s.t. Ri∈O（3）（19）

對(duì)應(yīng)的拉格朗日函數(shù)為

L（R，Λ）=tr（RT（MTM）R）+tr（Λ（RTR－nI3））（20）

其中：Λ=blkdiag（Λ1，…，Λn）∈Euclid Math TwoRAp3n×3n為拉格朗日乘子，根據(jù)拉格朗日定理［27］，將L對(duì)R的偏導(dǎo)數(shù)設(shè)置為零，可得到穩(wěn)定性條件：

2（MTM）R+2RΛ=0（21）

即（MTM）R=－RΛ，令ri（i=1，2，3）為MTM的任意三個(gè)特征向量，且λi是對(duì)應(yīng)的特征值，則R=［r1，r2，r3］同時(shí)滿足式（21）及約束RTR=nI3。也就是說(shuō)，任意三個(gè)特征向量都可以組成拉格朗日函數(shù)的一個(gè)穩(wěn)定點(diǎn)。如果ri（i=1，2，3）是MTM的最小特征值對(duì)應(yīng)的特征向量，則得到問(wèn)題式（18）的最小值，所以原優(yōu)化問(wèn)題轉(zhuǎn)換為求解矩陣的特征向量問(wèn)題。

最終求得了問(wèn)題式（18）的封閉解，但這是原問(wèn)題松弛形式下求得的解，可能不完全滿足旋轉(zhuǎn)矩陣的性質(zhì)，為了得到更精確的旋轉(zhuǎn)估計(jì)值，用F表示命題1得到的包含特征向量的3n×3的矩陣。其中Fi表示第i個(gè)位姿旋轉(zhuǎn)的估計(jì)值，需要通過(guò)奇異值分解Fi=Ui∑iVTi找到與其最接近的旋轉(zhuǎn)矩陣，并令R^Ti=UiVTi，再進(jìn)一步強(qiáng)制執(zhí)行det（R^i）=1，得到滿足旋轉(zhuǎn)約束的解。

2.2改進(jìn)的模型降階方法

在旋轉(zhuǎn)優(yōu)化模型建立后，需要進(jìn)行特征值和特征向量的求解。因?yàn)樵趯?shí)際應(yīng)用中需要求解的矩陣是高維且稀疏的，使用傳統(tǒng)算法會(huì)大量填充零元素，嚴(yán)重消耗內(nèi)存，所以需要尋求有效的求解方法。目前常用的方法是Arnoldi模型降階方法［26］，該方法用一個(gè)較小系統(tǒng)近似原始系統(tǒng)，穩(wěn)定性強(qiáng)，計(jì)算速度快，適用于大規(guī)模的特征求解問(wèn)題。本文在Arnoldi模型降階方法的初始化階段進(jìn)行了改進(jìn)。Arnoldi模型降階方法，首先要建立矩陣MTM的Krylov子空間的一組標(biāo)準(zhǔn)正交基（v1，…，vr），這組基的構(gòu)造需要經(jīng)過(guò)迭代計(jì)算

w=（MTM）vj（22）

得到w，然后再將w與之前的向量進(jìn)行正交，得到vj+1。然而，僅通過(guò)一次正交化可能得不到穩(wěn)定的系統(tǒng)，所以本文在正交化之后又加入重正交化步驟，這樣可以保證得到的子空間的基具有較好的正交性。算法1總結(jié)了改進(jìn)的模型降階方法。

算法1改進(jìn)的模型降階方法

輸入：對(duì)稱矩陣MTM和單位向量v1。

輸出：MTM的特征值λi與特征向量φi（i=1，2，…，r）。

a）for j=1：r-1

w=（MTM）vj

for i=1：j//正交化

hij=vTiw;w=w-hijvi;

end

for i=1：j//重正交化

s=vTiw;

hij=hij+s;w=w－svi;

end

if hj+1，j=0

stop

else vj+1=w/hj+1，j

end

Vj+1=［Vj，vj+1］;

end

Hr=［hi，j］

b）（MTM）Vr=VrHr+hr+1，rvr+1eTr/*得到r步Arnoldi分解，其中er是單位矩陣Ir的最后一行*/

c）計(jì)算矩陣Hr的特征值和對(duì)應(yīng)的特征向量，分別為

λi，ηi（i=1，2，…，r）;

d）矩陣MTM的特征值和特征向量分別為

λi=λi，φi=Vrηi（i=1，2，…，r）;

e）返回λi，φi（i=1，2，…，r）;

2.3基于特征分解的封閉解算法

通過(guò)2.1節(jié)的旋轉(zhuǎn)優(yōu)化模型找到了最優(yōu)解R*后，問(wèn)題式（8）可以寫為

minti∈Euclid Math TwoRAp3τ2ij‖tj－ti－R*iij‖2（23）

與2.1節(jié)類似，使用塊矩陣進(jìn)行推導(dǎo)，將平移向量ti（i=1，…，n）堆疊在t∈Euclid Math TwoRAp3n中

t=［tT1，…，tTn］T（24）

因此，式（23）可以重寫為

mint∈Euclid Math TwoRAp3n‖（BI3）t－C‖2（25）

其中：B∈Euclid Math TwoRApm×n是包含權(quán)重的鄰接矩陣，在與I3進(jìn)行Kronecker乘積后被擴(kuò)維，以匹配t的結(jié)構(gòu)；C∈Euclid Math TwoRAp3m是包含相對(duì)平移測(cè)量值的矩陣，C的第k行是R*iij，（ek=（i，j）∈E）。顯然，這是一個(gè)線性最小二乘問(wèn)題，封閉解為

t*=（BTB）－1BTC（26）

由于BTB是對(duì)稱的，可以使用Cholesky分解進(jìn)行預(yù)處理，這樣可以加快計(jì)算逆矩陣的速度。結(jié)合前面的旋轉(zhuǎn)優(yōu)化模型，得到了完整的EDPO算法。

算法2EDPO算法

輸入：圖的節(jié)點(diǎn)V、邊E、相對(duì)測(cè)量值（Rij，tij）以及權(quán)重k2ij、τ2ij。

輸出：位姿估計(jì)Ri，ti（i=1，2，…，n）。

a）將原PGO問(wèn)題劃分為式（7）（8）兩個(gè)子問(wèn)題

b）求解子問(wèn)題minRi∈SO（3）k2ij2‖Ri－ijRj‖2F

for ek=（i，j）∈E

A（i，j）=kij;D（i，i）=D（i，i）+kij

R（block（i，j））=Rij;R（block（i，i））=I3

end

L=D－A;

M=（（L13）

用算法1求解MTM的最小特征值和對(duì)應(yīng)的三個(gè)特征向量，記為ri（i=1，2，3）

for i=1：n

Fi=r（3*i－2：3*i，3）

［Ui，Σi，Vi］=SVD（Fi）

Ri=UiVTi

end

c）求解子問(wèn)題min τ2ij‖tj－ti－Riij‖2

for ek=（i，j）∈E

B（k，i）=－τij，B（k，j）=τij

C（3*k－2：3*k）=Riij

end

t*=（BTB）－1BTC

ti=t*（3*i－2：3*i）

d）返回Ri，ti（i=1，2，…，n）

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

在本章中分別在模擬和真實(shí)的3D PGO數(shù)據(jù)集上進(jìn)行實(shí)驗(yàn)，將提出的EDPO算法與高斯—牛頓法（記為G-N）、EIG算法［26］進(jìn)行對(duì)比，評(píng)估EDPO的性能。以下所有實(shí)驗(yàn)都是在Dell OptiPlex 7040微型臺(tái)式機(jī)電腦上進(jìn)行的，實(shí)驗(yàn)運(yùn)行環(huán)境配置為Intel Core i5-6500 CPU@3.20 GHz，4 GB內(nèi)存，運(yùn)行Windows 10系統(tǒng)。

3.1模擬噪聲實(shí)驗(yàn)

為了探究測(cè)量噪聲（包括旋轉(zhuǎn)和平移）、問(wèn)題規(guī)模大小對(duì)EDPO性能的影響，所以進(jìn)行了一組模擬實(shí)驗(yàn)。首先向原始cube數(shù)據(jù)子集的相對(duì)測(cè)量值添加噪聲，該數(shù)據(jù)集模擬了機(jī)器人沿直線穿過(guò)規(guī)則立方體的軌跡。測(cè)量值ij=（ij，ij）根據(jù)式（2）進(jìn)行采樣，其中，旋轉(zhuǎn)噪聲的標(biāo)準(zhǔn)差σR∈［0°，10°］，平移噪聲的標(biāo)準(zhǔn)差σT∈［0 m，1 m］。然后分別在不同噪聲水平（包括旋轉(zhuǎn)和平移）、不同規(guī)模大小的cube數(shù)據(jù)集上，對(duì)比了EDPO和G-N、EIG的目標(biāo)函數(shù)值以及運(yùn)行時(shí)間。本文的目標(biāo)函數(shù)為損失函數(shù)，所以數(shù)值越小說(shuō)明結(jié)果越精確。所有的統(tǒng)計(jì)數(shù)據(jù)都是經(jīng)過(guò)100次隨機(jī)實(shí)驗(yàn)取平均值得到的，模擬實(shí)驗(yàn)的結(jié)果如下。

其中，圖3的（a）（b）分別描述旋轉(zhuǎn)噪聲（平移噪聲固定為σT=0.1，位姿數(shù)量固定為53）對(duì)三種算法的目標(biāo)函數(shù)值和運(yùn)行時(shí)間的影響。從圖中可以看出，EDPO的速度比G-N和EIG要快得多，目標(biāo)函數(shù)值與G-N基本持平。這組模擬實(shí)驗(yàn)的結(jié)果表明，EDPO在合理的操作條件下可以得到SLAM問(wèn)題的全局最優(yōu)近似解。

同時(shí)，還記錄了三種算法的誤差對(duì)比分析結(jié)果，包括平均誤差、中值誤差和均方根誤差，誤差越小，表示算法越穩(wěn)定魯棒性越好。實(shí)驗(yàn)結(jié)果分別記錄在圖4的（a）～（c）中。可以看到，旋轉(zhuǎn)噪聲的標(biāo)準(zhǔn)差σR∈［0°，10°］時(shí)，EDPO的誤差值小于EIG，與G-N基本持平，這表明EDPO在一定的旋轉(zhuǎn)噪聲水平下可以恢復(fù)機(jī)器人的位姿軌跡。

圖5（a）（b）分別描述平移噪聲（旋轉(zhuǎn)噪聲固定為σR=5°，位姿數(shù)量固定為53）對(duì)三種算法的目標(biāo)函數(shù)和運(yùn)行時(shí)間的影響，從圖中可以看出，EDPO的速度比G-N和EIG要快，目標(biāo)函數(shù)值與G-N基本持平，所以總體性能要優(yōu)于G-N和EIG。

圖6（a）（b）分別描述位姿數(shù)量的增加對(duì)三種算法性能的影響（旋轉(zhuǎn)噪聲固定為σR=5°，平移噪聲固定為σT=0.1），從圖中可以看出，問(wèn)題規(guī)模的增加對(duì)EDPO運(yùn)行時(shí)間的影響要小于G-N和EIG，并且目標(biāo)函數(shù)值與G-N基本持平，這驗(yàn)證了EDPO具有可伸縮性，適用于大規(guī)模問(wèn)題。

3.2算法性能對(duì)比實(shí)驗(yàn)

選擇了六個(gè)公開(kāi)的3D SLAM數(shù)據(jù)集作為本文的實(shí)驗(yàn)數(shù)據(jù)集，其中sphere、torus和cube數(shù)據(jù)集是模擬數(shù)據(jù)集，cubicle、rim和garage是真實(shí)數(shù)據(jù)集。sphere數(shù)據(jù)集模擬了機(jī)器人在球面上運(yùn)動(dòng)的軌跡；torus數(shù)據(jù)集模擬了機(jī)器人在環(huán)面表面上運(yùn)動(dòng)時(shí)的軌跡；cube數(shù)據(jù)集是對(duì)機(jī)器人在三維網(wǎng)格世界上行走的軌跡進(jìn)行了模擬。garage是收集自Vertigo的真實(shí)數(shù)據(jù)集；cubicle、rim是RIM中心收集的兩個(gè)真實(shí)數(shù)據(jù)集，通過(guò)對(duì)3D激光掃描儀獲取的點(diǎn)云進(jìn)行ICP獲取了相對(duì)位姿約束。對(duì)于每個(gè)數(shù)據(jù)集，都將相應(yīng)的位姿數(shù)量（n）和相對(duì)測(cè)量值的數(shù)量（m）整理在了表1中。

在本部分實(shí)驗(yàn)中，將在以上3D SLAM數(shù)據(jù)集上評(píng)估EDPO的性能，這一組數(shù)據(jù)集更好地表示了實(shí)際應(yīng)用中的問(wèn)題分布。表2記錄了EDPO、G-N和EIG在每個(gè)數(shù)據(jù)集上的運(yùn)行結(jié)果，包括數(shù)據(jù)集的初始值、求解的目標(biāo)函數(shù)值、算法的運(yùn)行時(shí)間以及EDPO求得的數(shù)據(jù)集的最小特征值。表3記錄了三種算法優(yōu)化得到的軌跡與地面真實(shí)軌跡之間的誤差結(jié)果對(duì)比，包括平均誤差、中值誤差、均方根誤差以及標(biāo)準(zhǔn)差。

通過(guò)表2可以看到在每個(gè)數(shù)據(jù)集中，得到的結(jié)果與3.1節(jié)的模擬實(shí)驗(yàn)結(jié)果相似。在優(yōu)化結(jié)果方面，EDPO在torus、cube、cubicle、garage四個(gè)數(shù)據(jù)集上得到的解與G-N持平，優(yōu)化結(jié)果（機(jī)器人軌跡）如圖7所示；在sphere和rim兩個(gè)數(shù)據(jù)集上求解的結(jié)果比G-N差，但是差異較小。通過(guò)表3可以看到，在本節(jié)比較的六個(gè)數(shù)據(jù)集中，EDPO的軌跡誤差要低于EIG，與G-N基本持平。綜上，在三種優(yōu)化方法中，EIG的優(yōu)化結(jié)果要差于EDPO和G-N，因?yàn)镋IG將旋轉(zhuǎn)和平移同時(shí)優(yōu)化，只進(jìn)行單一的特征分解得到四個(gè)特征向量，然后再投影到SE（3）上，為了滿足SE（3）的形式，導(dǎo)致得到的平移估計(jì)不是最優(yōu)的。EDPO先對(duì)旋轉(zhuǎn)矩陣進(jìn)行優(yōu)化，然后用得到的最優(yōu)解求解平移部分，保證平移估計(jì)的準(zhǔn)確性。

另一方面，通過(guò)表2的相關(guān)數(shù)據(jù)可以看出，EDPO的求解速度要遠(yuǎn)遠(yuǎn)快于G-N和EIG。這是因?yàn)镋DPO執(zhí)行直接的全局優(yōu)化，用直接法對(duì)機(jī)器人的位姿進(jìn)行優(yōu)化求解，計(jì)算復(fù)雜度更低；而G-N是迭代搜索技術(shù)，需要對(duì)位姿不斷進(jìn)行迭代優(yōu)化直至收斂，所以運(yùn)行時(shí)間在很大程度上取決于迭代次數(shù)；EIG將位姿的旋轉(zhuǎn)部分和平移部分同時(shí)進(jìn)行優(yōu)化，擴(kuò)大了位姿表示的矩陣維數(shù)，增大了計(jì)算復(fù)雜度，所以要慢于EDPO。EDPO在求解速度上更具優(yōu)勢(shì)，在速度至關(guān)重要的低噪聲水平應(yīng)用中，EDPO的結(jié)果與G-N結(jié)果之間的區(qū)別可以忽略不計(jì)，因此，該算法可以快速地求解得到一個(gè)很好的解。這些結(jié)果進(jìn)一步證明了EDPO提供了一種有效的方法，可以恢復(fù)真實(shí)操作條件下的SLAM的全局最優(yōu)近似解，而且在計(jì)算速度上具有明顯的優(yōu)勢(shì)。計(jì)算速度的提高可以提升PGO技術(shù)的優(yōu)化性能，對(duì)于基于圖的SLAM，隨著位姿圖維數(shù)的增加，計(jì)算復(fù)雜度增大，EDPO可以提供一種平衡精度和時(shí)間的有效方法。

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

在PGO問(wèn)題中，最大似然估計(jì)能夠以較高的精度求解位姿，然而所涉及的優(yōu)化策略往往過(guò)于煩瑣，需要良好的初始化才能得到全局最優(yōu)近似解。本文提出了基于特征分解的封閉解算法，該算法可以快速進(jìn)行優(yōu)化求解，并適用于大規(guī)模的SLAM問(wèn)題。該算法由兩部分組成，先針對(duì)旋轉(zhuǎn)部分進(jìn)行優(yōu)化求解，得到旋轉(zhuǎn)最優(yōu)解后再對(duì)平移求解，并且針對(duì)該模型特點(diǎn)引入了模型降階方法，在不影響精度的情況下，極大地提高了計(jì)算效率。最后，在模擬和真實(shí)的SLAM數(shù)據(jù)集上進(jìn)行實(shí)驗(yàn)，結(jié)果表明，EDPO算法能夠在合理的操作條件下快速恢復(fù)SLAM問(wèn)題的全局最優(yōu)近似解。在以后的工作中，希望能將本文算法整合到一個(gè)完整的SLAM系統(tǒng)中，以進(jìn)一步驗(yàn)證和增強(qiáng)算法的實(shí)際性能。

參考文獻(xiàn)：

[1]Cadena C， Carlone L， Carrillo H， et al. Past， present， and future of simultaneous localization and mapping： toward the robust-perception age［J］.IEEE Trans on Robotics，2016，32（6）：1309-1332.

［2]高興波，史旭華，葛群峰，等.面向動(dòng)態(tài)物體場(chǎng)景的視覺(jué)SLAM綜述［J］.機(jī)器人，2021，43（6）：733-750.（Gao Xingbo， Shi Xuhua， Ge Qunfeng， et al. A review of visual SLAM for dynamic object scenarios［J］.Robot，2021，43（6）：733-750.）

［3]王燕，索寒生，賈貴金.石化危險(xiǎn)事故搜救移動(dòng)機(jī)器人SLAM問(wèn)題研究［J］.控制工程，2018，25（2）：346-351.（Wang Yan， Suo Hansheng， Jia Guijin. Research on MOBILE robot SLAM for petrochemical hazardous accident rescue［J］.Control Engineering of China，2018，25（2）：346-351.）

［4]李晨陽(yáng)，彭程，張振乾，等.融合里程計(jì)信息的農(nóng)業(yè)機(jī)器人定位與地圖構(gòu)建方法［J］.農(nóng)業(yè)工程學(xué)報(bào)，2021，37（21）：16-23.（Li Chen-yang， Peng Cheng， Zhang Zhenqian， et al. Localization and map construction method of agricultural robot integrating odometer information［J］.Trans of the Chinese Society of Agricultural Enginee-ring，2021，37（21）：16-23.）

［5]Bazeille S， Filliat D. Combining odometry and visual loopclosure detection for consistent topo-metrical mapping［J］.RAIRO：Operations Research，2010，44（4）：365-377.

［6]梁明杰，閔華清，羅榮華.基于圖優(yōu)化的同時(shí)定位與地圖創(chuàng)建綜述［J］.機(jī)器人，2013，35（4）：500-512.（Liang Mingjie， Min Huaqing， Luo Ronghua. Graph-based SLAM：a survey［J］.Robot，2013，35（4）：500-512.）

［7]Grisetti G， Kyummerle R， Stachniss C， et al. A tutorial on graph-based SLAM［J］.IEEE Intelligent Transportation Systems Magazine，2010，2（4）：31-43.

［8]張洪華，劉璇，陳付豪，等.基于圖優(yōu)化的SLAM后端優(yōu)化研究與發(fā)展［J］.計(jì)算機(jī)應(yīng)用研究，2019，36（1）：11-17.（Zhang Honghua， Liu xuan， Chen Fuhao， et al. Research and development of SLAM back-end optimization based on graph optimization［J］.Application Research of Computers，2019，36（1）：11-17.）

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

［10]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.

［11]Grisetti G， Stachniss C， Grzonka S， et al. A tree parameterization for efficiently computing maximum likelihood maps using gradient descent［C］//Proc of Conference on Robotics：Science and Systems.Boston：MIT Press，2007：3-9.

［12]Dellaert F， Kaess M. Square root SAM： simultaneous localization and mapping via square root information smoothing［J］.The International Journal of Robotics Research，2006，25（12）：1181-1203.

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

［14]Kaess M， Johannsson H， Roberts R， et al. iSAM2： incremental smoothing and mapping using the Bayes tree［J］.The International Journal of Robotics Research，2012，31（2）：216-235.

［15]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.

［16]Kümmerle R， Grisetti G， Strasdat H， et al. G2o： a general framework for graph optimization［C］//Proc of IEEE International Conference on Robotics and Automation.Piscataway，NJ：IEEE Press，2011：3607-3613.

［17]Carlone L， Aragues R， Castellanos J A， et al. A linear approximation for graph-based simultaneous localization and mapping［C］//Proc of Conference on Robotics：Science and Systems.Boston：MIT Press，2012：41-48.

［18]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.

［19]Nasiri S M， Moradi H， Hosseini R. A linear least square initialization method for 3D pose graph optimization problem［C］//Proc of IEEE International Conference on Robotics and Automation.Piscataway，NJ：IEEE Press，2018：2474-2479.

［20]Carlone L， Dellaert F. Duality-based verification techniques for 2D SLAM［C］//Proc of IEEE International Conference on Robotics and Automation.Piscataway，NJ：IEEE Press，2015：4589-4596.

［21]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.

［22]Rosen D M， Carlone L， Bandeira A S， et al. SE-Sync：a certifiably correct algorithm for synchronization over the special Euclidean group［J］.The International Journal of Robotics Research，2019，38（2-3）：95-125.

［23]Eriksson A， Olsson C， Kahl F， et al. Rotation averaging and strong duality［C］//Proc of IEEE Conference on Computer Vision and Pattern Recognition.Piscataway，NJ：IEEE Press，2018：127-135.

［24]Singer A. Angular synchronization by eigenvectors and semidefinite programming［J］.Applied amp; Computational Harmonic Analysis，2011，30（1）：20-36.

［25]Singer A， Shkolnisky Y. Three-dimensional structure determination from common lines in cryo-EM by eigenvectors and semidefinite programming［J］.SIAM Journal on Imaging Sciences，2011，4（2）：543-572.

［26]Arrigoni F， Rossi B， Fusiello A. Spectral synchronization of multiple views in SE（3）［J］.SIAM Journal on Imaging Sciences，2016，9（4）：1963-1990.

［27]Eriksson A， Olsson C， Kahl F， et al. Rotation averaging with the chordal distance： global minimizers and strong duality［J］.IEEE Trans on Pattern Analysis and Machine Intelligence，2019，43（1）：256-268.

［28]Jbilou K. An Arnoldi based algorithm for large algebraic Riccati equations［J］.Applied Mathematics Letters，2005，19（5）：437-444.

收稿日期：2022-02-22；

修回日期：2022-05-09

基金項(xiàng)目：上海市“科技創(chuàng)新行動(dòng)計(jì)劃”國(guó)內(nèi)科技合作項(xiàng)目（20015801100）

作者簡(jiǎn)介：李雨潔（1997-），女，山東淄博人，碩士研究生，主要研究方向?yàn)橐曈X(jué)SLAM；魏國(guó)亮（1973-），男（通信作者），河南新鄉(xiāng)人，教授，博導(dǎo)，博士，主要研究方向?yàn)榉蔷€性系統(tǒng)優(yōu)化、機(jī)器視覺(jué)、隨機(jī)系統(tǒng)（guoliang.wei@usst.edu.cn）；蔡潔（1996-），女，江蘇徐州人，博士研究生，主要研究方向?yàn)橐曈X(jué)SLAM；王苗苗（1997-），女，江蘇徐州人，碩士研究生，主要研究方向?yàn)橐曈X(jué)SLAM后端優(yōu)化.