CN115761286A - 复杂海况下基于激光雷达的水面无人艇航行障碍物检测方法 - Google Patents

Abstract

本发明涉及一种复杂海况下基于激光雷达的水面无人艇航行障碍物检测方法，包括以下步骤：步骤1、对激光雷达获取的点云数据进行多云多帧融合处理，构建水面无人艇的前视投影栅格地图；步骤2、基于步骤1所构建的水面无人艇的前视投影栅格地图，采用垂直属性进行传统RANSAC算法的改进，并对数据遍历优化改进，实现高效的杂波干扰点云数据的滤除；步骤3、基于步骤2获得杂波较少的水面无人艇航行障碍物点云数据信息，采用自适应参数的改进DBSCAN算法对激光雷达获取的点云数据进行聚类处理，获得水面无人艇航行障碍物信息。本发明能够解决复杂海况下水面无人艇路径自主规划过程中涉及避碰规划问题中的航行障碍物检测与识别问题。

Description

复杂海况下基于激光雷达的水面无人艇航行障碍物检测方法

技术领域

本发明属于无人船舶控制技术领域，涉及一种水面无人艇航行障碍物检测方法，尤其是一种复杂海况下基于激光雷达的水面无人艇航行障碍物检测方法。

背景技术

无人艇(Unmanned surface vehicle，水面无人艇)是一种日益被广泛应用的通用水面智能任务平台，它具有体积小、成本低、机动灵活、航速高、智能化、雷达反射面积小、无人员伤亡等特点。水面无人艇在海洋环境测量、海底地形地貌测量、海洋资源开发等领域中扮演着越来越重要的角色，具有广阔的应用前景。

水面无人艇局部路径自主规划(避碰规划)的首要任务是检测海域中的障碍物。普通船舶通常配备光电设备(可见光、红外)和海洋雷达传感器。光电设备输出具有高刷新率和高目标分辨率的时间序列图像信息；导航雷达输出扫描图像，探测距离长，可全天全天候提供导航。然而，这两种传感器都有各自的局限性，很难完全满足水面无人艇的环境传感需求。特别是在复杂水域、目标距离近、速度快的情况下，导航雷达信息更新频率慢、分辨率低、目标特征少。特别是在码头等近岸场景中，海杂波较多，很容易漏检和丢失目标。光电设备是被动检测设备，对天气和光线变化非常敏感，光学成像的本质是三维世界的二维图像平面投影，缺乏目标距离信息和真实尺寸，难以满足障碍物检测和定位的需要。因此，备光电设备(可见光、红外)和导航雷达传感器很难为水面无人艇提供可靠的障碍物检测和识别支持。

激光雷达具有导航雷达和光学成像的优点。它不仅可以获得目标的距离和方位信息，用于目标定位和导航避障，还可以获得目标三维轮廓信息，用于障碍物探测和识别。同时，激光雷达采用主动激光测距，不易受光线和天气影响，扫描频率快，非常适合水面无人艇平台上中短程目标的检测和识别。

激光雷达是一种主动式传感器，信息丰富且不受天气，光照影响，能够获取场景的三维信息，是无人艇检测目标，并对其进行尺寸测量、距离测量、速度测量的必要手段，对无人艇航行避障、目标定位与跟踪具有重要价值。但在激光雷达传感器点云稀疏，对小目标检测困难，易受海杂波干扰。

因此，如何基于激光雷达传感器在复杂海况和强干扰条件下进行有效的目标检测与识别是无人艇水面环境感知的关键。

经检索，未发现与本发明相同或相似的现有技术的专利文献。

发明内容

本发明的目的在于克服现有技术中的不足，提出一种复杂海况下基于激光雷达的水面无人艇航行障碍物检测方法，能够解决复杂海况下水面无人艇路径自主规划过程中涉及避碰规划问题中的航行障碍物检测与识别问题。

本发明解决其现实问题是采取以下技术方案实现的：

一种复杂海况下基于激光雷达的水面无人艇航行障碍物检测方法，包括以下步骤：

步骤1、对激光雷达获取的点云数据进行多云多帧融合处理，进而构建水面无人艇的前视投影栅格地图；

步骤2、基于步骤1所构建的水面无人艇的前视投影栅格地图，针对无人艇海面上存在碎波、尾浪产生的干扰，导致很多反射无效的点云数据，采用垂直属性进行传统RANSAC算法的改进，并对数据遍历优化改进，实现高效的杂波干扰点云数据的滤除；

步骤3、基于步骤2获得杂波较少的水面无人艇航行障碍物点云数据信息，采用自适应参数的改进DBSCAN算法对激光雷达获取的点云数据进行聚类处理，获得水面无人艇航行障碍物信息。

而且，所述步骤1的具体步骤包括：

(1)对激光雷达获取的点云数据进行三帧点云融合处理，对于得到的姿态与位移信息，采用欧拉角与旋转矩阵结合的方法，进行坐标变换：

所述步骤1第(1)步的具体步骤包括：

①激光雷达坐标系绕惯导坐标系x轴旋转的角度为其滚动角，滚动角取值范围为：-π～π；

在滚动角作用下，从原始坐标系到新坐标系下的坐标变换可用旋转矩阵公式(1)表示：

激光雷达坐标系绕惯导坐标系y轴旋转的角度为其俯仰角，俯仰角的取值范围为：-π/2～π/2；同理，由俯仰角导致的新旧坐标系坐标变化的旋转矩阵公式如下公式(2)所示：

雷达坐标系绕惯导坐标系z轴旋转的角度为其偏航角，航向角的取值范围为：-π～π。由偏航角导致的新旧坐标系坐标变化的旋转矩阵公式(3)所示。

②用欧拉角表示旋转变化时，在给定三轴旋转角度的同时，必须指定旋转顺序，指定的旋转顺序为：偏航角-俯仰角-滚动角，最终的旋转矩阵则是上面三个旋转矩阵按先后顺序相乘的结果。

(2)构建水面无人艇的栅格地图：

以激光雷达为坐标原点，设载体前进方向为X轴正方向，垂直前进方向的右手侧为Y轴正方向；根据激光雷达量程设定X轴与Y轴截断距离分别为X_turn与Y_turn，并设定栅格尺寸为g_cell；

输入点云P＝{P_i|P_i＝(x_i,y_i,z_i),i＝1,2,...,N}进行XY平面投影，并进行栅格化划分，其中点云中任意点p_i在栅格地图的坐标计算方法如式(4)所示：

至此，栅格地图表示为集合G＝{G_x,y＝(g_x,g_y)|g_x＝0,1,...,H,g_y＝0,1,...,W},，其行分辨率为H＝[2*X_turn/g_cell]，列分辨率为W＝[2*Y_turn/g_cell]；

(3)构建水面无人艇的前视投影栅格图：

设激光雷达在某一发光时刻，某激光器采集的原始信息为水平角度测量值θ_horizon，水平角度偏移α_{HorizonOffset}，实际激光点水平发射角表示如式(5)所示：

θ_horizon＝θ_measure+α_{HorizonOffset} (5)

故该激光点在前视栅格中的纵坐标计算如式(6)所示：

横坐标计算如式(7)所示：

row＝R_index (7)

由此，前视投影栅格图构建完毕。

而且，所述步骤2的具体方法为：

(1)基于步骤1所构建的水面无人艇的前视投影栅格地图，输入点云集的改进：尾流本身破碎且水平，障碍物点云反射稳定且较垂直，故可利用前视栅格图进行垂直属性标记后，可从垂直属性为水平和未知的栅格中提取对应数据点，将其作为RANSAC的输入点集；

(2)种子点的选取优化：将雷达视场划分为局部区域，取局部区域内输入点集中绝对高度最低点作为种子点，将局部区域中所有输入点集绝对高度进行TopK排序，取高度值最小的K个点，滤除K个点中小于中位数三倍标准差的异常点，之后得到剩余点的高度均值，将高度小于该均值的点作为初始种子点；

(3)经过对输入点集、种子点选取、数据遍历优化的改进，使用最简单的平面模型进行平面点估计，将拟合的平面包含的点集作为尾流点滤除：

其中，n＝[a b c]^T为估计平面法向量，X＝[x y z]^T为平面上任意一点；对于法向量，通过计算局内点协方差矩阵的方式进行求解：

其中

为种子点的均值。协方差矩阵刻画了点集的散布特性，利用SVD分解求解三个特征值与对应特征向量，可获得点集空间分布的三个主方向；由于尾流为平面，故平面的垂直法向量即最小特征值所计算得到的特征向量。

而且，所述步骤3的具体步骤包括：

(1)基于步骤1所构建的水面无人艇的前视投影栅格地图，根据激光雷达扫描线分布求出相邻两条扫描线的间距建立ε^*列表，然后依据每个扫描点的坐标值在列表中查找出对应的列表值，最后通过线性插值法确定对应的邻域值；

所述ε^*列表的建立方法是：假设激光雷达共有n条扫描线，无人艇安装的激光雷达原点距水平面高度为h时，扫描线的俯仰角构成集合α＝{α_i}(i＝1,2,3,...,n)，激光雷达的扫描线投射点到O点的径向距离构成集合r＝{r_i}(i＝1,2,3,...,n)，相邻两条激光扫描线投射点的距离构成集合ε＝{ε_i}(i＝1,2,3,...,n-1)，r，ε对应元素满足r_i＝h/tan(α_i)(i＝1,2,3,...,n)，ε_i＝r_i+1-r_i(i＝1,2,3,...,n)。

水面无人艇激光雷达安装高度距离水面为h＝2.5，则集合r修改为r_i＝2.5/tan(α_i)(i＝1,2,3,...,n)，记作r^*；对应的集合ε修改为

记作ε^*；至此，ε^*列表建立完成。

所述通过线性插值法确定对应的邻域值的方法为：

假设存在一个三维空间点P(x,y,z)，到z轴的距离

对应的

在r^*列表中寻找

使得

在ε^*列表中存在对应的

根据一阶线性插值法，则任意点P(x,y,z)处的ε值为：

(2)基于对激光雷达获取的点云数据进行聚类处理，获得水面无人艇航行障碍物信息。

本发明的优点和有益效果：

1、本发明以水面无人艇为平台，提出一种改进多种激光雷达的水面障碍物检测算法，能够在复杂的海面环境下，利用激光雷达对水面障碍物进行高效实时的检测，为水面无人艇路径自主规划中的局部避碰规划提供环境感知算法支持。

2、本发明提根据激光雷达点云数据的特点，采用一种激光雷达点云数据预处理算法，即多帧点云数据处理算法，考虑算法的实时性及对硬件的要求采用3帧点云数据融合预处理算法，然后通过建立前视网格网络，将三维点云数据放置在前视网格上进行处理，大大加快了点云数据的处理速度，提高了障碍物检测的实时性能。

3、本发明提出了一种改进的RANSAC滤波算法，对于含有大量杂波反射的激光雷达点云数据，例如：无人艇航行过程中产生的尾波反射的点云进行滤波，并采用改进的RANSAC算法对点云进行过滤，以减少杂波点云数据的干扰。

5、本发明针对水面航行障碍物检测获取的点云数据特点，进行点云目标聚类问题，提出了一种具有自适应参数的DBSCAN算法，并将其应用在水面航行障碍物检测，保留了传统DBSCAN在短距离点云聚类方面的优势，能够正确聚类长距离点云，在海上试验的障碍物检测效果良好。

附图说明

图1为本发明的基于激光雷达的水面无人艇航行障碍物检测系统图；

图2为旋转融合示意图(左)与平移融合示意图(右)；

图3为本发明的DBSCAN算法的搜索过程；

图4为激光雷达扫描线示意图；

图5为本发明的水面无人艇航行障碍物目标检测算法流程图；

图6为搭载激光雷达的水面无人艇效果图；

图7为本发明的激光雷达检测海面上靠近船只的效果图；

图8为本发明的激光雷达检测海边码头的效果图。

具体实施方式

以下结合附图对本发明实施例作进一步详述：

一种复杂海况下基于激光雷达的水面无人艇航行障碍物检测方法，如图5所示，包括以下步骤：

步骤1、对激光雷达获取的点云数据进行多云多帧融合处理，进而构建水面无人艇的前视投影栅格地图；

所述步骤1的具体步骤包括：

(1)对激光雷达获取的点云数据进行三帧点云融合处理，对于得到的姿态与位移信息，采用欧拉角与旋转矩阵结合的方法，进行坐标变换：

所述步骤1第(1)步的具体步骤包括：

①激光雷达坐标系绕惯导坐标系x轴旋转的角度为其滚动角，滚动角取值范围为：-π～π；

在滚动角作用下，从原始坐标系到新坐标系下的坐标变换可用旋转矩阵公式(1)表示：

激光雷达坐标系绕惯导坐标系y轴旋转的角度为其俯仰角，俯仰角的取值范围为：-π/2～π/2；同理，由俯仰角导致的新旧坐标系坐标变化的旋转矩阵公式如下公式(2)所示：

雷达坐标系绕惯导坐标系z轴旋转的角度为其偏航角，航向角的取值范围为：-π～π。由偏航角导致的新旧坐标系坐标变化的旋转矩阵公式(3)所示。

②用欧拉角表示旋转变化时，在给定三轴旋转角度的同时，必须指定旋转顺序，指定的旋转顺序为：偏航角-俯仰角-滚动角，最终的旋转矩阵则是上面三个旋转矩阵按先后顺序相乘的结果。

(2)构建水面无人艇的栅格地图：

以激光雷达为坐标原点，设载体前进方向为X轴正方向，垂直前进方向的右手侧为Y轴正方向；根据激光雷达量程设定X轴与Y轴截断距离分别为X_turn与Y_turn，并设定栅格尺寸为g_cell；

输入点云P＝{P_i|P_i＝(x_i,y_i,z_i),i＝1,2,...,N}进行XY平面投影，并进行栅格化划分，其中点云中任意点p_i在栅格地图的坐标计算方法如式(4)所示：

至此，栅格地图表示为集合G＝{G_x,y＝(g_x,g_y)|g_x＝0,1,...,H,g_y＝0,1,...,W},，其行分辨率为H＝[2*X_turn/g_cell]，列分辨率为W＝[2*Y_turn/g_cell]。

(3)构建水面无人艇的前视投影栅格图：

设激光雷达在某一发光时刻，某激光器采集的原始信息为水平角度测量值θ_horizon，水平角度偏移α_{HorizonOffset}，实际激光点水平发射角表示如式(5)所示：

θ_horizon＝θ_measure+α_{HorizonOffset} (5)

故该激光点在前视栅格中的纵坐标计算如式(6)所示：

横坐标计算如式(7)所示：

row＝R_index (7)

由此，前视投影栅格图构建完毕。

在本实施例中，首先，采用基于前视栅格投影的水面障碍物检测算法，水面无人艇的栅格地图的构建过程如下：以激光雷达为坐标原点，设载体前进方向为X轴正方向，垂直前进方向的右手侧为Y轴正方向。根据激光雷达量程设定X轴与Y轴截断距离分别为X_turn与Y_turn，并设定栅格尺寸为g_cell。输入点云P＝{P_i|P_i＝(x_i,y_i,z_i),i＝1,2,...,N}进行XY平面投影，并进行栅格化划分，其中点云中任意点p_i在栅格地图的坐标计算方法如式(4)所示。

至此，栅格地图表示为集合G＝{G_x,y＝(g_x,g_y)|g_x＝0,1,...,H,g_y＝0,1,...,W},，其行分辨率为H＝[2*X_turn/g_cell]，列分辨率为W＝[2*Y_turn/g_cell]。

前视投影栅格图具体构建方法如下：设激光雷达在某一发光时刻，某激光器采集的原始信息为水平角度测量值θ_horizon，水平角度偏移α_{HorizonOffset}，实际激光点水平发射角表示如式(5)所示：

θ_horizon＝θ_measure+α_{HorizonOffset} (5)

故该激光点在前视栅格中的纵坐标计算如式(6)所示：

横坐标计算如式(7)所示：

row＝R_index (7)

由此，前视投影栅格图构建完毕。

步骤2、基于步骤1所构建的水面无人艇的前视投影栅格地图，针对无人艇海面上存在碎波、尾浪产生的干扰，导致很多反射无效的点云数据，采用垂直属性进行传统RANSAC算法的改进，并对数据遍历优化改进，实现高效的杂波干扰点云数据的滤除。

所述步骤2的具体方法为：

(1)基于步骤1所构建的水面无人艇的前视投影栅格地图，输入点云集的改进：尾流本身破碎且水平，障碍物点云反射稳定且较垂直，故可利用前视栅格图进行垂直属性标记后，可从垂直属性为水平和未知的栅格中提取对应数据点，将其作为RANSAC的输入点集；

(2)种子点的选取优化：将雷达视场划分为局部区域，取局部区域内输入点集中绝对高度最低点作为种子点，将局部区域中所有输入点集绝对高度进行TopK排序，取高度值最小的K个点，滤除K个点中小于中位数三倍标准差的异常点，之后得到剩余点的高度均值，将高度小于该均值的点作为初始种子点；

(3)经过对输入点集、种子点选取、数据遍历优化的改进，使用最简单的平面模型进行平面点估计，将拟合的平面包含的点集作为尾流点滤除：

其中，n＝[a b c]^T为估计平面法向量，X＝[x y z]^T为平面上任意一点；对于法向量，通过计算局内点协方差矩阵的方式进行求解：

其中

为种子点的均值。协方差矩阵刻画了点集的散布特性，利用SVD分解求解三个特征值与对应特征向量，可获得点集空间分布的三个主方向；由于尾流为平面，故平面的垂直法向量即最小特征值所计算得到的特征向量。

在本实施例中，采用垂直属性进行传统RANSAC算法的改进，并对数据遍历优化改进，实现高效的尾流滤除，改进的RANSAC算法具体内容及过程如下：

(1)输入点云集的改进：尾流本身破碎且水平，障碍物点云反射稳定且较垂直，故可利用前视栅格图进行垂直属性标记后，可从垂直属性为水平和未知的栅格中提取对应数据点，将其作为RANSAC的输入点集。RANSAC算法的时间复杂度取决于点云整体规模与内点的选取比例。

(2)种子点的选取优化：水面无人艇航行过程中产生的尾流整体接近于水平，且尾流所在平面在高度上低于障碍物上的水平平面。具体改进方式为将雷达视场划分为局部区域，取局部区域内输入点集中绝对高度最低点作为种子点。为避免噪声影响，可将局部区域中所有输入点集绝对高度进行TopK排序，取高度值最小的K个点，滤除K个点中小于中位数三倍标准差的异常点，之后得到剩余点的高度均值，将高度小于该均值的点作为初始种子点。此方式可有效提高RANSAC种子点质量，能极大加快收敛速度。

(3)数据遍历优化：针对Pandar40激光雷传感器达产生的海量数据点，每次迭代都遍历所有的数据点所需要的计算资源和代价是昂贵的。拟合平面的初始参数采用随机采样的方式获取，而后期判断其余数据点是否属于该平面的操作可直接排除已归属于其他平面的点，仅对未归属任何平面的数据点进行遍历即可。对于任意两个平面，可通过判断其法向量夹角是否大于设定阈值的方法消除RANSAC算法拟合过程中出现的伪平面现象。

经过对输入点集、种子点选取、数据遍历优化的改进，使用最简单的平面模型进行平面点估计，将拟合的平面包含的点集作为尾流点滤除。

其中的n＝[a b c]^T为估计平面法向量，X＝[x y z]^T为平面上任意一点。对于法向量，可以通过计算局内点协方差矩阵的方式进行求解：

其中

为种子点的均值。协方差矩阵刻画了点集的散布特性，利用SVD分解求解三个特征值与对应特征向量，可获得点集空间分布的三个主方向。由于尾流为平面，故平面的垂直法向量即最小特征值所计算得到的特征向量。

步骤3、基于步骤2获得杂波较少的水面无人艇航行障碍物点云数据信息，采用自适应参数的改进DBSCAN算法对激光雷达获取的点云数据进行聚类处理，获得水面无人艇航行障碍物信息。

所述步骤3的具体步骤包括：

(1)基于步骤1所构建的水面无人艇的前视投影栅格地图，根据激光雷达扫描线分布求出相邻两条扫描线的间距建立ε^*列表，然后依据每个扫描点的坐标值在列表中查找出对应的列表值，最后通过线性插值法确定对应的邻域值；

所述ε^*列表的建立方法是：假设激光雷达共有n条扫描线，无人艇安装的激光雷达原点距水平面高度为h时，扫描线的俯仰角构成集合α＝{α_i}(i＝1,2,3,...,n)，激光雷达的扫描线投射点到O点的径向距离构成集合r＝{r_i}(i＝1,2,3,...,n)，相邻两条激光扫描线投射点的距离构成集合ε＝{ε_i}(i＝1,2,3,...,n-1)，r，ε对应元素满足r_i＝h/tan(α_i)(i＝1,2,3,...,n)，ε_i＝r_i+1-r_i(i＝1,2,3,...,n)。

水面无人艇激光雷达安装高度距离水面为h＝2.5，则集合r修改为r_i＝2.5/tan(α_i)(i＝1,2,3,...,n)，记作r^*；对应的集合ε修改为

记作ε^*；至此，ε^*列表建立完成。

所述通过线性插值法确定对应的邻域值的方法为：

假设存在一个三维空间点P(x,y,z)，到z轴的距离

对应的

在r^*列表中寻找

使得

在ε^*列表中存在对应的

根据一阶线性插值法，则任意点P(x,y,z)处的ε值为：

在本实施例中，如图4所示激光雷达共有n条扫描线，无人艇安装的激光雷达原点距水平面高度为h时，扫描线的俯仰角构成集合α＝{α_i}(i＝1,2,3,...,n)，激光雷达的扫描线投射点到O点的径向距离构成集合r＝{r_i}(i＝1,2,3,...,n)，相邻两条激光扫描线投射点的距离构成集合ε＝{ε_i}(i＝1,2,3,...,n-1)，r，ε对应元素满足r_i＝h/tan(α_i)(i＝1,2,3,...,n)，ε_i＝r_i+1-r_i(i＝1,2,3,...,n)。

(2)基于对激光雷达获取的点云数据进行聚类处理，获得水面无人艇航行障碍物信息。

在本实施例中，提出并采用了一种自适应参数的改进DBSCAN算法，这样既保留了传统的DBSCAN在近距离点云聚类时的优点，又能对远距离的点云正确地聚类。

根据如图4所示的激光雷达扫描线分布求出相邻两条扫描线的间距建立ε^*列表，然后依据每个扫描点的坐标值在列表中查找出对应的列表值，最后通过线性插值法确定对应的邻域值。

ε^*列表的建立方法是：假设激光雷达共有n条扫描线，无人艇安装的激光雷达原点距水平面高度为h时，如图4所示，扫描线的俯仰角构成集合α＝{α_i}(i＝1,2,3,...,n)，激光雷达的扫描线投射点到O点的径向距离构成集合r＝{r_i}(i＝1,2,3,...,n)，相邻两条激光扫描线投射点的距离构成集合ε＝{ε_i}(i＝1,2,3,...,n-1)，r，ε对应元素满足r_i＝h/tan(α_i)(i＝1,2,3,...,n)，ε_i＝r_i+1-r_i(i＝1,2,3,...,n)。

水面无人艇激光雷达安装高度距离水面为h＝2.5，则集合r修改为r_i＝2.5/tan(α_i)(i＝1,2,3,...,n)，记作r^*；对应的集合ε修改为

记作ε^*；至此，ε^*列表建立完成。

ε^*列表使用方法：假设存在一个三维空间点P(x,y,z)，到z轴的距离

对应的

在r^*列表中寻找

使得

在ε^*列表中存在对应的

根据一阶线性插值法，则任意点P(x,y,z)处的ε值为：

在本实施例中，如图1所示为一种复杂海况的水面无人艇障碍物检测方法系统组成图，水面无人艇采用激光雷达、组合导航、DSP加固计算机板卡。激光雷达采用国产Pandar40激光雷达传感器，Pandar40激光雷达的感知单元容纳共40个激光器，并拥有360度水平扫描视场与24度的垂直扫描视场，每秒可提供近百万个测量数据点。组合导航为GPS与惯性导航，由基于光纤陀螺、加速度计及GNSS接收板卡组成，通过多传感器融合及导航解析算法可实现在高精度移动测量系统应用领域中对姿态、航向以及位置信息的精确测量。作为激光雷达探测算法运行平台的DSP加固计算机板卡采用32GB内存、i7处理器+dsp6678、2.6GHz主频的计算机进行设计，以6U CPCI加固计算机板卡形式进行设计。

如图2所示为两帧激光雷达点云的旋转融合与平移融合示意图，左图中X₀、Y₀、Z₀代表当前点云的三维坐标，X_V、Y_V、Z_V代表前一帧点云的三维坐标；Δφ、Δθ、Δψ代表两帧点云的偏航夹角、俯仰夹角和滚动夹角。右图展示了的平移融合中两帧点云的相对平移量求解方法，由于在求解平移量的过程中使用的是全局坐标系下对东、北、天三个方向上的积分结果，因此在转化为激光雷达坐标系时，需要根据载体相对于全局坐标系下的偏航、俯仰和滚动角度，结合东、北、天三个方向上的平移量ΔN、ΔE、ΔZ来解算雷达坐标系下的平移量Δx、Δy、Δz，求解方法如公式8所示。

式8采用了简记法，c_ψ＝cos(Δψ)，其余类似；式10中Δx，Δy，Δz表示激光雷达在雷达坐标系中xyz三个方向上的偏移量；T(x,y,z)表示旋转平移变换前的点云坐标，T'(x',y',z')表示变换之后的点云坐标。

图3为本发明的DBSCAN算法的搜索过程，首先，DBSCAN算法具有两个重要的参数：

1)Eps：Eps为判断某个点是否为核心点的邻域搜索半径。

2)Minpts：Minpts指在当某个点为核心点时，其Eps邻域半径内所包含的除核心点外的其他最少点数。

根据激光雷达扫描线分布求出相邻两条扫描线的间距建立ε^*列表，然后依据每个扫描点的坐标值在列表中查找出对应的列表值，最后通过线性插值法确定对应的邻域值。

ε^*列表的建立方法是：假设激光雷达共有n条扫描线，无人艇安装的激光雷达原点距水平面高度为h时，如图4所示，扫描线的俯仰角构成集合α＝{α_i}(i＝1,2,3,...,n)，激光扫描线投射点到O点的径向距离构成集合r＝{r_i}(i＝1,2,3,...,n)，相邻两条激光扫描线投射点的距离构成集合ε＝{ε_i}(i＝1,2,3,...,n-1)，r，ε对应元素满足r_i＝h/tan(α_i)(i＝1,2,3,...,n)，ε_i＝r_i+1-r_i(i＝1,2,3,...,n)。

水面无人艇激光雷达安装高度距离水面为h＝2.5，则集合r修改为r_i＝2.5/tan(α_i)(i＝1,2,3,...,n)，记作r^*；对应的集合ε修改为

记作ε^*。至此，ε^*列表建立完成。

ε^*列表使用方法：假设存在一个三维空间点P(x,y,z)，到z轴的距离

对应的

在r^*列表中寻找

使得

在ε^*列表中存在对应的

根据一阶线性插值法，则任意点P(x,y,z)处的ε值为：

图5为本发明的水面无人艇航行障碍物目标检测算法流程图，对激光雷达获取的点云数据进行多云多帧融合处理，然后再进行构建栅格地图，再采用改进的DBSCAN聚类算法对点云数据进行处理，最终获得水面无人艇航行障碍物信息。针对无人艇海面上存在碎波、尾浪产生的干扰，采用改进RANSAC算法进行处理，获得较好的效果。

图6为搭载激光雷达的水面无人艇效果图，采用小型水面无人艇进行激光雷达试验搭载平台，该平台采用单主机单喷泵动力方式，具备较好的操控性。图7为本发明的激光雷达检测海面上靠近船只的效果图，图8为本发明的激光雷达检测海边码头的效果图。

本发明的创新之处在于：

1、本发明采用激光雷达点云融合预处理算法，对于得到的姿态与位移信息，需要对其进行坐标变换描述，坐标变换包括旋转变换和平移变换两个步骤。旋转变换的描述方法有很多，如旋转矩阵、欧拉角、单位四元数和角轴描述等方法。本发明使用的欧拉角与旋转矩阵结合的方法，该方法简单直观。欧拉角是由欧拉首先提出的用于确定三维欧几里得空间里定点旋转刚体取向的三个独立角参量。对于任何参考系，一个刚体的取向可以通过依一定顺序做三个欧拉角的旋转来描述。

2、本发明采用基于前视栅格网络的水面障碍物检测算法。

3、本发明采用基于密度的DBSCAN聚类算法，并针对于该算法在对远距离目标进行提取时存在的过分割问题，提出了一种自适应参数的改进DBSCAN算法，这样既保留了传统的DBSCAN在近距离点云聚类时的优点，又能对远距离的点云正确地聚类。

4、本发明针对水无人艇航行过程中喷泵或螺旋桨动力装置产生航行尾流、海面较大涌浪对激光雷达反射而生成的激光雷达点云数据，对后续激光雷达点云数据聚类产生较大影响，需要进行滤除。使用垂直属性进行传统RANSAC算法的改进，并对数据遍历优化改进，实现高效的无效激光雷达点云数据滤除。

综上所述，本发明提出一种复杂海况下的水面无人艇障碍物检测方法，水面无人艇采用激光雷达、组合导航、DSP加固计算机板卡，采用激光雷达点云融合预处理算法，对于得到的姿态与位移信息，需要对其进行坐标变换描述，坐标变换包括旋转变换和平移变换两个步骤。提出了一种基于前视网格投影的海面障碍物检测算法。首先介绍了前视网格投影图的构造方法。这种点云结构化方法适应了激光雷达的扫描特性，为目标点云的精细分割和高实时性提供了结构保证。针对水无人艇航行过程中喷泵或螺旋桨动力装置产生航行尾流、海面较大涌浪对激光雷达反射而生成的激光雷达点云数据，对后续激光雷达点云数据聚类产生较大影响，需要进行滤除。使用垂直属性进行传统RANSAC算法的改进，并对数据遍历优化改进，实现高效的无效激光雷达点云数据滤除。采用改进的DBSCAN聚类算法对激光雷达点云数据进行聚类。采用基于密度的DBSCAN聚类算法，提出了一种自适应参数的改进DBSCAN算法，可以对较远处的障碍物点云数据进行聚类，具备探测较远的障碍物目标。这样既保留了传统的DBSCAN在近距离点云聚类时的优点，又能对远距离的点云数据正确地聚类。

需要强调的是，本发明所述实施例是说明性的，而不是限定性的，因此本发明包括并不限于具体实施方式中所述实施例，凡是由本领域技术人员根据本发明的技术方案得出的其他实施方式，同样属于本发明保护的范围。

Claims (4)

1.一种复杂海况下基于激光雷达的水面无人艇航行障碍物检测方法，其特征在于：包括以下步骤：

步骤1、对激光雷达获取的点云数据进行多云多帧融合处理，进而构建水面无人艇的前视投影栅格地图；

步骤2、基于步骤1所构建的水面无人艇的前视投影栅格地图，针对无人艇海面上存在碎波、尾浪产生的干扰，导致很多反射无效的点云数据，采用垂直属性进行传统RANSAC算法的改进，并对数据遍历优化改进，实现高效的杂波干扰点云数据的滤除；

步骤3、基于步骤2获得杂波较少的水面无人艇航行障碍物点云数据信息，采用自适应参数的改进DBSCAN算法对激光雷达获取的点云数据进行聚类处理，获得水面无人艇航行障碍物信息。

2.根据权利要求1所述的一种复杂海况下基于激光雷达的水面无人艇航行障碍物检测方法，其特征在于：所述步骤1的具体步骤包括：

(1)对激光雷达获取的点云数据进行三帧点云融合处理，对于得到的姿态与位移信息，采用欧拉角与旋转矩阵结合的方法，进行坐标变换：

所述步骤1第(1)步的具体步骤包括：

①激光雷达坐标系绕惯导坐标系x轴旋转的角度为其滚动角，滚动角取值范围为：-π～π；

在滚动角作用下，从原始坐标系到新坐标系下的坐标变换可用旋转矩阵公式(1)表示：

激光雷达坐标系绕惯导坐标系y轴旋转的角度为其俯仰角，俯仰角的取值范围为：-π/2～π/2；同理，由俯仰角导致的新旧坐标系坐标变化的旋转矩阵公式如下公式(2)所示：

雷达坐标系绕惯导坐标系z轴旋转的角度为其偏航角，航向角的取值范围为：-π～π；由偏航角导致的新旧坐标系坐标变化的旋转矩阵公式(3)所示；

②用欧拉角表示旋转变化时，在给定三轴旋转角度的同时，必须指定旋转顺序，指定的旋转顺序为：偏航角-俯仰角-滚动角，最终的旋转矩阵则是上面三个旋转矩阵按先后顺序相乘的结果；

(2)构建水面无人艇的栅格地图：

以激光雷达为坐标原点，设载体前进方向为X轴正方向，垂直前进方向的右手侧为Y轴正方向；根据激光雷达量程设定X轴与Y轴截断距离分别为X_turn与Y_turn，并设定栅格尺寸为g_cell；

输入点云P＝{P_i|P_i＝(x_i,y_i,z_i),i＝1,2,...,N}进行XY平面投影，并进行栅格化划分，其中点云中任意点p_i在栅格地图的坐标计算方法如式(4)所示：

至此，栅格地图表示为集合G＝{G_x,y＝(g_x,g_y)|g_x＝0,1,...,H,g_y＝0,1,...,W},，其行分辨率为H＝[2*X_turn/g_cell]，列分辨率为W＝[2*Y_turn/g_cell]；

(3)构建水面无人艇的前视投影栅格图：

设激光雷达在某一发光时刻，某激光器采集的原始信息为水平角度测量值θ_horizon，水平角度偏移α_{HorizonOffset}，实际激光点水平发射角表示如式(5)所示：

θ_horizon＝θ_measure+α_{HorizonOffset} (5)

故该激光点在前视栅格中的纵坐标计算如式(6)所示：

横坐标计算如式(7)所示：

row＝R_index (7)

由此，前视投影栅格图构建完毕。

3.根据权利要求1所述的一种复杂海况下基于激光雷达的水面无人艇航行障碍物检测方法，其特征在于：所述步骤2的具体方法为：

(1)基于步骤1所构建的水面无人艇的前视投影栅格地图，输入点云集的改进：尾流本身破碎且水平，障碍物点云反射稳定且较垂直，故可利用前视栅格图进行垂直属性标记后，可从垂直属性为水平和未知的栅格中提取对应数据点，将其作为RANSAC的输入点集；

(2)种子点的选取优化：将雷达视场划分为局部区域，取局部区域内输入点集中绝对高度最低点作为种子点，将局部区域中所有输入点集绝对高度进行TopK排序，取高度值最小的K个点，滤除K个点中小于中位数三倍标准差的异常点，之后得到剩余点的高度均值，将高度小于该均值的点作为初始种子点；

(3)经过对输入点集、种子点选取、数据遍历优化的改进，使用最简单的平面模型进行平面点估计，将拟合的平面包含的点集作为尾流点滤除：

其中，n＝[a b c]^T为估计平面法向量，X＝[x y z]^T为平面上任意一点；对于法向量，通过计算局内点协方差矩阵的方式进行求解：

其中

为种子点的均值；协方差矩阵刻画了点集的散布特性，利用SVD分解求解三个特征值与对应特征向量，可获得点集空间分布的三个主方向；由于尾流为平面，故平面的垂直法向量即最小特征值所计算得到的特征向量。

4.根据权利要求1所述的一种复杂海况下基于激光雷达的水面无人艇航行障碍物检测方法，其特征在于：所述步骤3的具体步骤包括：

(1)基于步骤1所构建的水面无人艇的前视投影栅格地图，根据激光雷达扫描线分布求出相邻两条扫描线的间距建立ε^*列表，然后依据每个扫描点的坐标值在列表中查找出对应的列表值，最后通过线性插值法确定对应的邻域值；

所述ε^*列表的建立方法是：假设激光雷达共有n条扫描线，无人艇安装的激光雷达原点距水平面高度为h时，扫描线的俯仰角构成集合α＝{α_i}(i＝1,2,3,...,n)，激光雷达的扫描线投射点到O点的径向距离构成集合r＝{r_i}(i＝1,2,3,...,n)，相邻两条激光扫描线投射点的距离构成集合ε＝{ε_i}(i＝1,2,3,...,n-1)，r，ε对应元素满足r_i＝h/tan(α_i)(i＝1,2,3,...,n)，ε_i＝r_i+1-r_i(i＝1,2,3,...,n)；

水面无人艇激光雷达安装高度距离水面为h＝2.5，则集合r修改为r_i＝2.5/tan(α_i)(i＝1,2,3,...,n)，记作r^*；对应的集合ε修改为

记作ε^*；至此，ε^*列表建立完成；

所述通过线性插值法确定对应的邻域值的方法为：

假设存在一个三维空间点P(x,y,z)，到z轴的距离

对应的

在r^*列表中寻找

使得

在ε^*列表中存在对应的

根据一阶线性插值法，则任意点P(x,y,z)处的ε值为：

(2)基于对激光雷达获取的点云数据进行聚类处理，获得水面无人艇航行障碍物信息。

Images