基于立体视觉的交叉路口对向车辆运动状态估计*

舒方林 张海波 曹文冠

（上海工程技术大学机械与汽车工程学院 上海 201600）

1 引言

高级辅助驾驶系统目的是在复杂的架势环境中为驾驶员提供精确的路面信息，从而预先让驾驶员能够在最短时间内察觉到潜在的危险。快速、准确地检测出车辆前方运动目标位置以及它们相对于自身车辆的运动状态，是此类系统的重要基础之一。以往许多有关于此类系统中车辆的检测与跟踪大多采用毫米波雷达、激光雷达等传感器，但鉴于立体视觉传感器具有非接触性、抗干扰以及被动性的优点，立体视觉传感器逐渐被应用到高级辅助驾驶系统中，例如障碍物检测、车道线识别和交通标志识别［1～5］等。国内外也有许多学者提出了许多基于立体视觉的运动目标运动状态估计方法。文献［6］是最早解决这个问题的著作之一，文献中Young 等利用运动模型的扩展卡尔曼滤波器通过对一组点的三维位置的噪声观测，估计出这些点的运动参数。文献［7］中研究了利用标定的立体视觉装置计算刚体运动的三维路径问题，在立体视图中对运动目标的特征点进行检测与跟踪，并利用相似三角形原理计算特征点的三维位置。文献［8］通过立体视觉估计迎面而来车辆的运动，使用扩展卡尔曼滤波器跟踪车辆特征点云，进而估计出目标车辆的运动状态，包括车辆的速度、加速度以及偏航率。文献［9］提出了一种利用立体视觉估计月球车运动状态的算法，该算法提取图像的SIFT 特征点并建立相应帧间的对应关系，由此估计月球车的运动参数。文献［10］对车载双目视觉传感器采集的双目序列图像进行基于SURF 特征的跟踪匹配实现实时测距及自车速度的估计。以上利用立体视觉对目标运动参数的估计方法都依赖于目标的特征点，但随着立体视觉传感器视角的改变以及目标的自身遮挡，目标特征点的稳定性显著降低，当目标在三维空间中经历较大的旋转和平移时，特征点跟踪的精度就会受到较大影响。因此，本研究针对此问题基于非结构化道路交叉口交通场景，提出一种动态参考点方法估计目标车辆的速度，并提出基于车辆运动轨迹的航向角估计方法。

2 车辆姿态估计

2.1 参数表示

目标车辆采用特征点云以及外包长方体表示，如图1 所示。在立体视觉系统坐标系Oxyz下，t时刻目标车辆参数可以表示为Bt=(Xt，vt，Dt)，其中Xt=(xreft，yreft，zreft，φt) 为 目 标 车 辆 位 姿，(xreft，yreft，zreft)表示参考点坐标，φt为目标车辆的航向角即其运动方向与Z轴的夹角；vt=(vxt，vyt，vzt)表示目标车辆分别沿x、y、z轴的速度，假设车辆沿高度轴方向没有运动，则vt=(vxt，0，vzt)；Dt=(lt，wt，ht)为目标车辆外包长方体模型，其中lt、wt和ht分别为t时刻测量的外包长方体的长、宽、高。

图1 目标车辆及参数表示示意图

2.2 参数估计

2.2.1 基于动态参考点的目标车辆速度估计

动态参考点模型基本原理［11～12］：特征点云分布特性以致于无法利用外包几何体中心作为参考点，但只要目标车辆在自身车辆可观测范围内，都可以找到一个外包长方体体角点作为参考点，并且随着目标车辆位置的改变该参考点可以动态更新。

将非结构化道路交叉口对向车辆运动模式分为三种：直行、左转以及右转，基于此原理针对对向车辆每种运动模式选取不同的参考点。如图2 和图3 所示，当对向目标车辆直行通过交叉口时，由于车辆自遮挡导致侧部以及尾部数据丢失，立体视觉系统只能准确获取车辆外包长方体的宽度和高度参数；当对向目标车辆左转或者右转通过交叉口时，由于车辆侧部的数据部分丢失立体视觉系统只能准确获取车辆外包长方体的高度参数，而不能精确获取长度和宽度参数；且目标车辆无论是右转还是左转，观测车辆都存在传感器视场盲区，此种情况下立体视觉系统智能获得目标车辆的尾部数据。因此，动态参考点选择遵循以下原则。

图2 动态参考点选择示意图

图3 动态参考点选择鸟瞰图

1）初始化参考点：当目标车辆直行通过交叉口时，选择车辆外包长方体上角点作为参考点(xreft，yreft，zreft)，即图2 中的A3角点；当t时刻目标车辆左转或右转通过交叉口时，选择目标车辆外包长方体与自车最邻近上角点作为参考点(xreft，yreft，zreft)，分别如图2中的A9、A10角点。

2）参考点保持：当目标车辆不改变方向直行通过交叉口时，则参考点(xreft，yreft，zreft)仍为上一时刻车辆外包长方体角点。

3）参考点更新：当目标车辆先直行再左转进入左视场盲区通过交叉口时，参考点先由A3切换至A9，进入左视场盲区后再从A9切换至A11；同理当目标车辆先直行再右转进入右视场盲区通过交叉口时，参考点先由A3切换至A10，进入右视场盲区后再从A10切换至A12。

因此，对参考点前后时刻位置进行差分可求解目标车辆的速度：

式中T为前后时间间隔。

2.2.2 航向角估计

在复杂的道路交通环境中，车辆经常出现换道、转向等行为，这要求智能车必须能够准确、快速地识别其行驶航向角。基于车辆的轴对称性，目标车辆的航向角可以表示为其行驶方向与立体视觉系统坐标系Z轴的夹角，如图4 所示。

图4 目标车辆运动轨迹及航向角示意图

在短时间内（几个采用周期或几十个采样周期），对传感器测量数据进行逐段拟合，目标车辆的运动采用如下二次曲线表示：

二次曲线的参数a、b、c可由最小二乘估计法［13］求出，通过求导原则可以得到二次曲线的斜率：

曲线的斜率表示其切线方向，也即目标车辆的瞬时运动方向或相反方向，对曲线斜率求反正切便可以得到曲线切线方向与Z轴的夹角，即目标车辆相对于观测车辆的航向角：

3 车辆跟踪

应用于车辆跟踪的高级运动模型包括恒定速度模型、恒定加速度模型、恒定转率和速度模型以及恒定转率和加速度模型等。恒定速度模型和恒定加速度模型均为线性运动模型，实际的车辆跟踪显然不能使用简单的线性模型建模。基于交叉口车辆运动特性，本研究采用恒定转率和速度模型［14］作为基本研究模型，以恒定转率和速度模型构造扩展卡尔曼滤波对目标车辆运动状态进行估计。

在恒定转率和速度模型中，目标车辆的状态向量表示为

其中，(z，x)表示目标车辆在车辆坐标系下的位置，v，φ，ω分别表示目标车辆的速度、偏航角以及偏航角速度。

当ω≠0 时，恒定转率和速度模型的状态转移函数为

当ω=0 时，此时目标车辆变为直线行驶，因此，状态转移函数为

扩展卡尔曼滤波［15］是利用泰勒级数展开方法将非线性滤波问题转化成近似的线性滤波问题，利用线性滤波的理论求解非线性滤波问题的次优滤波算法。该算法的原理是根据前一时刻对状态的估计值和当前时刻的观测值来获得当前时刻的滤波值，因此，整个环路滤波过程可以分为状态预测和状态更新两部分。

1）预测

在以恒定转率和速度模型构造的扩展卡尔曼滤波系统中，系统的一步状态预测方程可写为

其中，g(xk，u)表示恒定转率和速度模型的状态转移函数，u表示处理噪声。

一步状态预测协方差为

其中，JA是对状态转移函数中每个元素求偏导得到的雅可比矩阵，Q为处理噪声的协方差矩阵。

2）更新

滤波增益：

式中，JH为传感器测量矩阵的雅可比矩阵，R为传感器测量噪声协方差矩阵。

状态估计方程：

式中，Zk为传感器测量值，h(xk)为非线性映射函数。

状态估计协方差：

式中，I为单位矩阵。

4 实验结果与分析

我们在典型的乡村交叉口交通场景对本研究提出的方法进行了试验。主要实验设备包括一台目标车辆和一台观测车辆，其中观测车主要搭载了一套立体视觉系统。立体相机的主要参数为基线16mm，视场角40°，图像帧率30fps。目标车辆作为观测车识别对象，实验主要通过对目标车辆的速度以及航向角计算来验证本研究所提出的方法。所有的实验都是在Intel i7-8700、3.20GHz、12GB 内存的核心处理器上进行的。

1）速度估计实验分析

为了更好地将目标车辆的真实运动速度与实验估计速度进行对比，实验目标车辆的运动模式为：目标车辆在距离交叉口12m 远处以25km/h 的初始速度减速至15km/h，并以此速度左转通过交叉路口。图5 为实验视频序列中截取帧，如图所示。在第20 帧中，目标车辆在距离观测车28m 处被检测到，此时车辆还处于直线运动阶段；在第65帧中，虽然目标车辆的运动方向没有明显改变，但预测的车辆运动轨迹此时已经开始弯曲；在第82帧中，目标车辆正在左转弯通过交叉口。

图5 交叉口目标车辆跟踪截取帧

图6 为目标车辆实验估计速度与真实速度曲线图，将速度估计结果与目标车辆真实速度进行比较可以看出，估计出的速度与目标车辆的真实速度非常接近，误差控制在4%以内。值得注意的是，在第100 帧时，目标车辆进入半视场盲区，此时参考点由A4角点切换至A8角点，但目标车辆的速度仍非常平稳，并未发生大的突变。

图6 目标车辆速度估计结果

由以上实验结果分析可知，针对非结构化交叉口道路，提出的动态参考点模型结合扩展卡尔曼滤波算法的速度估计方法在准确性和平稳性方面均能取得比较满意的结果，可以较好地反映目标车辆的实际运动速度。

2）航向角估计实验分析

航向角估计实验目标车辆采用与速度估计实验相同的运动模式。图7 为预测的目标车辆运动轨迹鸟瞰图，图8为目标车航向角估计结果。

图7 目标车辆运动轨迹鸟瞰图

由图8可知，目标车辆在第20帧被检测到至第65 帧开始左转，航向角估计值在15°上下微小波动（实验中设定左转航向角为正，右转为负），目标车辆从第65 帧开始至左转通过交叉路口，航向角估计值从15°逐渐增加至90°左转通过交叉口，整个过程航向角变化与实验设置的目标车辆运动过程基本相符。由上述分析可知，本研究提出的基于目标车辆运动轨迹的航向角估计方法能够获得比较理想的结果，可以较好地应用于交叉口车辆航向角估计。

图8 目标车辆航向角估计结果

5 结语

本文主要研究了在非结构化交叉路口下，采用立体视觉估计对向车辆的运动状态，包括车辆的速度和航向角。在速度估计中，针对采用车辆特征点作为参考点计算车辆速度方法容易受车辆自身遮挡以及观测点位置的影响，提出了一种动态参考点模型估计目标车辆的速度；还提出一种基于车辆运动轨迹的方法计算目标车辆的航向角。实验结果表明，目标车辆的速度估计误差在±4%范围以内，航向角估计也与目标车辆实际运动情况基本相符。多车辆相互遮挡情况下的运动状态估计，将是我们未来的工作之一。