CN115267827B - 一种基于高度密度筛选的激光雷达港区障碍物感知方法 - Google Patents

Abstract

本发明公开了一种基于高度密度筛选的激光雷达港区障碍物感知方法，包括：基于共线准则补全输入激光雷达点云数据中的缺失点；采用降采样方法进行点云信息预处理；通过立方体区域内点数量统计，消除海面反射杂波点；采用k维树结构存储预处理后的数据；自适应的聚类搜索距离阈值选取；对预处理完成后的点云数据进行聚类处理；将处理后的聚类结果集合中的每个点转换到无人艇中心的笛卡尔坐标系下，消除横摇、纵摇、偏航的影响；根据聚类结果集合中的高度信息，剔除海杂波等噪声；从聚类结果集合中提取障碍目标的参数。本发明实现较为精确地探测跟踪港区内障碍物的方位、距离、形状，减弱海杂波对目标探测的干扰，保障无人艇在港区内安全航行。

Description

一种基于高度密度筛选的激光雷达港区障碍物感知方法

技术领域

本发明属于无人艇的态势感知方法，特别是一种基于高度密度筛选的激光雷达港区障碍物感知方法。

背景技术

无人艇自主航行在离靠泊以及进出港阶段时，要求能够在预先获取的信息基础上，实时精确感知港区的静止和运动物体的位置、形状、运动信息，根据感知信息动态分析是否需要实施避障。由于港区内区域有限、地物和周边建筑物多、障碍物较为密集，导航雷达存在近区为探测盲区、受地物影响大、对目标的分辨率较低等问题，探测港区时虚警率、漏警率激增。光电、摄像头等光学手段也易受到复杂背景的干扰，目标的误检率、漏检率较高，而且没有距离探测数据。所以传统的导航雷达+光学的探测手段在港区内并不适用。

激光雷达Lidar(Light Detection And Ranging)是一种主动式、以激光束为信息载体的光学遥感技术，融合了传统雷达技术和现代激光技术，大大提高了辐射源的频率，其在大部分方面均优于普通雷达，主要缺点为：作用距离一般小于200米，而且易受物体遮挡。近年来大部分的激光雷达都是面向无人车领域的，使用环境一般为平坦的道路上，探测目标主要为人、车辆、标志物等道路目标，大多为凸形状目标。由于港区内无人艇航行速度相对较慢、海况较为平稳，适合使用激光雷达对港区环境进行感知，而且激光雷达探测精度较高、目标形状较为清晰，能够保障无人艇在狭小区域内也能找到合理的路径避开障碍物。

无人艇的激光雷达应用与无人车差异较大，无人艇在海上受到的天气影响和风浪流的物理影响大，在航行过程中无人艇姿态一直处于不稳定状态，无人艇平台自身姿态将在风、海流、周边船只运动带起的波浪等环境因素影响下出现不规则的横摇、纵摇等运动，这将大大影响激光雷达探测的精度和探测范围。针对无人艇应用激光雷达感知港区障碍物，现有技术存在的主要问题是：

(1)海上环境复杂多变，但由于激光雷达自身特性，激光的反射在海面上会发生散射，在返回数据中表现为一些零散的离散点，形成多个假目标，现有算法不能有效消除这些假目标；

(2)在船只姿态发生频繁的无规律的变化时，激光雷达无法接受到部分返回光束。现有算法不能解决数据点云缺失的问题，对目标跟踪不稳定；

(3)海上聚类的障碍物种类多样，大多为非凸形状，导致点云数据不完备，目标特征不明确；现有算法对点云数据不完备情况考虑较少，处理不稳定；

(4)激光雷达易受海杂波影响，会探测到周边波浪，真实小目标容易会淹没在杂波中；现有算法对海杂波缺乏剔除手段。

发明内容

本发明的目的在于提供一种基于高度密度筛选的激光雷达港区障碍物感知方法，利用无人艇的激光雷达感知输出的点云数据，尽可能地减少无人艇姿态变化、海杂波对探测的影响，较为精准地定位跟踪港区内的动静态目标，支持无人艇实现自主航行。

实现本发明发明目的的技术解决方案为：一种基于高度密度筛选的激光雷达港区障碍物感知方法，包括步骤：

步骤1、基于共线准则补全激光雷达探测输出点云中的缺失点；

步骤2、采用降采样方法进行点云信息预处理，减少点云数据量；

步骤3、通过立方体区域内点数量统计，消除海面反射点；

步骤4、采用k维树结构存储预处理后的数据；

步骤5、自适应的聚类搜索距离阈值选取；

步骤6、对点云数据VRLP进行聚类处理；

步骤7、将处理后的聚类结果集合VRSet中的每个点转换到无人艇中心的笛卡尔坐标系下，消除横摇、纵摇、偏航的影响；

步骤8、根据聚类结果集合VRSet中的高度信息，剔除海杂波等噪声；

步骤9、从聚类结果集合VRSet中提取障碍目标的参数。

本发明与现有技术相比，其显著优点为：

(1)能够根据航行需求优化聚类参数，将激光雷达扫描的点云图进行聚类，分割无人艇周围的障碍物，实现较为精确地确定周围障碍物的方位、距离、形状，这对于后续的避障和路径规划具有重要意义；

(2)能够把航行姿态进行变换，尽可能消除本艇姿态不规则变化的影响；

(3)通过高度过滤，减弱海杂波对目标探测的影响。

下面结合附图对本发明作进一步详细描述。

附图说明

图1是本发明方法的流程示意图。

具体实施方式

设输入点云的集合为LP＝{lp_i},且0<i≤N，其中为自然数域，N为自然数，lp_i＝[lpBm_i,lpDm_i,lpAm_i]，lpBm_i表示lp_i点的方位信息，lpDm_i表示lp_i点的距离信息，lpAm_i表示lp_i点的俯仰信息，都为相对于无人艇激光雷达中心的量测值。结合图1，一种基于高度密度筛选的激光雷达港区障碍物感知方法包括：

步骤1、基于共线准则补全点云中的缺失点

由于激光雷达中激光器是随着电机旋转固定频率发射激光进行扫描点测量的，每条扫描线上的点是按照时序关系进行有序排列的，因此可以根据同一扫描线上的点关联关系补全缺失点；按以下步骤实现：

第一步，将LP按照单帧扫描线归类为LPLineSet＝{LPline_p}，LPline_p表示第p根扫描线，LPline_p＝{lpline_p,t|lpline_p,t在第p根扫描线上}。

第二步，对且则选取距离lpline_p,t最近的左右各两个LP点记为lp_L1,lp_R1,lp_L2,lp_R2。计算各点之间角度：

其中ε为预设的角度门限值。

第三步，若isEmpty_p,t为1则认为lpline_p,t为缺失点，对lp_L1,lp_R1,lp_L2,lp_R2采用线性插值方式进行补全，将补全后的lpline_p,t纳入到LP中LP＝LP+lpline_p,t。

步骤2、采用降采样方法进行点云信息预处理，减少点云数据量。

第一步，将激光雷达感知范围均分为S个3维立方体区域{volRl_j},且0<j≤S，每个volRl_j＝[vRBm_j,up,vRDm_j,up,vRAm_j,up,vRBm_j,dw,vRDm_j,dw,vRAm_j,dw]，其中vRBm_j,up、vRBm_j,dw表示volRl_j区域的方位上、下界，vRDm_j,up、vRDm_j,dw表示volRl_j区域的距离上、下界，vRAm_j,up、vRAm_j,dw表示volRl_j区域的俯仰上、下界。

第二步，对若vRBm_j,dw≤lpBm_i≤vRBm_j,up且vRDm_j,dw≤lpDm_i≤vRDm_j,up且vRAm_j,dw≤lpAm_i≤vRAm_j,up，则将lp_i纳入到volRl_j，这样就把所有的点云划分到不同的立方体区域volRl_j中。

第三步，对于把划分到volRl_j中lp_i的距离、方位、俯仰进行平均，得到volRl_j的质心位置vrlp_j＝[vrlpBm_j,vrlpDm_j,vrlpAm_j]。

第四步，将所有非零的vrlp_j组合成一个新的集合VRLP＝{vrlp_j|vrlp_j≠[0,0,0]}，VRLP就是经过降采样的点云信息，大大减少了原有点云信息数量。

步骤3、通过立方体区域内点数量统计，消除海面反射点；

由于激光雷达海面反射点一般为离散点，可以根据立方体区域内点数量删除离散的海面反射点。即统计每个volRl_j中lp_i的数量NOfvolRl_j，若NOfvolRl_j<α，α为预定的门限值，则将volRl_j从VRLP中删除VRLP＝VRLP-volRl_j；

步骤4、采用k维树结构存储预处理后的数据。

k维树结构VRLPKd是基于二叉树索引树的k维空间点存储数据结构，在多维点数据的检索上能显著增加搜索效率。

第一步，另待处理k维二叉树数据集合kdLPData＝VRLP,数据所在空间集合kdLPRange＝VRLP。

第二步，随着树的深度轮流选择距离、方位、俯仰作为轴垂直分割面vrplsplit，即

其中Tie为二叉树层数；

第三步，确定该层的数据域，按照轴垂直分割面给kdLPData的值排序，例如轴垂直分割面vrplsplit为距离vrplDm，则按vrlpDm_j的值进行排序，位于正中间的那个数据点vrlp_j记为kdLPNode＝[knBm,knDm,knAm]，置kdLPData＝kdLPData-kdLPNode；

如果vrplsplit＝vrplDm，则：

kdLPDataleft＝{vrlp_j|vrlp_j∈kdLPDataand(vrlpDm_j≤knDm)}

kdLPDataright＝{vrlp_j|vrlp_j∈kdLPDataand(vrlpDm_j>knDm)}

如果vrplsplit＝vrpBm，则：

kdLPDataleft＝{vrlp_j|vrlp_j∈kdLPDataand(vrlpBm_j≤knBm)}

kdLPDataright＝{vrlp_j|vrlp_j∈kdLPDataand(vrlpBm_j>knBm)}

如果vrplsplit＝vrpAm，则：

kdLPDataleft＝{vrlp_j|vrlp_j∈kdLPData and(vrlpAm_j≤knAm)}

kdLPDataright＝{vrlp_j|vrlp_j∈kdLPData and(vrlpAm_j>knAm)}

其中kdLPDataleft为待处理左叉树数据集合和kdLPDataright为待处理右叉树数据集合；

第四步，对所有vrplsplit

kdLPrightRange＝{kdLPRange and kdLPDataright}

kdLPleftRange＝{kdLPRange and kdLPDataleft}

其中kdLPrightRange为右空间集合，kdLPleftRange为左空间集合。

另kdleft＝由{kdLPData＝kdLPDataleft，kdLPRange＝kdLPleftRange}建立的k维树结构，kdright＝由{kdLPData＝kdLPDataright，kdLPRange＝kdLPrightRange}建立的k维树结构，并设置kdleft和kdright的父节点为kdLPNode；对kdleft和kdright分别递归执行以上第二到第四步。这样将所有的VRLP存储到k维树结构VRLPKd中。

步骤5、自适应的聚类搜索距离阈值选取

面对多干扰的点云数据，在近处和远处采用不同的搜索半径，以满足不同距离条件下对于障碍物识别的需求。对于不同特征的区域，采取合适的搜索距离阈值，能够提高聚类的精确性和稳定性。选取方法为：

其中r为搜索距离阈值，Dm为与本艇激光雷达的距离，a₁、a₂、a₃为根据不同港区场景优化聚类的预先统计值。

步骤6、对点云数据VRLP进行聚类处理。

第一步，置DealLP＝VRLP，聚类结果集合k＝0。

第二步，建立一个队列

第三步，任取vrlp_j∈VRLP，将vrlp_j在k维树结构VRLPKd中搜索其临近点形成点集VRPs_j。

第四步，判断VRPs_j中的每个点距离点vrlp_j的距离，如果小于阈值r则检查其是否处理过，如果没有，将其放入队列vrQuene中。将形成的队列vrQuene，记为vrSet_k,加入到聚类结果集合中VRSet＝VRSet+vrSet_k，k＝k+1。将队列vrQuene中的点集从DealLP删除DealLP＝DealLP-vrQuene，清空

第五步，重复以上过程，直到将VRLP中所有点处理完，形成聚类集合VRSet。

步骤7、将处理后的聚类结果集合VRSet中的每个点转换到无人艇中心的笛卡尔坐标系下，消除横摇、纵摇、偏航的影响。

设此时无人艇的横摇角、纵摇角、艏向角为[φ,θ,μ]，本艇激光雷达在无人艇中心的笛卡尔坐标系下位置为[Li_x,Li_y,Li_z],则将vrlp_j＝[vrlpBm_j,vrlpDm_j,vrlpAm_j]转换到无人艇中心的笛卡尔坐标系下的位置vrlpXYZ_j＝[vrlpX_j,vrlpY_j,vrlpZ_j]公式为

步骤8、根据聚类结果集合VRSet中的高度信息，剔除海杂波等噪声

由于海杂波的反射点信息一般高度较低，可以通过vrlpZ_j信息进行剔除。方法为：

第一步，对于计算其vrlpZ_j的均值meanZ_k

其中vrlpXYZ_j∈vrSet_k。

第二步，若meanZ_k<β，β为预定的高度门限值，则将vrSet_k从VRSet中剔除VRSet＝VRSet-vrSet_k。

步骤9、从聚类结果集合VRSet中提取障碍目标的参数

对于记target_k＝[tgX_k,up,tgX_k,down,tgY_k,up,tgY_k,down,tgZ_k,up,tgZ_k,down]，计算方法为：

tgX_k,up＝max(vrlpX_j)

tgX_k,down＝min(vrlpX_j)

tgY_k,up＝max(vrlpY_j)

tgY_k,down＝min(vrlpY_j)

tgZ_k,up＝max(vrlpZ_j)

tgZ_k,down＝min(vrlpZ_j)

所有的target_k组成的集合即为输出的障碍目标。

本发明面向港口内区域有限、地物和周边建筑物多、障碍物较为密集的情况，基于无人艇上激光雷达实现对港区内航行障碍物的精确感知。利用无人艇上激光雷达设备探测港区周边输出的点云信息，采用了基于高度密度筛选的欧几里得聚类算法，通过缺失点补全、统计消除海面离散反射点、自适应聚类、消摇处理、高度过滤等方法，将激光雷达扫描的点云图进行聚类，分割无人艇周围的障碍物点云，完成对周边目标的探测跟踪；实现较为精确地探测跟踪港区内障碍物的方位、距离、形状，尽可能消除本艇姿态不规则变化的影响，减弱海杂波对目标探测的干扰，保障无人艇在港区内安全航行。

Claims (10)

1.一种基于高度密度筛选的激光雷达港区障碍物感知方法，其特征在于，包括步骤：

步骤1、基于共线准则补全激光雷达探测输出点云中的缺失点；

步骤2、采用降采样方法进行点云数据预处理；

步骤3、基于预处理点云，通过立方体区域内点数量统计，消除海面反射点；

步骤4、采用k维树结构存储消除海面反射点后的数据；

步骤5、自适应选取聚类搜索距离阈值；

步骤6、基于聚类搜索距离阈值，对点云数据进行聚类处理得到聚类集合；

步骤7、将聚类集合中的每个点转换到无人艇中心的笛卡尔坐标系下；

步骤8、根据聚类结果集合中的高度信息剔除噪声；

步骤9、从聚类结果集合中提取障碍目标的参数，确定障碍目标。

2.根据权利要求1所述的基于高度密度筛选的激光雷达港区障碍物感知方法，其特征在于，所述步骤1中基于共线准则补全激光雷达探测输出点云中的缺失点具体为：

定义点云集合为且0<i≤N，其中为自然数域，N为自然数，lp_i＝[lpBm_i,lpDm_i,lpAm_i]，lpBm_i表示lp_i点的方位信息，lpDm_i表示lp_i点的距离信息，lpAm_i表示lp_i点的俯仰信息，均为相对于无人艇激光雷达中心的量测值；

将LP按照单帧扫描线归类为LPLineSet＝{LPline_p}，LPline_p表示第p根扫描线，LPline_p＝{lpline_p,t|lpline_p,t在第p根扫描线上}；

对且则选取距离lpline_p,t最近的左右各两个LP点记为lp_L1,lp_R1,lp_L2,lp_R2；计算各点之间角度δ_u,v为：

另

其中ε为预设的角度门限值；

若isEmpty_p,t为1则认为lpline_p,t为缺失点，对lp_L1,lp_R1,lp_L2,lp_R2采用线性插值方式进行补全，将补全后的lpline_p,t纳入到LP中LP＝LP+lpline_p,t。

3.根据权利要求2所述的基于高度密度筛选的激光雷达港区障碍物感知方法，其特征在于，所述步骤2中采用降采样方法进行点云数据预处理具体为：

步骤2-1，将激光雷达感知范围均分为S个3维立方体区域且0<j≤S，每个volRl_j＝[vRBm_j,up,vRDm_j,up,vRAm_j,up,vRBm_j,dw,vRDm_j,dw,vRAm_j,dw]，其中vRBm_j,up、vRBm_j,dw表示volRl_j区域的方位上、下界，vRDm_j,up、vRDm_j,dw表示volRl_j区域的距离上、下界，vRAm_j,up、vRAm_j,dw表示volRl_j区域的俯仰上、下界；

步骤2-2，对若vRBm_j,dw≤lpBm_i≤vRBm_j,up且vRDm_j,dw≤lpDm_i≤vRDm_j,up且vRAm_j,dw≤lpAm_i≤vRAm_j,up，则将lp_i纳入到volRl_j，把所有的点云划分到不同的立方体区域volRl_j中；

步骤2-3，对于把划分到volRl_j中lp_i的距离、方位、俯仰进行平均，得到volRl_j的质心位置vrlp_j＝[vrlpBm_j,vrlpDm_j,vrlpAm_j]；

步骤2-4，将所有非零的vrlp_j组合成一个新的点云集合VRLP＝{vrlp_j|vrlp_j≠[0,0,0]}。

4.根据权利要求3所述的基于高度密度筛选的激光雷达港区障碍物感知方法，其特征在于：所述步骤3中通过立方体区域内点数量统计，消除海面反射点，具体为：

根据立方体区域内点数量删除离散的海面反射点，统计每个volRl_j中lp_i的数量NOfvolRl_j，若NOfvolRl_j<α，α为预定的门限值，则将volRl_j从VRLP中删除VRLP＝VRLP-volRl_j。

5.根据权利要求4所述的基于高度密度筛选的激光雷达港区障碍物感知方法，其特征在于，所述步骤4中采用k维树结构VRLPKd存储消除海面反射点后的数据VRLP,具体为：

步骤4-1，另待处理k维二叉树数据集合kdLPData＝VRLP,数据所在空间集合kdLPRange＝VRLP；

步骤4-2，随着树的深度轮流选择距离、方位、俯仰为轴垂直分割面vrplsplit，即

其中Tie为二叉树层数；

步骤4-3，确定该层的数据域，按照轴垂直分割面给kdLPData的值排序，另轴垂直分割面vrplsplit为距离vrplDm，则按vrlpDm_j的值进行排序；将位于正中间的数据点vrlp_j记为kdLPNode＝[knBm,knDm,knAm]，另kdLPData＝kdLPData-kdLPNode；

如果vrplsplit＝vrplDm，则：

kdLPDataleft＝{vrlp_j|vrlp_j∈kdLPDataand(vrlpDm_j≤knDm)}

kdLPDataright＝{vrlp_j|vrlp_j∈kdLPDataand(vrlpDm_j>knDm)}；

如果vrplsplit＝vrpBm，则：

kdLPDataleft＝{vrlp_j|vrlp_j∈kdLPDataand(vrlpBm_j≤knBm)}

kdLPDataright＝{vrlp_j|vrlp_j∈kdLPDataand(vrlpBm_j>knBm)}；

如果vrplsplit＝vrpAm，则：

kdLPDataleft＝{vrlp_j|vrlp_j∈kdLPDataand(vrlpAm_j≤knAm)}

kdLPDataright＝{vrlp_j|vrlp_j∈kdLPDataand(vrlpAm_j>knAm)}；

其中kdLPDataleft为待处理左叉树数据集合和kdLPDataright为待处理右叉树数据集合；

步骤4-4，对所有轴垂直分割面vrplsplit有：

kdLPrightRange＝{kdLPRange andkdLPDataright}

kdLPleftRange＝{kdLPRange andkdLPDataleft}

其中kdLPrightRange为右空间集合，kdLPleftRange为左空间集合；

另kdleft＝由{kdLPData＝kdLPDataleft，kdLPRange＝kdLPleftRange}建立的k维树结构，kdright＝由{kdLPData＝kdLPDataright，kdLPRange＝kdLPrightRange}建立的k维树结构，并设置kdleft和kdright的父域为VRLPKd；对kdleft和kdright分别递归执行步骤4-2-步骤4-4，直至将所有的VRLP存储到k维树结构VRLPKd中。

6.根据权利要求5所述的基于高度密度筛选的激光雷达港区障碍物感知方法，其特征在于，所述步骤5中聚类搜索距离阈值为：

其中r为聚类搜索距离阈值，Dm为与本艇激光雷达的距离，a₁、a₂、a₃为根据不同港区场景优化聚类的预先统计值。

7.根据权利要求6所述的基于高度密度筛选的激光雷达港区障碍物感知方法，其特征在于，所述步骤6中对点云数据进行聚类处理得到聚类集合具体为：

步骤6-1，另待聚类处理集合DealLP＝VRLP，聚类结果集合k＝0；

步骤6-2，建立一个队列

步骤6-3，任取vrlp_j∈VRLP，将vrlp_j在k维树结构VRLPKd中搜索其临近点形成点集VRPs_j；

步骤6-4，判断VRPs_j中的每个点距离点vrlp_j的距离，如果小于阈值r则检查其是否处理过，如果没有，将其放入队列vrQuene中，将形成的队列vrQuene，记为vrSet_k,加入到聚类结果集合中VRSet＝VRSet+vrSet_k，k＝k+1；将队列vrQuene中的点集从DealLP删除DealLP＝DealLP-vrQuene，清空

步骤6-5，重复以上步骤，直到将VRLP中所有点处理完，形成聚类集合VRSet。

8.根据权利要求7所述的基于高度密度筛选的激光雷达港区障碍物感知方法，其特征在于，所述步骤7中将聚类集合中的每个点转换到无人艇中心的笛卡尔坐标系下具体为：

设此时无人艇的横摇角、纵摇角、艏向角为[φ,θ,μ]，本艇激光雷达在无人艇中心的笛卡尔坐标系下位置为[Li_x,Li_y,Li_z],则将vrlp_j＝[vrlpBm_j,vrlpDm_j,vrlpAm_j]转换到无人艇中心的笛卡尔坐标系下的位置vrlpXYZ_j＝[vrlpX_j,vrlpY_j,vrlpZ_j]公式为：

9.根据权利要求8所述的基于高度密度筛选的激光雷达港区障碍物感知方法，其特征在于，所述步骤8中根据聚类结果集合中的高度信息剔除噪声具体为：

步骤8-1，对于计算其vrlpZ_j的均值meanZ_k

其中vrlpXYZ_j∈vrSet_k；

步骤8-2，若meanZ_k<β，则将vrSet_k从VRSet中剔除VRSet＝VRSet-vrSet_k，β为预定的高度门限值。

10.根据权利要求9所述的基于高度密度筛选的激光雷达港区障碍物感知方法，其特征在于，所述步骤9中从聚类结果集合中提取障碍目标的参数具体为：

对于记target_k＝[tgX_k,up,tgX_k,down,tgY_k,up,tgY_k,down,tgZ_k,up,tgZ_k,down]，计算target_k值为：

tgX_k,up＝max(vrlpX_j)

tgX_k,down＝min(vrlpX_j)

tgY_k,up＝max(vrlpY_j)

tgY_k,down＝min(vrlpY_j)

tgZ_k,up＝max(vrlpZ_j)

tgZ_k,down＝min(vrlpZ_j)

所有的target_k组成的集合即为输出的障碍目标。