视觉惯导紧耦合的行人室内自主定位方法
曾继超1,2，许广富1,2，刘锡祥1,2
(1.东南大学 仪器科学与工程学院，南京210096；
2.微惯性仪表与先进导航技术教育部重点实验室，南京210096)
摘要:针对行人在大型复杂建筑环境中的高精度和高可靠性室内定位需求，传统的基于视觉点特征方法易受环境纹理缺失、相机快速运动导致图像模糊而定位失效问题，提出了一种基于视觉点线特征与IMU紧耦合的行人室内自主定位方法。在视觉惯导融合导航系统框架下，前端部分，在点特征基础上引入结构化建筑环境中丰富的线特征，并采取基于梯度密度过滤机制的改进线特征提取策略，剔除局部线特征密集区域；利用点线特征与IMU紧耦合优化机制提高行人位姿估计及定位的准确性和稳定性。通过利用EuRoC数据集和在实际楼道场景下的实验，特别是在弱纹理、光照变化等条件下实验，验证了所提方法进行行人室内定位的准确性和可行性。
关键词：点线特征；IMU；梯度密度过滤机制；室内自主定位
0 引言
导航定位关乎国防建设和个人生活，基于位置的服务需求日益强烈。目前室外环境下的全球导航卫星系统(global navigation satellite system, GNSS)定位技术已基本满足位置服务的需求，但GNSS信号因为建筑遮挡或干扰等因素无法提供室内定位服务。如何在现代大型建筑复杂室内环境中快速准确定位，满足紧急场景侦查救助、商业场景引导、物流场景跟踪等位置服务需求，室内定位技术具有重要的研究及应用价值日益成为研究的热点[1]。
目前室内主流的定位技术包括基于无线信号定位(Ultra WideBand, UWB/Wireless Local Area Networks, WLAN/红外线等)、基于惯性器件定位和基于视觉定位等[2]。然而，采用无线信号定位的方式，通常需要事先架设基站或安装感应器件，在复杂室内环境容易受到遮挡或信号干扰，应用场景局限且稳定性差；采用惯性器件定位的方式，具有体积小、响应频率高且不受外界干扰的优点，但由于消费级MEMS微惯性器件偏置和噪声在长时间积分过程中累积导致位姿解算发散，航迹推算和定位精度不可靠；采用视觉定位的方式，具有成本相对低廉、定位精度高、长时间测量下不会产生累积误差的优点[3]，但成像质量受场景光照变化或纹理丰富度影响大，且相机快速运动时容易导致特征误匹配跟踪失效从而定位失败。
在复杂室内场景下采用单一定位方式难以实现高精度、高可靠定位。融合多种传感器的组合定位方式，能够充分利用多种传感器间优势互补，提高定位精度和鲁棒性[4]。由于视觉观测信息作为IMU约束能抑制IMU的累积误差[5]，而IMU的高频特性能提供视觉失效时的位姿跟踪以及恢复单目相机的尺度信息，所以结合视觉和IMU的定位方式具有优势。然而，室内环境通常存在大面积弱纹理白墙、走廊或者单一重复纹理地板，通过引入线特征机制，能克服传统的基于视觉点特征的定位方式容易提取不到足够特征点或存在大量特征点误匹配导致定位失败的问题[6-8]。目前应用广泛的线特征检测算法有Canny边缘检测算法和LSD(LineSegment Detector)算法[9-10]。但Canny边缘检测算法实时性达不到行人移动定位需求，且针对室内复杂场景误检率高；LSD算法虽然检测速度快，但是缺乏筛选过滤机制，容易在图像的局部线特征密集区域提取大量相似线条，导致线特征误匹配和消耗不必要的计算时间，如复杂室内装饰元素：密集条纹装饰墙，室外的空调出风口网格、百叶窗等。
因此，本文提出一种基于单目视觉点线特征与IMU紧耦合的行人室内自主定位方法(Improved MonocularPoint Line—Visual Inertial Navigation System，IMPL-VINS)，前端部分处理IMU高频数据并进行预积分，图像预处理利用梯度密度过滤机制改进线特征提取，过滤线特征密集区域并提取点线特征进行关键帧匹配跟踪，通过优化机制充分利用IMU数据和视觉点线数据的关联性，得到行人的位姿估计和轨迹输出，提升行人在弱纹理、光照变化剧烈等复杂室内场景的定位精度和鲁棒性。
1 算法中的数学描述及测量模型
1.1 点线特征表示
2 改进的视觉惯性定位方法(IMPL-VINS)
基于视觉和惯导数据联合估计行人位姿是一个高度非线性的状态估计问题。本文参考经典的VINS-mono框架[15]，VINS-mono是基于视觉点特征和IMU紧耦合的同时定位与建图系统，其包含五个功能模块：数据预处理、初始化、后端非线性优化、闭环检测及闭环优化。
本文提出了一种改进的单目视觉点线特征与IMU紧耦合的行人室内自主定位方法(IMPL-VINS),系统的整体框架如图2所示，包含数据预处理、初始化、滑窗非线性优化机制估计位姿3个部分。其中边框加粗的部分为重点改进部分：引入线特征，在数据预处理部分针对线特征进行梯度密度过滤剔除局部线特征密集区域后再提取跟踪线特征；滑窗内进行点线特征及IMU数据的联合非线性优化。
2.1 数据预处理
数据预处理部分分3个线程分别处理IMU、点特征、线特征数据。针对高频的IMU数据，需要进行预积分，获得当前时刻的状态量：位置、速度、旋转，计算两个相机关键帧之间的预积分量以备和视觉约束一起在滑窗内进行非线性优化。点特征是人工设计的图像中的角点，角点检测算法发展较早且成熟，目前经典的算法有SIFT、SURF、FAST、ORB、Harris等。
考虑算法的复杂度和系统的实时性要求，本文采用Harris角点。对获取的单目图像进行金字塔分层并提取Harris角点，光流跟踪匹配特征点，通过RANSAC算法剔除匹配异常点。将特征点保存以备滑窗非线性优化之需。
2.1.1 基于梯度密度过滤机制的改进线特征提取
在线特征处理线程，首先通过梯度密度过滤机制[16]对单目图像进行剔除局部线特征密集区域的处理，再采用LSD算法提取线特征。梯度密度过滤机制的目的是只保留局部线特征密集区域的最边缘方框的线特征，主要思想是借鉴LSD的梯度检测思想，通过统计局部区域内像素梯度高于某阈值时判定为线特征密集区域，并在执行LSD线特征检测算法时跳过该区域。具体实施方式为：
本文每帧单目图像分辨率为3T×3T，首先设定为5的小区域，筛选出线特征密集区域，然后以该小区域为中心的的区域设置为线特征密集区域，不进行LSD算法检测线特征。图3和图4为添加梯度密度过滤机制的LSD算法线特征提取效果图。图像中空调压缩机的表面拦网是线特征密集区域，增加了梯度密度过滤机制的LSD算法只对空调压缩机的边缘进行线特征检测，减少了拦网区域不必要的线特征检测时间和线的误匹配。
2.2 初始化
初始化部分借鉴文献[15]的V部分具体内容，包括纯视觉的SFM(struct from motion)和视觉惯导融合对齐，其目的是为了下文在滑窗内进行视觉惯性紧耦合优化提供良好的初值。纯视觉的SFM通过连续帧间的点特征提取跟踪，由对极几何模型求解帧间的相对变换，并选择滑窗内具有相同观测但最远的两个关键帧进行三角化获得3D点的逆深度。
由于IMU带有零偏，且零偏包括是下文滑窗优化机制中要估计的状态变量，视觉惯性联合初始化可利用视觉SFM的旋转约束估计陀螺仪零偏初始值。加速度计零偏由于和重力作用比较影响甚微这里忽略不计。利用平移约束可以估计重力方向、速度以及单目相机的尺度信息。
2.3 滑窗非线性优化机制估计位姿
对于视觉惯导数据的关联性，可以使用基于图优化框架的紧耦合方法优化滑窗中的所有视觉惯导状态量，图5利用因子图的方式可以表明相机、路标点、IMU三者之间的约束关系。
3 实验
为了验证基于视觉点线特征与IMU紧耦合的视觉惯性定位方法(IMPL-VINS)的有效性与可行性，分别在公开数据集和楼宇走廊环境进行实验验证。
3.1 数据集验证
EuRoC数据集[17]是苏黎世联邦理工学院公开的一套经典的VINS系统测试数据集，由微小型无人机MAV搭载VISensor(相机频率20FPS，IMU频率200Hz)在大型工业场景中动态采集，并提供动作捕捉系统VICON采集MAV的定位数据作为真值轨迹。由于目前缺乏带有真值的室内大场景下的行人视觉惯导数据集，故本文采用可类比室内行人运动特征的EuRoC数据集进行定量实验。本文选取数据集中工业厂房场景的5个数据序列，其根据MAV的运动速度、环境纹理特征丰富情况、光线变化情况分为简单、中等、复杂3类。由于本文是单目 IMU定位系统，仅采用数据集的左目相机和IMU数据。
将本文方法IMPL-VINS与目前经典的方法VINS-mono通过绝对位置误差(absolute position error,APE)进行定量对比评估。VINS-mono是港科大开源的仅采用视觉点特征和IMU紧耦合的方式进行定位的方案。两种算法的实验均在内存为8GB，1.6GHz的Inter Core i5-4200U笔记本电脑中进行。
图6为本文方法IMPL-VINS在数据集序列MH_04_difficult（简称MH_04）场景下的点线特征提取效果图。MAV在该区域巡航时，存在环境纹理特征不丰富、光线条件不理想等挑战性因素，点特征提取效果不理想，而本文添加了线特征后的IMPL-VINS方法在该区域提取了丰富的线特征，弥补了单纯依赖视觉点特征的缺点。两种方法的定位轨迹图如图7所示，两种方法的定位轨迹与真值估计基本重合，但IMPL-VINS的轨迹与VINS-mono的轨迹相比较在大部分区域更接近真值，说明点线特征和IMU紧耦合定位方法提高了定位精度和鲁棒性，尤其是环境纹理特征不丰富、光线条件不理想的室内环境。
图8为两种方法在数据序列MH_04_difficult中每个时刻各自轨迹和真值轨迹的绝对位置误差(absolute position error,APE)曲线图。从图中可以看出，IMPL-VINS的绝对位置误差曲线总体上位于VINS-mono绝对位置误差曲线的下方，说明点线特征和IMU紧耦合定位方法总体上提升了定位精度。
图9表示了两种方法在3个不同方向上的位置随时间变化情况。由图中可知，在x轴两种方法都很好地符合真值轨迹。但在y轴和z轴上，本文方法IMPL-VINS比VINS-mono在整体上更接近真值轨迹。
表1给出了两种方法在5个数据序列下各自定位轨迹和真值轨迹的比对结果。由表1可知，本文改进的基于点线特征与IMU紧耦合的定位方法IMPL-VINS比基于视觉点特征与IMU融合的方法VINS-mono定位精度高, 均方根误差(root mean square error,RMSE)平均提升约10%。
3.2 真实场景验证
课题组搭建了室内行人定位装置：头盔上搭载小觅相机，背包中搭载英伟达的JetsonTX2开发板。由行人穿戴该定位装置在实验楼楼道和学生宿舍楼楼道进行实验。由于缺乏动作捕捉系统对行人进行定位作为真值轨迹，只进行定性比较实验。
1）实验楼楼道实验。
图10为IMPL-VINS在实验楼楼道提取点线特征的效果图，楼道环境内两侧墙壁存在大面积白墙，属于大面积弱纹理区域；楼道地板为水磨的水晶石地板，属于简单重复纹理区域；这两种情况极易造成点特征的失效和误匹配，导致基于视觉点特征的方法定位精度下降。且楼道中间段灯光昏暗，楼道尽头外界光线从窗户照入光线较强，属于光线变换剧烈的情况。分别在本文行人定位装置上运行两种定位系统，由实验人员穿戴行人定位装置沿着楼道预设轨迹从同一出发点出发，在楼道内折返后又回到原出发点，即出发点与终点重合，通过比较定位系统的轨迹始终点能否重合定量比较定位系统的优良。定位轨迹图11所示，轨迹总长80 m，改进的基于点线特征与IMU紧耦合的定位方法IMPL-VINS的定位轨迹出发点与终点近乎重合，而基于视觉点特征与IMU融合的方法定位轨迹终点严重偏离出发点，偏移约2.24 m。说明本文改进的定位方法提高了定位精度和鲁棒性。
2）学生宿舍楼楼道实验。
在学生宿舍楼，由实验人员穿戴定位装置，同样分别运行两种定位系统，进一步验证改进的IMPL-VINS定位方法在上下楼梯场景下定位的可行性和稳定性。实验人员的运动轨迹如图12所示，包括从某一学生寝室出发，穿过楼道，上楼梯，在上层楼道折返，下楼梯，回到原来出发的寝室。
图13为本文方法在宿舍楼道的点线特征提取效果图。两种方法的定位轨迹如图14所示，轨迹总长120 m。改进的基于点线特征与IMU紧耦合的定位方法IMPL-VINS的定位轨迹出发点与终点近乎重合，而基于视觉点特征与IMU融合的方法定位轨迹终点严重偏离出发点，偏移约3 m。说明本文改进的定位方法提高了定位精度和鲁棒性。
4 结束语
本文提出了一种基于视觉点线特征与IMU紧耦合的行人室内自主定位方法(IMPL-VINS)。借鉴VINS-mono的框架，引入线特征以及在预处理阶段采用梯度密度过滤机制剔除局部线特征密集区域，在滑窗中紧耦合优化点线特征和IMU数据获得行人位姿，并输出行人运动轨迹。最后在EuRoC数据集上定量对比改进的IMPL-VINS比VINS-mono的整体系统定位精度提高了约10%。在实验楼楼道和宿舍楼楼道两个场景验证了所搭建的实验平台及系统进行行人室内定位的可行性和稳定性。本文方法不涉及闭环检测和行人建图功能，增添点线库进行全局闭环修正可进一步提升定位精度，增添建图功能可进行路径规划适应更广泛的室内场景需求。
作者简介：曾继超（1994——），男，福建漳州人，硕士研究生，主要研究方向为基于相机和IMU融合的定位导航。E-mail: 1071868447@qq.com
基金项目：国家自然科学基金项目(61973079,51979041)
通信作者：刘锡祥 教授 E-mail: scliuseu@163.com
,
