基于三维激光雷达的无人船水面目标检测及路径规划方法

摘要

本发明公开了一种基于三维激光雷达的无人船水面目标检测及路径规划方法，意在感知水面环境并根据环境信息进行自主导航。首先定义激光雷达坐标系，然后对激光雷达参数进行标定，再将极坐标下的角度和距离信息转化为笛卡尔坐标系下的坐标，接下来为了区分地面点云与水面点云，进行地面分割，对地面点云进行过滤，从而减少后续算法的计算量，然后对激光雷达扫描到的水面障碍物进行聚类，最终根据障碍物信息进行水面路径规划，使无人船在水面上避开障碍物规划出一条路径，以确保可以安全、准确地到达指定的目标点。

说明书

技术领域

本发明属于目标检测技术领域，具体涉及一种目标检测及路径规划方法。

背景技术

随着计算及性能和传感技术的不断提高，自动驾驶技术发展迅速，其应用领域已从陆地扩展到了空中和水面，而无人船则是典型的水面无人驾驶设备。为了实现无人船的安全航行，需要进行水面路径规划，环境感知是进行路径规划的前提条件。

激光雷达是无人船进行水面环境感知中最常用的传感器之一，其是一种使用激光探测位置、形状和速度等几何特征的系统。它向目标物体发射激光，然后接收这些物体反射的信号。通过比较接收信号与发送信号的相位差，可以得到目标物体的信息，如距离、反射率等。通过进一步的算法，还可以得到位置、高度、速度、姿态和形状。一般来说，激光雷达能够探测到精度为厘米级的目标。常见的二维激光雷达能在较宽的范围内提供周围船只的相对方位和距离，具有合理的探测性能。然而，由于传感器的传感特性和环境干扰，当传感器在近距离到达盲区时，二维激光雷达的检测性能会下降。此外，二维激光雷达的采样率低、分辨率低，因此在识别高速船只或突出水面的小物体时，二维激光雷达的检测性能会下降。因此，需要增加传感器的维度和精度从而实现对无人船周围水面环境更为精确的检测与感知。三维激光雷达由垂直排列的多个激光束组成，通过旋转激光器来收集周围障碍物信息，获得360°的环境全景，从而提高目标检测的性能。

在获取到周围环境的障碍物信息后，相应地，进行环境地图构建和路径规划便成为了下一步的研究目标。无人船作为小型水面平台，需要在不可预测的环境中进行自主导航，如何确保航行的安全是重中之重。为了满足这一要求，无人船需具备强大的环境感知能力，即无人船可以从周围环境中收集数据，构造相应的环境信息模型进行分析，从而规划出一条安全可行的航行路径，以顺利执行航行任务。目前常见的路径规划算法有Dijkstra算法、A

发明内容

为了克服现有技术的不足，本发明提供了一种基于三维激光雷达的无人船水面目标检测及路径规划方法，意在感知水面环境并根据环境信息进行自主导航。首先定义激光雷达坐标系，然后对激光雷达参数进行标定，再将极坐标下的角度和距离信息转化为笛卡尔坐标系下的坐标，接下来为了区分地面点云与水面点云，进行地面分割，对地面点云进行过滤，从而减少后续算法的计算量，然后对激光雷达扫描到的水面障碍物进行聚类，最终根据障碍物信息进行水面路径规划，使无人船在水面上避开障碍物规划出一条路径，以确保可以安全、准确地到达指定的目标点。

本发明解决其技术问题所采用的技术方案包括以下步骤：

步骤1：定义坐标系；

定义船体坐标系o

定义激光雷达坐标系o

步骤2：标定激光雷达参数；

所述激光雷达被固定在无人船顶端，以水面为参考，将激光雷达所在位置的(x,y,z)坐标设为(0,0,-1)；

步骤3：激光雷达点云坐标映射；

将极坐标下的角度和距离信息转化为笛卡尔坐标系下的x,y,z坐标，转换关系为

式中，r为实测距离，ω为激光的垂直角度，α为激光的水平旋转角度，x,y,z为极坐标投影到笛卡尔坐标系X,Y,Z轴上的坐标；

步骤4：地面点云分割；

步骤4-1：将激光雷达获取到的单帧点云数据作为输入，定义为P

步骤4-2：随机选取种子点，计算平面模型；对地面使用平面模型进行表示：

ax+by+cz＝d (1)

其中，a、b、c、d分别为不同的计算出的平面模型参数；

式(1)等号左右同时除以a，得：

进而表示为：

x+αy+βz＝χ (2)

其中，α、β、χ分别表示不同的简化后的平面模型参数；

步骤4-3：根据阈值Th

步骤4-4：创建两个列表L

步骤4-5：重复迭代步骤4-2到步骤4-4，直到最大迭代次数N

步骤4-6：选择迭代过程中包含地面点最多的平面模型作为最优地面模型，并输出对应的地面点云P

步骤5：水面障碍物聚类；

步骤5-1：输入非地面点云P

步骤5-2：根据输入的非地面点云P

步骤5-3：创建存储聚类对象的列表C和存储被检测点的列表Q，随机选择P

步骤5-4：将障碍物列表C中的障碍物点数大于等于N

步骤5-5：输出聚类的障碍物列表C；

步骤6：根据障碍物信息进行水面路径规划；

步骤6-1：首先产生第一个节点x

步骤6-2：在产生随机点x

步骤6-3：定义步进变量EPS，当找到x

步骤6-4：障碍物的碰撞机制；

步骤6-4-1：若障碍物为近似圆形，将圆形障碍物进行线性化处理，判断x

如果圆形障碍物的圆心坐标为(x

x

y

步骤6-4-2：若障碍物为矩形，在扩展KD-Tree的过程中，由x

第一步：判断x

第二步：x

1、x

2、x

通过对每一个障碍物进行上述逻辑判断使KD-Tree的扩展避开障碍物，从而在X

本发明的有益效果如下：

本发明的无人船可以从周围环境中收集数据，构造相应的环境信息模型进行分析，从而规划出一条安全可行的航行路径，以顺利执行航行任务。

附图说明

图1是本发明方法激光雷达坐标系。

图2是本发明方法雷达极坐标和XYZ坐标映射示意图。

图3是本发明实施例激光雷达扫描轮廓图。

图4是本发明实施例路径规划两点均位于矩形外侧且位于某一条边两侧与矩形相交情况。

图5是本发明实施例路径规划碰撞机制数学模型。

具体实施方式

下面结合附图和实施例对本发明进一步说明。

一种基于三维激光雷达的无人船水面目标检测及路径规划方法，包括以下步骤：

步骤1：为了表示激光点云与激光雷达位置之间的关系，定义坐标系；

定义船体坐标系o

定义激光雷达坐标系o

步骤2：标定激光雷达参数；

由于激光雷达被固定在无人船顶端，距离地面1.2m，当无人船在水面中航行时，无人船会下沉0.2m，又由于激光雷达坐标系{L}中z轴是垂直地面向下的，故以水面为参考，将激光雷达所在位置的(x,y,z)坐标设为(0,0,-1)；

步骤3：激光雷达点云坐标映射；

由于雷达封装的数据包仅为水平旋转角度和距离参量，为了呈现三维点云图的效果，将极坐标下的角度和距离信息转化为笛卡尔坐标系下的x,y,z坐标。这样，当激光雷达扫描到物体时，会呈现出如图3所示的类似双曲线分布轮廓图。

如图2所示，将极坐标下的角度和距离信息转化为笛卡尔坐标系下的x,y,z坐标，转换关系为

式中，r为实测距离，ω为激光的垂直角度，α为激光的水平旋转角度，x,y,z为极坐标投影到笛卡尔坐标系X,Y,Z轴上的坐标；

步骤4：地面点云分割；

通过三维激光雷达进行障碍物检测的过程中，激光雷达的点云数据是一帧一帧地进行传输的，每一帧激光点云数据不仅包括障碍物信息，同时还存在大量的地面数据，比如湖岸线以及岸上的树木和行人等。而地面分割就是将原始点云数据中的地面点和非地面点进行准确的分割，去除地面上的点云数据，保留非地面点，即水面障碍物信息。通过对地面点云的分割可以减少目标检测过程中来自地面的干扰，提高目标检测的准确度。在平静的水面中，地面点占据了激光点云数据的绝大部分，对地面点进行分割可以在后续处理中减少不必要的计算。

步骤4-1：将激光雷达获取到的单帧点云数据作为输入，定义为P

步骤4-2：随机选取种子点，计算平面模型；对地面使用平面模型进行表示：

ax+by+cz＝d (1)

其中，a、b、c、d分别为不同的计算出的平面模型参数；

式(1)等号左右同时除以a，得：

进而表示为：

x+αy+βz＝χ (2)

其中，α、β、χ分别表示不同的简化后的平面模型参数；

步骤4-3：根据阈值Th

步骤4-4：创建两个列表L

步骤4-5：重复迭代步骤4-2到步骤4-4，直到最大迭代次数N

步骤4-6：选择迭代过程中包含地面点最多的平面模型作为最优地面模型，并输出对应的地面点云P

步骤5：水面障碍物聚类；

通过地面分割，得到了单帧点云数据的非地面点，根据激光雷达的工作原理可以知道，得到的是大环境下所有障碍物的单点位置信息和强度信息，要想获取真实的障碍物几何形状，位置等特征信息，必须将扫描到的某一障碍物的所有点通过某种规则进行整合。障碍物检测就是通过聚类算法，将点云分割成多个单独的障碍物，从而实现对各障碍物的检测目的。聚类就是根据一组对象中各个元素间在某些属性方面的相似程度，将这些对象分成若干类。

步骤5-1：输入非地面点云P

步骤5-2：根据输入的非地面点云P

步骤5-3：创建存储聚类对象的列表C和存储被检测点的列表Q，随机选择P

步骤5-4：将障碍物列表C中的障碍物点数大于等于N

步骤5-5：输出聚类的障碍物列表C；

步骤6：根据障碍物信息进行水面路径规划；通过激光雷达获取到无人船周围的障碍物信息后，根据路径规划算法使无人船在水面上避开障碍物规划出一条路径，以确保可以安全、准确地到达指定的目标点。

步骤6-1：首先产生第一个节点x

步骤6-2：在产生随机点x

步骤6-3：定义步进变量EPS，当找到x

步骤6-4：障碍物的碰撞机制；

为了简化问题，聚类用于路径规划的障碍物均视作较为规则的几何形，如圆，多边形等。

步骤6-4-1：对于圆形障碍物来说，圆形边界的判断属于非线性问题，通常将圆形进行线性化处理(转化为多边形)。只需要判断x

x

y

步骤6-4-2：若障碍物为矩形，在扩展KD-Tree的过程中，由x

第一步：判断x

第二步：x

1、x

2、如图4所示，x

如图5所示，x

式中，k表示直线的斜率。

整个碰撞检测过程的逻辑如下：

对于矩形的一条边设定一个布尔值bool

定义一个判断函数judge(M,N,P)，其中：

M＝(x

judge(M,N,P)＝((y-y

所以judge函数是一个布尔函数，当等式右边为真时judge＝1，反之judge＝0。则对于每一个边，判断逻辑可以写成

bool

and

(judge(x

式中，Vertex

针对图5中的具体情况:

布尔函数表示为：

bool

and

(judge(x

首先判断且逻辑左半部分，

judge(x

judge(x

所以，(judge(x

接着判断且逻辑的右半部分，

judge(x

judge(x

所以，(judge(x

因此bool

bool函数且逻辑左半部分与x

judge(x

judge(x

所以，(judge(x

因此x

其他边的判断方法是相同的，当且仅当

noColision＝(bool

才表明x

通过对每一个障碍物进行上述逻辑判断使KD-Tree的扩展避开障碍物，从而在X