如何在AirSim中优雅的运行SLAM/VIO

无人机

本文首先解释了什么是VIO，介绍了VIO和SLAM的联系并对现有的VIO框架进行了调研，最后以Rovio和Vins为例，展示了如何在AirSim里跑VIO。

何为SLAM/VIO

视觉惯性里程计（Visual-Inertial Odometry, VIO）是一种将图像和IMU数据作为输入，相机移动轨迹/相机当前位置作为输出的模块。它与SLAM（Simultaneous Localization And Mapping）有着紧密的联系。经典视觉SLAM框架如下图所示，图参考了高翔博士的《视觉SLAM十四讲》。

假定我们的对象为一个搭载有各种传感器的机器人。传感器一般为相机图像，IMU和码盘等，将读取到的数据送至前端进行处理；前端可以通过读取到的数据计算相邻数据帧之间的运动；前端得到的运动轨迹并不全局一致，后端通过非线性优化获得全局一致的结果，系统中不一定有回环检测，若有将回环的结果也加入后端优化中结果会更准确；回环检测判断机器人是否处于之前走过的位置，若是则将数据传至后端进行优化；建图是根据之前的数据建立符合任务要求的地图。一般可以认为VIO是SLAM的一个子集，VIO包括了传感器读取前端后端的部分，不一定有回环检测；而定位+建图才是SLAM，SLAM必定包括回环，因为SLAM建立的地图是全局一致的。

下面还有几点需要说明：

  1. 若前端处理的只有图像数据则为视觉里程计（Visual Odometry, VO）例如GASSE03使用的ygz_slam_ros前端只利用了图像是一个VO（貌似可以加IMU不过好像没有完成？）。

  2. VIO是融合了图像和IMU数据，融合的方式有松耦合和紧耦合两种。松耦合是将图像输入VO得到位姿$\mathbf{T}^-$后融合IMU数据得到更可靠的位姿$\mathbf{T}^+$；紧耦合是输入图像，提取特征，与IMU数据一起估计位姿。

  3. 关于后端非线性优化的部分，一般有滤波和优化两种方法，可以认为滤波方法是只进行了一次迭代的优化方法。而关于滤波和优化的优劣还以实物效果为准，优化方法的精度较高但计算量大实时性差，滤波方法相反效率高但精度有所下降。

VIO现状

视觉SLAM（Simultaneous Localization And Mapping）的代表性工作包括PATM、LSD-SLAM、SVO、ORB-SLAM。关于SLAM的综述推荐文献[1]。目前较为成熟的VIO框架和模型有MSCKF[2], ROVIO[3], OKVIS[4]和VINS-Mono[5]。

MSCKF(Multi-State Constraint Kalman Filter)是基于滤波器SLAM的经典实现。MSCKF相比于传统的滤波器SLAM的创新在于应用了滑动窗口。在传统的EKF-SLAM框架中，特征点的信息会加入到特征向量和协方差矩阵中，相当于特征点的信息给定了初始深度和初始协方差的值。如果这个值错误，后续的计算中会发散。MSCKF维护一个FIFO的滑动窗口当特征点在滑动窗口的几个位姿都被观察到的时建立几个位姿间的约束进而更新滤波器。

ROVIO是于2015年提出的以EKF为主的VIO，它将将视觉信息和IMU信息通过迭代拓展卡尔曼滤波(IEKF, Iterated Extended Kalman Filter)进行紧耦合。ROVIO将路标点及其周围的图像作为路标点的描述子，得到光度误差后变换至IEKF的更新项。整个系统使用固连关信息从而减小非线性引起的误差，而空间路标信息被分解为集束向量和深度倒数从而使得初始化无延迟。ROVIO精度和鲁棒性相对不错，在低速移动条件下可以以20Hz输出位姿信息（测试平台的CPU为i7-2760QM）；而角速度约为3.5rad/s的高速移动条件下可以较好拟合真实的移动数据；实机飞行实验中，无外加校准信息时在起飞阶段即可完成初始化。

OKVIS是由 Stefan Leutenegge 等人提出的基于双目+惯导的视觉里程计，其算法是基于关键帧优化的紧耦合VIO。OKVIS是通过 IMU 测量值对当前状态做预测，根据预测进行特征提取和特征匹配，3D 点特征和二维图像特征构成优化中重投影，同时预测的 IMU 的状态量和优化的参数之间构成 IMU 测量误差，这两项误差放在一起做优化。

VINS-Mono是以优化关键帧为主的VI-SLAM。首先在第一个关键帧中寻找最强的150个特征点，通过非极大值抑制半径减少误匹配。下一关键帧输入后通过光流法进行特征点匹配，经过相机畸变等校正后将位置信息发出。若在运行过程中特征点丢失会用最强最新的特征点补足。在高度为60m的实机飞行测试中，总长5.62km的路程累计漂移约为0.29%（16m）。VINS-Mono使得SLAM的应用更偏向单目视觉+IMU。单目视觉能够适应室内、室外及不同光照的环境，具有较好的适应性。

关于VIO的对比表

优雅的运行VIO

如何在AirSim里面优雅（并不）的运行VIO呢，要解决的问题主要有两个：坐标系对齐和数据频率。以ROVIO和Vins-Fusion为例。有关二者的编译和运行参见：

ROVIO：https://github.com/ethz-asl/rovio

Vins-Fusion：https://github.com/HKUST-Aerial-Robotics/VINS-Fusion

Euroc数据集是一个标准的数据集，可以用来测试VIO/SLAM的精度。我们的思路是先把Euroc数据集跑通，然后从Euroc数据集推广到AirSim的数据。由于服务器在国外，官网下载比较慢，20kb/s龟速下载，之前找了百度云的分享，关注公众号回复“数据集”即可获得下载链接。

运行数据集

Rovio比较简单，自带launch文件，运行launch文件后播放数据集

1
2
roslaunch rovio rovio_node.launch
rosbag play <下载的数据集>

打开launch文件康康，可以看到滤波器配置和相机配置文件的路径，如果坐标系的转换关系、相机参数、滤波器参数改变修改对应的配置文件。

Vins也差不多，不过没有现成的launch文件自己写一个，从运行的例子可以看出，配置文件主要在config文件夹下。

1
rosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_mono_imu_config.yaml

自己写launch文件，后面修改相机参数就比较方便了

![](https://hexo-blog-bs.oss-cn-beijing.aliyuncs.com/_blog-img/2020-06VIO/Screenshot from 2020-06-04 18-08-21.png)

调整坐标系

首先定义坐标系，相机系和机体系，在Euroc数据集中的图示大概如下。从相机系转到机体系绕相机系的z轴转-90°。一般而言红色对应x轴，绿色对应y轴，蓝色对应z轴。

在Rovio中旋转用四元数表示，在VINS中用齐次矩阵表示，可以通过配置文件的参数看出，旋转对应着绕z轴转-90°

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
Rovio
Camera0
{
	qCM_x  0.00666398307551;
	qCM_y  -0.0079168224269;
	qCM_z  -0.701985972528;
	qCM_w   0.712115587266;
	MrMC_x -0.0111674199187;
	MrMC_y -0.0574640920022;
	MrMC_z 0.0207586947896;
}

VINS
body_T_cam0: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975,
           0.999557249008, 0.0149672133247, 0.025715529948,  -0.064676986768,
           -0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949,
           0, 0, 0, 1]

经过测试，使用AirSim的SampleFlight时机体系是FRD，则对应的转换矩阵为

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
Rovio
Camera0
{
   qCM_x  -0.500878515967;
   qCM_y  -0.50215775337;
   qCM_z  -0.499875139004;
   qCM_w  0.497074590357;
   MrMC_x -0.00479785;
   MrMC_y 0.005052;
   MrMC_z -0.01413185;
}

VINS
body_T_cam0: !!opencv-matrix
  rows: 4
  cols: 4
  dt: d
  data: [0, 0, 1, 0.35,
        1, 0, 0, 0.05,
        0, 1, 0, 0.1, 
        0., 0., 0., 1 ]

在

MAVROS中是FLU

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
Rovio
Camera0
{
   qCM_x  0.500878515967;
   qCM_y  -0.50215775337;
   qCM_z  0.499875139004;
   qCM_w  0.497074590357;
   MrMC_x -0.00479785;
   MrMC_y 0.005052;
   MrMC_z -0.01413185;
}


VINS
body_T_cam0: !!opencv-matrix
  rows: 4
  cols: 4
  dt: d
  data: [0, 0, 1, 0.35,
        -1, 0, 0, 0.05,
        0, -1, 0, 0.1, 
        0., 0., 0., 1 ]

数据频率

然后就是数据频率的问题，上一篇推文写了仿真的数据频率和实际的数据频率相比是差很多的，主要是IMU的频率差了5-10倍，因而效果会比较糟糕。大多数时候跑着跑着就发散了23333，不过AirSim有调整时钟频率选项，现实世界经过1s，AirSim的仿真环境中经过xs。改小之后情况有所改善，恩，有所改善。没尝试过放慢后用SITL跑VIO，SITL时的IMU数据是从mavros中产生，大概也会有相同的效果？

1
"ClockSpeed": x,

优雅（并不）的上手

下面是在分布式情况下跑的例子，用的SampleFlight，会尝试跑SITL但很容易发散。

首先修改配置文件，利用AirSim的ros包获取IMU和图像数据，只有在Vehicles里面的实例中添加传感器信息AirSim的ros包才会获取相关数据。配置文件两台机子上要一致。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
{
  "SeeDocsAt": "https://github.com/Microsoft/AirSim/blob/master/docs/settings.md",
  "SettingsVersion": 1.2,
  "SimMode": "Multirotor",
  "ViewMode": "NoDisplay",
  "ClockSpeed": 0.25,
  "Vehicles": {
    "mav_00": {
      "VehicleType": "SimpleFlight",
      "Sensors": {"imu": {"SensorType": 2, "Enabled": true } },
      "Cameras": {"ccam": {"CaptureSettings": [{"ImageType": 0, "Width": 640, "Height": 480, "FOV_Degrees": 90, "TargetGamma": 1.5 } ], "X": 0.35, "Y": -0.05, "Z": -0.1, "Pitch": 0, "Roll": 0, "Yaw": 0 } },
      "X": 0, "Y": 0, "Z": 0, "Pitch": 0, "Roll": 0, "Yaw": 0
    }
  }  

}

运行AirSim的ros包，cd到AirSim源码目录，编译，运行。编译的时候好像要求gcc8，当时遇到过坑不过好像问题不大，搜一下就解决了，所以忘记了（吐舌）。airsim_node.launch里面改一下ip。数据量还是蛮大的，大概30MB/s建议配一个千兆路由器或者网线直连。

1
2
3
4
cd /AirSim_Linux/ros
catkin_make
source devel/setup.bash
roslaunch airsim_ros_pkgs airsim_node.launch

IMU数据和图像数据就推出来了，修改VIO的launch文件后运行，需要remap一下相应的话题

对Rovio而言需要多写一个图像转换的节点，Rovio要求输入的图像是灰度图，滤波器和相机的配置文件复制了一份原版，然后按照上一步调整坐标系的步骤修改了参数。

1
2
source devel/setup.bash
roslaunch rovio <自己写的launch>

对Vins而言也差不多，把3.1写的launch跑起来就ok了。

下面是演示视频

文献列表

[1] 刘浩敏, 章国锋, 鲍虎军. 基于单目视觉的同时定位与地图构建方法综述[J]. 计算机辅助设计与图形学学报, 2016, 28(6): 855-868.

[2] Mourikis A, Roumeliotis S. A multi-state constraint Kalman filter for vision-aided inertial navigation[C]. Proceedings of IEEE International Conference on Robotics and Automation. Los Alamitos: IEEE Computer Society Press, 2007: 3565-3572

[3] Sun K, Mohta K, Pfrommer B. Robust stereo visual inertial odometry for fast autonomous flight[J]. IEEE Robotics and Automation Letters, 2018, 3(2): 965-972.

[4] Leutenegger S, Lynen S, Bosse M. Keyframe-based visual–inertial odometry using nonlinear optimization[J]. The International Journal of Robotics Research, 2015, 34(3): 314-334.

[5] Qin T, Li P, Shen S. Vins-mono: A robust and versatile monocular visual-inertial state estimator[J]. IEEE Transactions on Robotics, 2018, 34(4): 1004-1020.

P.S. 终于写够了原创的篇数可以开启赞赏啦！这篇写了大概3500+吧，如果喜欢我的分享请给我一点支持和鼓励哟。顺便，一开始准备沿用孤独的梦想家的名号的，被占用只好改了hhhh，可怜弱小无助还特么能吃。

本文标题:如何在AirSim中优雅的运行SLAM/VIO

文章作者:BrightSoul

发布时间:2020-06-04, 00:00:00

原始链接:https://brightsoulxyhy.github.io/articles/2020-06VIO/

许可协议: "署名-非商用-相同方式共享 4.0" 转载请保留原文链接及作者。

文章目录
  1. 1. 何为SLAM/VIO
  2. 2. VIO现状
  3. 3. 优雅的运行VIO
    1. 3.1. 运行数据集
    2. 3.2. 调整坐标系
    3. 3.3. 数据频率
    4. 3.4. 优雅（并不）的上手
  4. 4. 文献列表
|