介紹主流的VIO初始化方案

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

1 前言

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

2 問(wèn)題概述

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

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

s是未知的尺度。

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

預(yù)積分公式為：

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

3.3 視覺(jué)慣性對(duì)準(zhǔn)

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

陀螺儀bias標(biāo)定

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

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

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

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

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

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

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

其中，

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

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

重力方向細(xì)化

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

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

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

4 結(jié)論

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

審核編輯：劉清