VIO的IMU旋转运动学与误差模型和标定
Section 1 VIO 概述
VIO：（Visual-Inertial Odometry）以视觉与 IMU 融合实现里程计
IMU（Inertial Measurement Unit），惯性测量单元
- 典型 6 轴 IMU 以较高频率（≥ 100Hz）返回被测量物体的角速度与加速度。高频率运动。
- 受自身温度、零偏、振动等因素干扰，积分得到的平移和旋转容易漂移。可通过标定去除干扰。
六自由度 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 问题
已知：
- 状态量初始值：特征点的三维坐标，相机的位姿。
- 系统测量值：特征点在不同图像上的图像坐标。
解决方式：
构建误差函数，利用最小二乘得到状态量的最优估计