周 哲，胡钊政，2，王志强，肖汉彪

（1. 武汉理工大学智能交通系统研究中心，武汉 430063；2. 武汉理工大学重庆研究院，重庆 401120）

前言

获取高精度的位置信息是智能车进行路径规划、精确导航和障碍物避碰的必要条件，是保证智能车安全有序行驶的基础，被认为是智能驾驶中最关键的问题之一。目前，GPS、北斗导航系统获取的智能车位置信息难以满足全时高精度定位的需求。为了提高智能车的定位能力，利用车载传感器获取车辆的位置信息受到广泛关注（如激光雷达、视觉传感器等）。然而，激光雷达价格昂贵，一定程度上限制了智能车的普及。相比之下，视觉传感器价格低廉，而且能够提供丰富的纹理信息，因此，基于视觉的智能车定位成为近几年研究的热点。

基于车载传感器的定位方法需要事先对待定位场景进行地图构建，而后利用传感器实时获取的传感器数据与地图数据匹配以获取当前车辆的位置信息，而正确的地图匹配是实现车辆高精度定位的关键。Li 等和Lategahn 等分别在文献［5］和文献［6］中，将待定位场景离散为一序列节点，每个节点包含场景的视觉特征和位置信息。定位阶段，找到视觉地图中与当前图像最相似的地图图像，并用PnP（perspective-n-points）方法求解当前车辆的位置信息。特别是，近几年随着图像局部特征描述符的快速发展（如SIFT、SURF、ORB），图像中的场景信息可由多维特征向量进行稀疏表示，大幅提高了图像匹配的性能，其在智能车定位中的应用也得到广泛关注。Zhang 等利用SIFT 描述符的旋转和尺度不变性，提出全景视觉里程计与自然路标相结合的混合路标定位方法。由于SIFT 算法的计算效率低，近年来，更高效的SURF 和ORB 等特征描述符被用来进行视觉定位。但随着待定位场景的增大，特征匹配耗时增加，使得基于局部特征点匹配的方法难以在实时系统中应用。

为此，Galvez-Lpezde 等将文本检索技术应用于图像检索领域，提出视觉词袋模型。在离线阶段生成视觉词典，每幅图像由1 个特征向量表示，大幅提高匹配效率，使得视觉词袋模型被广泛的应用。Garcia-Fidalgo 和Ortiz利用词袋模型，提出层次化的图像检索方案，可以逐步缩小匹配范围，提高检索效率。但生成视觉词典的过程复杂，需要消耗较长的时间。为了提高特征匹配效率，Ma等在文献［16］中将视觉地图中相邻的5 张图像用一个GIST 描述符表示，并利用GIST 描述符进行初定位，可以有效降低图像搜索范围。在文献［17］中，刘国忠等将图像重置为固定大小的图像块，并提取图像中心的ORB 描述符作为整副图像的描述，匹配效率得到极大的提升。为了提高系统运行效率，本文中利用ORB 全局描述符构建发射概率计算模型。此外，随着深度学习的层次化特征提取不断成熟，并在图像检索领域得到普遍关注。在文献［18］中，王相龙等提出BoO（bag of objects）模型。首先利用AlexNet 网络提取场景中较为稳定的语义特征，生成表示场景的特征向量，在复杂光照条件下，能够有效提高算法的鲁棒性。在文献［19］中，Khaliq 等利用卷积神经网络中间层提取图像的全局特征实现位置识别。然而，这些图像特征均是通过设计的算法对图像的特征进行提取和计算的，在纹理贫瘠的场景中鲁棒性差。

上述算法无论是特征相似度匹配，还是利用深度学习进行的场景识别，其本质是利用单帧图像进行匹配，并无考虑连续帧之间的约束关系。在高相似度的场景中，难以实现精确匹配，这也是视觉定位面临的巨大挑战之一。为了解决这一问题，Milford 等利用连续帧之间的差异性作为相似性判断标准，但该方法需要计算连续帧灰度值之差，当场景较大时，会严重降低其计算效率，而且该方法对行驶速度限制苛刻，可扩展性较差。另外，Qiao 等利用全局特征和局部特征相结合的多特征序列匹配，以提高匹配效率和精确度。虽然基于图像序列的匹配提高了匹配的精度，但是这些方法并没有考虑运动的拓扑信息，制约了其在实际场景中的应用。

针对目前文献中方法的不足之处，本文在视觉地图构建的基础上，提出一种基于2 阶隐马尔科夫模型（HMM）的视觉地图匹配方法，将智能车视觉地图定位问题转化为HMM中最佳隐含状态（对应为视觉地图中的节点）估计问题。其新颖之处在于：（1）利用智能车运动约束（即短时间内匀速运动约束）构建基于2 阶HMM 模型的隐含状态转移概率模型，并通过前向算法将运动约束、道路拓扑结构与视觉场景约束进行有机融合，以实现视觉地图最优匹配；（2）在2阶HMM 模型中，提出基于ORB全局特征匹配的发射概率计算方法，并构建高斯模型来描述查询图像与视觉地图节点之间的发射概率，从而有效提高地图匹配与定位的计算效率。

1 研究方法

1.1 面向视觉地图匹配的2阶HMM模型

视觉地图通过预先获取道路场景的特征、三维结构等信息完成场景表征。例如，文献［5］和文献［6］中利用每帧图像提取的特征、轨迹与三维结构，构造地图节点，完成视觉地图构建，地图结构如图1所示。在定位过程中，智能车实时获取道路场景的图像数据并提取特征，通过与地图中存储的节点匹配，完成自身定位。可以发现，视觉地图定位过程中最核心的问题即为当前图像与地图节点的最优匹配问题。

图1 视觉地图示意

本文中提出利用2 阶HMM 模型来实现地图最优节点匹配，从而实现智能车地图定位。其核心思想是将地图中的节点定义为隐含状态，利用地图节点与当前查询图像的汉明距离定义发射概率，并利用运动约束来建立隐含状态之间的转移关系，这种状态转移模型充分描述了道路的拓扑结构与车辆的运动特征。然而，传统的HMM 认为时刻车辆的状态只取决于- 1时刻的状态，而与- 1之前的状态无关。在智能车定位过程中，这一假设显然不能有效描述车辆复杂的运动特性。于是，为了适应复杂的运动，提高匹配的成功率，提出基于2 阶HMM 的地图匹配模型，即车辆在时刻的状态由- 1 和-2 两个时刻的状态共同决定。从而，假设智能车在短时间内满足匀速运动约束，即可通过前两个状态计算出车辆短时间的速度。与经典的HMM 相比，2 阶HMM 模型拓展了算法的适用性。于是，本文中提出了基于2 阶HMM 的视觉地图匹配算法，其流程如图2所示。

图2 基于2阶HMM的视觉地图定位流程

该模型在视觉地图构建的基础上，通过车辆-1和- 2两个历史状态预测时刻的车辆状态，并利用构建的高斯模型计算发射概率，而后通过前向算法求解当前车辆与地图各节点的概率，对应的最大概率被认为是最优估计状态，也即当前车辆的地图匹配结果，记作。其核心为前向概率估计问题，因此在本文中将地图匹配问题用最大条件概率表示，即为

式中：表示时刻最优估计状态；X为时刻的时间序列；=｛，，…，Z｝表示观测序列。为求解上述问题，式（1）等号右边可由全概率公式展开，又因观测序列为已知，故有

而直接计算式（2）需要边缘化序列，即

因车辆的历史状态和观测均为已知，利用这些信息预测车辆在时刻的状态，故边缘化时刻车辆的状态，即可获取式（2）的结果，表示在序列观测条件下对时刻的车辆状态进行估计。然而随着时间序列的增加，其计算复杂度呈指数级增加。为了降低计算复杂度，前向算法利用HMM的条件独立性假设对计算过程执行递归运算。为了实现递归运算，定义：

式（4）为实现前向算法迭代运算的关键，其推导与计算过程见2.2节。

1.2 基于视觉地图的2阶HMM构建

基于视觉地图的智能车定位其主要任务是找到视觉地图中与当前车辆获取图像最相近的地图节点。本研究中，概率最大的隐状态认为是当前车辆匹配的最优地图节点，如式（1）所示。于是，在2 阶HMM 定位模型中，主要任务是找到每一时刻的，然而其计算复杂度较高。为了获取当前状态的概率，研究中利用前向算法求解当前时刻各状态的概率，如图3 所示。为了降低计算负担，提高算法迭代效率，定义- 1时刻的前向概率为

图3 基于2阶HMM的智能车视觉地图匹配与定位

则时刻的前向概率为

式中、、分别表示- 2、- 1 和时刻的状态序列，x、x、x分别表示对应的状态。式（6）由链式法则展开，变换后其迭代过程如下：

式中：发射概率由观测独立假设获得，即当前时刻的观测仅与当前时刻的状态有关，记为b（z）；表示当前状态；Z表示观测的图像数据；状态转移概率由当前时刻的车辆状态与- 2和- 1两个状态之间的关系确定，记为a。式（7）可表述为

式中为序列长度。时刻所获取各状态的概率为

则当前时刻最大概率对应的隐含状态即为当前时刻的地图匹配结果，即为

1.3 2阶HMM模型参数设定

基于2 阶HMM 的地图匹配模型包括状态初始概率向量、隐状态集合、观测状态集合、状态转移概率和发射概率5 个基本元素。所有视觉地图中的节点位置组成隐状态集合={X= x|∈[1，]}，每个隐状态由唯一的图像数据与其对应，={Z=z|∈[1，]}为观测状态集合。在2 阶HMM 中，状态转移概率(X|X，X)表示当前时刻的状态由- 2 和- 1 两个时刻的状态决定。发射概率表示当前状态观测到图像数据的概率，表示为(Z|X)，如图3 所示。在该模型中状态转移概率和发射概率的正确表述是影响该算法性能的关键因素，具体过程如下文所述。

1.3.1 系统初始化

在2 阶HMM 中，初始化包括初始状态概率和初始状态转移概率。初始状态与1 阶隐马尔科夫中的状态初始化相同，均表示车辆在各状态的概率。在本研究中，初始化状态通常为已知，故

初始状态转移概率则表示由初始状态转移到下一时刻状态的概率，由={c|，∈[1，]}表示。由于初始状态已知，故当π= 0时，到其他状态的概率为0，则

式中：为状态转移过程的标准差，为经验值，本实验中取=0.5；则根据= 1 和= 2 时刻的状态差而确定，以适应不同速度的运动模型。= 1 时的定位结果为

此时刻的状态则由式（9）和式（10）获得。

1.3.2 基于高斯分布的状态转移建模

在传统HMM 中，假设时刻的状态仅由- 1 时刻的状态决定，这显然难以应对复杂的车辆运动。实际上车辆当前时刻的状态不仅直接受- 1 时刻的状态影响，而且也受- 2，- 3，…等历史状态的影响。为了兼顾模型复杂度和车辆运动特征，研究中选取2 阶状态转移模型，即时刻的状态受影响于- 1和- 2两个状态。此时状态转移概率矩阵是3维矩阵，其大小为××（为地图节点数）。

通常认为状态转移过程服从高斯分布（如图4所示）。例如，当- 2 和- 1 时刻的状态分别为x和x，则时刻最大概率状态为x，其分布概率为

图4 利用高斯分布进行状态转移建模

式中：= 2x- x，提供局部速度约束；根据构建地图和移动速度的不同而不同，为经验值，本文中选取=0.5。

1.3.3 基于全局ORB匹配的发射概率建模

发射概率b(z)表示车辆在x状态下观测到当前图像z的概率。为了提升状态估计时的匹配效率，本研究利用全局图像描述符计算地图节点中存储的图像信息与当前获取图像的相似度，具体为，把图像重置为63×63 pixel 大小的图像块，将图像中心作为整幅图像的唯一特征点，并提取该特征点的ORB 描述符向量，如图5 所示。图像相似度由两个全局ORB 描述符的汉明距离计算得到，汉明距离(，)的计算方式如下所示：

图5 全局特征描述符

式中：表示当前图像的全局描述符；表示地图节点对应图像的全局描述符；f和分别表示和的第位（∈[1，]）；为异或运算，汉明距离越小则对应发射概率越大。

通过式（16）可以计算当前图像与候选状态对应图像之间的汉明距离。为了描述随着图像相似度的降低（汉明距离增大）发射概率逐渐变小这一现象，本文中通过构建0 均值高斯模型，以描述观测图像与地图节点图像之间的相似度，也即HMM模型的发射概率为

式中：(，)为当前观测图像与状态对应的图像数据的汉明距离；为方差；b(z)为状态x到图像z的发射概率。

于是，车辆当前时刻的状态可由观测图像序列与视觉地图的匹配结果获取，其匹配过程见式（6）～式（8）。其中，- 1 时刻的前向概率可由观测序列和- 1、- 2时刻的车辆状态获取，时刻车辆转移到各状态的概率由- 1 和- 2 时刻的车辆状态获得，如式（15），时刻的观测信息由b(z)获取，如式（17），则其对应的最大隐状态即为时刻的匹配结果，故基于2阶HMM的地图匹配结果可表示为

2 实验与结果分析

为了验证本算法的有效性，分别收集3 个不同路段的数据集进行测试。其中前2 组试验利用试验室参与开发的智能车系统进行数据采集，如图6 所示。该平台配备有DGPS 和单目相机（point grey camera），图像分辨率为1920×1080 pixel。第1 组实验数据在武汉某科技公司园区采集，如图7（a）所示。第2 组实验采集开放道路的数据，数据采集路段为武汉市二环线的一段快速路，如图7（b）所示。在快速路上车辆速度相对较高，节点间距比园区和KITTI数据集中的间距大。第3 组实验在公开的KITTI 数据集上测试。3 组实验在相同的计算机硬件条件下进行测试，其配置为Intel Core i7-7700HQ 2.8GHz CPU，开发环境为Visual Studio 2017/C++。为了比较不同算法之间的性能差异，选取基于BoW模型的单帧匹配和基于深度学习的算法以及序列匹配的seqSLAM为对比算法。

图6 智能车视觉系统

图7 测试路段

视觉地图采用文献［5］中的地图构建方法，将待定位场景离散为一序列节点，每个节点包括视觉特征、三维信息和轨迹信息，其中视觉信息采用全局ORB 特征描述，全局ORB 特征提取如1.3.2 节中所述，所构建视觉地图如图1所示。

2.1 封闭工业园区测试

在定位阶段，仅使用单目摄像机作为数据采集传感器，实验场景包括开阔路段和阴影路段，长近500 m，共采集图像324张，测试场景如图8所示。定位试验是在完成视觉地图构建的路段进行测试。在封闭园区车辆行驶速度相对较低，视觉地图节点平均间距约为3 m，共构建164个视觉地图节点。在构建视觉地图的基础上，利用采集的226 张测试数据进行验证。

图8 工业园区测试场景

首先利用前30 张采集数据确定2 阶HMM 模型的最优参数。完成2 阶HMM 模型参数确定后，利用测试集剩余的196 张图像进行验证。通过智能车的前2 个时刻的位置对系统进行初始化，根据初始化位置，确定下一时刻车辆转移到各地图节点的概率。然后提取当前图像的ORB 全局描述符，并计算当前图像与各地图节点对应图像的汉明距离，求得当前图像与各节点图像的相似度。最后利用状态转移概率和计算的发射概率，通过前向算法获取当前位置与视觉地图中节点之间的联合概率，其最大概率认为是当前时刻的定位结果。本文研究的重点为智能车节点级定位，结果评价采用闭环检测中常用的衡量标准，即当前车辆在相邻节点之间时，通常认为最近的节点和次近的节点都是正确的匹配结果。

本文中误差帧是指获取的地图节点与真实节点之间的节点差。同时，与当前车辆相邻的两个节点被同等地认为是与当前车辆最近的节点，其累积分布如图9 所示。因对比算法的定位结果波动比较大，为了便于数据的统计与分析，当误差大于4 帧时均按4帧处理。

图9 园区定位实验误差分布

本研究与经典的基于BoW 的方法和基于深度学习的位置识别算法以及序列匹配的seqSLAM 算法进行比较，其匹配范围是由状态预测的粗定位范围。其定位误差均值分别为0.12、0.48、0.43和0.46帧，误差标准差分别为0.52、0.92、0.72 和1.04 帧。与对比算法的程序耗时74.55、86.38 和286.24 ms相比，本研究利用全局特征描述符来计算发射概率，省去了特征提取与卷积过程，而且仅需异或运算即可获得汉明距离，大幅降低了计算耗时，如表1所示。算法成功找到最近节点的概率分别为93.24%、84.29%、89.10%和88.44%。此外，如图9 所示，对比算法定位误差大于等于4 帧的比例较大，表明定位结果波动大。由上述数据显示，本文中提出的2 阶HMM 定位算法将拓扑信息和序列图像信息相融合，提高了定位的稳定性和精度，其性能优于对比算法。

表1 不同算法在园区场景的性能对比

2.2 开放道路测试

为了验证本算法对不同场景的适应性，采集武汉市一段开放道路的数据进行验证，该测试路段共采集375 张图像，路段长度约为1200 m，测试路段如图7（b）所示。由于此路段为快速路，车辆行驶速度相对较高，测试场景如图10 所示。为了与封闭园区具有相似的测试数据集数量，该测试场景构建的视觉地图节点数为150 个，相邻节点之间的间距平均为8 m。利用采集的225张图像作为测试数据，参数确定过程与2.1节相同。

图10 快速路的测试场景

开放道路上的实验测试结果如图11 所示，由概率分布可以看出，快速路场景下本算法的性能优于园区的匹配结果，其平均匹配误差为0.01 帧，匹配标准差为0.08 帧，性能均优于对比算法，与对比算法的性能比较如表2 所示。经分析，快速路场景下，本文算法相比于园区的匹配结果较好是因为在快速路场景下光照条件好，图像特征显著，而且该测试路段不存在较大的转弯等场景，因此视觉地图匹配结果相对较高。由此也能够说明本算法更适应在相对开阔、光照条件好的场景下使用。

图11 快速路定位实验误差分布

表2 不同算法在快速路场景的性能对比

2.3 公开数据集KITTI测试

KITTI数据集是智能驾驶领域使用最为广泛的公开数据集之一。本文中选取的测试数据集的采集设备如图12所示，选取里程计数据集中的sequence_00作为测试数据，其图像大小为1241×376 pixel，其测试路段场景如图13所示，为了说明算法的可比性，选取包括两个转弯和一段阴影路段进行测试。选取测试数据387张，距离近430 m，该场景构建视觉地图节点数为181 个，节点平均间距约2.4 m。在确定2 阶HMM最优参数后，利用采集的206张图像进行测试。

图12 KITTI中用的数据采集系统

图13 KITTI测试道路场景

在公开数据集上的实验结果如图14 所示，由图中可以看出，在KITTI数据集中本算法的匹配性能与开放道路场景下的匹配性能相当，其成功匹配的概率为99.33%。其与对比算法的平均匹配误差分别为0.01、0.47、0.43 和1.07 帧，匹配误差标准差分别为0.08、0.99、0.78 和1.78 帧，且程序平均耗时仅为3.31 ms，如表3 所示。因此，本研究中提出的基于2阶HMM 的视觉地图匹配算法在实地采集的数据集和公开数据集中均有较好的定位精度和稳定性。

图14 KITTI数据集定位实验误差分布

表3 不同算法在KITTI数据集中的性能对比

3 结论

本文中利用历史状态预测车辆的当前位置，构建2阶HMM 的状态转移矩阵，并提取图像的ORB全局描述符构建基于汉明距离的发射概率计算模型，最后通过前向算法完成车辆状态估计，实现基于2阶HMM 的智能车视觉地图匹配。该方法可将图像序列匹配与轨迹的拓扑信息相融合，有效降低数据存储空间和计算复杂度。为了验证算法的有效性，在不同场景和公开数据集上进行测试，试验结果表明，本算法在不同场景中均表现较好的匹配性能，其中在开放道路和KITTI 数据集上的正确匹配率高达99%以上，具有较高的匹配精度和稳定性。本研究能够实现精确的车辆实时定位，但单一摄像头容易受外界干扰，在后续研究中，将开发基于多传感器融合的2阶HMM智能车地图匹配方案。