[发明专利]一种面向港口自动驾驶的同时制图与定位

权利要求书

1.一种面向港口自动驾驶的同时制图与定位，包括三维激光SLAM、LeGO-LOAM建图与NDT-MATCH定位，其特征在于：所述三维激光SLAM包括多线激光雷达数据，所述LeGO-LOAM建图包括点云分割、点云特征提取、激光里程计与激光建图，所述NDT-MATCH定位包括点云地图、点云配准。

2.根据权利要求1所述的一种面向港口自动驾驶的同时制图与定位，其特征在于：所述点云分割是将点云分为地面和非地面数据，地面检测模型需要适应平面、坡道以及两者的过度过程，地面检测过的过程如下：segment(pi)＝首先在xy平面通过角度分辨率Δα和公式(1)、(2)将点云分为若干块；

xy平面根据公式(3)，将P_s分为若干块

每个块选取z轴最小值作为地面上的点，同时作为该块的代表点

对每个分块从近到远，从左到右，获得直线方程y＝mx+b；

地面上的点需要满足如下条件：

(1)直线的坡度m不能超过合理的阈值T_m；

(2)如果坡度m<T_smali,直线的截距b的绝对值不能超过一个合理的阈值T_b，保证它包含在地面上；

(3)P的直线拟合均方差不能超过一个合理的阈值T_RMSE；

(4)第一个点到前一条直线的距离不能超过一个合理的阈值T_dpre；

然后对非地面数据进行聚类，采用基于深度图像的聚类方法；同一类别的点云数据会标记唯一的标签；通过滤除点云数量比较小的类别，提高处理速度，加快点云的特征提取；

所述点云特征提取为从聚类后的点云和地面点云中提取特征点云，首先设定S为连续的点云数据，利用点云的距离信息r，根据公式(4)计算点Pi在点击S的粗糙程度c；设S的点云数目|S|为10；

通过c与设定阈值c_th的大小判定点云数据的特征类型；c大于c_th则为边缘点，c小于c_th则为平面点；S中边缘点c最大的点作为边缘特征点，同理平面点中c最小的作为平面特征点。

3.根据权利要求1所述的一种面向港口自动驾驶的同时制图与定位，其特征在于：所述激光里程计是通过相邻帧点云数据估计激光雷达的姿态变化；相邻帧的变换关系通过点边和点面匹配得到；第一步进行标签匹配，平面特征点只采用了地面上的平面特征点，用来做匹配，这样可以提高匹配的精度和速度，第二步两阶段的L-M优化，最优转换矩阵同过两步获得，首先t_s，θ_roli，θ_pitch通过平面特征匹配获得，剩下的通过边缘特征匹配获得；值得注意的是，第一步的t_s，θ_roli，θ_pitch可以作为第二步的输入优化匹配；

所述激光建图是对当前激光特征再次与前一时刻的地图进行匹配，通过L-M算法优化转换关系；更新关键帧及关键帧姿态；当前地图即为到最新时刻为止全部关键帧的点云数据，通过关键帧姿态进行融合，一般情况下会降采样为稀疏点云地图。

4.根据权利要求1所述的一种面向港口自动驾驶的同时制图与定位，其特征在于：所述点云地图采用NDT可以用来紧凑的描述一个平面，首次提出时用来配准二维激光点云，之后被广泛应用在点云的配准和建图中；通过概率密度函数(PDFs)描述局部栅格平面的形状特性，首先将点云数据栅格化，然后计算每个栅格的概率密度函数，如公式(4)；

其中为栅格点云，D为点云的维度，和∑为均值和方差向量；

在二维和三维点云中，协方差矩阵的特征向量和特征值可以用来评估表面的取向和光滑度；

所述点云配准使用NDT进行，目标是找到和参考点云匹配度最高的点云的姿态；当前点云集合空间变换函数通过姿态移动点计算概率密度函数PDF最优的姿态应该获得最大的相似概率；

或者最小的-log(Ψ)，