CN112528793A - 一种车辆的障碍物检测框抖动消除方法及装置 - Google Patents

Abstract

本发明提供一种车辆的障碍物检测框抖动消除方法及装置，该方法包括：获取当前帧航向角和上一帧第二航向转换角，将当前帧航向角转换到0至2π范围；障碍物的当前目标帧航向角和上一目标帧航向角的夹角大于90度，用第一航向转换角更新第二航向转换角；障碍物运动方向与车辆运动方向一致，将第一航向转换角和第三航向转换角转换到‑π至π的范围；基于一阶惯性滤波算法结合第四航向转换角和第五航向转换角的加权平均值更新第四航向转换角；将更新后的第四航向转换角转换到0至2π范围。在本方案中，运用一阶惯性滤波算法更新当前帧航向角，减少了航向角的小范围抖动，解决了自动驾驶车辆过程中车辆检测框抖动引发的车辆驾驶安全性和舒适性的问题。

Description

一种车辆的障碍物检测框抖动消除方法及装置

技术领域

本发明涉及感知技术领域，尤其涉及一种车辆的障碍物检测框抖动消除方法及装置。

背景技术

自动驾驶车辆是汽车电子、智能控制以及互联网等技术发展融合的产物，其原理为自动驾驶系统利用感知技术与定位系统，获取车辆自身方位以及外界环境信息，经过计算系统分析信息、做出决策，控制执行系统实现车辆加速、减速或转向，从而在无需驾驶员介入的情况下，完成自动行驶。自动驾驶汽车在行驶过程中通过感知系统中的传感器感知周围环境进行自动驾驶。图像信息是自动驾驶车辆进行环境感知最重要的信息之一。

在现有技术中，视觉传感器通过获取原始图像信息，经过特征提取算法给出自动驾驶车辆进行环境感知的相关信息，包括：车辆、行人检测框的位置、速度、尺寸大小、朝向、类别等。

但是传感器上的相机镜头发生的畸变会从视角中心到边缘逐渐递增，导致在相机视角边界处的物体的检测效果会严重下降。此外，车辆运行过程中会引起振动，导致相机图像中车辆障碍物检测框的朝向会出现抖动现象，而相邻车道的车辆检测框抖动会引起车辆急刹、逼停等问题，因此在自动驾驶车辆过程中，车辆检测框抖动会带来车辆驾驶安全性和舒适性的问题。

发明内容

有鉴于此，本发明实施例提供一种应用于车辆的障碍物检测框抖动消除方法及装置，以解决现有技术中自动驾驶车辆过程中车辆检测框抖动引发的车辆驾驶安全性和舒适性的问题。

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

本发明实施例第一方面公开了一种车辆的障碍物检测框抖动消除方法，所述方法包括：

获取车辆的障碍物检测框的当前帧航向角和存储的上一帧的第二航向转换角，将所述当前帧航向角转换至0至2π范围内，得到转换后的当前帧的第一航向转换角，其中，所述当前帧不是第一帧；

若所述障碍物检测框中出现障碍物且所述障碍物处于运动状态，判断所述障碍物的当前目标帧航向角和所述障碍物的上一目标帧航向角的夹角是否大于90度；

若所述夹角大于90度，利用所述第一航向转换角更新第二航向转换角，得到第三航向转换角；

基于所述当前目标帧航向角判断所述障碍物的运动方向是否与所述车辆的运动方向是否一致；

如果方向一致，将所述第一航向转换角和所述第三航向转换角转换到-π至π的范围内，得到当前帧的第四航向转换角和上一帧的第五航向转换角；

基于一阶惯性滤波算法结合所述第四航向转换角和所述第五航向转换角的加权平均值更新所述第四航向转换角；

将更新后的第四航向转换角转换到0至2π范围内，得到第六航向转换角并存储，使所述第六航向转换角作为更新下一帧航向角时的上一帧的第二航向转换角使用。

可选的，所述方法还包括：

若获取到的当前帧为第一帧，将所述当前帧的航向角作为上一帧的航向角；

将所述当前帧的航向角和所述上一帧的航向角转换到0至2π范围内，得到转换后的当前帧的第一航向转换角和上一帧的第二航向转换角。

可选的，将所述当前帧航向角转换至0至2π范围内，得到转换后的当前帧的第一航向转换角，包括：

判断所述当前帧航向角是否大于0且小于2π；

若当前帧航向角大于2π，将所述当前帧航向角减去2nπ，返回继续判断，直至当前航向角处于0至2π范围内，得到转换后的当前帧的第一航向转换角，其中，n的取值为大于等于1的正整数；

若当前帧航向角小于0，将所述当前帧航向角加上2nπ，返回继续判断，直至当前航向角处于0至2π范围内，得到转换后的当前帧的第一航向转换角。

可选的，若所述夹角大于90度，利用所述第一航向转换角更新第二航向转换角，得到第三航向转换角，包括：

若所述夹角大于90度，将所述第一航向转换角直接赋值给所述第二航向转换角，得到第三航向转换角。

可选的，所述基于所述当前目标帧航向角判断所述障碍物的运动方向是否与所述车辆的运动方向是否一致，包括：

判断第一航向转换角与所述当前目标帧航向角的夹角是否小于设定的阈值角度；

若所述夹角小于设定的阈值角度，确定所述障碍物的运动方向与所述车辆的运动方向一致；

若所述夹角大于等于设定的阈值角度，确定所述障碍物的运动方向与所述车辆的运动方向不一致。

可选的，所述如果方向一致，将所述第一航向转换角和所述第三航向转换角转换到-π至π的范围内，得到当前帧的第四航向转换角和上一帧的第五航向转换角，包括：

所述如果方向一致，判断所述第一航向转换角和所述第三航向转换角的角度是否大于-π且小于π；

若所述第一航向转换角和所述第三航向转换角大于-π且小于π，将所述第一航向转换角作为当前帧的第四航向转换角，将所述第三航向转换角作为上一帧的第五航向转换角；

若所述第一航向转换角和所述第三航向转换角大于π，将所述第一航向转换角和所述第三航向转换角分别减去2π，返回继续判断，直至所述第一航向转换角和所述第三航向转换角处于-π至π范围内，得到当前帧的第四航向转换角和上一帧的第五航向转换角。

可选的，所述基于一阶惯性滤波算法结合所述第四航向转换角和所述第五航向转换角的加权平均值更新所述第四航向转换角，包括：

根据公式Heading＝αHeading_pre+(1-α)Heading，以及所述第四航向转换角和所述第五航向转换角的加权平均值对所述第四航向转换角进行更新，其中，Heading为所述第四航向转换角，Heading_pre为所述第五航向转换角，α为滤波系数控制历史帧的权重，0<α<1。

本发明实施例第二方面公开了一种车辆的障碍物检测框抖动消除装置，所述装置包括：

第一转换模块，用于获取车辆的障碍物检测框的当前帧航向角和存储的上一帧的第二航向转换角，将所述当前帧航向角转换至0至2π范围内，得到转换后的当前帧的第一航向转换角，其中，所述当前帧不是第一帧；

第一更新模块，用于若所述障碍物检测框中出现障碍物且所述障碍物处于运动状态，判断所述障碍物的当前目标帧航向角和所述障碍物的上一目标帧航向角的夹角是否大于90度；若所述夹角大于90度，利用所述第一航向转换角更新第二航向转换角，得到第三航向转换角；

第二转换模块，用于基于所述当前目标帧航向角判断所述障碍物的运动方向是否与所述车辆的运动方向是否一致；如果方向一致，将所述第一航向转换角和所述第三航向转换角的角度转换到-π至π的范围内，得到当前帧的第四航向转换角和上一帧的第五航向转换角；

第二更新模块，用于基于一阶惯性滤波算法结合所述第四航向转换角和所述第五航向转换角的加权平均值更新所述第四航向转换角；

第三转换模块，用于将更新后的第四航向转换角转换到0至2π范围内，得到第六航向转换角并存储，使所述第六航向转换角作为更新下一帧航向角时的上一帧的第二航向转换角使用。

可选的，所述装置还包括：

检测单元，用于检测所述当前帧是否是第一帧，若获取到的当前帧为第一帧，将所述当前帧的航向角作为上一帧的航向角；

第一转换模块，还用于将所述当前帧的航向角和所述上一帧的航向角转换到0至2π范围内，得到转换后的当前帧的第一航向转换角和上一帧的第二航向转换角。

可选的，所述第二更新模块，具体用于：

根据公式Heading＝αHeading_pre+(1-α)Heading进行计算，得到所述第四航向转换角和所述第五航向转换角的加权平均值，其中，Heading为所述第四航向转换角，Heading_pre为所述第五航向转换角，α为滤波系数控制历史帧的权重，0<α<1；利用所述加权平均值更新所述第四航向转换角。

基于上述本发明实施例提供的一种车辆的障碍物检测框抖动消除方法及装置，该方法包括：获取车辆的障碍物检测框的当前帧航向角和存储的上一帧的第二航向转换角，将当前帧航向角转换至0至2π范围内，得到转换后的当前帧的第一航向转换角，其中，当前帧不是第一帧；若障碍物检测框中出现障碍物且障碍物处于运动状态，判断障碍物的当前目标帧航向角和障碍物的上一目标帧航向角的夹角是否大于90度；若夹角大于90度，利用第一航向转换角更新第二航向转换角，得到第三航向转换角；基于当前目标帧航向角判断障碍物的运动方向是否与车辆的运动方向是否一致；如果方向一致，将第一航向转换角和第三航向转换角转换到-π至π的范围内，得到当前帧的第四航向转换角和上一帧的第五航向转换角；基于一阶惯性滤波算法结合第四航向转换角和第五航向转换角的加权平均值更新第四航向转换角；将更新后的第四航向转换角转换到0至2π范围内，得到第六航向转换角并存储，使第六航向转换角作为更新下一帧航向角时的上一帧的第二航向转换角使用。在本方案中，运用一阶惯性滤波算法结合上一帧航向角和当前帧航向角的加权平均值对当前帧航向角进行更新，也就是对原始数据的车辆检测框的航向角，从而减少了航向角的小范围抖动，实现自动驾驶车辆过程中抑制或消除车辆检测框抖动，提高车辆驾驶安全性和舒适性。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。

图1为本发明实施例提供的一种车辆的障碍物检测框抖动消除方法的流程示意图；

图2为本发明实施例提供的一种应用场景示意图；

图3为本发明实施例提供的一种车辆的障碍物检测框抖动消除方法中将当前帧航向角转换至0至2π范围内的流程示意图；

图4为本发明实施例提供的一种车辆的障碍物检测框抖动消除方法中判断障碍物的运动方向是否与车辆的运动方向是否一致的流程示意图；

图5为本发明实施例提供的一种车辆的障碍物检测框抖动消除方法中将第一航向转换角和第三航向转换角转换到-π至π的范围内的流程示意图；

图6为本发明实施例提供的另一种车辆的障碍物检测框抖动消除方法的流程示意图；

图7为本发明实施例提供的一种车辆的障碍物检测框抖动消除装置的结构示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

在本申请中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

由背景技术可知，传感器上的相机镜头发生的畸变会从视角中心到边缘逐渐递增，导致在相机视角边界处的物体的检测效果会严重下降。此外，车辆运行过程中会引起振动，导致相机图像中车辆障碍物检测框的朝向会出现抖动现象，而相邻车道的车辆检测框抖动会引起车辆急刹、逼停等问题，而且车辆检测框抖动会带来车辆驾驶安全性和舒适性的问题。

因此，本发明实施例提供一种车辆的障碍物检测框抖动消除方法及装置，运用一阶惯性滤波算法结合上一帧航向角和当前帧航向角的加权平均值对当前帧航向角进行更新，从而减少了航向角的小范围抖动，解决了自动驾驶车辆过程中车辆检测框抖动引发的车辆驾驶安全性和舒适性的问题。以实现自动驾驶车辆过程中抑制或消除车辆检测框抖动，提高车辆驾驶安全性和舒适性的目的。

参见图1，为本发明实施例提供的一种车辆的障碍物检测框抖动消除方法的流程图，所述车辆的障碍物检测框抖动消除方法包括以下步骤：

步骤S101：获取车辆的障碍物检测框的当前帧航向角和存储的上一帧的第二航向转换角，将当前帧航向角转换至0至2π范围内，得到转换后的当前帧的第一航向转换角。

在步骤S101中，当前帧不是第一帧。

在具体实现步骤S101的过程中，通过车辆的感知系统中的传感器获取车辆的障碍物检测框的当前帧航向角和存储的上一帧的第二航向转换角，判断当前帧航向角是否大于0且小于2π，若当前帧航向角既不大于0也不小于2π，则将当前帧航向角转换到0至2π范围，得到转换后的当前帧的第一航向转换角。

需要说明的是，若获取到的当前帧为第一帧，则将所述当前帧的航向角作为上一帧的航向角；采用上述同样的方式将当前帧的航向角和作为上一帧的航向角的当前帧转换到0至2π范围内，得到转换后的当前帧的第一航向转换角和上一帧的第二航向转换角。

也就是说，若获取到的当前帧为第一帧，则得到转换后的当前帧的第一航向转换角和上一帧的第二航向转换角相同。

步骤S102：判断障碍物检测框中出现的障碍物是否处于运动状态，若障碍物处于运动状态，执行步骤S103；若障碍物不处于运动状态，执行步骤S108。

在具体实现步骤S102的过程中，若障碍物检测框中出现障碍物，判断障碍物是否处于运动状态，若障碍物处于运动状态，说明障碍物的目标帧航向角对障碍物后续的运动有影响，执行步骤S103；若障碍物不处于运动状态，说明障碍物的目标帧航向角对障碍物后续的运动没有影响，执行步骤S108。

步骤S103：判断障碍物的当前目标帧航向角和障碍物的上一目标帧航向角的夹角是否大于90度，若夹角大于90度，执行步骤S104；若夹角不大于90度，执行步骤S105。

在具体实现步骤S103的过程中，将障碍物的当前目标帧航向角和障碍物的上一目标帧航向角的夹角与90度进行比较，若夹角大于90度，说明夹角不是由于障碍物检测框导致的，而是因为障碍物运动状态发生了较大变化，执行步骤S104；若夹角不大于90度，说明障碍物运动状态变化不明显，执行步骤S105。

步骤S104：利用第一航向转换角更新第二航向转换角，得到第三航向转换角。

在具体实现步骤S104的过程中，基于一阶惯性滤波算法结合第一航向转换角和第二航向转换角的加权平均值更新第二航向转换角，得到第三航向转换角。具体的，根据公式(1)得到更新第二航向转换角后的第三航向装换角Heading′：

Heading′＝αHeading_pre+(1-α)Heading (1)

Heading’＝Heading_pre＝Heading (1)

其中，Heading′为第三航向转换角，Heading为第一航向转换角，Heading_pre为第二航向转换角。

例如，第一航向转换角和第二航向转换角分别为30°和125°，夹角为95°大于90°，则根据公式(1)进行计算，因此第三航向转换角为30°。

步骤S105：基于当前目标帧航向角判断障碍物的运动方向是否与车辆的运动方向是否一致，若方向一致，执行步骤S106；若方向不一致，执行步骤S107。

在具体实现步骤S105的过程中，根据当前目标帧航向角，判断障碍物的运动方向是否与车辆的运动方向是否一致，若方向一致，说明障碍物实际航向角变化区间为[0，θ]与[2π-θ，2π]，θ为方向判断阈值，此时变化区间为不连续区间。为保证角度计算的连续性，应保证航向角变化区间连续，因此将航向角转换至-π至π的范围内，此时航向角变化区间为[-θ，θ]，是连续区间，因此执行步骤S106；若方向不一致，说明障碍物实际航向角变化区间为[θ，2π-θ]，是连续区间，执行步骤S107。

例如，如图2所示出的应用场景的X-Y坐标系，障碍物在自车的前方，且与自车同方向，此时自车的前视相机视角边界如图2所示，因此，需要将航向角转换至-π至π的范围内，也就是，将第一航向转换角和第三航向转换角转换到-π至π的范围内，以更利于后续的更新。

步骤S106：将第一航向转换角和第三航向转换角转换到-π至π的范围内，得到当前帧的第四航向转换角和上一帧的第五航向转换角。

在具体实现步骤S106的过程中，判断第一航向转换角和第三航向转换角是否大于-π且小于π，若第一航向转换角和第三航向转换角既不大于-π也不小于π，将第一航向转换角和第三航向转换角转换到-π至π的范围内，得到当前帧的第四航向转换角和上一帧的第五航向转换角。

步骤S107：基于一阶惯性滤波算法结合第四航向转换角和第五航向转换角的加权平均值更新第四航向转换角。

在具体实现步骤S107的过程中，根据公式(2)利用第四航向转换角和第五航向转换角的加权平均值更新第四航向转换角。

Heading’＝αHeading_pre+(1-α)Heading (2)

其中，转换到公式(2)中，Heading为第四航向转换角，Heading_pre为第五航向转换角，α为滤波系数控制历史帧的权重，0<α<1。

步骤S108：将更新后的第四航向转换角转换到0至2π范围内，得到第六航向转换角并存储，使第六航向转换角作为更新下一帧航向角时的上一帧的第二航向转换角使用。

在具体实现步骤S108的过程中，判断更新后的第四航向转换角是否大于0小于2π范围，若更新后的第四航向转换角既不大于0也不小于2π，则将更新后的第四航向转换角转换到0至2π范围内，得到第六航向转换角，将第六航向转换角进行存储，使第六航向转换角作为更新下一帧航向角时的上一帧的第二航向转换角使用。

基于本发明实施例公开的车辆的障碍物检测框抖动消除方法，通过获取车辆的障碍物检测框的当前帧航向角和存储的上一帧的第二航向转换角，将当前帧航向角转换至0至2π范围内，得到转换后的当前帧的第一航向转换角；若障碍物检测框中出现障碍物且障碍物处于运动状态，判断障碍物的当前目标帧航向角和障碍物的上一目标帧航向角的夹角是否大于90度；若夹角大于90度，利用第一航向转换角更新第二航向转换角，得到第三航向转换角；基于当前目标帧航向角判断障碍物的运动方向是否与车辆的运动方向是否一致；如果方向一致，将第一航向转换角和第三航向转换角转换到-π至π的范围内，得到当前帧的第四航向转换角和上一帧的第五航向转换角；基于一阶惯性滤波算法结合第四航向转换角和第五航向转换角的加权平均值更新第四航向转换角；将更新后的第四航向转换角转换到0至2π范围内，得到第六航向转换角并存储，使第六航向转换角作为更新下一帧航向角时的上一帧的第二航向转换角使用。在本方案中，运用一阶惯性滤波算法结合上一帧航向角和当前帧航向角的加权平均值对当前帧航向角进行更新，从而减少了航向角的小范围抖动，解决了自动驾驶车辆过程中车辆检测框抖动引发的车辆驾驶安全性和舒适性的问题。实现自动驾驶车辆过程中抑制或消除车辆检测框抖动，提高车辆驾驶安全性和舒适性的目的。

上述本发明实施例图1公开的步骤S101涉及到的将当前帧航向角转换至0至2π范围内，得到转换后的当前帧的第一航向转换角的过程，参考图3，为本发明实施例提供的将当前帧航向角转换至0至2π范围内的流程图，包括以下步骤：

步骤S301：判断当前帧航向角是否大于0且小于2π，若当前帧航向角大于0且小于2π，执行步骤S302；若当前帧航向角既不大于0也不小于2π，执行步骤S303。

在具体实现步骤S301的过程中，从车辆的感知系统中的传感器获取车辆的障碍物检测框的当前帧航向角后，将当前帧航向角与0和2π进行比较，若当前帧航向角大于0且小于2π，执行步骤S302；若当前帧航向角既不大于0也不小于2π，说明当前帧航向角小于0或大于2π，执行步骤S303。

步骤S302：得到转换后的当前帧的第一航向转换角。

在具体实现步骤S302的过程中，若第一次判断时，当前帧航向角即大于0且小于2π，则直接将当前帧航向角作为当前帧的第一航向转换角。假设当前帧航向角为

直接将

作为当前帧的第一航向转换角。

若非第一次判断，则得到的转换后的当前帧的第一航向转换角为经过后续步骤S304或步骤S306处理的航向角。

步骤S303：判断当前帧航向角是否大于2π，若当前帧航向角大于2π，执行步骤S304；若当前帧航向角不大于2π，执行步骤S305。

在具体实现步骤S303的过程中，将当前帧航向角与2π进行比较，若当前帧航向角大于2π，执行步骤S304；若当前帧航向角不大于2π，说明当前帧航向角小于0，执行步骤S305。

步骤S304：将当前帧航向角减去2nπ，返回执行步骤S301。

需要说明的是，2nπ中n的取值为大于等于1的正整数，比如n＝1，n＝2。

在具体实现步骤S304的过程中，当n＝1时，将当前航向角与2π进行减法计算，得到第一次转换后的当前帧的第一航向转换角，返回判断第一次转换后的当前帧的第一航向转换角是否大于0小于2π，若第一次转换后的当前帧的第一航向转换角处于0至2π范围内，结束判断；若第一次转换后的当前帧的第一航向转换角不处于0至2π范围内，继续执行步骤S301，直至转换后的当前帧的第一航向转换角处于0至2π范围内，结束判断。

例如，当前帧航向角为

当n取1时，将当前帧航向角减去2π得到第一次转换后的当前帧的第一航向转换角，即：

返回判断

是否大于0小于2π，

处于0至2π范围内，结束判断。

又例如，当前帧航向角为

当n取2时，将当前帧航向角减去4π得到第一次转换后的当前帧的第一航向转换角，即：

返回判断

是否大于0小于2π，

不处于0至2π范围内，继续执行步骤S301，直至转换后的当前帧的第一航向转换角处于0至2π范围内，结束判断。

步骤S305：判断当前帧航向角是否小于0，若当前帧航向角小于0，执行步骤S306；若当前帧航向角不小于0，执行步骤S301。

在具体实现步骤S305的过程中，将当前帧航向角与0进行比较，若当前帧航向角小于0，执行步骤S306；若当前帧航向角不小于0，说明当前帧航向角大于0，执行步骤S301。

步骤S307：将当前帧航向角加上2nπ，返回继续执行步骤S301。

在具体实现步骤S307的过程中，当n＝1时，将当前航向角与2π进行加法计算，得到第一次转换后的当前帧的第一航向转换角，返回判断第一次转换后的当前帧的第一航向转换角是否大于0小于2π，若第一次转换后的当前帧的第一航向转换角处于0至2π范围内，结束判断；若第一次转换后的当前帧的第一航向转换角不处于0至2π范围内，继续执行步骤S301，直至转换后的当前帧的第一航向转换角处于0至2π范围内，结束判断。

例如，当前帧航向角为

当n取1时，将当前帧航向角减去2π得到第一次转换后的当前帧的第一航向转换角，即：

返回判断

是否大于0小于2π，

处于0至2π范围内，结束判断。

又例如，当前帧航向角为

当n取2时，将当前帧航向角减去4π得到第一次转换后的当前帧的第一航向转换角，即：

返回判断

是否大于0小于2π，

不处于0至2π范围内，继续执行步骤S301，直至转换后的当前帧的第一航向转换角处于0至2π范围内，结束判断。

基于本发明实施例公开的车辆的障碍物检测框抖动消除方法，通过判断当前帧航向角是否大于0且小于2π，若当前帧航向角大于2π，将当前帧航向角减去2nπ，返回继续判断，直至当前航向角处于0至2π范围内，得到转换后的当前帧的第一航向转换角；若当前帧航向角小于0，将当前帧航向角加上2nπ，返回继续判断，直至当前航向角处于0至2π范围内，得到转换后的当前帧的第一航向转换角。通过上述过程，基于角度的周期性，将角度在后续更新之前，转换至一个统一的范围内，可以简化后续的计算过程，提高了运算速率。

上述本发明实施例图1公开的步骤S105涉及到的基于当前目标帧航向角判断障碍物的运动方向是否与车辆的运动方向是否一致，参考图4，为本发明实施例提供的判断障碍物的运动方向是否与车辆的运动方向是否一致的流程图，包括以下步骤：

步骤S401：判断第一航向转换角与当前目标帧航向角的夹角是否小于设定的阈值角度，若夹角小于设定的阈值角度，执行步骤S402；若夹角大于等于设定的阈值角度，执行步骤S403。

在具体实现步骤S401的过程中，假设设定的阈值角度为某个具体的角度，判断第一航向转换角与当前目标帧航向角的夹角是否小于该具体的角度，若夹角小于该具体的角度，说明障碍物的运动方向与车辆的运动方向一致，执行步骤S402；若夹角大于等于该具体的角度，说明障碍物的运动方向与车辆的运动方向不一致，执行步骤S403。

例如，设定的阈值角度为30°，判断第一航向转换角与当前目标帧航向角的夹角是否小于30°，若夹角小于30°，说明障碍物的运动方向与车辆的运动方向一致；若夹角大于等于30°，说明障碍物的运动方向与车辆的运动方向不一致。

需要说明的是，设定的阈值角度可以是30°，也可以是45°，并不仅限于本发明上述公开的阈值角度，也可以由技术人员基于技术需求自行进行设置。

步骤S402：确定障碍物的运动方向与车辆的运动方向一致。

在具体实现步骤S402的过程中，若夹角小于基于步骤S401设定的阈值角度，说明障碍物的运动方向与车辆的运动方向一致，确定障碍物的运动方向与车辆的运动方向一致。

步骤S403：确定障碍物的运动方向与车辆的运动方向不一致。

在具体实现步骤S403的过程中，若夹角大于等于基于步骤S401设定的阈值角度，说明障碍物的运动方向与车辆的运动方向不一致，确定障碍物的运动方向与车辆的运动方向不一致。

基于本发明实施例公开的车辆的障碍物检测框抖动消除方法，通过判断第一航向转换角与当前目标帧航向角的夹角是否小于设定的阈值角度，若夹角小于设定的阈值角度，确定障碍物的运动方向与车辆的运动方向一致；若夹角大于等于设定的阈值角度，确定障碍物的运动方向与车辆的运动方向不一致。以此确定障碍物的运动方向与车辆的运动方向是否一致，判断过程简单，提高了判断效率。

上述本发明实施例图1公开的步骤S106涉及到的如果方向一致，将第一航向转换角和第三航向转换角转换到-π至π的范围内，得到当前帧的第四航向转换角和上一帧的第五航向转换角的过程，参考图5，为本发明实施例提供的将第一航向转换角和第三航向转换角转换到-π至π的范围内的流程图，包括以下步骤：

步骤S501：如果方向一致，判断第一航向转换角和第三航向转换角是否大于-π且小于π，若第一航向转换角和第三航向转换角大于-π且小于π，执行步骤S502；若第一航向转换角和第三航向转换角既不大于-π也不小于π，执行步骤S503。

在具体实现步骤S501的过程中，如果方向一致，将第一航向转换角和第三航向转换角分别与-π和π进行比较，若第一航向转换角和第三航向转换角大于-π且小于π，执行步骤S502；若第一航向转换角和第三航向转换角既不大于-π也不小于π，执行步骤S503。

步骤S502：得到当前帧的第四航向转换角和上一帧的第五航向转换角。

在具体实现步骤S502的过程中，若第一次判断时，第一航向转换角和第三航向转换角大于-π且小于π，则直接将第一航向转换角作为当前帧的第四航向转换角，将第三航向转换角作为上一帧的第五航向转换角。

假设第一航向转换角为

第三航向转换角为

直接将

作为当前帧的第四航向转换角，将

作为上一帧的第五航向转换角，得到当前帧的第四航向转换角和上一帧的第五航向转换角。

若非第一次判断，则得到的转换后的当前帧的第四航向转换角和上一帧的第五航向转换角为经过后续步骤S504或步骤S506处理的航向角。

步骤S503：判断第一航向转换角和第三航向转换角是否大于π，若第一航向转换角和第三航向转换角大于π，执行步骤S504；若第一航向转换角和第三航向转换角不大于π，执行步骤S505。

在具体实现步骤S503的过程中，将第一航向转换角和第三航向转换角分别与π进行比较，若第一航向转换角和第三航向转换角大于π，执行步骤S504；若第一航向转换角和第三航向转换角不大于π，说明第一航向转换角和第三航向转换角小于-π，执行步骤S505。

步骤S504：将第一航向转换角和第三航向转换角分别减去2π，返回继续执行步骤S501的判断。

在具体实现步骤S504的过程中，将第一航向转换角和第三航向转换角分别与2π进行减法计算，得到第一次转换后当前帧的第四航向转换角和上一帧的第五航向转换角，返回判断第一次转换后当前帧的第四航向转换角和上一帧的第五航向转换角是否大于-π小于π，若第一次转换后当前帧的第四航向转换角和上一帧的第五航向转换角处于-π至π范围内，结束判断。若第一次转换后当前帧的第四航向转换角和上一帧的第五航向转换角不处于-π至π范围内，继续执行步骤S501，直至第一航向转换角和第三航向转换角处于-π至π范围内，结束判断。

例如，第一航向转换角为

第三航向转换角为

将第一航向转换角和第三航向转换角分别减去2π，得到第一次转换后当前帧的第四航向转换角和上一帧的第五航向转换角，即：

返回判断

和

是否大于-π小于π，

和

处于-π至π范围内，结束判断。

步骤S505：判断第一航向转换角和第三航向转换角是否小于-π，若第一航向转换角和第三航向转换角小于-π，执行步骤S506；若第一航向转换角和第三航向转换角不小于-π，执行步骤S501。

在具体实现步骤S505的过程中，将第一航向转换角和第三航向转换角分别与-π进行比较，若第一航向转换角和第三航向转换角小于-π，执行步骤S506；若第一航向转换角和第三航向转换角不小于-π，说明第一航向转换角和第三航向转换角大于-π，执行步骤S501。

步骤S506：将第一航向转换角和第三航向转换角分别加上2π，返回继续执行步骤S501的判断。

在具体实现步骤S506的过程中，将第一航向转换角和第三航向转换角分别与2π进行加法计算，得到第一次转换后当前帧的第四航向转换角和上一帧的第五航向转换角，返回判断第一次转换后当前帧的第四航向转换角和上一帧的第五航向转换角是否大于-π小于π，若第一次转换后当前帧的第四航向转换角和上一帧的第五航向转换角处于-π至π范围内，结束判断。若第一次转换后当前帧的第四航向转换角和上一帧的第五航向转换角不处于-π至π范围内，继续执行步骤S501，直至第一航向转换角和第三航向转换角处于-π至π范围内，结束判断。

例如，假设第一航向转换角为

第三航向转换角为

将第一航向转换角和第三航向转换角分别加上2π，得到第一次转换后当前帧的第四航向转换角和上一帧的第五航向转换角，即：

返回判断

和

是否大于-π小于π，

和

处于-π至π范围内，结束判断。

基于本发明实施例公开的车辆的障碍物检测框抖动消除方法，通过判断第一航向转换角和第三航向转换角的角度是否大于-π且小于π，若第一航向转换角和第三航向转换角大于π，将第一航向转换角和第三航向转换角分别减去2π，返回继续判断，直至第一航向转换角和第三航向转换角处于-π至π范围内，直接得到当前帧的第四航向转换角和上一帧的第五航向转换角；若第一航向转换角和第三航向转换角小于-π，将第一航向转换角和第三航向转换角分别加上2π，返回继续判断，直至第一航向转换角和第三航向转换角处于-π至π范围内，得到当前帧的第四航向转换角和上一帧的第五航向转换角。通过上述过程，将位于同一方向的障碍物和自身车辆的航向角转换至一个统一的范围内，可以简化后续的计算过程，提高了运算速率。

参见图6，为本发明提供的另一种车辆的障碍物检测框抖动消除方法的流程图，所述车辆的障碍物检测框抖动消除方法具体包括以下步骤：

步骤S601：判断获取到的当前帧是否为第一帧，若获取到的当前帧为第一帧，执行步骤S602；若获取到的当前帧不是第一帧，执行步骤S603。

在具体实现步骤S601的过程中，通过车辆的感知系统中的传感器获取车辆的障碍物检测框的当前帧，判断所获取到的当前帧是否为第一帧，若获取到的当前帧为第一帧，执行步骤S602；若获取到的当前帧不是第一帧，执行步骤S603。

步骤S602：若获取到的当前帧为第一帧，将当前帧的航向角作为上一帧的航向角。

在具体实现步骤S602的过程中，若获取到的当前帧为第一帧，利用当前帧的航向角更新上一帧的航向角。

步骤S603：将当前帧的航向角和上一帧的航向角转换到0至2π范围内，得到转换后的当前帧的第一航向转换角和上一帧的第二航向转换角。

在具体实现步骤S603的过程中，判断当前帧的航向角和上一帧的航向角是否大于0且小于2π，若当前帧的航向角和上一帧的航向角既不大于0也不小于2π，将当前帧的航向角和上一帧的航向角转换到0至2π范围内，得到转换后的当前帧的第一航向转换角和上一帧的第二航向转换角。

步骤S604：若障碍物检测框中出现障碍物，判断障碍物是否处于运动状态，若判断障碍物处于运动状态，执行步骤S605；若障碍物不处于运动状态，执行步骤S610。

步骤S605：判断障碍物的当前目标帧航向角和障碍物的上一目标帧航向角的夹角是否大于90度，若夹角大于90度，执行步骤S606；若夹角不大于90度，执行步骤S607。

步骤S606：利用第一航向转换角更新第二航向转换角，得到第三航向转换角。

步骤S607：基于当前目标帧航向角判断障碍物的运动方向是否与车辆的运动方向是否一致，若方向一致，执行步骤S608；若方向不一致，执行步骤S609。

步骤S608：将第一航向转换角和第三航向转换角转换到-π至π的范围内，得到当前帧的第四航向转换角和上一帧的第五航向转换角。

步骤S609：基于一阶惯性滤波算法结合第四航向转换角和第五航向转换角的加权平均值更新第四航向转换角。

步骤S610：将更新后的第四航向转换角转换到0至2π范围内，得到第六航向转换角并存储，使第六航向转换角作为更新下一帧航向角时的上一帧的第二航向转换角使用。

上述步骤S604至步骤S610的执行原理和过程与图1中公开的步骤S102至步骤S108的执行原理和过程相同，可参见，这里不再赘述。

基于本发明实施例公开的车辆的障碍物检测框抖动消除方法，通过判断所获取到的当前帧是否为第一帧若获取到的当前帧为第一帧，将当前帧的航向角作为上一帧的航向角，将当前帧的航向角和上一帧的航向角转换到0至2π范围内，得到转换后的当前帧的第一航向转换角和上一帧的第二航向转换角；若障碍物检测框中出现障碍物且障碍物处于运动状态，判断障碍物的当前目标帧航向角和障碍物的上一目标帧航向角的夹角是否大于90度；若夹角大于90度，利用第一航向转换角更新第二航向转换角，得到第三航向转换角；基于当前目标帧航向角判断障碍物的运动方向是否与车辆的运动方向是否一致；如果方向一致，将第一航向转换角和第三航向转换角转换到-π至π的范围内，得到当前帧的第四航向转换角和上一帧的第五航向转换角；基于一阶惯性滤波算法结合第四航向转换角和第五航向转换角的加权平均值更新第四航向转换角；将更新后的第四航向转换角转换到0至2π范围内，得到第六航向转换角并存储，使第六航向转换角作为更新下一帧航向角时的上一帧的第二航向转换角使用。在本方案中，运用一阶惯性滤波算法结合上一帧航向角和当前帧航向角的加权平均值对当前帧航向角进行更新，从而减少了航向角的小范围抖动，解决了自动驾驶车辆过程中车辆检测框抖动引发的车辆驾驶安全性和舒适性的问题。实现自动驾驶车辆过程中抑制或消除车辆检测框抖动，提高车辆驾驶安全性和舒适性的目的。

基于本发明实施例公开的车辆的障碍物检测框抖动消除方法，本发明实施例还对应公开了一种车辆的障碍物检测框抖动消除装置，参见图7，该车辆的障碍物检测框抖动消除装置包括：检测单元70、第一转换模块71、第一更新模块72、第二转换模块73、第二更新模块74以及第三转换模块75。

检测单元70，用于检测当前帧是否是第一帧，若获取到的当前帧为第一帧，将当前帧的航向角作为上一帧的航向角。

第一转换模块71，用于获取车辆的障碍物检测框的当前帧航向角和存储的上一帧的第二航向转换角，将当前帧航向角转换至0至2π范围内，得到转换后的当前帧的第一航向转换角，其中，当前帧不是第一帧。

其中，将当前帧航向角转换至0至2π范围内，得到转换后的当前帧的第一航向转换角的第一转换模块71，具体用于判断所述当前帧航向角是否大于0且小于2π；若当前帧航向角大于2π，将所述当前帧航向角减去2nπ，返回继续判断，直至当前航向角处于0至2π范围内，得到转换后的当前帧的第一航向转换角，其中，n的取值为大于等于1的正整数；若当前帧航向角小于0，将所述当前帧航向角加上2nπ，返回继续判断，直至当前航向角处于0至2π范围内，得到转换后的当前帧的第一航向转换角。

第一更新模块72，用于若障碍物检测框中出现障碍物且障碍物处于运动状态，判断障碍物的当前目标帧航向角和障碍物的上一目标帧航向角的夹角是否大于90度；若夹角大于90度，利用第一航向转换角更新第二航向转换角，得到第三航向转换角。

其中，若夹角大于90度，利用第一航向转换角更新第二航向转换角，得到第三航向转换角的第一更新模块72，具体用于若所述夹角大于90度，基于一阶惯性滤波算法结合所述第一航向转换角和所述第二航向转换角的加权平均值更新所述第二航向转换角，得到第三航向转换角。

第二转换模块73，用于基于当前目标帧航向角判断障碍物的运动方向是否与车辆的运动方向是否一致；如果方向一致，将第一航向转换角和第三航向转换角的角度转换到-π至π的范围内，得到当前帧的第四航向转换角和上一帧的第五航向转换角。

其中，基于当前目标帧航向角判断障碍物的运动方向是否与车辆的运动方向是否一致的第二转换模块73，具体用于判断第一航向转换角与所述当前目标帧航向角的夹角是否小于设定的阈值角度；若所述夹角小于设定的阈值角度，确定所述障碍物的运动方向与所述车辆的运动方向一致；若所述夹角大于等于设定的阈值角度，确定所述障碍物的运动方向与所述车辆的运动方向不一致。

如果方向一致，将第一航向转换角和第三航向转换角的角度转换到-π至π的范围内，得到当前帧的第四航向转换角和上一帧的第五航向转换角的第二转换模块73，具体用于所述如果方向一致，判断所述第一航向转换角和所述第三航向转换角的角度是否大于-π且小于π；若所述第一航向转换角和所述第三航向转换角大于-π且小于π，将所述第一航向转换角作为当前帧的第四航向转换角，将所述第三航向转换角作为上一帧的第五航向转换角；若所述第一航向转换角和所述第三航向转换角大于π，将所述第一航向转换角和所述第三航向转换角分别减去2π，返回继续判断，直至所述第一航向转换角和所述第三航向转换角处于-π至π范围内，得到当前帧的第四航向转换角和上一帧的第五航向转换角。

第二更新模块74，用于基于一阶惯性滤波算法结合第四航向转换角和第五航向转换角的加权平均值更新第四航向转换角。

其中，所述第二更新模块74，具体用于根据公式(2)进行计算，即根据第四航向转换角和第五航向转换角的加权平均值对第四航向转换角进行更新，得到更新后的第四航向转换角，其中，转换到公式(2)中，Heading为第四航向转换角，Heading_pre为第五航向转换角，α为滤波系数控制历史帧的权重，0<α<1。

第三转换模块65，用于将更新后的第四航向转换角转换到0至2π范围内，得到第六航向转换角并存储，使第六航向转换角作为更新下一帧航向角时的上一帧的第二航向转换角使用。

需要说明的是，上述本发明实施例公开的车辆的障碍物检测框抖动消除装置中的各个模块具体的原理和执行过程，与上述本发明实施的车辆的障碍物检测框抖动消除方法相同，可参见上述本发明实施例公开的车辆的障碍物检测框抖动消除方法中相应的部分，这里不再进行赘述。

基于本发明实施例公开的车辆的障碍物检测框抖动消除装置，通过获取车辆的障碍物检测框的当前帧航向角和存储的上一帧的第二航向转换角，将当前帧航向角转换至0至2π范围内，得到转换后的当前帧的第一航向转换角，其中，当前帧不是第一帧；若障碍物检测框中出现障碍物且障碍物处于运动状态，判断障碍物的当前目标帧航向角和障碍物的上一目标帧航向角的夹角是否大于90度；若夹角大于90度，利用第一航向转换角更新第二航向转换角，得到第三航向转换角；基于当前目标帧航向角判断障碍物的运动方向是否与车辆的运动方向是否一致；如果方向一致，将第一航向转换角和第三航向转换角转换到-π至π的范围内，得到当前帧的第四航向转换角和上一帧的第五航向转换角；基于一阶惯性滤波算法结合第四航向转换角和第五航向转换角的加权平均值更新第四航向转换角；将更新后的第四航向转换角转换到0至2π范围内，得到第六航向转换角并存储，使第六航向转换角作为更新下一帧航向角时的上一帧的第二航向转换角使用。在本方案中，运用一阶惯性滤波算法结合上一帧航向角和当前帧航向角的加权平均值对当前帧航向角进行更新，从而减少了航向角的小范围抖动，解决了自动驾驶车辆过程中车辆检测框抖动引发的车辆驾驶安全性和舒适性的问题。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (10)

1.一种车辆的障碍物检测框抖动消除方法，其特征在于，所述方法包括：

获取车辆的障碍物检测框的当前帧航向角和存储的上一帧的第二航向转换角，将所述当前帧航向角转换至0至2π范围内，得到转换后的当前帧的第一航向转换角，其中，所述当前帧不是第一帧；

若所述障碍物检测框中出现障碍物且所述障碍物处于运动状态，判断所述障碍物的当前目标帧航向角和所述障碍物的上一目标帧航向角的夹角是否大于90度；

若所述夹角大于90度，利用所述第一航向转换角更新第二航向转换角，得到第三航向转换角；

基于所述当前目标帧航向角判断所述障碍物的运动方向是否与所述车辆的运动方向是否一致；

如果方向一致，将所述第一航向转换角和所述第三航向转换角转换到-π至π的范围内，得到当前帧的第四航向转换角和上一帧的第五航向转换角；

基于一阶惯性滤波算法结合所述第四航向转换角和所述第五航向转换角的加权平均值更新所述第四航向转换角；

将更新后的第四航向转换角转换到0至2π范围内，得到第六航向转换角并存储，使所述第六航向转换角作为更新下一帧航向角时的上一帧的第二航向转换角使用。

2.根据权利要求1所述的方法，其特征在于，所述方法还包括：

若获取到的当前帧为第一帧，将所述当前帧的航向角作为上一帧的航向角；

将所述当前帧的航向角和所述上一帧的航向角转换到0至2π范围内，得到转换后的当前帧的第一航向转换角和上一帧的第二航向转换角。

3.根据权利要求1所述的方法，其特征在于，将所述当前帧航向角转换至0至2π范围内，得到转换后的当前帧的第一航向转换角，包括：

判断所述当前帧航向角是否大于0且小于2π；

若当前帧航向角大于2π，将所述当前帧航向角减去2nπ，返回继续判断，直至当前航向角处于0至2π范围内，得到转换后的当前帧的第一航向转换角，其中，n的取值为大于等于1的正整数；

若当前帧航向角小于0，将所述当前帧航向角加上2nπ，返回继续判断，直至当前航向角处于0至2π范围内，得到转换后的当前帧的第一航向转换角。

4.根据权利要求1所述的方法，其特征在于，若所述夹角大于90度，利用所述第一航向转换角更新第二航向转换角，得到第三航向转换角，包括：

若所述夹角大于90度，将所述第一航向转换角直接赋值给所述第二航向转换角，得到第三航向转换角。

5.根据权利要求1所述的方法，其特征在于，所述基于所述当前目标帧航向角判断所述障碍物的运动方向是否与所述车辆的运动方向是否一致，包括：

判断第一航向转换角与所述当前目标帧航向角的夹角是否小于设定的阈值角度；

若所述夹角小于设定的阈值角度，确定所述障碍物的运动方向与所述车辆的运动方向一致；

若所述夹角大于等于设定的阈值角度，确定所述障碍物的运动方向与所述车辆的运动方向不一致。

6.根据权利要求1所述的方法，其特征在于，所述如果方向一致，将所述第一航向转换角和所述第三航向转换角转换到-π至π的范围内，得到当前帧的第四航向转换角和上一帧的第五航向转换角，包括：

所述如果方向一致，判断所述第一航向转换角和所述第三航向转换角的角度是否大于-π且小于π；

若所述第一航向转换角和所述第三航向转换角大于-π且小于π，将所述第一航向转换角作为当前帧的第四航向转换角，将所述第三航向转换角作为上一帧的第五航向转换角；

若所述第一航向转换角和所述第三航向转换角大于π，将所述第一航向转换角和所述第三航向转换角分别减去2π，返回继续判断，直至所述第一航向转换角和所述第三航向转换角处于-π至π范围内，得到当前帧的第四航向转换角和上一帧的第五航向转换角。

7.根据权利要求1所述的方法，其特征在于，所述基于一阶惯性滤波算法结合所述第四航向转换角和所述第五航向转换角的加权平均值更新所述第四航向转换角，包括：

根据公式Heading＝αHeading_pre+(1-α)Heading，以及所述第四航向转换角和所述第五航向转换角的加权平均值对所述第四航向转换角进行更新，其中，Heading为所述第四航向转换角，Heading_pre为所述第五航向转换角，α为滤波系数控制历史帧的权重，0<α<1。

8.一种车辆的障碍物检测框抖动消除装置，其特征在于，所述装置包括：

第一转换模块，用于获取车辆的障碍物检测框的当前帧航向角和存储的上一帧的第二航向转换角，将所述当前帧航向角转换至0至2π范围内，得到转换后的当前帧的第一航向转换角，其中，所述当前帧不是第一帧；

第一更新模块，用于若所述障碍物检测框中出现障碍物且所述障碍物处于运动状态，判断所述障碍物的当前目标帧航向角和所述障碍物的上一目标帧航向角的夹角是否大于90度；若所述夹角大于90度，利用所述第一航向转换角更新第二航向转换角，得到第三航向转换角；

第二转换模块，用于基于所述当前目标帧航向角判断所述障碍物的运动方向是否与所述车辆的运动方向是否一致；如果方向一致，将所述第一航向转换角和所述第三航向转换角的角度转换到-π至π的范围内，得到当前帧的第四航向转换角和上一帧的第五航向转换角；

第二更新模块，用于基于一阶惯性滤波算法结合所述第四航向转换角和所述第五航向转换角的加权平均值更新所述第四航向转换角；

第三转换模块，用于将更新后的第四航向转换角转换到0至2π范围内，得到第六航向转换角并存储，使所述第六航向转换角作为更新下一帧航向角时的上一帧的第二航向转换角使用。

9.根据权利要求8所述的装置，其特征在于，所述装置还包括：

检测单元，用于检测所述当前帧是否是第一帧，若获取到的当前帧为第一帧，将所述当前帧的航向角作为上一帧的航向角；

第一转换模块，还用于将所述当前帧的航向角和所述上一帧的航向角转换到0至2π范围内，得到转换后的当前帧的第一航向转换角和上一帧的第二航向转换角。

10.根据权利要求8所述装置，其特征在于，所述第二更新模块，具体用于：

根据公式Heading＝αHeading_pre+(1-α)Heading进行计算，得到所述第四航向转换角和所述第五航向转换角的加权平均值，其中，Heading为所述第四航向转换角，Heading_pre为所述第五航向转换角，α为滤波系数控制历史帧的权重，0<α<1；利用所述加权平均值更新所述第四航向转换角。

Images