引言

车辆的相对位置与行驶速度对于行车安全是至关重要的，准确快速地获取这些运动参数能为驾驶员提供参考信息以便及时做出决策。与现有的测量方式诸如激光、雷达等相比，视觉测量具有被动性、非接触性及抗干扰的优点，在运动参数测量方面得到了广泛应用^[1-2]。利用视觉技术不仅能估算车辆的运动参数，还能很容易地把这些信息融合到诸如行人检测、车辆检测的视觉算法中，这样就避免了使用其他传感器所带来的配准难题^[3]。目前，视频测速已大量运用于交通监控中，但它只能在特定测速区内对车辆测速，使用不灵活; 车载视觉测速系统一般是在车辆底盘上安装单目视觉传感器，连续采集车辆行驶时的地面序列图像，再根据地面图像的跟踪匹配进而计算出车辆的行驶速度^[4]，但这种方法无法完成测距定位，需另加传感器以确定车辆相对于周围环境的位置。

鉴于双目立体视觉可以有效地获取物体的景深，因此可以通过双目视觉同时进行车辆的实时测距定位与自车速度估计。基于此，本文将双目视觉系统侧向安装在车辆上，连续采集车辆行驶时的周围环境图像; 并根据各个时刻左右图像的立体匹配结果，计算出车辆相对于环境的实时位置; 同时对相邻两帧图像进行特征跟踪匹配，估算出车辆相对于环境的行驶速度。

1   基于双目视觉的车辆定位与速度估计原理

本文采用光轴平行的双目视觉系统，摄像机坐标系以其瞬时成像点为原点，X_C轴与车辆瞬时运动方向相同，Y_C轴垂直地面向下，采集序列图像时的时间间隔Δt恒定不变。

双目视觉测距的原理就是利用左右图像的视差根据三角测量方法获取物体景深。先对t_k(t=1, 2, …)帧左右图像进行立体匹配以获取视差，再结合双目视觉系统的标定参数从而计算出环境特征点在左摄像机坐标系下的三维坐标，其中Z坐标值就是车辆到这些环境特征点的距离，实现了车辆在周围环境中的实时定位。

基于双目视觉的自车速度估计是依据序列图像的特征跟踪匹配进行的。图 1为车辆自车速度估计模型示意图。假定车辆在路面上做刚体运动，根据运动学和空间解析几何理论可知，相邻两帧间车辆的运动参数可以用旋转和平移来表示，而摄像机固定在车辆上，所以能够用相邻两帧间摄像机坐标系的变换参数来描述车辆的运动参数。记集合{(P_i^t_k、P_i^t_k+1), i=1, 2, …, N}为车辆运动前后两帧图像中对应特征匹配点对的集合，其中P_i^t_k和P_i^t_k+1分别代表t_k时刻和t_k+1时刻第i组匹配点对在前后两帧左摄像机坐标系下的三维坐标。则它们有如下关系：

图  1  车辆自车速度估计模型示意图

Figure  1.  Schematic diagram of ego-velocityestimation for vehicle

下载: 全尺寸图片 幻灯片

式中:R、T为相邻两帧左摄像机坐标系之间变换的正交旋转矩阵和平移矢量，表明了车辆在t_k+1时刻相对于t_k时刻的位姿变化。

根据路面的实际情况，车辆在短时间内的运动可以看着是平面刚体运动。因此本文假设在相邻两帧之间，车辆在X_X-Z_C平面上运动，如图 1所示，那么旋转矩阵R和平移矢量T可简化为

式中:θ表示车辆的偏转角度。求解出旋转矩阵R和平移矢量T之后，就可以计算出车辆从t_k时刻到t_k+1时刻的平移速度和偏转角速度，即：

根据上述原理，车辆测距定位与自车速度估计的详细流程如图 2所示。由于Δt非常小，因此计算出的速度可以当作车辆的瞬时速度。

图  2  基于双目视觉的车辆定位与自车速度估计方法流程图

Figure  2.  Flow chart of vehicle location and ego-velocity estimation based on binocular vision

下载: 全尺寸图片 幻灯片

2   基于SURF特征的立体匹配与特征跟踪匹配

在进行图像匹配之前，首先对图像进行校正、滤波、直方图均衡化等方面的预处理。校正的目的是使图像的外极线与扫描线重合，从而将立体匹配时的搜索范围限制在一个像素行中，提高搜索效率。对校正后的图像再进行高斯滤波和直方图均衡化处理，以去除噪声和增加图像的对比度。

2.1   左右目图像立体匹配

由于车辆在运动过程中，摄像机观察周围场景的角度、环境亮度等不可避免地会发生变化，因此采集到的图像也会发生旋转、缩放及亮度等方面的变化; SURF与SIFT(scale invariant feature transform)算法一样，对图像旋转和缩放等具有鲁棒性^[5-6]，但它的速度比SIFT算法快很多，所以可采用SURF算法来提取特征。

在提取SURF特征后，以左目图像为基准图并选定特征点，然后在右目图像的相应搜索范围中进行匹配搜索。采用欧式距离作为立体匹配特征点相似性度量函数，即：

式中：S_p为左图中点p的特征向量; S_p为右图中点q特征向量。

鉴于图像校正算法的误差，实际图像中不能保证所有特征点都和其对应点处于同一像素行中，因此将左图中的一个特征点p的实际搜索范围限制在右图中以同一行为中心的上下3行，这样既能减少匹配时间，又能增加匹配的成功率。具体匹配过程^[7]如下：在右图搜索范围中搜索与点p的特征向量欧式距离最近和次近的2个最邻近特征点q和q′，它们与p的欧式距离分别为d和d′，如果欧式距离的比值r=d/d′小于某一阈值K(推荐的阈值为0.8)，则认为p和q是一对匹配点。

按上述过程匹配所有检测到的特征点，就能获得一个匹配点对集合，但在这个匹配集合中依然存在左图中多个特征点对应右图中一个特征点的情况。因此可以以右图为基准图，运用相同方法反向匹配，再获得一个匹配集合。最后将2个集合进行交集运算，求得的公共子集即为左、右双目匹配点对集合。对于得到的初始匹配点对可以采用随机抽样一致性^[8] (random sample consensus, RANSAC)算法进行过滤，进一步降低误匹配率。

在完成立体匹配后，获取视差图并对其进行滤波处理，最后结合摄像机标定参数即可计算出这些匹配特征点的三维坐标。

2.2   特征跟踪匹配

特征跟踪匹配的目的是建立车辆运动前后主图像(左图像)特征点间的对应关系。当车辆运动到t_k+1时刻获取双目图像后，将左图像与运动前t_k时刻的左图像进行特征跟踪匹配。由于本文为估算车辆的实时速度，图像采样间隔较小，前后两帧图像之间有较大重合度，因此可以采用前述立体匹配方法进行特征跟踪匹配。

本文假设摄像机随车辆做平面刚体运动，因此后一帧图像的成像面虽然较前一帧图像的成像面发生了旋转与平移，但在垂直于运动平面的方向上并未运动，那么同一特征点在前后图像中理论上依然位于同一像素行。根据此约束条件，在前后两帧之间进行特征跟踪匹配时，其搜索范围被限制在以同一像素行为中心的上下3行。

在减少误匹配方面，可以采用以下方法：设W₁、W₂为前一帧图像中的两个特征点的三维坐标，W′₁、W′₂为它们在后一帧图像中对应匹配点的三维坐标。根据空间中任意两点之间的距离在不同坐标系下保持不变，则有：

令$\gamma = \frac{{\left\| {{W^\prime }_2 - {W^\prime }_1} \right\|}}{{\left\| {{W_2} - {W_1}} \right\|}}$，理论上前后两帧图像特征点准确匹配时γ的值应该为1。在实际情况中可以设定一个阈值范围，当处于这个阈值范围中时，可以认为是正确的匹配点对，否则就为误匹配。结合本文实际，选择阈值范围为0.8~1.2。

3   变换参数计算

经过图像立体匹配和特征跟踪匹配之后，可以得到车辆运动前后N组特征匹配点对的坐标集合{(P_i^t_k、P_i^t_k+1), i=1, 2, …, N}，接下来要解决的问题就是通过这些坐标值计算出(1)式中的正交旋转矩阵R和平移矢量T，从而计算出车辆的速度。对于旋转矩阵R求解方法有奇异值分解法(SVD)、正交分解法(OD)和单位四元数分解法^[9]。单位四元数分解法稳定性好，能够得到很好的数值结果。旋转矩阵R与单位四元数q=[q₀, q₁, q₂, q₃]之间的关系为

本文采用单位四元数法和随机抽样一致性算法(RANSAC)相结合的方式来计算R和T。具体过程如下：先从N组特征点对集合{(P_i^t_k、P_i^t_k+1), i=1, 2, …, N}中随机选取3组非共线的特征点对(P₁^t_k、P₃^t_k+1)、(P₂^t_k、P₂^t_k+1)、(P₃^t_k、P₂^t_k+1)，依据文献[10]所述的单位四元数法求解旋转矩阵R_e和平移矢量T_e; 然后用RANSAC法剔除这N组特征匹配点对中的误匹配^[11]。将特征点对集合中的各组匹配点对的三维坐标代入(1)式中，并设置一个误差限e，若各匹配点对的误差满足：

则认为这对匹配点对为内点，否则为外点。经过多次计算，选择出内点数目最多的一组，利用这些内点重新计算，得到最终的旋转矩阵R和平移矢量T，再通过(2)式就可求得θ。

4   实验及结果

为了验证所提方法的有效性，在室内进行了模拟小车实验。如图 3所示为实验所用AGV小车，其上安装的双目视觉系统为Bumblebee2，相机的分辨率为648 pixel×488 pixel，帧频为48 fps。实验中小车沿“直线-圆弧-直线”路线行驶，各阶段以给定速度大小匀速前进。

图  3  实验装置

Figure  3.  Experimental equipment

下载: 全尺寸图片 幻灯片

对采集到的双目序列图像首先进行极线校正、高斯滤波和直方图均衡化处理。接着对t_k(k=1, 2, …, N)时刻的左右图像进行基于SURF特征的立体匹配，图 4为实验中某一帧左右图像的匹配结果。根据匹配结果计算特征点的三维坐标，其Z坐标值就是小车到该环境特征点的距离，表 1为部分特征点的三维坐标值。

图  4  某一帧左右图像立体匹配结果

Figure  4.  Stereo matching results of left and rightimages for one frame

下载: 全尺寸图片 幻灯片

表  1  某相邻两帧间部分对应环境特征点的三维坐标  单位：mm

Table  1.  3D coordinates of some corresponding environmental feature points between two adjacent frames

特征点编号	tk时刻特征点坐标(X, Y, Z)	tk+1时刻特征点坐标(X, Y, Z)
1	(8.670，19.344，566.393)	(127.463，20.283，571.327)
2	(2.908，-29.373，583.549)	(122.217，-28.423，588.623)
3	(10.918，2.246，571.093)	(129.843，3.175，575.927)
4	(10.790，9.766，569.612)	(129.677，10.689，574.461)
5	(18.595，-24.373，581.284)	(137.824，-23.405，585.815)
6	(17.429，14.277，566.984)	(136.228，15.205，571.607)

下载: 导出CSV 

| 显示表格

为了验证方法的稳定性，本文在不同车速、不同采样时间间隔的条件下，进行了3组实验。实验中，利用前述特征跟踪匹配方法对各相邻两帧左图像进行匹配，并结合相应的立体匹配结果计算特征点的三维坐标; 然后利用这些三维坐标，根据前文方法计算出小车运动前后左摄像机坐标系之间的变换参数，即旋转矩阵R和平移矢量T。最后根据(4)式和(5)式就可求得小车在相邻两帧间的平移速度和偏转角速度。实验结果如图 5和图 6所示。其中图 5为车速较低时使用不同采样间隔的实验结果; 图 6为车速较高时的实验结果。从图 5和图 6中可知，实验中各个条件下的速度计算结果整体上都比较平稳，与理论真值相比，计算结果的平均误差都在6%以内。特别是当采样间隔Δt较小及车速较低时，计算结果更趋稳定，这是因为这种情况下相邻两帧图像的重合度较高，有更多的匹配特征点，计算所用数据更多。

图  5  车速较低时的实验结果

Figure  5.  Experimental results of slower vehicle speed

下载: 全尺寸图片 幻灯片

图  6  车速较高时的实验结果

Figure  6.  Experimental results of higher vehicle speed

下载: 全尺寸图片 幻灯片

5   结论

针对常见的车辆定位与测速方法的不足，本文提出了一种基于双目视觉的车辆测距定位与自车速度估计方法。利用车载双目视觉传感器采集环境序列图像，再通过图像中环境特征点的立体匹配和跟踪匹配，同步获取了车辆相对于周围环境的位置与自车速度信息。模拟实验表明该方法是可行的，具有较高的精度。在下一步工作中，应该采用分辨率高、视场大的摄像机进行实车实验，进一步验证该方法的稳定性。