一种基于激光雷达的水面目标检测与跟踪方法[发明专利]

(19)中华人民共和国国家知识产权局
(12)发明专利申请
(10)申请公布号 (43)申请公布日 (21)申请号 201910604318.3
(22)申请日 2019.07.05
(71)申请人 上海大学
地址 200444 上海市宝山区上大路99号
(72)发明人 杨毅　张志萌　罗均　谢少荣　
彭艳　蒲华燕　
(74)专利代理机构 郑州翊博专利代理事务所
(普通合伙) 41155
代理人 付红莉　周玉青
(51)Int.Cl.
G01S 7/48(2006.01)
G01S 17/58(2006.01)
G01S 17/66(2006.01)
G06K 9/62(2006.01)
G06T 7/136(2017.01)
G06T 7/277(2017.01)
(54)发明名称一种基于激光雷达的水面目标检测与跟踪方法(57)摘要本发明涉及环境感知领域，具体涉及一种基于激光雷达的水面目标检测与跟踪方法，步骤如下：（1）通过激光雷达获取第t帧海面环境的点云数据；（2）对点云数据进行滤波处理；（3）对滤波后的点云数据进行分割，得到点云簇；（4）对点云簇进行聚类，形成目标，对目标进行阈值筛选，得到第t帧海面环境的目标；（5）重复步骤（1）～（4）对第t+1帧海面环境进行处理，获取第t+1帧海面环境的目标；（6）将第t帧海面环境目标与第t+1帧海面环境目标进行匹配关联，得到匹配目标对；（7）求取匹配目标对中目标运动状态，对目标运动状态进行跟踪。

该方法目标检测和跟踪准确率高，稳定性好，提高了无人艇目标检测与跟踪
的准确性和稳定性。

权利要求书1页 说明书4页CN 110275153 A 2019.09.24
C N 110275153
A
1.一种基于激光雷达的水面目标检测与跟踪方法，其特征在于，包括以下步骤：
（1）通过激光雷达获取第t帧海面环境的点云数据；
（2）对步骤（1）获取的点云数据进行滤波处理；
（3）对经步骤（2）处理后的点云数据进行分割，得到点云簇；
（4）对步骤（3）得到的点云簇进行聚类，形成目标，对形成的目标进行阈值筛选，舍弃包含点云簇数量低于设定阈值的目标，得到第t帧海面环境中的最终目标；
（5）重复步骤（1）～（4）对第t+1帧海面环境进行处理，获取第t+1海面环境中的最终目标；
（6）将第t帧海面环境中的最终目标与第t+1帧海面环境中的最终目标进行匹配关联，得到匹配目标对；
（7）求取匹配目标对中目标的运动状态，然后采用滤波器对目标运动状态进行跟踪。

2.根据权利要求1所述的基于激光雷达的水面目标检测与跟踪方法，其特征在于，步骤
（2）中所述滤波处理具体操作为：去除步骤（1）获取的点云数据中的杂波，然后对去除杂波后的点云数据进行平滑滤波。

3.根据权利要求2所述的基于激光雷达的水面目标检测与跟踪方法，其特征在于，所述平滑滤波采用的滤波器为高斯滤波器。

4.根据权利要求2所述的基于激光雷达的水面目标检测与跟踪方法，其特征在于，步骤
（3）中采用LCCP算法对点云数据进行分割。

5.根据权利要求1～4任一所述的基于激光雷达的水面目标检测与跟踪方法，其特征在于，步骤（4）中采用欧几里得聚类提取算法对点云簇进行聚类。

6.根据权利要求5所述的基于激光雷达的水面目标检测与跟踪方法，其特征在于，步骤
（4）中所述阈值的范围为10～500。

7.根据权利要求6所述的基于激光雷达的水面目标检测与跟踪方法，其特征在于，步骤
（6）中采用ICP算法或ICP算法的变种将第t帧海面环境中的最终目标与第t+1帧海面环境中的最终目标进行匹配关联。

8.根据权利要求7所述的基于激光雷达的水面目标检测与跟踪方法，其特征在于，步骤
（7）中在求取匹配目标对中目标的运动状态之前，需要剔除匹配目标对中错误匹配的目标对，剔除错误匹配的目标对的具体操作为：根据目标在第t帧和第t+1帧之间可能运动的最大距离设定一个距离阈值，所述距离阈值等于目标在第t帧和第t+1帧之间的最大运动距离，舍弃匹配目标对中两目标之间距离大于距离阈值的目标对。

9.根据权利要求7或8所述的基于激光雷达的水面目标检测与跟踪方法，其特征在于，步骤（7）中所述滤波器为卡尔曼滤波器。

10.根据权利要求9所述的基于激光雷达的水面目标检测与跟踪方法，其特征在于，步骤（7）中所述运动状态包括目标相对于激光雷达的距离和目标相对于地球的运动速度；求取匹配目标对中目标运动状态的具体操作为：分别计算匹配目标对中两个目标在空间直角坐标系x、y、z方向上的所有点云坐标的平均值，得到两个目标中心的空间坐标；根据两个目标中心的空间坐标，求取两个目标中心之间的欧式距离，然后用两目标中心之间的欧式距离除以第t帧与第t+1帧之间的检测时间差，即得目标相对于地面的运动速度。

权　利　要　求　书1/1页CN 110275153 A
一种基于激光雷达的水面目标检测与跟踪方法
[0001]
技术领域
[0002]本发明涉及激光雷达环境感知技术领域，具体涉及一种基于激光雷达的水面目标检测与跟踪方法。

背景技术
[0003]水面无人艇，又称无人艇，无人艇是一种新型的水面无人设备，可广泛用于执行各种水上任务，如海岸线巡逻、近海防卫、港口巡逻、军舰护航、环境监测、海域建图等等。

随着无人艇的发展，未来无人艇将会有更加广泛的应用。

[0004]无人艇如果要在多变的、附在的海况下安全自主地完成各项任务，则无人艇必须具备良好的环境感知能力。

无人艇在实际使用的过程中，无人艇对于其所处周边环境的感知一直是一个技术难点，目前无人艇的环境感知大都是采用激光雷达扫描的方式或者视觉检测的方式，来完成无人艇对周边环境的感知，实现无人艇对周边环境的建模，从而进一步完成跟踪和避障等动作。

视觉检测的方式是使用价格相对较低的摄像头进行检测，其探测距离较远，但是其受环境影响严重，在光照不足，或者光照过强的情况下都是无法进行工作的，因此，使用视觉检测在进行运动物体检测时，检测精度很低，误差很大，对于精准跟踪很难使用此方法；而且视觉检测在进行图像分割时常常会受到海面波浪的影响，分离小型移动船体和静止物体效果很差。

激光雷达检测的方式虽然在距离上不如摄像头，但是它作为一种主动式的传感器，固有一些优良的性质，可以接近于全天候情况使用，所以对于一种短距离的目标检测和跟踪方案，激光雷达无疑是最好的选择。

[0005]现有关于海面目标检测的方法都是基于2.5D栅格的方法进行的，这种方法首先将点云投射到一个平面栅格上，栅格中数值代表的就是当前点的高度信息，然后再利于一些分割和聚类的方法对目标进行处理，这种方法忽略了目标点云的形状信息，没有很好的保留点云的原始信息，所以在对点云进行基于形状的匹配时就不能达到目标。

发明内容
[0006]针对现有技术存在的问题和不足，本发明的目的是提供一种基于激光雷达的水面目标检测与跟踪方法。

[0007]为实现发明目的，本发明采用的技术方案如下：
一种基于激光雷达的水面目标检测与跟踪方法，包括以下步骤：
（1）通过激光雷达获取第t帧海面环境的点云数据；
（2）对步骤（1）获取的点云数据进行滤波处理；
（3）对经步骤（2）处理后的点云数据进行分割，得到点云簇；
（4）对步骤（3）得到的点云簇进行聚类，形成目标，对形成的目标进行阈值筛选，舍弃包含点云簇数量低于设定阈值的目标，得到第t帧海面环境中的最终目标；
（5）重复步骤（1）～（4）对第t+1帧海面环境进行处理，获取第t+1海面环境中的最终目标；
（6）将第t帧海面环境中的最终目标与第t+1帧海面环境中的最终目标进行匹配关联，得到匹配目标对；
（7）求取匹配目标对中目标的运动状态，然后采用滤波器对目标运动状态进行跟踪。

[0008]根据上述的基于激光雷达的水面目标检测与跟踪方法，优选地，步骤（2）中所述滤波处理具体操作为：去除步骤（1）获取的点云数据中的杂波，然后对去除杂波后的点云数据进行平滑滤波。

更加优选地，去除点云数据中杂波的具体操作为：将距离激光雷达10米以内的所有点云全部去除。

[0009]根据上述的基于激光雷达的水面目标检测与跟踪方法，可以采用多种滤波器对去除杂波后的点云数据进行平滑滤波；优选地，所述平滑滤波采用的滤波器为高斯滤波器。

[0010]根据上述的基于激光雷达的水面目标检测与跟踪方法，步骤（3）中可以采用任意一种直接作用于点云的分割算法对点云数据进行分割；优选地，采用LCCP算法（Locally Convex Connected Patches，局部凸包连接算法）对点云数据进行分割。

[0011]根据上述的基于激光雷达的水面目标检测与跟踪方法，步骤（4）中根据场景不同可以选择不同的聚类算法对点云簇进行聚类；优选地，采用欧几里得聚类提取算法（Euclidean Cluster Extraction）对点云簇进行聚类。

[0012]根据上述的基于激光雷达的水面目标检测与跟踪方法，优选地，步骤（4）中所述阈值的范围为10～500。

[0013]根据上述的基于激光雷达的水面目标检测与跟踪方法，优选地，步骤（6）中采用ICP算法或ICP算法的变种将第t帧海面环境中的最终目标与第t+1帧海面环境中的最终目标进行匹配关联。

[0014]根据上述的基于激光雷达的水面目标检测与跟踪方法，优选地，步骤（7）中在求取匹配目标对中目标的运动状态之前，需要剔除匹配目标对中错误匹配的目标对，剔除错误匹配的目标对的具体操作为：根据目标在第t帧和第t+1帧之间可能运动的最大距离设定一个距离阈值，所述距离阈值等于目标在第t帧和第t+1帧之间的最大运动距离，舍弃匹配目标对中两目标之间距离大于距离阈值的目标对。

采用ICP算法或ICP算法的变种将第t帧海面环境存在的目标与第t+1帧海面环境存在的目标进行匹配关联，一旦两个目标匹配形成匹配目标对后，ICP算法就会认为它们是同一个物体产生的不同帧上的运动结果；但是有些匹配上的目标对，它们之间距离大于实际物体在两帧之间运动的最大可能值，这说明该匹配目标对为错误匹配；因此，需要将匹配目标对中两目标之间距离大于距离阈值的目标对舍弃。

[0015]根据上述的基于激光雷达的水面目标检测与跟踪方法，优选地，步骤（7）中可以采用多种滤波器对目标的运动状态进行跟踪，优选地，所述滤波器为卡尔曼滤波器。

[0016]根据上述的基于激光雷达的水面目标检测与跟踪方法，优选地，步骤（7）中所述运动状态包括目标相对于激光雷达的距离和目标相对于地球的运动速度。

[0017]根据上述的基于激光雷达的水面目标检测与跟踪方法，优选地，步骤（7）中求取匹配目标对中目标运动状态的具体操作为：分别计算匹配目标对中两个目标在空间直角坐标系x、y、z方向上的所有点云坐标的平均值（以x方向为例，x方向坐标平均值的求取方法为：
在x方向上对目标中所有点云的x坐标求和，然后将x坐标之和除以点云数量就得到了目标中心在x方向的坐标平均值；y方向坐标平均值、z方向坐标平均值的求取方法与x方向坐标平均值得求取方法相同），得到两个目标中心的空间坐标；根据两个目标中心的空间坐标，求取两个目标中心之间的欧式距离，然后用两目标中心之间的欧式距离除以第t帧与第t+1帧之间的检测时间差，即得目标相对于地面的运动速度。

[0018]根据上述的基于激光雷达的水面目标检测与跟踪方法，优选地，所述激光雷达为3D激光雷达。

[0019]与现有技术相比，本发明取得的积极有益效果为：
（1）本发明检测时直接基于点云进行处理的，保留了目标点云的形状细节特征，提高了前后帧之间目标匹配成功的概率，极大地提高了目标检测和跟踪的准确性；而且在对目标运动状态进行跟踪时使用了滤波器进行运动状态滤波，即使出现几帧的检测丢失，也能稳定的进行目标跟踪，提高了目标跟踪的准确性和稳定性；因此，本发明的方法目标检测和跟踪准确率高，稳定性好，极大地提高了水面无人艇对目标检测与跟踪的准确性和稳定性。

[0020]（2）由于激光雷达的检测误差，以及回波信号的干扰，获取的点云数据平滑性差，导致后续目标配对准确性差，采用滤波器对点云数据进行平滑滤波，能够提高点云数据的平滑性，滤波之后的点云数据平滑性好，便于后期目标配对，提高了目标配对的准确性。

[0021]（3）对滤波后的点云数据进行分割，可以将一帧中的所有目标都分割出来，但是分割之后的点云簇包含了误检测的点云簇，通过对点云簇的聚类以及对聚类后形成的目标中点云簇数量进行阈值筛选，去除点云簇数量低于设定阈值的目标，可以除去大部分误检测的点云簇；因此，本发明采用对滤波后的点云数据先进行分割处理，然后再进行聚类、阈值筛选的操作极大了提高海面环境目标提取的正确性和准确性。

具体实施方式
[0022]以下通过具体的实施例对本发明作进一步详细的说明，但并不限制本发明的范围。

另外还需要说明的是，本文所用的所有技术和科学术语与本发明所属技术领域的技术人员通常理解的含义相同。

[0023]一种基于3D激光雷达的水面目标检测与跟踪方法，包括以下步骤：（1）通过安装在水面无人艇上的3D激光雷达（3D激光雷达安装在水面无人艇的正上方，并且是水平安装，激光雷达运转之后会以一定的频率对海面环境进行数据采集）对海面环境进行检测，获取第t帧海面环境的点云数据。

[0024]（2）对步骤（1）获取的点云数据进行滤波处理，所述滤波处理的具体操作为：将距离激光雷达10米以内的所有点云全部去除，以除去水面无人艇艇体尾浪产生的杂波，然后采用高斯滤波器对去除杂波后的点云数据进行平滑滤波，由于3D激光雷达的检测误差，以及回波信号的干扰，点云数据的平滑性差，导致后续目标配对准确性差，通过对点云数据进行平滑滤波，滤波之后的点云数据平滑性好，便于后期目标配对，提高了目标配对的准确性。

[0025]（3）采用LCCP算法（Locally Convex Connected Patches，局部凸包连接算法）对经步骤（2）处理后的点云数据进行分割，得到点云簇；
（4）采用欧几里得聚类提取算法（Euclidean Cluster Extraction）对步骤（3）得到的
点云簇进行聚类，形成目标，对形成的目标进行阈值筛选，舍弃包含点云簇数量低于设定阈值的目标，得到第t帧海面环境中的最终目标；其中，根据应用场景的不同，阈值可以设定为10～500范围的任意一个取值。

[0026]（5）重复步骤（1）～（4）对第t+1帧海面环境进行处理，获取第t+1帧海面环境中的最终目标。

[0027]（6）采用ICP算法将第t帧海面环境中的最终目标与第t+1帧海面环境中的最终目标进行匹配关联，得到匹配目标对。

[0028]（7）根据目标在第t帧和第t+1帧之间可能运动的最大距离设定一个距离阈值，所述距离阈值等于目标在第t帧和第t+1帧之间的最大运动距离，舍弃匹配目标对中两目标之间距离大于距离阈值的目标对（采用ICP算法或ICP算法的变种将第t帧海面环境存在的目标与第t+1帧海面环境存在的目标进行匹配关联，一旦两个目标匹配形成匹配目标对后，ICP算法就会认为它们是同一个物体产生的不同帧上的运动结果；但是有些匹配上的目标对，它们之间距离大于实际物体在两帧之间运动的最大可能值，这说明该匹配目标对为错误匹配；因此，需要将匹配目标对中两目标之间距离大于距离阈值的目标对舍弃）；求取剩余匹配目标对中目标的运动状态，然后采用卡尔曼滤波器滤波器（KalmanFilter）对目标运动状态进行跟踪。

[0029]其中，所述运动状态包括目标相对于激光雷达的距离和目标相对于地球的运动速度。

求取匹配目标对中目标运动状态的具体操作为：分别计算匹配目标对中两个目标在空间直角坐标系x、y、z方向上的所有点云坐标的平均值（以x方向为例，x方向坐标平均值的求取方法为：在x方向上对目标中所有点云的x坐标求和，然后将x坐标之和除以点云数量就得到了目标中心在x方向的坐标平均值；y方向坐标平均值、z方向坐标平均值的求取方法与x 方向坐标平均值得求取方法相同），得到两个目标中心的空间坐标；根据两个目标中心的空间坐标，求取两个目标中心之间的欧式距离，然后用两目标中心之间的欧式距离除以第t帧与第t+1帧之间的检测时间差，即得目标相对于地面的运动速度。

[0030]重复上述步骤（1）～（7），即可完成不间断的目标检测与跟踪。

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