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

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


背景技术：

2.激光雷达是感知系统的重要的传感器之一，具有高精度、高分辨率的主动测距测角能力，对激光雷达采集到的点云数据进行处理，可以实现对智能汽车周围环境的目标检测、跟踪及识别等功能。目标形状估计是目标检测步骤中重要的一个环节，良好的形状估计结果，可以为后续的跟踪、预测环节提供准确的目标信息，从而提高智能汽车环境感知能力。目标形状信息包括目标的长宽高、位置及航向角等信息。
3.现有的技术中，激光雷达只能观测其视角下的可见轮廓部分对应的点云数据，再基于观测点云建立模型，例如l型拟合三维边界框、关键点拟合边界框。激光点云形成的障碍物数据中l型点云是较多的一类。综合上述，现有的智能汽车目标形状估计方法受到激光雷达的点云稀疏性及噪点等不确定性因素的影响，从而对l型点云拟合出的三维边界框的尺寸、航向的准确度较差。


技术实现要素：

4.本发明的目的在于提供一种目标形状及航向估计方法及系统来克服或至少减轻现有技术的上述缺陷中的至少一个。
5.为实现上述目的，本发明提供一种目标形状及航向估计方法，该方法包括：
6.步骤s1，获取原始的目标点云数据，并聚类，获得聚类点云数据；
7.步骤s2，通过三维边界框描述聚类点云数据；
8.步骤s3，将聚类点云数据中的每个点云的x与y坐标沿一设定方向旋转预设步长；
9.步骤s4，以三维边界框对应在xoy平面的平面矩形框的中心o1为原点，建立二维直角坐标系xo1y，形成四个象限，根据由式(2)描述的点云特征点o2(x
c
、y
c
)所处的象限，确定平面矩形框面积、点云到平面矩形框距离和及点数极值差，进而根据聚类点云数据中点云分布呈现的形状，计算目标函数值；
[0010][0011]
式中，x
ri
为由步骤s3获得的旋转后第i个点云的x的坐标，y
ri
为由步骤s3获得的旋转后第i个点云的y的坐标，n为聚类点云数据中点云的总数量；
[0012]
步骤s5，判断当前航向角度是否处于设定遍历角度范围内，如果是，则返回步骤s3；反之，则进入步骤s6；航向角为目标的朝向与激光雷达坐标系的x轴之间的夹角；
[0013]
步骤s6，选择最大的目标函数值对应的航向角，作为最优航向角；
[0014]
步骤s7，根据最优航向角对应的平面矩形框的中心在激光雷达坐标系下的位置，
确定目标的最优三维边界框的位置、尺寸及姿态。
[0015]
进一步地，步骤s4中，目标函数值l在点云分布形状呈l型情形下通过式(17)计算，目标函数值l
′
在点云分布形状呈非l型情形下通过式(25)计算：
[0016]
l＝(n
d
/s)2/d
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(17)
[0017]
l
′
＝1/(s*d
n
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(25)
[0018]
式中，n
d
为点数极值差，s为平面矩形框面积，d和d
n
分别为点云分布形状呈l型、非l型的点云到平面矩形框距离和。
[0019]
进一步地，步骤s4中，n
d
通过式(7)计算得到：
[0020]
n
d
＝n
max
‑
n
min
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(7)
[0021]
式中，n
max
为点云特征点所在的象限的点云数量，n
min
为与点云特征点所在的象限呈对角的象限的点云数量。
[0022]
进一步地，步骤s4中，s通过式(6)计算得到：
[0023]
s＝a*b
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(6)
[0024]
a＝x
max
‑
x
min
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(4)
[0025]
b＝y
max
‑
y
min
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(5)
[0026]
式中，x
max
为x
ei
中的最大值，x
min
为x
ri
中的最小值，x
max
为y
ri
中的最大值，y
min
为y
ri
中的最小值。
[0027]
进一步地，步骤s4中，d
n
通过式(24)计算得到：
[0028][0029]
d
′
1i
＝min{|x
ri
‑
x
max
|,|y
ri
‑
y
max
|}
ꢀꢀꢀꢀꢀ
(20)
[0030]
d
′
2i
＝min{|x
ri
‑
x
max
|,|y
ri
‑
y
min
|}
ꢀꢀꢀꢀꢀ
(21)
[0031]
d
′
3i
＝min{|x
ri
‑
x
min
|,|y
ri
‑
y
min
|}
ꢀꢀꢀꢀꢀ
(22)
[0032]
d
′
4i
＝min{|x
ri
‑
x
min
|,|y
ri
‑
y
max
|}
ꢀꢀꢀꢀꢀ
(23)
[0033]
式中，d
′
1i
为式(20)描述的第i个点云在第一象限时与平面矩形框的最近距离，d
′
2i
为式(21)描述的第i个点云在第二象限时与平面矩形框的最近距离，d
′
3i
为式(22)描述的第i个点云在第三象限时与平面矩形框的最近距离，d
′
4i
为式(23)描述的第i个点云在第四象限时与平面矩形框的最近距离。
[0034]
进一步地，步骤s4中，d通过式(16)计算得到：
[0035][0036]
式中，d
1i
为第i个点云在第一象限中与平面矩形框的最近距离，d
2i
为第i个点云在第二象限中与平面矩形框的最近距离，d
3i
为第i个点云在第三象限中与平面矩形框的最近距离，d
4i
为第i个点云在第四象限中与平面矩形框的最近距离。
[0037]
进一步地，o2位于第一象限的情形下，d
1i
、d
2i
、d
3i
和d
4i
通过下式(8)和式(9)计算获得：
[0038][0039]
d
3i
＝k*max{|x
ri
‑
x
min
|,|y
ri
‑
y
min
|}
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(9)
[0040]
o2位于第二象限的情形下，d
1i
、d
2i
、d
3i
和d
4i
通过下式(10)和式(11)计算获得：
[0041][0042]
d
4i
＝k*max{|x
ri
‑
x
min
|,|y
ri
‑
y
max
|}
ꢀꢀꢀꢀꢀꢀꢀꢀ
(11)
[0043]
o2位于第三象限的情形下，d
1i
、d
2i
、d
3i
和d
4i
通过下式(12)和式(13)计算获得：
[0044][0045]
d
1i
＝k*max{|x
ri
‑
x
max
|,|y
ri
‑
y
max
|}
ꢀꢀꢀꢀꢀꢀꢀꢀ
(13)
[0046]
o2位于第四象限的情形下，d
1i
、d
2i
、d
3i
和d
4i
通过下式(14)和式(15)计算获得：
[0047][0048]
d
2i
＝k*max{|x
ri
‑
x
max
|,|y
ri
‑
y
min
|}
ꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(15)。
[0049]
本发明还提供一种目标形状及航向估计系统，该系统包括：
[0050]
数据解析模块，其用于获取原始的目标点云数据；
[0051]
点云聚类模块，其用于对目标点云数据进行聚类，获得聚类点云数据；
[0052]
目标形状及航向估计模块，其用于将聚类点云数据描述为三维边界框，并将聚类点云数据中的每个点云的x与y坐标沿一设定方向旋转预设步长，然后以三维边界框对应在xoy平面的平面矩形框的中心o1为原点，建立二维直角坐标系xo1y，形成四个象限，再根据由式(2)描述的点云特征点o2(x
c
、y
c
)所处的象限，确定平面矩形框面积、点云到平面矩形框距离和及点数极值差，进而根据聚类点云数据中点云分布呈现的形状计算目标函数值，再在设定遍历角度范围内选择最大目标函数值对应的航向角作为最优航向角，最后根据最优航向角对应的平面矩形框的中心在激光雷达坐标系下的位置，确定目标的最优三维边界框的位置、尺寸及姿态；
[0053][0054]
式中，x
ri
为由步骤s3获得的旋转后第i个点云的x的坐标，y
ri
为由步骤s3获得的旋转后第i个点云的y的坐标，n为聚类点云数据中点云的总数量，航向角为目标的朝向与激光雷达坐标系的x轴之间的夹角。
[0055]
进一步地，目标函数值l在点云分布形状呈l型情形下通过式(17)计算，目标函数值l
′
在点云分布形状呈非l型情形下通过式(25)计算：
[0056]
l＝(n
d
/s)2/d
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(17)
[0057]
l
′
＝1/(s*d
n
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(25)
[0058]
式中，n
d
为点数极值差，s为平面矩形框面积，d和d
n
分别为点云分布形状呈l型、非l
型的点云到平面矩形框距离和。
[0059]
进一步地，n
d
通过式(7)计算，s通过式(6)计算，d通过式(16)计算，d
n
通过式(24)计算得到；
[0060]
n
d
＝n
max
‑
n
min
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(7)
[0061]
s＝a*b
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(6)
[0062]
a＝x
max
‑
x
mi
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(4)
[0063]
b＝y
max
‑
y
min
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(5)
[0064][0065][0066]
式中，n
max
为点云特征点所在的象限的点云数量，n
min
为与点云特征点所在的象限呈对角的象限的点云数量，x
max
为x
ri
中的最大值，x
min
为x
ri
中的最小值，x
max
为y
ri
中的最大值，y
min
为y
ri
中的最小值，d
′
1i
为式(20)描述的第i个点云在第一象限时与平面矩形框的最近距离，d
′
2i
为式(21)描述的第i个点云在第二象限时与平面矩形框的最近距离，d
′
3i
为式(22)描述的第i个点云在第三象限时与平面矩形框的最近距离，d
′
4i
为式(23)描述的第i个点云在第四象限时与平面矩形框的最近距离，d
1i
为第i个点云在第一象限中与平面矩形框的最近距离，d
2i
为第i个点云在第二象限中与平面矩形框的最近距离，d
3i
为第i个点云在第三象限中与平面矩形框的最近距离，d
4i
为第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坐标为(x
i
、y
i
)，那么，第i个点云经由步骤s3旋转后的x与y坐标为(x
ri
、y
ri
)，则对应关系表示为式(1)，θ为当前航向角：
[0086][0087]
步骤s4，如图4所示，以三维边界框对应在xoy平面的平面矩形框的中心o1为原点，建立二维直角坐标系xo1y，形成四个象限，根据由式(2)描述的点云特征点o2(x
c
、y
c
)所处的象限，确定平面矩形框面积s、点云到平面矩形框距离和d及点数极值差n
d
，进而根据聚类点云数据中点云分布呈现的形状，计算目标函数值l。
[0088][0089]
式中，x
ri
为由步骤s3获得的旋转后第i个点云的x的坐标，y
ri
为由步骤s3获得的旋转后第i个点云的y的坐标，n为聚类点云数据中点云的总数量。
[0090]
其中，o1(x
ac
,y
ac
)可以利用下式(3)
‑
式(6)计算：
[0091][0092]
式中，x
max
为x
ri
中的最大值，x
min
为x
ri
中的最小值，x
max
为y
ri
中的最大值，y
min
为y
ri
中的最小值。
[0093]
结合图2，三维边界框的a和b通过下式(4)和式(5)计算：
[0094]
a＝x
max
‑
x
min
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(4)
[0095]
b＝y
max
‑
y
min
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(5)
[0096]
平面矩形框面积s则可以通过式(6)计算得到：
[0097]
s＝a*b
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(6)
[0098]
结合参考图1，由于点云的遮挡性，道路上普遍为刚性封闭表面目标(也可以称之为障碍物)，因此原始点云数据中点云的分布所呈现的形状特征大致为l型。下面首先以l型的点云分布形状为例，说明步骤s4的实现方式。
[0099]
如图4所示，根据平面矩形框中心o1(x
ac
,y
ac
)与点云特征点o2(x
c
、y
c
)的位置关系对点云分为如下四种情况：
[0100]
其中黑色圆点为点云，黑色方块为计算得出的o2。图4中的a示意的是o2位于xo1y的第一象限、图4中的b示意的是o2位于xo1y的第二象限、图4中的c示意的是o2位于xo1y的第三象限,图4中的d示意的是o2位于xo1y的第四象限。o2所在的象限为点云数量最多的象限，而与其相对的象限，即为点云数量最少的象限，则计算象限中点云数量最大值与点云数量最小值之间的差值，即式(7)表示的点数极值差n
d
：
[0101]
n
d
＝n
ma
‑
n
min
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(7)
[0102]
其中，n
max
为点云特征点所在象限的点云总点数，n
min
为点云特征点相对的象限中点云总点数。
[0103]
由于点云分布在四种情况下不同，因此，计算点云距离平面矩形框和d的方法有差异，下面进行分情况阐明：
[0104]
第一种情况，如图4中的a所示，o2位于xo1y的第一象限：为了获得更贴近各个点云的平面矩形框，因此计算第i个点云与平面矩形框最近的一条边的距离(以下简称平面矩形框的最近距离)。由于第i个点云可能位于第一象限至第四象限中的任意一个象限中，因此对于不同象限指定不同的规则，在本情况中大部分点云分布在第一象限、第二象限、第四象限，因此对于这三个象限指定如下的计算规则，计算公式如下式(8)所示：
[0105][0106]
其中，d
1i
为第i个点云在第一象限中与平面矩形框的最近距离，d
2i
为第i个点云在第二象限中与平面矩形框的最近距离，d
4i
为第i个点云在第四象限中与平面矩形框的最近距离。
[0107]
对于第三象限中，由于在合理的平面矩形框坐标划分下，此象限中的点云数量最少，因此为了让点云数量在此象限尽可能的少，对在此象限的点云计算平面矩形框距离时
采用惩罚策略，选择计算该象限中点云与平面矩形框最远一条边的距离，由此来增加其数值在点云到平面矩形框距离和中的占比，同时通过惩罚系数放大该数值，首先设定惩罚系数k(k>1)，k的取值应大于1，本发明中k＝2，但不限于数值为2。因此第三象限中的点云要计算其中与最远的一条边的距离的计算公式如下式(9)：
[0108]
d
3i
＝k*max{|x
ri
‑
x
min
|,|y
ri
‑
y
min
|}
ꢀꢀꢀꢀꢀꢀꢀ
(9)
[0109]
第二种情况，如图4中的b所示，o2位于xo1y的第二象限：在本情况中大部分点云分布在第一象限、第二象限、第三象限，因此对于这三个象限指定如下的计算规则，计算公式如下式(10)：
[0110][0111]
其中，d
1i
为第i个点云在第一象限中与平面矩形框的最近距离，d
2i
为第i个点云在第二象限中与平面矩形框的最近距离，d
3i
为第i个点云在第三象限中与平面矩形框的最近距离。
[0112]
对于第四象限中，此象限中的点云数量最少，对在此象限的点云计算平面矩形框距离时采用惩罚策略，选择计算该象限中点云与平面矩形框最远一条边的距离，同时通过惩罚系数放大数值，设定惩罚系数k＝2，但不限于数值为2，由此来增加其数值在点云到平面矩形框距离和中的占比。计算公式如下式(11)：
[0113]
d
4i
＝k*max{|x
ri
‑
x
min
|,|y
ri
‑
y
max
|}
ꢀꢀꢀꢀꢀꢀꢀꢀ
(11)
[0114]
第三种情况，如图4中的c所示，o2位于xo1y的第三象限：在本情况中大部分点云分布在第二象限、第三象限、第四象限，因此对于这三个象限指定如下的计算规则，计算公式如下式(12)：
[0115][0116]
其中，d
2i
为第i个点云在第二象限中与平面矩形框的最近距离，d
3i
为第i个点云在第三象限中与平面矩形框的最近距离，d
4i
为第i个点云在第四象限中与平面矩形框的最近距离。
[0117]
对于第一象限中，此象限中的点云数量最少，对在此象限的点云计算平面矩形框距离时采用惩罚策略，选择计算该象限中点云与平面矩形框最远一条边的距离，同时通过惩罚系数放大数值，设定惩罚系数k＝2，但不限于数值为2，由此来增加其数值在点云到平面矩形框距离和中的占比。计算公式如下式(13)：
[0118]
d
1i
＝k*max{|x
ri
‑
x
max
|,|y
ri
‑
y
max
|}
ꢀꢀꢀꢀꢀꢀꢀꢀ
(13)
[0119]
第四种情况，如图4中的d所示，o2位于xo1y的第四象限：在本情况中大部分点云分布在第一象限、第三象限、第四象限，因此对于这三个象限指定如下的计算规则，计算公式如下式(14)：
[0120][0121]
其中，d
1i
为第i个点云在第二象限中与平面矩形框的最近距离，d
3i
为第i个点云在第三象限中与平面矩形框的最近距离，d
4i
为第i个点云在第四象限中与平面矩形框的最近距离。
[0122]
对于第二象限中，此象限中的点云数量最少，对在此象限的点云计算平面矩形框距离时采用惩罚策略，选择计算该象限中点云与平面矩形框最远一条边的距离，同时通过惩罚系数放大数值，设定惩罚系数k＝2，但不限于数值为2，由此来增加其数值在点云到平面矩形框距离和中的占比。计算公式如下式(15)：
[0123]
d
2i
＝k*max{|x
ri
‑
x
ma
|,|y
ri
‑
y
min
|}
ꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(15)
[0124]
然后综合上述四种情况，可以得到不同点云分布下的d
1i
、d
2i
、d
3i
及d
4i
，由此来计算点云到平面矩形框的距离和d，不同情况的计算公式均为如下式(16)所示：
[0125][0126]
然后根据平面矩形框面积s、点云到平面矩形框距离和d及点数极值差n
d
，利用三个指标设计的目标函数l，l可以采用下式(17)计算得到：
[0127]
l＝(n
d
/s)2/d
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(17)
[0128]
该目标函数的数值越大说明平面矩形框越合理即受三个因素影响，平面矩形框面积s越小越好，点云到平面矩形框距离和d越小越好，点数极值差n
d
越大越好。计算完毕目标函数值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
′
mi
、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{|x
ri
‑
x
max
|,|y
ri
‑
y
max
|}
ꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(20)
[0142]
b.点云坐标位于第二象限，其中第i个点云在第二象限时与平面矩形框的最近距离d
′
2i
的计算公式如下式(21)：
[0143]
d
′
2i
＝min{|x
ri
‑
x
max
|,|y
ri
‑
y
min
|}
ꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(21)
[0144]
c.点云坐标位于第三象限，其中第i个点云在第三象限时与平面矩形框的最近距离d
′
3i
的计算公式如下式(22)：
[0145]
d
′
3i
＝min{|x
ri
‑
x
min
|,|y
ri
‑
y
min
|}
ꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(22)
[0146]
d.点云坐标位于第四象限，其中第i个点云在第四象限时与平面矩形框的最近距离d
′
4i
的计算公式如下式(23)：
[0147]
d
′
4i
＝min{|x
ri
‑
x
min
|,|y
ri
‑
y
max
|}
ꢀꢀꢀꢀꢀꢀꢀ
(23)
[0148]
综合上述四种情况，可以得到不同点云位于不同象限下的d
′
1i
、d
′
2i
、d
′
3i
及d
′
4i
，由此来计算点到平面矩形框的最近距离和d
n
,不同情况的计算公式均为如下式(24)所示：
[0149][0150]
然后利用平面矩形框面积s及点到平面矩形框的最近距离和d
n
设计目标函数l
′
，l
′
可以通过下式(25)计算：
[0151]
l
′
＝1/(s*d
n
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(25)
[0152]
l
′
的数值越大，说明平面矩形框越合理，平面矩形框面积s越小越好，点到平面矩形框的最近距离和d
n
越小越好。计算完毕目标函数的值后，将当前遍历的航向角θ与目标函数值l进行记录。
[0153]
综上所述，本发明对三维点云形状估计方法描述，适用于所有采集的三维点云数据，因此，所有关于三维点云形状估计方法在不脱离本发明的总体构思下的变化和修改，应属于本发明的保护范围之内。
[0154]
如图6所示，本发明实施例还提供一种目标形状及航向估计系统，该系统包括：
[0155]
数据解析模块，其用于获取原始的目标点云数据；
[0156]
点云聚类模块，其用于对目标点云数据进行聚类，获得聚类点云数据；
[0157]
目标形状及航向估计模块，其用于将聚类点云数据描述为三维边界框，并将聚类点云数据中的每个点云的x与y坐标沿一设定方向旋转预设步长，然后以三维边界框对应在xoy平面的平面矩形框的中心o1为原点，建立二维直角坐标系xo1y，形成四个象限，再根据由式(2)描述的点云特征点o2(x
c
、y
c
)所处的象限，确定平面矩形框面积、点云到平面矩形框距离和及点数极值差，进而根据聚类点云数据中点云分布呈现的形状计算目标函数值l，再在设定遍历角度范围内选择最大l对应的航向角作为最优航向角，最后根据最优航向角对应的平面矩形框的中心在激光雷达坐标系下的位置，确定目标的最优三维边界框的位置、尺寸及姿态。
[0158]
在一个实施例中，目标函数值l在点云分布形状呈l型情形下通过式(17)计算，目标函数值l
′
在点云分布形状呈非l型情形下通过式(25)计算。
[0159]
在一个实施例中，n
d
通过式(7)计算，s通过式(6)计算，d通过式(16)计算，d
n
通过式(24)计算得到。
[0160]
在车辆运行过程中，车载端配置的激光雷达对周围道路环境采集激光雷达点云数据，数据解析模块用于获取激光雷达采取的点云信息，将点云信息输入至点云聚类模块，对于聚类后的目标点云进行目标形状及航向估计，计算并输出所有目标的边界框的位置、尺寸及航向信息，从而该系统实现上述目标形状及航向估计方法的步骤。
[0161]
最后需要指出的是：以上实施例仅用以说明本发明的技术方案，而非对其限制。本领域的普通技术人员应当理解：可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。