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

Abstract

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

Description

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

技术领域

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

背景技术

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

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

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

发明内容

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

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

步骤1：定义坐标系；

定义船体坐标系o_Bx_By_Bz_B，记为{B}；船体坐标系的原点o_B为无人船的浮心，轴x_B沿船体轴线方向并指向前方，轴y_B在水平面内垂直于x_B，轴z_B垂直水平面向上；

定义激光雷达坐标系o_Lx_Ly_Lz_L，记为{L}；激光雷达坐标系x_L轴和y_L轴分别与船体坐标系的x_B轴和y_B轴方向一致，z_L轴方向与船体坐标系的z_B轴相反，垂直水平面向下；

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

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

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

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

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

步骤4：地面点云分割；

步骤4-1：将激光雷达获取到的单帧点云数据作为输入，定义为P_in；初始化平面模型最大迭代次数N_iter和判断数据点是否属于地面点的阈值Th_g；

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

ax+by+cz＝d (1)

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

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

进而表示为：

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

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

步骤4-3：根据阈值Th_g将点云P_in分割为地面点云P_g和非地面点云P_ng；

步骤4-4：创建两个列表L_g和L_ng，将地面点云P_g和非地面点云P_ng分别加入L_g和L_ng；

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

步骤4-6：选择迭代过程中包含地面点最多的平面模型作为最优地面模型，并输出对应的地面点云P_{g_max}和非地面点云P_{ng_min}；

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

步骤5-1：输入非地面点云P_{ng_min}、初始化参数d_th、N_min；d_th为判断邻域点是否为同一障碍物的聚类阈值，N_min为障碍物包含的最小点数；

步骤5-2：根据输入的非地面点云P_{ng_min}创建Kd-Tree，Kd-Tree使用二叉树原理对点云数据进行划分，使用点云x,y,z三个维度，首先按x方向将点云分为两半，然后再按y方向进行对分，接着按照z方向进行对分，再继续循环构建一个多维的对分查找树结构Kd-Tree；

步骤5-3：创建存储聚类对象的列表C和存储被检测点的列表Q，随机选择P_{ng_min}的一点p_i并将其放入列表Q，然后检索出p_i在P_{ng_min}中所有距离小于聚类阈值d_th的近邻点并将所有/>放入列表Q，然后将Q放入列表C中，作为一种聚类结果，同时清空列表Q；然后继续对P_in的其他点进行相同的操作，直到所有点都被处理完；

步骤5-4：将障碍物列表C中的障碍物点数大于等于N_min以上的障碍物判定为有效障碍物；

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

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

步骤6-1：首先产生第一个节点x_init属于自由空间X_free，在每一次循环中，产生一个随机点x_rand，随机点的生成是任意的，即在整个状态空间X内任意地点；

步骤6-2：在产生随机点x_rand后，遍历Kd-Tree中的每一个节点，计算每一个节点与本次循环生成的随机点之间的距离，找出距离此随机点x_rand最近的节点，记为x_near；

步骤6-3：定义步进变量EPS，当找到x_near时，朝向x_near与x_rand连线方向扩展EPS步长，扩展后产生新的节点x_new；

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

步骤6-4-1：若障碍物为近似圆形，将圆形障碍物进行线性化处理，判断x_new的横纵坐标是否在圆内：如果x_new位于圆形障碍物的外接正方形内，就视为碰撞；

如果圆形障碍物的圆心坐标为(x₀,y₀)，半径为r，考虑无人船的尺寸，对障碍物进行膨化处理，膨胀尺寸inf，所以碰撞条件为：

x₀-r-inf<x_new,x<x₀+r+inf

y₀-r-inf<x_new,y<y₀+r+inf

步骤6-4-2：若障碍物为矩形，在扩展KD-Tree的过程中，由x_near到x_new连接的边不能与矩形障碍物的任何一边相交，即将矩形障碍物碰撞检测问题转化为直线与矩形相交问题；直线与矩形相交的判断分为两步：

第一步：判断x_near与x_new是否在矩形同一条边一条边的一侧：如果x_near与x_new在矩形任意一条边的同侧，则x_near与x_new连线必定不与矩形相交；如果x_near与x_new位于同一条边的两侧，则进行第二步判断；

第二步：x_near与x_new在矩形任意一边的不同侧时，分为两种情况：

1、x_new位于矩形内部，则x_near与x_new连线必定与矩形相交；

2、x_near与x_new两点均在矩形外部，当x_near与x_new位于矩形外侧且位于矩形上边的两侧，但两点连线与矩形相交，利用直线与矩形的性质进行避碰计算；

通过对每一个障碍物进行上述逻辑判断使KD-Tree的扩展避开障碍物，从而在X_free中进行路径规划。

本发明的有益效果如下：

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

附图说明

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

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

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

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

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

具体实施方式

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

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

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

定义船体坐标系o_Bx_By_Bz_B，记为{B}；船体坐标系的原点o_B为无人船的浮心，轴x_B沿船体轴线方向并指向前方，轴y_B在水平面内垂直于x_B，轴z_B垂直水平面向上；

定义激光雷达坐标系o_Lx_Ly_Lz_L，记为{L}；激光雷达坐标系x_L轴和y_L轴分别与船体坐标系的x_B轴和y_B轴方向一致，z_L轴方向与船体坐标系的z_B轴相反，垂直水平面向下；如图1所示；

步骤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_in；初始化平面模型最大迭代次数N_iter和判断数据点是否属于地面点的阈值Th_g；

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

ax+by+cz＝d (1)

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

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

进而表示为：

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

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

步骤4-3：根据阈值Th_g将点云P_in分割为地面点云P_g和非地面点云P_ng；

步骤4-4：创建两个列表L_g和L_ng，将地面点云P_g和非地面点云P_ng分别加入L_g和L_ng；

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

步骤4-6：选择迭代过程中包含地面点最多的平面模型作为最优地面模型，并输出对应的地面点云P_{g_max}和非地面点云P_{ng_min}；

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

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

步骤5-1：输入非地面点云P_{ng_min}、初始化参数d_th、N_min；d_th为判断邻域点是否为同一障碍物的聚类阈值，N_min为障碍物包含的最小点数；

步骤5-2：根据输入的非地面点云P_{ng_min}创建Kd-Tree，Kd-Tree是一种为了对高维度数据快速查询的数据结构。从本质上说其是一种二叉树，Kd-Tree使用二叉树的原理对k维数据进行划分，每一次划分采用数据的不同维度，在本发明处理的点云数据进行Kd-Tree构建的过程中，使用点云x,y,z三个维度，首先按x方向将点云分为两半，然后再按y方向进行对分，接着按照z方向进行对分，再继续循环构建一个多维的对分查找树结构Kd-Tree；

步骤5-3：创建存储聚类对象的列表C和存储被检测点的列表Q，随机选择P_{ng_min}的一点p_i并将其放入列表Q，然后检索出p_i在P_{ng_min}中所有距离小于聚类阈值d_th的近邻点并将所有/>放入列表Q，然后将Q放入列表C中，作为一种聚类结果，同时清空列表Q；然后继续对P_in的其他点进行相同的操作，直到所有点都被处理完；

步骤5-4：将障碍物列表C中的障碍物点数大于等于N_min以上的障碍物判定为有效障碍物；N_min可以过滤由于地面分割不完全或水面涟漪造成的少数噪声点，这样可以甄别出有效的障碍物。

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

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

步骤6-1：首先产生第一个节点x_init属于自由空间X_free，在每一次循环中，产生一个随机点x_rand，随机点的生成是任意的，即在整个状态空间X内任意地点；

步骤6-2：在产生随机点x_rand后，遍历Kd-Tree中的每一个节点，计算每一个节点与本次循环生成的随机点之间的距离，找出距离此随机点x_rand最近的节点，记为x_near；

步骤6-3：定义步进变量EPS，当找到x_near时，朝向x_near与x_rand连线方向扩展EPS步长，扩展后产生新的节点x_new；

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

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

步骤6-4-1：对于圆形障碍物来说，圆形边界的判断属于非线性问题，通常将圆形进行线性化处理(转化为多边形)。只需要判断x_new的横纵坐标是否在圆内，这里我们规定如果x_new位于圆形障碍物的外接正方形内，就视为碰撞。如果圆形障碍物的圆心坐标为(x₀,y₀)，半径为r，考虑无人船的尺寸，对障碍物进行膨化处理，膨胀尺寸inf，所以碰撞条件为：

x₀-r-inf<x_new,x<x₀+r+inf

y₀-r-inf<x_new,y<y₀+r+inf

步骤6-4-2：若障碍物为矩形，在扩展KD-Tree的过程中，由x_near到x_new连接的边不能与矩形障碍物的任何一边相交，即将矩形障碍物碰撞检测问题转化为直线与矩形相交问题；直线与矩形相交的判断分为两步：

第一步：判断x_near与x_new是否在矩形同一条边一条边的一侧：如果x_near与x_new在矩形任意一条边的同侧，则x_near与x_new连线必定不与矩形相交；如果x_near与x_new位于同一条边的两侧，则进行第二步判断；

第二步：x_near与x_new在矩形任意一边的不同侧时，分为两种情况：

1、x_new位于矩形内部，则x_near与x_new连线必定与矩形相交；

2、如图4所示，x_near与x_new两点均在矩形外部，当x_near与x_new位于矩形外侧且位于矩形上边的两侧，但两点连线与矩形相交，利用直线与矩形的性质进行避碰计算；

如图5所示，x_near与x_new位于矩形障碍物AB边的两侧且均位于矩形的外侧，两点连线与矩形相交，Dx_near与Ax_near是两个边界，即当x_near与x_new连线位于Dx_near与Ax_near两直线下方时，x_near与x_new连线必定与矩形相交。反之，若不在Dx_near与Ax_near两直线下方则表现为不与障碍物发生碰撞。对于AD边来说必发生碰撞的过程可以用如下公式表示：

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

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

对于矩形的一条边设定一个布尔值bool_i，当bool_i＝1时，表示发生碰撞，当bool_i＝0时，表示不发生碰撞。

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

M＝(x₁,y₁),N＝(x₂,y₂),P＝(x,y)

judge(M,N,P)＝((y-y₁)(x₂-x₁)>(y₂-y₁)(x-x₁))

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

bool_i＝(judge(x_near,Vertex₁,Vertex₂)≠judge(x_new,Vertex₁,Vertex₂))

and

(judge(x_near,x_new,Vertex₁)≠judge(x_near,x_new,Vertex₂))

式中，Vertex_i表示矩形障碍物某一条边的两个定点。

针对图5中的具体情况:

布尔函数表示为：

bool_i＝(judge(x_near,A,D)≠judge(x_new,A,D))

and

(judge(x_near,x_new,A)≠judge(x_near,x_new,D))

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

judge(x_near,A,D)＝((y_D-y₁)(x_A-x₁)>(y_A-y₁)(x_D-x₁))＝1

judge(x_new,A,D)＝((y_D-y₂)(x_A-x₂)>(y_A-y₂)(x_D-x₂))＝0

所以，(judge(x_near,A,D)≠judge(x_new,A,D))＝1，意味着x_near与x_new位于AD边的两侧。

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

judge(x_near,x_new,A)＝((y_A-y₁)(x₂-x₁)>(y₂-y₁)(x_A-x₁))＝0

judge(x_new,x_new,D)＝((y_D-y₁)(x₂-x₁)>(y₂-y₁)(x_D-x₁))＝1

所以，(judge(x_near,x_new,D)≠judge(x_near,x_new,D))＝1，意味着x_near与x_new连线位于Dx_near与Ax_near两直线下方。根据此判断条件发现，x_new与x″_new属于同一种情况，不必再分情况讨论。

因此bool₁＝1，x_near与x_new连线与矩形这一条边相交，发生碰撞。

bool函数且逻辑左半部分与x_new相同，对于x′_new而言，因为且逻辑左半部分判断两点是否在一条边两侧。且逻辑右半部分

judge(x_near,x′_new,A)＝((y_A-y₁)(x₂-x₁)>(y₂-y₁)(x_A-x₁))＝0

judge(x_new,x′_new,D)＝((y_D-y₁)(x₂-x₁)>(y₂-y₁)(x_D-x₁))＝0

所以，(judge(x_near,x′_new,D)≠judge(x_near,x′_new,D))＝0，故bool₁＝0。

因此x_near与x′_new连线不会与矩形这一条边相交，不会发生碰撞。

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

noColision＝(bool₁＝0&&bool₂＝0&&bool₃＝0&&bool₄＝0)＝1

才表明x_near与x′_new连线不与矩形相交，不会发生碰撞，也将其视为有效点插入KD-Tree中。

通过对每一个障碍物进行上述逻辑判断使KD-Tree的扩展避开障碍物，从而在X_free中进行路径规划。

Claims (1)

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

步骤1：定义坐标系；

定义船体坐标系o_Bx_By_Bz_B，记为{B}；船体坐标系的原点o_B为无人船的浮心，轴x_B沿船体轴线方向并指向前方，轴y_B在水平面内垂直于x_B，轴z_B垂直水平面向上；

定义激光雷达坐标系o_Lx_Ly_Lz_L，记为{L}；激光雷达坐标系x_L轴和y_L轴分别与船体坐标系的x_B轴和y_B轴方向一致，z_L轴方向与船体坐标系的z_B轴相反，垂直水平面向下；

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

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

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

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

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

步骤4：地面点云分割；

步骤4-1：将激光雷达获取到的单帧点云数据作为输入，定义为P_in；初始化平面模型最大迭代次数N_iter和判断数据点是否属于地面点的阈值Th_g；

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

ax+by+cz＝d (1)

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

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

进而表示为：

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

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

步骤4-3：根据阈值Th_g将点云P_in分割为地面点云P_g和非地面点云P_ng；

步骤4-4：创建两个列表L_g和L_ng，将地面点云P_g和非地面点云P_ng分别加入L_g和L_ng；

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

步骤4-6：选择迭代过程中包含地面点最多的平面模型作为最优地面模型，并输出对应的地面点云P_{g_max}和非地面点云P_{ng_min}；

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

步骤5-1：输入非地面点云P_{ng_min}、初始化参数d_th、N_min；d_th为判断邻域点是否为同一障碍物的聚类阈值，N_min为障碍物包含的最小点数；

步骤5-2：根据输入的非地面点云P_{ng_min}创建Kd-Tree，Kd-Tree使用二叉树原理对点云数据进行划分，使用点云x，y，z三个维度，首先按x方向将点云分为两半，然后再按y方向进行对分，接着按照z方向进行对分，再继续循环构建一个多维的对分查找树结构Kd-Tree；

步骤5-3：创建存储聚类对象的列表C和存储被检测点的列表Q，随机选择P_{ng_min}的一点p_i并将其放入列表Q，然后检索出p_i在P_{ng_min}中所有距离小于聚类阈值d_th的近邻点并将所有/>放入列表Q，然后将Q放入列表C中，作为一种聚类结果，同时清空列表Q；然后继续对P_in的其他点进行相同的操作，直到所有点都被处理完；

步骤5-4：将障碍物列表C中的障碍物点数大于等于N_min以上的障碍物判定为有效障碍物；

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

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

步骤6-1：首先产生第一个节点x_init属于自由空间X_free，在每一次循环中，产生一个随机点x_rand，随机点的生成是任意的，即在整个状态空间X内任意地点；

步骤6-2：在产生随机点x_rand后，遍历Kd-Tree中的每一个节点，计算每一个节点与本次循环生成的随机点之间的距离，找出距离此随机点x_rand最近的节点，记为x_near；

步骤6-3：定义步进变量EPS，当找到x_near时，朝向x_near与x_rand连线方向扩展EPS步长，扩展后产生新的节点x_new；

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

步骤6-4-1：若障碍物为近似圆形，将圆形障碍物进行线性化处理，判断x_new的横纵坐标是否在圆内：如果x_new位于圆形障碍物的外接正方形内，就视为碰撞；

如果圆形障碍物的圆心坐标为(x₀，y₀)，半径为r，考虑无人船的尺寸，对障碍物进行膨化处理，膨胀尺寸inf，所以碰撞条件为：

x₀-r-inf＜x_new，x＜x₀+r+inf

y₀-r-inf＜x_new，y＜y₀+r+inf

步骤6-4-2：若障碍物为矩形，在扩展KD-Tree的过程中，由x_near到x_new连接的边不能与矩形障碍物的任何一边相交，即将矩形障碍物碰撞检测问题转化为直线与矩形相交问题；直线与矩形相交的判断分为两步：

第一步：判断x_near与x_new是否在矩形同一条边一条边的一侧：如果x_near与x_new在矩形任意一条边的同侧，则x_near与x_new连线必定不与矩形相交；如果x_near与x_new位于同一条边的两侧，则进行第二步判断；

第二步：x_near与x_new在矩形任意一边的不同侧时，分为两种情况：

1）、x_new位于矩形内部，则x_near与x_new连线必定与矩形相交；

2）、x_near与x_new两点均在矩形外部，当x_near与x_new位于矩形外侧且位于矩形上边的两侧，但两点连线与矩形相交，利用直线与矩形的性质进行避碰计算；

通过对每一个障碍物进行上述逻辑判断使KD-Tree的扩展避开障碍物，从而在X_free中进行路径规划。