【VSLAM系列】二：VIO相关原理笔记

VIO(Visual-Inertial Odometry)

第一章 概述

1.1 IMU特点

IMU(Inertial Measurement Unit)，惯性测量单元，由一个陀螺仪和加速度计组成
优势：快速响应，角速度普遍比较准确，课估计绝对尺度
劣势：存在零偏，低精度IMU积分姿态发散，高精度价格昂贵

IMU与视觉的互补性质：
IMU适合计算短时间、快速的运动
视觉适合计算长时间、慢速的运动
同时可利用视觉定位信息来估计IMU的零偏，减少IMU由零偏导致的发散和累积误差，反之，IMU可以为视觉提供快速运动的定位
松耦合：IMU与视觉各自运算，后处理的方式进行融合，卡尔曼滤波，融合过程对二者本身不影响
紧耦合：IMU与视觉共同参与位姿估计，融合的过程会影响视觉与IMU的参数MSCKF和非线性优化 ，一次性建模所有的运动和测量信息

1.2 Eigen库相关

1、旋转向量

//初始化旋转向量：旋转角为alpha，旋转轴为(x,y,z)
Eigen::AngleAxisd rotation_vector(alpha,Vector3d(x,y,z));

// 旋转向量转[旋转矩阵](https://so.csdn.net/so/search?q=旋转矩阵&spm=1001.2101.3001.7020)
Eigen::Matrix3d rotation_matrix;rotation_matrix=rotation_vector.[matrix];
Eigen::Matrix3d rotation_matrix;rotation_matrix=rotation_vector.toRotationMatrix();

//旋转向量转[欧拉角](https://so.csdn.net/so/search?q=欧拉角&spm=1001.2101.3001.7020)(Z-Y-X，即RPY)
Eigen::Vector3d eulerAngle=rotation_vector.matrix().eulerAngles(2,1,0);

//旋转向量转四元数
Eigen::Quaterniond quaternion(rotation_vector);
Eigen::Quaterniond quaternion;Quaterniond quaternion;
Eigen::Quaterniond quaternion;quaternion=rotation_vector;

2、旋转矩阵

//初始化旋转矩阵

Eigen::Matrix3d rotation_matrix;
rotation_matrix<<x_00,x_01,x_02,x_10,x_11,x_12,x_20,x_21,x_22;

// 旋转矩阵转旋转向量
Eigen::AngleAxisd rotation_vector(rotation_matrix);
Eigen::AngleAxisd rotation_vector;rotation_vector=rotation_matrix;
Eigen::AngleAxisd rotation_vector;rotation_vector.fromRotationMatrix(rotation_matrix);

//旋转矩阵转欧拉角(Z-Y-X，即RPY)
Eigen::Vector3d eulerAngle=rotation_matrix.eulerAngles(2,1,0);

//旋转矩阵转四元数
Eigen::Quaterniond quaternion(rotation_matrix);
Eigen::Quaterniond quaternion;quaternion=rotation_matrix;

3、欧拉角

//初始化欧拉角(Z-Y-X，即RPY)
Eigen::Vector3d eulerAngle(yaw,pitch,roll);

//欧拉角转旋转向量
Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitZ())); Eigen::AngleAxisd rotation_vector;
rotation_vector=yawAngle*pitchAngle*rollAngle;

// 欧拉角转旋转矩阵
Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitZ())); Eigen::Matrix3d rotation_matrix;rotation_matrix=yawAngle*pitchAngle*rollAngle;

// 欧拉角转四元数
Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitZ())); Eigen::Quaterniond quaternion;quaternion=yawAngle*pitchAngle*rollAngle;

4、四元数

//初始化四元数
Eigen::Quaterniond quaternion(w,x,y,z);

//四元数转旋转向量
Eigen::AngleAxisd rotation_vector(quaternion);
Eigen::AngleAxisd rotation_vector;
rotation_vector=quaternion;

//四元数转旋转矩阵
Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.matrix();
Eigen::Matrix3d rotation_matrix;rotation_matrix=quaternion.toRotationMatrix();

// 四元数转欧拉角(Z-Y-X，即RPY)
Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(2,1,0);

1.3 相关公式推导

使用旋转矩阵 R 时,角速度为 ω,那么 R 相对于时间的导数可写作:

$$\dot{R}=R{\omega }^{\wedge }$$
左扰动雅克比

$$\frac{\partial (Rp)}{\partial \phi }=-(Rp{)}^{\wedge }$$
右扰动雅克比

$$\frac{\partial (Rp)}{\partial \phi }=-R{p}^{\wedge }$$
旋转连乘的雅可比:
$$\frac{dln(R_1{R}_2)}{d{R}_2}=J{r}^{-1}(ln(R_1{R}_2{)}^{\vee })$$
旋转连乘的雅可比:

$$\frac{dln(R_1{R}_2)}{d{R}_1}=J{r}^{-1}(ln(R_1{R}_2{)}^{\vee })R_2^T$$
对R的逆求导
$$\frac{\partial (R^{-1}p)}{\partial \phi }=(Rp{)}^{\wedge }$$

$$\frac{dln(R_1{R}_2^{-1})}{d{R}_2}=-J{r}^{-1}(ln(R_1{R}_2^{-1}{)}^{\vee })R_2$$

第二章 IMU模型

2.1 旋转运动学

旋转的几种表示方法
（1）旋转矩阵：使用9个量描述3自由度旋转，运算方便，但是会有额外约束(单位正交矩阵)，求导困难
（2）旋转向量：经凑的表示形式。但是具有周期性(但是微小变化时可以使用)
（3）欧拉角：直观，用户友好，但是需要指定旋转顺序，存在万向节锁死问题，无法进行球面平滑差值
（4）四元数：紧凑的不带其一的表示方法，对用户不直观，要求党委四元数，表示旋转也会过参数化

粒子在坐标系中 z = h 中的平面做圆周运动,坐标为:
$$r=(acos\theta ,asin\theta ,h{)}^{\top }$$
对坐标求导得:
$$\dot{r}=\omega \times r$$
其中
$$\omega =d(\dot{\theta })z$$
| θ̇| 是角速度大小。
对公式 (1) 两边取模得:
$$|\dot{r}|=|\omega ||r|sin\phi =a|d(\dot{\theta })|$$

质量块在 body 坐标系下的坐标为:
$$r^b=(x_1,x_2,x_3{)}^{\top }$$
忽略平移,只考虑旋转,则旋转到惯性系
下有:
$$r^i(t)=R_{ib}{r}^b$$
I系下的速度为对时间求导有:
$${\dot{r}}^i=v^i=R_{ib}{v}^b+\omega \times r^i$$
I系下的加速度为对速度求导得到:
$$\ddot{r}=a=a^i-2\omega \times v-\dot{\omega }\times r^i-\omega \times (\omega \times r^i)$$
科氏力 欧拉力 离心力

2.2 IMU测量模型与运动学模型

MEMS 加速度计利用电容或者电阻桥来等原理来测量 a_m
$$加速度计测量值：a_m=a-g$$
陀螺仪主要用来测量物体的旋转角速度,按测量原理分有振动陀螺,光纤陀螺等。低端 MEMS 陀螺上一般采用振动陀螺原理,通过测量 Coriolis force 来间接得到角速度

2.3 IMU误差模型

加速度计和陀螺仪的误差可以分为:确定性误差,随机误差
• 确定性误差可以事先标定确定,包括:bias, scale …
• 随机误差通常假设噪声服从高斯分布,包括:高斯白噪声,bias 随机游走…

确定性误差：

Bias每次启动都不一样，运行过程中可能也会变化
理论上,当没有外部作用时,IMU 传感器的输出应该为 0。但是,实际数据存在一个偏置 b。加速度计 bias 对位姿估计的影响:
$$v_{err}=b^{a}t,p_{err}=0.5b^a{t}^2$$
scale 可以看成是实际数值和传感器输出值之间的比值。

xyz 轴可能不垂直

六面法标定加速度 bias 和 scale factor
六面法是指将加速度计的 3 个轴分别朝上或者朝下水平放置一段时间，采集 6 个面的数据完成标定。

六面法标定陀螺仪，陀螺仪的真实值由高精度转台提供,这里的 6 面是指各个轴顺时针和逆时针旋转

温度标定 soak method: 控制恒温室的温度值,然后读取传感器数值进行标定。

随机误差

高斯白噪声 ：IMU 数据连续时间上受到一个均值为 0,方差为 σ,各时刻之间相互独立的高斯过程 n(t):

Bias 随机游走： 常用维纳过程 (wiener process) 来建模 bias 随时间连续变化的过程,离散时间下称之为随机游走。
$$\dot{b}(t)=n_b(t)={\sigma }_{b}w(t)$$
Bias的倒数是高斯噪声 其中 w 是方差为 1 的白噪声

艾伦方差标定

第三章 基于优化的IMU与视觉信息融合

3.1 基于BA的VIO融合

视觉SLAM里面的BA问题

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

解决方式
构建误差函数,利用最小二乘得到状态量的最优估计
$$argmin(q,p,f)\sum _{i=1}^m\sum _{j=1}^n||\pi (q_{w{c}_i},p_{w{c}_i},f_j)-z_{f_j}^{c_i}||{\Sigma }_{ij}$$
q:旋转四元数, • p:平移向量 • f :特征点 3D 坐标 • c i :第 i 个相机系 • π(·):投影函数 • z c f i :c i 对 f j 的观测
j • Σ ij :Σ 范数

备注：四元数乘法，可以转化为一个矩阵乘一个四元素[4X4] [4X1] = [4X1]

3.2 最小二乘问题迭代法求解

找一个下降方向使损失函数随 x 的迭代逐渐减小,直到 x 收敛到 x ∗ :F (x k+1 ) < F (x k )
分两步:第一,找下降方向单位向量 d,第二,确定下降步长 α.

1）最速下降法: 适用于迭代的开始阶段
梯度的负方向为最速下降方向
$$∆x=\frac{-J}{||J||}$$
2）牛顿法:适用于最优值附近
在局部最优点 x ∗ 附近,如果 x + ∆x 是最优解,则损失函数对 ∆x 的导数等于 0,
$$H^{-1}∆x=-J^{\top }$$
3）阻尼法，
$$(H+\mu I)∆x=-J^{\top }$$
4）高斯牛顿法
$$(J^{\top }J）∆x_{gn}=-J^{\top }f=b$$

5）列文伯格-马夸尔特法LM
$$(J^{\top }J+\mu I)∆x_{lm}=-J^{\top }f=b$$

JTJ信息矩阵

鲁邦核函数，M估计，自适应估计

3.3 VIO残差函数的构建

$$r=\Vert f(x){\Vert }^2\Sigma =f^{\top }(x){\Sigma }^{-1}f(x)$$

其中*,f (x)* 服从高斯分布,协方差为 Σ。 Σ可以将各残差项归一化，并给个权重深度参数化

重投影误差：

IMU的残差

IMU PVQ 对时间的导数可写成：
$$\begin{gathered} p{\dot{}}_{wbt}=v_t^w \\ v{\dot{}}_t^w=a_t^w \\ q{\dot{}}_{wbt}=q_{wbt}\otimes [0,\frac{1}{2}{\omega }^{bt}{]}^T \end{gathered}$$
从第 i 时刻的 PVQ 对 IMU 的测量值进行积分得到第 j 时刻的 PVQ:(注意，这里积分项中的姿态是相对于世界坐标系的)
$$\begin{gathered} p_{wbj}=p_{wbi}+v_i^w∆t+\int {\int }_{t\in [i,j]}(q_{w{b}_t}{a}^{b_t}-g^w)\delta t^2 \\ {v}_j^w=v_i^w+{\int }_{t\in [i,j]}(q_{w{b}_t}{a}_t^b-g^w)\delta t \\ {q}_{wbj}={\int }_{t\in [i,j]}{q}_{wbt}\otimes [0,\frac{1}{2}{\omega }^{b}t{]}^T\delta t \end{gathered}$$
IMU预积分

引入预积分的核心是不要使IMU积分量与优化变量有关

先以上一个时刻的位姿开始积分，然后将积分结果转化到世界坐标系。
预积分量仅仅预IMU的测量值有关，将一段时间内的IMU数据直接积分起来就得到了预积分量。一个很简单的公式转换，就可以将积分模型转为预积分模型：
$$q_{w{b}_t}=q_{w{b}_i}\otimes q_{b_i{b}_t}$$
那么，PVQ 积分公式中的积分项则变成相对于第 i 时刻的姿态，而不 是相对于世界坐标系的姿态：（可以将上面的PVQ积分公式中的积分项变成相对于第i时刻的位姿）

预积分量仅仅跟 IMU 测量值有关，它将一段时间内的 IMU 数据直接积分起来就得到了预积分量：
$$\begin{gathered} {\alpha }_{bibj}=\int {\int }_{t\in [i,j]}(q_{b_i{b}_t}{a}^{bt})\delta t^2 \\ {\beta }_{bibj}={\int }_{t\in [i,j]}(q_{b_i{b}_t}{a}^{bt})\delta t \\ {q}_{bibj}={\int }_{t\in [i,j]}{q}_{b_i{b}_t}\otimes [0,\frac{1}{2}{\omega }^{b}t{]}^T\delta t \end{gathered}$$

IMU预积分误差 定义：一段时间内 IMU 构建的预积分量作为测量值，对两时刻之间的 状态量进行约束，

预积分量的方差

误差的传递由两部分组成:当前时刻的误差传递给下一时刻,当前时刻测量噪声传递给下一时刻

假设已知了相邻时刻误差的线性传递方程:
$${\eta }_{ik}=F_{k-1}{\eta }_{ik-1}+G_{k-1}{n}_{k-1}$$
协方差矩阵可以通过递推计算得到:
$${\Sigma }_{ik}=F_{k-1}{\Sigma }_{ik-1}{F}_{k-1}^{\top }+G_{k-1}{\Sigma }_n{G}_{k-1}$$
用一阶泰勒展开的推导方式推导误差的递推公式: [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-r9svmjEk-1670151619451)(/media/jeet/My Passport/SLAM学习资料/学习之路/已学VIO/文档图片/4.png)]

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-Tf5jnTfk-1670151619451)(/media/jeet/My Passport/SLAM学习资料/学习之路/已学VIO/文档图片/5.png)]

3.4 误差卡尔曼滤波

https://www.bilibili.com/read/cv14531305

误差卡尔曼滤波就是以误差值作为滤波状态量. IMU预积分中使用误差卡尔曼滤波来计算置信度，得到误差的协方差矩阵。（主要是因为旋转四元数引起的过参数化问题），误差状态帮助IMU预积分计算协方差矩阵时避免过参数化。

1为什么要用误差卡尔曼滤波？
IMU预积分中使用四元数表示旋转，但是四元数是过参数化的，需要用4x4的矩阵来描述置信度；在误差卡尔曼滤波中，用旋转向量作为参数，这样就可以用3x3的协方差矩阵来衡量旋转的置信度.
误差通常是很小的，接近0，这样就避免了旋转向量的周期性问题.
误差量很小，可以忽略二阶导数的计算，只计算一阶导数更快.
误差变化很慢，用卡尔曼滤波进行状态修正时，可以降低观测频率.

标称状态与真实状态的均值是一致的，误差状态则与真实状态的协方差矩阵一致

误差的状态方程

需要推导离散时间下的预积分项，预积分残差，预积分误差项雅可比矩阵和预积分误差项协方差矩阵，下面为误差的传递方差

雅可比矩阵传播

可以从k时刻单位雅可比矩阵开始通过此式传播，得到i+1时刻的雅可比矩阵，该雅可比矩阵可用于前文提到的零偏变化情况下对预积分进行调整。

协方差矩阵传播

顾线性方程协方差传递的性质
有误差传递方程
首先，最初的协方差矩阵肯定是一个全零矩阵根据协方差传递的性质，有

其中，Q是噪声的协方差矩阵，记为

可以根据一开始的零协方差矩阵开始传播，得到i+1时刻的协方差矩阵。协方差矩阵可用于评判残差项的置信度。

第四章 基于滑动窗口的VIO：可观性与一致性

4.1 高斯分布与信息矩阵

$$J^{\top }{\Sigma }^{-1}J\delta \xi =-J^{\top }{\Sigma }^{-1}r$$

segema协方差矩阵 的逆为信息矩阵，可以理解为每个状态的权重

协方差矩阵的逆可以计算联合高斯分布

$$p(x_1,x_2,x_3)=p(x_2)p(x_1|x_2)p(x_3|x_2)$$
协方差逆矩阵中如果坐标为 (i, j) 的元素为 0,表示元素 i 和 j 关于其他变量条件独立,协方差中非对角元素 Σ ij > 0 表示两变量是正相关。
• 信息矩阵中非对角元素为负数,甚至为 0。Λ 12 < 0 表示在变量x 3 发生的条件下,元素 x 1 和 x 2 正相关。元素大于0表示负相关

4.2 边缘化与舒尔补

舒尔补定义
给定任意的矩阵块 M,如下所示:
$$\begin{gathered} M=[AB \\ CD] \end{gathered}$$
• 如果,矩阵块 D 是可逆的,则 A − BD ^−1 C 称之为 D 关于 M的舒尔补。
• 如果,矩阵块 A 是可逆的,则 D − CA^ −1 B 称之为 A 关于 M的舒尔补。把矩阵变成上三角或者下三角过程中遇到的角落处的矩阵

**使用舒尔补可以快速求解矩阵的逆 **

基于边际概率的滑动窗口算法直接丢弃变量和对应的测量值，会损失信息，正确的做法是使用边际概率，将被marge的变量所携带的信息传递给剩余变量

单目 + IMU 系统是 4 自由度不可观:yaw 角 + 3 自由度位置不可观。roll 和 pitch 由于重力的存在而可观,尺度因子由于加速度计的存在而可观。

第五章 后端优化

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-Q4LpnT8C-1670151619455)(/media/jeet/My Passport/SLAM学习资料/学习之路/已学VIO/文档图片/6.png)]

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-pBJTW3M2-1670151619456)(/media/jeet/My Passport/SLAM学习资料/学习之路/已学VIO/文档图片/7.png)]

GTSAM第三方优化库

滑动窗口更新先验残差

• 目的:虽然先验信息矩阵固定不变,但随着迭代的推进,变量被不断优化,先验残差需要跟随变化。否则,求解系统可能奔溃。
• 方法:先验残差的变化可以使用一阶泰勒近似。
$$\begin{gathered} b_p^{\prime }=b_p+\frac{\partial b}{\partial x_p}\delta x_p \\ =b_p+\frac{\partial (-J^{\top }{\Sigma }^{-}1r)}{\partial x_p}\delta x_p \\ =b_p-{\Lambda }_p\delta x_p \end{gathered}$$

第六章 前端

光流法最早，最成熟，缺点也明显（抗光照干扰弱，依赖角点）

• FAST+ 光流是比较实用的快速算法/GFTT+ 光流效果更好，也具备实时性
• 特征匹配需要提的特征具有旋转平移缩放不变性，SIFT/SURF 是
最好的一类，BRISK 等次之，ORB 算比较差的
• 特征匹配和光流都非常依赖角点，日常生活场景中角点不明显的
很多

后端通常实时性较差，不适合处理所有帧；如果相机停止，可能给后端留下无用的优化，甚至导致后端问题退化

1 关键帧之间不必太近（退化或三角化问题）
2 关键帧之间不能太远（共视点太少）
3 VIO 中，定期选择关键帧（假设 b g , b a 在关键帧期间不变）

对于非关键帧，只执行前端算法，不参与后端优化• 因此，对于非关键帧，它的误差会逐渐累积。直接该帧被作为关键帧插入后端，BA 才会保证窗口内的一致性

第七章 VINS初始化

通过相机与IMU的旋转估计外参数和bias，通过平移约束估计重力向量速度与尺度，但是重力向量的模长是固定的，优化过程中并没有加模长限制，而实际上只有两个自由度，三维向量自由度为2，可以采用球面坐标系参数化

数据集

evo安装与使用

https://www.codenong.com/cs105594262/

git clone https://github.com/MichaelGrupp/evo.git cd evo pip install -i https://pypi.tuna.tsinghua.edu.cn/simple evo --upgrade --no-binary evo

代码讲解

https://blog.csdn.net/weixin_40224537

第八章 时间补偿与同步

8.1 时间戳延迟估计方法

1 基于轨迹匀速模型的时间戳补偿算法

姿态补偿
假设 t 为对齐时刻,延迟量 t d ,延迟后时间戳为 t cam ,
则 t 时刻的相机姿态为
$$T_{wc}(t)=T_{wb}(t^{cam}-t_d)\cdot T_{bc}$$
具体地,假设相机匀速运动,t cam 时刻 IMU 坐标系速度为 v wb i ,角速度测量值为 ω b i (忽略 bias),补偿公式为
$$\begin{gathered} R_{w{b}_i,-t_d}\approx R_{w{b}_i}Exp(-{\omega }_{b_i}{t}_d) \\ {p}_{w{b}_i,-t_d}\approx p_{w{b}_i}-v_{b_i}^w{t}_d \end{gathered}$$

补偿后的相机姿态为
$$\begin{gathered} R_{w{c}_i}=R_{w{b}_i,-t_d}{R}_{bc}\approx R_{w{b}_i}Exp(-{\omega }_{b_i}{t}_d)R_{bc} \\ {p}_{w{c}_i}=R_{w{b}_i,-t_d}{p}_{bc}+p_{w{b}_i,-t_d} \\ \approx R_{w{b}_i}Exp(-{\omega }_{b_i}{t}_d)p_{bc}+p_{w{b}_i}-v_{b_i}^w{t}_d \end{gathered}$$
时间戳延迟变量只对先验残差和视觉重投影误差有影响。IMU 预积分误差中由于不
涉及相机传感器数据,不受时间延迟的影响

基于视觉特征匀速模型的时间戳补偿算法

时间戳延迟使得特征的重投影坐标和真实坐标之间存在误差,可以直接补偿特征坐标 假设视觉特征在图像平面上匀速移动

两种方法的分析
特征匀速模型:计算简单,代码改动小。然而视觉特征在图像平面是非线性运动,特征匀速的假设精度低。
• 轨迹匀速模型:相比于特征匀速,轨迹匀速的假设更接近实际,模型精度高。但雅克比矩阵等计算复杂,代码改动多。

两匀速模型算法对比
3 时间戳同步算法扩展
其他时间戳延迟估计算法
VIO 初始化阶段的时间戳估计