VIO的IMU旋转运动学与误差模型和标定

秃头队长
603 阅读3分钟

本文已参与「新人创作礼」活动，一起开启掘金创作之路。

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=hz = h中的平面做圆周运动，其中aa为运动半径，则坐标为:r=(acosθ,asinθ,h)Tr = (a cos θ, a sin θ, h)^T对坐标求导得：

rˊ=(−aθˊsinθ,aθˊcosθ,0)T\acute{r}=(-a\acute{\theta} sin\theta,a\acute{\theta} cos\theta,0)^T
=[0−θˊ0θˊ00000][acosθasinθ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

ω\omega为反对称矩阵，因此 ω\omega矢量为[0,0,θˊ]T[0,0,\acute{\theta}]^T，其中，ω=θˊz\omega=\acute{\theta}z，∣θˊ∣|\acute{\theta}|是角速度大小。对上式取模，得：∣rˊ∣=∣ω∣∣r∣sinϕ=∣ω∣a=a∣θˊ∣|\acute{r}|=|\omega||r|sin\phi=|\omega|a=a|\acute{\theta}|

旋转坐标系下的运动学

质量块在 body 坐标系下的坐标为：rB=(x1,x2,x3)Tr_B = (x1, x2, x3)^T，旋转到惯性系下有： rI(t)=x1(t)i+x2(t)j+x3(t)k=RIBrBr_I (t) = x_1(t)i + x_2(t)j + x_3(t)k = R_{IB}r_B其中，body frame在惯性坐标系下的表示为(i,j,k)(i,j,k)

在这里插入图片描述

对时间求导有：

rˊI=RIBrˊB+RˊIBrB\acute{r}_I = R_{IB}\acute{r}_B + \acute{R}_{IB}r{B}
=RIBrˊB+[RIBωb]×rI= R_{IB} \acute{r}_B + [R_{IB}ω_b]×r_I
=RIBvB+ω×rI= R_{IB}v_B + ω × r_I
vI≡RIBvB+ω×rI⇔RIBvB≡vI−ω×rIv_I ≡ R_{IB}v_B + ω × r_I ⇔ R_{IB}v_B ≡ v_I - ω × r_I

其中ω=RIBωBω=R_{IB}ω_B表示body坐标系的角速度在惯性坐标系下的表示。

Section 3 IMU 误差模型

确定性误差

理论上，当没有外部作用时，IMU 传感器的输出应该为 0。但是，实际数据存在一个偏置 b。加速度计 bias 对位姿估计的影响：verr=bat,perr(平移)=12bat2v_{err} = b_at, p_{err}(平移) = {1 \over 2}b_at^2 scale 可以看成是实际数值和传感器输出值之间的比值。

在这里插入图片描述

Nonorthogonality/Misalignment Errors 多轴 IMU 传感器制作的时候，由于制作工艺的问题，会使得 xyz 轴可能不垂直，如下图所示：

在这里插入图片描述

六面法标定加速度

六面法是指将加速度计的 3 个轴分别朝上或者朝下水平放置一段时间，采集 6 个面的数据完成标定。如果各个轴都是正交的，那很容易得到 bias 和 scale：l=Sa+bl=Sa+b

b=lfup+lfdown2,S=lfup−lfdown2gb={l_f^{up}+l_f^{down}\over 2} , S={l_f^{up}-l_f^{down} \over 2g}

其中，l 为加速度计某个轴的测量值，g 为当地的重力加速度。 当各个轴不正交时，即考虑轴间误差的时候，实际加速度和测量值之间的关系为：

![在这里插入图片描述](p3-juejin.byteimg.com/tos-cn-i-k3…

同理水平静止放置 6 面，利用最小二乘就能够把 12 个变量求出来。

六面法标定陀螺仪

bias 和 scale factor，和加速度计六面法不同的是，陀螺仪的真实值由高精度转台提供，这里的 6 面是指各个轴顺时针和逆时针旋转。

温度相关的参数标定

目的：这个标定的主要目的是对传感器估计的 bias 和 scale 进行温度补偿，获取不同温度时 bias 和 scale 的值，绘制成曲线。 两种标定方法：

  • soak method: 控制恒温室的温度值，然后读取传感器数值进行标定。
  • ramp method：记录一段时间内线性升温和降温时传感器的数据来进行标定。

Section 4 基于 Bundle Adjustment 的 VIO 融合

视觉 SLAM 里的 Bundle Adjustment 问题

已知：

  • 状态量初始值：特征点的三维坐标，相机的位姿。
  • 系统测量值：特征点在不同图像上的图像坐标。

解决方式： 构建误差函数，利用最小二乘得到状态量的最优估计

avatar
文章
阅读
粉丝