自動(dòng)駕駛中建圖定位的線性優(yōu)化問(wèn)題解析

一、優(yōu)化的“本質(zhì)”

先從濾波講起~

SLAM首先是一個(gè)狀態(tài)估計(jì)問(wèn)題：在前端中，我們需要從k-1時(shí)刻的狀態(tài)量 出發(fā)，推斷出k時(shí)刻的狀態(tài) ，最簡(jiǎn)單的情況下，狀態(tài)僅包含位姿，如果用四元數(shù)表達(dá)旋轉(zhuǎn)的話，狀態(tài)向量就是7維的（position 3 + orientation 4)。我們用來(lái)完成狀態(tài)估計(jì)的信息有：k-1時(shí)刻到k時(shí)刻之間的運(yùn)動(dòng)測(cè)量 和k時(shí)刻的環(huán)境觀測(cè) 。。 上面部分，我們構(gòu)建了兩個(gè)時(shí)刻間的優(yōu)化問(wèn)題，并可以用各種優(yōu)化策略取去求解。但相比于濾波方法，這并沒(méi)有什么優(yōu)越性，其精度和效率都是等價(jià)的。

優(yōu)化方法真正的優(yōu)勢(shì)在于它可以考慮更多時(shí)刻的信息，從而大大提升精度。這里得提一下貝葉斯濾波框架的局限：貝葉斯濾波方法依賴馬爾可夫性，也即每一時(shí)刻的狀態(tài)僅與上一時(shí)刻有關(guān)，這就決定了上一時(shí)刻之前的測(cè)量信息將被丟掉。 優(yōu)化方法則可以考慮所有時(shí)刻的測(cè)量信息，上面部分中，我們僅考慮了兩個(gè)時(shí)刻間的誤差項(xiàng)，現(xiàn)在，我們把所有過(guò)往時(shí)刻（1, 2, ... k-1, k）的誤差項(xiàng)都添加到目標(biāo)函數(shù)中，我們以所有時(shí)刻的狀態(tài)為自變量，求目標(biāo)函數(shù)（也即總體誤差）的最小值，這樣就得到了整體的最優(yōu)狀態(tài)估計(jì)！精度大大提升。當(dāng)然，由于我們考慮了所有時(shí)刻的信息，此時(shí)的維度會(huì)非常大，并且隨著時(shí)間的推移維度將越來(lái)越大！但所幸，目標(biāo)函數(shù)中的單個(gè)誤差項(xiàng)并不是與所有時(shí)刻的狀態(tài)都相關(guān)，所以求解過(guò)程中的相關(guān)矩陣會(huì)呈現(xiàn)出稀疏性，這就允許我們對(duì)高維問(wèn)題實(shí)時(shí)求解。 我們也可以只取相近若干時(shí)刻的狀態(tài)進(jìn)行優(yōu)化（而非所有時(shí)刻），也即構(gòu)建一個(gè)“滑窗”。

在此基礎(chǔ)上，更高級(jí)的做法是，對(duì)滑窗邊緣的那個(gè)時(shí)刻，我們對(duì)其進(jìn)行邊緣化處理以使其能夠融合更早時(shí)刻的信息（而不是把更早時(shí)刻的信息直接丟掉），這樣，我們就構(gòu)建了“滑窗優(yōu)化”，這方面的代表作如VINS。 優(yōu)化作為一種方法，既可以用于前端，也可以用于后端，甚至你想用的任何地方。用于后端時(shí)，通常是全局的優(yōu)化，也即“考慮所有時(shí)刻的狀態(tài)和所有測(cè)量誤差項(xiàng)”的優(yōu)化。發(fā)生閉環(huán)時(shí)，意味著當(dāng)前時(shí)刻的狀態(tài) 和歷史上某一個(gè)時(shí)刻的狀態(tài) 之間構(gòu)建了誤差項(xiàng)，由于位姿漂移，這個(gè)誤差項(xiàng)往往很大！當(dāng)我們?cè)偃?duì)全局誤差進(jìn)行最小化時(shí)，我們就需要“微調(diào)”全局各個(gè)時(shí)刻的狀態(tài)量，以把這個(gè)誤差“分散”到各個(gè)時(shí)刻去，從而實(shí)現(xiàn)全局誤差的最小化。這個(gè)“微調(diào)”的過(guò)程，同樣由優(yōu)化來(lái)完成。 因此，從以上各個(gè)角度來(lái)看，一個(gè)現(xiàn)代的SLAM系統(tǒng)，本質(zhì)上就是一個(gè)優(yōu)化的系統(tǒng)，SLAM本質(zhì)上就是一個(gè)優(yōu)化問(wèn)題。而所有的視覺里程計(jì)，激光里程計(jì)，輪式，imu等等，它們的作用就是構(gòu)建出一個(gè)個(gè)的誤差項(xiàng)。

一個(gè)誤差項(xiàng)，也即一個(gè)約束。

二、線性化與非線性化優(yōu)化問(wèn)題

優(yōu)化問(wèn)題本質(zhì)來(lái)說(shuō)都是最小二乘問(wèn)題，其關(guān)鍵的地方在于：如何設(shè)計(jì)與建立SLAM的problem structure：

除非理想情況下，實(shí)際中都是非線性化問(wèn)題，但是我們不知道非線性模型，無(wú)法求解實(shí)際問(wèn)題，于是我們通過(guò)對(duì)非線性問(wèn)題進(jìn)行線性化分析，忽略一些因素，構(gòu)建線性化方程求解非線性化問(wèn)題。

非線性化：1、通俗來(lái)說(shuō)，非線性優(yōu)化就是求函數(shù)的極值，2、再多說(shuō)一句，在非線性優(yōu)化里面通常求最小值。

1、我們想求一個(gè) 函數(shù)的極值問(wèn)題的時(shí)候，線性函數(shù)是最簡(jiǎn)單的，因?yàn)槭蔷€性的嘛，單調(diào)增或者單調(diào)減，那么找到邊界就可以求到極值。例如 f(x)=ax+b。

2、簡(jiǎn)單的非線性函數(shù)也是很容易求得極值的，例如f(x)=x*x.可以通過(guò)求導(dǎo)得到極值點(diǎn)，然后求得其極值。

3、但是對(duì)于復(fù)雜的非線性函數(shù)，或者復(fù)雜的數(shù)學(xué)模型，求導(dǎo)很困難或者無(wú)法求導(dǎo)的時(shí)候怎么求極值呢？那么就出現(xiàn)了很多非線性優(yōu)化的算法。來(lái)解決對(duì)于復(fù)雜數(shù)學(xué)模型的求極值的問(wèn)題。

三、全局優(yōu)化與局部?jī)?yōu)化

全局優(yōu)化的英文是（global optimization） 1、全局優(yōu)化是找到在整個(gè)可行區(qū)域內(nèi)使目標(biāo)函數(shù) 最小的 可行點(diǎn)x的問(wèn)題。通俗來(lái)說(shuō)就是從所有可能的x值里面找到最小值 2、通常，可能是一個(gè)非常困難的問(wèn)題，隨著參數(shù)數(shù)量的增加，難度將成倍增加。比如上面的例子是x1和x2兩個(gè)參數(shù)，有著不同的組合方式。那么如何有x1~x100個(gè)參數(shù)呢，會(huì)有多少種組合方式呢？

3、實(shí)際上，除非知道有關(guān) f 的特殊信息，否則甚至無(wú)法確定是否找到了真正的全局最優(yōu)值，因?yàn)榭赡軙?huì)出現(xiàn) f 值的突然下降，且這個(gè)值隱藏在您尚未查找的參數(shù)空間中。 局部?jī)?yōu)化的英文是（local optimization） 1、局部?jī)?yōu)化是一個(gè)容易得多的問(wèn)題。它的目標(biāo)是找到一個(gè)僅是局部最小值的可行點(diǎn)x：f（x）小于或等于所有附近可行點(diǎn)的f值 2、通常，非線性優(yōu)化問(wèn)題可能有很多局部最小值，算法確定的最小值位置通常取決于用戶提供給算法的起點(diǎn)。3、另一方面，即使在非常高維的問(wèn)題中（尤其是使用基于梯度的算法），局部?jī)?yōu)化算法通常也可以快速定位局部最小值。

四、優(yōu)化使用的方法

具體的優(yōu)化求解策略包括最速下降法、牛頓法、高斯-牛頓法（G-N），基于信任區(qū)域的L-M法等。

1、梯度下降法

函數(shù)的下降方向?qū)⒂肋h(yuǎn)為函數(shù)的負(fù)梯度方向 梯度下降法是一個(gè)一階最優(yōu)化算法，通常也稱為最速下降法。要使用梯度下降法找到一個(gè)函數(shù)的局部極小值，必須向函數(shù)上當(dāng)前點(diǎn)對(duì)應(yīng)梯度（或者是近似梯度）的反方向的規(guī)定步長(zhǎng)距離點(diǎn)進(jìn)行迭代搜索。因此指保留一階梯度信息。缺點(diǎn)是過(guò)于貪心，容易走出鋸齒路線。

2、牛頓法 牛頓法是一個(gè)二階最優(yōu)化算法，基本思想是利用迭代點(diǎn)處的一階導(dǎo)數(shù)(梯度)和二階導(dǎo)數(shù)(Hessen矩陣)對(duì)目標(biāo)函數(shù)進(jìn)行二次函數(shù)近似。因此保留二階梯度信息。缺點(diǎn)是需要計(jì)算H矩陣，計(jì)算量太大。

3、高斯牛頓法

其實(shí)其就是使用上式，對(duì)牛頓法的H矩陣進(jìn)行替換

有可能為奇異矩陣或變態(tài)，Δx也會(huì)造成結(jié)果不穩(wěn)定，因此穩(wěn)定性差

4、LM信任區(qū)域法

但是這個(gè)近似假設(shè)的成立是有一定程度限制的，我們可以設(shè)立一個(gè)正值變量△使得模型在一個(gè)以x為圓心為半徑的圓中被視為可以精確近似。這個(gè)圓就是我們所說(shuō)的信任區(qū)域。然后我們就可以基于以下公式求解變化值h。 LM算法大致與高斯牛頓算法理論相同，但是不同于高斯牛頓算法，LM算法是基于信賴區(qū)域理論(Trust Region Method)進(jìn)行計(jì)算的。這是因?yàn)楦咚古ｎD法中的泰勒展開只有在展開點(diǎn)附近才會(huì)有比較好的效果，因此為了確保近似的準(zhǔn)確性我們需要設(shè)定一個(gè)具有一定半徑的區(qū)域作為信賴區(qū)域。

基于信賴區(qū)域我們能夠重新構(gòu)建一個(gè)更有效的優(yōu)化框架 采用信賴區(qū)域法我們就需要明確該區(qū)域該怎么確定。在LM算法中信賴區(qū)大小的確定也是運(yùn)用增益比例來(lái)進(jìn)行判定的。

四、非線性優(yōu)化解退化問(wèn)題

對(duì)于SLAM而言，視覺傳感器需要紋理信息決定特征點(diǎn)，距離傳感器需要空間幾何結(jié)構(gòu)信息決定特征點(diǎn)，在特征點(diǎn)缺乏時(shí)，狀態(tài)估計(jì)方法會(huì)進(jìn)行退化。定位的退化主要是因?yàn)榧s束的減少，比如NDT需要三個(gè)正交方向的約束才能很好的匹配，但若在狹長(zhǎng)的走廊上或者隧道環(huán)境，條件單一，即使人肉眼觀看激光雷達(dá)數(shù)據(jù)，也很難判斷機(jī)器人所處的位置。因?yàn)榧す狻翱吹健钡沫h(huán)境都是一樣的，在隧道方向是沒(méi)有約束的；并且在隧道中GPS信號(hào)是沒(méi)的，只能依靠高精度的姿態(tài)傳感器和輪速機(jī)的融合來(lái)做，當(dāng)然隨著運(yùn)行時(shí)間的增加，誤差會(huì)慢慢增加。 處理退化的常用方法有: 1）當(dāng)出現(xiàn)退化時(shí)換一種方法;這要求在設(shè)計(jì)時(shí)需要一個(gè)備用方法可用 2）在狀態(tài)估計(jì)過(guò)程中加入人工約束，如恒速模型的約束。

即使問(wèn)題本身是可解決的情況下也會(huì)帶來(lái)不必要的誤差。我們的方法是在原始問(wèn)題的部分子空間添加約束。原始問(wèn)題的解可以分為非退化方向和退化方向，在處理問(wèn)題時(shí)，首先確定退化方向，然后將求出原始問(wèn)題非退化方向的解，對(duì)于退化方向的解使用猜測(cè)值。在最后的實(shí)踐算法中，只使用非線性優(yōu)化解在非退化方向的分量，不考慮退化方向的分量。 正常情況下，約束應(yīng)該是分布在空間中的多個(gè)方向，從各個(gè)角度約束解，如下面所示，綠色點(diǎn)表示問(wèn)題的解，被非退化約束限制在了一個(gè)小區(qū)域。這樣的解就比較準(zhǔn)確，約束發(fā)生小變化時(shí)，解的變化也很小，被限制在一個(gè)局部區(qū)域，這樣的解就是一個(gè)比較理想的解。

如果解的約束大多近似平行，那么他們就是退化的方向（藍(lán)色箭頭表示的方向），這個(gè)時(shí)候解在退化方向收到的約束就很差?？紤]絕對(duì)平行時(shí)，解的約束也是一個(gè)平行的方向，那么這種情況下的解是需要避免的，如果其中一個(gè)約束發(fā)生了小的偏移，那么解所在的局部區(qū)域會(huì)發(fā)生較大的變化，這樣的解是比較糟糕的解。

每次求解時(shí) 優(yōu)化解=原始解在非退化方向投影+估計(jì)值在退化方向投影（實(shí)際算法中可以將退化方向解丟棄，只考慮非退化方向） ?

編輯：黃飛

?