一种基于改进光流法的视觉惯性状态估计器

刘奕杉

（广东工业大学，广东 广州510006）

1 概述

实时定位和建图(SLAM)起源于20 世纪80 年代，它广泛应用在移动机器人、无人机、虚拟现实、无人驾驶等领域。它解决的主要问题是机器人在未知环境的定位和建图问题。传统的定位方式多以GPS 为主，然而这种定位方式难以适应复杂陌生环境，所以SLAM 技术应运而生。SLAM 根据后端方法的差异，可以分为基于滤波和基于优化这两大类，根据传感器差异也可以分为激光SLAM 和视觉SLAM[1]。然而，单一传感器难以满足高精度的定位效果，所以在实际应用中，常见融合多种传感器的信息用于机器人定位，常见的有视觉和惯性单元的融合。视觉传感器可以提取现实场景丰富的信息，但它受环境影响大，当面对光照变化大、纹理缺失、快速运动等情形，无法有效追踪物体运动。惯性单元可以得到加速度和角速度，且不受图像质量的影响、采样频率较高，但存在长时间累计误差的问题。所以视觉传感器和IMU 具有优劣互补的特点。

本文研究基于光流法的视觉惯性SLAM 系统。我们结合改进的光流法和IMU 信息，然后基于LM 的非线性优化算法，实现了一个精度较高的视觉里程计。本文将从改进的光流法、惯性单元预积分，系统框架三部分对该方案进行描述，最后将通过实验展示该系统的运行效果。

2 正文

2.1 光流法

Lucas-Kanade 光流法是Bruce D. Lucas 等人于1981 年在文献[2][3]提出来的。它被广泛地应用到图像配准、运动追踪，目标检测等方面。根据计算图像的部分像素或是全部像素的运动，可以将它分为稀疏光流法和稠密光流法。光流法的原理依赖于三个假设。第一个是灰度不变假设 I ( x + dx,y + dy,t + dt ) =I ( x, y , t )，它要求t 时刻在坐标(x,y)处的灰度值 I ( x, y , t)和t+1 时刻的灰度值I ( x + dx, y + dy, t +dt )相等。第二个是要求相邻图像帧是小运动。第三个是空间一致性，它假定相邻像素之间具有相同的运动。光流法在视觉SLAM 中的前端被广泛使用，它具有响应速度快、计算量小等特点。但同时光流法受光照影响大、相机发生大尺度运动容易跟丢、图像的单个像素点没有区分度等缺点。

为了更好地适应实际环境，需要对传统的LK 算法进行改进。为提高光流追踪的速度，我们采用逆向组合[4]的方法。传统的LK 算法在更新参数的过程中需要不断计算雅可比矩阵和Hessian 矩阵，降低了光流的追踪效率。而使用逆向组合的方法可以使得算法在迭代过程中只需要计算一次Hessian 矩阵，这有效提高了光流算法的效率。传统的光流法对光照变化比较敏感，所以使用一种局部差异平方和[5]的形式，将处理单个像素点的形式转化为处理像素块的形式。我们把相邻图像间对应像素块的运动追踪问题转换为估计变换矩阵T。该变换矩阵T 最小化对应像素块之间的差值，我们将它定义为如下的残差形式。

图1 前端流程

2.2 IMU 预积分

由于纯视觉的定位方式难以实现鲁棒的定位要求，所以采用视觉融合IMU 的方式可以更好的让系统适应现实需求。根据IMU 模型，我们得到连续时间下的运动模型。

图2 不同噪声条件的参考轨迹和估计轨迹

2.3 系统框架

该系统主要分为基于光流法的前端、基于LM 非线性优化的后端、系统初始化部分。首先，光流法前端不断接收单目相机的图像数据，并对图像进行FAST 角点的提取，再使用光流法追踪相邻图像帧间的特征点，从而得到运动轨迹。同时，系统不断接收IMU 的加速度和角速度的数据，并对图像帧间的IMU 数据进行积分处理。然后进行系统初始化，该方法主要是借鉴了VINSMONO 的系统初始化[7]，可以恢复出尺度、重力、速度和IMU 的bias，从而将视觉轨迹和IMU 轨迹对齐。而后，将视觉和IMU 得到的轨迹放入到后端非线性优化当中，该部分主要使用了LM 算法不断迭代优化相机位姿，从而得到准确的相机运动轨迹。

3 实验与分析

该系统的实验环境是ubuntu18.04，使用多线程进行数据传输。同时，使用了pangolin 库可视化相机的运动轨迹。此外，生成自定义轨迹测试系统，并设置了三种噪声情况，分别是噪声轨迹1: acc-n: 0.02,gyr-n: 0.015,acc-w: 0.0001,gyr-w: 1.0e-5；噪声轨迹2: acc-n: 0.04,gyr-n: 0.03,acc-w: 0.0002,gyr-w: 2.0e-5；噪声轨迹3: acc-n: 0.2,gyr-n: 0.15,acc-w: 0.001,gyr-w: 1.0e-4。我们使用evo 评估系统在三种噪声情况下的运动误差，得到了不同情况下参考轨迹与估计轨迹的绝对误差，具体数值如表1所示。不同的噪声情况得到的参考轨迹和估计轨迹如图2 所示。实验结果表明，系统估计的轨迹能较好拟合参考轨迹，证明了系统对不同噪声环境的有效性和鲁棒性。但是，从表1 的数据分析，该系统会出现残差平方和较大的情况，而本次实验的场景不够丰富，且稳定性不足，因此，该系统还有待进一步完善。

表1 不同噪声条件的绝对轨迹误差