张文安汪 伟付明磊陆春校何军强

(1.浙江工业大学信息工程学院，浙江 杭州 310023；2.浙江富春江通信集团有限公司，浙江 杭州 311400；3.杭州鸿泉物联网技术股份有限公司，浙江 杭州 310030)

近年来，随着人工智能技术的兴起，无人驾驶得到了快速发展。获得实时、准确的车辆位姿信息是实现无人驾驶的关键技术之一[1]。其中，全球导航卫星系统(Global Navigation Satellite System，GNSS)与惯性导航系统(Inertial Navigation System，INS)的融合可以提供高精度的全局定位[2－3]，是实现这一关键技术的重要方法。然而，在隧道、地下车库等缺失GNSS信号的场景中，基于GNSS/INS组合导航系统的车辆定位性能迅速下降。因此，缺失GNSS信号条件下的低速无人车定位问题具有重要的研究价值。

由于单一传感器的定位精度和适用范围有限，目前无人车定位普遍采用多传感器数据融合方法[4]。其中，基于激光雷达(Light Detection and Ranging，LIDAR)里程计的定位技术具有算法成熟、精度高、实时性好等优点[5]，但是LIDAR定位技术不适合成本敏感的定位产品。而基于相机的视觉里程计(Visual Odometry，VO)因其具有感知信息丰富、体积小、成本低等优点而受到无人驾驶领域的广泛关注[6]。但是，由于其无法有效地应对运动模糊、低纹理场景、光照变化、遮挡和动态对象的影响，因此常与INS相融合，形成基于相机与惯性测量单元(Inertial Measur-ement Unit，IMU)融合的视觉惯性里程计(Visual Inertial Odometry，VIO)[7]。VIO利用相机和IMU具有互补的特性，有效融合惯性测量与图像观测信息，从而获得比使用单个传感器更好的累积性能。

VIO方向的研究工作主要分为基于优化框架的方法和基于滤波框架的方法。其中，基于优化框架的方法通过一个最小二乘误差函数的迭代最小化来估计未知参数，被广泛认为在精度上优于基于滤波框架的方法[8－9]。但是，由于其迭代机制，这类方法需要较高的计算量。与基于优化框架的方法相反，基于滤波框架的方法在观测数据可用时以递归方式估计参数[10－11]，具有较高的计算效率。文献[12]提出了一种基于误差状态扩展卡尔曼滤波器(Extended Kalman Filter，EKF)的多状态约束卡尔曼滤波(Multi-State Constraint Kalman Filter，MSCKF)算法。MSCKF推导出一种能够表达从多个相机位姿观察到静态特征时产生的几何约束的测量模型，并在系统状态向量中保留了一个含固定数量的过去相机位姿的滑动窗口和当前的IMU状态，在计算效率、算法精度等方面均取得了良好的效果，非常适合部署在计算资源受限的低速无人车平台上。同时，由于MSCKF算法的独特优势，在其框架下出现了大量的后续工作来提升其性能和适用性[13－16]。

然而，低速无人车在实际运行过程中经常会由于避让行人和其他车辆，或者交通堵塞而出现的较长时间停车等待。在这种静态场景中，由于无法发生特征点三角化将导致MSCKF中基于相机的测量更新过程停止，而基于IMU的状态传播过程正常进行。受到IMU累积误差的影响，无人车的定位性能迅速下降。为了解决上述问题，本文引入零速修正技 术(Zero Velocity Update，ZUPT)[17－18]，结 合MSCKF框架，提出一种基于自适应零速修正机制的低速无人车定位方法(Adaptive Zero Velocity Update Mechanism MSCKF，AZUPT-MSCKF)。相比于传统MSCKF算法，AZUPT-MSCKF方法能够有效地处理无人车由于较长时间处于静止状态而导致的定位性能下降等问题，具有更高的定位精度和更强的鲁棒性。

本文的主要贡献包括:1)为了准确检测出无人车的运动状态，提出一种基于多条件约束的静止状态检测算法；2)为了消除无人车由于较长时间处于静止状态而导致的定位累积误差，利用零速度、相同位置和相同姿态等静态约束，提出一种基于自适应因子的自适应零速更新策略；3)将静止状态检测算法和自适应零速更新策略与MSCKF有效融合，提出AZUPT-MSCKF方法，并进行实验验证。

1 低速无人车定位系统描述

1.1 坐标系定义

考虑一类基于相机与IMU融合的低速无人车定位系统，并对该系统中使用到的相关坐标系进行定义。如图1所示，主要包括世界坐标系{W}、IMU坐标系{I}和相机坐标系{C}。其中，{W}是固定不动的，其原点Ow与{I}的原点O i在算法初始运行时刻重合，且{W}的X w－O w－Yw平面平行于地面，Z w轴指向与重力g相反的方向，Yw轴指向无人车的正前方，X w轴由右手定则决定；{I}，又称为无人车车体坐标系，与无人车车体固连，随无人车运动而变化；{C}是设置在左目相机的坐标系上。同时，由于{C}和{I}之间的相对位姿是刚性的，不会随无人车运动而发生改变，所以在实验前对相机和IMU进行联合标定，获得{C}和{I}间的转换矩阵，用于两者之间的相对位姿转换。最后，低速无人车定位系统的任务转换为实时估计{I}相对于{W}的六自由度位姿。

图1 低速无人车定位系统中坐标系示意图

1.2 状态向量定义

所提出的AZUPT-MSCKF 方法是基于MSCKF框架设计，因此，在任意时刻，系统状态向量中包括当前IMU状态和一个含N个过去相机位姿的滑动窗口。

IMU状态向量定义为:

根据式(1)，IMU误差状态向量定义为:

式中:对于四元数，误差值δq、真实值q和估计值＾q之间满足:q＝δq⊗＾q，且⊗表示四元数乘法；对于速度、位置和偏差，误差值等于真实值减去估计值(例如:W~v I＝W v I－W＾v I)。同时，误差轴角δθ是误差四元数δq的最小表示，且两者之间近似满足:

假设在K时刻，系统状态向量中保留了一个含N个过去相机位姿(位置和姿态)的滑动窗口，则AZUPT-MSCKF状态向量定义为:

类似于式(2)，AZUPT-MSCKF误差状态向量定义为:

最后，低速无人车定位系统的任务进一步转换为实时估计W p I和

2 基于自适应零速修正机制的低速无人车定位方法

本文提出的基于自适应零速修正机制的低速无人车定位方法利用相机与消费级IMU等低成本传感器，设计了一种自适应零速修正机制，并将其集成到MSCKF框架中作为一种自然测量更新过程，能够有效地处理无人车由于较长时间处于静止状态而导致的定位性能下降等问题，其整体流程如图2所示。

图2 基于自适应零速修正机制的低速无人车定位方法整体流程

本文提出的自适应零速修正机制相关步骤已在流程图中使用蓝色重点标注；同时，黑色表示传统MSCKF算法部分。系统输入为相机图像和IMU数据。一旦接收到新的测量，首先判断是否完成初始化，若未完成初始化，则执行初始化，并构造AZUPT-MSCKF状态向量，若已经完成初始化，则执行IMU状态及协方差传播、图像特征提取及跟踪和相机状态及协方差扩增；其次通过静止状态检测部分检测出无人车的当前运动状态，若无人车处于静止状态，执行本文新增的自适应零速更新过程，而MSCKF测量更新过程将会停止，若无人车处于运动状态，则执行MSCKF测量更新过程；同时，任何可能会导致IMU传播出现偏差的变量都可以在自适应零速更新过程中进行修正；最后进行滑动窗口状态管理，并实时输出无人车的位姿估计值，从而实现无人车定位。

综上，所提出的AZUPT-MSCKF方法主要包括传统MSCKF算法和自适应零速修正机制两部分。有关传统MSCKF算法的内容可参考文献[12]，在此不再叙述；接下来将重点介绍本文新增的自适应零速修正机制的相关内容。

3 自适应零速修正机制

本文提出一种自适应零速修正机制，主要包括静止状态检测和自适应零速更新两部分。一旦通过静止状态检测部分检测出无人车当前处于静止状态，将启动自适应零速更新。接下来将详细介绍基于多条件约束的静止状态检测算法和基于自适应因子的自适应零速更新策略。

3.1 基于多条件约束的静止状态检测算法

结合车载IMU中的陀螺仪和加速度计测量数据，提出一种基于多条件约束的静止状态检测算法，用于实时检测无人车的运动状态。

定义a k∈R3和ωk∈R3，分别表示k时刻的三轴加速度矢量和三轴角速度矢量。静止状态检测就是给定测量序列在区间[n，n＋M－1]内，滑动窗口大小为M的条件下，根据下式完成无人车的当前运动状态检测:

式中:T(a n，ωn)表示检测算法的统计数据，g表示当地重力加速度分别表示加速度计和陀螺仪的测量噪声方差，且两者之间的比值反应了来自加速度计与陀螺仪数据中的扰动，影响检测算法的性能，表示矢量a的大小，λ表示检测阈值表示区间[n，n＋M－1]内的样本均值，定义如下:

最后，若式(6)成立，则判断出无人车处于静止状态；反之，无人车处于运动状态。综上，其整体流程如表1所示。

表1 基于多条件约束的静止状态检测算法

3.2 基于自适应因子的自适应零速更新策略

当检测到无人车处于静止状态时，首先利用IMU速度为零、滑动窗口内最新与第二新的相机位姿相同等静态约束条件定义测量残差；其次构建测量残差与AZUPT-MSCKF误差状态向量之间的残差模型；最后执行测量更新，并采用基于自适应因子的测量噪声协方差矩阵在线调整策略实现自适应零速更新，完成状态修正，其整体流程如表2所示。

表2 基于自适应因子的自适应零速更新策略

3.2.1 测量残差定义

①IMU零速度残差

无人车处于静止状态时，IMU当前速度的真实值为零，定义为:

此时，通过IMU状态传播得到IMU当前速度的估计值，定义为:

由式(8)和式(9)，可得IMU零速度残差，定义为:

由上式可知，IMU零速度残差可由IMU当前速度的估计值来获得，并且该残差是关于误差状态W~v I的线性函数。

②相机位置相同残差

无人车处于静止状态时，滑动窗口内最新与第二新相机位置的真实值相同，定义为:

式中:W p C N为滑动窗口内最新相机位置的真实值，W p C N－1为滑动窗口内第二新相机位置的真实值。

此时，通过相机状态扩增得到滑动窗口内最新与第二新相机位置的估计值，定义为:

由式(11)和式(12)，可得滑动窗口内最新与第二新相机位置相同残差，定义为:

由式(13)可知，滑动窗口内最新与第二新相机位置相同残差可由滑动窗口内最新与第二新相机位置的估计值来获得，并且该残差是关于误差状态的线性函数。

③相机姿态相同残差

无人车处于静止状态时，滑动窗口内最新与第二新相机姿态的真实值相同，即定义为:

此时，通过相机状态扩增得到滑动窗口内最新与第二新相机姿态的估计值，定义为:

综上，可得滑动窗口内最新与第二新相机姿态相同残差，定义为:

由于AZUPT-MSCKF误差状态向量中使用误差轴角δθ代替误差四元数δq，则该残差又可近似表达为:

式中:{·}i jk表示取四元数虚部的三个系数构成一个3×1的向量。

由上式可知，滑动窗口内最新与第二新相机姿态相同残差可由滑动窗口内最新与第二新相机姿态的估计值来获得，并且该残差是关于误差状态和的线性函数。

3.2.2 残差模型构建

3.2.3 测量更新执行

由前文构建的残差模型，按照EKF更新方式执行AZUPT-MSCKF测量更新。

计算卡尔曼增益KZUPT:

式中:P表示AZUPT-MSCKF状态协方差矩阵，与MSCKF状态协方差矩阵相同，可参考文献[12]，QZUPT表示测量噪声nZUPT的协方差矩阵，定义为:

执行状态更新:

执行状态协方差矩阵更新:

式中:τ＝15＋6N表示协方差矩阵的维度。

此外，任何可能会导致IMU偏差传播的变量都可以通过此次测量更新过程进行修正。

3.2.4 基于自适应因子的测量噪声协方差矩阵在

线调整策略

在零速更新过程中，考虑到可能会存在将慢速或匀速运动检测为静止状态等错误检测情况，因此可以采用少量的QZUPT来提高算法的容错性。然而，在工程实现中，确定最优的QZUPT较为复杂。对此，本文提出了一种基于自适应因子的测量噪声协方差矩阵在线调整策略来提高算法的适应性，即通过构造自适应因子，并利用测量噪声协方差矩阵与自适应因子的比值实现测量噪声协方差矩阵的在线修正。

首先，定义自适应因子αZUPT为:式中:1.0＜c0＜1.5和3.5＜c1＜4.5分别表示常数阈值，表示预报残差，定义为:

式中:σnZUPT表示测量噪声nZUPT的均方差。

其次，定义测量噪声协方差矩阵QZUPT与自适应因子αZUPT的比值为等效测量噪声协方差矩阵

4 实验结果及分析

为了验证本文所提方法的有效性和优越性，在校园内的实际场景中进行实验测试，分别对比本文所提方法与传统MSCKF算法[12]和VINS-Mono算法[8]的定位效果，且部分实验过程图如图3所示。实验使用搭载小觅双目摄像头深度版(D1000－IR－120/Color)和带有FR－07阿克曼线控底盘的低速无人车，在装有Linux操作系统(Ubuntu 16.04)和机器人操作系统(ROS)的Lenovo G50笔记本电脑(核心处理器:Intel Core i5)上进行，如图3所示。其中，D1000－IR－120/Color包含一个BMI088 MEMS IMU和两个AR0135全局快门相机，输出硬件同步的30 Hz全局快门图像和200 Hz IMU测量，安装在低速无人车上的高度约为0.95 m。另外，由于本文所提方法属于单目VIO，所以实验中只使用了左目相机图像和IMU测量数据。

图3 实验平台及部分实验过程图

另一方面，图像特征提取及跟踪采用FAST特征检测器和KLT光流跟踪算法相结合的方式，且相关实验参数的设置如表3所示。

表3 实验参数设置

4.1 交通灯路口等候红灯实验

实验一，在一段总长约10 m的模拟交通灯路口中进行，模拟了无人车在交通灯路口遇红灯停车等候然后向左拐弯行驶全过程，且无人车停车位置已在图中标注，如图4(a)所示。实验中，首先考虑到无人车在交通灯路口遇红灯停车等候的时间不同，故将停车等候时间划分为短时间(5 s～10 s)、中等时间(20 s～25 s)和长时间(35 s～40 s)等三个区段；其中，单次运行以上每组实验花费的时间分别约为25 s、40 s、55 s，且无人车行驶时的平均速度约为0.5 m/s；其次按照图4(a)中所示轨迹以及不同停车等候时间分别运行本文所提方法和传统MSCKF算法，得到以上两种不同方法估计出的无人车运动轨迹，并通过EVO软件绘制成相应曲线，如图4(b)、(c)、(d)所示；然后根据曲线定性分析出两种不同方法对应的无人车定位轨迹在停车处的轨迹漂移结果；最后重复运行每组实验各10次，分别计算出两种不同方法对应的无人车定位轨迹的均方根误差(Root-Mean-Squared Error，RMSE)，如表4所示。

表4 基于交通灯路口等候红灯实验的平均RMSE单位:m

通过对比图4(b)、(c)、(d)中的无人车定位轨迹可以发现，随着停车时间的增长，停车处(图中圆圈处)出现的轨迹漂移量逐渐增大，且停车处之后的轨迹偏差也逐渐增大；同时，本文提出的AZUPTMSCKF方法对应的停车处轨迹漂移小于传统MSCKF算法，且整体轨迹更接近于真实轨迹，取得了更好的定位效果。这是由于实验中使用的BMI088属于低成本IMU，有偏传播的误差是不可忽视的。当无人车处于静止状态时，由于无法发生特征点三角化而导致传统MSCKF算法中的相机测量更新停止，此时无人车定位轨迹由于IMU的有偏传播而发生漂移，而当无人车再次开始移动时，即将到来的有效相机测量将导致传统MSCKF算法对应的无人车定位轨迹发生突变。然而，本文所提方法通过新增的自适应零速修正机制来校正IMU的偏差传播，使得无人车定位轨迹具有较小的漂移量。表4给出了两种方法在交通灯路口等候红灯实验中的平均RMSE结果，其中本文所提方法对应的无人车定位轨迹的RMSE值小于传统MSCKF算法。综上，说明了本文所提方法的定位性能优于传统MSCKF算法。

图4 交通灯路口等候红灯实验结果

4.2 高楼之间闭合轨迹运行实验

实验二，在一段总长约106 m的室外高楼之间的闭合道路上进行，环境较开阔，部分道路不平稳，时有行人穿过，且路边有部分结构化设施；同时，使无人车做闭合轨迹运行，行驶过程中共在七处停车，已在图中标注，如图5(a)所示。其中，单次实验花费的时间约为109 s，且无人车行驶时的平均速度约为1.2 m/s。实验中，首先按照图5(a)中所示轨迹分别运行本文所提方法和传统MSCKF算法，得到以上两种不同方法对应的无人车定位轨迹，并通过EVO软件绘制成相应曲线，如图5(b)所示；然后重复运行每组实验各10次，分别计算出两种不同方法对应的无人车定位轨迹的平均闭环误差，如表5所示。

表5 实验二的平均轨迹闭环误差 单位:m

图5 实验二结果

另外，为了进一步验证本文所提方法的有效性和优越性，在该实验环境下与VINS-Mono算法进行定位精度对比，即实验三。考虑到本文所提方法中无回环检测，且VINS-Mono算法中无零速修正机制，因此，为了公平对比，在进行对比实验时，关闭VINS-Mono算法中的回环检测，且无人车在行驶过程中未有停车，其他实验步骤与上述实验二相同。最后，重复运行该实验10次，分别计算出两种不同方法对应的无人车定位轨迹的平均闭环误差，如表6所示。

表6 实验三的平均轨迹闭环误差 单位:m

由图5(b)可以看出，本文所提方法和传统MSCKF算法均能成功估计出无人车真实轨迹的大致走势。然而，一旦无人车处于静止状态并再次运行后，传统MSCKF算法对应的无人车定位轨迹明显出现漂移现象，并且随着无人车停车次数的增加，总体轨迹偏差也逐渐增大；而本文所提方法相比于传统MSCKF算法则出现较小的轨迹漂移，且估计出的终点位置也更接近于真实的终点位置，如图5(b)中的圆圈处所示。表5给出了两种方法的平均轨迹闭环误差结果，其中本文所提方法对应的无人车定位轨迹的平均闭环误差小于传统MSCKF算法，取得了较好的定位效果，说明了本文所提方法在室外环境下的定位精度高于传统MSCKF算法。同时，由表6可以看出，相比于VINS-Mono算法，本文所提方法对应的无人车定位轨迹的平均闭环误差较小，具有较好的定位性能，从而进一步验证了本文所提方法的有效性和优越性。

4.3 地下停车场规定轨迹运行实验

实验四，在一段总长约25 m的地下停车场中进行，模拟了无人车倒车入库出库全过程，且起点与终点之间的距离约0.94 m；同时车辆运动情况较复杂，包括直线、转弯、快速、慢速和停车等状态，且无人车在行驶过程中共有两处停车，已在图中标注，如图6(a)所示。其中，单次实验花费的时间约为44 s，且无人车行驶时的平均速度约为0.7 m/s。实验中，首先在路径上设置四个坐标点(0.00，0.00)、(－7.90，1.10)、(－5.20，4.00)、(0.00，0.94)；其次按照图6(a)中所示轨迹分别运行本文所提方法和传统MSCKF算法，得到以上两种不同方法对应的无人车定位轨迹，并通过EVO软件绘制出相应曲线，如图6(b)所示；然后重复运行每组实验各10次，分别计算出两种不同方法对应的无人车定位轨迹的平均闭环误差，如表7所示。

图6 地下停车场规定轨迹运行实验结果

表7 基于地下停车场规定轨迹运行实验的平均轨迹闭环误差 单位:m

由图6(b)可以看出，相比于传统MSCKF算法，本文所提方法对应的无人车定位轨迹更接近于真实轨迹，且估计出的无人车终点位置也更接近于真实的终点位置；同时，当无人车处于静止状态时，本文所提方法对应的无人车定位轨迹也具有较小的漂移量，如图6(b)中的圆圈处所示。表7给出了两种不同方法在地下停车场规定轨迹运行实验中的平均轨迹闭环误差结果，其中本文所提方法对应的无人车定位轨迹的平均闭环误差小于传统MSCKF算法，取得了较好的定位效果，说明了本文所提方法在室内环境下的定位精度高于传统MSCKF算法。

5 结论

本文针对缺失GNSS信号条件下的低速无人车定位问题，在MSCKF框架下，提出了一种基于自适应零速修正机制的低速无人车定位方法(AZUPTMSCKF)。该方法首先设计了一种自适应零速修正机制，并将其集成到MSCKF框架中作为一种自然测量更新过程，能够有效处理无人车由于较长时间处于静止状态而导致的定位性能下降等问题。实验结果表明，本文所提方法相比于传统MSCKF算法及VINS-Mono算法(关闭回环检测)具有更高的定位精度和更强的鲁棒性。在下一阶段的工作中，我们将继续优化AZUPT-MSCKF方法中的图像特征提取机制，并在真实行车环境中进一步测试算法的定位性能。