Section 1 VIO 概述
VIO：（Visual-Inertial Odometry）以視覺與 IMU 融合實作裡程計
IMU（Inertial Measurement Unit），慣性測量單元
- 典型 6 軸 IMU 以較高頻率（≥ 100Hz）傳回被測量物體的角速度與加速度。高頻率運動。
-
受自身溫度、零偏、振動等因素幹擾，積分得到的平移和旋轉容易漂移。可通過标定去除幹擾。
六自由度 IMU 本身由一個陀螺儀和一個加速度計組成，分别測量自身的角速度和加速度。
VIO的IMU旋轉運動學與誤差模型和标定
視覺 Visual Odometry
- 以圖像形式記錄資料，頻率較低（15 60Hz 居多）
- 通過圖像特征點或像素推斷相機運動
整體上，視覺和 IMU 定位方案存在一定互補性質：
- IMU 适合計算短時間、快速的運動；
-
視覺适合計算長時間、慢速的運動。
同時，可利用視覺定位資訊來估計 IMU 的零偏，減少 IMU 由零偏導緻的發散和累積誤差；
反之，IMU 可以為視覺提供快速運動時的定位。
Section 2 旋轉運動學
線速度與角速度
粒子在坐标系中 z = h z = h z=h中的平面做圓周運動，其中 a a a為運動半徑，則坐标為: r = ( a c o s θ , a s i n θ , h ) T r = (a cos θ, a sin θ, h)^T r=(acosθ,asinθ,h)T對坐标求導得：
r ˊ = ( − a θ ˊ s i n θ , a θ ˊ c o s θ , 0 ) T \acute{r}=(-a\acute{\theta} sin\theta,a\acute{\theta} cos\theta,0)^T rˊ=(−aθˊsinθ,aθˊcosθ,0)T = [ 0 − θ ˊ 0 θ ˊ 0 0 0 0 0 ] [ a c o s θ a s i n θ h ] = ω × r = \begin{bmatrix} 0 & -\acute{\theta} & 0 \\ \acute{\theta} & 0 & 0\\ 0&0&0 \\ \end{bmatrix} \begin{bmatrix} acos\theta \\ asin\theta\\ h \\ \end{bmatrix} =\omega \times r =⎣⎡0θˊ0−θˊ00000⎦⎤⎣⎡acosθasinθh⎦⎤=ω×r
ω \omega ω為反對稱矩陣，是以 ω \omega ω矢量為 [ 0 , 0 , θ ˊ ] T [0,0,\acute{\theta}]^T [0,0,θˊ]T，其中， ω = θ ˊ z \omega=\acute{\theta}z ω=θˊz， ∣ θ ˊ ∣ |\acute{\theta}| ∣θˊ∣是角速度大小。對上式取模，得： ∣ r ˊ ∣ = ∣ ω ∣ ∣ r ∣ s i n ϕ = ∣ ω ∣ a = a ∣ θ ˊ ∣ |\acute{r}|=|\omega||r|sin\phi=|\omega|a=a|\acute{\theta}| ∣rˊ∣=∣ω∣∣r∣sinϕ=∣ω∣a=a∣θˊ∣
旋轉坐标系下的運動學
品質塊在 body 坐标系下的坐标為： r B = ( x 1 , x 2 , x 3 ) T r_B = (x1, x2, x3)^T rB=(x1,x2,x3)T，旋轉到慣性系下有：
r I ( t ) = x 1 ( t ) i + x 2 ( t ) j + x 3 ( t ) k = R I B r B r_I (t) = x_1(t)i + x_2(t)j + x_3(t)k = R_{IB}r_B rI(t)=x1(t)i+x2(t)j+x3(t)k=RIBrB其中，body frame在慣性坐标系下的表示為 ( i , j , k ) (i,j,k) (i,j,k)
對時間求導有：
r ˊ I = R I B r ˊ B + R ˊ I B r B \acute{r}_I = R_{IB}\acute{r}_B + \acute{R}_{IB}r{B} rˊI=RIBrˊB+RˊIBrB = R I B r ˊ B + [ R I B ω b ] × r I = R_{IB} \acute{r}_B + [R_{IB}ω_b]×r_I =RIBrˊB+[RIBωb]×rI = R I B v B + ω × r I = R_{IB}v_B + ω × r_I =RIBvB+ω×rI v I ≡ R I B v B + ω × r I ⇔ R I B v B ≡ v I − ω × r I v_I ≡ R_{IB}v_B + ω × r_I ⇔ R_{IB}v_B ≡ v_I - ω × r_I vI≡RIBvB+ω×rI⇔RIBvB≡vI−ω×rI其中 ω = R I B ω B ω=R_{IB}ω_B ω=RIBωB表示body坐标系的角速度在慣性坐标系下的表示。
Section 3 IMU 誤差模型
确定性誤差
理論上，當沒有外部作用時，IMU 傳感器的輸出應該為 0。但是，實際資料存在一個偏置 b。加速度計 bias 對位姿估計的影響： v e r r = b a t , p e r r ( 平 移 ) = 1 2 b a t 2 v_{err} = b_at, p_{err}(平移) = {1 \over 2}b_at^2 verr=bat,perr(平移)=21bat2
scale 可以看成是實際數值和傳感器輸出值之間的比值。
Nonorthogonality/Misalignment Errors
多軸 IMU 傳感器制作的時候，由于制作工藝的問題，會使得 xyz 軸可能不垂直，如下圖所示：
六面法标定加速度
六面法是指将加速度計的 3 個軸分别朝上或者朝下水準放置一段時間，采集 6 個面的資料完成标定。如果各個軸都是正交的，那很容易得到 bias 和 scale： l = S a + b l=Sa+b l=Sa+b
b = l f u p + l f d o w n 2 , S = l f u p − l f d o w n 2 g b={l_f^{up}+l_f^{down}\over 2} , S={l_f^{up}-l_f^{down} \over 2g} b=2lfup+lfdown,S=2glfup−lfdown其中，l 為加速度計某個軸的測量值，g 為當地的重力加速度。
當各個軸不正交時，即考慮軸間誤差的時候，實際加速度和測量值之間的關系為：
同理水準靜止放置 6 面，利用最小二乘就能夠把 12 個變量求出來。
六面法标定陀螺儀
bias 和 scale factor，和加速度計六面法不同的是，陀螺儀的真實值由高精度轉台提供，這裡的 6 面是指各個軸順時針和逆時針旋轉。
溫度相關的參數标定
目的：這個标定的主要目的是對傳感器估計的 bias 和 scale 進行溫度補償，擷取不同溫度時 bias 和 scale 的值，繪制成曲線。
兩種标定方法：
- soak method: 控制恒溫室的溫度值，然後讀取傳感器數值進行标定。
- ramp method：記錄一段時間内線性升溫和降溫時傳感器的資料來進行标定。
Section 4 基于 Bundle Adjustment 的 VIO 融合
視覺 SLAM 裡的 Bundle Adjustment 問題
已知：
- 狀态量初始值：特征點的三維坐标，相機的位姿。
- 系統測量值：特征點在不同圖像上的圖像坐标。
解決方式：
建構誤差函數，利用最小二乘得到狀态量的最優估計