IMU在slam系统中的应用(一)

（1）IMU简介

        IMU：Inertial Measurement Unit，惯性测量单元，典型的轴IMU以较高频率返回被测物体三个不同方向的角速度和角加速度，包括，，，，，。但是受到自身温度、零偏和振动等因素干扰，积分得到的姿态变换容易发生偏移。

        视觉定位：以图像的形式记录数据（单目、双目和）等，通过特征点或者像素推断相机位姿变化，频率一般为。

方案	IMU	视觉
优势	快速响应  不受成像质量的影响 角速度普遍比较准确 可估计绝对尺度	不产生漂移 直接测量旋转和平移
劣势	存在零偏 低精度IMU积分存在偏移 高精度IMU价格昂贵	易受到遮挡，运动干扰等影响 单目视觉无法测量尺度 单目纯旋转运动无法估计 快速运动时容易丢失

        可以看出，IMU可以为视觉提供快速运动时的定位，视觉定位信息可以用来矫正IMU的漂移，二者实际上是互补关系。

（2）松耦合和紧耦合系统

        松耦合loose couple：自己算自己的，最后整合数据，是相对独立的；将IMU定位与视觉的位姿直接融合，融合过程对二者本身不产生影响，作为后处理方式输出。典型方案为卡尔曼滤波（后续更新卡尔曼滤波方法）。

        紧耦合tightly couple：利用视觉的图像特征和IMU积分的位置、方向和速度融合，输出优化后的位置、方向和速度。

（3）基本数学知识

四元数的乘法

        四元数表示物体绕旋转轴旋转一定角度，所以可以用一个向量和一个旋转角表示，假设存在两个四元数，用复数表示：

        因此，四元数相乘表示为：

        如果令，

        那么四元数的乘积可以表示为：

四元数对时间的导数

        假设某个旋转运动的旋转轴为单位向量，即角速度指向的方向，即有绕该轴旋转角度为，那么对应的单位四元数表示为：

        当旋转时间很短，旋转角度微小，为，趋近于时，可以得到四元数为：

        对时间求导数，这里旋转角对时间的导数为，可以得到：

（4）IMU的状态表示PVQ

        在slam中，IMU的状态通常用位移，速度和旋转表示。用高中物理的知识，假设某时刻IMU测量时，物体的真实角速度为，加速度为，而实际测量存在零偏，噪声和重力加速度的影响，测量值为和。

        上标表示陀螺仪，上标表示加速度计，上标指的是IMU本体坐标系，上标指的是世界坐标系，指的是从世界坐标系到本体坐标系的变换，为噪声，为偏置：

        所以，位移，速度和加速度在时刻对时间的导数分别为:

        因此结合运动方程，实际上观测量转变到真实量是要去除噪声和偏置，这里为使公式简单，用和指代已经减去了噪声和偏置的和，这里不考虑偏置和噪声的情况下，可以得到IMU由时刻到时刻积分为：

(5)IMU的预积分模型

        由上小节可以看出，每次IMU接收到新的数据，状态优化更新时，都需要重新积分，运算量大。所以可利用下式将积分模型转化为预积分模型：

        那么，PVQ的积分项就不是相对世界坐标系积分，而转变成相对时刻的位姿进行积分：

        所以只需要对相邻时刻求解预计分即可，预积分量为：

        对IMU预计分形式重新整理，可得

        IMU预积分的前提假设是其零偏已知，即：