SLAM領(lǐng)域中的VIO（視覺慣性里程計(jì)）

筆者在閑暇之余學(xué)習(xí)了SLAM相關(guān)的內(nèi)容，深感機(jī)器人狀態(tài)估計(jì)領(lǐng)域的博大精深。今天，筆者就拋磚引玉，談?wù)凷LAM領(lǐng)域中的VIO（視覺慣性里程計(jì)）。

初始SLAM

SLAM(Simultaneous Localization and Mapping)，是指搭載特定傳感器的主體，在沒有環(huán)境先驗(yàn)信息的情況下，于運(yùn)動(dòng)過程中建立環(huán)境的模型，同時(shí)估計(jì)自己的運(yùn)動(dòng)。SLAM可以解決自主運(yùn)動(dòng)的兩大基本問題：

我在什么地方？——定位

周圍環(huán)境是什么樣子？——建圖

定位與建圖=內(nèi)外兼修，定位側(cè)重對(duì)自身的了解，建圖側(cè)重對(duì)外在的了解。

為什么用VIO？

單目視覺SLAM無法解決尺度問題，而將IMU估計(jì)的位姿序列與相機(jī)估計(jì)的位姿序列進(jìn)行對(duì)齊可以恢復(fù)出相機(jī)軌跡的真實(shí)尺度；

視覺SLAM一般取第一幀作為世界坐標(biāo)系，而IMU中的加速度計(jì)測(cè)量值包括重力向量，可以根據(jù)其將估計(jì)的位姿轉(zhuǎn)換到垂直于地面的世界坐標(biāo)系中；

IMU幀間積分得到的位姿可以預(yù)測(cè)出圖像幀在下一時(shí)刻的位姿以及特征點(diǎn)在下一幀圖像上的位置，并將其作為初值帶入非線性優(yōu)化中，減少優(yōu)化迭代次數(shù)；

IMU測(cè)量可以提高視覺SLAM在某些復(fù)雜場(chǎng)景的魯棒性，在于相機(jī)快速運(yùn)動(dòng)、動(dòng)態(tài)障礙物、光線明暗變化明顯、環(huán)境中紋理缺失等導(dǎo)致圖像信息質(zhì)量較差的情況下，利用IMU信息仍能得到有效的定位結(jié)果。

緊耦合的VIO

根據(jù)狀態(tài)向量中是否加入圖像特征信息來判斷是否為松緊耦合，如圖所示，在VIO中將圖像特征以及IMU的位置、方向、速度作為狀態(tài)，構(gòu)建非線性優(yōu)化問題進(jìn)行求解，采用梯度下降的方法同時(shí)對(duì)圖像特征以及位置、方向、速度進(jìn)行優(yōu)化，從而得到對(duì)機(jī)器人狀態(tài)的最優(yōu)估計(jì)。

VIO的框架

VIO過程可以用上圖進(jìn)行表示，其中，為路標(biāo)點(diǎn)，，,為三個(gè)時(shí)刻相機(jī)的位置點(diǎn)，，，為三個(gè)時(shí)刻IMU的位置點(diǎn)。由于IMU存在著高斯白噪聲以及導(dǎo)數(shù)服從高斯分布的偏置，相機(jī)也存在著光學(xué)畸變誤差、機(jī)械誤差等，因此無論是相機(jī)觀測(cè)到的數(shù)據(jù)還是IMU測(cè)量的數(shù)據(jù)都不可靠。因?yàn)橄鄼C(jī)觀測(cè)的數(shù)據(jù)以及IMU測(cè)量的數(shù)據(jù)具有一定的關(guān)聯(lián)性，基于此構(gòu)建優(yōu)化問題，便可以得到對(duì)真實(shí)路標(biāo)點(diǎn)的最優(yōu)估計(jì)，同時(shí)也可以得到對(duì)IMU或者相機(jī)（機(jī)器人本體）的最優(yōu)估計(jì)。

基于以上原理，VIO的優(yōu)化問題可以表示為：

以上三項(xiàng)分別為邊緣化的先驗(yàn)信息、IMU的測(cè)量殘差、視覺的重投影誤差。其中，視覺的重投影誤差是指像素坐標(biāo)（觀測(cè)到的投影位置）與3D點(diǎn)按照當(dāng)前估計(jì)的位姿進(jìn)行第二次投影得到的位置相比較得到的誤差。邊緣化的先驗(yàn)信息是指邊緣化后的剩余變量更新后的殘差，具體的會(huì)在邊緣化一節(jié)進(jìn)行分析。

邊緣化

要講邊緣化，首先大家要知道實(shí)際使用中SLAM的優(yōu)化方法，它并不是對(duì)所有的觀測(cè)進(jìn)行優(yōu)化，而是會(huì)構(gòu)建一個(gè)窗口，只對(duì)窗口內(nèi)觀測(cè)到的狀態(tài)值進(jìn)行優(yōu)化，這樣可以大大的減少計(jì)算量以及優(yōu)化的時(shí)間。但是由于在移動(dòng)過程中，會(huì)不斷的觀測(cè)到新的狀態(tài)，因此需要讓構(gòu)建的窗口滑動(dòng)起來，從而能夠包換最新觀測(cè)到的狀態(tài)，并且扔掉最早觀測(cè)到的狀態(tài)，這種優(yōu)化方法叫做基于滑動(dòng)窗口的優(yōu)化方法。

但是，由于在連續(xù)的一些時(shí)刻中，相機(jī)可能會(huì)觀測(cè)到用一個(gè)路標(biāo)點(diǎn)，所以不能簡(jiǎn)單的扔掉之前觀測(cè)到的狀態(tài)值，需要其信息全部傳遞到之后的狀態(tài)中。簡(jiǎn)單的說，這是一個(gè)求邊際概率的問題，形如：

b可以指代為需要扔掉的最早觀測(cè)到的狀態(tài)。通過求解邊際概率，最早觀測(cè)到狀態(tài)量信息便傳遞到了該滑動(dòng)窗口剩余的狀態(tài)量中。此后，我們通過講最新觀測(cè)到的狀態(tài)量加入到滑動(dòng)窗口中，便可以進(jìn)行新一輪的非線性優(yōu)化。

講到了這里，我想大家已經(jīng)能夠理解邊緣化的先驗(yàn)誤差是什么了吧，沒錯(cuò)，他就是通過上述求解邊際概率方式更新后的誤差，該誤差加上最新觀測(cè)到的狀態(tài)量的誤差，也就是IMU的測(cè)量誤差以及相機(jī)的觀測(cè)誤差即可構(gòu)建出上一節(jié)等式的優(yōu)化問題。

寫到最后

VIO不僅包含上述的非線性優(yōu)化問題，還有其他的一些問題：

首先是VIO的初始化，初始化的目的是為了恢復(fù)單目相機(jī)的尺度信息，求解IMU的偏置，IMU與相機(jī)之間的轉(zhuǎn)換關(guān)系，并將優(yōu)化的坐標(biāo)系轉(zhuǎn)換到世界坐標(biāo)系下（可根據(jù)重力加速度為g得到z軸）。

除了初始化，還有回環(huán)檢測(cè)問題，基于優(yōu)化的方法依舊會(huì)有誤差，尤其是長(zhǎng)時(shí)間運(yùn)轉(zhuǎn)后，因此可以通過檢測(cè)觀測(cè)到路標(biāo)點(diǎn)是否之前已經(jīng)觀測(cè)到，構(gòu)建回環(huán)，從而增強(qiáng)優(yōu)化問題的約束，避免陷入局部極小值點(diǎn)，保證結(jié)果的最優(yōu)性。

最后，就是建圖模塊了，根據(jù)優(yōu)化后的狀態(tài)量（機(jī)器人本體的位姿以及路標(biāo)點(diǎn)的位置）可以構(gòu)建出地圖，從而用于機(jī)器人的導(dǎo)航。