视觉与单路侧单元辅助的车辆定位方法*

盛树轩，荆崇波，蒋朝阳

（北京理工大学机械与车辆学院，北京 100081）

前言

智能运输系统因其在改善道路安全和提高交通运行效率方面的巨大潜力而受到广泛关注，车辆定位是其中的一项关键技术。目前，全球导航卫星系统（global navigation satellite system，GNSS）是应用最广泛的车辆定位工具，它能提供车辆的绝对位置信息，但易受到环境干扰且定位频率低。惯性测量单元（inertial measurement unit，IMU）能高频率提供相对定位信息，但定位结果存在累计误差，利用二者互补性的组合导航方法得到了广泛的研究与应用。然而在城市峡谷等环境下，由于多路径和非视距等原因导致GNSS定位精度低，IMU得不到及时的校正时，算法很快失效。随着通信技术的发展，V2X（vehicle-to-everything）协同定位方法提供了一种新的解决思路。Rohani等提出了一种集中式车-车（V2V）协同定位方法，自车与周围车辆构成一个车载自组织网络（vehicular ad-hoc network ，VANET），VANET 中的车辆之间可互相通信与测距，利用扩展卡尔曼滤波融合车辆各自的GNSS 测量和车间距离测量，仿真结果表明可获得比单纯GNSS 测量更优的定位结果。Hoang 等提出了类似的方法并采用超宽带（ultra wideband，UWB）进行车辆间距离测量，通过实验测试了UWB 测距的可行性。Alam 等进一步考虑了IMU 测量，并提出通过交换GNSS 多普勒频移数据来计算车辆之间的距离，取代测距传感器的直接测量，但利用GNSS 多普勒频移数据间接获取车辆之间的距离是不可靠的，误差有时会超过1 m，这对于协同定位来说不可接受。

V2V 协同定位方法存在的不足限制了其应用。首先，V2V 协同定位的效果很大程度取决于VANET中车辆先验位置估计的准确性，如果协同车辆位置估计的误差很大，自车与其协同后反而导致定位精度降低。其次，由于车辆运动受到道路和交通规则的强约束，而道路的纵向长度远远大于横向车道宽度（例如3.7 m），VANET 拓扑构型通常沿着与道路共线的方向变形，导致与道路垂直方向的误差持续变大并在网络中传播，进一步恶化协同定位的效果。

一些学者考虑在V2V 的基础上引入基础设施辅助定位并挖掘VANET 中更加丰富的协作信息，如相对角度和相对速度。Hoang 等提出一种基于粒子滤波的数据融合框架，利用IMU 和轮速计测量进行航迹推算，然后和车间距离测量、车辆到基础设施距离测量共同进行滤波得到最终的定位结果，该方法须部署多个路侧单元。Zhu 等采用UWB 作为测距传感器，部署在路侧的多个UWB 跟车载UWB 进行测距，基于三点定位法得到车辆的位置，用来提高GNSS受限区域内的定位精度。Zhang等提出一种V2I 协同定位方法，利用IMU 进行航迹推算，同时估计车辆与RSU 之间的相对距离和相对角度，利用加权最小二乘法计算车辆的位置，但仅进行了仿真验证，没有给出可以实际应用的角度传感器。Fan等在考虑距离测量的基础上，进一步考虑VANET中车辆间相对速度的测量，将协同定位看作一个非线性优化问题，借助拟牛顿算法进行求解，同样只进行了数值仿真来验证方法的有效性。

综上所述，目前大多数协同定位方法都处于仿真模拟阶段，仍存在一些不足而限制了其实际应用，包括：①实际应用过程中，相对角度和相对速度等信息难以准确测量；②密集部署RSU 导致成本过高；③一些可用于车辆定位的交通语义信息未能得到充分利用。针对这些不足，本文中提出了一种视觉与单RSU 辅助的车辆定位方法，利用相机观测车辆到车道线的横向距离，利用单RSU 与车辆进行测距与通信，结合车载GNSS、IMU 测量，借助误差状态卡尔曼滤波（error state Kalman filtering，ESKF）算法融合多传感器信息得到车辆位姿。该方法的特点如下：

（1）只须部署单个RSU，成本低。

（2）利用RSU 测距信息降低纵向定位误差，利用车辆到车道线的横向距离观测降低横向定位误差，二者优势互补，取得水平定位误差均方根小于10 cm的定位精度。

（3）其有效性得到实车实验的验证。

1 方法概述

本文中提出的车辆定位方法基于这样一个事实：IMU 产生不易受环境干扰的带有噪声的测量且很少出现离群值，但存在累计定位误差，若能利用GNSS等其他传感器观测对累计误差进行修正，即可得到比较准确的定位结果。但GNSS 在城市峡谷等环境下定位精度低，无法对IMU 累计误差起到有效修正，因此，本文进一步考虑视觉与单RSU 观测辅助，并采用ESKF 算法融合IMU、GNSS、RSU 和相机观测信息得到车辆位姿。

ESKF算法将系统状态分为真实状态、名义状态和误差状态。其中，不考虑传感器测量噪声和系统扰动的情况下得到的系统状态称为名义状态，真实状态与名义状态之间的差值为误差状态。算法运行时，名义状态预测模块根据IMU 测量和递推方程进行名义状态（包括姿态、速度等）预测，误差状态预测模块根据误差状态预测方程对误差状态进行预测（包括姿态误差、速度误差等），二者并行。当GNSS、RSU 和相机观测可用时，根据观测值基于卡尔曼滤波算法对误差状态估计进行修正，然后将修正后的误差状态注入到名义状态中，修正后的名义状态作为定位结果输出，当其他传感器不可用时直接输出名义状态预测。由于利用其他传感器观测对IMU累计误差进行了周期性修正，故可取得良好的定位结果。方法框图如图1所示。

图1 定位方法框架图

2 视觉与单RSU辅助的定位方法

首先介绍如何基于视觉观测车辆到车道线的横向距离以及RSU 与车载单元（on board unit，OBU）之间的测距过程，然后给出利用ESKF 算法融合多传感器信息得到车辆位姿的推导过程。

2.1 基于视觉的横向距离观测

相机面向前方安装在车辆顶部，位于车辆纵向中心面中且向下倾斜朝向路面，如图2 所示。横向距离观测从阈值化处理开始，然后进行逆透视变换，并使用滑动窗口提取车道线对应的像素点坐标，使用多项式拟合出车道线方程，最后根据标定参数计算出车辆到车道线的横向距离。

图2 车载单目相机观测横向距离示意图

（1）阈值化处理 采用梯度阈和颜色阈相结合的方法进行阈值化处理。由于车道线沿着图像纵向分布，故选取Sobel 算子进行横向梯度检测，阈值的下限为10，阈值的上限为50。我国公路上的车道线分为白色和黄色两种，考虑到色相-饱和度-亮度（hue-saturation-lightness，HSL）颜色空间下的L通道和Lab 颜色空间下的b 通道分别对白色和黄色敏感，因此在HSL 颜色空间下的L 通道处理白色车道线，在Lab 颜色空间下的b 通道处理黄色车道线，阈值的下限皆为220，上限皆为255。最后根据式（1）将阈值化处理后的二值化图像进行合并。

式中：(，)为阈值化处理后的图像；(，)为原始图像；(，)为像素点坐标；为设定的梯度阈值区间和颜色阈值区间的交集。图像阈值化处理后如图3（b）所示。

（2）车道线提取 对经过阈值化处理的图片进行逆透视变换，使车道线以平行的方式呈现在鸟瞰图中。经过逆透视变换后得到尺寸为×的图像，其中坐标为(，)的像素点的灰度值用(，)表示，对每一列像素点的灰度值进行求和计算：

采用滑动窗口法从图像底端向上提取车道线像素点集。选取矩形窗口，窗口的宽设为100，高设为60。首先将使sum取最大值的作为窗口的滑动起始点，记录滑动窗口内的非零值像素点（即车道线）坐标，并统计像素点横坐标的平均值作为滑动窗口下一阶段的起始点，重复上一阶段操作直至到达图像顶端。车道线提取结果如图3（c）所示。

（3）车道线拟合 对滑动窗口提取到的像素点坐标进行最小二乘多项式拟合：

式中：、和为图像坐标中多项式的系数，其中原点位于图像的左上角；轴指向右侧，轴指向下方。车道线拟合结果如图3（d）所示。

图3 基于视觉的横向距离观测

（4）横向距离计算 车道线拟合后可得到以像素为单位的横向距离，然后乘以一个从像素距离到现实距离的换算系数，得到以米为单位的横向距离。换算系数的计算公式为

式中：为相机中心到车道线的横向距离，m；为从图像中心到车道线横跨图像最底行的像素数，选择最底行是因为它具有最高的分辨率。

2.2 RSU测距

本文中重点关注如何利用RSU 提供的测距信息设计定位算法以提高车辆的定位精度，不关心RSU采用何种通信协议等具体的硬件实现细节。因此仅简要描述RSU 与OBU 之间的测距过程，如图4所示。RSU 部署在路侧，OBU 向RSU 广播请求信号，RSU接收请求信号并进行相关处理，然后RSU向OBU 发送包括ID、位置坐标、请求信号接收时刻和数据包发出时刻等在内的数据包。OBU根据接收到的数据包的信息，基于双向测量的方法计算得出与RSU之间的距离：

图4 RSU测距示意图

式中：为OBU 发出请求的时刻；、分别为RSU 接收请求和发出数据包的时刻；为OBU 接收到数据包的时刻；为信号传播速度。

2.3 基于ESKF算法的多传感器信息融合

利用ESKF 算法融合IMU、GNSS、RSU 和相机观测信息得到车辆位姿的推导过程。

名义状态的递推方程为

式中：为地球自转角速率，其值常取7.2921151467 × 10rad/s；、分别为所在位置的纬度、高度；v、v分别为导航坐标系下东向和北向的速度；R、R的表达式见式（12）和式（13）。

式中：R和分别为地球被近似描述为参考旋转椭球体的长半径和扁率，按WGS-84标准，地球长半径等于6 378 137 m，扁率等于1/298.257。

定义误差状态变量为

假设失准角很小，且不考虑地球重力模型误差，则误差状态微分方程可写为

在上述误差状态微分方程的基础上，考虑系统过程噪声，得到误差状态方程为

1) 通过Simulink对简化的起重机起升机构的模型进行系统仿真，得出了该系统的仿真结构图，为系统的仿真分析与研究奠定了基础。

其中

设角度随机游走为n，速度随机游走为n，则系统噪声协方差矩阵定义为

式中：- 1、为相邻的两个时刻；△为两个时刻的间隔；为单位矩阵，维度与F相同。

2.3.2 误差状态修正

利用GNSS、RSU 和相机观测信息对误差状态的先验估计进行修正。要将车辆到车道线的横向距离观测纳入滤波算法中，必须根据当前车辆位置找到车道线上的相关路点，本文假设车辆已知所处车道和关联车道线路点地图，各路点坐标事先已知，根据名义位置估计在路点坐标集内进行搜索，找到欧式距离最近的路点作为相关路点，如图5所示。

图5 相关路点查找示意图

考虑GNSS、RSU 和相机观测信息，得到误差状态的观测方程为

其中

其中

对系统观测噪声协方差矩阵建模为

式中σ、σ、σ、σ分别为GNSS 速度观测误差、GNSS位置观测误差、RSU 测距误差和视觉横向距离观测误差，根据实验分别取值为0.5 m/s、5 m、0.1 m、0.1 m。

式中：K为时刻的卡尔曼矩阵；为单位矩阵，维度与KH一致。

2.3.3 误差状态注入与重置

将经过滤波后的误差状态注入到名义状态中，对名义状态进行修正：

误差状态注入完成后，将误差状态根据式（32）进行重置，并用于下一次估计。

3 实验与结果分析

3.1 实验设备与测试场景

为验证所提出方法的有效性，进行实车测试。选用UWB 传感器来实现RSU 与OBU 之间的测距和通信功能。UWB 分为锚和标签两种角色，其中标签安装在车辆上随车辆运动充当OBU，锚部署在路侧充当RSU，二者可以实时测距与通信。具体地说，测试车辆配备了Novatel PwrPak7D-E1 高精度惯性导航系统、司南M300 卫星导航系统、XSENS MTi670 IMU、Nooploop UWB、大恒MER-231-41U3C 彩色相机和笔记本电脑。PwrPak7D-E1接收千寻公司基站发送的RTK 差分数据，其输出被认为是车辆行驶过程中定位结果的真实值。所有传感器通过串口与电脑相连。所有传感器的数据在机器人操作系统（robot operating system，ROS）框架下进行采集，不同传感器之间的时间同步也通过ROS 提供的时间戳实现。车辆测试平台如图6所示。

图6 车辆测试平台

选择典型校园场景进行测试，如图7 所示。实车实验路线包含基本道路要素和场景要素，能代表大部分道路，同时道路两侧有许多树和高楼，给车辆定位带来挑战。车辆由静止开始，沿着道路由南向北行驶，此时只有GNSS 和IMU 可用，行驶一段距离左转进入测试区域A，此时GNSS、IMU 和RSU 可用，继续向前行驶进入区域B，GNSS、IMU、RSU 和视觉横向距离观测均可用。

图7 测试场景说明

3.2 实验结果分析

将考虑GNSS、IMU 信息的方法称为GNSS/IMU，将考虑GNSS、IMU 和RSU 测距信息的方法称为GNSS/IMU/DIS，将考虑GNSS、IMU、RSU 测距和视觉横向距离观测信息的方法称为GNSS/IMU/DIS/CAM。

图8 给出3 种方法在“NED”坐标系下对应的轨迹和RSU 的位置。开始时只有GNSS、IMU 可用，3种方法对应的轨迹是重合的。当车辆驶过一个路口后，转为由东向西行驶，此时由于道路两侧的树木及高楼的遮挡，GNSS测量开始变差，GNSS/IMU 方法对应轨迹与真实轨迹明显分离。驶入区域A 后，RSU距离测量可用，在RSU 测距信息辅助下车辆轨迹很快接近真实值。车辆继续向前行驶，逐渐远离RSU，GNSS/IMU/DIS 方法对应的轨迹逐渐偏离真实值，而GNSS/IMU/DIS/CAM 方法对应的轨迹始终接近真实值。

图8 “NED”坐标系下的车辆轨迹图

图9和图10 分别示出3 种方法在“NED”坐标系下对应的北向位置和东向位置随时间的变化情况。GNSS/IMU/DIS 方法在RSU 测距信息的辅助下可修正北向和东向位置，且可以看出随着离RSU 的距离逐渐增加，对北向位置的修正效果逐渐降低，而对东向位置始终能显著修正。原因是车辆行驶时受到道路约束，而道路的纵向长度远远大于道路的宽度（单车道宽度一般为3.7 m）。当采用布置在路侧的单RSU 进行测距时，随车辆逐渐远离RSU，RSU 与车辆之间距离测量的横向（本例中近似为北向）分量在整个距离测量中所占的比例逐渐降低，如图11 所示。时刻相比于时刻，RSU距离测量的横向分量在整个距离测量中所占比例远小于纵向分量所占比例，距离测量中所蕴含的横向信息逐渐被纵向信息稀释，导致GNSS/IMU/DIS 方法对车辆横向位置的修正作用随远离RSU 逐渐降低。GNSS/IMU/DIS/CAM 方法基于视觉观测车辆到车道线的横向距离，结合车道线路点坐标对车辆横向位置进行估计，始终对车辆横向位置进行有效修正，弥补了GNSS/IMU/DIS 方法的不足。

图9 “NED”坐标系下车辆北向位置变化图

图10 “NED”坐标系下车辆东向位置变化图

图11 单RSU测距横向分量稀释示意图

图12（a）和图12（b）分别示出车辆北向位置误差和东向位置误差变化图。由图可见，开始时只有GNSS、IMU 可用，误差是相同的。从15 s 开始RSU测距可用，在测距信息的辅助下，北向和东向位置误差均明显降低。从21 s 开始，视觉横向距离观测可用，在横向距离观测的辅助下，GNSS/IMU/DIS/CAM方法对应的北向位置误差始终显著降低，而GNSS/IMU/DIS 方法由于RSU 测距信息的横向分量被不断稀释，对应的北向位置误差逐渐增大。这表明单RSU测距信息主要降低纵向（本例中近似东向）定位误差，横向距离观测可以有效降低横向（本例中近似北向）定位误差，二者优势互补。

进一步对比不同横向距离观测精度对定位结果的影响。在横向距离观测真值的基础上添加均值为0、标准差为0.2 的高斯噪声生成模拟的横向距离观测，实际横向距离观测、横向距离观测真值和模拟横向距离观测三者的对比如图13 所示。可以看出，模拟横向距离观测存在很大误差，用于表征横向距离观测特别不准确的情况。

图13 横向距离观测对比图

3 种不同横向距离观测精度对应的位置误差如图12（a）和图12（b）所示，CAM、CAM（真值）和CAM（Std=0.2）分别代表横向距离的实际观测值、真值和模拟值，并用以区分不同横向距离观测精度对应的定位误差。可以看出，当横向距离观测为真值，车辆的横向定位误差最小，几乎为零。当横向距离观测为视觉系统实际测得值，横向距离观测存在较小的误差，车辆的横向定位误差略有增加。当横向距离观测为模拟值，模拟的横向距离观测存在较大误差，其对车辆横向定位误差的降低作用减弱，但相比于不考虑横向距离观测信息的GNSS/IMU、GNSS/IMU/DIS 方法，仍能有效降低横向定位误差，弥补单RSU在消除横向定位误差方面的不足。不同精度的横向距离观测对应的纵向定位误差则没有明显区别。

图12 定位误差图

表1中汇总了GNSS、IMU、RSU 和视觉横向距离观测均可用区域内3 种定位方法对应的北向、东向位置误差和总的水平位置误差的平均绝对误差（mean absolute error，MAE）以 及 均 方 根 差（root mean square error，RMSE）。以RMSE 指标为例，在北向上，GNSS/IMU/DIS/CAM 方法的误差比GNSS/IMU方法降低了95.9%，比GNSS/IMU/DIS 方法降低了81.9%。在东向上，GNSS/IMU/DIS 方法和GNSS/IMU/DIS/CAM 方法对应的误差均小于10 cm，后者的误差比GNSS/IMU 方法降低了95.5%。同时，GNSS/IMU/DIS/CAM 方法的水平定位的MAE 和RMSE 误差均小于10 cm，实现了厘米级定位。

表1 定位误差

4 结论

提出一种视觉与单RSU 辅助的车辆定位方法，利用ESKF算法对GNSS测量、IMU测量、RSU测距和视觉横向距离观测信息进行融合，实现对车辆位姿的准确估计，并得到了实车测试的验证。研究结果表明：

（1）单RSU 测距信息可以有效降低车辆纵向定位误差，但由于存在测距横向分量的稀释问题，对车辆横向定位误差的修正作用随车辆远离RSU 逐渐降低。

（2）基于视觉观测车辆到车道线的横向距离，可以有效降低车辆横向定位误差，弥补单RSU 测距横向分量稀释问题，而无须借助密集部署RSU 来弥补。

（3）视觉与单RSU 辅助的定位方法的水平定位的MAE 和RMSE 误差均小于10 cm，可实现厘米级定位。

最后需要指出的是，受到实验条件的限制，仅在校园园区内进行测试，今后须在其他道路环境中开展更广泛的测试。