一种面向城市道路场景的雷达航迹排队管理方法

技术领域

本发明属于交通雷达技术领域，具体涉及一种面向城市道路场景的雷达航迹排队管理方法。

背景技术

随着机动车保有量的持续增长，城市道路路口交通拥堵现象越来越严重。车辆目标依据车道顺序排列的现象就是目标排队，从车道排队的第一个目标至排队的最后一个目标之间的距离称为目标排队长度。根据路口目标排队情况的实时反馈能够帮助交通部门及时调整红绿灯时长并能帮助车主避开交通拥堵路段。因此，需要一种能够最准确反映路口目标排队情况的算法。

目前，用于反映路口车辆目标排队情况的传感器主要有摄像头、地磁传感器、毫米波雷达、微波传感器等。其中基于摄像头视频图像信息的目标排队处理应用最为广泛，其具有可视范围广、价格低、能够回放等优点，能有效识别车辆类型。但是其缺点较为明显，光学摄像头一般对于光照、天气信息较为敏感，低可见度光照、雨雪、雾霾等天气条件下，摄像头对目标车辆的检测受限。而毫米波雷达具有全天时、全天候的优势，并且不受恶劣天气条件的影响，相较于摄像头能够更好地反映路口目标的排队情况。

毫米波雷达对目标跟踪的传统步骤为航迹预测、聚类、关联、滤波、航迹管理。当目标轨迹是简单的匀加速、匀转弯运动时，航迹较稳定不容易出现航迹中断、航迹ID切换的现象。但在城市复杂道路中，车辆的运动涉及停车、起步、变道等，沿用目标跟踪的传统方法会出现同一目标航迹ID切换，静止目标航迹消失的问题。因此，需要一种能够面向城市道路目标复杂运动的跟踪算法。

发明内容

本发明提出了一种面向城市道路的雷达航迹排队管理方法，以解决通过滤除静态杂波导致雷达无法检测静止物体的问题。其致力于解决在目标车辆停止时的目标排队问题，并对目标排队长度进行准确估计。

为实现上述目的，本发明的技术方案如下：

一种面向城市道路场景的雷达航迹排队管理方法，包括以下步骤：

步骤一，航迹匹配：包括ID匹配和区域匹配两部分，遍历当前帧的排队队列和跟踪航迹，将排队目标和跟踪航迹进行航迹匹配；

步骤二，航迹外推：对ID和区域均未匹配到的排队目标进行外推；

步骤三，排队队列更新：包括目标进入排队区域、目标在排队区域中变道、目标驶出排队区域三种情形，需要对排队队列更新。

所述的步骤一中ID匹配的具体方法为：车辆排队过程中在排队区域内的航迹不应中断新起，即ID切换现象。由于每条跟踪航迹和排队队列里的目标都有一个相应的ID号，如果排队目标相匹配的跟踪航迹一直未中断，那么排队目标和此航迹的ID一致。遍历当前航迹和排队目标的ID，如果一致则航迹匹配成功。

所述的步骤一中区域匹配的具体方法为：实际场景中排队的头车先启动后车跟着启动，所以在区域匹配过程中，只对车道的头车进行区域匹配。在具体的区域匹配判定中，选定跟踪航迹和排队目标的纵向距离差和跟踪航迹的航向角、横向速度作为判定参数。航向角和横向速度判定门限可防止排队目标与横穿错误航迹匹配。区域匹配时，将匹配到的输入航迹的ID号映射成区域匹配到的排队队列航迹的ID号，保证跟踪航迹ID号的唯一不变性。

所述步骤二中航迹外推的具体方法为：在航迹外推过程中，对头车和非头车排队目标有不同的处理。对于头车目标来说，其前方没有目标，当头车目标的速度低于速度阈值时，即目标将要停止，并且目标还未到达停车线时，对目标的纵向位置和速度更新按匀减速运动处理。当目标到达停止线后，将停车状态标志位Stop_Flag设置为1，目标位置保持，速度置0。

对于非头车的排队目标，需要判定其和前车之间的间距。当两车间距低于车距阈值dis_thresh时，认为两车相距较近，对目标位置状态不做更新，将停车状态标志Stop_Flag设置为1。当两车间距大于车距阈值dis_thresh时，并且目标仍处于运动状态，目标做跟随运动，对目标的纵向位置和纵向速度进行更新。

所述步骤三中排队队列更新的具体方法为：对于匹配成功的航迹，判断是否有变道现象。由于排队队列中的目标是按照车道号、车序号依次排列的，因此如果存在变道现象，需要对位于变道目标后的原车道排队目标序号依次前移一个，对于变道目标插入位置处的新车道排队目标序号依次后移一个，更新排队队列信息。

对于目标进入排队区域，在排队刚开始时初始化排队区域为雷达朝向方向上停车线至最大停车纵向距离maxParkY处，当车道内最后一辆排队目标位置停留在maxParkY附近时，对maxParkY进行扩展。

目标移出排队队列主要是为了删除长时间停留的目标和假的停车队列节点，以及目标车辆驶出排队区域。例如当雷达覆盖范围内出现大车，例如大型公交车，其尺寸较大，点迹分布比较分散，很容易出现大车分裂现象，即一个目标分裂出两条航迹。分裂目标在排队区域内有可能长时间停留，因此需要对删除排队队列中长时间停留的虚假目标。

遍历排队队列，当排队目标没有关联航迹并且存在时间超过一定门限，从排队队列删除该节点。反之对该目标与输入航迹进行关联，累计关联上的航迹个数，当超过关联个数门限Asso_Thresh时，判定其为虚假目标，删除虚假目标节点。对于已经驶出排队区域的目标，将其从排队队列中删除，更新排队队列。

对于排队长度的估计，还需要结合当前车道头车和尾车车型进行估计。目前实验数据中有小轿车、公交车等路面常见车型，不同车型的车长不同，计算排队长度。

有益效果：本发明与现有技术相比，其显著优点是：

(1)本发明提出的排队队列航迹与原始航迹的匹配方法，能有效解决交通排队区车辆低速漏检导致的航迹ID切换的问题，提升车辆航迹的稳定性。

(2)本发明提出的排队队列航迹外推的方法，对于航迹已经消失的排队目标依据运动学规律进行航迹位置外推更新，能有效解决车辆停止时无法检测使得排队目标消失的问题。

(3)本发明提出的排队队列更新的方法，对路口排队区动态更新排队队列能更准确直观反映路口目标排队情况。

附图说明

图1为本发明的方法流程示意图；

图2为本发明的目标排队队列航迹信息更新图；

图3为本发明的方法中目标排队情形的整体示意图。

具体实施方式

附图仅用于示例性说明，不能理解为对本专利的限制；

为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际的尺寸；

下面结合附图并举实施例，对本发明进行详细描述。

一种面向城市道路场景的雷达航迹排队管理方法，流程图如图1所示，该方法包括以下步骤：

步骤一，航迹匹配：包括ID匹配和区域匹配两部分，遍历当前帧排队队列和输入航迹，将排队目标和输入航迹进行航迹匹配；

步骤二，航迹外推：如图2所示，对ID和区域均未匹配到的排队目标进行外推；

步骤三，排队队列更新：如图3所示，包括目标进入排队区域、目标在排队区域中变道、目标驶出排队区域三种情形，需要对排队队列更新。

所述的步骤一中ID匹配的具体方法为：车辆排队过程中在排队区域内的航迹不应中断新起，即ID切换现象。由于每条跟踪航迹和排队队列里的目标都有一个相应的ID号，如果排队目标相匹配的跟踪航迹一直未中断，那么排队目标和此航迹的ID一致。遍历当前航迹和排队目标的ID，如果一致则航迹匹配成功。

所述的步骤一中区域匹配的具体方法为：如果ID匹配未成功，那么就进行区域匹配。在区域匹配过程中，只对车道的头车进行区域匹配。在具体的区域匹配判定中，选定跟踪航迹和排队目标的纵向距离差和跟踪航迹的航向角、横向速度作为判定参数。航向角和横向速度判定门限可防止排队目标与横穿错误航迹匹配。

如果在排队目标周围存在新起航迹，恰好能和当前目标区域匹配上，排队目标开辟一个新变量名为区域ID号，赋值为新起航迹的ID，新起航迹的ID就和排队目标的区域ID形成映射关系。

对于航迹匹配成功的排队目标，将排队队列中目标状态更新为关联到的输入航迹状态，排队队列中当前航迹的关联标志assoFlag置1，对于被匹配到的输入航迹的匹配标志parkAndDriveFlag也置为1，防止一条航迹被多次匹配。如果排队目标不存在匹配航迹，那么就将关联标志assoFlag置0，在步骤二排队目标航迹外推部分对其进行航迹状态外推。

所述步骤二中航迹外推的具体方法为：当目标速度低于一定值后，目标行驶缓慢，很容易被检测成静止目标，导致航迹中断，从而排队目标与输入航迹匹配不上。因此需要对缓慢运动目标进行航迹外推，更新其状态信息。

在航迹外推过程中，对头车和非头车排队目标有不同的处理。对于头车目标来说，其前方没有目标，当头车目标的速度低于速度阈值时，即目标将要停止，并且目标还未到达停车线时，对目标的纵向位置更新,表达式如下：

式中，y

当目标到达停止线后，将停车状态标志位Stop_Flag设置为1，目标停止。当头车目标的速度大于速度阈值V_thresh时，对目标的纵向位置和纵向速度进行更新，纵向速度更新表达式如下：

v

式中，v

对于非头车的排队目标，需要判定其和前车之间的间距。当两车间距低于车距阈值dis_thresh时，认为两车相距较近，对目标位置状态不做更新，将停车状态标志位Stop_Flag设置为1。当两车间距大于车距阈值dis_thresh时，并且目标处于非停车状态，目标做跟随运动，对目标的纵向位置和纵向速度进行更新。

所述步骤三中排队队列更新的具体方法为：对于匹配成功的航迹，判断是否有变道现象。由于排队队列中的目标是按照车道号、车序号依次排列的，因此如果存在变道现象，需要对位于变道目标后的原车道排队目标序号依次前移一个，对于变道目标插入位置处的新车道排队目标序号依次后移一个，更新排队队列信息。

对于目标进入排队区域，在排队刚开始时初始化排队区域为雷达朝向方向上停车线至最大停车纵向距离maxParkY处，当车道内最后一辆排队目标位置停留在maxParkY附近时，对最大停车纵向距离进行扩展，表达式如下：

maxParkY＝maxParkY+ΔY (3)

式中，ΔY为每次区域扩展的变化量。将满足区域判定和满足来向目标条件的目标依据车道信息存入到排队队列中，对排队队列进行更新。

目标移出排队队列主要是为了删除长时间停留的目标和假的停车队列节点，以及目标车辆驶出排队区域。例如当雷达覆盖范围内出现大车，例如大型公交车，其尺寸较大，点迹分布比较分散，很容易出现大车分裂现象，即一个目标分裂出两条航迹。分裂目标在排队区域内有可能长时间停留，因此需要对删除排队队列中长时间停留的虚假目标。

遍历排队队列，当排队目标没有关联航迹并且存在时间超过一定门限，从排队队列删除该节点。反之对该目标与输入航迹进行关联，累计关联上的航迹个数，当超过关联个数门限Asso_Thresh时，判定其为虚假目标，删除虚假目标节点。对于已经驶出排队区域的目标，将其从排队队列中删除，更新排队队列。

对于排队长度的估计，还需要结合当前车道头车和尾车车型进行估计。目前实验数据中有小轿车、公交车等路面常见车型，不同车型的车长不同，计算排队长度的表达式如下：

式中，y_curr为当前目标纵向距离，y_first为头车目标纵向距离，len_first为头车车长，len_curr为当前车车长。

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。