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

不会算法的阿召

已于 2023-08-26 13:42:56 修改

阅读量4.9k

点赞数 8

文章标签：自动驾驶 c++ 计算机视觉

于 2022-03-24 18:11:03 首次发布

本文链接：https://blog.csdn.net/qq_41916893/article/details/123715217

版权

（1）IMU简介

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

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

方案

IMU

视觉

优势

快速响应

不受成像质量的影响

角速度普遍比较准确

可估计绝对尺度

不产生漂移

直接测量旋转和平移

劣势

存在零偏

低精度IMU积分存在偏移

高精度IMU价格昂贵

易受到遮挡，运动干扰等影响

单目视觉无法测量尺度

单目纯旋转运动无法估计

快速运动时容易丢失

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

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

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

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

（3）基本数学知识

四元数的乘法

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

$q_1=a+b\vec{i}+c\vec{j}+d\vec{k}$

$q_2=e+f\vec{i}+g\vec{j}+h\vec{k}$

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

$q_1 q_2=(ae-(bf+cg+dh))+(be+af+ch-dg)\vec{i}+(ce+ag+df-bh)\vec{j}+(de+ah+bg-cf)\vec{k}$

如果令 $\vec{v}=(b, c, d)$ ， $\vec{u}=(f,g,h)$

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

$q_1q_2=ae-\vec{u}\cdot \vec{v}+e\vec{v}+a\vec{u}+e\vec{v}+\vec{v}\times\vec{u}$

四元数对时间的导数

假设某个旋转运动的旋转轴为单位向量 $\vec{u}$ ，即角速度 $\vec{w}$ 指向的方向，即有 $\vec{u}=\dfrac{\vec{w}}{w}$ 绕该轴旋转角度为 $\theta$ ，那么对应的单位四元数表示为：

$q=\left[\begin{array}{c} \cos \dfrac{\theta}{2} \\ \vec{u} \sin \dfrac{\theta}{2} \end{array}\right]$

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

$\Delta q=\left[\begin{array}{c} \cos \dfrac{\Delta \theta}{2} \\ u \sin \dfrac{\Delta \theta}{2} \end{array}\right]\approx \left[\begin{array}{c} 1\\ \vec{u} \dfrac{\Delta \theta}{2} \end{array}\right]= \left[\begin{array}{c} 1\\ \dfrac{\vec{w}}{w} \dfrac{\Delta \theta}{2} \end{array}\right]$

$\Delta q$ 对时间求导数，这里旋转角 $\Delta \theta$ 对时间的导数为 $\omega$ ，可以得到：

$\begin{aligned} \dot{q} & \triangleq \lim _{\Delta t \rightarrow 0} \frac{q(t+\Delta t)-q(t)}{\Delta t} \\ &=\lim _{\Delta t \rightarrow 0} \frac{q \bigotimes \Delta q-q}{\Delta t} \\ &=\lim _{\Delta t \rightarrow 0} \frac{q \bigotimes\left(\left[\begin{array}{c} 1 \\ \frac{1}{2} \frac{\vec{w}}{w} \Delta \theta \end{array}\right]-\left[\begin{array}{l} 1 \\ 0 \end{array}\right]\right)}{\Delta t} \\ &=q \bigotimes\left[\begin{array}{c} 0 \\ \frac{1}{2}\vec{ \omega} \end{array}\right] \end{aligned}$

（4）IMU的状态表示PVQ

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

上标 $g$ 表示陀螺仪 $gyroscope$ ，上标 $a$ 表示加速度计 $accelerometer$ ，上标 $b$ 指的是IMU本体坐标系，上标 $w$ 指的是世界坐标系， $q_{bw}$ 指的是从世界坐标系到本体坐标系的变换， $n$ 为噪声， $b$ 为偏置：

$\hat{w}^b=w^b+b^{g}+n^{g}$

$\hat{a} ^b= q_{bw}(a^w+g^w)+b^a+n^a$

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

$\dot{\mathbf{P}}_{wb_t}=\mathbf{V}^w_t$

$\dot{\mathbf{V}}^w_t=\mathbf{a}^w_t$

$\dot{\mathbf{q}}_{wb_t}=\mathbf{q}_{wb_t} \bigotimes\left[\begin{array}{c} 0 \\ \frac{1}{2}\vec{ \mathbf{\omega}}^{bt} \end{array}\right]$

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

$\mathbf{P}_{wb_j}=\mathbf{P}_{wb_i}+\mathbf{V}^w_i\Delta t+\iint_{t\in [i,j]}^{}(\mathbf{q}_{wb_t}\mathbf{a}^{bt}-\mathbf{g}^w)\delta t^2$

$\mathbf{V}_j^w=\mathbf{V}_i^w+\int_{t\in [i,j]}^{}(\mathbf{q}_{wb_t}\mathbf{a}^{b_t}-\mathbf{g}^w)\delta t$

$\mathbf{q}_{wb_j}=\int_{t\in [i,j]}^{}\mathbf{q}_{wb_i} \bigotimes\left[\begin{array}{c} 0 \\ \frac{1}{2}\vec{ \omega}^{bt} \end{array}\right]\delta t$

(5)IMU的预积分模型

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

$q_{wb_t}=q_{wb_i}\bigotimes q_{b_ib_t}$

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

$\mathbf{P}_{wb_j}=\mathbf{P}_{wb_i}+\mathbf{V}^w_i\Delta t-\dfrac{1}{2}\mathbf{g}^w\Delta{t^2}+\mathbf{q}_{wb_i}\iint_{t\in [i,j]}^{}(\mathbf{q}_{b_ib_t}\mathbf{a}^{bt})\delta t^2$

$\mathbf{V}_j^w=\mathbf{V}_i^w-\mathbf{g}^w\Delta t +\mathbf{q}_{wbt_i} \int_{t\in [i,j]}^{}(\mathbf{q}_{b_ib_t}\mathbf{a}^{b_t})\delta t$

$\mathbf{q}_{wb_j}=\mathbf{q}_{wb_i} \int_{t\in [i,j]}^{}\mathbf{q}_{b_ib_t} \bigotimes\left[\begin{array}{c} 0 \\ \frac{1}{2}\vec{ \omega}^{bt} \end{array}\right]\delta t$

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

$\boldsymbol{\alpha}_{b_ib_j}=\iint_{t\in [i,j]}^{}(\mathbf{q}_{b_ib_t}\mathbf{a}^{bt})\delta t^2$

$\boldsymbol{\beta}_{b_ib_j}= \int_{t\in [i,j]}^{}(\mathbf{q}_{b_ib_t}\mathbf{a}^{b_t})\delta t$

$\mathbf{q}_{b_ib_j}=\int_{t\in [i,j]}^{}\mathbf{q}_{b_ib_t} \bigotimes\left[\begin{array}{c} 0 \\ \frac{1}{2}\vec{ \omega}^{bt} \end{array}\right]\delta t$

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

$\left[\begin{array}{c} \mathbf{P}_{w b_{j}} \\ \mathbf{V}_{j}^{w} \\ \mathbf{q}_{w b_{j}} \\ \mathbf{b}_{j}^{a} \\ \mathbf{b}_{j}^{g} \end{array}\right]=\left[\begin{array}{c} \mathbf{P}_{w b_{i}}+\mathbf{V}_{i}^{w} \Delta t-\frac{1}{2} \mathbf{g}^{w} \Delta t^{2}+\mathbf{q}_{w b_{i}} \boldsymbol{\alpha}_{b_{i} b_{j}} \\ \mathbf{V}_{i}^{w}-\mathbf{g}^{w} \Delta t+\mathbf{q}_{w b_{i}} \boldsymbol{\beta}_{b_{i} b_{j}} \\ \mathbf{q}_{w b_{i}} \mathbf{q}_{b_{i} b_{j}} \\ \mathbf{b}_{i}^{a} \\ \mathbf{b}_{i}^{g} \end{array}\right]$