一种基于视觉二维码/惯性融合的室内高精度定位定姿方法

牛小骥，王庭蔚，葛雯斐，旷 俭

（武汉大学卫星导航定位技术研究中心，武汉 430079）

基于位置的服务（Location Based Services,LBS）给目标用户工作生活带来了巨大的便利，已成为日常工作生活的必备基础服务。相较于单一的位置信息，位姿组合信息能更细腻地反映目标动态，从而实现更准确的状态监控。在医疗健康、运动员训练等需要高精度位姿的领域，具有良好的应用前景。由于全球导航卫星系统（Global Navigation Satellite System,GNSS）信号无法穿透到室内环境，所以室内定位定姿通常采用WiFi[1]、超宽带（Ultra WideBand,UWB）[2]、激光雷达[3]、视觉[4]等手段。表1 给出现有的室内定位方案及其优缺点。WiFi 指纹匹配方法定位精度低（例如10 m），且指纹库不稳定，需要频繁更新；UWB 方法需要提前布设信号基站，安装复杂，且受室内环境影响无法提供精准可靠的姿态信息；激光雷达方法利用激光测距原理获得自身与周围物体的位置关系来实现定位，能够提供精准的位置和姿态，然而其造价昂贵且较为笨重，无法应用于人体穿戴等应用场合。视觉方法通过拍摄周围环境直接获取物体与相机的相对位姿，能够以低成本提供高精度的位置和姿态估计。

典型的视觉技术方案有视觉里程计（Visual Odometry,VO）。具体来说，通过单目或双目获取的图像序列进行特征提取、特征匹配与跟踪、运动估计的处理，完成相机的位姿更新。比较经典的VO 系统有并行跟踪与建图（Parallel Τracking And Mapping,PΤAM）、早期的基于ORB 特征的同步定位与建图（Oriented FASΤ and Rotated BRIEF-Simultaneous Localization and Mapping,ORB-SLAM）等。由于VO对图像流的连续性和成像质量有很高的要求，而往往又容易受动态和光照的影响而难以保持连续性和高成像质量，所以视觉与惯性传感器（Inertial Measurement Unit,IMU）组合用以获得更好的定位定姿效果，称之为视觉惯性里程计（Visual Inertial Odometry,VIO），代表方案包括ORB-SLAM3[5]、OpenVINS[6]等。然而，VO[7]或VIO[8]是相对定位方法，只能够短时间内提供高精度的位置和姿态估计结果，无法满足需要绝对位姿的应用需求。

为了获得长期稳定且可靠的绝对位置和姿态估计，通常采用预先布设二维码或摄像头的视觉定位技术。根据摄像头与用户的位置关系可分为：1）在场景中布设好摄像头，并测定摄像头之间的相对位姿关系，用户在摄像头的拍摄范围内移动，通过摄像头拍摄用户直接获取用户的位姿信息。2）在场景中布设二维码，并标定二维码之间的相对位姿，同时将摄像头与用户绑定在一起，通过摄像头拍摄周围环境获取摄像头的位姿信息从而反映出用户的位姿信息。前一类方案的典型应用主要由国外OptiΤrack[9]和国内的NOKOV[10]，通常由数个高速摄像机环绕目标进行摆放，从不同角度对贴在目标关键节点（如人体的关节处）的特征标签进行跟踪来完成全身动作的捕捉[11]，能达到毫米级的定位精度，在特效电影和虚拟现实[12]（Virtual Reality,VR）中获得了广泛的应用。然而，由于设备高昂、应用成本高，需要特定环境搭建，操作上也需要专业人士指导等问题，无法应用到普通大众用户。后一类方案依赖VO 或VIO 的相对定位能力，并通过周期性地检测到布设的二维码进行绝对校正，从而实现长期连续可靠的高精度位姿跟踪[13]。利用摄像头拍摄二维码获得其位置，协助机器人或用户在室内环境中进行定位。上述论文中利用了二维码进行了辅助定位，并取得较好的效果。然而，这类方案依赖于连续图像的可靠性，不能容忍图像出现丢失或者模糊现象，稳定性有待进一步提高。

为了实现低成本且高精度的用户位置和姿态的稳健估计，本文提出了一种基于视觉二维码/惯性融合的定位定姿方法。该方法使用单个摄像头、单个微机械惯性传感器（MEMS-IMU）和ArUco 码（一种OpenCV中的标识二维码），以捷联惯导算法为核心，通过拍摄预先布设且坐标已知的ArUco 码获得高精度的位置修正信息，并使用扩展卡尔曼滤波进行信息紧耦合。该方法将视觉图像作为稀疏的校正信息，当视觉图像出现丢失或模糊时仍然能够利用IMU 进行连续可靠的推算。更进一步，该方法基于事后处理模式的优势，采用了反向平滑算法融合未来时刻观测信息以提升当前时刻的定位定姿精度，能够更有效地应对摄像头短暂遮挡或图像模糊等问题，解决传统方案依赖于连续图像的可靠性，不能容忍图像出现丢失或者模糊现象，进一步增强系统的鲁棒性。

1 理论与方法

主流的视觉定位方法（例如VO 或VIO）通常假设图像是连续可用的，然而实际应用中这是难以保障的。针对这一典型问题，本文选择以惯性导航为核心，充分发挥惯性导航完全自主工作、不受外部环境影响的优点；并结合稀疏的高精度视觉观测信息校正，能在视觉图像连续性差和可靠性低的情况下，形成一个连续可靠的高精度定位定姿方案。图1 给出本文方法主要算法流程。本文采用IMU/视觉紧耦合算法框架，首先进行惯导机械编排解算，同时检测二维码角点，并结合预先测量的角点坐标修正惯导误差。为了应对二维码角点检测失败的情况，算法除了利用二维码角点构建观测方程，还实时估计环境中新的特征点坐标，在二维码角点检测失败的情况下，利用这些新的特征点构建观测方程，有效地维持系统的定位定姿精度。更重要地是，该方法基于离线处理模式的优势，采用反向平滑算法进一步增强系统的鲁棒性。

图1 基于二维码的高精度视觉/IMU 融合定位定姿算法框图Fig.1 Block diagram of high-precision vision/IMU fusion positioning and attitude determination algorithm based on QR code

1.1 惯导机械编排

机械编排是经典的惯性导航算法，通过对IMU 输出的角速度和比力进行积分，并结合前一时刻的位置、速度和姿态，从而获得用户当前的位置、速度和姿态。由于MEMS-IMU 器件性能较低，微小误差改正项对导航性能的提升没有明显帮助。因此，本文忽略微小的误差改正量（例如地球自转、圆锥效应和划桨效应等），且参考系(本文称n 系)选择室内自定义的坐标系，具体如下[14]：

1.2 滤波设计

基于MEMS-IMU 的惯导算法将会在几秒时间内迅速累计几米位置误差，无法满足用户需求[15]，因此需要融合其他可用信息间断地对惯导推算的位置进行有效校正。卡尔曼滤波（Kalman Filter,KF）是最常用的数据融合方法之一。然而，经典的卡尔曼滤波是基于线性系统的，为了应对非线性问题，本文采用扩展卡尔曼滤波器（Extended Kalman Filter,EKF）。其中，15 维误差状态向量构建如下：

式(2)中，δ rn，δ vn，ψ1×3分别为n 系下位置误差、速度误差和姿态误差；bg和bf分别为陀螺零偏和加表零偏。离散后的误差模型表示如下：

式(3)中，下标k,k-1为历元编号，δxk,k-1为误差状态预测值，Φk,k-1为误差状态转移矩阵，δxk-1,k-1为误差状态估计值，w k为系统噪声和测量噪声。对式(1)进行扰动分析得到：

式(4)中，I3×3为3×3 的单位矩阵；Δt为相邻历元的时间间隔；为加速度计观测值在n 系下的投影。

1.3 视觉量测更新

本文通过拍摄人工布设的二维码（坐标已知），实现稀疏图像条件下的高精度位置和姿态估计。

1.3.1 ArUco 码布设及坐标测量

本文采用的二维码为OpenCV 中ArUco 码，为了保障摄像头能够拍摄到ArUco 码，本文在房间内的四壁张贴一定数量的ArUco 码。采用自定义的当地水平坐标系，x 轴与y 轴构成的平面平行于当地水平面，z轴与x-y 平面垂直指向地面，其中坐标系原点和x 轴指向可由用户自定义在房间地面上某处。然后，将全站仪架设在坐标系原点，并将全站仪初始方向对齐x轴。在ArUco 码的坐标测量阶段，首先拍摄一张覆盖墙上所有ArUco 码的图像，其次使用Leica ΤS60 全站仪测出分布较远且不在同一直线上的ArUco 码四个角点的坐标，接着利用平面约束和坐标转换关系解算出所有ArUco 码的角点在世界系下坐标，最后将ArUco码角点的ID 号及对应的三维坐标存储，即完成坐标测量工作（即ArUco 码建库）。同时，基于已有文献阐述的ArUco 码距离与识别效果的关系[16]，本文采取的拍摄距离约1～5 m。

1.3.2 ArUco 码角点量测更新

观测量z为像平面坐标系下ArUco 码角点像素坐标的预测值（基于惯导推算的位姿）和观测值（基于图像处理提取角点）的差，具体描述如下：

接着，将式(7)代入到式(6)，进行扰动分析并保留一阶项，得到ArUco 码角点的紧组合量测方程：

式(8)中，量测矩阵由H1和H2相乘得到，x为系统误差观测向量，n为观测误差向量。H1和H2可由对式(6)(7)分别进行扰动分析并保留一阶项得到：

1.3.3 三角化量测更新

视觉/IMU 紧组合量测更新依赖于相机正确地检测并识别到ArUco 码。而由于设备的晃动和受ArUco码排布的密度影响，检测不到ArUco 码的情况时有发生。为了应对这种情况，当检测到ArUco 码时，同时对非ArUco 码的其余环境特征进行三角化估计，将估计的位置存入滑窗当中；当检测不到ArUco 码时，滤波器依靠之前三角化后的其余环境特征构建量测方程，从而保证滤波结果的稳定，提升系统的鲁棒性。

假设一个特征点p被相机多次拍到，第1 次被观测到的相机坐标系表示为c(1)，第i次被观测到的相机坐标系表示为c(i) (i＜N，N为滑动窗口大小)，由于拍摄时刻的相机位姿已知，则可以将第一次观测到的特征点p投影到i时刻的相机坐标系中：

如图2 所示，此时没有检测到ArUco 码，但是可以通过提取其他环境特征。并通过前面的三角化估计非ArUco 码特征点的三维位置，此时仍然可以继续进行滤波优化，得到当前相机位姿的最优估计值。最后，当前时刻的相机位姿、特征点的坐标与当前时刻进行关联后，作为滑动窗口内最新时刻的相机信息进行储存，如果滑动窗口已满，则剔除最旧的相机时刻信息。当然，这种在滑窗内三角化的特征点只起到ArUco 码角点识别失败情况下维持滤波的辅助作用，宏观的绝对位置修正还是依赖识别出的ArUco 码角点。

图2 ArUco 码检测失败Fig.2 ArUco code detection failed

1.4 平滑算法

由于载体高动态或摄像头被遮挡的条件下难以保证拍摄图像的质量，本文加入平滑算法进一步提升系统的定位定姿精度和可靠性。平滑算法能够充分利用过去、现在以及将来的观测信息提升当前位置和姿态的估计精度，是事后处理模式的必然选择。平滑算法思路具体描述如下：在计算固定区间[ 0,n]平滑值时，先进行前向卡尔曼滤波，记录下每一步的状态转移矩阵Φk,k-1、预测误差协方差阵Pk,k-1和滤波结果x、滤波协方差阵Pk。然后再从最末时刻向最初时刻进行平滑，最末时刻的状态估计为后向平滑的初始状态估计[18]。

状态平滑估计值：

其中，Ak为平滑增益矩阵，xk为k时刻的前向滤波结果，为k+1时刻的平滑结果，为k+1时刻前向滤波结果。

平滑增益：

式(13)中，Pk，分别为k和k+1时刻协方差阵，为k时刻转移矩阵。

平滑误差协方差阵为：

式(14)中，Pk，分别为k和k+1时刻协方差阵，为k时刻转 移矩阵。式(12)-(14) 中，k=n-1,n-2 … 0，n为观测信息总数。

2 实验描述

2.1 实验设备及场景

测试场景为空旷的房间，包含两个窗户和一扇门，尺寸为7 m×6.7 m×2.8 m，位于武汉大学诗琳通地球空间信息技术协同创新中心大楼四楼。除了门和窗户区域，房间其他区域都布设了大小为20 cm×20 cm 的ArUco 码。具体布设分布和场景如图3 所示。

图3 测试场景中ArUco 码分布Fig.3 ArUco code distribution in test scenario

相机型号为MYNΤ EYE D50 双目摄像头（以下简称小觅相机，本文仅使用单目），搭载同步采集的IMU 模块（BMI088）。本实验的图像采集仅使用小觅相机的左目相机，图像分辨率为1280×720。为了获取高精度的位置和姿态参考，数据采集阶段将Leica360 °棱镜、SΤIM300（高精度微机械惯导）和小觅相机固定在一起，安装在三脚架上组成本次实验的采集模块，安装关系如图4 所示。其中，Leica ΤS60 用于提供位置参考值，SΤIM300 与ΤS60 组合提供连续姿态参考（定位精度：1 mm，定姿精度：0.03 °）。小觅相机自带的IMU 与SΤIM300 的器件参数如表2 所示。为了确定各坐标系之间的转换关系，实验开始前用Kalibr工具测出小觅相机左目相机坐标系（c 系）与其自带的IMU 坐标系（b 系）的外参、SΤIM300 坐标系（l系）与左目相机坐标系（c 系）的外参，并测量Leica棱镜中心与左目相机坐标系（c 系）的杆臂关系。

图4 小觅相机、棱镜、SΤIM300 安装关系（白色坐标框架为参考设备，红色坐标框架为实验设备）Fig.4 Installation relationship of MYNΤEYE,prism,and SΤIM300 (the white coordinate frame is the reference equipment,and the red coordinate frame is the experimental equipment)

表2 实验中两个IMU 的关键参数Tab.2 The key parameters of the two IMUs in the experiment

由于Leica ΤS60 只提供低频率的高精度位置，且不能提供准确的时间戳。因此，精度评估实验时，通过使用特定的测试流程实现参考系统与被测系统的时间对齐。首先，测试者手持采集模块沿着地砖缝行走，并且每隔一块地砖将采集模块静止3-5 s，重复该流程，直到测试结束。然后，根据坐标变化提取出静止时刻，与SΤIM300 的静止时刻相关联，并使用静止时段高精度坐标与SΤIM300 进行组合解算，得到被测系统连续运动的参考位姿。测试数据总时长约为20 min。其中，三脚架每运动约10 s，就静止约3～5 s，共持续96 次，参考轨迹如图5 所示。

图5 参考轨迹Fig.5 Reference trajectory

图6 方案1、2、4 与参考系统的定位定姿差异（视觉观测正常）Fig.6 Τhe positioning and attitude differences between scheme 1,2,4 and the reference system (normal visual observation)

2.2 结果分析及讨论

本部分使用实测数据评估本文所提方法的位姿估计性能。为了分析提出的方案各部分的性能，共评估四种方案的位姿估计性能，具体包括：

1）纯视觉[19]定位定姿；

2）视觉/IMU 紧组合[20]定位定姿；

3）方案2 的基础上，启用环境特征点三角化辅助（具体见1.3.3）；

4）方案3 的基础上，进行反向平滑（具体见1.4）。

视觉观测正常情况下，环境特征点三角化未达到启用条件，方案2 与方案3 没有差异。图7 给出了视觉图像观测正常条件下方案1、2、4 与参考系统的位姿估计差异。可以看出，在视觉观测正常条件下，四种方案的定位误差均在10 cm 以内，定姿差异在0.7 °以内。相较于方案1，方案2 和4 获得了更平稳的位姿估计结果，表明IMU 的相对定位能力达到了较好的平滑效果。

图7 给出了位置和姿态差异累积分布图，表3-4给出了位置和姿态差异的均方根和CEP95 统计值。由于ArUco 码布设在平整的同一墙面，视觉观测能够很好地将相机位姿约束到与墙面平行的状态（即俯仰角和航向角），故ArUco 码的定姿精度主要由横滚角精度来体现。可以看出，方案1、2、4 的定位精度和俯仰角和航向角精度基本一致，而方案2 和4 估计的横滚角精度优于方案1，性能分别提升50%和62.5%。最终，方案4 能够提供3.17 cm（RMS）和7.20 cm（95%）的位置和0.1°（RMS）和0.21°（95%）的姿态。

表3 视觉观测正常时方案1、2、4 的位置误差均方根和CEP95Tab.3 RMS and CEP95 of the position error of scheme 1,2,and 4 when the visual observation is normal

表4 视觉观测正常时方案1、2、4 的姿态差异均方根和CEP95Tab.4 RMS and CEP95 of the attitude differences of scheme 1,2,and 4 when the visual observation is normal

为了评估本文方法在视觉观测中断情况下的位姿估计性能，通过手动关闭ArUco 角点检索和关闭视觉图像两种方式来模拟。具体设置如下：选择5 个10 s区间，区间内检测不到ArUco 码角点；选择5 个10 s区间，区间内图像观测完全中断；这两组区间的时段不重叠。如图8 所示，第一种中断为虚线框，第二种中断为实线框。

图8 方案1、2、3、4 与参考系统的定位定姿差异（视觉观测中断）Fig.8 Τhe positioning and attitude differences between scheme 1,2,3,4 and the reference system (visual observation interruption)

在检测不到ArUco 角点和关闭视觉图像的时间窗口内，方案1 均无定位定姿数据输出；方案2 在角点中断和关闭视觉图像时均有发散，这是因为方案2 中使用的IMU/视觉紧组合算法会在此两种情况下仅依靠惯导推算，而惯导推算的位姿有随时间快速发散的特性，导致中断期间定位结果快速发散，虽然试验进程中载体停止3-5 s 可以采用零速修正限制惯导误差，但由于间隔校准时间较长，无法满足厘米级的定位精度；方案3 在关闭ArUco 角点下可正常运行，但在图像关闭条件下发散，这是因为方案3 在ArUco 角点中断前正常运行时提取了环境中的非ArUco特征点并三角化出这些特征点的三维位置，在不能观测到完整ArUco 码时仍可以用它们作为图像坐标观测值；方案4在角点中断和图像中断的条件下均能正常运行，这是因为方案4 在方案3 的基础上加入的反向平滑算法，利用了未来时刻的观测来优化填补这部分丢失的精度。

图9 给出方案1、2、3、4 的位置和姿态误差累积分布图。表5-6 给出了位置和姿态差异的均方根和CEP95。可以看出，在定位方面，方案3、4 明显优于方案2，方案4 略优于方案3。最终，方案4 能够提供3.21 cm（RMS）和7.35 cm（95%）的位置和0.1 °（RMS）和0.21 °（95%）的姿态，与视觉观测正常时紧组合正向滤波的精度水平相当。

图9 视觉观测存在中断时方案1、2、3、4 与参考系统的定位定姿误差累计密度函数Fig.9 Cumulative density function of positioning and attitude errors between schemes 1,2,3,4 and the reference system when visual observation is interrupted

表5 视觉观测存在中断时方案1、2、3、4 的位置误差均方根和CEP95Tab.5 RMS and CEP95 of the position error of scheme 1,2,3 and 4 when the visual observation is interrupted

表6 视觉观测存在中断时方案1、2、3、4 的姿态差异均方根和CEP95Tab.6 RMS and CEP95 of the attitude differences of scheme 1,2,3 and 4 when the visual observation is interrupted

2.3 ArUco 码密度的影响分析

表7 给出了6 组ArUco 码的布设密度情况，图10为6 组布设密度与本文系统的定位定姿的精度关系。统计的误差统计量为静止期间位置误差最大值（SPE_MAX）、本文系统定位结果误差（95%概率，WPD_95）、本文系统与参考系统的姿态差异（95%概率，WAD_95）。从误差统计结果可以看出，当ArUco码密度大于1 个/m2时，定位定姿的精度没有出现明显的下降，这在实际应用场景中是容易满足的。

图10 ArUco 码铺设密度和定位定姿精度的关系Fig.10 Τhe relationship between ArUco code laying density and positioning accuracy

表7 ArUco 码的布设密度Tab.7 The layout density of ArUco code

3 结论

为了获得较高动态下精确稳定的室内定位定姿信息，本文以视觉/IMU 融合为框架，以三维坐标已知的室内ArUco 码为视觉识别对象；并利用ArUco 码角点附近被三角化的其它特征点作为补充特征，以供ArUco 码检测不到时使用，保证了系统运行的连续性；并在最后使用RΤS 反向平滑算法，进一步增强了系统的鲁棒性。实验表明，相较于传统的视觉/惯导紧组合来说，本文设计的算法能应对视觉条件不良的情况，能够连续可靠地实现3.2 cm（RMS）的定位精度，并达到与参考系统的定姿差异优于0.1 °（RMS）。