一种改进点线特征融合的双目视觉惯性定位算法*

丁 晓，晏玉祥，张永建，兰卫旗，白晓亮

（1. 航空工业成都飞机工业（集团）有限责任公司，成都 610092；2. 西北工业大学，西安 710072）

在航空航天等大型复杂产品装配过程中，装配工艺复杂[1]，由于时间和空间上的限制，目前装配工作大部分依赖操作人员对照工艺文件和现场环境展开装配，信息不集中且可视化程度不高，装配工作量约占整个产品研制和生产时间的50%[2]。利用增强现实 （Augmented reality，AR）辅助装配[3–4]可以在一定程度上解决采用二维纸质工艺文件带来的劣势问题，给操作工人提供一种真实与虚拟引导信息无缝衔接的混合现实操作场景，能提高操作的代入感和真实感，满足产品装配质量的同时也可以节省时间和成本。

增强现实辅助装配依赖虚拟对象和真实场景的注册一致性，定位技术是实现虚拟引导信息与装配现场实时融合的核心。增强现实辅助装配通常通过识别场景标签来对增强现实设备进行定位，但要求保证标签实时出现在相机视野中，才能实现装配引导的效果。这种传统的视觉定位方式，在纹理少的区域无法工作，当运动过快或者剧烈晃动的时候，相机拍摄的图像由于运动而导致模糊；惯性测量单元 （Inertial measurement unit，IMU）可以测量角速度和加速度，对于短时间内的快速运动具有较好的估计，但是在静止情况下测量值明显存在漂移。相比IMU，相机拍摄的照片不会存在漂移，使得相机在慢速运动之后的位姿估计依然有效[5]，但当图像发生变化之后，相机自身是难以感知运动的，而IMU 能够感受到本体的运动信息，可减少动态物体的影响。由于IMU 和相机具有明显的互补性，基于多传感器融合的位姿估计技术在增强现实方面得到了越来越多的应用[6]。

山东大学李民[7]采用IMU 和视觉标签Apriltag 融合的办法组合测量头盔的位姿，这种定位方式缺点在于光照对相机产生干扰使其检测不到标签。西安交通大学王崴等[8]在面向增强现实的算法中，提出一种基于互补滤波和无迹卡尔曼滤波的定位方案，该定位方法要求AR 头盔实时处在摄像机视野中，且头盔标记点易受光照遮挡影响。融合了全球定位系统 （Global positioning system，GPS）、IMU 以及方向传感器的融合方法可有效降低累积误差。在AR 应用中，为了与场景交互，即时定位与地图构建 （Simultaneous localization and mapping，SLAM）算法必须提供鲁棒的带正确尺度信息的相机位姿估计，视觉惯性定位技术成为主流的发展趋势。传统的SLAM 框架主要包括前端模块和后端模块。前端部分主要通过传感器来感知场景信息、估算位姿和速度等初始信息。根据处理方法的不同，后端接收前端的初始数据，可以分为基于滤波和基于优化两种方法[9]。

基于优化的方法使用了更多信息，随着计算机应用技术的发展，采用图优化处理SLAM 的方法逐渐替代滤波的方法成为主流，这其中需要采用第三方库g2o[10]、Ceres-Solver[11]、GTSAM[12]等来进行优化。图优化方法核心是将所有的点和边联合优化，对自身位姿进行全局最优处理。

Leutenegger 等[13]提出了一种基于关键帧的非线性优化的视觉惯性融合SLAM 系统，能够得到比较好的精度和鲁棒性。香港科技大学提出的VINS-Mono[6]系统在经典的视觉SLAM 框架之上融合了相机和IMU，视觉前端是利用KLT[14]光流进行角点跟踪，后端是采用了滑动窗口来进行非线性优化。此后，为了避免VINS-Mono 初始化不稳定的情况，提出了其双目版VINS-Fusion，根据双目立体获得深度信息的原理，可以实时提供稳定可靠的场景尺度信息。在视觉信息中，点特征是最常用的特征类型，但在弱纹理的情形下，采用点特征跟踪的方法可能会追踪不到足够的特征点而出现跟踪失败或者无法跟踪的情况[15]。沈冉[16]提出了一种点特征为主，线特征为辅的点–线融合的单目视觉惯性里程计方法，确保了系统在昏暗环境中提取跟踪的点线数据的稳定性，获得了较为鲁棒的定位效果，但其单目无法提供稳定可靠的深度场景信息。而针对结构化重复纹理场景，He等[17]提出一种使用点线特征融合的视觉前端、视觉和IMU 紧耦合后端优化的PL-VIO 算法，优化最小化误差函数，实现了较好的定位效果。但该算法不支持在线使用，且计算量大，不具备场景深度恢复功能。赵良玉等[18]提出了一种采用Shi-Tomasi 角点提取点特征信息，同时增加快速直线检测 （Fast line detector，FLD）提取线特征的点线特征融合的双目惯性SLAM 算法，在保证定位精度的同时能够获得更为丰富的环境地图，具备较好的鲁棒性。其采用的Shi-Tomasi 角点提取方法，可以获得较快的提取速度，但在使用KLT 光流法实现特征点的跟踪时，可能会因为相机的剧烈运动而导致帧间匹配失败，追踪不够稳定。

针对复杂装配任务、弱纹理下强旋转导致特征容易丢失，增强现实设备持续稳定的跟踪定位非常困难的问题，本文提出了一种改进的PL-VIO[17]（Improved PLVIO，IPL-VIO）算法，使用多传感器融合的位姿估计技术，通过视觉和IMU 的定位方式，对相机进行内参标定并进行误差分析，提高系统采集数据的精确度，对图像进行特征分析，提取特征点和线，并采用PL-VIO 算法为主体框架，针对点特征，采用ORB（Oriented fast and rotated brief）算法提取特征点，并改进光度不变假设，采用多层光流进行跟踪。与文献[18]中的Shi-Tomasi 角点提取相比，ORB 可以用来对图像中的关键点快速创建特征向量，并用这些特征向量来识别图像中的对象，因此帧间匹配时追踪更为稳定；针对线特征，通过改进的线段检测器 （Line segment detector，LSD）提取算法，建立合适的空间表征描述方法，同时进行双目左右相机之间的匹配计算位姿。采取视觉和IMU 紧耦合的方式，建立基于点线特征的视觉和IMU 融合的位姿误差融合模型，通过对比PL-VIO 算法试验，本文提出的IPL-VIO算法在结构化场景下的绝对位移和旋转误差比原算法误差更小，同时结构化场景信息更加丰富，能够应用在弱纹理的AR 装配现场中，为增强现实辅助装配平台提供稳定可靠的位姿数据，以提升多传感器融合的位姿估计精度，实现AR 设备的实时定位。

1 多传感器标定

相机与IMU 的标定包括在线标定和离线标定两种方式，在线标定是指在定位系统初始化过程中估计相机与IMU 的外参，方便可靠，但对初始化过程要求比较高。考虑到装配场景及装配任务的复杂性，难以保证完成在线标定。因此，本文主要采取离线标定的方式，采用Rehder 等[19]提出的集成多相机标定、相机和IMU标定的离线标定工具Kalibr，在估计出ZED Mini 相机内参之后，可进行优化迭代操作。ZED Mini 相机是立体相机公司Stereolabs 研发的一款迷你版的立体声深度测绘摄像机，可以作为虚拟现实 （Virtual reality，VR）头显HTC VIVE 的增幅设备，为头显提供实时的拍摄视频传输、深度感知和环境映射，让VR 头显转变为AR 头显。在连续运动场景下标定相机系统的内外参，同时还可以标定相机和IMU 的相对位姿，以及相机的重投影误差信息。

双目标定试验结果包含相机的左目和IMU 单元的相对位置信息TIC和TCI，即

2 图像特征提取与跟踪

2.1 图像特征提取

特征是图像信息的另一种数字表达形式。在视觉SLAM 中，常用的特征点提取算法有SIFT[20]、SURF[21]、FAST[22]、ORB[23]和BRIEF[24]等。SIFT 特征点具有信息量丰富、高速性、可扩展性强的特点，具有较强的图像特征提取鲁棒性，对旋转、尺度缩放、亮度变化保持不变。SURF 改善了SIFT 计算复杂度高的缺点，对特征向量描述进行了改进。ORB 是一种快速特征点提取和描述的算法，其结合了FAST 特征点的检测方法和BRIEF 的特征描述子的优点进行改进和优化。BRIEF特征点提取算法与SIFT 的提取方法一致，不同之处在于其在特征点附近随机选取若干点，根据灰度值的大小，描述为二进制字符串，大大加快了描述子建立速度，但旋转不变性差。

检测匹配主要流程是首先对采集的照片进行特征点的提取，分别提取SIFT、SURF 和BRIEF、ORB 特征，并两两进行特征点的比较和匹配，同时对匹配的结果进行RANSAC[25]提纯计算，计算出其中的内点。统计匹配的特征点（即内点）所占总特征点数目的比例来计算内点率，再根据内点率及匹配耗时计算获得每个内点所需要的平均耗时 （即时效比），为保证较高的内点率，调整合适的提取特征点数量。建立模型，即

内点率=匹配内点数/特征点总数 （1）

时效比=内点率/耗时 （2）

传统SLAM 系统中均采用点特征来进行跟踪，而在装配环境中，由于工人头部转动导致图像模糊，无法完成定位。而装配场景存在结构化的线特征，因此采取LSD[26]直线检测方法提取装配场景中的点特征进行匹配。

5 种检测匹配算法的时效比分析如图1 所示。分析可知，SIFT 和SURF 检测的准确率高；ORB 的检测匹配速度最快，时效比最高；LSD 的匹配效果更好，但是检测比较耗时。单独从特征提取的角度来看，SIFT、ORB、BRIEF 耗时较少。考虑到结构化的装配场景，为了保证在复杂狭小空间内工人头部转动和相机剧烈抖动的情况下追踪稳定，选用匹配效果较好的LSD 算法进行直线特征提取，下文主要以ORB 点特征和LSD 线特征为例进行研究。

LSD 算法用来表征场景的结构特征，但无参数调整的LSD 算法在图像模糊或者有噪声时，易检测出过多的短线段特征，另外，检测时一条长线段容易分割成多条线段，破坏了线特征的完整性，因此通过改进LSD 算法进行线特征提取。首先通过图片大小设置最小长度阈值进行线段长度的初步筛选，LSD 检测线段的两个端点分为起点和终点，根据线段支持域的最小外接矩形的主方向、线段之间的距离、线段的最近两个端点之间的距离和线段的描述子之间的距离来判断两条线段是否在同一条直线上，如果满足阈值条件则合并两条线段，减少短线段的干扰。

2.2 图像特征跟踪

结构化装配场景下特征提取点的提取和描述子的计算非常耗时，为了克服这个缺陷，本研究采用直接法来进行特征点的跟踪，即根据像素灰度差异值来计算，而且装配场景存在明暗变化的情况，直接法可以直接工作[5]。本研究以文献[27]为基础，使用双目相机和IMU 进行耦合。在装配场景中，工人头部在转动时，利用IMU 计算头盔空间位姿，从而预测图片特征点的位姿，再利用非线性优化 （如滑动窗口优化）来估计位姿。在特征点匹配完成之后，利用随机采样一致性 （Random sample consensus，RANSAC）的方法进行异常匹配对的移除。

本研究使用Lucas-Kanade 光流法进行特征跟踪，由于场景中的亮度是改变的，因此使用参数α和β来校正两个图像间的光度变化[28]，图像Ik和Ik+1之间的光度残差为

式中，ΩL是内点的集合；ξ表示从Ik到Ik+1变换矩阵；u为像素坐标；u'为图像中u对应点像素坐标。

3 基于视觉和惯性融合的位姿误差融合模型

为了在AR 装配过程中完成真实与虚拟装配场景的融合，需要计算AR 设备的位姿估计，位姿主要包含相机的旋转和平移变换，其6 维度的状态变量x可以表示为

式中，R和p代表相机的旋转和位移。当相机运动起来后，场景中特征点比较多，特征点的管理较为复杂，为了更好实现位姿的估计，需要参考当前时刻周围的位姿，由于地图特征可以通过边缘化的方法转换为位姿间的约束，简化为对位姿序列的估计[29]。可以用位姿图的方式来直接描述。纯视觉里程计位姿估计因子图中的节点对应相机在不同时刻的位置和姿态，而边则描述了位姿与位姿间的空间约束关系，这种约束关系通过里程计或观测信息的配准得到。

在基于点线融合的视觉里程计局部地图中，相机观测到视野中的点特征和线特征，根据投影方程建立点和线特征的误差模型，对优化的R和p进行状态约束。在对IMU 的参数标定试验中，通过对IMU 的零偏变换曲线分析可以得出零偏是一个动态量，其满足一阶马尔科夫的性质。本文采取一种紧耦合的方式使用相机的位姿、速度和IMU 的零偏构建目标方程进行状态估计。系统的状态变量x共15 维，可以表示为

式中，R、p和v分别是相机的旋转、平移和速度；ba和bg分别是IMU 的加速度计和陀螺仪的零偏。为了同时对状态变量进行优化，采用捆绑调整[30]（Bundle adjustment，BA）的方法，同时本研究根据固有的核心BA 算法，将原有的BA 问题分解成若干因子图，如图2 所示，在因子图中，各个子图具有各自的局部坐标系并且可以进行优化。

图2 点线融合的视觉惯性里程计位姿估计因子图Fig.2 Position and pose estimation factor diagram of visual inertial odometer based on point and line fusion

在图2 所示的视觉惯性里程计的位姿估计图中，首先计算出每一帧的位姿、速度和零偏。然后将位姿估计问题转化为联合优化问题，优化状态变量x主要包括相机的位姿、旋转速度以及IMU 的零偏，通过最小化特征点和IMU 的误差来优化当前帧的位姿，误差方程可以表示为

而针对线特征重投影误差[31]，图优化问题的本质是找到一个最优的状态值p，如果利用测量协方差矩阵Σz的多元正态分布对似然分布进行建模，则状态值p*可以表示为

式中，Z为图像中线特征测量值；εZ为线特征的投影预测误差。

综上所述，最小化特征点、线特征和IMU 的误差方程可以表示为

4 定位精度试验与分析

为了进一步研究IPL-VIO 算法在复杂场景下的定位精度，选取公开数据集，通过计算公开的数据集数据，对现有的算法和IPL-VIO 算法进行评测，公开数据集的选取要反映出线束装配场景的复杂性，存在结构化特征、重复纹理、弱纹理的特点，同时也要体现出装配任务的复杂性。本研究选取EuRoc 数据集，它是著名的应用于视觉惯性导航的数据集之一。用数据集的好处在于数据集的采集使用高精度的设备，可以提供精准的真值进行对比，减少了因设备和客观因素导致的误差。

EuRoc 提供20 Hz 的双目相机图像、200 Hz 的IMU数据和相机真实的运动位姿，选取和装配场景类似的Machine Hall 离线测试集，图像分辨率为752×480，频率为20 Hz，IMU 频率为200 Hz，从均方根误差、均方根旋转误差、运行时间、GPU 占用率和内存占用率这5个方面对VINS-Fusion[6]、PL-VIO 和IPL-VIO 算法进行对比。VINS-Fusion 是香港科技大学Qin 等[6]提出的一种基于双目视觉惯性的状态估计算法，采用紧耦合、非线性优化的方法，融合预积分后的IMU 测量值和特征观测值，获得高精度的视觉里程计。而PL-VIO 在VINS-Mono 的基础上加入了线特征约束。

试验采用的计算机配置：CPU 为i7–10750 的12 核英特尔处理器、16 GB运行内存、Ubuntu18.04操作系统。为充分测试算法处理效果，数据集回放速率为正常速率的1/2，以此减少由于显示和加载导致的数据延迟问题。

VINS-Fusion 运行测试数据集MH_05_difficult 的运行效果双目特征检测与跟踪基本一致，特征点分布均匀。表1 所示为VINS-Fusion 运行数据集时的参数，可以看出，运行数据集MH_03_medium 时位移绝对误差最小，运行MH_05_difficult 时旋转绝对误差、内存占用率最小。

表1 VINS-Fusion 运行数据集的参数Table 1 Parameters of the VINS-Fusion run data set

使用PL-VIO 运行测试数据集MH_05_difficult 的运行效果点线特征检测与跟踪效果基本一致。表2 所示为PL-VIO 运行数据集时的参数，可以看出，运行数据集MH_03_medium 时位移绝对误差最小，运行MH_05_difficult 时旋转绝对误差、运行时间和CPU 占用率最小。和VINS-Fusion 相比，PL-VIO 在绝对位移和旋转上有所提高，由于线特征的提取和计算耗费更多的资源，使运行时间和CPU 占用率显著提升。

表2 PL-VIO 运行数据集的参数Table 2 Parameters of the PL-VIO run data set

使用IPL-VIO 算法运行测试数据集MH_05_difficult，运行效果如图3 所示，点特征提取分布均匀，跟踪效果良好。双目基本可以检测出场景的线特征，且双目相机的左右目匹配一致，跟踪效果良好。

图3 IPL-VIO 算法测试数据集的点线特征跟踪Fig.3 Point and line feature tracking of IPL-VIO algorithm test data set

使用IPL-VIO 算法运行测试数据集MH_05_difficult，位姿估计效果如图4 所示，可以发现基于双目的线特征检测数量较多，且IPL-VIO 算法可以较好地反映出局部线段纵横交汇处的结构信息，丰富的场景信息有利于后期场景深度恢复和地图构建。

图4 IPL-VIO 算法测试数据集点线特征位姿估计效果Fig.4 Pose estimation effect of point and line feature in test data set by IPL-VIO algorithm

表3 所示为IPL-VIO 算法运行数据集时的参数，可以看出，运行数据集MH_03_medium 时位移绝对误差最小，运行MH_05_difficult 时旋转绝对误差、运行时间和CPU 占用率最小。

表3 IPL-VIO 算法运行数据集的参数Table 3 Parameters of run data set by the IPL-VIO algorithm

VINS-Fusion、PL-VIO 的平均位移绝对误差分别为0.19550 m 和0.16736 m，IPL-VIO 算法的平均位移绝对误差为0.14035 m，相比于VINS-Fusion、PL-VIO，IPLVIO 算法平均位移绝对误差最小。VINS-Fusion、PL-VIO的平均旋转绝对误差分别为0.02755°和0.02331°，IPLVIO 算法的平均旋转绝对误差为0.01949°，相比于VINSFusion、PL-VIO，IPL-VIO 算法平均旋转绝对误差最小。而在运行时间、CPU 占有率上，IPL-VIO 算法相对于VINS-Fusion、PL-VIO 都有一定的增加。总之，和VINSFusion、PL-VIO 相比，IPL-VIO 算法在绝对位移和旋转上有明显提高，但同时在PL-VIO 的基础之上，由于双目线特征余数的存在以及优化变量的增多，使运行时间和CPU 占用率显著提升。

综上所述，通过对比VINS-Fusion 和PL-VIO 算法试验，双目定位相比单目定位精度更高，且场景恢复信息丰富；IPL-VIO 算法在结构化场景下的绝对位移和旋转误差比原算法误差更小，同时结构化场景信息更加丰富，但耗费计算资源较原来的算法多。

将IPL-VIO 算法应用到座舱线束引导装配定位中，试验采用的头戴式设备HTC VIVE Pro 为一种虚拟现实头戴式设备，可以通过基站进行定位，也可用连接的摄像头观察外部真实世界实现增强现实的效果。试验采取远程协同的方式，通过专家端和工人端来实现复杂产品装配。使用本系统装配试验的若干关键环节包括：（1）装配工人启动装配系统，装配系统通过相机与HTC坐标原点对齐等完成初始化任务； （2）装配工人通过装配中心查看装配任务，以座舱下某管线敷设为例，系统提示工人佩戴挂载ZED Mini 相机的头盔并在装配现场通过AR 场景演示装配零件状态信息，便于学习； （3）当在零件区拿取零件之后，AR 相机渲染装配模型信息于座舱上，信息中心提示装配人员进行装配操作； （4）完成后信息中心提示进行安装检查操作，结束后信息中心下达结束该工序命令。

试验以线束敷设安装为例，为便于展示虚实融合效果，本文以第一和第三视角进行混合叙述。开始后，试验人员根据系统提示佩戴AR 辅助终端，在装配现场通过AR 效果学习线束系统布局的大体空间位置信息。开始敷线操作时，试验人员拿取束线管之后，前往座舱装配区，此时系统将束线管和座舱模型融合，试验人员在现场观察虚实融合情况，图5 所示为试验人员第三视角和信息管理中心的第一视角下的虚拟束线管与座舱融合情况，可以看出，系统对试验人员头部的虚实遮挡情况处理得比较好。然后试验人员根据系统提示，按照束线管的走向和布局进行束线操作。

图5 座舱线束引导装配定位效果图Fig.5 Guiding assembly and positioning rendering of cabin wiring harness

装配人员可以随意走动观察零件空间位置信息，线束模型定位稳定，与真实场景融合良好，且装配人员在走动时，AR 头戴式设备以及与其绑定的相机也随之运动的情况下，虚拟线束模型在真实的座舱中虚实注册稳定，没有出现模型剧烈移动现象。

5 结论

提出了一种点线特征融合的双目视觉惯性定位算法，并对该方法进行了全面的研究，试验结果表明，本文提出的IPL-VIO 算法误差更小，定位精度较高，能够应用在弱纹理的AR 装配现场中，为增强现实辅助装配平台提供稳定可靠的位姿数据。近年来随着AR 技术的发展，特别是在大场景弱纹理的AR 装配现场，本方法有着较大的应用前景。下一步将考虑采取结构化的直线代替装配场景中杂乱无章的线段，这将在线段的提取、匹配上节省一定的时间和算力，从而减少定位的时间并提高定位的精度，更好地实现增强现实设备的实时定位。