基于路侧多传感器融合的车辆信息检测方法

魏吉敏，张长隆

长沙智能驾驶研究院有限公司

【摘要】 针对未来道路上网联车辆和非网联车辆混行的情况，本文提出了结合毫米波雷达、摄像头、路侧单元优点的路侧多传感器融合算法对车辆的信息进行检测。路侧多传感器融合的算法包含如下几部分 ： 将不同传感器检测的目标进行时间同步，将不同传感器检测的目标位置统一至WGS-84坐标系中 ； 采用全局最近邻算法实现不同传感器检测的多个目标与融合跟踪列表中的多个目标匹配 ； 利用卡尔曼滤波和扩展卡尔曼滤波对目标的状态进行估计，最终实现车辆的类型、位置、速度的检测。本文在真实道路部署传感器进行多传感器融合的实验，验证提出的算法的可行性。实验结果表明本论文提出的算法能够准确地区分网联车辆和非网联车辆，同时获得其准确的位置、速度信息。

【关键词】 路侧多传感器融合，毫米波雷达，摄像头，路侧单元，网联车辆

A Method of Vehicle Information Detection Based on Roadside Multi-sensor Fusion

Wei Jimin, Zhang Changlong

Changsha Intelligent Driving Institute Co ., Ltd.

Abstract: This paper proposes a roadside multi-sensor fusion algorithm combining the advantages of millimeter-wave radar,camera,and roadside unit to get the information of vehicles in the traffic condition mixed with connected vehicles and non-connected vehicles in future.Roadside multi-sensor fusion algorithm includes the following parts:the position of targets detected by different sensors is unified with the WGS-84 coordinate system and the time of different sensors is synchronized; the Global Nearest Neighbor algorithm is used to match the multiple targets detected by different sensors and multiple targets in the tracking list; the Kalman filter and Extended Kalman filter is used to estimate the state of vehicles.The feasibility of the roadside multi-sensor fusion algorithm is verified by deploying sensors on the real road.The experimental result shows that the algorithm proposed in this paper can accurately distinguish the connected vehicle and non-connected vehicle,and obtain the accurate location and speed.

Key words: roadside multi-sensor fusion,millimeter-wave radar,camera,roadside unit,connected vehicle

引言

当前网联车辆的渗透率不断提升，而现有非网联车辆报废年限长达10～15年，因此未来很长一段时间会存在网联车辆与非网联车辆混行的情况。目前越来越多的学者开始研究网联车辆与非网联车辆混合通行对交通通行的影响。Rongjian Dai ^[1] 等通过仿真工具研究网联车辆渗透率不断提升对现有的交通系统的影响。Omar Hussain ^[2] 等研究在网联车辆和非网联车辆混行的交通状况通过最优车道的分配来提升道路的通行能力。Nianfeng Wan ^[3] 等通过仿真研究主干道路上最优车速建议对网联车辆和非网联车辆的影响。上述针对网联车辆和非网联车辆的混合通行研究多采用仿真方式进行研究，未来要研究网联车辆和非网联车辆在真实道路上混合通行的情况需要分别获取网联车辆和非网联车辆的真实的位置、速度、类型等车辆信息进行研究分析。

当前获得真实道路上车辆的位置、速度、类型等信息多采用多传感器融合的方式。多传感器融合主要包含了时空同步、数据关联、状态估计等部分。多传感器融合选取的多传感器主要有毫米波雷达、摄像头和激光雷达。JieBai ^[4] 、Yanjin Fu ^[5] 等提出通过路侧摄像头和毫米波雷达的多传感器融合实现对车辆位置、速度等信息的检测，但这些方法无法区分网联车辆和非网联车辆。

目前只有部分摄像头可通过触发的方式实现时间同步，而用于在路侧的网络摄像头和毫米波雷达并不支持上述硬件同步方式。Yanjin Fu ^[5] 等提出一种软同步方法，即在软件中开辟不同的传感器数据接收线程接收传感器的最新数据帧，一旦数据融合的线程触发运行时就从不同的传感器数据接收线程中提取最新的数据帧进行融合，从而实现时间同步。该方法忽略了不同传感器因采样周期不同导致的时间差。对于路侧端的多传感器融合的空间同步目前有将不同传感器的坐标系统一到像素坐标系中 ^[5] ，统一至像素坐标系中虽然能够实现多传感器的融合，但是路侧感知的最终目的是获得车辆的WGS-84坐标，通过上述方式标定后还需再次进行图像坐标系至WGS-84坐标系的标定。

数据关联的作用是将传感器检测的目标与融合跟踪列表的目标匹配，常用的数据关联算法有最近邻算法（NN）、多目标假设算法（MHT）、联合概率密度算法（JPDA）。联合概率密度算法和多目标假设算法在处理多目标匹配时会比最近邻算法效果好，但是随着目标数量的增加，其计算量会呈指数级增长。

状态估计算法主要有卡尔曼滤波（KF）、扩展卡尔曼滤波（EKF）、无迹卡尔曼滤波（UKF）等算法。KF算法能够根据当前传感器检测的目标信息以及目标之前的状态信息对当前目标的状态信息进行估计，但是KF算法只适用于对线性对象进行估计，不适用于非线性对象。EKF通过对KF算法进行改进，将非线性对象通过泰勒公式进行线性化处理，因而其可以应用于对非线性对象的状态估计。Julier ^[6] 等为改善对非线性对象的状态估计效果提出了基于unscented变换的UKF算法，该算法通过对非线性对象进行unscented变换实现对非线性对象的状态估计。与EKF相比UKF能够获得更好的状态估计精度，但增加了计算负荷。

随着行驶在道路上的网联车辆越来越多，现有的毫米波雷达和摄像头等传感器能够检测出道路行驶的所有车辆的信息，但是其无法区分出网联车辆和非网联车辆。如何结合现有的车辆信息检测方法和网联通信技术分别获取道路上的网联车辆和非网联车辆的信息是研究网联车辆和非网联车辆混行对交通流的影响需要面对的问题。本论文利用结合毫米波雷达、摄像头、路侧单元优点，通过路侧多传感器融合算法实现道路上网联车辆和非网联车辆的类型、位置、速度信息的检测，为未来真实道路上的网联车辆和非网联车辆混行的研究打下基础。

1 路侧多传感器融合系统框架

本论文提出路侧多传感融合系统主要由摄像头、毫米波雷达、路侧单元（RSU）、边缘计算单元和交换机等设备组成。路侧传感器的安装示意如图1所示，整个系统的数据流向如图2所示。毫米波雷达和摄像头安装在路侧的横杆上不会受道路上高度较高的车辆的遮挡，从而最大范围地检测道路上通行的车辆。

毫米波雷达通过发射和接收毫米波实现目标检测，但是毫米波雷达反射回的杂波与静止目标的反射波混在一起，不容易区分。为避免毫米波雷达杂波的影响，本论文通过选取速度大于15km/h的目标为有效目标，从而能够避免杂波的干扰。摄像头原始图像数据在边缘计算单元中通过深度学习方法实现对目标类型的检测并获得其特征数据，如目标在图像坐标系中的位置、类型。路侧单元（RSU）采用LTE-V通信方式与网联车辆进行通信，从而可获得网联车辆的位置、速度等信息。

2 路侧多传感器融合算法

基于路侧多传感器融合的车辆信息检测算法框架如图3所示，其包含传感器的时间和空间同步、数据关联、状态估计几大部分。

2.1 时间和空间同步

要实现对毫米波雷达、摄像头、RSU检测的车辆信息融合，需要将不同传感器检测的目标进行时间同步和空间同步。由于现有路侧使用的网络摄像头和毫米波雷达不支持输出带GPS时间戳的目标信息，因此无法实现硬件时间同步，只能采用软同步的方式。本论文采取的时间同步方法下所示：在图3中融合预处理模块中创建不同传感器预处理的线程，任何一传感器线程一旦完成接收数据及空间同步，该线程就会立刻触发后续融合模块数据融合的过程。该方法避免了不同传感器的因采样周期不同而造成的时间不同步的问题。考虑到检测的车辆位置信息的最终用途，本文将毫米波雷达、摄像头、RSU检测的目标位置都统一到WGS-84坐标系中，对应空间同步实现方法如下所示。

毫米波雷达的坐标系如图4所示，毫米波雷达获取目标的坐标（ x _r ， y _r ），单位为m，其在WGS-84坐标中上对应点为（ lat ， lon ），单位为°。毫米波雷达坐标系与WGS-84坐标之间的转换关系见公式（1）：

毫米波雷达的标定本质是将毫米波雷达坐标系中 x ， y 坐标经过旋转、平移和缩放后变成WGS-84坐标系下的经度和纬度。通过采集同一网联车辆在毫米波雷达检测区域的多对毫米波雷达数据和WGS-84坐标系中数据用最小二乘法可以求解出公式（1）中的系数。

目前通过深度学习的方法可以将图像中车辆等目标提取出来从而获得车辆在图像坐标系中的坐标 ^[7] 。摄像头的标定是将摄像头检测的目标的像素坐标（ u ， v ）经过变换成为WGS-84坐标中的（ lat ， lon ）。本文同样用网联车辆实现摄像头的标定，网联车辆沿着在摄像头检测范围内的最左和最右侧车道行驶，在边缘计算单元中通过YOLO v3算法 ^[7] 检测获得网联车辆检测框底边中间点的图像坐标，同时通过RSU与网联车辆通信可以获得该点对应的GPS坐标，由图像坐标和GPS坐标组成一对标定数据对，其对应关系见公式（2）：

式中 H 矩阵中系数表示由图像坐标系映射至WGS-84坐标系对应的旋转和缩放关系； T 矩阵中系数表示由图像坐标系映射至WGS-84坐标系对应的平移关系。每条车道由近及远分别选取4对数据用于实现摄像头的标定，通过最小二乘法估计出 H ， T 的参数。

RSU获取网联车辆的位置坐标是WGS-84坐标中的经度和纬度，无须进行转换。通过上述方式将不同传感器检测的目标坐标转换为在同一空间坐标系中。获得不同传感器的检测目标的WGS-84坐标后通过UTM投影的正解公式 ^[8] 可以将WGS-84坐标转变为笛卡儿坐标（正东方为 x 轴，正北方为 y 轴）用于后续的数据关联和状态估计计算。

2.2 数据关联

数据关联是将传感器检测的多个目标与融合跟踪列表中的多个目标进行关联匹配起来。融合跟踪列表中是放置之前匹配成功和未匹配成功的目标的信息，其初始化是用任意传感器首次检测的目标信息进行初始化，后续是根据不同传感器匹配成功的目标和未匹配成功的目标的信息进行更新。本文考虑数据关联算法的计算量和实际使用需要，选用全局最近邻算法进行数据关联。全局最近邻算法（GNN）的流程如下：①根据下一节选取的目标的运行模型估计当前时刻跟踪列表中 N 个目标的最新位置信息；②计算当前传感器检测出 M 个目标需要跟融合跟踪列表中估计当前的 N 个目标的距离，从而构建一个 M × N 的距离代价矩阵，距离代价矩阵中的第 i （ i =1，2，…， M ）行第 j （ j =1，2，…， N ）列的值为传感器检测的第 i 个目标与跟踪列表中的第 j 个目标之间的距离，数据关联的目的是使总的距离或关联代价达到最小；③通过上述距离代价矩阵计算将匹配关联问题转变为二维的最优分配问题，使用匈牙利匹配算法可以获得传感器检测的目标与融合跟踪列表中的目标之间的最佳匹配关系 ^[9] 。当RSU检测的目标与融合跟踪列表中目标匹配成功，则融合跟踪列表中的目标车辆类型更新为网联车辆，否则该目标为非网联车辆。

2.3 状态估计

当传感器检测的目标与融合跟踪列表中的目标匹配成功后，需要对匹配成功的目标进行状态估计。本文选用卡尔曼滤波和扩展卡尔曼滤波对车辆的位置、速度进行估计，为实现位置、速度的估计需要选取合适的运动模型，目前常用的运动模型有常速度模型（CV）、常加速度模型（CA）、恒定速度和转速模型（CTRV）等模型 ^[10] 。考虑到估计目标状态所需的参数及模型的复杂程度，本文采用的运动模型为常速度模型，选取的状态变量为 ，对应的状态方程如下。

观测方程为

对毫米波雷达和RSU，其能够获得检测目标在笛卡儿坐标系中的 X ， Y 坐标及速度，因此选取的观测方程为

对应的观测量为 Z _k =[ x _k y _k v _k ] ^T 。由于观测方程为非线性方程，因此对毫米波雷达和RSU检测的目标采用EKF算法进行估计。对非线性观测方程进行泰勒公式展开得到雅克比矩阵为

对于毫米波雷达和RSU，对应测量噪声的协方差矩阵 R _j （ j =1，2）为

摄像头无法获得检测目标的速度，只能够获得检测目标在笛卡儿坐标系中的 X ， Y 坐标。因此选取的观测方程为

它对应的观测量为 Z _k =[ x _k y _k ] ^T 。摄像头检测的目标对应的系统状态方程和观测方程都为线性方程，因此对摄像头检测的目标采用KF算法进行估计。对于摄像头检测的目标的测量噪声的协方差矩阵 R ₃ 为

系统噪声的协方差矩阵 Q 为

式中， x _k ， y _k ， v _k 分别表示在 k 时刻目标在笛卡儿坐标系中的位置 X 坐标，位置 Y 坐标及目标的速度，单位分别为m，m，m/s； 分别表示目标的速度在笛卡儿坐标系的 X 轴和 Y 轴上的速度分量，单位为m/s；Δ t 表示前后两帧数据的时间差，单位为s； σ ² 为系统噪声的协方差， 分别表示对应传感器测量的目标位置在X轴方向、Y轴方向的协方差及测量目标速度的协方差（ j =1表示为毫米波雷达， j =2表示为RSU）。

经过路侧多传感器融合后获得目标的位置为笛卡儿坐标系中的 X ， Y 坐标，通过UTM投影的逆解公式 ^[8] 可以将笛卡儿坐标转变为WGS-84坐标中的经纬度。

3 实验结果

为了验证本文提出的路侧多传感器融合方法检测车辆信息的效果，在国家智能网联汽车（长沙）测试区部署一套验证系统，该系统由主要由一个摄像头、一个毫米波雷达、一个RSU、一台边缘计算单元、一台交换机、一台网联车辆和一台非网联车辆等组成。毫米波雷达、摄像头、RSU安装在路边的横杆上，交换机和边缘计算单元安装在路侧的抱杆机箱内。网联车辆和非网联车辆都位于上述传感器感知的区域，毫米波雷达的最远感知距离约为230m，摄像头能够检测的车辆的最远距离约为230m，RSU可以接收周围约800m范围内的网联车辆的OBU广播的信息。毫米波雷达检测目标的周期约为72ms，摄像头检测的数据经过边缘计算单元处理后的检测目标的周期约为150ms，RSU接收到网联车的OBU广播的信息的周期约为100ms。本系统中主要设备的规格型号见表1。本实验的路侧多传感器融合算法的参数见表2。

在整个实验过程中非网联车辆一直停在道路上，网联车辆从远处往靠近传感器所在位置的方向行驶。毫米波雷达检测的目标经过空间同步后的轨迹如图5a所示。因为毫米波雷达在实验中设置了毫米波雷达检测目标的速度阈值（15km/h），因此在毫米波雷达检测的有效目标中只有行驶过程中的车辆。图中箭头为车辆行驶的方向。摄像头检测的目标经过空间同步后的轨迹如图5b所示。由图5b可以看出摄像头能够检测出静止的车辆，但是其检测的运动车辆的轨迹并不连续，存在丢失数据的现象。由图5b左下角可看出通过摄像头检测出静止车辆的位置并不固定，存在抖动的现象。RSU接收到网联车辆的轨迹如图5c所示，由图5c中可以看出网联车辆的轨迹也存在断续的情况，这说明RSU在接收网联车辆OBU广播的数据也存在部分数据丢失的情况。将上述三种方法获得目标轨迹在笛卡儿坐标系中进行展示如图6所示，由图中可以看出对于网联车辆其从远处靠近传感器行驶的过程中其最初被RSU检测，然后其被毫米波雷达和RSU同时检测到，最后其被摄像头、毫米波雷达和RSU同时检测到；对于静止非网联车辆其只被摄像头检测到。

由图5d和图7可以看出融合后网联车辆行驶的轨迹更加平滑，不存在丢失数据的情况，不存在行驶过程中数据丢失的情况。这说明经过路侧多传感器融合能够综合多个传感器的优点从而获得行驶在道路上的车辆更完整的位置信息。网联车辆的速度变化平滑，不存在速度突变的情况，同时车辆类型全程都是保持为网联车辆类型。非网联车辆在实验过程中是处于静止状态，根据图8可以看出其轨迹一直保持在同一位置没有发生变化的情况，对比图5b和图6b摄像头检测的静止非网联车辆的位置并不是一直保持同一位置，存在抖动的现象，这是因为通过深度学习检测出非网联车辆的检测框存在小幅抖动的情况，但经过融合之后可以看到非网联车辆并未出现位置抖动的情况。非网联车辆的速度全程都为0km/h，因此可认定非网联车辆在实验过程一直处于静止状态，与实际情况相符；整个过程中车辆类型一直保持为非网联车的类型。由上述的实验可以看出通过路侧的多传感器融合后可准确地区分网联车辆和非网联车辆的车辆类型，同时可以获取其准确的位置、速度，这为后续在真实道路上的网联车辆和非网联车辆的混行研究奠定了基础。

4 结论

本文针对未来网联车辆渗透率不断提升的情形下，提出基于路侧毫米波雷达、摄像头和RSU的路侧多传感器融合方法实现对网联车辆和非网联车辆的信息检测。实验结果表明本文提出的方法主要包含以下优点：①本文提出的方法能够准确地区分行驶在道路上的网联车辆和非网联车辆，为真实道路上网联车辆与非网联车辆混行的研究奠定了基础；②本文提出的方法能够获得网联车辆和非网联车辆准确的位置、速度信息，解决了单一传感器检测的车辆位置和速度不完整的问题。

由于实验条件限制，本文实验选取的车辆数量较少，未来可以在以下几个方面进行更进一步的研究：①在公开道路上采集更多的网联车辆和非网联车辆的数据进行验证；②设计一套真值对比验证系统，验证本文提出的方法识别的网联车辆和非网联车辆信息的准确度。

参考文献

[1]DAI R J,LU Y R,DING C,et al.The Effect of Connected Vehicle Environment on Global Travel Efficiency and Its Optimal Penetration Rate[J].Journal of Advanced Transportation,2017:1-10.

[2]OMAR H,AMIR G,LI X P.Freeway Lane Management Approach in Mixed Traffic Environment with Connected Autonomous Vehicles[J].CoRR abs/1609.02946,2016:1-10.

[3]WAN N F,ARDALAN V,ANDRE L.Optimal speed advisory for connected vehicles in arterial roads and the impact on mixed traffic[J].Transportation Research Part C:Emerging Technologies,2016,69:548-563.

[4]BAI J,LI S,ZHANG H,et al.Robust Target Detection and Tracking Algorithm Based on Roadside Radar and Camera[J].Sensor,2021,21(4).

[5]FU Y J,TIAN D X DUAN X,et al.A Camera-Radar Fusion Method Based on Edge Computing[C]//IEEE International Conference on Edge Computing(EDGE),Beijing:IEEE,2020:9-14.

[6]SIMON J J,JEFFREY K U.A New Extension of the Kalman Filter to Nonlinear Systems[C]//Signal Processing,Sensor Fusion,and Target Recognition VI,Orlando:SPIE,1997:182-193.

[7]JOSEPH R,ALI F.YOLOv3:An Incremental Improvement[C]//CoRR 1804.02767,2018.

[8]VINCENTY T.Direct and inverse solutions of geodesics on the ellipsoid with application of nested equations[J].Surv Rev,1975,23(176):88-93.

[9]JAMES M.Algorithms for the assignment and transportation problems[J].Society for Industrial and Applied Mathematics,1957,5(1):32-38.

[10]ROBIN S,CHRISTIAN A,MARCUS O,et al.Empirical Evaluation of Vehicular Models for Ego Motion Estimation[C]//IEEE Intelligent Vehicles Symposium(IV),Germany:IEEE,2011:534-53. 0+nkigUUNw2Py+Tt5T08/g4ODNcBiaTd7K2l7aXmNod2DQ67YQDwyjiTRa7o2mbX