ORBSLAM3 IMU理论推导部分

APS2023

已于 2023-03-29 09:34:45 修改

阅读量1.5k

点赞数 3

分类专栏： ORB-SLAM3代码解析文章标签：人工智能计算机视觉 slam 算法 c++

于 2023-03-29 09:34:12 首次发布

本文链接：https://blog.csdn.net/qq_41694024/article/details/129374894

版权

ORB-SLAM3代码解析专栏收录该内容

14 篇文章 10 订阅

订阅专栏

6.1 引子：以ORBSLAM2的重投影误差为例，说明为什么需要残差对于状态量的雅可比

0.IMU简介及参数说明

IMU内参
参数说明
噪声传感器都会有
随机游走由于环境导致IMU数据有偏置
scale 实际数值和输出值之间的比值

         在ORBSLAM3中内参一般有四个数：陀螺仪的噪声，加速度计的噪声，陀螺仪的随机游走，加速度计的随机游走。

        随机游走是指IMU比较敏感，受环境一些因素都对IMU的输出有或多或少的影响，导致IMU数据里面伴随固定的偏置，这个偏置会随时间的变化有短时的漂移，每一时刻的偏置都不同。

        IMU外参：
# Transformation from body-frame (imu) to left camera
Tbc: !!opencv-matrix
   rows: 4
   cols: 4
   dt: f
   data: [ -0.999963,  0.00400649, 0.00759498,0.0107000004500151, 
          -0.0039695, -0.99998, 0.00487938, 1.2732925824821e-11,
         0.00761438, 0.00484905,0.999959, -2.91038304567337e-11,
          0.0, 0.0, 0.0, 1.0]

# Do not insert KFs when recently lost
InsertKFsWhenLost: 0

# IMU noise
IMU.NoiseGyro: 1e-3 # 0.000005148030141 # rad/s^0.5 
IMU.NoiseAcc: 1e-2 # 0.000066952452471 # m/s^1.5
IMU.GyroWalk: 0.000000499999999 # rad/s^1.5
IMU.AccWalk: 0.000099999997474 # m/s^2.5
IMU.Frequency: 200
这里b是imu的坐标系， $T_{bc}$ 为左相机到IMU坐标系的变化矩阵。

IMU内参
参数	说明
噪声	传感器都会有
随机游走	由于环境导致IMU数据有偏置
scale	实际数值和输出值之间的比值

1.前提公式（李群与李代数回顾）

        小写加粗的字母是向量，大写加粗的字母是矩阵，小写没加粗头上有个箭头的也是向量。

        第一个公式是一个向量的反对称矩阵的表示方法。

        第二个公式是反对陈矩阵的性质。

        第三个公式是旋转向量（李代数）到旋转矩阵的转换过程，当输入是一个旋转向量的时候 $\vec{\phi }$ 用大EXP，当输入是一个 $\phi$ 的反对陈矩阵的时候用小exp，最后他们的结果都是个旋转向量。

        第四个第五个公式是当给旋转向量 $\vec{\phi }$ 一个扰动 $\delta \vec{\phi }$ 的时候，等于给扰动之前的exp乘以右雅可比矩阵的扰动。

        最后一个公式是， $R$ 乘以一个旋转 $Exp(\vec{\phi })$ 再乘以一个 $R^T$ 的性质。

2.预积分的推导

2.1 什么是积分

$R_j = R_i \cdot \prod_{k=i}^{j-1}Exp((\tilde{w_k}-b_{k}^{g}-\eta _{k}^{gd})\cdot \Delta t)$

        这是关于旋转的积分，下角标 $i,j$ 代表第 $i$ 时刻（某一帧）、第 $j$ 时刻（某一帧）的旋转矩阵，这期间有若干个imu数据，给定了角速度和加速度，将角速度乘以时间再连乘起来可以得到角度的积分即i时刻的角度连乘(k=i -> j-1)得到第j时刻的角度。 $\tilde{w_k}$ 代表原始的没有经过处理的陀螺仪的角速度数据，另外还需要减去偏置和噪声， $\Delta t$ 表示两个imu数据之间的时间。 $\omega t$ 相乘代表旋转向量，再做一个Exp将旋转向量转化为一个旋转矩阵得到了一个从i时刻到j时刻的变化的旋转矩阵。

        即我们得到了一个从 $i$ 时刻到 $j$ 时刻的旋转矩阵 $R_{ji}$ 。

$v_j = v_i + \textbf{g}\Delta t_{ij} + \sum_{k=i}^{j-1}R_{k}\cdot (\tilde{f_k}-b_{k}^{a}-\eta _{k}^{ad})\cdot \Delta t$

        这是关于速度的积分， $g$ 表示重力。 $\tilde{f_k}$ 是加速度计的原始的读数，也是需要减去偏置再减去误差得到“真实”的加速度的值（速度 = 速度 + 加速度* $\Delta t$ ），因为我们读imu数据都是基于imu坐标系的，在我们算完 $a\cdot \Delta t$ 即速度的改变量后左乘以一个 $R_k$ 将状态从IMU坐标系转换到世界坐标系下再在叠加到 $v_i$ 上， $\textbf{g}\Delta t_{ij}$ 这一部分是为了平衡调g（这是因为我们在IMU读数的时候其中的加速度的读数是包含重力的，也就是说我们式子右面的第三部分是把重力算在内的，而我们是要把重力排除在外的）。

        这是关于位置的积分， $p_j$ 是第 $j$ 时刻的imu的位置， $\sum_{k=i}^{j-1}v_k\cdot \Delta t$ 表示从 $i$ 时刻到 $j-1$ 时刻的每一时刻的速度对位移的影响，其实就是高中物理的内容（这里只是积分了一下）---- $x_t = x_0 + v_0t + \frac{1}{2}at^2$ 。第二项还是取消对重力的影响。第三项是加速度对位移的影响。

2.2 由积分引出预积分

        由积分引出预积分：预积分里面的每一项与起始状态无关，可以认为都是相对量，这个好处在于计算预积分时不需要考虑起始状态，值得注意的是关于速度与位置的预积分里面都包含了重力。预积分计算方式：

        1、消除第 i 时刻对积分的影响

        2、保留重力的影响

        关于旋转的预积分：

$\Delta R_{ij}\doteq R_{wi}^TR_{wj} =\prod_{k=i}^{j-1}Exp((\tilde{w_k}-b_{k}^{g}-\eta _{k}^{gd})\cdot \Delta t)$

        关于速度的预积分：

$\Delta v_{ij} \doteq R_{wi}^T(v_j-v_i-g\cdot \Delta t_{ij})=\sum_{k=i}^{j-1}\Delta R_{ik}\cdot (\tilde{f_k}-b_{k}^{a}-\eta _{k}^{ad})\cdot \Delta t$

        关于位置的预积分：

$\Delta p_{ij} \doteq \sum_{k=i}^{j-1}[\Delta v_{ik}\cdot \Delta t +\frac{1}{2}\Delta R_{ik}\cdot(\tilde{f_{k}}-b_{k}^a-\eta _k^{ad})\cdot \Delta t^2 ]$

3. 噪声分离

3.1 旋转

        目的：上面推预积分时对imu的读数会减去它的偏置与误差，其中偏置可以作为状态量去得出，但是误差是没有办法得出的，我们能做的就是拿到imu数据减去偏置后直接使用，通常的办法就是通过计算误差的方式过滤掉这部分误差，无论是优化还是滤波都跳不过一个重要的矩阵——预积分的信息矩阵(协方差矩阵的逆)。由于假设了噪声是高斯白噪声，所以噪声的方差对状态方差的影响可以通过高斯分布推理过来。因此此节我们的目的就是推导出噪声对预积分的影响。

        本节我们的目的就是推导出标定好的imu噪声对预积分的影响，也就是预积分的偏差关于噪声的式子，下一节推出协方差方差的关系。

        对于VIO来说优化的状态量有15维，3维旋转、3维速度、3维位置、3维加速度计偏置、3维陀螺仪偏置。偏置可以在估计中得到，因此可以认为是已知的。
        由于假设了噪声为高斯白噪声，也就是服从了高斯分布，因此预积分噪声同样为高斯分布，整个过程以推导出预积分噪声的表达式为主，令预积分的测量噪声为：（关于角度、速度、位置的噪声）

$\eta _{ij}^{\Delta} \doteq [\delta \vec{\phi _{ij}^T},\delta v_{ij}^T, \delta p_{ij}^T]^T$

        下面分别对 3 个向量噪声进行推导，推导方式：分离噪声成如下形式，可以理解成：真实值 = 测量值 – 误差

        对于旋转项：

        我们令 $\mathbf{J}_{r}^{k} = \mathbf{J}_r((\tilde{w_k}-b_i^g)\Delta t)$ ：

左部分已经消去imu的误差对预积分的影响，右面的部分是imu的误差对预积分的影响

        对比于我们要分离出的式子： $\Delta R_{ij} =\tilde{ \Delta R_{ij}} \cdot Exp(-\delta \tilde{\phi_{ij}} )$ 于是有了第二张图片的推导过程。带大弯勾的表示分离掉误差的项，最终得到旋转项的高斯误差：

$\delta\vec{ \phi _{ij}} = -Log(\prod_{k=i}^{j-1}Exp(-\Delta \vec{R_{k-1 ->j}^T J^K_r\eta _{k}^{gd}\Delta t} ))$

        经过化简后，我们得到关于旋转项的高斯误差为：

$\delta\vec{ \phi _{ij}} = \sum_{k=i}^{j-1}\Delta \tilde{R_{k+1,j}^T}\cdot J^{k}_{r}\cdot \eta _{k}^{gd}\Delta t$

3.2 速度

3.3 位置

4. 噪声递推

        我们现在已经有了状态的噪声关于IMU噪声的表达式，我们可以根据表达式推出协方差矩阵的表达式，有了协方差矩阵后我们在优化的时候以及在预积分递推的时候可以根据公式算出来。

        上面求出了三个状态量误差的表达式，但由于式子要么是求和，要么是多积导致每次新来一个数据都需要从头计算，这给计算平台来带来资源的浪费，因此这节我们要推出误差的递推形式，即通过 $\delta p_{i->j-1}$ 推出 $\delta p_{i->j}$ 。

        即由上一个状态推出到当前状态的式子。

$\delta\vec{ \phi _{i->j}} = \sum_{k=i}^{j-1}\Delta \tilde{R_{k+1,j}^T}\cdot J^{k}_{r}\cdot \eta _{k}^{gd}\Delta t$

$\delta v_{i->j} = \sum_{k=i}^{j-1}[\Delta \vec{R}_{ik}\eta _{k}^{ad}\Delta t - \Delta \vec{R}_{ik}\cdot (\tilde{f_k}-b_{i}^a)^\wedge \cdot \delta \vec{\phi _{ik}}\cdot \Delta t]$

$\delta p_{i->j} = \sum_{k=i}^{j-1}[\delta v_{ik}\Delta t-\frac{1}{2}\Delta \tilde{R}_{ik}\cdot (\tilde{f}_k-b_i^a)^\wedge \delta \vec{\phi }_{ik}\Delta t^2+\frac{1}{2}\tilde{R}_{ik}\eta _k^{ad}\Delta t^2]$

        综上所述，我们可以写出 $\eta _{ij}^{\Delta }\doteq [\delta \vec{\phi }_{ij}^T,\delta v_{ij}^T,\delta p_{ij}^T]^T$ ， $\eta _{k}^d$ 表示误差、 $\eta _{k}^{gd}$ 表示陀螺仪的噪声， $\eta _{k}^{ad}$ 表示加速度的噪声。

        到这里我们就有了A矩阵和B矩阵：

$n_{ij}^{\Delta } = A_{j-1}\eta _{i,j-1}^{\Delta } + B_{j-1}\eta _{j-1}^{d}$

        这个就是状态量的误差的递推的形式，当前状态量的误差相对于上一状态量的误差的递推的形式和当前时刻的IMU的误差的形式。

        于是我们已知上个时刻的协方差矩阵，通过A、B矩阵还有关于IMU误差的协方差矩阵就能推出当前时刻的协方差矩阵。

5. Bias更新时对预积分的影响

        首先说明前面去除噪声时假设了这段时间内偏置不变，但偏置在vio算法中会作为状态量来优化，所以当通过优化后偏置会更新，这样一来如果重新计算这段时间的预积分会很浪费时间，所以本节目的是为了推出当偏置变化时直接求得新的预积分结果。

        其中， $\Delta \bar{\tilde{p_{ij}}}$ 表示偏置量改变后的预积分的值。

        到此，偏置更新且我们知道偏置的更新量的时候，它对预积分有什么影响这个过程我们就推出来了。

        关于速度：

        和角度类似，在优化过程中如果我们知道了偏置到底改变了多少 $\delta b_{i}^g$ ，以及已知的这些偏导数，以及原始的数据，我们就能知道最新的速度的预积分了。

        关于位置：

6. 求残差关于状态量的雅可比

6.1 引子：以ORBSLAM2的重投影误差为例，说明为什么需要残差对于状态量的雅可比

        重投影误差是用三维点经过相机外参R，t将三维点转换到相机坐标系下，在用内参K转换到像素坐标系下，由于之前有匹配关系，求像素的误差平方和得到了重投影误差。

        有了重投影误差后，我们可以求出重投影误差对于状态量（待优化目标）的雅可比（三维点、R、t）。

        对于IMU也一样，VIO定义了残差（和重投影误差比较像），也是为了优化状态量而做的。

6.2 定义残差

        对于角度： $\Delta \bar{\tilde{R}}_{ij}$ 表示偏置改变后的预积分的量、 $\Delta R_{ij}$ 是第 $i$ 时刻到第 $j$ 时刻IMU的旋转量（可能受到投影点的影响...）

$r_{\vec{\phi }_{ij}} \doteq Log(\Delta \bar{\tilde{R}}_{ij}^T \Delta R_{ij})= Log[(\Delta \bar{\tilde{R}}_{ij}\cdot Exp(\frac{\vartheta \Delta \tilde{R}_{ij}}{\vartheta b^g}))^TR_{i}^TR_j]$

        对于速度：状态量-预积分量

$r_{\Delta v_{ij}} \doteq \Delta v_{ij} - \Delta \bar{\tilde{v}}_{ij} =R_{i}^T(v_j-v_i-g\cdot \Delta t_{ij})- (\Delta \tilde{v}_{ij}+ \frac{\vartheta \Delta \tilde{v}_{ij}}{\vartheta b^g}\delta b_{i}^g + \frac{\vartheta \Delta \tilde{v}_{ij}}{\vartheta b^a}\delta b_{i}^a)$

        对于位移：状态量-预积分量

$r_{\Delta p_{ij}} \doteq \Delta p_{ij} - \Delta \bar{\tilde{p}}_{ij} =R_{i}^T(p_j-p_i-v_{i}\cdot \Delta t_{ij}-\frac{1}{2}g\cdot \Delta t_{ij}^2)- (\Delta \tilde{p}_{ij}+ \frac{\vartheta \Delta \tilde{p}_{ij}}{\vartheta b^g}\delta b_{i}^g + \frac{\vartheta \Delta \tilde{p}_{ij}}{\vartheta b^a}\delta b_{i}^a)$

6.3 定义扰动

        需要注意的是，这些量都是状态量，其与预积分无关。

         $\delta b_{i}^g \ \ \delta b_{i}^a$ 是偏置的改变量。

        现在我们已经知道了哪些量参与优化以及他们的扰动是如何定义的。

6.4 残差对于状态的雅可比

6.4.1 旋转残差

$r_{\vec{\phi }_{ij}} \doteq Log(\Delta \bar{\tilde{R}}_{ij}^T \Delta R_{ij})= Log[(\Delta \bar{\tilde{R}}_{ij}\cdot Exp(\frac{\vartheta \Delta \tilde{R}_{ij}}{\vartheta b^g}))^TR_{i}^TR_j]$

        其中不包含 $p_i,p_j,v_i,v_j,\delta b_{i}^a$ ，因此关于这些状态的雅可比矩阵都是0。

        下面分别推一下对于其他状态量的雅可比：

        推导思路是给待求量一个右扰动，带进残差的式子中，要把 $\delta \vec{\phi }_i$ 分离出式子中去，转换成一阶泰勒展开的样子。

6.4.2 速度残差

$r_{\Delta v_{ij}} \doteq \Delta v_{ij} - \Delta \bar{\tilde{v}}_{ij} =R_{i}^T(v_j-v_i-g\cdot \Delta t_{ij})- (\Delta \tilde{v}_{ij}+ \frac{\vartheta \Delta \tilde{v}_{ij}}{\vartheta b^g}\delta b_{i}^g + \frac{\vartheta \Delta \tilde{v}_{ij}}{\vartheta b^a}\delta b_{i}^a)$

其中不包含 $p_{i},p_{j},v_{i},v_{j},\delta b_{i}^a$ ，因此关于这些状态的雅可比矩阵都是0。

下面分别推一下对于其他状态量的雅可比：

6.4.3 位置残差

$r_{\Delta p_{ij}} \doteq \Delta p_{ij} - \Delta \bar{\tilde{p}}_{ij} =R_{i}^T(p_j-p_i-v_{i}\cdot \Delta t_{ij}-\frac{1}{2}g\cdot \Delta t_{ij}^2)- (\Delta \tilde{p}_{ij}+ \frac{\vartheta \Delta \tilde{p}_{ij}}{\vartheta b^g}\delta b_{i}^g + \frac{\vartheta \Delta \tilde{p}_{ij}}{\vartheta b^a}\delta b_{i}^a)$

其中不包含 $v_j,R_j$ ，因此关于这些状态的雅可比矩阵都是0。

下面分别推一下对于其他状态量的雅可比：

7. 总结

首先我们定义了积分，根据论文中的定义方式我们又推出了预积分，我们发现在推预积分的过程中有很多误差量是未知的，因此推出的预积分是没有办法直接用的，我们需把误差项分离出来，由于优化或者滤波的算法中并没有把误差具体多少算出来而是通过信息矩阵的方式给误差加权重，因此我们需要推导信息矩阵是怎么得出的，我们根据状态误差关于IMU误差的表达形式后得出了协方差的形式，有了协方差就有了信息矩阵。在优化中，偏置会作为一个状态量去优化，这就导致了这个过程中偏置会改变，我们又推出了Bias更新时对预积分的影响，在最后我们定义了残差的形式，推出了残差相对于状态量的雅可比矩阵。