基于改进图像增强的低照度场景视觉惯性定位方法

李磊磊，钟 傲，梁 琳，吕春明，左 涛，田晓春

（1.北京理工大学 自动化学院，北京 100081；2.北京自动化控制设备研究所，北京 100074）

由于传统的基于全球定位系统（Global Positioning System,GPS）的定位方法在拒止环境下无法正常工作，自主导航定位技术近年来成为了研究热点。视觉惯性定位方法作为自主导航的关键技术，具有传感器成本低、体积小等优点。同时，视觉传感器与惯性测量单元（Inertial Measurement Unit,IMU）特性互补，当视觉传感器在快速移动时失效，IMU 数据可以为系统提供短时间内的准确定位；而视觉传感器在系统低速运动时精度较高，可以有效减少IMU 长时间的累积误差。

得益于传感器的上述优势，视觉惯性同时定位与地图构建（Simultaneous Localization and Mapping,SLAM）近年来迅速发展，成为目前主流的视觉惯性融合定位方法。目前具有代表性的算法有VINS-Mono[1]、MSCKF-VIO[2]、VI-DSO[3]、StructVIO[4]、ORB-SLAM3[5]等。

视觉惯性导航系统对输入图像的质量要求较高，而低照度图像缺乏清晰的纹理特征，即使是上述这些最先进的方法，对照明变化的鲁棒性仍然很差，实验表明在低照度场景下容易出现位姿漂移甚至跟踪失败等问题[6]。在导航系统实际应用领域存在许多低照度场景，如全天候导航、井下机器人、夜间单兵导航等。而目前主流的视觉惯性定位方法由于无法提取到足够多的特征点，难以在低照度场景下实现准确定位。对于低照度场景图像的处理方法主要分为传感器处理和算法处理两个方向。文献[6]提出了一种基于单目红外相机的视觉里程计，使用红外相机解决低照度场景下图像纹理弱的问题，但传感器成本显著提高。而且与算法处理方式相比，红外图像纹理较弱,信噪比低，常用的特征点性能下降明显。文献[7]提出的LMVI-SLAM 算法在特征追踪前，采用自适应伽马校正和对比度限制自适应直方图均衡化相结合的图像增强方法对普通相机低照度图像进行预处理，解决了传感器成本高的问题，但该方法在黑暗场景效果较差；文献[8]使用生成对抗网络（Generative Adversarial Nets,GAN）对低照度图像进行增强，再利用ORB-SLAM2进行定位，有效提升了黑暗场景下的定位精度，但对实验平台计算资源要求较高。文献[7][8]为纯视觉SLAM 方法，在相机移动过快时容易跟踪失败，系统鲁棒性较差。

为解决上述问题，本文提出一种基于相机响应模型图像增强的视觉惯性定位方法。在保证低成本、计算资源需求小的前提下，融合惯性传感器，提升低照度场景下系统定位精度与鲁棒性。首先对低照度图像增强算法进行研究，设计了增强结合去噪的图像预处理模块。然后采用基于光流法的视觉惯性里程计实现低照度场景定位。最后搭建了一个视觉惯性实验平台，采集了多个低照度场景下的SLAM 数据集，并通过实验验证了本文定位方法的有效性。

1 系统框架

本文提出的低照度场景下视觉惯性定位算法流程如图1 所示。视觉传感器采集到的低照度图像首先进入图像增强模块进行图像预处理，对图像整体进行提亮同时展现出更多细节，接着进行图像去噪，便于后续的特征追踪。但增强后的低照度图像质量也难以达到正常图像水平，基于特征点匹配的特征跟踪方法难以实现[9]，本文采用光流法进行特征追踪，得到匹配点对。对视觉与惯性信息进行联合标定校准，后端采用滑动窗口优化方法修正关键帧位姿，以控制优化变量维度，实现位姿实时更新。

图1 算法流程图Fig.1 Algorithm flowchart

2 图像增强模块

针对低照度图像在增强后产生大量噪声的问题，本文在Ying 等[10]提出的基于相机响应模型的低照度图像增强算法基础上做出改进，设计了高斯滤波器对增强后的图像进行去噪处理，从而形成一个完整的图像增强算法。下面对算法理论进行分析并对实际低照度图像进行实验。

2.1 相机响应模型

由于低照度图像存在局部曝光不足的情况，为避免区域过曝，不能采用全局统一曝光率来曝光所有像素。将图像增强过程建模为：

其中，P、P′分别为输入低照度图像和增强后输出图像；G表示相机响应模型；K表示曝光率矩阵，包含了每个像素的期望曝光率，亮区域对应较小曝光率，暗区域对应较大曝光率。

通过对同一场景不同曝光图像直方图的观察，将相机响应模型确定为：

其中，p、k分别为某一像素值和该像素值对应的曝光率；g表示对每一个像素的增强函数；β表示线性放大系数；γ表示伽马校正系数；c表示该像素对应的辐照度；a、b为相机响应模型的参数。为保证算法在不同相机上的适用性，选取可以适合大多数相机的固定相机参数(a=-0.3293,b=1.1258)[10]。相机响应函数f如式(3)所示，描述了图像辐照度到像素值的转换。

已知像素值，由式(3)可求得对应的辐照度c。

2.2 曝光率矩阵

曝光率图与照明图T成反比，首先根据文献[11]算法求解照明图T，将待优化照明估计与初始估计值的差异和平滑度作为约束，建立目标优化函数为：

其中，x表示图像中的某一像素；I(x)表示像素x初始照明估计值；T(x)为待优化的照明值；α为平衡两残差项的系数；h、v分别表示水平与竖直方向；Wd(x)表示像素x的权重，如式(5)所示；∇dT(x)、∇d I(x)分别表示待优化照明值和初始照明值在d方向的一阶导数；ε表示无穷小量。优化目标函数，得到照明图T的最优解，对T中所有元素取倒数即得到曝光率矩阵K。

2.3 图像去噪

低照度图像经算法增强后会产生大量高斯噪声，图像中的高斯噪声与周围像素间存在灰度值的突变，影响角点的提取，不利于后续对图像进行特征跟踪。为减少图像噪声对特征跟踪的影响，提高系统定位精度，本文在上述算法基础上加入图像去噪模块，进一步提升图像质量。

考虑整个定位系统的实时性能，需保证图像去噪算法的低复杂度以及高效率，并且与上述图像增强算法相匹配。由于上述图像增强过程中产生的噪声主要为高斯噪声，本文选择高斯滤波器对图像进行去噪。

高斯滤波器取卷积核内像素的加权均值作为中心点灰度值输出，从而达到减小噪声、平滑图像的效果，高斯滤波器方程为：

其中，(x,y)表示卷积核内某一像素点坐标；G(x,y)表示该像素点的权重；σ表示高斯分布的标准差；e为自然常数。本文设置卷积核大小为3 × 3，σ设置为1.5。

2.4 增强效果

为评估本文图像增强方法的性能，采集了夜间室外低照度图像数据集，并使用本文方法对原始图像进行增强，将原始图像与增强后的图像分别进行光流追踪，效果如图2 所示。

图2 光流追踪效果对比图Fig.2 Optical flow tracing effect comparison chart

图2 中绿点表示光流成功追踪的特征点，可见使用本文方法增强后的图像有更多的特征点被成功提取和追踪，在低照度场景下对光流追踪效果提升明显，尤其是在图像中较暗的区域。

3 视觉惯性里程计

3.1 光流追踪

由于低照度场景下的图像特征及纹理不明显，不利于特征点提取与描述子匹配，特征点法容易失效。本文采用光流法进行特征追踪，在保证精度的同时，减少描述子匹配的耗时。

低照度场景下的视觉信息满足亮度恒定、时间连续、空间一致假设，本文采用LK（Lucas-Kanade）光流算法进行求解。光流追踪过程如图3 所示，设图像中某一点的像素坐标为(x,y)，该点在t时刻的灰度值为V(x,y,t)。由于相机运动，经过时间Δt后，该点运动到了新位置(x+Δx,y+Δy)，此时灰度值变为V(x+Δx,y+Δy,t+Δt)。根据亮度恒定假设，该点的灰度值保持不变，即：

图3 光流追踪示意图Fig.3 Schematic diagram of optical flow tracing

对上式左端进行泰勒展开，得：

β为二阶无穷小项，可忽略不计。将式(8)化简，得到：

式(9)等式两边同除以dt，得到光流约束方程：

取包含该像素点的一个邻域Ω，设邻域Ω 中有n个像素点，每个像素点具有相同的速度矢量(u,v)，则得到如式(11)所示的超定方程组。

利用最小二乘法求出方程组(11)的最优解(u*,v*)，从而得到经过时间Δt后像素点的位置，实现低照度图像的特征追踪。为确保在相机快速运动时满足要求，使用高斯金字塔[12]结合LK 光流提高准确性。

3.2 视觉/IMU 联合标定

为保证系统的定位精度，将联合视觉与IMU 信息，对陀螺仪偏置（bias）、速度（v）、重力向量（g）以及尺度（s）进行标定，标定过程如图4 所示。

图4 标定过程Fig.4 Calibration process

首先利用对极几何模型求解两帧间的旋转与平移，用三角测量求得两帧中观测特征点的三维坐标，再根据这些三维特征点与其他帧观测的二维特征点，使用PnP（Perspective-n-Point）求解滑动窗口中其他帧的位姿（平移、旋转）以及三维特征点。

将视觉与IMU 信息融合，将IMU 预积分得到的旋转信息作为估计值，将上述视觉初始化过程得到的旋转信息作为观测值，忽略测量噪声，两者之间的误差只与陀螺仪偏置有关。将滑动窗口内所有帧的旋转信息与IMU 旋转信息做差构建最小化误差模型。利用最小二乘法求解得到陀螺仪偏置。同理，利用视觉与IMU 的平移、速度信息作为约束，完成对系统速度、重力加速度以及尺度的标定[13]。

3.3 位姿修正

为减少估计误差，进一步提高系统定位精度，采用基于滑动窗口的优化方法，对位姿进行修正。维持滑动窗口内图像帧的数量不变，每当新帧加入窗口内，旧帧就会被移出，以此控制计算规模，使位姿估计可以实时更新。滑窗优化过程如图5 所示。

图5 滑窗优化示意图Fig.5 Illustration of sliding window optimization

基于滑动窗口构建视觉惯性紧耦合优化模型。将光流追踪结果作为特征点观测值，与三维特征点的投影位置作差，建立滑动窗口内特征点的视觉重投影误差；结合IMU 预积分误差、先验误差作为约束，优化目标函数，使总体误差达到最小，从而求得最优位姿估计。定义目标优化函数为：

其中，χ为优化变量，其中包含滑动窗口内每一帧的位姿、速度、IMU 偏置、IMU 到相机的外参以及三维特征点的逆深度。

IMU 预积分约束中[14]，A表示当前滑动窗口内所有IMU 测量的集合表示两个连续帧bl和bl+1之间IMU 的测量值，eA表示IMU 预积分测量误差，为IMU 预积分噪声的协方差矩阵。

视觉重投影约束中,B表示当前滑动窗口内至少被观测到两次的特征点集合表示第i个特征点在第j帧中的观测值，eB表示视觉重投影误差为视觉观测噪声的协方差矩阵，h(x)为Huber 范数。h(x)的定义如式(13)所示。

先验约束中，ep、Hp来自移出滑动窗口的旧帧[15]。移出滑动窗口后，旧帧的位姿不再参与优化，但旧帧与滑动窗口内其他帧之间的约束关系作为先验信息加入优化函数中，在控制优化变量维度的同时，不会减少约束信息，有利于求解精度的提高。

4 实验验证

为了验证本文方法的有效性，搭建了一个车载实验平台，在夜晚室外停车场环境进行实验，并与其他先进方法进行比较，验证本文方法在低照度环境下的效果。

4.1 实验平台

实验平台如图6 所示，分为软件和硬件两部分。软件部分基于ROS 系统开发，使用C++实现本文提出的低照度视觉惯性定位算法。硬件部分为自主搭建的车载实验平台，相机采用 PointGrey FL3-U3-32S2C-CS，采用8 mm 焦距镜头，IMU 以及GPS 设备型号为MTi-G-710-GNSS/INS，实验前已完成相机、IMU 参数标定以及联合标定。数据采集与算法处理在Intel NUC（Intel Core i7-5850EQ，16GB RAM）上进行。

图6 实验装置Fig.6 Experimental setup

4.2 夜间车载实验

实验开展地点为北京理工大学中关村校区，车载平台夜间围绕楼宇与停车场采集低照度数据集，包括低照度图像与IMU 数据，并使用GPS 数据作为轨迹真值，实验环境如图7 所示。将本文方法（图中用改进图像增强表示）的定位结果分别与无图像增强的方法和改进前的方法（图中用原始图像增强表示）相比，验证本文方法对提升低照度场景下定位精度的有效性。

图7 低照度实验场景Fig.7 Low-light experimental scene

车载实验平台围绕北京理工大学北区停车场以及6 号楼行驶，实验一全长约365 m，非闭环；实验二全长约445 m，闭环；实验三全长约445 m，闭环。实验结果如图8-13 所示。

图8 实验一结果Fig.8 Results of experiment one

图9 实验一误差图Fig.9 Experiment one error plot

分别对没有图像增强的视觉惯性系统、使用原始图像增强的系统以及使用本文改进后图像增强的系统进行定位实验，验证本文所提出的方法以及改进的有效性。根据图8、图10 和图12 可知，本文改进图像增强算法后的定位结果在整体上最接近轨迹真值，其次是使用原始图像增强的系统，最后是未使用图像增强的系统，实验结果表明本文方法以及改进是有效的。选用绝对轨迹误差（Absolute Trajectory Error，ATE）作为定位误差，即同一时刻真值位置与估计值位置的差，三次实验得到的定位误差结果如表1-3 所示。采用均方根误差（Root Mean Square Error，RMSE）来评估算法的定位精度，结果如表4 所示。

表1 实验一误差对比（单位：m）Tab.1 Comparison of errors in experiment one (Unit: m)

表2 实验二误差对比（单位：m）Tab.2 Comparison of errors in experiment two (Unit: m)

表3 实验三误差对比（单位：m）Tab.3 Comparison of errors in experiment three (Unit: m)

表4 定位精度（单位：m）Tab.4 Positioning accuracy (Unit: m)

图10 实验二结果Fig.10 Results of experiment two

图11 实验二误差图Fig.11 Experiment two error plot

图12 实验三结果Fig.12 Results of experiment three

图13 实验三误差图Fig.13 Experiment three error plot

从表4 可以看出，与没有图像增强的视觉惯性系统和使用原始图像增强的系统相比，本文改进图像增强后的系统在低照度场景下的三次实验中表现最好，平均定位精度相比前两种方法分别提高了25.59%和6.38%。

5 结论

低照度场景下由于图像特征提取困难，视觉惯性导航系统容易出现累积误差、位姿漂移甚至跟踪失败等问题，难以提供准确的定位。为了解决上述问题，本文提出一种结合图像增强技术的视觉惯性定位算法。基于相机响应模型，对低照度图像进行预处理，提高图像亮度和对比度，并利用高斯滤波器对图像进行去噪。同时采用光流法跟踪特征，构建视觉与惯性紧耦合优化模型实现位姿估计。车载实验表明，本文方法能在低照度环境下实现稳定准确的定位，定位精度与没有图像增强的方法相比提升明显。引入去噪模块后的系统定位精度得到进一步提升。

在极暗的环境下，图像增强算法会引入更多的噪声，对特征点的提取与跟踪带来更大的挑战。下一步研究将使用微光传感器以获得更好的图像质量，使系统能在更加黑暗的环境下实现准确的定位。