了解imu以及imu的标定

Posted 2023-05-03

参考技术A 因为实验室项目原因，需要用到imu，所以以前从来没有听说过，所以这一段时间都在鼓捣这么个小东西。关于它的一些定义就不说了，网上一大堆，也不是很重要。就说一些自己有用到的东西吧。

imu中文叫做惯性测量单元，它能够获取自身的加速度、角速度信息，有的imu还能够获得地磁量。实验室中使用的是BNO055 Xplained Pro。它有3个加速度计，3个陀螺仪，3个地磁计，能够测量x，y，z方向的信息。因为还在初步学习阶段，没有去了解地磁计，所以只使用了加速度计和陀螺仪获得的加速度和角速度信息。（这里有一个点我要吐槽一下，实验的时候自己烧坏了两个imu和一个usb，原因是BNO055 Xplained Pro这个模块它的管脚不是完全被焊锡包住的，安装在实验室的小车上时管脚直接与小车的金属壳体接触就给烧了，真是蠢哭了- -）l

imu在使用之前一般都要标定，加速度计和陀螺仪的标定是分开的，因为加速度计获得的加速度值飘忽不定，不是一个常量，所以不能直接在结果上减去一个值来完成标定，常用的方法有最小二乘法，具体的方法及原理可以参考这里。陀螺仪获得的信息是一个不变的常量，所以可以直接在结果上减去这个常量就可以完成标定。

本来是打算写一个程序来对加速度计进行标定的，写到后面解其次方程的时候没想到要怎么解，结果发现有用于imu标定的包，于是打算直接用 这个 。按照它的REAME.md来进行，应该是能够得到标定参数的。但是它这里面安装Ceres Solver的链接失效了，可以参考 这里 来安装。

因为写这篇文章的时候在外出差，手边没有imu采集数据，没有办法来做实验，等到回学校了再来验证是否可行吧～

VINS初始化与相机IMU外参标定

VINS（四）初始化与相机IMU外参标定

 和单目纯视觉的初始化只需要获取R，t和feature的深度不同，VIO的初始化话通常需要标定出所有的关键参数，包括速度，重力方向，feature深度，以及相机IMU外参 Rbc

和 pbc

。

一. 外参旋转矩阵初始化

在Feature Detection and Tracking模块中，利用Harris特征点匹配通过基础矩阵和Ransac恢复出 RCkCk+1

；

相应的使用IMU陀螺仪数据积分得到 Rbkbk+1

；

这两个测量满足：

Rbkbk+1⋅Rbc=Rbc⋅RCkCk+1

将测量得到的 R

用四元数表示，并利用四元数的运算（左乘右乘），可以得到一个约束方程：

Qkk+1⋅qbc=0

利用多组数据可以获得一个超定方程:

QN⋅qbc=0

通过求解最小二乘解，得到 qbc

。注意其中 QN 是包含使用Huber核的权重的，权重是通过残差矩阵计算得到，权重通过计算IMU项和视觉findFundamentalMat计算得到的旋转矩阵之间的差别得到，差别越大的 Qkk+1 ，在 QN

中权重越小。

 

旋转初始化成功判定依据：

如果IMU三轴充分旋转，那么4*4矩阵 QN

的null space零空间的秩为1，可以得到稳定解；

如果旋转退化，则 QN

零空间的秩大于1，使用SVD分解，判断倒数第二小（第三大）的奇异值是否大于阈值。

因此，旋转标定初始化的关键在有充分的三轴旋转。

 

二. 速度，重力方向，feature深度，以及外参平移矩阵初始化

可以将 Rbc

， Rbkbk+1 ， Rb0bk+1

当做已知固定值，使用最大似然估计（Maximum Likelihood Estimation, MLE）和Mahalanobis范数（可以理解为带权重的二范数（椭球向量范数））构建cost function，构建无偏的状态估计问题。

在Sliding Window（例如选取5帧）中对测量值构建最小二乘问题：

 

VINS中feature没有通过描述子匹配跟踪，也不仅像ORB-SLAM中选取观测次数多（跟踪效果好）的特征点，另外加入了视角足够大这个条件。虽然最后选取出的特征点数量比较少（150个左右），但是都是可观性很好的特征点。

（一）IMU测量模型

构建目标函数需要对测量模型建模，IMU的测量模型和预积分一样，预积分得到的测量值 z^

与状态估计值之间的差值取Mahalanobis范数，其中权重为协方差矩阵的逆（信息矩阵）。

参考预积分公式：

得到IMU测量的误差方程：

其中

 

为了求解高斯分布的最大似然，需要知道测量的协方差及其对应的状态转移方程（用于协方差的更新）

通过IMU的运动学方程微分，可以获得状态转移方程

假设初始IMU协方差 Pbkbk

为0，则协方差传播方程为

 

协方差矩阵的传播和卡尔曼滤波的预测过程中协方差的传播是一样的，从系统的微分运动方程获取的。VINS中使用的是first-order discrete-time propagation，或者称为forward Euler method，其中加速度计的噪声 Qt

需要预先知道或者使用imu_tk标定。

（二）视觉测量模型

重投影误差在VIO中，通常需要经过IMU坐标系的变换；这样有两个好处：

• 将IMU的参数（外参）整合在视觉的residual中一起参与优化；
• IMU的坐标系在预积分中是已知的，不需要再去计算；

因此将i帧获取的2D feature重投影到j帧上满足观测方程：

其中的坐标变换如下图所示：

  

初始化收敛判定：

cost function的协方差矩阵中外参平移向量 pbc

对应的协方差值（SVD分解）小于设定阈值时，收敛，初始化完成。由于要获得协方差矩阵（对应整个状态向量，而不是每一块误差项），需要对信息矩阵求逆，计算量较大，因此会放在新的线程中去完成（这里似乎有可以优化的空间）。