CN114994705A - 一种基于激光雷达角度校正的无人艇避障方法 - Google Patents

Abstract

本发明公开了一种基于激光雷达角度校正的无人艇避障方法。该无人艇避障方法如下：一、安装在无人艇上的激光雷达设备持续进行数据采集；二、对点云数据执行聚类方法；三、对各聚类中的障碍点分别进行线性判断。四、分别对各个聚类进行扫描矫正。五、根据各个聚类确定的各障碍物位置和宽度，无人艇避开障碍物继续行驶。本发明中基于激光雷达姿态角数据进行障碍物探测点矫正的方法，有效降低因激光雷达姿态变化而导致的障碍物测量误差，使得处理后的障碍物信息更接近于真实数据。本发明矫正误差扫描点后，将原先错误扫描点所导致的机器人不可达区域矫正成可达区域，根据各个扇区的避障权重选择避障路径，使得水面机器人能够实现准确避障。

Description

一种基于激光雷达角度校正的无人艇避障方法

技术领域

本发明属于无人艇避障领域技术，具体涉及一种基于激光雷达角度数据修正的无人艇躲避水面障碍物的方法。

背景技术

针对水面无人艇避障问题，国内外学者已经进行了一些研究。在避障之前，障碍物的探测方法至关重要，目前主流的障碍物探测技术主要分为以下四种：超声波探障技术、立体视觉探障技术、多传感融合探障技术和激光雷达探障技术。超声波探障技术主要是利用声波的回波特性计算出障碍物的位置，由于声波存在多普勒效应，如果行驶速度达到一定程度，会导致数据异常。立体视觉探障技术主要是通过图像处理中的数学原理计算，将平面二维视觉技术扩展到三维空间，获取更多目标物体信息，但是会加大成本跟信息处理的复杂度。多传感融合探障技术主要是将两个或两个以上同类或者异构的传感器信息进行综合处理、优化、合并，从而能获取更多的环境特征表达，但是难点在于融合算法，实际上很难选择合适的可匹配多个相同或不同传感设备的融合算法。激光雷达探障技术主要是利用发射激光线与反射激光线的时间差，通过计算得出障碍点的角度和距离值，该探测系统具有精度高、分辨力强、实时性好等优点。基于传统二维激光雷达的水面无人艇避障方法，都以激光雷达探测的障碍信息是正确的为前提进行避障路径规划，但二维激光雷达在实际的应用中，会因为无人艇装载激光雷达的姿态角变化而导致测量点畸变，与真实测得的障碍物信息不一致。对于任何的避障路径规划方法，只有前期获取障碍物的真实数据，才能实现较好地躲避水面障碍物效果。本发明提出了一种基于激光雷达角度数据修正的无人艇避障方法，利用角度数据修正方法提高障碍物定位效果，根据扇形区域计算避障代价实现避障角度决策。

发明内容

本发明的目的在于提供一种基于激光雷达姿态角数据进行障碍物探测点矫正及有效避障扇区选择的方法，无人艇根据该方法对水面障碍物进行有效避障。

该基于激光雷达角度校正的无人艇避障方法，包括以下步骤：

步骤一、安装在无人艇上的激光雷达设备持续进行数据采集；在激光雷达设备采集到非空的点云数据P后，安装在无人艇上的姿态传感设备检测无人艇当前的俯仰角θ和横滚角φ，并对点云数据P进行中值滤波。

步骤二、对点云数据P执行聚类方法，得到多个聚类P₁,P₂,...,P_k；k为聚类数量。

步骤三、对k个聚类中的障碍点分别进行线性判断。

步骤四、分别对各个聚类进行扫描矫正；若一个聚类中的障碍点呈线性，则执行类长方体障碍扫描点矫正方法；若一个聚类中的障碍点呈非线性，则执行类长方体障碍扫描点矫正方法。

类长方体障碍扫描点矫正方法具体如下：

提取被矫正聚类的各障碍点中的实测边缘扫描点L的极坐标为P_L(r_L,a_L)，以及实测正面垂直点M的极坐标P_M(r_M,a_M)；

根据点L和点M，修正得到无人艇在水平状态下的点J和点I。点J和点I的极坐标P_J、P_I的表达式如下：

其中，Φ、

为两个过渡参数，其表达式如下：

点J为无人艇平视姿态下障碍物的一侧边缘位置；点I为无人艇平视姿态下障碍物正对无人艇的位置。无人艇平视姿态下障碍物的另一侧边缘位置W的获取方式与点J一致；根据点I、J、W，确定障碍物的位置和宽度。

类圆柱体障碍扫描点矫正方法具体如下：

提取被矫正聚类的各障碍点中的实测边缘扫描点D、F的极坐标P_D(r_D,a_D)、P_F(r_F,a_F)、中间点E的极坐标P_E(r_E,a_E)。

要根据点F和点D，修正得到无人艇在水平状态下测得的点H和点C。点H和点C的极坐标P_H、P_C的表达式如下：

其中，Φ、

为两个过渡参数，其表达式如下：

Φ＝sin^-1(cos(|a_D-a_E|)·sinθ+sin(|a_D-a_E|)·sinφ)

点C为无人艇平视姿态下障碍物的一侧边缘位置；点H为无人艇平视姿态下障碍物扫描点中心的位置。无人艇平视姿态下障碍物的另一侧边缘位置A的获取方式与点C一致；根据点A、C，确定障碍物的位置和宽度。

步骤五、根据各个聚类确定的各障碍物位置和宽度，无人艇避开障碍物继续行驶。

作为优选，步骤一中，点云数据P为极坐标形式，点云数据P的第i个障碍点P_i的极坐标点为(r_i,a_i)，其中，r_i表示第i个障碍点与激光雷达之间的直线距离；a_i表示由激光雷达所在的极坐标系原点与第i个障碍物点构成的射线与激光雷达扫描极坐标系0°基准线所成的夹角，i＝1,2,...,N。N为点云数据总的障碍点总数。

作为优选，步骤三中，对同一聚类中进行线性判断的方式采用皮尔逊相关系数法，其具体过程如下：

计算皮尔逊相关系数ρ(X,Y)如下：

其中，X_i表示被判断的聚类的第i个极坐标的距离参数；Y_i表示被判断的聚类中的第i个极坐标的角度参数；μ_X表示被判断的聚类中的所有极坐标距离参数的均值；μ_Y表示被判断的聚类中的所有极坐标角度参数的均值；cov(X,Y)表示被判断的聚类中的所有极坐标距离和角度参数的协方差；σ_X，σ_Y分别表示被判断的聚类中所有极坐标的距离参数标准差和角度参数标准差；ρ(X,Y)表示聚类P_j极坐标的皮尔逊相关系数值。

根据皮尔逊相关系数ρ(X,Y)的数值，确定被判断的聚类中的各障碍点呈线性或非线性。

作为优选，步骤五的具体过程如下：

5-1.对无人艇上的激光雷达传感器的探测范围R_Lidar进行分区，分区数为n，则分辨率

由此，第x个分区的起始角度为λ＝α(x-1)。

5-2.扇区代价评估。将n个分区组成一个集合T{s₁,s₂,...,s_n}，每个元素s_x代表第x个扇区对应的值。先将集合T内所有元素初始化为0；之后，根据障碍物扫描点P_i(r_i,a_i)的极坐标数据，判断该障碍物点位于第x个分区，然后将集合T中的第x个元素s_x的值置1，设值判断如下式所示：

5-3.候选运动方向选择。根据当前更新好的集合T，确定无人艇下一步的运动方向，在集合T中，元素s_x的值为1表示不可行分区，值为0则表示可行分区。元素值为0的连续分区，将构成可行运动区域，无人艇优先选择距离当前无人艇朝向角度最接近的可行运动区域的中间方向，作为行进方向。

本发明具有的有益效果如下：

1、本发明中基于激光雷达姿态角数据进行障碍物探测点矫正的方法，有效降低因激光雷达姿态变化而导致的障碍物测量误差，使得处理后的障碍物信息更接近于真实数据。

2、本发明矫正误差扫描点后，将原先错误扫描点所导致的机器人不可达区域矫正成可达区域，根据各个扇区的避障权重选择避障路径，使得水面机器人能够实现准确避障，更快、更有效地到达目标点。

附图说明

图1是本发明基于激光雷达角度修正的避障方法流程图；

图2是本发明的激光雷达水平扫描示意图；

图3是本发明的激光雷达偏差扫描示意图；

图4是本发明的类长方体障碍物探测误差纠正模型示意图；

图5是本发明的类圆柱体障碍物探测误差纠正模型示意图；

图6是本发明的激光雷达扫描范围分区示意图；

图7是本发明的无人艇航行场景示意图；

图8是本发明的无人艇避障决策示意图；

图9是本发明的类长方体障碍物角度修正前后测量误差对比图；

图10是本发明的类圆柱体障碍物角度修正前后测量误差对比图。

具体实施方式

以下结合附图对本发明内容作进一步说明。

如图1所示，基于激光雷达角度数据修正的无人艇避障方法包括以下主要步骤：

步骤0、获取当前时刻下固定在无人艇上的激光雷达设备返回的点云数据；若无返回点云数据，则无人艇按照当前航行方向继续前进，无需进行后续步骤。若激光雷达设备返回激光雷达扫描点云数据P_i(r_i,a_i),i＝1,2,3,...,N；其中，P_i(r_i,a_i)表示第i个障碍点的极坐标点；ri表示第i个障碍点与激光雷达之间的直线距离；a_i表示由激光雷达所在的极坐标系原点与第i个障碍物点构成的射线与激光雷达扫描极坐标系0°基准线所成的夹角；N为激光雷达设备返回的障碍点总数；之后，获取当前时刻固定在无人艇上的姿态传感设备返回的姿态角数据，包括俯仰角θ和横滚角φ，如图2、图3所示。

步骤1、对步骤0中得到的激光雷达点云数据进行中值滤波处理。中值滤波是一种快速、高效的非线性平滑法，对脉冲干扰级噪声的抑制效果好，滤波后的数据保留原来的变化趋势同时去除了尖峰脉冲对分析造成的影响。主要操作方法是对一个滑动窗口内的数值进行排序，用排序后的中值代替原窗口中心元素的数值，如下式所示：

P_mid＝med{P_i-m,...,P_i-1,P_i,P_i+1,...,P_i+m}

其中，P_mid为经过中值滤波后的数据，med{}表示取数据序列中间值的函数，P_i为单个激光雷达障碍物扫描点数据，m为预设滑动窗口领域的大小，设置为5。

步骤2、将经过步骤1滤波后的激光雷达点云数据，使用K-means聚类进行障碍物扫描点分类，K-means聚类方法简单快速，能够符合无人艇环境探测所需的实时性。

K-means聚类方法的实现步骤分为以下五步：

2-1.为了有效剔除噪声对障碍物探测的影响，本发明中设置聚类个数k大小为3，使得算法最终分成k个分组。

2-2.在经过步骤1处理的激光雷达点云数据中随机选择k个点作为每个簇的初始中心。

2-3.选定数据对象与k个簇中心的距离(欧几里得距离)作为聚类特征值，通过计算每一个数据对象的特征值；将激光雷达点云数据中各离散点分别归类入距离最近的簇中心对应的簇中。

2-4.当所有的离散点都被聚类后，针对每一个簇分别计算出其新的簇中心。

2-5.重复操作步骤2-3和步骤2-4，直到步骤2-4中更新的簇中心与原簇中心的距离小于预先设定的阈值(即簇中心基本不再发生变化)，聚类完成停止。激光雷达点云数据最终分为k组，即激光雷达点云数据P{P₁,P₂,...,P_k}。

步骤3、在进行K-means聚类后，采用皮尔逊相关系数法，对每一聚类里的障碍物点进行线性判断，即判断属于同一类的各数据点是否成线性或者非线性。

皮尔逊相关系数ρ(X,Y)是用当前k组中的第j个聚类P_j里极坐标点之间的协方差cov(X,Y)除以其标准差得到的，如式所示：

其中，X_i表示第j个聚类P_j里的第i个极坐标的距离参数；Y_i表示第j个聚类P_j里的第i个极坐标的角度参数；μ_X表示第j个聚类P_j里的所有极坐标距离参数的均值；μ_Y表示第j个聚类P_j里的所有极坐标角度参数的均值；cov(X,Y)表示第j个聚类P_j里的所有极坐标距离和角度参数的协方差；σ_X、σ_Y分别表示第j个聚类P_j里所有极坐标的距离参数标准差和角度参数标准差；ρ(X,Y)表示第j个聚类P_j极坐标的皮尔逊相关系数值。

皮尔逊相关系数ρ(X,Y)能够使得输出范围归一化到[-1,+1]之间，0代表数据无相关性，负值为负相关，正值为正相关。当两个变量线性关系增强时，相关系数趋于1或-1；当一个变量增大，另一个变量也增大时，表明它们之间是正相关的，相关系数大于0；如果一个变量增大，另一个变量却减小，表明它们之间是负相关的，相关系数小于0；如果相关系数等于0，表明它们之间不存在线性相关关系。在计算同一类数据点的皮尔逊相关系数，将系数的绝对值超过阈值Th的判定为线性相关数据，反之归类为非线性相关数据，本文设定阈值Th为0.9。

步骤4、根据步骤3得到的各个聚类对应的皮尔逊相关系数，确定各个聚类中的数据是否呈线性；根据线性判断结果，对激光雷达扫描面为线性或者非线性的障碍物采取不同的矫正方法。由于无人艇配置的惯导等姿态传感器，可以精确地测量出无人艇当前的姿态角度，因此可以根据当前的姿态角度对激光雷达数据进行校准。将数据点成线性相关的聚类执行步骤4-1，非线性相关的聚类执行步骤4-2。类长方体障碍物与类圆柱体障碍物模型如图4和图5所示。

4-1.类长方体障碍扫描点矫正

对类长方体障碍物扫描点进行矫正，需要建立在两个前提条件下：1、在俯视障碍物视野下，存在激光雷达扫描线与障碍物正面垂直的扫描点；2、在正视障碍物视野下，最左与最右扫描点均落在类长方体障碍物两侧边缘。

提取任意一个聚类的长方体障碍物的实测边缘扫描点L的极坐标P_L(r_L,a_L)，实测正面垂直点M(即在俯视视角下与障碍物正面垂直的激光雷达扫描线探测的障碍物扫描点)的极坐标P_M(r_M,a_M)；

由于无人艇在水中航行时的姿态角变化，给极坐标P_L(r_L,a_L)和P_D(r_D,a_D)可能是在无人艇头端处于倾斜向上或向下的姿态下测得，故需要根据点L和点M，修正得到无人艇在水平状态下测得的点J和点I。点J和点I的极坐标P_J、P_I的表达式如下：

其中，Φ、

为两个过渡参数，其数值通过ΔIOM，ΔJOL，ΔVOM求得，表达式如下：

点J为无人艇平视姿态下障碍物的一侧边缘位置；点I为无人艇平视姿态下障碍物正对无人艇的位置。无人艇平视姿态下障碍物的另一侧边缘位置W的获取方式与点J一致；根据点I、J、W，确定障碍物的位置和宽度，以便于无人艇避开障碍物。

4-2.类圆柱体障碍扫描点矫正

对类圆柱体障碍物扫描点进行矫正，需要满足在正视障碍物下，最左与最右扫描点均落在类长方体障碍物两侧边缘。

提取任意一个聚类的圆柱体障碍物的实测边缘扫描点D、F的极坐标P_D(r_D,a_D)，P_F(r_F,a_F)，中间点E的极坐标P_E(r_E,a_E)，为扫描点D、F连线的中点；

由于无人艇在水中航行时的姿态角变化，给极坐标P_G(r_G,a_G)和P_D(r_D,a_D)可能是在无人艇头端处于倾斜向上或向下的姿态下测得，故需要根据点G和点D，修正得到无人艇在水平状态下测得的点H和点C。点H和点C的极坐标P_H、P_C的表达式如下：

其中，Φ、

为两个过渡参数，其数值可通过ΔEOD、ΔBOE、ΔCOD求得，计算如下：

Φ＝sin^-1(cos(|a_D-a_E|)·sinθ+sin(|a_D-a_E|)·sinφ)

点C为无人艇平视姿态下障碍物的一侧边缘位置；点H为无人艇平视姿态下障碍物扫描点中心的位置。无人艇平视姿态下障碍物的另一侧边缘位置A的获取方式与点C一致；根据点A、C，确定障碍物的位置和宽度，以便于无人艇避开障碍物。

步骤5、得到步骤4输出的更加准确的激光雷达扫描点云数据后，利用这些环境感知数据进入避障决策环节。

下面是避障决策方法的实现步骤，总共分为3步，分别如下：

5-1.对无人艇船载激光雷达传感器的探测范围R_Lidar进行分区，分区数为n，则分辨率

由此，第x个分区的起始角度为λ＝α(x-1)，如图6所示。

5-2.扇区代价评估。将n个分区组成一个集合T{s₁,s₂,...,s_n}，每个元素s_x代表第x个扇区对应的值。先将集合T内所有元素初始化为0；之后，根据障碍物扫描点P_i(r_i,a_i)的极坐标数据，判断该障碍物点位于第x个分区，然后将集合T中的第x个元素s_x的值置1，如图7所示，设值判断如下式所示：

5-3.候选运动方向选择。根据当前更新好的集合T，确定无人艇下一步的运动方向，在集合T中，该元素值为1表示不可行分区，值为0则表示可行分区。元素值为0的连续分区，将构成具有一定宽度的可行运动区域，如图8所示，无人艇优先选择距离当前无人艇朝向角度最近的可行运动区域，并选择该运动区域的中间分区对应的角度方向，作为下一步的运动方向。

步骤6、根据步骤5输出的下一步航行方向，航行固定步长step距离，step距离设置为3m。如果到达目的地，则结束整个流程；如果没有到达目的地，则返回第0步骤。

为验证本发明中提出的激光雷达点云数据角度修正的效果，分别对宽度为1m的长方体障碍物和半径为0.22m的圆柱体障碍物进行激光雷达扫描，并根据无人艇在俯仰角与横滚角的取值为(5°,10°,15°,20°,25°,30°)情况下，对激光雷达障碍物扫描点云数据进行角度修正处理来验证具体效果；结果对比如图9和图10所示；图9的(a)～(c)分别表示，无人艇存在横滚角变化的情况下，激光雷达与长方体障碍物直线距离为4m、6m、8m时，激光雷达点云数据角度修正前后数据对比。图9的(d)～(f)分别表示，无人艇存在俯仰角变化的情况下，激光雷达与长方体障碍物直线距离为4m、6m、8m时，激光雷达点云数据角度修正前后数据对比；可以看出，矫正后的数据使得测量数据与障碍物真实宽度的误差相比于矫正前更小；图10的(a)、(b)分别表示，在类圆柱体障碍物的直线距离为4m的前提下，当无人艇存在横滚角、俯仰角变化时激光雷达点云数据角度修正前后的对比；可以看出，矫正后的数据使得测量数据与障碍物真实半径的误差相比于矫正前更小。上例验证了本发明提出的激光雷达点云数据角度修正的效果能使扫描结果更接近真实环境情况。

由于水流水浪等作用，无人艇航行时必不可少地存在横滚与俯仰等姿态波动，船载二维激光雷达对水面上的障碍物扫描结果与实际情况存在明显差异。为了提高无人艇在海面航行时环境感知的准确性，根据惯导等姿态设备测得的无人艇实时倾角数据，对激光雷达对类长方体和类圆柱体障碍物扫描返回的点云数据，采用与障碍物形状相应的极坐标角度修正处理方法，可以提高无人艇对水面障碍环境感知的准确性和海上航行的安全性。最后，本发明利用扫描扇区的避障权值迭代进行无人艇避障方向决策，实现了分布式的水面障碍物探测和高效避障路径规划。

Claims (4)

1.一种基于激光雷达角度校正的无人艇避障方法，其特征在于：包括以下步骤：

步骤一、安装在无人艇上的激光雷达设备持续进行数据采集；在激光雷达设备采集到非空的点云数据P后，安装在无人艇上的姿态传感设备检测无人艇当前的俯仰角θ和横滚角φ，并对点云数据P进行中值滤波；

步骤二、对点云数据P执行聚类方法，得到多个聚类P₁,P₂,...,P_k；k为聚类数量；

步骤三、对k个聚类中的障碍点分别进行线性判断；

步骤四、分别对各个聚类进行扫描矫正；若一个聚类中的障碍点呈线性，则执行类长方体障碍扫描点矫正方法；若一个聚类中的障碍点呈非线性，则执行类长方体障碍扫描点矫正方法；

类长方体障碍扫描点矫正方法具体如下：

提取被矫正聚类的各障碍点中的实测边缘扫描点L的极坐标为P_L(r_L,a_L)，以及实测正面垂直点M的极坐标P_M(r_M,a_M)；

根据点L和点M，修正得到无人艇在水平状态下的点J和点I；点J和点I的极坐标P_J、P_I的表达式如下：

其中，Φ、

为两个过渡参数，其表达式如下：

点J为无人艇平视姿态下障碍物的一侧边缘位置；点I为无人艇平视姿态下障碍物正对无人艇的位置；无人艇平视姿态下障碍物的另一侧边缘位置W的获取方式与点J一致；根据点I、J、W，确定障碍物的位置和宽度；

类圆柱体障碍扫描点矫正方法具体如下：

提取被矫正聚类的各障碍点中的实测边缘扫描点D、F的极坐标P_D(r_D,a_D)、P_F(r_F,a_F)、中间点E的极坐标P_E(r_E,a_E)；

要根据点F和点D，修正得到无人艇在水平状态下测得的点H和点C；点H和点C的极坐标P_H、P_C的表达式如下：

其中，Φ、

为两个过渡参数，其表达式如下：

Φ＝sin^-1(cos(|a_D-a_E|)·sinθ+sin(|a_D-a_E|)·sinφ)

点C为无人艇平视姿态下障碍物的一侧边缘位置；点H为无人艇平视姿态下障碍物扫描点中心的位置；无人艇平视姿态下障碍物的另一侧边缘位置A的获取方式与点C一致；根据点A、C，确定障碍物的位置和宽度；

步骤五、根据各个聚类确定的各障碍物位置和宽度，无人艇避开障碍物继续行驶。

2.根据权利要求1所述的一种基于激光雷达角度校正的无人艇避障方法，其特征在于：步骤一中，点云数据P为极坐标形式，点云数据P的第i个障碍点P_i的极坐标点为(r_i,a_i)，其中，r_i表示第i个障碍点与激光雷达之间的直线距离；a_i表示由激光雷达所在的极坐标系原点与第i个障碍物点构成的射线与激光雷达扫描极坐标系0°基准线所成的夹角，i＝1,2,...,N；N为点云数据总的障碍点总数。

3.根据权利要求1所述的一种基于激光雷达角度校正的无人艇避障方法，其特征在于：步骤三中，对同一聚类中进行线性判断的方式采用皮尔逊相关系数法，其具体过程如下：

计算皮尔逊相关系数ρ(X,Y)如下：

其中，X_i表示被判断的聚类的第i个极坐标的距离参数；Y_i表示被判断的聚类中的第i个极坐标的角度参数；μ_X表示被判断的聚类中的所有极坐标距离参数的均值；μ_Y表示被判断的聚类中的所有极坐标角度参数的均值；cov(X,Y)表示被判断的聚类中的所有极坐标距离和角度参数的协方差；σ_X，σ_Y分别表示被判断的聚类中所有极坐标的距离参数标准差和角度参数标准差；ρ(X,Y)表示聚类P_j极坐标的皮尔逊相关系数值；

根据皮尔逊相关系数ρ(X,Y)的数值，确定被判断的聚类中的各障碍点呈线性或非线性。

4.根据权利要求1所述的一种基于激光雷达角度校正的无人艇避障方法，其特征在于：步骤五的具体过程如下：

5-1.对无人艇上的激光雷达传感器的探测范围R_Lidar进行分区，分区数为n，则分辨率

由此，第x个分区的起始角度为λ＝α(x-1)；

5-2.扇区代价评估；将n个分区组成一个集合T{s₁,s₂,...,s_n}，每个元素s_x代表第x个扇区对应的值；先将集合T内所有元素初始化为0；之后，根据障碍物扫描点P_i(r_i,a_i)的极坐标数据，判断该障碍物点位于第x个分区，然后将集合T中的第x个元素s_x的值置1，设值判断如下式所示：

5-3.候选运动方向选择；根据当前更新好的集合T，确定无人艇下一步的运动方向，在集合T中，元素s_x的值为1表示不可行分区，值为0则表示可行分区；元素值为0的连续分区，将构成可行运动区域，无人艇优先选择距离当前无人艇朝向角度最接近的可行运动区域的中间方向，作为行进方向。

Images