一起自学SLAM算法：10.2 VINS算法

2023-05-16

连载文章，长期更新，欢迎关注：

写在前面

第1章-ROS入门必备知识

第2章-C++编程范式

第3章-OpenCV图像处理

第4章-机器人传感器

第5章-机器人主机

第6章-机器人底盘

第7章-SLAM中的数学基础

第8章-激光SLAM系统

第9章-视觉SLAM系统

第10章-其他SLAM系统

  10.1 RTABMAP算法

        10.2 VINS算法

        10.3 机器学习与SLAM

第11章-自主导航中的数学基础

第12章-典型自主导航系统

第13章-机器人SLAM导航综合实战

不管是激光SLAM还是视觉SLAM，由于传感器采样率、传感器测量精度、主机计算力等因素的限制，在高速运动状态下定位追踪极易丢失。虽然轮式里程计能为激光SLAM系统提供短期运动预测以避免高速运动时丢失的风险，轮式里程计还能在长走廊这样的低特征环境下提供定位辅助。但是轮式里程计也不是万能的，当地面起伏较大或轮子打滑时，轮式里程计将不再可靠；对于在三维空间工作的视觉SLAM来说，提供二维空间定位信息的轮式里程计很难应用其中。而在高速运动（尤其是高速旋转）或特征严重缺失（比如白墙、天空、地面等场景）时视觉SLAM基本上无法工作。采用IMU进行融合，无疑成了解决这些棘手问题的香饽饽。

关于IMU数据融合的问题，大体上又分为内部融合和外部融合两种，如表10-3所示。所谓内部融合，就是利用IMU模块内部各个轴的数据（acc、gyro、mag）进行姿态融合（也叫数据滤波），求解出IMU模块在空间中的姿态角（roll、pitch、yaw），在4.1.5节中已经给出了用于姿态融合的几种常用滤波算法，即卡尔曼滤波和互补滤波。通过内参标定，能大大提高IMU原始测量数据的精度，而姿态融合过程不仅能解算出姿态角，还能进一步修正IMU原始测量数据。而外部融合，是将IMU数据与其他传感器（比如轮式里程计（odom）、视觉（camera）、激光雷达（lidar）、GPS等）的数据进行融合。在外部融合过程中，既可以使用仅经过内参标定校正后的IMU原始测量数据（acc、gyro或acc、gyro、mag）也可以使用经姿态融合后得到的姿态角（roll、pitch、yaw）及IMU原始测量数据（acc、gyro或acc、gyro、mag），然后将IMU与其他传感器建立松/紧耦合联系，当然需要对IMU与其他传感器的安装坐标关系（也就是外参）进行标定，最后进行位姿估计实现定位追踪。物体在三维空间的状态可以由空间位姿（姿态角和位置）、线速度、线加速度、角速度、角加速度等描述，由于这里不涉及物体运动学问题，仅讨论空间姿态。内部融合只求出了空间姿态的姿态角orientation(roll,pitch,yaw)分量，也就是模块在空间的朝向。有些朋友可能会说，利用加速度和角速度积分不就能求出位移量，这样不就求出了空间姿态的位置position(x,y,z)分量了。这样做确实是可以的，但是IMU测量数据严重的长期漂移问题，求解出的位置很难用于像机器人这样需要大规模全局定位的场景。既然通过IMU测量数据本身无法提供可靠的位姿估计，那么就需要引入额外的传感器测量数据，也就是外部融合。在条件允许的情况下，配备越多的传感器理论上讲越有利于位姿估计，但是系统设计难度也更困难，本节重点讨论比较流行的一种融合方案（IMU与视觉融合，即VIO）。当然IMU还有很多其他用途，比如机器人上下坡判断（这对于单线激光雷达正确分割出地面很重要）、激光雷达运动畸变校正、机器人实时运动操控（IMU能提供机器人高实时性的加速度、角速度、线速度等运动信息，这些高实时的反馈数据能让机器人运动操控更加精确，这对于后面自主导航中路径规划和轨迹跟踪控制很重要）等。

表10-3 IMU数据融合

其实前面已经讨论过的Cartographer、ORB-SLAM3和RTABMAP都已经支持IMU融合，当然专门针对VIO开发出来的框架（比如MSCKF、OKVIS、ROVIO等）也已发展多年，不过本节要介绍的VINS是众多同类算法中比较优秀的一个，并且VINS的亮点都体现在IMU融合这个点上，鉴于此拿来进行分析学习。下面将从原理分析、源码解读和安装与运行这3个方面展开讲解VINS算法。

10.2.1 VINS原理分析

VINS算法是IMU与视觉融合的典型代表，融合首先要讨论的就是各个传感器的标定问题，其次是传感器数据的融合方式（也就是耦合），最后结合论文[3,4]对VINS系统框架展开具体分析。

1.标定

这里主要讨论IMU和单目相机的数据融合，在数据融合前需要先对传感器的内参和外参进行标定，标定结果的好坏对融合精度至关重要，下面具体讨论。

（1）IMU内参标定

由于制造工艺的误差，IMU模块内部会存在轴偏差、尺度偏差、零偏等问题。通过对IMU建立数学模型，并对模型中的误差项进行校准，能大大提高IMU原始测量数据的精度。常见的误差模型，如式（4-1）所示，具体标定过程见4.1.3节。

（2）单目相机内参标定

同样，由于制造工艺的误差，单目相机内部也存在各种误差。对于小孔成像模型的相机，需要对焦距和光心进行标定，有时也需要对畸变进行标定，其误差模型如式（4-110）和（4-111）所示，具体标定过程见4.3.1节。

（3）IMU与单目相机外参标定

关于IMU与单目相机外参标定，分为离线标定和在线标定两种。所谓离线标定，就是采集传感器的数据后离线进行处理求出待标定外参数，其中较常用的一个标定工具是kalibr。下面就以kalibr为例，简单介绍一下IMU与单目相机外参的离线标定过程，如图10-19所示。

图10-19 离线外参标定

其中，固定在环境中的标定板以世界坐标系 $O_{w}$ 为参考，而相机坐标系 $O_{c}$ 与世界坐标系 $O_{w}$ 之间的转移关系 $T_{w,c}(k)$ 其实就是相机在世界坐标系的位姿，该位姿可以通过标定板上的角点到相机像素点之间的3D-2D投影关系求解。求解出来的相机位姿 $T_{w,c}(k)$ 是离散的，而相机实际的运动轨迹显然是连续的，通过样条曲线（这里用的是B-spline）很容易从相机的离散位姿点得到连续轨迹 $T_{w,c}(t)$ 。由于相机与IMU之间的转移关系是固定的常量 $T_{c,i}$ ，在相机运动轨迹 $T_{w,c}(t)$ 叠加上这个固定转移关系 $T_{c,i}$ 就得到了IMU运动轨迹 $T_{w,i}(t)$ 。将 $T_{w,i}(t)$ 中的平移分量对时间进行两次求导就得到了IMU运动时的加速度 $a(t)$ ，将 $T_{w,i}(t)$ 中的旋转分量对时间求导就得到了IMU运动时的角速度 $\omega (t)$ 。考虑到相机与IMU之间采样时间存在一定的延迟 $d$ ，最后通过IMU的内参模型后就得到实际加速度测量值 $a(k)$ 和角速度测量值 $\omega (k)$ 。整个过程涉及到三个核心模型，如式（10-2）~（10-4）所示。

其中 $u$ 、 $a(k)$ 和 $\omega (k)$ 均为从传感器得到的观测量， $u$ 是相机观测到的像素， $a(k)$ 和 $\omega (k)$ 为IMU观测到的加速度和角速度。 $F_{cam}$ 是相机内参模型，包含焦距、光心和畸变参数； $F_{imu}$ 是IMU内参模型，包含轴偏差、尺度偏差和零偏参数。三个模型的重投影误差分别用 $e_{u}$ 、 $e_{a}$ 和 $e_{\omega }$ 表示，通过最小化重投影误差即可求出模型内参 $F_{cam}$ 和 $F_{imu}$ 、外参 $T_{c,i}$ 以及相机与IMU之间的时间延迟 $d$ ，如式（10-5）所示。

可以发现用kalibr进行标定时，不仅求出了外参 $T_{c,i}$ 和延迟 $d$ ，还对内参 $F_{cam}$ 和 $F_{imu}$ 进行了优化改善。有关kalibr外参标定方面的详细原理，请参考原论文[5]。

每次运行算法之前都要对传感器进行标定，然后手动将标定参数载入系统，这样很麻烦，并且对模型参数容易变化的时候，离线标定就不好用了，这就要提到在线标定了。所谓在线标定，就是在系统运行过程中标定程序自动采集数据并完成模型参数标定，这个过程不需要人为干预，因此也叫自动标定。比如VINS中就集成了在线标定功能，具体原理可以参考论文[4]。

（4）拓展

其实在机器人中涉及到各种标定问题，不仅限于上面的IMU与相机的标定。对于不同驱动形式的底盘（比如两轮差分、四轮差分、阿克曼、全向轮等），其轮式里程计的数学模型都不一样，要设计专门的标定方法对底盘进行标定。以两轮差分底盘为例，最常见的标定参数是动力系数和轴距，对于底盘中两个轮子差异较大的情况，两个轮子的动力系数还需要单独标定。当机器人搭载多个激光雷达时，需要对这些激光雷达的外参进行标定以保证数据能够正确融合在一起。对于配备有超声波测距的机器人，需要对超声波测距仪与其他传感器之间的外参进行标定。不管是何种传感器，标定方法同上面IMU与相机之间标定思路类似，都是从传感器中采集观测数据并利用其中的某些内在约束对标定参数进行建模并求解。标定对于多传感器融合算法来说既是重点也是难点，需要特别注意。

2.融合

通过第7章的介绍，我们已经知道SLAM问题其实就是利用观测数据对机器人位姿和路标进行估计的状态估计问题。而如何构建出观测量与待估量之间的约束关系就至关重要，接着就是求解该状态估计问题。众所周知，求解方法分为滤波方法和优化方法两大派别。当只用单传感器提供观测时，直接将观测送入滤波器（比如EKF）或优化器（比如PoseGraph）求解机器人位姿和路标即可。而当有多个传感器同时提供观测时，需要先考虑各个观测数据之间的融合问题，然后再送入滤波器或优化器求解机器人位姿和路标。关于融合方式，可以分为松耦合和紧耦合两种。这样的话，多传感器融合的SLAM问题就可以分为4种情况，以下主要讨论IMU和视觉两种传感器融合的SLAM问题，常见的一些方案如表10-4所示。

表10-4 常见的Visual-IMU融合SLAM方案

Visual-IMU

SLAM

滤波方法

优化方法

松耦合

ssf

msf

紧耦合

MSCKF

ROVIO

OKVIS

ORB-SLAM3

VINS

（1）松耦合

对于滤波方法的松耦合，比较典型的方案有ssf和msf。所谓松耦合，就是图像先通过单独的估计模块（black box）处理得到视觉里程计（VO），然后再将VO和IMU组合成一个状态向量送入滤波器（EKF）进行更新，如图10-20所示。

图10-20 滤波方法的松耦合方式（左图和右图）

而对于优化方法的松耦合，这方面的研究并不多，主要原因是效果不如紧耦合好。论文[7]提出过一种优化方法的松耦合方案，感兴趣的读者可以了解一下，如图10-21所示。其实也是图像先通过单独的估计模块处理得到视觉里程计（VO），那么两帧相机的位姿转移量利用相机与IMU的外参很容易变换为IMU的位姿转移量，最后将这个IMU的位姿转移量与IMU其他约束量一起在IMU框架下进行优化求解。

图10-21 优化方法的松耦合方式

（2）紧耦合

对于滤波方法的紧耦合，比较典型的方案有MSCKF[8]和ROVIO[9]。所谓紧耦合，就是图像中提取出来的路标特征点直接作为观测数据与IMU观测数据一起送入滤波器（EKF）进行更新，如图10-22所示。

图10-22 滤波方法的紧耦合方式

而对于优化方法的紧耦合是当前研究的热点，比较典型的方案有OKVIS[10]、ORB-SLAM3[11]和VINS[3]。对于基于优化的纯视觉SLAM问题，机器人位姿和路标作为待估计量由图结构中的节点表示，观测约束由图结构中的边表示，最后利用所有约束进行优化求解；而当IMU紧耦合到视觉SLAM时，同样机器人位姿和路标作为待估计量由图结构中的节点表示，只是观测约束边除了由视觉提供外还由IMU提供，同样最后利用所有约束进行优化求解，如图10-23所示。

图10-23 优化方法的紧耦合方式

（3）松耦合与紧耦合对比

在基于滤波方法的SLAM中，待估计量（机器人位姿和路标）用一个状态向量来描述，然后滤波器（比如EKF）利用观测数据（视觉和IMU）对待估状态向量进行迭代更新。对于松耦合的情况，视觉图像先通过单独模块处理得到VO，然后再将VO与IMU送入滤波器用于状态更新；而对于紧耦合的情况，视觉特征点与IMU直接就送入滤波器用于状态更新。不难发现，图像特征点在经过单独模块处理得到VO后，VO数据维度相比原始图像特征点要小很多，但会引入额外误差。也就是说松耦合比紧耦合的计算复杂度低，而紧耦合比松耦合的精度高。

在基于优化方法的SLAM中，待估计量（机器人位姿和路标）用图结构中的节点描述，而观测数据（视觉和IMU）提供节点之间的边约束，最后利用所有约束进行优化求解。对于松耦合的情况，视觉图像也是先通过单独模块处理得到VO，然后再利用VO约束和IMU约束进行优化求解；而对于紧耦合的情况，视觉图像特征观测约束与IMU约束用于构建一个整体的图结构，然后对这个整体约束进行优化求解。不难发现，松耦合计算复杂度更低，并且更易于进行多系统融合。所谓多系统融合，比如同时将多个独立运行的SLAM系统的VO约束提取出来进行松耦合，然后在构建出的新约束中优化求解新VO，这样就能在不重构原有SLAM的代码的情况下轻易融合众多优秀SLAM算法。而紧耦合完全保留了原始观测数据提供的约束信息，融合结果的精度显然更高。

3.VINS系统框架

根据上面的介绍，已经知道基于优化的紧耦合方案的融合精度是最高的，而VINS正是基于优化的紧耦合方案的典型代表。到这里就可以分析VINS的系统框架了，结合算法原作者清晰的论文思路[3]，很容易理解整个算法的组成架构。VINS有多个版本，最初的版本为VINS-Mono，仅支持IMU和单目；后来的版本VINS-Fusion在VINS-Mono的基础上进行了扩展，增加了对双目的支持；而VINS-Mono还专门被移植到手机，也就是版本VINS-Mobile。不过只要掌握了VINS-Mono的原理，其他版本很容易上手。如图10-24所示，为VINS-Mono系统框架。系统架构非常清晰，由观测预处理模块（Measurement Preprocessing）、初始化模块（Initialization）、VIO融合模块（Local Visual-inertial Odometry with Relocalization）和全局优化模块（Global Pose Graph Optimization and Reuse）构成，下面展开进一步的分析。