CN111578894B - 一种确定障碍物航向角的方法及装置 - Google Patents

Abstract

本发明公开一种确定障碍物航向角的方法及装置。该方法包括：获取障碍物的运动状态信息和点云簇，根据障碍物的运动状态信息计算障碍物的航向角，再根据该航向角确定障碍物的运动工况，根据障碍物的运动工况、障碍物的航向角以及障碍物的点云簇确定障碍物的最终航向角。基于本发明公开的技术方案，在确定障碍物的最终航向角的过程中，综合了障碍物的运动状态信息和点云簇，并结合障碍物的工况，因此能够提高最终获得的航向角的准确性。

Description

一种确定障碍物航向角的方法及装置

技术领域

本发明涉及自动驾驶技术领域，具体地说，涉及一种确定障碍物航向角的方法及装置。

背景技术

自动驾驶车辆的出现给人们的生产和生活带来了便利。自动驾驶车辆能够合理地规划行驶路线，缓解交通堵塞，提高交通通行率，也能减少驾驶员的负担，降低人为失误导致的事故率。在自动驾驶车辆的行进过程中，预测障碍物的轨迹是提升自动驾驶车辆安全行驶的关键，而障碍物的航向角在预测障碍物的轨迹中起到重要的作用，因此，准确地确定障碍物的航向角尤为重要。

目前，主要由以下途径确定障碍物的航向角：第一种方案是，基于运动参数的航向角估计，即利用传感器获取障碍物的位置、速度等信息，据此估计障碍物的航向角，但传感器易受环境噪声影响，因此难以保证航向角的准确性；第二种方案是，基于OBB(OrientedBounding Box，有向包围盒)的航向角估计，即利用三维点云信息构建OBB来估计障碍物的航向角，但这种方法不考虑障碍物的运动信息，因此也难以保证航向角的准确性。

发明内容

有鉴于此，本发明的目的在于提供一种确定障碍物航向角的方法及装置，以提高获得的障碍物的航向角的准确性。

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

一方面，本发明提供一种确定障碍物航向角的方法，包括：

获取障碍物的运动状态信息，获取所述障碍物的点云簇；

根据所述障碍物的运动状态信息确定所述障碍物的航向角；

根据所述障碍物的航向角和预设对应关系确定所述障碍物的运动工况，其中，所述预设对应关系用于表征航向角和运动工况的对应关系；

根据所述障碍物的航向角、所述障碍物的运动工况和所述障碍物的点云簇，计算得到所述障碍物的最终航向角。

可选的，所述获取障碍物的运动状态信息，包括：

获取所述障碍物的第一初始运动状态信息和第二初始运动状态信息；其中，所述第一初始运动状态信息通过第一传感器获得，所述第二初始运动状态信息通过第二传感器获得，所述第一传感器和所述第二传感器为不同类型的传感器；

根据所述第一初始运动状态信息和所述第二初始运动状态信息对所述障碍物进行相似性判断；

在通过相似性判断的情况下，根据所述第一初始运动状态信息、所述第二初始运动状态信息、第一融合权重系数和第二融合权重系数确定所述障碍物的运动状态信息。

可选的，所述第一融合权重系数和所述第二融合权重系数通过如下方式确定，包括：

确定所述第一初始运动状态信息对应的第一协方差信息，确定所述第二初始运动状态信息对应的第二协方差信息；

计算所述第一协方差信息和所述第二协方差信息的协方差和值信息；

根据所述第二初始运动状态信息和所述协方差和值信息确定所述第一融合权重系数，根据所述第一初始运动状态信息和所述协方差和值信息确定所述第二融合权重系数。

可选的，所述根据所述障碍物的航向角和预设对应关系确定所述障碍物的运动工况，包括：

比较所述障碍物的航向角与预设的第一航向角阈值和第二航向角阈值，其中，所述第一航向角阈值小于所述第二航向角阈值；

如果所述障碍物的航向角的绝对值小于所述第一航向角阈值，且所述障碍物的横向坐标的绝对值小于预设的横向坐标阈值，则确定所述障碍物处于第一工况，如果所述障碍物的航向角的绝对值大于所述第二航向角阈值，则确定所述障碍物处于第二工况，否则，确定所述障碍物处于第三工况；

其中，所述第一工况表征本车传感器检测到的是所述障碍物的车头或车尾，所述第二工况表征本车传感器检测到的是所述障碍物的侧面，所述第三工况表征本车传感器既能检测到所述障碍物的车头或车尾，又能检测到所述障碍物的侧面。

可选的，所述根据所述障碍物的航向角、所述障碍物的运动工况和所述障碍物的点云簇，计算得到所述障碍物的最终航向角，包括：

根据所述障碍物的运动工况确定出所述障碍物的点云簇中的有效点和断点，其中，所述有效点为所述障碍物的点云簇中与本车存在碰撞风险的点，所述断点根据所述障碍物的点云簇中的点与本车的点云信息探测点的距离确定；

确定经过所述断点且角度位于所述障碍物的航向角的允许误差范围内的多条特征直线；

针对每条特征直线，计算所述有效点与该特征直线的垂直距离的和值；

根据所述障碍物的运动工况和各条特征直线的和值，确定出目标直线；

将所述目标直线的角度作为所述障碍物的最终航向角。

可选的，确定经过所述断点且角度位于所述障碍物的航向角的允许误差范围内的多条特征直线，包括：

根据所述障碍物的航向角和预设角度误差确定所述障碍物的航向角的允许误差范围；

在所述障碍物的航向角的允许误差范围内确定出多个特征角度值；

将经过所述断点且角度为不同特征角度值的多条直线确定为特征直线。

可选的，所述根据所述障碍物的运动工况确定出所述障碍物的点云簇中的有效点和断点，包括：

如果所述障碍物处于第一工况，则将所述障碍物的点云簇的所有点确定为所述有效点；确定所述障碍物的点云簇中的最小方位角点和最大方位角点，在所述障碍物的点云簇的所有点中，确定到所述最小方位角点和所述最大方位角点之间连线的垂直距离最大的点，将该点确定为所述断点；

如果所述障碍物处于第二工况，则将所述障碍物的点云簇中纵向坐标最小点确定为所述断点；将所述障碍物的点云簇中纵向坐标小于或等于所述断点的纵向坐标与纵向坐标阈值之和的点确定为所述有效点；

如果所述障碍物处于第三工况，则确定所述障碍物的点云簇中的最小方位角点和最大方位角点，在所述障碍物的点云簇的所有点中，确定到所述最小方位角点和所述最大方位角点之间连线的垂直距离最大的点，将该点确定为所述断点；当所述障碍物的航向角大于零时，将所述障碍物的点云簇中方位角大于所述断点的方位角的点确定为所述有效点，当所述障碍物的航向角小于零时，将所述障碍物的点云簇中方位角小于所述断点的方位角的点确定为有效点。

可选的，所述根据所述障碍物的运动工况和各条特征直线的和值，确定出目标直线，包括：

如果所述障碍物处于第一工况，则将和值最大的特征直线确定为所述目标直线；

如果所述障碍物处于第二工况或第三工况，则将和值最小的特征直线确定为所述目标直线。

可选的，所述根据所述障碍物的运动状态信息确定所述障碍物的航向角，包括：

根据公式

计算所述障碍物的航向角；

其中，ObjHA_motion为所述障碍物的航向角，ObjP_x为所述障碍物在车辆坐标系的x轴的位置，ObjP_y为所述障碍物在车辆坐标系的y轴的位置，ObjV_x为所述障碍物在车辆坐标系x轴的速度，ObjV_y为所述障碍物在车辆坐标系y轴的速度，EgoYR为本车的横摆角速度，EgoSpeed为本车的速度。

另一方面，本发明提供一种确定障碍物航向角的装置，包括：

信息获取单元，用于获取障碍物的运动状态信息，获取所述障碍物的点云簇；

初级航向角确定单元，用于根据所述障碍物的运动状态信息确定所述障碍物的航向角；

运动工况确定单元，根据所述障碍物的航向角和预设对应关系确定所述障碍物的运动工况，其中，所述预设对应关系用于表征航向角和运动工况的对应关系；

最终航向角确定单元，根据所述障碍物的航向角、所述障碍物的运动工况和所述障碍物的点云簇，计算得到所述障碍物的最终航向角。

可选的，所述信息获取单元包括：

信息获取子单元，用于获取所述障碍物的第一初始运动状态信息和第二初始运动状态信息；其中，所述第一初始运动状态信息通过第一传感器获得，所述第二初始运动状态信息通过第二传感器获得，所述第一传感器和所述第二传感器为不同类型的传感器；

相似性判断子单元，用于根据所述第一初始运动状态信息和所述第二初始运动状态信息对所述障碍物进行相似性判断；

运动状态信息子确定单元，用于在通过相似性判断的情况下，根据所述第一初始运动状态信息、所述第二初始运动状态信息、第一融合权重系数和第二融合权重系数确定所述障碍物的运动状态信息。

可选的，所述信息获取单元还包括：

融合权重系数确定子单元，用于确定所述第一初始运动状态信息对应的第一协方差信息，确定所述第二初始运动状态信息对应的第二协方差信息，计算所述第一协方差信息和所述第二协方差信息的协方差和值信息，根据所述第二初始运动状态信息和所述协方差和值信息确定所述第一融合权重系数，根据所述第一初始运动状态信息和所述协方差和值信息确定所述第二融合权重系数。

可选的，所述运动工况确定单元具体用于：

比较所述障碍物的航向角与预设的第一航向角阈值和第二航向角阈值，其中，所述第一航向角阈值小于所述第二航向角阈值；如果所述障碍物的航向角的绝对值小于所述第一航向角阈值，且所述障碍物的横向坐标的绝对值小于预设的横向坐标阈值，则确定所述障碍物处于第一工况，如果所述障碍物的航向角的绝对值大于所述第二航向角阈值，则确定所述障碍物处于第二工况，否则，确定所述障碍物处于第三工况；

其中，所述第一工况表征本车传感器检测到的是所述障碍物的车头或车尾，所述第二工况表征本车传感器检测到的是所述障碍物的侧面，所述第三工况表征本车传感器既能检测到所述障碍物的车头或车尾，又能检测到所述障碍物的侧面。

可选的，所述最终航向角确定单元包括：

特征点选择子单元，用于根据所述障碍物的运动工况确定出所述障碍物的点云簇中的有效点和断点，其中，所述有效点为所述障碍物的点云簇中与本车存在碰撞风险的点，所述断点根据所述障碍物的点云簇中的点与本车的点云信息探测点的距离确定；

特征直线确定子单元，用于确定经过所述断点且角度位于所述障碍物的航向角的允许误差范围内的多条特征直线；

和值计算子单元，用于针对每条特征直线，计算所述有效点与该特征直线的垂直距离的和值；

目标直线确定子单元，用于根据所述障碍物的运动工况和各条特征直线的和值，确定出目标直线；

最终航向角确定子单元，用于将所述目标直线的角度作为所述障碍物的最终航向角。

可选的，所述特征直线确定子单元具体用于：

根据所述障碍物的航向角和预设角度误差确定所述障碍物的航向角的允许误差范围；在所述障碍物的航向角的允许误差范围内确定出多个特征角度值；将经过所述断点且角度为不同特征角度值的多条直线确定为特征直线。

可选的，所述特征点选择子单元具体用于：

如果所述障碍物处于第一工况，则将所述障碍物的点云簇的所有点确定为所述有效点；确定所述障碍物的点云簇中的最小方位角点和最大方位角点，在所述障碍物的点云簇的所有点中，确定到所述最小方位角点和所述最大方位角点之间连线的垂直距离最大的点，将该点确定为所述断点；

如果所述障碍物处于第二工况，则将所述障碍物的点云簇中纵向坐标最小点确定为所述断点；将所述障碍物的点云簇中纵向坐标小于或等于所述断点的纵向坐标与纵向坐标阈值之和的点确定为所述有效点；

如果所述障碍物处于第三工况，则确定所述障碍物的点云簇中的最小方位角点和最大方位角点，在所述障碍物的点云簇的所有点中，确定到所述最小方位角点和所述最大方位角点之间连线的垂直距离最大的点，将该点确定为所述断点；当所述障碍物的航向角大于零时，将所述障碍物的点云簇中方位角大于所述断点的方位角的点确定为所述有效点，当所述障碍物的航向角小于零时，将所述障碍物的点云簇中方位角小于所述断点的方位角的点确定为有效点。

可选的，所述目标直线确定子单元具体用于：

如果所述障碍物处于第一工况，则将和值最大的特征直线确定为所述目标直线；如果所述障碍物处于第二工况或第三工况，则将和值最小的特征直线确定为所述目标直线。

可选的，所述初级航向角确定单元具体用于：

根据公式

计算所述障碍物的航向角；

其中，ObjHA_motion为所述障碍物的航向角，ObjP_x为所述障碍物在车辆坐标系的x轴的位置，ObjP_y为所述障碍物在车辆坐标系的y轴的位置，ObjV_x为所述障碍物在车辆坐标系x轴的速度，ObjV_y为所述障碍物在车辆坐标系y轴的速度，EgoYR为本车的横摆角速度，EgoSpeed为本车的速度。

本发明提供的确定障碍物航向角的方法及装置，获取障碍物的运动状态信息和点云簇，根据障碍物的运动状态信息计算障碍物的航向角，再根据该航向角确定障碍物的运动工况，根据障碍物的运动工况、障碍物的航向角以及障碍物的点云簇确定障碍物的最终航向角。本发明提供的确定障碍物航向角的方法，基于障碍物的运动状态信息初步确定障碍物的航向角，并根据该航向角确定障碍物的工况，再根据障碍物的工况、障碍物的航向角和障碍物的点云簇来确定障碍物的最终航向角，由于在确定障碍物的最终航向角的过程中，综合了障碍物的运动状态信息和点云簇，并结合障碍物的工况，因此能够提高最终获得的航向角的准确性。

附图说明

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

图1为本发明实施例公开的一种确定障碍物航向角的方法的流程图；

图2为本发明实施例公开的计算障碍物的最终航向角的方法的流程图；

图3为本发明实施例公开的确定障碍物的最终航向角的示意图；

图4为本发明实施例公开的获取障碍物的运动状态信息的方法的流程图；

图5为本发明实施例公开的一种确定障碍物航向角的装置的结构示意图。

具体实施方式

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

本发明实施例提供一种确定障碍物航向角的方法，以获得准确的障碍物的航向角。

参见图1，图1为本发明实施例公开的一种确定障碍物航向角的方法的流程图，该方法包括以下步骤：

步骤S1：获取障碍物的运动状态信息，获取该障碍物的点云簇。

其中，障碍物的运动状态信息是通过本车安装的传感器得到的。障碍物的点云簇是通过点云探测设备(如激光雷达)得到的。本车是指：实施本发明公开的技术方案的车辆。

需要说明的是，行车环境通常包括多个障碍物，通过点云探测设备得到的三维点云信息通常包含多个障碍物的点云信息，通过传感器得到多个障碍物的运动状态信息。本发明以行车环境中的一个车辆障碍物为例进行说明。

实施中，对获取到的障碍物的运动状态信息和三维点云信息进行时空对齐。其中，时空对齐包括时间域对齐和空间域对齐。时间域对齐是将运动状态信息和三维点云信息的时间戳对齐，空间域对齐是将运动状态信息和三维点云信息转换至同一坐标系下，例如，转换至本车的车辆坐标系下。对经过时空对齐的三维点云信息进行聚类，得到各个障碍物的点云簇，通过比较各个障碍物的点云簇和各个障碍物的运动状态信息，确定点云簇和运动状态信息之间的对应关系，从而获得同一个障碍物的运动状态信息和点云簇。

当然，在其他实施例中，也可以先确定障碍物的点云簇，再进行时空对齐，进而确定点云簇和运动状态信息之间的对应关系，从而获得同一个障碍物的运动状态信息和点云簇。

障碍物的运动状态信息包括但不限于障碍物的位置、速度及加速度等信息。例如，障碍物的运动状态信息包括：障碍物在车辆坐标系中的纵向位置、障碍物在车辆坐标系中的横向位置、障碍物在车辆坐标系中的纵向速度、障碍物在车辆坐标系中的横向速度、障碍物在车辆坐标系中的纵向加速度、障碍物在车辆坐标系中的横向加速度。

可以理解的是，本发明中的车辆坐标系是指本车的车辆坐标系。

步骤S2：根据障碍物的运动状态信息确定该障碍物的航向角。

可以理解的是，步骤S2是根据障碍物的运动状态信息初步确定的航向角，不是最终值，为了与后续的最终航向角进行区分，步骤S2中的航向角可以称为初级航向角。

步骤S3：根据障碍物的航向角和预设对应关系确定障碍物的运动工况。

其中，该预设对应关系用于表征航向角和运动工况的对应关系。障碍物的运动工况用于表征障碍物与本车之间的位置关系。

S4：根据障碍物的航向角、障碍物的运动工况和障碍物的点云簇，计算得到障碍物的最终航向角。

本发明提供的确定障碍物航向角的方法，获取障碍物的运动状态信息和点云簇，根据障碍物的运动状态信息计算障碍物的航向角，再根据该航向角确定障碍物的运动工况，根据障碍物的运动工况、障碍物的航向角以及障碍物的点云簇确定障碍物的最终航向角。本发明提供的确定障碍物航向角的方法，基于障碍物的运动状态信息初步确定障碍物的航向角，并根据该航向角确定障碍物的工况，再根据障碍物的工况、障碍物的航向角和障碍物的点云簇来确定障碍物的最终航向角，由于在确定障碍物的最终航向角的过程中，综合了障碍物的运动状态信息和点云簇，并结合障碍物的工况，因此能够提高最终获得的航向角的准确性。

在一个实施例中，障碍物的运动工况包括第一工况、第二工况和第三工况。其中，第一工况表征本车的传感器检测到的是障碍物的车头或车尾，第二工况表征本车的传感器检测到的是障碍物的侧面，第三工况表征本车的传感器既能检测到障碍物的车头或车尾，又能检测到障碍物的侧面。

在一个实施例中，根据障碍物的航向角和预设对应关系确定障碍物的运动工况，采用如下方案：

比较障碍物的航向角与预设的第一航向角阈值和第二航向角阈值；其中，第一航向角阈值小于第二航向角阈值；

如果障碍物的航向角的绝对值小于第一航向角阈值，且障碍物的横向坐标的绝对值小于预设的横向坐标阈值，则确定障碍物处于第一工况，如果障碍物的航向角的绝对值大于第二航向角阈值，则确定障碍物处于第二工况，否则，确定障碍物处于第三工况。

即：

如果满足|ObjHA_motion|＜Front_HA_thres，且|ObjY|＜Front_Y_thres，则确定障碍物处于第一工况；如果满足|ObjHA_motion|＞Cross_HA_thres，则确定障碍物处于第二工况；否则，确定障碍物处于第三工况。

其中，|ObjHA_motion|为障碍物的航向角的绝对值，Front_HA_thres为第一航向角阈值，Cross_HA_thres为第二航向角阈值，|ObjY|为障碍物的横向坐标的绝对值，Front_Y_thres为预设的横向坐标阈值。其中，在实际应用中，第一航向角阈值、第二航向角阈值、横向坐标阈值等阈值可通过标定或经验确定。

需要说明的是，第一工况表征本车的传感器检测到的是障碍物的车头或车尾，第二工况表征本车的传感器检测到的是障碍物的侧面，第三工况表征本车的传感器既能检测到障碍物的车头或车尾，又能检测到障碍物的侧面。

另外，第一工况可以称为车头车尾工况，第二工况可以称为横穿工况，第三工况可以称为斜交工况。

在一个实施例中，根据障碍物的航向角、障碍物的运动工况和障碍物的点云簇，计算得到障碍物的最终航向角，可以采用如图2所示的方法。包括：

步骤S401：根据障碍物的运动工况确定出障碍物的点云簇中的有效点和断点。

其中，有效点为障碍物的点云簇中与本车存在碰撞风险的点，断点根据障碍物的点云簇中的点与本车的点云信息探测点的距离确定。可选的，将障碍物的点云簇中与本车的点云信息探测点的距离最近的点作为断点。当然，断点的选择不限定于此，例如将障碍物的点云簇中与本车的点云信息探测点的距离次近的点作为断点。其中，点云信息探测点是指传感器中(如激光雷达)进行点云信息探测的部分，如收发天线所在位置。

步骤S402：确定经过断点且角度位于障碍物的航向角的允许误差范围内的多条特征直线。

可以理解的是，多条特征直线均经过断点，并且多条特征直线的角度均位于障碍物的航向角的允许误差范围内。

在一种可能的实现方式中：根据障碍物的航向角和预设角度误差确定障碍物的航向角的允许误差范围；在障碍物的航向角的允许误差范围内确定出多个特征角度值；将经过该断点且角度为不同特征角度值的多条直线确定为特征直线。其中，预设角度误差可根据经验或标定确定，通常情况下的参考因素是根据障碍物的运动状态信息确定障碍物的航向角时的实际误差范围。

例如，根据障碍物的运动状态信息确定出的障碍物的航向角为70°，预设角度误差为10°，那么障碍物的航向角的允许误差范围为60°至80°，在该范围内确定多个特征角度值，将经过该断点且角度为前述特征角度值的多条直线确定为特征直线。

也就是说，根据障碍物的运动状态信息初步确定障碍物的航向角之后，在该航向角的允许误差范围内进行离散，得到多个特征角度值，将经过断点且角度为各个特征角度值的直线作为特征直线。例如，特征角度值的数量为M，那么共得到M条特征直线。

可选的，按照预设的角度步长在障碍物的航向角的允许误差范围内确定多个特征角度值。这里以障碍物的航向角的允许误差范围为60°至80°为例，假如预设的角度步长为1°，那么多个特征角度值可以选择为60°、61°62°、…、79°、80°，当然，最小的特征角度值和最大的特征角度值并不限定于允许误差范围的两个端点值。

另外需要说明的是，特征角度值的取值并不限定为在障碍物的航向角的允许误差范围内均匀分布。例如，在距离障碍物的航向角的较近范围内，确定出数量更多的特征角度值。

步骤S403：针对每条特征直线，计算有效点与该特征直线的垂直距离的和值。

步骤S404：根据障碍物的运动工况和各条特征直线的和值，确定出目标直线。

步骤S405：将目标直线的角度作为障碍物的最终航向角。

在确定多条特征直线后，针对每一条特征直线，计算所有有效点到该特征直线的垂直距离的和值，再根据多条特征直线对应的和值确定出目标直线，该目标直线与障碍物的实际航向角最为接近。

本发明图2所示的方法，根据障碍物的运动工况确定出障碍物的点云簇中的有效点和断点，之后确定经过该断点且角度位于该障碍物的航向角的允许误差范围内的多条特征直线，针对每条特征直线，计算所有有效点到该特征直线的垂直距离的和值，之后结合障碍物的运动工况和多条特征直线的和值，就可以在多条特征直线中确定出与障碍物的实际航向角最为接近的目标直线，将该目标直线的角度作为障碍物的最终航向角。

需要说明的是：在实际应用中，确定最终航向角的方式并不限于图2所示实施例。例如，在其他实施例中，也可以根据障碍物的运动工况和障碍物的点云簇确定出一个航向角，将该航向角和初级航向角取平均或者利用加权的方式确定出最终航向角。

还需要说明的是，针对不同的运动工况，从点云簇中确定有效点和断点的方案，以及根据多条特征直线对应的和值，在多条直线中确定出目标直线的方案，是不同的。下面分别进行介绍。

1)、如果障碍物处于第一工况，则将障碍物的点云簇的所有点确定为有效点；确定障碍物的点云簇中的最小方位角点和最大方位角点，在障碍物的点云簇的所有点中，确定到该最小方位角点和该最大方位角点之间连线的垂直距离最大的点，将该点确定为断点。

也就是说，如果障碍物处于第一工况，那么断点通常为障碍物的车头中心点或者车尾中心点。

相应的，在多条特征直线中，将和值最大的特征直线确定为目标直线。

2)、如果障碍物处于第二工况，则将障碍物的点云簇中纵向坐标最小点确定为断点；将障碍物的点云簇中纵向坐标小于或等于该断点的纵向坐标与纵向坐标阈值之和的点确定为有效点。

相应的，在多条特征直线中，将和值最小的特征直线确定为目标直线。

3)、如果障碍物处于第三工况，则确定障碍物的点云簇中的最小方位角点和最大方位角点，在障碍物的点云簇的所有点中，确定到该最小方位角点和该最大方位角点之间连线的垂直距离最大的点，将该点确定为断点；当障碍物的航向角大于零时，将障碍物的点云簇中方位角大于该断点的方位角的点确定为有效点，当障碍物的航向角小于零时，将障碍物的点云簇中方位角小于该断点的方位角的点确定为有效点。

也就是说，如果障碍物处于第三工况，那么断点通常为障碍物的车尾和车身侧边的角点。

相应的，在多条特征直线中，将和值最小的特征直线确定为目标直线。

需要说明的是，障碍物的航向角是指障碍物的速度方向与本车的x轴正方向之间的夹角，逆时针为正，顺时针为负。

也就是说，在确定多条特征直线后，针对每一条特征直线，计算所有有效点到该条特征直线的垂直距离的和值。如果障碍物处于第一工况，那么将多条特征直线中和值最大的直线确定为目标直线，将该目标直线的角度作为障碍物的最终航向角；如果障碍物处于第二工况或者第三工况，那么将多条特征直线中和值最小的特征直线确定为目标直线，将该目标直线的角度作为障碍物的最终航向角。

需要说明的是：在实际使用中，上述各种工况下断点和有效点的确定并不限于此。例如，在其他实施例中，对于第一工况，也可以舍去一些点云簇中的点，如将位于点云簇的边缘点舍弃，将剩余的点作为有效点。另外，在其他实施例中，对于第一工况，也可以确定到最小方位角点和该最大方位角点之间连线的垂直距离次大的点，将该点确定为断点。对于第二工况，也可以将障碍物的点云簇中纵向坐标次小的点确定为断点；将障碍物的点云簇中纵向坐标小于或等于该断点的纵向坐标与纵向坐标阈值之和的点确定为有效点。对于第三工况，确定障碍物的点云簇中的最小方位角点和最大方位角点后，在障碍物的点云簇的所有点中，确定到该最小方位角点和该最大方位角点之间连线的垂直距离次大的点，将该点确定为断点。

当然，在其他实施例中，对于第一工况，也可以将多条特征直线中和值次大的直线确定为目标直线，将该目标直线的角度作为障碍物的最终航向角；对于处于第二工况或者第三工况，也可以将多条特征直线中和值次小的特征直线确定为目标直线，将该目标直线的角度作为障碍物的最终航向角。

图3示出了障碍物处于第三工况时，确定障碍物的最终航向角的示意图。在图3中，a为断点，b为最大方位角点，c为最小方位角点，L1为根据障碍物的运动状态信息确定出的初级航向角，L2为目标直线，指示障碍物的最终航向角。为了直观体现初步计算出的航向角与最终航向角之间的偏差，将L2向左侧平移得到L3，L3指示障碍物的最终航向角。

在一个实施例中，获取障碍物的运动状态信息，可以采用：通过安装于车辆上的传感器获得障碍物的运动状态信息。

例如，通过毫米波雷达获得障碍物的运动状态信息。

例如，通过摄像头采集障碍物的影像，通过分析障碍物的影像，确定障碍物的运动状态信息。

当然，还可以采用其他方案获取障碍物的运动状态信息。

在另一个实施例中，按照图4所示的方法获取障碍物的运动状态信息，该方法包括：

步骤S101：获取障碍物的第一初始状态信息和第二初始状态信息。

需要说明的是，该第一初始状态信息是通过第一传感器获得的障碍物的运动状态信息，该第二初始状态信息是通过第二传感器获得的障碍物的运动状态信息。例如，第一初始状态信息是基于毫米波雷达输出的数据所确定的障碍物的运动状态信息，第二初始状态信息是基于摄像头输出的数据所确定的障碍物的运动状态信息。当然，毫米波雷达和摄像头仅是一种举例。

步骤S102：根据第一初始状态信息和第二初始状态信息对障碍物进行相似性判断。

需要说明的是，对障碍物进行相似性判断是：确定通过第一传感器获得的运动状态信息和通过第二传感器获得的运动状态信息是否为同一障碍物的运动状态信息。

为了区分说明，以X_rad表示通过第一传感器(例如毫米波雷达)获得的障碍物的运动状态信息，以X_cam表示通过第二传感器(例如摄像头)获得的障碍物的运动状态信息，X_rad和X_cam均可以用如下矩阵表示：

X＝[x y v_x v_y a_x a_y]^T

其中，x表示障碍物在车辆坐标系中的纵向位置，y表示障碍物在车辆坐标系中的横向位置，v_x表示障碍物的纵向速度，v_y表示障碍物的横向速度，a_x表示障碍物的纵向加速度，a_y表示障碍物的横向加速度。

其中，车辆坐标系定义为：原点为车辆的后轴的中心，x轴的正方向为朝前、y轴的正方向为朝左、z轴的正方向为朝上。

在一种可能的实现方式中，基于第一传感器和第二传感器的多个历史帧信息对障碍物进行相似性判断：

其中，w_i为第i个时刻对应的权重系数，i＝1,2，…，N，N为正整数，X_rad,i为在第i个时刻通过第一传感器获得的障碍物的运动状态信息，X_cam,i为在第i个时刻通过第二传感器获得的障碍物的运动状态信息，当dist的值小于预设阈值时，可以确定第一传感器和第二传感器检测到的是同一个障碍物。

在另一种可能的实现方式中，将通过第一传感器获得的障碍物的位置和第二传感器获得的障碍物的位置进行比较，如果两者的位置差距处于预设范围内，则可以确定通过第一传感器和第二传感器检测到的是同一个障碍物。

步骤S103：在通过相似性判断的情况下，根据第一初始状态信息、第二初始状态信息、第一融合权重系数和第二融合权重系数确定障碍物的运动状态信息。如果未通过相似性判断，则结束流程。

在一个实施例中，确定第一融合权重系数和第二融合权重系数，具体为：

确定第一初始状态信息对应的第一协方差信息，确定第二初始状态信息对应的第二协方差信息；计算第一协方差信息和第二协方差信息的协方差和值信息；根据第二初始状态信息和该协方差和值信息确定第一融合权重系数，根据第一初始状态信息和该协方差和值信息确定第二融合权重系数。

为了方便进行介绍，我们用P_X表示对应于上文描述中X的协方差信息，P_X可以用如下矩阵表达式表示：

其中，σ_xx为障碍物的纵向位置的协方差，σ_xy为障碍物的纵向位置及障碍物的横向位置的协方差，…，矩阵内的其他元素以此类推，此处不一一说明。

一种可能的情况下，确定通过毫米波雷达获得的障碍物的运动状态信息X_rad对应的协方差信息，用P_X_rad表示，确定通过摄像头获得的障碍物的运动状态信息X_cam对应的协方差信息，我们用P_X_cam表示。

毫米波雷达对应的融合权重系数用W_rad表示，摄像头对应的融合权重系数用W_cam表示，可以由下式获得：

W_rad＝P_X_cam/(P_X_rad+P_X_cam)

W_cam＝P_X_rad/(P_X_rad+P_X_cam)

再根据以下公式确定障碍物的运动状态信息：

X_fus＝W_rad*X_rad+W_cam*X_cam

式中，X_fus表示通过毫米波雷达获取的障碍物的运动状态信息与通过摄像头获取的障碍物的运动状态信息进行融合的结果，即最终确定的障碍物的运动状态信息。

当然，在其他实施例中，第一融合权重系数和第二融合权重系数可以采用预先设定的经验值。

本发明图4所示的获取障碍物的运动状态信息的方法，通过第一传感器获得障碍物的第一初始状态信息，通过第二传感器获得障碍物的第二初始状态信息，并根据第一初始状态信息和第二状态信息进行相似性判断，如果通过相似性判断，则根据第一初始状态信息、第二初始状态信息、第一融合权重系数和第二融合权重系数确定障碍物的运动状态信息，也就是，对通过第一传感器和第二传感器获得的障碍物的运动状态信息进行融合，得到障碍物最终的运动状态信息。与通过一个传感器确定障碍物的运动状态信息相比，对通过多个传感器获得的障碍物的运动状态信息进行融合，最终得到的障碍物的运动状态信息更加准确。相应的，确定出的障碍物的最终航向角更加准确。

前文所述的根据障碍物的运动状态信息确定障碍物的航向角，下面进行介绍。

根据以下公式计算障碍物的航向角：

其中，ObjHA_motion表示障碍物的航向角，ObjP_x表示障碍物在车辆坐标系的x轴的位置，ObjP_y表示障碍物在车辆坐标系的y轴的位置，ObjV_x表示障碍物在车辆坐标系的x轴的速度，ObjV_y表示障碍物在车辆坐标系的y轴的速度，EgoYR表示本车的横摆角速度，EgoSpeed表示本车的速度。

优选的，ObjP_x、ObjP_y、ObjV_x、ObjV_y为：根据第一初始状态信息、第二初始状态信息、第一融合权重系数和第二融合权重系数确定的障碍物的运动状态信息。

本发明实施例公开了确定障碍物航向角的方法，相应的，本发明还公开确定障碍物航向角的装置，说明书中关于两者的描述可以相互参考。

参见图5，图5为本发明实施例公开的一种确定障碍物航向角的装置的结构示意图，该装置包括信息获取单元10、初级航向角确定单元20、运动工况确定单元30和最终航向角确定单元40。

其中：

信息获取单元10，用于获取障碍物的运动状态信息，获取所述障碍物的点云簇。

初级航向角确定单元20，用于根据所述障碍物的运动状态信息确定所述障碍物的航向角。

运动工况确定单元30，根据所述障碍物的航向角和预设对应关系确定所述障碍物的运动工况，其中，所述预设对应关系用于表征航向角和运动工况的对应关系。

最终航向角确定单元40，根据所述障碍物的航向角、所述障碍物的运动工况和所述障碍物的点云簇，计算得到所述障碍物的最终航向角。

可选的，信息获取单元10包括：

信息获取子单元，用于获取障碍物的第一初始运动状态信息和第二初始运动状态信息；其中，第一初始运动状态信息通过第一传感器获得，第二初始运动状态信息通过第二传感器获得，第一传感器和第二传感器为不同类型的传感器；

相似性判断子单元，用于根据第一初始运动状态信息和第二初始运动状态信息对障碍物进行相似性判断；

运动状态信息子确定单元，用于在通过相似性判断的情况下，根据第一初始运动状态信息、第二初始运动状态信息、第一融合权重系数和第二融合权重系数确定障碍物的运动状态信息。

可选的，信息获取单元10还包括：

融合权重系数确定子单元，用于确定第一初始运动状态信息对应的第一协方差信息，确定第二初始运动状态信息对应的第二协方差信息，计算第一协方差信息和第二协方差信息的协方差和值信息，根据第二初始运动状态信息和协方差和值信息确定第一融合权重系数，根据第一初始运动状态信息和协方差和值信息确定第二融合权重系数。

可选的，运动工况确定单元30具体用于：

比较障碍物的航向角与预设的第一航向角阈值和第二航向角阈值，其中，第一航向角阈值小于第二航向角阈值；如果障碍物的航向角的绝对值小于第一航向角阈值，且障碍物的横向坐标的绝对值小于预设的横向坐标阈值，则确定障碍物处于第一工况，如果障碍物的航向角的绝对值大于第二航向角阈值，则确定障碍物处于第二工况，否则，确定障碍物处于第三工况；

其中，第一工况表征本车传感器检测到的是障碍物的车头或车尾，第二工况表征本车传感器检测到的是障碍物的侧面，第三工况表征本车传感器既能检测到障碍物的车头或车尾，又能检测到障碍物的侧面。

可选的，最终航向角确定单元40包括：

特征点选择子单元，用于根据障碍物的运动工况确定出障碍物的点云簇中的有效点和断点，其中，有效点为障碍物的点云簇中与本车存在碰撞风险的点，断点根据障碍物的点云簇中的点与本车的点云信息探测点的距离确定；

特征直线确定子单元，用于确定经过断点且角度位于障碍物的航向角的允许误差范围内的多条特征直线；

和值计算子单元，用于针对每条特征直线，计算有效点与该特征直线的垂直距离的和值；

目标直线确定子单元，用于根据障碍物的运动工况和各条特征直线的和值，确定出目标直线；

最终航向角确定子单元，用于将目标直线的角度作为障碍物的最终航向角。

可选的，特征直线确定子单元具体用于：

根据障碍物的航向角和预设角度误差确定障碍物的航向角的允许误差范围；在障碍物的航向角的允许误差范围内确定出多个特征角度值；将经过断点且角度为不同特征角度值的多条直线确定为特征直线。

可选的，特征点选择子单元具体用于：

如果障碍物处于第一工况，则将障碍物的点云簇的所有点确定为有效点；确定障碍物的点云簇中的最小方位角点和最大方位角点，在障碍物的点云簇的所有点中，确定到最小方位角点和最大方位角点之间连线的垂直距离最大的点，将该点确定为断点；

如果障碍物处于第二工况，则将障碍物的点云簇中纵向坐标最小点确定为断点；将障碍物的点云簇中纵向坐标小于或等于断点的纵向坐标与纵向坐标阈值之和的点确定为有效点；

如果障碍物处于第三工况，则确定障碍物的点云簇中的最小方位角点和最大方位角点，在障碍物的点云簇的所有点中，确定到最小方位角点和最大方位角点之间连线的垂直距离最大的点，将该点确定为断点；当障碍物的航向角大于零时，将障碍物的点云簇中方位角大于断点的方位角的点确定为有效点，当障碍物的航向角小于零时，将障碍物的点云簇中方位角小于断点的方位角的点确定为有效点。

可选的，目标直线确定子单元具体用于：

如果障碍物处于第一工况，则将和值最大的特征直线确定为目标直线；如果障碍物处于第二工况或第三工况，则将和值最小的特征直线确定为目标直线。

可选的，在上述装置中，初级航向角确定单元20具体用于：

根据公式

计算障碍物的航向角；

其中，ObjHA_motion为障碍物的航向角，ObjP_x为障碍物在车辆坐标系的x轴的位置，ObjP_y为障碍物在车辆坐标系的y轴的位置，ObjV_x为障碍物在车辆坐标系x轴的速度，ObjV_y为障碍物在车辆坐标系y轴的速度，EgoYR为本车的横摆角速度，EgoSpeed为本车的速度。

最后，还需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

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

Claims (10)

1.一种确定障碍物航向角的方法，其特征在于，包括：

获取障碍物的运动状态信息，获取所述障碍物的点云簇；

根据所述障碍物的运动状态信息确定所述障碍物的航向角；

根据所述障碍物的航向角和预设对应关系确定所述障碍物的运动工况，其中，所述预设对应关系用于表征航向角和运动工况的对应关系；

根据所述障碍物的航向角、所述障碍物的运动工况和所述障碍物的点云簇，计算得到所述障碍物的最终航向角。

2.根据权利要求1所述的方法，其特征在于，所述获取障碍物的运动状态信息，包括：

获取所述障碍物的第一初始运动状态信息和第二初始运动状态信息；其中，所述第一初始运动状态信息通过第一传感器获得，所述第二初始运动状态信息通过第二传感器获得，所述第一传感器和所述第二传感器为不同类型的传感器；

根据所述第一初始运动状态信息和所述第二初始运动状态信息对所述障碍物进行相似性判断；

在通过相似性判断的情况下，根据所述第一初始运动状态信息、所述第二初始运动状态信息、第一融合权重系数和第二融合权重系数确定所述障碍物的运动状态信息。

3.根据权利要求2所述的方法，其特征在于，所述第一融合权重系数和所述第二融合权重系数通过如下方式确定，包括：

确定所述第一初始运动状态信息对应的第一协方差信息，确定所述第二初始运动状态信息对应的第二协方差信息；

计算所述第一协方差信息和所述第二协方差信息的协方差和值信息；

根据所述第二初始运动状态信息和所述协方差和值信息确定所述第一融合权重系数，根据所述第一初始运动状态信息和所述协方差和值信息确定所述第二融合权重系数。

4.根据权利要求1所述的方法，其特征在于，所述根据所述障碍物的航向角和预设对应关系确定所述障碍物的运动工况，包括：

比较所述障碍物的航向角与预设的第一航向角阈值和第二航向角阈值，其中，所述第一航向角阈值小于所述第二航向角阈值；

如果所述障碍物的航向角的绝对值小于所述第一航向角阈值，且所述障碍物的横向坐标的绝对值小于预设的横向坐标阈值，则确定所述障碍物处于第一工况，如果所述障碍物的航向角的绝对值大于所述第二航向角阈值，则确定所述障碍物处于第二工况，否则，确定所述障碍物处于第三工况；

其中，所述第一工况表征本车传感器检测到的是所述障碍物的车头或车尾，所述第二工况表征本车传感器检测到的是所述障碍物的侧面，所述第三工况表征本车传感器既能检测到所述障碍物的车头或车尾，又能检测到所述障碍物的侧面。

5.根据权利要求1至4中任一项所述的方法，其特征在于，所述根据所述障碍物的航向角、所述障碍物的运动工况和所述障碍物的点云簇，计算得到所述障碍物的最终航向角，包括：

根据所述障碍物的运动工况确定出所述障碍物的点云簇中的有效点和断点，其中，所述有效点为所述障碍物的点云簇中与本车存在碰撞风险的点，所述断点根据所述障碍物的点云簇中的点与本车的点云信息探测点的距离确定；

确定经过所述断点且角度位于所述障碍物的航向角的允许误差范围内的多条特征直线；

针对每条特征直线，计算所述有效点与该特征直线的垂直距离的和值；

根据所述障碍物的运动工况和各条特征直线的和值，确定出目标直线；

将所述目标直线的角度作为所述障碍物的最终航向角。

6.根据权利要求5所述的方法，其特征在于，确定经过所述断点且角度位于所述障碍物的航向角的允许误差范围内的多条特征直线，包括：

根据所述障碍物的航向角和预设角度误差确定所述障碍物的航向角的允许误差范围；

在所述障碍物的航向角的允许误差范围内确定出多个特征角度值；

将经过所述断点且角度为不同特征角度值的多条直线确定为特征直线。

7.根据权利要求5所述的方法，其特征在于，所述根据所述障碍物的运动工况确定出所述障碍物的点云簇中的有效点和断点，包括：

如果所述障碍物处于第一工况，则将所述障碍物的点云簇的所有点确定为所述有效点；确定所述障碍物的点云簇中的最小方位角点和最大方位角点，在所述障碍物的点云簇的所有点中，确定到所述最小方位角点和所述最大方位角点之间连线的垂直距离最大的点，将该点确定为所述断点；

如果所述障碍物处于第二工况，则将所述障碍物的点云簇中纵向坐标最小点确定为所述断点；将所述障碍物的点云簇中纵向坐标小于或等于所述断点的纵向坐标与纵向坐标阈值之和的点确定为所述有效点；

如果所述障碍物处于第三工况，则确定所述障碍物的点云簇中的最小方位角点和最大方位角点，在所述障碍物的点云簇的所有点中，确定到所述最小方位角点和所述最大方位角点之间连线的垂直距离最大的点，将该点确定为所述断点；当所述障碍物的航向角大于零时，将所述障碍物的点云簇中方位角大于所述断点的方位角的点确定为所述有效点，当所述障碍物的航向角小于零时，将所述障碍物的点云簇中方位角小于所述断点的方位角的点确定为有效点。

8.根据权利要求5所述的方法，其特征在于，所述根据所述障碍物的运动工况和各条特征直线的和值，确定出目标直线，包括：

如果所述障碍物处于第一工况，则将和值最大的特征直线确定为所述目标直线；

如果所述障碍物处于第二工况或第三工况，则将和值最小的特征直线确定为所述目标直线。

9.根据权利要求1所述的方法，其特征在于，所述根据所述障碍物的运动状态信息确定所述障碍物的航向角，包括：

根据公式

计算所述障碍物的航向角；

其中，ObjHA_motion为所述障碍物的航向角，ObjP_x为所述障碍物在车辆坐标系的x轴的位置，ObjP_y为所述障碍物在车辆坐标系的y轴的位置，ObjV_x为所述障碍物在车辆坐标系x轴的速度，ObjV_y为所述障碍物在车辆坐标系y轴的速度，EgoYR为本车的横摆角速度，EgoSpeed为本车的速度。

10.一种确定障碍物航向角的装置，其特征在于，包括：

信息获取单元，用于获取障碍物的运动状态信息，获取所述障碍物的点云簇；

初级航向角确定单元，用于根据所述障碍物的运动状态信息确定所述障碍物的航向角；

运动工况确定单元，根据所述障碍物的航向角和预设对应关系确定所述障碍物的运动工况，其中，所述预设对应关系用于表征航向角和运动工况的对应关系；

最终航向角确定单元，根据所述障碍物的航向角、所述障碍物的运动工况和所述障碍物的点云簇，计算得到所述障碍物的最终航向角。

Images