視覺慣性融合技術(shù)應(yīng)用于機(jī)器人和自動駕駛方面。單目相機(jī)和低成本IMU成為最佳選擇。因?yàn)閂IO系統(tǒng)的高度非線性，初值對視覺慣性系統(tǒng)很重要,但很難獲得精確的初始狀態(tài)。VIO初始化的主要意義有：（1）對齊相機(jī)的世界坐標(biāo)系和慣性系，因此需要估計(jì)重力方向（2）為后續(xù)的VIO算法提供較為準(zhǔn)確的初始參數(shù)和狀態(tài)（尺度、IMU bias、初始速度）,使得算法可以快速穩(wěn)定地進(jìn)入正常運(yùn)行階段。良好的初始化對VIO算法的整體精度和魯棒性至關(guān)重要。本文旨在介紹主流的VIO初始化方案

1 前言

目前的初始化方法分為松耦合（Vins-Mono、ORBSLAM3）和緊耦合方案（OpenVINS），松耦合方案用的較多，并且表現(xiàn)更好。從求解方法上，有濾波、圖優(yōu)化、閉式解三種方法。但是,濾波會很早將狀態(tài)邊緣化,會失去最優(yōu)性。圖優(yōu)化由于計(jì)算復(fù)雜性高。大多數(shù)現(xiàn)有的初始化方法需要系統(tǒng)靜止或已知初始狀態(tài)。不能適應(yīng)動態(tài)運(yùn)動、空中初始化。閉合式解法缺乏傳感器噪聲模型。本文主要介紹基于VINS-Mono的松耦合方案。

2 問題概述

已知：（1）IMU 和圖像時(shí)間戳已同步；（2）IMU 和相機(jī)外參數(shù)已知求解：尺度、圖像幀的速度、重力方向、IMU bias

3 方法

3.1 進(jìn)行單目結(jié)構(gòu)重建（SFM）,獲得up-to-scale的相機(jī)姿態(tài)和特征點(diǎn)位置

設(shè)定一定長度的滑動窗口，它維護(hù)了幾個(gè)空間分離的圖像幀?？臻g幀由足夠的視差的圖像幀來選擇。選擇兩幀具有足夠特征視差的幀。然后使用五點(diǎn)法恢復(fù)這兩幀之間的相對旋轉(zhuǎn)和帶尺度的平移。然后任意設(shè)置尺度，對這兩幀中觀察到的所有特征進(jìn)行三角剖分。基于這些三角化特征，采用PnP方法估計(jì)窗口中其他幀的姿態(tài)。

最后，應(yīng)用全局BA最小化所有特征觀測值的總重投影誤差得到所有的幀姿態(tài)和特征位置。已知相機(jī)與IMU之間的外參數(shù)，則所有變量都可以從相機(jī)坐標(biāo)系轉(zhuǎn)換到IMU坐標(biāo)系：

s是未知的尺度。

3.2 對IMU測量進(jìn)行預(yù)積分,得到IMU的相對運(yùn)動約束

預(yù)積分公式為：

分別表示相對位置、速度和旋轉(zhuǎn)約束。可以看出，慣性測量通常被積分以形成與圖像幀的初始位置和速度無關(guān)的相對運(yùn)動約束。

3.3 視覺慣性對準(zhǔn)

我們已經(jīng)得到了視覺SFM和IMU預(yù)積分中獲得了相機(jī)姿勢。本節(jié)將介紹對齊這兩個(gè)軌跡的方法。

陀螺儀bias標(biāo)定

窗口中兩個(gè)連續(xù)的圖像幀bk和bk+1，我們有來自視覺結(jié)構(gòu)的相對旋轉(zhuǎn)以及來自IMU預(yù)積分的相對旋轉(zhuǎn)約束。通過最小化兩者之間的誤差來估計(jì)陀螺儀的bias：

注意每次更新bg，都要重新計(jì)算預(yù)積分量。上面第二個(gè)式子用到了快速更新預(yù)積分量的方法。

估計(jì)速度、重力方向和尺度參數(shù)。

重力方向估計(jì)并強(qiáng)制其模長等于已知值。即需要估計(jì)的變量有

考慮預(yù)積分位置和速度相關(guān)的約束，IMU軌跡在相機(jī)坐標(biāo)系c0下有：

利用外參數(shù)，得到相機(jī)坐標(biāo)系下的相機(jī)軌跡與預(yù)積分的關(guān)系：

將待估計(jì)變量放到方程右邊，有：

其中，

上式中，帶尺度的相機(jī)位置和姿態(tài)已經(jīng)通過視覺SFM獲得。通過求解最小二乘問題：

可以得到視覺坐標(biāo)系下的速度和重力矢量，以及尺度。視覺結(jié)構(gòu)的平移分量將被縮放到米制單位。通過強(qiáng)制范數(shù)約束，估計(jì)的重力將經(jīng)歷另一輪的細(xì)化。

重力方向細(xì)化

上面的優(yōu)化中，并沒有約束重力的大小。通過約束重力矢量的大小，可以對前一步得到的重力矢量進(jìn)行細(xì)化。在大多數(shù)情況下，重力矢量的大小是已知的。但是，如果我們直接將這個(gè)范數(shù)約束加入到上面的優(yōu)化問題中，就會變得非線性，難以求解。在這里，VINS-Mono使用一種方法通過優(yōu)化其切線空間上的二維誤差狀態(tài)來強(qiáng)制執(zhí)行重力范數(shù)。

上圖表示重力方向參數(shù)化示意圖。由于重力的大小是已知的，重力的自由度為2，我們可以在它的切空間上用兩個(gè)變量來參數(shù)化重力。我們將重力參數(shù)化為：

b1和b2是張成切平面的兩個(gè)正交基。w1和w2分別是指向b1和b2的位移。我們可以用Gram-Schmidt過程(一種構(gòu)造單位正交基的方法)很容易地求出b1、 b2的集合。這樣，我們在其切空間上用兩種狀態(tài)重新參數(shù)化重力，然后將g替換為上式，也是線性形式。重新優(yōu)化上面的最小二乘問題，迭代幾次，直到g收斂。在細(xì)化重力矢量之后，我們根據(jù)重力矢量將所有變量從視覺坐標(biāo)系旋轉(zhuǎn)到世界坐標(biāo)系（重力方向和Z軸平行的坐標(biāo)系）：

4 結(jié)論

此時(shí)，初始化過程完成，這些變量將被輸入到一個(gè)緊密耦合的非線性視覺慣性估計(jì)器，即VIO過程。還有幾點(diǎn)需要解釋：對于旋翼飛行器來說，直線運(yùn)動或零加速度運(yùn)動等簡并運(yùn)動是不可避免的。這時(shí)候在SFM窗口內(nèi)的IMU測量可能無法使整個(gè)系統(tǒng)可見，我們可以通過檢查初始化過程中的預(yù)積分的值來拒絕小的加速度運(yùn)動，初始化過程只有在IMU測量中包含足夠的激勵(lì)時(shí)才開始。在初始化過程中，加速度偏差很難標(biāo)定，因?yàn)榧铀俣韧ǔＴ谛⌒D(zhuǎn)下與重力耦合。因此該方法不涉及加速度bias的標(biāo)定。忽略加速度偏差不會對初始化結(jié)果造成顯著的負(fù)面影響，可以在VIO過程中引入，這里不做介紹。

審核編輯：劉清