1.本发明属于计算机和自动化技术，主要涉及到目标跟踪技术领域，具体涉及一种路侧单元协同的目标跟踪系统及方法。
背景技术：2.目标跟踪技术对智能车辆的决策控制与路径规划发挥着重要作用，是智能车辆安全预警及自动驾驶的技术基础。
3.中国专利申请：高速公路多雷达跨区域组网多目标跟踪方法(申请号：202011481467.4)，公开了一种通过管控中心对高速公路全路段路侧雷达数据进行坐标转换和数据关联，从而生成该次跟踪信息，该方法只是突破了单台雷达检测范围的限制，将雷达的检测距离无限延伸，使目标能在全路段上跟踪目标，但各雷达之间并未利用相邻雷达的协同观测来进行跟踪。中国专利申请：一种基于改进联合概率数据关联的分布式目标跟踪方法(申请号：cn201610821318.5)，公开了一种针对联合概率数据关联进行改进的目标跟踪方法，其对概率数据关联方法计算各目标跟踪门内的回波关联概率进行修正，并计算相应的状态估计，然后把各自传感器的状态估计进行空间关联，最后融合同一目标的状态估计得到最终的目标状态估计值，但是该方法同样未能在分布式的相邻传感器之间进行协同。
4.本发明针对上述问题，通过相邻rsu之间的协同来共享所跟踪的目标轨迹，实现对道路目标持续稳定的跟踪，提高道路目标跟踪精度，增强智能车辆的感知能力与安全性。
技术实现要素：5.本发明旨在解决以上现有技术的问题，提出了一种路侧单元协同的目标跟踪系统及方法来减少车路协同系统中对道路目标跨rsu进行跟踪时新目标的跟踪起始判断，以得到稳定、连续的跟踪轨迹，从而有效提高车路协同系统中路侧传感器目标跟踪的性能。本发明的技术方案如下：
6.一种路侧单元协同的目标跟踪系统，其包括：感知模块、通信模块、融合跟踪模块。所述感知模块包括激光雷达、毫米波雷达、摄像头在内的感知硬件，检测rsu周边一定范围内的行人、车辆等道路目标，得到目标量测信息；所述融合跟踪模块对感知模块提供的道路目标量测数据进行坐标转换、数据关联及融合滤波，得到道路目标的准确运动状态信息；通信模块用于向通信范围内的车辆发布道路目标信息，以及在相邻路侧单元之间交换目标轨迹信息。
7.进一步的，在每一个rsu上，所述感知模块各传感器及融合跟踪模块所采用的坐标系为固定在rsu上的局部坐标系，通信模块发送目标轨迹时的坐标系为地球坐标系，包括但不限于wgs-84坐标系。
8.进一步的，在每一个rsu上，所述感知模块检测行人车辆等道路目标，得到量测集
9.进一步的，在每一个rsu上，所述通信模块在目标状态广播时，需要将本地局部坐标系下的目标轨迹转化到地球坐标系，再向相邻rsu及车辆广播，广播的信息包括：本rsu的id以及在地球坐标系下的坐标；每一个目标的id，目标运动状态以及跟踪滤波误差协方差矩阵。
10.进一步的，在每一个rsu上，所述通信模块在接收相邻rsu发送的轨迹时，将接收的目标运动状态转化到本地局部坐标系下，构造本rsu局部坐标系下的潜在轨迹集合t
p
；
11.进一步的，所述融合跟踪模块在本rsu的局部坐标系下，建立目标运动状态模型与量测模型来描述目标运动：
12.x
k+1
＝fkxk+γkωk,k∈n
13.zk＝hkxk+υk,k∈n
14.其中表示k时刻目标的x位置、x速度、y位置、y速度、宽、高，zk＝[x,y,w,h]
t
表示k时刻目标的量测，n表示自然数集，fk为状态转移矩阵，γk为噪声矩阵，hk为k时刻目标量测矩阵，ωk和vk分别为相互独立的过程噪声和量测噪声。
[0015]
进一步的，所述融合跟踪模块将量测集与目标轨迹关联，对量测集zk与本地已有目标轨迹tc及潜在目标轨迹t
p
进行关联，为关联成功的目标分配对应的量测；对关联成功的目标轨迹集合中的每一目标，对关联后的目标状态进行跟踪滤波，得到k时刻目标运动状态的最优估计与滤波误差的协方差矩阵。跟踪滤波方法包括但不限于卡尔曼滤波。
[0016]
进一步的，所述融合跟踪模块量测集与目标轨迹关联方法，包括以下步骤：
[0017]
(1)目标运动状态预测：对已有轨迹集合tc中的每一目标根据目标运动状态方程及前一时刻目标的状态估计与协方差矩阵分别计算目标状态的一步预测值及一步预测误差的协方差矩阵
[0018]
(2)量测集zk与已有轨迹集合tc中目标t
ic
的一步预测值进行关联，将关联成功的量测分配给对应目标t
ic
，未关联成功的量测记为
[0019]
(3)潜在目标运动状态预测：对潜在轨迹集合t
p
中的每一目标t
ip
，根据目标运动状态方程及前一时刻目标的状态估计与协方差矩阵分别计算目标状态的一步预测值及一步预测误差的协方差矩阵
[0020]
(4)量测集z
′k与潜在轨迹集合t
p
中的每一目标的一步预测值进行关联，将关联成功的量测分配给对应目标未关联成功的量测记为
[0021]
(5)对量测集z
″k，通过跟踪起始方法，提取新目标轨迹。跟踪起始方法包括但不限于基于hough变换的航迹起始方法。
[0022]
进一步的，所述融合跟踪模块对关联成功的目标轨迹以及潜在目标，维持其id不变，对通过跟踪起始方法提取的新目标，由rsu赋予目标id。
[0023]
进一步的，目标id的构成包括两部分，rsu的id及该rsu提取的新目标的序号，rsu每提取一个新目标，该序号加一。
[0024]
进一步的，所述量测集与已有轨迹及潜在轨迹的数据关联方法，包括但不限于最近邻方法。
[0025]
一种路侧单元协同的目标跟踪方法，其包括以下步骤：
[0026]
(1)在路侧部署安装若干rsu，所述rsu具有感知、通信、计算功能，并分别具有自身的id；所述待检测路段上安装的所有rsu中任意相邻两台rsu的检测区域部分重叠；
[0027]
(2)为每个rsu分配唯一id，测量并记录rsu在地球坐标系下的坐标值；
[0028]
(3)rsu接收相邻rsu广播发送的信息；
[0029]
(4)rsu实时检测行人、车辆等道路目标，与本地目标轨迹、相邻rsu广播发送的目标轨迹进行关联、滤波，更新已有目标状态，进行持续跟踪，对关联失败的量测则通过跟踪起始提取新目标；
[0030]
(5)rsu把持续跟踪目标及新目标的信息转化到地球坐标系，并向相邻rsu及通信范围内的道路车辆广播。
[0031]
一种计算机可读存储介质，其该计算机可读存储介质上存储有计算机程序，该计算机程序被处理器执行时实现如任一项所述的路侧单元协同的目标跟踪方法。
[0032]
本发明的优点及有益效果如下：
[0033]
本发明提出一种路侧单元协同的目标跟踪系统及方法，在该方法中相邻的rsu之间通过通信模块共享目标轨迹信息，作为本rsu的潜在目标轨迹。当目标从相邻rsu传感器的检测范围进入本rsu检测范围时，本rsu的传感器量测首先与本地目标轨迹进行关联，未关联上的量测继续与潜在目标轨迹关联，剩余的才作为未关联量测进行跟踪起始。如果rsu之间没有协同，则来自相邻rsu的潜在目标轨迹信息将得不到有效利用，来自这些目标的量测将被视为新目标参与跟踪起始，一方面跟踪起始需要多个时刻的量测才能提取出新目标，从而造成跟踪延迟，另一方面新目标的跟踪起始误差与实际总存在差异，滤波器需要一定的时间才能收敛到合适的精度，从而不能实现目标的连续平稳跟踪，影响目标跟踪滤波器收敛速度及跟踪精度。本专利通过相邻路侧单元的协同，利用了相邻rsu的目标轨迹信息来与本rsu量测进行关联，实现道路目标的连续稳定跟踪，提高道路目标跟踪精度，增强自动驾驶或辅助驾驶系统的安全性。
附图说明
[0034]
图1是本发明提供优选实施例一种路侧单元协同的目标跟踪系统结构图
[0035]
图2是本发明一种路侧单元协同的目标跟踪方法流程图
[0036]
图3是本发明一种路侧单元协同的目标跟踪系统应用场景图
[0037]
图4是本发明一种路侧单元协同的目标跟踪方法效果图
具体实施方式
[0038]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、详细地描述。所描述的实施例仅仅是本发明的一部分实施例。
[0039]
本发明解决上述技术问题的技术方案是：
[0040]
如图1所示为本发明一种路侧单元协同的目标跟踪系统结构图，rsu系统由感知模块、通信模块、融合跟踪模块三部分组成。
[0041]
其中，感知模块包括激光雷达、毫米波雷达、摄像头在内的感知硬件，检测rsu周边一定范围内的行人、车辆等道路目标，得到目标量测信息；
[0042]
融合跟踪模块根据感知模块提供的道路目标量测数据及通信模块接收到的相邻路侧单元发布的目标轨迹信息进行数据关联、跟踪滤波以及新目标提取，得到道路目标的准确运动状态信息；
[0043]
通信模块用于向通信范围内的车辆发布道路目标信息，以及在相邻路侧单元之间交换目标轨迹信息。
[0044]
如图2所示为本发明一种路侧单元协同的目标跟踪方法流程图，包含如下步骤：
[0045]
(1)传感器目标检测：在时刻k，通过感知模块检测目标，得到量测集(1)传感器目标检测：在时刻k，通过感知模块检测目标，得到量测集
[0046]
(2)传感器量测与已有目标轨迹关联：首先进行已有目标运动状态预测，对已有轨迹集合tc中的每一目标t
ic
，根据目标运动状态方程及前一时刻目标的状态估计与协方差矩阵分别计算目标状态的一步预测值及一步预测误差的协方差矩阵量测集zk与已有轨迹集合tc中目标t
ic
的一步预测值进行关联，将关联成功的量测分配给对应目标t
ic
，未关联成功的量测记为
[0047]
(3)传感器量测与潜在轨迹关联：首先接收来自相邻rsu的潜在轨迹，构造本rsu局部坐标系下的潜在轨迹集合t
p
；再进行潜在目标运动状态预测，对潜在轨迹集合t
p
中的每一目标根据目标运动状态方程及前一时刻目标的状态估计与协方差矩阵分别计算目标状态的一步预测值及一步预测误差的协方差矩阵最后量测集zk′
与潜在轨迹集合t
p
中的每一目标的一步预测值进行关联，将关联成功的量测分配给对应目标未关联成功的量测记为
[0048]
(4)跟踪起始提取新目标：对量测集z
″k，通过跟踪起始方法，提取新目标轨迹；
[0049]
(5)目标状态滤波：对关联成功的目标轨迹集合中的每一目标，对关联后的目标状态进行跟踪滤波，得到k时刻目标运动状态的最优估计与滤波误差的协方差矩阵；
[0050]
(6)目标状态广播：将更新后的目标运动状态的最优估计转化到地球坐标系，与滤波误差的协方差矩阵一起，通过通信模块向相邻rsu及车辆广播。
[0051]
如图3是本发明一种路侧单元协同的目标跟踪系统应用场景图。该场景中rsu1检测到cv1并进行持续稳定跟踪，rsu2检测到cv2并进行持续稳定跟踪。在k时刻，cv2首次被rsu1检测到。当rsu1及rsu2无协同时，rsu1上没有关于cv2的轨迹信息，无法对cv2进行关联、滤波跟踪，只能将cv2的量测用于跟踪起始，在若干个周期后才能将cv2作为新目标提取出来，跟踪过程出现间断。在本发明中，当rsu1及rsu2有协同时，对cv2的跟踪可以从rsu2迁移到rsu1上继续进行，实现持续稳定的跟踪，改善车路协同场景下的目标跟踪性能。
[0052]
如图4所示为本发明一种路侧单元协同的目标跟踪方法效果图。在rsu之间有协同时，可以从相邻rsu获得目标的跟踪误差协方差pk，而rsu之间无协同时，不能准确获得pk，新目标只能采用预先给定的滤波误差协方差初值p0。图a为目标低速运动时，rsu有协同和无协同情况下的跟踪误差，其中实线为rsu之间有协同情况下的连续跟踪误差，虚线为p0《pk情况下新目标跟踪起始的跟踪误差，点划线为p0》pk情况下新目标跟踪起始的跟踪误差。显然rsu之间有协同情况下的跟踪精度远远高于无协同的情况。图b为目标高速运动时的跟踪效果，rsu之间有协同情况下的跟踪精度同样远远高于无协同的情况。
[0053]
上述实施例阐明的系统、装置、模块或单元，具体可以由计算机芯片或实体实现，或者由具有某种功能的产品来实现。一种典型的实现设备为计算机。具体的，计算机例如可以为个人计算机、膝上型计算机、蜂窝电话、相机电话、智能电话、个人数字助理、媒体播放器、导航设备、电子邮件设备、游戏控制台、平板计算机、可穿戴设备或者这些设备中的任何设备的组合。
[0054]
计算机可读介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。计算机的存储介质的例子包括，但不限于相变内存(pram)、静态随机存取存储器(sram)、动态随机存取存储器(dram)、其他类型的随机存取存储器(ram)、只读存储器(rom)、电可擦除可编程只读存储器(eeprom)、快闪记忆体或其他内存技术、只读光盘只读存储器(cd-rom)、数字多功能光盘(dvd)或其他光学存储、磁盒式磁带，磁带磁磁盘存储或其他磁性存储设备或任何其他非传输介质，可用于存储可以被计算设备访问的信息。按照本文中的界定，计算机可读介质不包括暂存电脑可读媒体(transitory media)，如调制的数据信号和载波。
[0055]
还需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、商品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、商品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个
……”
限定的要素，并不排除在包括所述要素的过程、方法、商品或者设备中还存在另外的相同要素。
[0056]
以上这些实施例应理解为仅用于说明本发明而不用于限制本发明的保护范围。在阅读了本发明的记载的内容之后，技术人员可以对本发明作各种改动或修改，这些等效变化和修饰同样落入本发明权利要求所限定的范围。
技术特征：1.一种路侧单元协同的目标跟踪系统，其特征在于，包括：感知模块、通信模块、融合跟踪模块，所述感知模块包括激光雷达、毫米波雷达、摄像头在内的感知硬件，检测rsu周边一定范围内的行人、车辆在内的道路目标，得到目标量测信息；所述融合跟踪模块根据感知模块提供的道路目标量测数据及通信模块接收到的相邻路侧单元发布的目标轨迹信息进行数据关联、跟踪滤波以及新目标提取，得到道路目标的准确运动状态信息；所述通信模块用于向通信范围内的车辆发布道路目标信息，以及在相邻路侧单元之间交换目标轨迹信息。2.一种基于权利要求1所述系统的目标跟踪方法，其特征在于，对每一个rsu，包括以下步骤：2.1系统建模：在本rsu上建立局部坐标系，并在局部坐标系下建立目标运动状态模型与量测模型：x
k+1
＝f
k
x
k
+γ
k
ω
k
,k∈nz
k
＝h
k
x
k
+υ
k
,k∈n其中表示k时刻目标的x位置、x速度、y位置、y速度、宽、高，z
k
＝[x,y,w,h]
t
表示k时刻目标的量测，n表示自然数集，f
k
为状态转移矩阵，γ
k
为噪声矩阵，h
k
为k时刻目标量测矩阵，ω
k
和v
k
分别为相互独立的过程噪声和量测噪声；2.2潜在轨迹构造：在时刻k，接收相邻rsu发送的轨迹，构造本rsu局部坐标系下的潜在轨迹集合t
p
；2.3传感器目标检测：在时刻k，通过感知模块检测目标，得到量测集2.3传感器目标检测：在时刻k，通过感知模块检测目标，得到量测集2.4量测集与目标轨迹关联：对量测集z
k
与本地已有目标轨迹t
c
及潜在目标轨迹t
p
进行关联，为关联成功的目标分配对应的量测；2.5目标状态滤波：对关联成功的目标轨迹集合中的每一目标，对关联后的目标状态进行跟踪滤波，得到k时刻目标运动状态的最优估计与滤波误差的协方差矩阵；2.6目标状态广播：将更新后的目标运动状态的最优估计转化到地球坐标系，与滤波误差的协方差矩阵一起，通过通信模块向相邻rsu及车辆广播。3.根据权利要求2所述的目标跟踪方法，其特征在于，所述量测集与目标轨迹关联方法，包括以下步骤：3.1目标运动状态预测：对已有轨迹集合t
c
中的每一目标t
ic
，根据步骤2.1所述的目标运动状态方程及前一时刻目标的状态估计与协方差矩阵分别计算目标状态的一步预测值及一步预测误差的协方差矩阵3.2量测集z
k
与已有轨迹集合t
c
中目标t
ic
的一步预测值进行关联，将关联成功的量测分配给对应目标t
ic
，未关联成功的量测记为3.3潜在目标运动状态预测：对潜在轨迹集合t
p
中的每一目标根据步骤2.1所述的目标运动状态方程及前一时刻目标的状态估计与协方差矩阵分别计算目标状态的一步预测值及一步预测误差的协方差矩阵3.4量测集z
′
k
与潜在轨迹集合t
p
中的每一目标的一步预测值进行关联，将关联
成功的量测分配给对应目标未关联成功的量测记为3.5对量测集z
″
k
，通过跟踪起始方法，提取新目标轨迹。4.一种计算机可读存储介质，其特征在于，该计算机可读存储介质上存储有计算机程序，该计算机程序被处理器执行时实现如权利要求2～3任一项所述的路侧单元协同的目标跟踪方法。
技术总结本发明请求保护一种路侧单元协同的目标跟踪系统、方法及存储介质。该系统由感知模块、融合跟踪模块、通信模块组成。其中，感知模块包括多种环境感知传感器，用于检测路侧单元周边一定范围内的行人、车辆等目标；融合跟踪模块根据感知模块提供的道路目标量测数据及通信模块接收到的相邻路侧单元发布的目标轨迹信息进行数据关联、跟踪滤波以及新目标提取，得到道路目标的准确运动状态信息；通信模块用于向通信范围内的车辆发布道路目标信息，以及在相邻路侧单元之间交换目标轨迹信息。本专利通过相邻路侧单元的协同来实现道路目标的连续稳定跟踪，提高道路目标跟踪精度，增强自动驾驶或辅助驾驶系统的安全性。驶或辅助驾驶系统的安全性。驶或辅助驾驶系统的安全性。
技术研发人员：岑明 张毅 杜文峰 黎城 曾素华
受保护的技术使用者：重庆邮电大学
技术研发日：2022.02.24
技术公布日：2022/7/5
