季晨　宋燕燕　秦軍　敖甜甜

摘 ?要：由于SLAM（Simultaneous Localization and Mapping）算法能夠在陌生環(huán)境中進行高精準度的實時定位以及對當前環(huán)境進行重建地圖的特點，SLAM技術逐漸成為當前研究熱點。本文主要研究基于圖優(yōu)化的同時定位與地圖創(chuàng)建，即SLAM創(chuàng)建中非線性圖優(yōu)化的算法。在基于圖優(yōu)化的SALM問題中，最主要的就是解決非線性最小二乘問題。本文對非線性最小二乘問題的算法和常見的非線性優(yōu)化方案進行闡述與分析，分析最速下降法、高斯—牛頓法、列文伯格—馬夸爾特法的原理和步驟，總結比較三種方法的特征和缺點，在SLAM框架中選擇最適合的優(yōu)化算法。

關鍵詞：圖優(yōu)化;非線性最小二乘;SLAM

中圖分類號：TP391.41 ? ? 文獻標識碼：A

Research on Simultaneous Localization and Mapping based on

Nonlinear Optimization Algorithm

JI Chen， SONG Yanyan， QIN Jun， AO Tiantian

（Communication University of China， Nanjing， Nanjing 211172， China）

592619059@qq.com; sophiesong1231@163.com; 1059182465@qq.com; 986952863@qq.com

Abstract： This paper studies nonlinear graph optimization algorithm in Simultaneous Localization and Mapping （SLAM）. In the SLAM problem， the major issue is to solve the nonlinear least squares problem. In this paper， algorithm of nonlinear least squares problem and common nonlinear optimization schemes are analyzed， such as the principles and steps of the steepest descent method， Gauss Newton method and Levenberg-Marquardt method. Characteristics and shortcomings of the three methods are compared with each other， and the most suitable optimization algorithm in the SLAM framework is selected.

Keywords： graph optimization; nonlinear least squares; SLAM

1 ? 引言（Introduction）

由于SLAM（Simultaneous Localization and Mapping）算法能夠在陌生環(huán)境中進行高精準度的同時定位以及對當前環(huán)境進行重建地圖的特點，SLAM技術逐漸成為當前研究熱點。對當下研究熱點SLAM技術中的圖優(yōu)化算法進行研究[1-3]， SLAM模型由運動模型和觀測模型組成，最大似然估計模型作為SLAM模型的“虛擬觀測模型”，可以近似于最小二乘問題[4]。由于噪聲的存在，運動方程和觀測方程會對測量結果有偏差，需要在有噪聲的數(shù)據(jù)中進行準確的狀態(tài)估計，從噪聲數(shù)據(jù)中恢復出有用信息，優(yōu)化處理噪聲數(shù)據(jù)，深入到圖優(yōu)化。圖優(yōu)化技術最重要的就是解決最小二乘問題。鑒于此，本文通過最速下降法、高斯—牛頓法（Gauss-Newton）和列文伯格—馬夸爾特方法（Levenberg-Marquardt，LM）的推導原理，可知迭代法是解決最小二乘問題的核心，運用不同的迭代思路，加快求解的速度、保證收斂性是圖優(yōu)化的方向[5，6]。

2 ? SLAM算法原理（Principle of SLAM algorithm）

SLAM模型由兩個部分組成：運動模型和觀測模型。由于實際操作中往往存在觀測噪聲，我們通過觀測信息所得到的數(shù)據(jù)通常不具備一致性，并且利用運動方程和觀測方程進行的計算對測量結果存在差異。

2.1 ? 運動模型和觀測模型

運動模型用于描述相機的運動狀態(tài)，影響SLAM模型中相機位置姿態(tài)的準確性。通常用運動方程計算得出。運動模型方程可簡化為式（1）：

（1）

式中，含義為t時刻時，ut為相機的位移，xt和xt-1指這一時刻和上一時刻相機的位姿，wt是噪聲[5]。

觀測模型用于描述相機對待測環(huán)境的感知狀態(tài)，影響SLAM模型中重建地圖的精準度。由于觀測模型受諸多方面的影響，其中觀測傳感器的類型和傳感器噪聲導致隨機誤差較多，該誤差一般符合正態(tài)分布。對于普通傳感器，通用的觀測模型方程為式（2）：

（2）

式中，zt，j表示t時刻觀測傳感器返回的數(shù)據(jù)，yj為環(huán)境地圖中觀測到的第j個路標的位姿，xt為t時刻相機的位姿，vt，j為傳感器噪聲誤差，函數(shù)h表示實際情況與傳感器數(shù)據(jù)的關系，由傳感器類型與原理決定[5]。

2.2 ? 最大似然估計模型

對于相機位置姿態(tài)之間的相關性可以視為一種“虛擬觀測”[4]。我們可以根據(jù)這一種觀測得到的一系列數(shù)據(jù)對相機的狀態(tài)（包括位置姿態(tài)的連續(xù)數(shù)據(jù)）作最大似然估計（Maximize Likelihood Estimation，MLE）：是一種使用概率模型求估計量的方法，最終目的是找到以較高概率產(chǎn)生觀測數(shù)據(jù)的系統(tǒng)發(fā)生樹。我們可以利用這一原理，首先找到相機在當前位置姿態(tài)下會產(chǎn)生的觀測數(shù)據(jù)，利用所知的觀測數(shù)據(jù)和最大似然估計可以得到“相機在什么樣的條件下，最有可能產(chǎn)生當前得到的觀測數(shù)據(jù)”。

用P={Pi}表示相機的位置姿態(tài)集合，用T={}表示相機位置姿態(tài)之間的相對變化集合，則在設立一定的觀測信息T條件下，對P作最大似然估計可表示為

（3）

=//獨立性假設 ? ? ? ? ? ? （4）

=//馬爾可夫性 ? ? ? ? ? （5）

=//符號替換 ? ? ? ? ? ? （6）

其中，E表示相機位置姿態(tài)改變時，相鄰位置姿態(tài)相對變化的概率分布的集合。式（4）使用了條件獨立假設，即假定在所設置的一定的相機姿態(tài)信息的條件下左右的觀測之間都是相互獨立。式（5）應用了馬爾可夫性，PR（m）組成了Tm的馬爾可夫鄰域[4]。由于每個約束Tij（即每個相鄰位置姿態(tài)相對變化的概率分布）只與兩個位置姿態(tài)節(jié)點Pi和Pj相關，簡化后，可以得到式（6）。

3 ?非線性最小二乘問題（Nonlinear least squares problem）

SLAM中的圖優(yōu)化問題我們可以看成是非線性最小二乘問題。最小二乘問題求解非線性優(yōu)化的主要思想為：不通過求解問題全局的最優(yōu)解，而是通過求解局部的最優(yōu)解，再利用不斷迭代去接近全局找到最優(yōu)解作為求解結果。

若假定相機的位置姿態(tài)滿足Pi和Pj的條件，此時Tij服從均值為-Pi（符號表示位姿復合的逆操作）、方差為Σij的高斯分布，即Tij～N（Pj-Pi，Σij），再對P作最大似然估計，可以得到等價于非線性最小二乘問題：

（7）

（8）

由此可以看出，要想解決非線性最小二乘問題，首先要將非線性最小二乘問題轉換成線性最小二乘問題，再通過不斷迭代求出最優(yōu)解[4]。

假設對F（x）=這個函數(shù)求解，過程如下。

設置初始迭代點：xk，k=1;

第一步 線性化：把fi（x）在xk處用泰勒展開，高階項忽略不計，函數(shù)可以寫成為φi（x），此時φi（x）就是fi（x）在x=xk處的線性相關函數(shù)，可以近似代表fi（x）。

第二步 求導：我們已經(jīng)把函數(shù)F（x）里面的所有非線性函數(shù)項都轉化成了線性項，就可以進一步轉換成線性最小二乘問題，設定ψ（x）=，再用ψ（x）近似F（x）。

此時我們就可以對ψ（x）求導，即令ψ'（x）=0，求解x，得到下一次的最優(yōu)點x=xk+1，再用所求的新的最優(yōu)點xk+1為基礎，把F（x）在xk+1處泰勒展開，循環(huán)執(zhí)行以上兩個步驟進行迭代。

迭代停止條件：迭代停止的條件有很多種，例如當xk+1-x<閾值滿足時，就可認為xk+1使F（x）接近極小值，迭代停止。其中閾值決定了x估計值的精確度以及最優(yōu)化過程的迭代次數(shù)。

4 ? 非線性優(yōu)化方案（Nonlinear optimization scheme）

非線性優(yōu)化最常見的方案是有最速下降法、高斯—牛頓法、列文伯格—馬夸爾特方法等。在做最優(yōu)化計算時，需要提供變量的初始值。由于目標函數(shù)復雜，對問題提供不同的初始值都容易陷入局部最小值。

4.1 ? 最速下降法

最速下降法：利用選取最速下降梯度作為迭代的方向，局部收斂速度可以達到最快，但是整體收斂速度不一定是最快的，而且極容易陷入局部極小值。此方法的優(yōu)點在于收斂速度比較穩(wěn)定。

最速下降法步驟為：

（1）給定初始點x0。

（2）若足夠小，停止迭代，否則計算。

（3）由線性搜索計算步長。

（4）令，k：=k+1，繼續(xù)（2）。

迭代公式：

（為迭代步長，為梯度方向）

因為最速下降法是從局部出發(fā)找最優(yōu)解，其下降速率就只能在局部中體現(xiàn)，從整體收斂速度來看反而是比較慢的。

4.2 ? 高斯—牛頓法

高斯—牛頓法：將目標函數(shù)在初始點進行泰勒一階展開，本質上則是步長為1的梯度下降法，優(yōu)點在于計算雅各比矩陣代替海森矩陣，減少計算量。但是高斯—牛頓法依賴于展開點的選取。

以如下非線性最小二乘問題作為例子：

（9）

首先將進行一階的泰勒展開：

（10）

帶到式（9）中：

（11）

我們對上式進行求導，可得：

（12）

令H=JTJ，B=-JTf，代入式（12）得到：

（13）

求解式（13）得調整增量，所以可得高斯—牛頓法的步驟為：

（1）給定初始值x0。

（2）對第k次迭代，計算出雅克比矩陣J，矩陣H、B;根據(jù)式（13）計算增量。

（3）如果足夠小，就停止迭代，否則。

（4）循環(huán)（2）（3）兩步驟，直到最大循環(huán)次數(shù)，或者滿足迭代停止條件。

從算法中可以看出，增量方程求解占據(jù)主要位置。必須滿足近似H矩陣是正定且可逆的。當初始值和所求解相差較大時，高斯—牛頓法就無法確保是收斂的了。當出現(xiàn)近似奇異的時候，高斯—牛頓法也不能收斂。即使排除這一情況，在求步長時，若它過大，會使得局部近似存在較大偏差，也無法保證它的收斂性[6]。

4.3 ? 列文伯格—馬夸爾特方法

列文伯格—馬夸爾特方法：是將最速下降法與高斯—牛頓法相結合的算法，通過設置阻尼參數(shù)調節(jié)減少步長。LM算法收斂速度快，能夠避免算法不收斂，對初值依賴較小。

高斯—牛頓法在展開點附近利用泰勒展開才會有相對較好的近似結果，而LM算法則給添加了一個信賴區(qū)域（Trust Region）用于提高準確性。我們可以使用來衡量泰勒近似效果，作為調整信賴區(qū)域大小的依據(jù)：

上式中的分子表示函數(shù)實際的下降值，分母表示近似模型的下降值。若約等于1則近似效果好;若過小，則縮小信賴區(qū)域;若過大，則放大信賴區(qū)域。

列文伯格—馬夸爾特的求解步驟如下：

（1）給定初始值，，v0=2。

（2）求解梯度，如果小于或等于閾值，則退出;否則繼續(xù)。

（3）通過求解，如果足夠小，就停止迭代;否則繼續(xù)。

（4）令，計算。

（5）如果>0，則，，

vk+1=2;否則，，重復步驟（2）。

（6）判斷收斂性，若不收斂，則返回（2）;若收斂，則停止。

與高斯—牛頓法相比較，LM法增加了一項。當較小時，H占主要部分，由此我們可以得知二次近似模型在該區(qū)域范圍內是可以運用的;當較大時，將成為主要部分，此時就更接近于最速下降法，由此可知此區(qū)域附近的二次近似是不能運用的[6，7]。LM法是一種非常高效的迭代算法，該算法的許多變量決定了迭代的效率和結果的收斂性，而這些參數(shù)的理想值往往依目標函數(shù)不同而不同，LM法要做矩陣求逆，運算量過大，所以我們必須要用其他方法求解數(shù)據(jù)量過大的問題。列文伯格—馬夸爾特法避免了一定程度的線性方程組的非奇異問題和病態(tài)問題，提供了更穩(wěn)定的求解運算，提高了增量的準確性。

4.4 ? 算法分析與總結

根據(jù)對三種算法的原理和推導，我們可以對其作出分析和總結，詳見表1。

5 ? 結論（Conclusion）

本篇是基于SLAM算法中非線性優(yōu)化問題展開的核心算法學習。主要介紹了SLAM算法中的運動模型和觀測模型以及最大似然估計模型，以及由此引出的非線性最小二乘問題。我們從解決最小二乘問題入手，進一步了解非線性優(yōu)化的基本算法，通過對最速下降法、高斯—牛頓法及列文伯格—馬夸爾特方法的學習，總結分析了三種方法的思路和不足之處。最速下降法由于步長難以確定且線性搜索法的計算量過大，在SLAM技術中并不能起到很好的效果。高斯—牛頓法對H矩陣要求為正定的，其他情況都會造成偏差甚至不收斂，需要根據(jù)情況適用。列文伯格—馬夸爾特方法增加了一個信賴區(qū)域，雖然下降速度會有所降低，但是可以通過計算在最速下降法和高斯—牛頓法之間擇優(yōu)選擇，是解決最小二乘問題最常用的方法。

參考文獻（References）

[1] Liu L.， Wang Y.， Zhao L.， et al. Evaluation of Different SLAM Algorithms using Google Tangle Data[M]. Proceedings of the 2017 12th Ieee Conference on Industrial Electronics and Applications， 2017： 1954-1959.

[2] Mur-Artal Raul， Tardos Juan D. Visual-Inertial Monocular SLAM With Map Reuse[J].IEEE ROBOTICS AND AUTOMATION LETTERS， 2017， 2（2）： 769-803.

[3] Saeedi Sajad，Trentini Michael， Seto， et al. Multiple-Robot Simultaneous Localization and Mapping： A Review[J]. JOURNAL OF FIELD ROBOTICS， 2016， 33（1）： 3-46.

[4] 梁明杰，閔華清，羅榮華.基于圖優(yōu)化的同時定位與地圖創(chuàng)建綜述[J].機器人，2013，35（04）：500-512.

[5] 翁瀟文.基于圖優(yōu)化的移動機器人SLAM算法研究[D].華南理工大學，2019.

[6] 張麗麗.最小二乘問題的算法與應用研究[D].華北電力大學，2017.

[7] 徐子鋒，石超，王永鋒，等.基于ORB+PROSAC誤匹配剔除算法的視覺SLAM研究[J].軟件工程，2019，22（5）：9-14.

作者簡介：

季 ? 晨（1999-），女，本科生.研究領域：數(shù)字媒體技術.

宋燕燕（1978-），女，碩士，副教授.研究領域：虛擬現(xiàn)實技術.

秦 ? 軍（1956-），女，本科，教授.研究領域：數(shù)字媒體技術.

敖甜甜（1998-），女，本科生.研究領域：數(shù)字媒體技術.