一种目标形状及航向估计方法及系统

摘要

本发明公开了一种目标形状及航向估计方法及系统，该方法包括：步骤S1，获取原始的目标点云数据，并聚类；步骤S2，通过三维边界框描述聚类点云数据；步骤S3，将点云的x与y坐标沿一设定方向旋转预设步长；步骤S4，以三维边界框对应在xoy平面的平面矩形框的中心为原点，建立二维直角坐标系，根据点云特征点所处的象限和聚类点云数据中点云分布呈现的形状，计算目标函数值；步骤S5，判断当前航向角度是否处于设定遍历角度范围内，如果是，则返回步骤S3；反之，则进入步骤S6；步骤S6，选择最大的目标函数值对应的航向角为最优航向角；步骤S7，根据最优航向角对应的平面矩形框的中心在激光雷达坐标系下的位置，确定目标的最优三维边界框的位置、尺寸及姿态。

权利要求

1.一种目标形状及航向估计方法，其特征在于，包括：步骤S1，获取原始的目标点云数据，并聚类，获得聚类点云数据；

步骤S2，通过三维边界框描述聚类点云数据；

步骤S3，将聚类点云数据中的每个点云的x与y坐标沿一设定方向旋转预设步长；

步骤S4，以三维边界框对应在xoy平面的平面矩形框的中心O1为原点，建立二维直角坐标系xO1y，形成四个象限，根据由式(2)描述的点云特征点O2(xc、yc)所处的象限，确定平面矩形框面积、点云到平面矩形框距离和及点数极值差，进而根据聚类点云数据中点云分布呈现的形状，计算目标函数值；

式中，xri为由步骤S3获得的旋转后第i个点云的x的坐标，yri为由步骤S3获得的旋转后第i个点云的y的坐标，n为聚类点云数据中点云的总数量；

步骤S5，判断当前航向角度是否处于设定遍历角度范围内，如果是，则返回步骤S3；反之，则进入步骤S6；航向角为目标的朝向与激光雷达坐标系的x轴之间的夹角；

步骤S6，选择最大的目标函数值对应的航向角，作为最优航向角；

步骤S7，根据最优航向角对应的平面矩形框的中心在激光雷达坐标系下的位置，确定目标的最优三维边界框的位置、尺寸及姿态。

2.如权利要求1所述的目标形状及航向估计方法，其特征在于，步骤S4中，目标函数值L在点云分布形状呈L型情形下通过式(17)计算，目标函数值L′在点云分布形状呈非L型情形下通过式(25)计算：

2

L＝(nd/S) /D            (17)L′＝1/(S*Dn)             (25)式中，nd为点数极值差，S为平面矩形框面积，D和Dn分别为点云分布形状呈L型、非L型的点云到平面矩形框距离和。

3.如权利要求2所述的目标形状及航向估计方法，其特征在于，步骤S4中，nd通过式(7)计算得到：

nd＝nmax‑nmin           (7)式中，nmax为点云特征点所在的象限的点云数量，nmin为与点云特征点所在的象限呈对角的象限的点云数量。

4.如权利要求2所述的目标形状及航向估计方法，其特征在于，步骤S4中，S通过式(6)计算得到：

S＝a*b               (6)a＝xmax‑xmin    (4)b＝ymax‑ymin    (5)式中，xmax为xri中的最大值，xmin为xri中的最小值，ymax为yri中的最大值，ymin为yri中的最小值。

5.如权利要求4所述的目标形状及航向估计方法，其特征在于，步骤S4中，Dn通过式(24)计算得到：

d′1i＝min{|xri‑xmax|，|yri‑ymax|}    (20)d′2i＝min{|xri‑xmax|，|yri‑ymin|}    (21)d′3i＝min{|xri‑xmin|，|yri‑ymin|}    (22)d′4i＝min{|xri‑xmin|，|yri‑ymax|}    (23)式中，d′1i为式(20)描述的第i个点云在第一象限时与平面矩形框的最近距离，d′2i为式(21)描述的第i个点云在第二象限时与平面矩形框的最近距离，d′3i为式(22)描述的第i个点云在第三象限时与平面矩形框的最近距离，d′4i为式(23)描述的第i个点云在第四象限时与平面矩形框的最近距离。

6.如权利要求2至4中任一项所述的目标形状及航向估计方法，其特征在于，步骤S4中，D通过式(16)计算得到：

式中，d1i为第i个点云在第一象限中与平面矩形框的最近距离，d2i为第i个点云在第二象限中与平面矩形框的最近距离，d3i为第i个点云在第三象限中与平面矩形框的最近距离，d4i为第i个点云在第四象限中与平面矩形框的最近距离。

7.如权利要求6所述的目标形状及航向估计方法，其特征在于，O2位于第一象限的情形下，d1i、d2i、d3i和d4i通过下式(8)和式(9)计算获得：d3i＝k*max{|xri‑xmin|，|yri‑ymin|}    (9)O2位于第二象限的情形下，d1i、d2i、d3i和d4i通过下式(10)和式(11)计算获得：d4i＝k*max{|xri‑xmin|，|yri‑ymax|}     (11)O2位于第三象限的情形下，d1i、d2i、d3i和d4i通过下式(12)和式(13)计算获得：d1i＝k*max{|xri‑xmax|，|yri‑ymax|}    (13)O2位于第四象限的情形下，d1i、d2i、d3i和d4i通过下式(14)和式(15)计算获得：d2i＝k*max{|xri‑xmax|，|yri‑ymin|}   (15)式中，xmax为xri中的最大值，xmin为xri中的最小值，ymax为yri中的最大值，ymin为yri中的最小值，k为惩罚系数，k＞1。

8.一种目标形状及航向估计系统，其特征在于，包括：数据解析模块，其用于获取原始的目标点云数据；

点云聚类模块，其用于对目标点云数据进行聚类，获得聚类点云数据；

目标形状及航向估计模块，其用于将聚类点云数据描述为三维边界框，并将聚类点云数据中的每个点云的x与y坐标沿一设定方向旋转预设步长，然后以三维边界框对应在xoy平面的平面矩形框的中心O1为原点，建立二维直角坐标系xO1y，形成四个象限，再根据由式(2)描述的点云特征点O2(xc、yc)所处的象限，确定平面矩形框面积、点云到平面矩形框距离和及点数极值差，进而根据聚类点云数据中点云分布呈现的形状计算目标函数值，再在设定遍历角度范围内选择最大目标函数值对应的航向角作为最优航向角，最后根据最优航向角对应的平面矩形框的中心在激光雷达坐标系下的位置，确定目标的最优三维边界框的位置、尺寸及姿态；

式中，xri为由所述目标形状及航向估计模块获得的旋转后第i个点云的x的坐标，yri为由步骤S3获得的旋转后第i个点云的y的坐标，n为聚类点云数据中点云的总数量，航向角为目标的朝向与激光雷达坐标系的x轴之间的夹角。

9.如权利要求8所述的目标形状及航向估计系统，其特征在于，目标函数值L在点云分布形状呈L型情形下通过式(17)计算，目标函数值L′在点云分布形状呈非L型情形下通过式(25)计算：

2

L＝(nd/S) /D           (17)L′＝1/(S*Dn)             (25)式中，nd为点数极值差，S为平面矩形框面积，D和Dn分别为点云分布形状呈L型、非L型的点云到平面矩形框距离和。

10.如权利要求9所述的目标形状及航向估计系统，其特征在于，nd通过式(7)计算，S通过式(6)计算，D通过式(16)计算，Dn通过式(24)计算得到；

nd＝nmax‑nmin     (7)S＝a*b           (6)a＝xmax‑xmin    (4)b＝ymax‑ymin    (5)式中，nmax为点云特征点所在的象限的点云数量，nmin为与点云特征点所在的象限呈对角的象限的点云数量，xmax为xri中的最大值，xmin为xri中的最小值，ymax为yri中的最大值，ymin为yri中的最小值，d′1i为点云分布形状呈非L型时第i个点云在第一象限时与平面矩形框的最近距离，d′2i为点云分布形状呈非L型时第i个点云在第二象限时与平面矩形框的最近距离，d′3i为点云分布形状呈非L型时第i个点云在第三象限时与平面矩形框的最近距离，d′4i为点云分布形状呈非L型时第i个点云在第四象限时与平面矩形框的最近距离，d1i为点云分布形状呈L型时第i个点云在第一象限中与平面矩形框的最近距离，d2i为点云分布形状呈L型时第i个点云在第二象限中与平面矩形框的最近距离，d3i为点云分布形状呈L型时第i个点云在第三象限中与平面矩形框的最近距离，d4i为点云分布形状呈L型时第i个点云在第四象限中与平面矩形框的最近距离。

说明书

一种目标形状及航向估计方法及系统

技术领域

[0001] 本发明涉及自动驾驶感知技术领域，特别是关于一种目标形状及航向估计方法及系统。

背景技术

[0002] 激光雷达是感知系统的重要的传感器之一，具有高精度、高分辨率的主动测距测角能力，对激光雷达采集到的点云数据进行处理，可以实现对智能汽车周围环境的目标检
测、跟踪及识别等功能。目标形状估计是目标检测步骤中重要的一个环节，良好的形状估计
结果，可以为后续的跟踪、预测环节提供准确的目标信息，从而提高智能汽车环境感知能
力。目标形状信息包括目标的长宽高、位置及航向角等信息。

[0003] 现有的技术中，激光雷达只能观测其视角下的可见轮廓部分对应的点云数据，再基于观测点云建立模型，例如L型拟合三维边界框、关键点拟合边界框。激光点云形成的障
碍物数据中L型点云是较多的一类。综合上述，现有的智能汽车目标形状估计方法受到激光
雷达的点云稀疏性及噪点等不确定性因素的影响，从而对L型点云拟合出的三维边界框的
尺寸、航向的准确度较差。

发明内容

[0004] 本发明的目的在于提供一种目标形状及航向估计方法及系统来克服或至少减轻现有技术的上述缺陷中的至少一个。

[0005] 为实现上述目的，本发明提供一种目标形状及航向估计方法，该方法包括：

[0006] 步骤S1，获取原始的目标点云数据，并聚类，获得聚类点云数据；

[0007] 步骤S2，通过三维边界框描述聚类点云数据；

[0008] 步骤S3，将聚类点云数据中的每个点云的x与y坐标沿一设定方向旋转预设步长；

[0009] 步骤S4，以三维边界框对应在xoy平面的平面矩形框的中心O1为原点，建立二维直角坐标系xO1y，形成四个象限，根据由式(2)描述的点云特征点O2(xc、yc)所处的象限，确定
平面矩形框面积、点云到平面矩形框距离和及点数极值差，进而根据聚类点云数据中点云
分布呈现的形状，计算目标函数值；

[0010]

[0011] 式中，xri为由步骤S3获得的旋转后第i个点云的x的坐标，yri为由步骤S3获得的旋转后第i个点云的y的坐标，n为聚类点云数据中点云的总数量；

[0012] 步骤S5，判断当前航向角度是否处于设定遍历角度范围内，如果是，则返回步骤S3；反之，则进入步骤S6；航向角为目标的朝向与激光雷达坐标系的x轴之间的夹角；

[0013] 步骤S6，选择最大的目标函数值对应的航向角，作为最优航向角；

[0014] 步骤S7，根据最优航向角对应的平面矩形框的中心在激光雷达坐标系下的位置，确定目标的最优三维边界框的位置、尺寸及姿态。

[0015] 进一步地，步骤S4中，目标函数值L在点云分布形状呈L型情形下通过式(17)计算，目标函数值L′在点云分布形状呈非L型情形下通过式(25)计算：

[0016] L＝(nd/S)2/D                    (17)

[0017] L′＝1/(S*Dn)                    (25)

[0018] 式中，nd为点数极值差，S为平面矩形框面积，D和Dn分别为点云分布形状呈L型、非L型的点云到平面矩形框距离和。

[0019] 进一步地，步骤S4中，nd通过式(7)计算得到：

[0020] nd＝nmax‑nmin                    (7)

[0021] 式中，nmax为点云特征点所在的象限的点云数量，nmin为与点云特征点所在的象限呈对角的象限的点云数量。

[0022] 进一步地，步骤S4中，S通过式(6)计算得到：

[0023] S＝a*b                          (6)

[0024] a＝xmax‑xmin                    (4)

[0025] b＝ymax‑ymin                     (5)

[0026] 式中，xmax为xri中的最大值，xmin为xri中的最小值，xmax为yri中的最大值，ymin为yri中的最小值。

[0027] 进一步地，步骤S4中，Dn通过式(24)计算得到：

[0028]

[0029] d′1i＝min{|xri‑xmax|，|yri‑ymax|}              (20)

[0030] d′2i＝min{|xri‑xmax|，|yri‑ymin|}              (21)

[0031] d′3i＝min{|xri‑xmin|，|yri‑ymin|}              (22)

[0032] d′4i＝min{|xri‑xmin|，|yri‑ymax|}              (23)

[0033] 式中，d′1i为式(20)描述的第i个点云在第一象限时与平面矩形框的最近距离，d′2i为式(21)描述的第i个点云在第二象限时与平面矩形框的最近距离，d′3i为式(22)描述
的第i个点云在第三象限时与平面矩形框的最近距离，d′4i为式(23)描述的第i个点云在第
四象限时与平面矩形框的最近距离。

[0034] 进一步地，步骤S4中，D通过式(16)计算得到：

[0035]

[0036] 式中，d1i为第i个点云在第一象限中与平面矩形框的最近距离，d2i为第i个点云在第二象限中与平面矩形框的最近距离，d3i为第i个点云在第三象限中与平面矩形框的最近
距离，d4i为第i个点云在第四象限中与平面矩形框的最近距离。

[0037] 进一步地，O2位于第一象限的情形下，d1i、d2i、d3i和d4i通过下式(8)和式(9)计算获得：

[0038]

[0039] d3i＝k*max{|xri‑xmin|，|yri‑ymin|}         (9)

[0040] O2位于第二象限的情形下，d1i、d2i、d3i和d4i通过下式(10)和式(11)计算获得：

[0041]

[0042] d4i＝k*max{|xri‑xmin|，|yri‑ymax|}         (11)

[0043] O2位于第三象限的情形下，d1i、d2i、d3i和d4i通过下式(12)和式(13)计算获得：

[0044]

[0045] d1i＝k*max{|xri‑xmax|，|yri‑ymax|}         (13)

[0046] O2位于第四象限的情形下，d1i、d2i、d3i和d4i通过下式(14)和式(15)计算获得：

[0047]

[0048] d2i＝k*max{|xri‑xmax|，|yri‑ymin|}         (15)。

[0049] 本发明还提供一种目标形状及航向估计系统，该系统包括：

[0050] 数据解析模块，其用于获取原始的目标点云数据；

[0051] 点云聚类模块，其用于对目标点云数据进行聚类，获得聚类点云数据；

[0052] 目标形状及航向估计模块，其用于将聚类点云数据描述为三维边界框，并将聚类点云数据中的每个点云的x与y坐标沿一设定方向旋转预设步长，然后以三维边界框对应在
xoy平面的平面矩形框的中心O1为原点，建立二维直角坐标系xO1y，形成四个象限，再根据
由式(2)描述的点云特征点O2(xc、yc)所处的象限，确定平面矩形框面积、点云到平面矩形框
距离和及点数极值差，进而根据聚类点云数据中点云分布呈现的形状计算目标函数值，再
在设定遍历角度范围内选择最大目标函数值对应的航向角作为最优航向角，最后根据最优
航向角对应的平面矩形框的中心在激光雷达坐标系下的位置，确定目标的最优三维边界框
的位置、尺寸及姿态；

[0053]

[0054] 式中，xri为由步骤S3获得的旋转后第i个点云的x的坐标，yri为由步骤S3获得的旋转后第i个点云的y的坐标，n为聚类点云数据中点云的总数量，航向角为目标的朝向与激光
雷达坐标系的x轴之间的夹角。

[0055] 进一步地，目标函数值L在点云分布形状呈L型情形下通过式(17)计算，目标函数值L′在点云分布形状呈非L型情形下通过式(25)计算：

[0056] L＝(nd/S)2/D                    (17)

[0057] L′＝1/(S*Dn)                    (25)

[0058] 式中，nd为点数极值差，S为平面矩形框面积，D和Dn分别为点云分布形状呈L型、非L型的点云到平面矩形框距离和。

[0059] 进一步地，nd通过式(7)计算，S通过式(6)计算，D通过式(16)计算，Dn通过式(24)计算得到；

[0060] nd＝nmax‑nmin                       (7)

[0061] S＝a*b                              (6)

[0062] a＝xmax‑xmin                        (4)

[0063] b＝ymax‑ymin                        (5)

[0064]

[0065]

[0066] 式中，nmax为点云特征点所在的象限的点云数量，nmin为与点云特征点所在的象限呈对角的象限的点云数量，xmax为xri中的最大值，xmin为xri中的最小值，xmax为yri中的最大
值，ymin为yri中的最小值，d′1i为式(20)描述的第i个点云在第一象限时与平面矩形框的最
近距离，d′2i为式(21)描述的第i个点云在第二象限时与平面矩形框的最近距离，d′3i为式
(22)描述的第i个点云在第三象限时与平面矩形框的最近距离，d′4i为式(23)描述的第i个
点云在第四象限时与平面矩形框的最近距离，d1i为第i个点云在第一象限中与平面矩形框
的最近距离，d2i为第i个点云在第二象限中与平面矩形框的最近距离，d3i为第i个点云在第
三象限中与平面矩形框的最近距离，d4i为第i个点云在第四象限中与平面矩形框的最近距
离。

[0067] 本发明由于采用上述技术方案，具有如下优点：

[0068] 其一，点云数据具有遮挡性，因此在距离激光雷达最近表面点云最多，利用该特性设计对应目标函数，从而得到更优的边界框结果。

[0069] 其二，利用遍历搜索平面矩形框的方法，将平面矩形框拟合转为优化问题，并可以选择增大遍历步长，从而达到计算加速的效果。

附图说明

[0070] 图1为本发明实施例提供的目标形状及航向估计方法的流程图；

[0071] 图2为本发明实施例提供的坐标系定义图；

[0072] 图3为本发明实施例提供的航向角遍历过程示意图；

[0073] 图4为本发明实施例提供的平面矩形框点云坐标系划分图，a是点云特征点位于第一象限，b点云特征点位于第二象限，c点云特征点位于第三象限，d点云特征点位于第四象
限；

[0074] 图5为本发明实施例提供的平面矩形框点云坐标系划分图，其中点云非L型分布情况；

[0075] 图6为本发明实施例提供的目标形状及航向估计系统图。

具体实施方式

[0076] 在附图中，使用相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面结合附图对本发明的实施例进行详细说明。

[0077] 在本发明的描述中，术语“中心”、“纵向”、“横向”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关
系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有
特定的方位、以特定的方位构造和操作，因此不能理解为对本发明保护范围的限制。

[0078] 本发明实施例提供的目标形状及航向估计方法包括：

[0079] 步骤S1，获取原始的目标点云数据，并聚类，获得聚类点云数据。

[0080] 其中，聚类方法不局限于欧氏聚类或非欧式聚类方法，本发明实施例仅对聚类后的目标点云进行处理，因此不再描述获得聚类点云数据的过程。

[0081] 步骤S2，通过三维边界框描述聚类点云数据。

[0082] 其中，聚类点云数据中包括目标在激光雷达扫描到其表面后反射的三维空间的点，且每个聚类点云数据可以通过在激光坐标系中的三维边界框进行描述，因此，三维边界
框的x、y、z的坐标值对应为目标点云数据的x、y、z的坐标值。例如：激光雷达安装在车辆上
时，激光坐标系三轴的定义为：x轴正方向为车辆正前方，y轴正方向为车头的正右手方向，z
轴正方向为垂直车辆向上的方向，即车辆高度方向。那么，航向角θ为目标的朝向与x轴之间
的夹角，θ0表示航向角的初值，例如0°。

[0083] 如图2所示，图2中的三维边界框描述的是聚类点云数据。黑色圆点表征为聚类点云数据中的点云，激光雷达坐标系的原点O为激光雷达所在位置，a用于描述聚类点云数据
沿x轴的尺寸，b用于描述聚类点云数据沿y轴的尺寸，c用于描述聚类点云数据沿z轴的尺
寸。

[0084] 步骤S3，将聚类点云数据中的每个点云的x与y坐标沿一设定方向旋转预设步长Δθ。例如：Δθ＝1°。图3示出的是沿逆时针方向旋转。然而也可以沿顺时针方向旋转。

[0085] 例如：聚类点云数据中的点云总数量为n，第i个点云的x与y坐标为(xi、yi)，那么，第i个点云经由步骤S3旋转后的x与y坐标为(xri、yri)，则对应关系表示为式(1)，θ为当前航
向角：

[0086]

[0087] 步骤S4，如图4所示，以三维边界框对应在xoy平面的平面矩形框的中心O1为原点，建立二维直角坐标系xO1y，形成四个象限，根据由式(2)描述的点云特征点O2(xc、yc)所处的
象限，确定平面矩形框面积S、点云到平面矩形框距离和D及点数极值差nd，进而根据聚类点
云数据中点云分布呈现的形状，计算目标函数值L。

[0088]

[0089] 式中，xri为由步骤S3获得的旋转后第i个点云的x的坐标，yri为由步骤S3获得的旋转后第i个点云的y的坐标，n为聚类点云数据中点云的总数量。

[0090] 其中，O1(xac，yac)可以利用下式(3)‑式(6)计算：

[0091]

[0092] 式中，xmax为xri中的最大值，xmin为xri中的最小值，ymax为yri中的最大值，ymin为yri中的最小值。

[0093] 结合图2，三维边界框的a和b通过下式(4)和式(5)计算：

[0094] a＝xmax‑xmin                        (4)

[0095] b＝ymax‑ymin                        (5)

[0096] 平面矩形框面积S则可以通过式(6)计算得到：

[0097] S＝a*b                         (6)

[0098] 结合参考图1，由于点云的遮挡性，道路上普遍为刚性封闭表面目标(也可以称之为障碍物)，因此原始点云数据中点云的分布所呈现的形状特征大致为L型。下面首先以L型
的点云分布形状为例，说明步骤S4的实现方式。

[0099] 如图4所示，根据平面矩形框中心O1(xac，yac)与点云特征点O2(xc、yc)的位置关系对点云分为如下四种情况：

[0100] 其中黑色圆点为点云，黑色方块为计算得出的O2。图4中的a示意的是O2位于xO1y的第一象限、图4中的b示意的是02位于xO1y的第二象限、图4中的c示意的是O2位于xO1y的
第三象限，图4中的d示意的是O2位于xO1y的第四象限。O2所在的象限为点云数量最多的象
限，而与其相对的象限，即为点云数量最少的象限，则计算象限中点云数量最大值与点云数
量最小值之间的差值，即式(7)表示的点数极值差nd：

[0101] nd＝nmax‑nmin                  (7)

[0102] 其中，nmax为点云特征点所在象限的点云总点数，nmin为点云特征点相对的象限中点云总点数。

[0103] 由于点云分布在四种情况下不同，因此，计算点云距离平面矩形框和D的方法有差异，下面进行分情况阐明：

[0104] 第一种情况，如图4中的a所示，O2位于xO1y的第一象限：为了获得更贴近各个点云的平面矩形框，因此计算第i个点云与平面矩形框最近的一条边的距离(以下简称平面矩形
框的最近距离)。由于第i个点云可能位于第一象限至第四象限中的任意一个象限中，因此
对于不同象限指定不同的规则，在本情况中大部分点云分布在第一象限、第二象限、第四象
限，因此对于这三个象限指定如下的计算规则，计算公式如下式(8)所示：

[0105]

[0106] 其中，d1i为第i个点云在第一象限中与平面矩形框的最近距离，d2i为第i个点云在第二象限中与平面矩形框的最近距离，d4i为第i个点云在第四象限中与平面矩形框的最近
距离。

[0107] 对于第三象限中，由于在合理的平面矩形框坐标划分下，此象限中的点云数量最少，因此为了让点云数量在此象限尽可能的少，对在此象限的点云计算平面矩形框距离时
采用惩罚策略，选择计算该象限中点云与平面矩形框最远一条边的距离，由此来增加其数
值在点云到平面矩形框距离和中的占比，同时通过惩罚系数放大该数值，首先设定惩罚系
数k(k＞1)，k的取值应大于1，本发明中k＝2，但不限于数值为2。因此第三象限中的点云要
计算其中与最远的一条边的距离的计算公式如下式(9)：

[0108] d3i＝k*max{|xri‑xmin|，|yri‑ymin|}      (9)

[0109] 第二种情况，如图4中的b所示，O2位于xO1y的第二象限：在本情况中大部分点云分布在第一象限、第二象限、第三象限，因此对于这三个象限指定如下的计算规则，计算公式
如下式(10)：

[0110]

[0111] 其中，d1i为第i个点云在第一象限中与平面矩形框的最近距离，d2i为第i个点云在第二象限中与平面矩形框的最近距离，d3i为第i个点云在第三象限中与平面矩形框的最近
距离。

[0112] 对于第四象限中，此象限中的点云数量最少，对在此象限的点云计算平面矩形框距离时采用惩罚策略，选择计算该象限中点云与平面矩形框最远一条边的距离，同时通过
惩罚系数放大数值，设定惩罚系数k＝2，但不限于数值为2，由此来增加其数值在点云到平
面矩形框距离和中的占比。计算公式如下式(11)：

[0113] d4i＝k*max{|xri‑xmin|，|yri‑ymax|}         (11)

[0114] 第三种情况，如图4中的c所示，O2位于xO1y的第三象限：在本情况中大部分点云分布在第二象限、第三象限、第四象限，因此对于这三个象限指定如下的计算规则，计算公式
如下式(12)：

[0115]

[0116] 其中，d2i为第i个点云在第二象限中与平面矩形框的最近距离，d3i为第i个点云在第三象限中与平面矩形框的最近距离，d4i为第i个点云在第四象限中与平面矩形框的最近
距离。

[0117] 对于第一象限中，此象限中的点云数量最少，对在此象限的点云计算平面矩形框距离时采用惩罚策略，选择计算该象限中点云与平面矩形框最远一条边的距离，同时通过
惩罚系数放大数值，设定惩罚系数k＝2，但不限于数值为2，由此来增加其数值在点云到平
面矩形框距离和中的占比。计算公式如下式(13)：

[0118] d1i＝k*max{|xri‑xmax|，|yri‑ymax|}         (13)

[0119] 第四种情况，如图4中的d所示，O2位于xO1y的第四象限：在本情况中大部分点云分布在第一象限、第三象限、第四象限，因此对于这三个象限指定如下的计算规则，计算公式
如下式(14)：

[0120]

[0121] 其中，d1i为第i个点云在第二象限中与平面矩形框的最近距离，d3i为第i个点云在第三象限中与平面矩形框的最近距离，d4i为第i个点云在第四象限中与平面矩形框的最近
距离。

[0122] 对于第二象限中，此象限中的点云数量最少，对在此象限的点云计算平面矩形框距离时采用惩罚策略，选择计算该象限中点云与平面矩形框最远一条边的距离，同时通过
惩罚系数放大数值，设定惩罚系数k＝2，但不限于数值为2，由此来增加其数值在点云到平
面矩形框距离和中的占比。计算公式如下式(15)：

[0123] d2i＝k*max{|xri‑xmax|，|yri‑ymin|}             (15)

[0124] 然后综合上述四种情况，可以得到不同点云分布下的d1i、d2i、d3i及d4i，由此来计算点云到平面矩形框的距离和D，不同情况的计算公式均为如下式(16)所示：

[0125]

[0126] 然后根据平面矩形框面积S、点云到平面矩形框距离和D及点数极值差nd，利用三个指标设计的目标函数L，L可以采用下式(17)计算得到：

[0127] L＝(nd/S)2/D                    (17)

[0128] 该目标函数的数值越大说明平面矩形框越合理即受三个因素影响，平面矩形框面积S越小越好，点云到平面矩形框距离和D越小越好，点数极值差nd越大越好。计算完毕目标
函数值L后，将当前遍历的航向角θ与目标函数值L进行记录。

[0129] 当然，也可以将式(17)变形后，来计算L。比如，将式(17)的2次方改成1次方或3次方或其它次方均可。

[0130] 步骤S5，判断当前航向角度是否处于设定遍历角度范围内，如果是，则返回步骤S3；反之，则进入步骤S6。其中，设定遍历角度范围为[‑180°，180°)或(‑180°，180°]。

[0131] 步骤S6，选择数值最大L对应的航向角，作为最优航向角θ′。

[0132] 步骤S7，根据最优航向角θ′对应的平面矩形框的中心在激光雷达坐标系下的位置，确定目标的最优三维边界框的位置、尺寸及姿态。

[0133] 最优航向角θ′旋转后对应的平面矩形框中心坐标(x′ac，y′ac)如下式(18)所示：

[0134]

[0135] 其中，目标点云旋转最优航向角θ′角度后的坐标值中x的最大值为x′max、x的最小值x′min、y的最大值y′max与y的最小值y′min。

[0136] 那么在激光雷达坐标系下三维边界框的中心点坐标(x，y，z)及三轴上的尺寸(a、b、c)的计算公式如下式(19)所示，从而确定该物体的最优边界框的位置、尺寸及姿态。

[0137]

[0138] 在另外一个实施例中，针对于树丛等中心具有空洞的目标或位于激光雷达正前方的车辆等目标，激光雷达获取的目标点云分布为非L型，说明步骤S4的实现方式。

[0139] xO1y划分图如图5所示，点云会分布在第一象限至第四象限中的其中任意一个，为了获得更贴近各个点云的平面矩形框，因此计算第i个点云与其中与最近的一条边的距离，
对于点云分布在四个象限下的不同，计算点云距离平面矩形框之和D的方法有差异，下面进
行分四种情况阐明：

[0140] a.点云坐标位于第一象限，其中第i个点云在第一象限时与平面矩形框的最近距离d′1i的计算公式如下式(20)：

[0141] d′1i＝min{|xri‑xmax|，|yri‑ymax|}         (20)

[0142] b.点云坐标位于第二象限，其中第i个点云在第二象限时与平面矩形框的最近距离d′2i的计算公式如下式(21)：

[0143] d′2i＝min{|xrixmax|，|yriymin|}          (21)

[0144] c.点云坐标位于第三象限，其中第i个点云在第三象限时与平面矩形框的最近距离d′3i的计算公式如下式(22)：

[0145] d′3i＝min{|xrixmin|，|yriymin|}          (22)

[0146] d.点云坐标位于第四象限，其中第i个点云在第四象限时与平面矩形框的最近距离d′4i的计算公式如下式(23)：

[0147] d′4i＝min{|xrixmin|，|yri‑ymax|}       (23)

[0148] 综合上述四种情况，可以得到不同点云位于不同象限下的d′1i、d′2i、d′3i及d′4i，由此来计算点到平面矩形框的最近距离和Dn，不同情况的计算公式均为如下式(24)所示：

[0149]

[0150] 然后利用平面矩形框面积S及点到平面矩形框的最近距离和Dn设计目标函数L′，L′可以通过下式(25)计算：

[0151] L′＝1/(S*Dn)                   (25)

[0152] L′的数值越大，说明平面矩形框越合理，平面矩形框面积S越小越好，点到平面矩形框的最近距离和Dn越小越好。计算完毕目标函数的值后，将当前遍历的航向角θ与目标函
数值L进行记录。

[0153] 综上所述，本发明对三维点云形状估计方法描述，适用于所有采集的三维点云数据，因此，所有关于三维点云形状估计方法在不脱离本发明的总体构思下的变化和修改，应
属于本发明的保护范围之内。

[0154] 如图6所示，本发明实施例还提供一种目标形状及航向估计系统，该系统包括：

[0155] 数据解析模块，其用于获取原始的目标点云数据；

[0156] 点云聚类模块，其用于对目标点云数据进行聚类，获得聚类点云数据；

[0157] 目标形状及航向估计模块，其用于将聚类点云数据描述为三维边界框，并将聚类点云数据中的每个点云的x与y坐标沿一设定方向旋转预设步长，然后以三维边界框对应在
xoy平面的平面矩形框的中心O1为原点，建立二维直角坐标系xO1y，形成四个象限，再根据
由式(2)描述的点云特征点O2(xc、yc)所处的象限，确定平面矩形框面积、点云到平面矩形框
距离和及点数极值差，进而根据聚类点云数据中点云分布呈现的形状计算目标函数值L，再
在设定遍历角度范围内选择最大L对应的航向角作为最优航向角，最后根据最优航向角对
应的平面矩形框的中心在激光雷达坐标系下的位置，确定目标的最优三维边界框的位置、
尺寸及姿态。

[0158] 在一个实施例中，目标函数值L在点云分布形状呈L型情形下通过式(17)计算，目标函数值L′在点云分布形状呈非L型情形下通过式(25)计算。

[0159] 在一个实施例中，nd通过式(7)计算，S通过式(6)计算，D通过式(16)计算，Dn通过式(24)计算得到。

[0160] 在车辆运行过程中，车载端配置的激光雷达对周围道路环境采集激光雷达点云数据，数据解析模块用于获取激光雷达采取的点云信息，将点云信息输入至点云聚类模块，对
于聚类后的目标点云进行目标形状及航向估计，计算并输出所有目标的边界框的位置、尺
寸及航向信息，从而该系统实现上述目标形状及航向估计方法的步骤。

[0161] 最后需要指出的是：以上实施例仅用以说明本发明的技术方案，而非对其限制。本领域的普通技术人员应当理解：可以对前述各实施例所记载的技术方案进行修改，或者对
其中部分技术特征进行等同替换；这些修改或者替换，并不使相应技术方案的本质脱离本
发明各实施例技术方案的精神和范围。