非線性?xún)?yōu)化同時(shí)定位與地圖創(chuàng)建問(wèn)題

季晨　宋燕燕　秦軍　敖甜甜

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

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

中圖分類(lèi)號(hào)：TP391.41 ? ? 文獻(xiàn)標(biāo)識(shí)碼：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)境中進(jìn)行高精準(zhǔn)度的同時(shí)定位以及對(duì)當(dāng)前環(huán)境進(jìn)行重建地圖的特點(diǎn)，SLAM技術(shù)逐漸成為當(dāng)前研究熱點(diǎn)。對(duì)當(dāng)下研究熱點(diǎn)SLAM技術(shù)中的圖優(yōu)化算法進(jìn)行研究[1-3]， SLAM模型由運(yùn)動(dòng)模型和觀測(cè)模型組成，最大似然估計(jì)模型作為SLAM模型的“虛擬觀測(cè)模型”，可以近似于最小二乘問(wèn)題[4]。由于噪聲的存在，運(yùn)動(dòng)方程和觀測(cè)方程會(huì)對(duì)測(cè)量結(jié)果有偏差，需要在有噪聲的數(shù)據(jù)中進(jìn)行準(zhǔn)確的狀態(tài)估計(jì)，從噪聲數(shù)據(jù)中恢復(fù)出有用信息，優(yōu)化處理噪聲數(shù)據(jù)，深入到圖優(yōu)化。圖優(yōu)化技術(shù)最重要的就是解決最小二乘問(wèn)題。鑒于此，本文通過(guò)最速下降法、高斯—牛頓法（Gauss-Newton）和列文伯格—馬夸爾特方法（Levenberg-Marquardt，LM）的推導(dǎo)原理，可知迭代法是解決最小二乘問(wèn)題的核心，運(yùn)用不同的迭代思路，加快求解的速度、保證收斂性是圖優(yōu)化的方向[5，6]。

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

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

2.1 ? 運(yùn)動(dòng)模型和觀測(cè)模型

運(yùn)動(dòng)模型用于描述相機(jī)的運(yùn)動(dòng)狀態(tài)，影響SLAM模型中相機(jī)位置姿態(tài)的準(zhǔn)確性。通常用運(yùn)動(dòng)方程計(jì)算得出。運(yùn)動(dòng)模型方程可簡(jiǎn)化為式（1）：

（1）

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

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

（2）

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

2.2 ? 最大似然估計(jì)模型

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

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

（3）

=//獨(dú)立性假設(shè) ? ? ? ? ? ? （4）

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

=//符號(hào)替換 ? ? ? ? ? ? （6）

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

3 ?非線性最小二乘問(wèn)題（Nonlinear least squares problem）

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

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

（7）

（8）

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

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

設(shè)置初始迭代點(diǎn)：xk，k=1;

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

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

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

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

4 ? 非線性?xún)?yōu)化方案（Nonlinear optimization scheme）

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

4.1 ? 最速下降法

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

最速下降法步驟為：

（1）給定初始點(diǎn)x0。

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

（3）由線性搜索計(jì)算步長(zhǎng)。

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

迭代公式：

（為迭代步長(zhǎng)，為梯度方向）

因?yàn)樽钏傧陆捣ㄊ菑木植砍霭l(fā)找最優(yōu)解，其下降速率就只能在局部中體現(xiàn)，從整體收斂速度來(lái)看反而是比較慢的。

4.2 ? 高斯—牛頓法

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

以如下非線性最小二乘問(wèn)題作為例子：

（9）

首先將進(jìn)行一階的泰勒展開(kāi)：

（10）

帶到式（9）中：

（11）

我們對(duì)上式進(jìn)行求導(dǎo)，可得：

（12）

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

（13）

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

（1）給定初始值x0。

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

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

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

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

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

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

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

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

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

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

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

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

（4）令，計(jì)算。

（5）如果>0，則，，

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

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

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

4.4 ? 算法分析與總結(jié)

根據(jù)對(duì)三種算法的原理和推導(dǎo)，我們可以對(duì)其作出分析和總結(jié)，詳見(jiàn)表1。

5 ? 結(jié)論（Conclusion）

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

參考文獻(xiàn)（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)化的同時(shí)定位與地圖創(chuàng)建綜述[J].機(jī)器人，2013，35（04）：500-512.

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

[6] 張麗麗.最小二乘問(wèn)題的算法與應(yīng)用研究[D].華北電力大學(xué)，2017.

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

作者簡(jiǎn)介：

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

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

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

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