作者：小飛飛 | 來源：3DCV

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

1 前言

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

2 問題概述

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

3 方法

3.1 進行單目結構重建（SFM）,獲得up-to-scale的相機姿態(tài)和特征點位置

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

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

s是未知的尺度。

3.2 對IMU測量進行預積分,得到IMU的相對運動約束

預積分公式為：

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

3.3 視覺慣性對準

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

陀螺儀bias標定

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

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

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

重力方向估計并強制其模長等于已知值。即需要估計的變量有

考慮預積分位置和速度相關的約束，IMU軌跡在相機坐標系c0下有：

利用外參數(shù)，得到相機坐標系下的相機軌跡與預積分的關系：

將待估計變量放到方程右邊，有：

其中，

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

可以得到視覺坐標系下的速度和重力矢量，以及尺度。視覺結構的平移分量將被縮放到米制單位。通過強制范數(shù)約束，估計的重力將經歷另一輪的細化。

重力方向細化

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

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

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

4 結論

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

這里也推薦一門「3D視覺工坊」近期剛推出的新課「深度剖析面向機器人領域的3D激光SLAM技術原理、代碼與實戰(zhàn)」