CN110749895B - 一种基于激光雷达点云数据的定位方法 - Google Patents

Abstract

本发明公开了一种基于激光雷达点云数据的定位方法，步骤如下：采集移动设备当前所在位置实时周边环境的点云数据，把点云中的各点依次相连形成多边形。对实时点云数据进行筛选处理，形成凸多边形。对凸多边形进行等距分割并采样，得到定位分割点云数据集合。根据移动设备的位姿，基于移动设备可能所在区域的已知地图，对该区域的障碍物像素点的点云数据进行采样。将障碍物像素点的点云数据转换，并把点云中的各点依次相连形成多边形。对多边形的每条边进行等距分割并采样，得到障碍物分割点云数据集合。采用ICP算法对定位分割点云数据集合和障碍物分割点云数据集合进行配对计算，得到当前移动设备的真实位置坐标和修正后的位姿数据，完成定位。

Description

一种基于激光雷达点云数据的定位方法

技术领域

本发明属于导航技术领域，尤其涉及一种基于激光雷达点云数据的定位方法。

背景技术

在智能机器人或无人驾驶领域，定位技术是实现路径规划、控制决策和执行任务的先决条件，对导航、避障、建图起着尤为关键的作用。定位问题可以描述为移动设备（机器人或车辆）如何通过传感器感知环境或自身运动状态，经过合理的数学模型和算法处理，最终得到自身在工作环境中的精确位置。对于采用激光雷达导航系统进行定位的移动设备来说，设备在开机时，处于定位未知的一种状态，如何可以提高此时的定位精度和定位效率是业内需要解决的问题之一。

发明内容

为了克服现有技术的不足，本发明的目的在于提供一种可以提高定位精度及效率的基于激光雷达点云数据的定位方法。

本发明的目的采用以下技术方案实现：

一种基于激光雷达点云数据的定位方法，包括以下步骤：

S1、实时点云数据采集步骤，通过激光雷达采集移动设备当前所在位置实时周边环境的点云数据；

S2、实时点云数据多边形构建步骤；基于采集到的周边环境的实时点云数据，利用点云距离信息，把点云中的各点按照角度分辨率从大到小的顺序依次相连，形成多边形；

S3、实时点云数据筛选处理步骤，对实时点云数据进行筛选处理，形成凸多边形；

S4、实时点云数据分割步骤，对凸多边形的每一条边进行等距分割，得到定位分割点云数据，对定位分割点云数据采样，得到定位分割点云数据集合；

S5、障碍物像素点的点云数据采集步骤，根据移动设备的位姿，基于移动设备可能所在区域的已知地图，对该区域的障碍物像素点的点云数据进行采样；

S6、障碍物像素点的点云数据转换步骤，将障碍物像素点的点云数据转换为基于距离信息的点云数据；

S7、障碍物像素点点云数据多边形构建步骤，基于转换后得到的障碍物点云数据，利用点云距离信息，把点云中的各点按照角度分辨率从大到小的顺序依次相连，形成多边形；障碍物像素点点云数据分割步骤，对障碍物像素点点云数据多边形构建步骤得到的多边形的每条边进行等距分割，得到障碍物分割点云数据，对障碍物分割点云数据采样，得到障碍物分割点云数据集合；

S8、定位计算步骤，采用ICP算法对定位分割点云数据集合和障碍物分割点云数据集合进行配对计算，得到当前移动设备的真实位置坐标和修正后移动设备的位姿数据，完成移动设备的定位。

进一步的，实时点云数据筛选处理步骤中，对于实时点云数据多边形构建步骤中所构建的多边形，依次检查多边形相邻的两条边，如果相邻的两条边所形成的外角小于180度，则将两条边的交点从多边形中删除，再将这两条边剩余的两个点相连，形成一个新的多边形；重复以上步骤，直至得到的多边形的两两相邻的边形成的外角均不小于180°。

进一步的，实时点云数据筛选处理步骤中，在删除障碍物的点云数据之前，还包括干扰物删除步骤，该步骤如下：对实时点云数据多边形构建步骤中所构建的多边形，如果多边形相邻的两个点之间的距离大于设定阈值，则将这些相邻的点删除，对于剩下的点按照角度分辨率从大到小的顺序依次相连。

进一步的，实时点云数据分割步骤和障碍物像素点点云数据分割步骤中的分割长度相等。

相比现有技术，本发明的有益效果在于：本发明通过采集移动设备开机时实时周边环境的激光雷达点云数据，并基于点云数据以构建凸多边形的方式，对数据进行过滤筛选，以去除障碍物等错误数据，避免错误数据带来的干扰导致错误识别，来提高定位准确性，而且将错误数据删除后，减少了定位时的数据处理量，在提高定位效率上也得到了改善；同时本发明通过实时采集的周边环境的定位点云数据和基于已知地图对应区域的像素点云数据，利用定位分割点云数据集合和障碍物分割点云数据集合通过ICP算法进行配对计算，进行移动设备的重新定位，解决了如何实现移动设备在开机后准确定位的问题。

附图说明

图1为本发明方法的流程图；

图2为本发明方法构建多边形步骤的示意图；

图3a为移动设备采用激光雷达导航时构建的栅格地图；

图3b为对栅格地图中已知区域的障碍物像素点的点云数据进行采样后的地图。

具体实施方式

下面，结合具体实施方式，对本发明做进一步描述。如图1所示，本发明的定位方法步骤如下：

S1、激光雷达实时点云数据采集步骤；移动设备开机后，通过激光雷达导航系统采集移动设备当前所在位置实时周边环境的（一帧）点云数据，本步骤中采集到的实时点云数据是移动设备周边环境基于雷达距离信息的点云数据。

S2、基于实时点云数据构建多边形；基于采集到的周边环境的实时点云数据，利用点云距离信息，把点云中的各点按照角度分辨率从大到小的顺序依次相连，形成多边形的点云数据；如图2所示，点云Q包括点A、B、C、D、E、F、G、H、I、J，根据各个点的角度分辨率，依次将各个点相连，形成一个多边形A→B→C→D→E→F→G→H→I→J→A。

S3、实时点云数据筛选处理，对多边形的实时点云数据进行筛选处理的目的是将障碍物或门、通道等结构删除掉，这些结构不用于作为定位点云数据集合，以提高定位精度和效率；

对上一步骤所构建的多边形的点云数据进行筛选删除，方法是：依次检查多边形相邻的两条边，如果相邻的两条边所形成的外角小于180°，则将两条边的交点从多边形中删除，如图2中的多边形，具有多条相邻的边，如AB和BC、BC和CD、CD和DE等，依次检查这些相邻边所形成的外角，发现边CD和边DE所形成外角小于180°，即D点是呈现凹进去的状态，将这种凹进去的点认为是障碍物，从所构建的多边形中删除，将两条相邻边的交点删除后，再将这两条边剩余的两个点相连，形成一个新的多边形，即将D点删除后，将C点和E点相连，得到多边形A→B→C→E→F→G→H→I→J→A，重复相同的步骤，继续寻找多边形中凹进去的点（即外角小于180的相邻边的交点），将凹进去的点删除，重新构建多边形，得到凸多边形的点云数据；对于多边形A→B→C→E→F→G→H→I→J→A，边CE和边EF的外角小于180°，则将E点删除，连接C点和F点，得到多边形A→B→C→F→G→H→I→J→A，以此类推，直至最后得到的多边形的两两相邻的边形成的外角均不小于180°，如图2中的多边形A→B→C→G→H→I→J→A，此时的多边形为凸多边形。

优选的，在筛选被认为是障碍物的点（凹进去的点）之前，增加一个干扰物删除步骤，将门、玻璃、通道等认为是干扰物，将多边形中被认为是干扰物的点删除，方法为：如果多边形相邻的两个点之间的距离大于设定阈值，则认为这些相邻的点为干扰物，将所有相邻点间距离大于设定阈值的点对都删除，删除后，对于剩下的点同样按照角度分辨率从大到小的顺序依次相连形成多边形；设定阈值为经验值，可根据实际情况相应设置，例如可设为10cm等。

S4、对上一步骤得到的凸多边形点云数据的每一条边进行等距分割，得到定位分割点云数据；分割时，对于凸多边形的每条边都按照固定的长度进行分割，例如按照分割长度L=5cm～10cm来分割多边形的每一条边，得到定位分割点云数据，对定位分割点云数据采样，得到定位分割点云数据集合Qi。

S5、根据已知的移动设备的位姿（x,y,yaw），并基于移动设备可能所在区域的已知地图（图3a），该地图可以是移动设备的导航系统已经预先建立好的地图，对该区域的障碍物像素点的点云数据进行采样，得到基于像素点点数的障碍物点云数据（图3b）；图3a是ROS机器人预先利用gmapping建图得到的栅格地图，基于该已知的栅格地图，对机器人所在区域地图中的障碍物像素点的点云数据进行采样，如图3b所示；本步骤中采集到的点云数据是基于像素点点数的点云数据。

S6、将障碍物像素点的点云数据转换为基于雷达距离信息的点云数据；基于像素点点数的点云数据和第一个步骤中激光雷达采集的基于距离的点云数据的距离单位不一样，因此要进行两种点云数据（单位）的转换；

对于采用激光雷达和惯性导航原理进行定位的方法，可以利用此方法逆推，将障碍物像素点的点云数据转换为基于雷达距离信息的点云数据，将像素点点云数据转换为基于距离信息的点云数据的方法为现有技术的常规方法，不是本发明的创新之处，此处不再赘叙。

S7、对转换后得到的障碍物点云数据按照步骤S2的方法构建多边形，对得到的多边形的每条边按照步骤S4的方法进行等距分割，分割的长度L’=L，得到障碍物分割点云数据，对障碍物分割点云数据采样，得到障碍物分割点云数据集合Pi。

S8、采用ICP算法对定位分割点云数据集合Qi和障碍物分割点云数据集合Pi进行配对计算，得到当前移动设备的真实位置坐标和修正后移动设备的位姿数据（x’,y’,yaw’），完成移动设备的定位。

在前面步骤中得到的定位分割点云数据包含了移动设备真实周边环境障碍物的距离信息，但是该数据中没有定位信息，而障碍物分割点云数据集合中包含了基于已知地图采集到的真实周边环境障碍物的信息，该数据中具有定位信息，但已知地图中包含的机器人位置信息可能存在偏差，导致与实际机器人的初始定位对应不上，因此通过ICP算法配准将两组数据集合进行配准，通过算法计算得到的转化关系（旋转矩阵）来修正机器人的初始定位，即可在已知地图中找到机器人相对应的准确位姿，完成定位。

ICP算法的计算过程如下：

a.假设给移动设备的初始位置为M（x,y），将定位分割点云数据集合Qi和障碍物分割点云数据集合Pi分别向M点进行投影，得到定位分割点云数据投影集合Qj和障碍物分割点云数据集合Pj，寻找两个投影集合Qj、Pj中距离最近的点对（即两个点坐标位置相同，基于中心点距离相同）；

b.利用找到的点对根据误差函数E(R,t)计算最优匹配参数，误差函数

，式中的n为距离最近的点对的数量，R为旋转矩阵，t为平移向量，通过使得误差函数最小，计算出最优匹配参数R、t；

c.将障碍物分割点云数据集合Pi利用旋转矩阵R和平移向量t进行旋转和平移变换，得到变换点集Pi’；

d.计算变换点集Pi’和定位分割点云数据集合Qi的平均距离

，如果平均距离d小于给定阈值或者大于预设最大迭代次数，则停止计算，否则返回步骤a，重新假设一个初始位置，重复以上步骤，直至计算得到的平均距离d小于给定阈值或者迭代计算的次数大于预设最大迭代次数；

停止迭代计算后，根据旋转矩阵R和平移向量t，以及已知的移动设备的位姿（x,y,yaw），获取当前移动设备的真实位置坐标和修正后移动设备的位姿数据（x’,y’,yaw’），完成移动设备的定位。原已知的移动设备的位姿数据（x,y,yaw）可能存在误差，通过ICP算法进行配对计算可以对移动设备的位姿数据进行修正，得到准确的位姿数据。

对于本领域的技术人员来说，可根据以上描述的技术方案以及构思，做出其它各种相应的改变以及变形，而所有的这些改变以及变形都应该属于本发明权利要求的保护范围之内。

Claims (3)

1.一种基于激光雷达点云数据的定位方法，其特征在于，包括以下步骤：

S1、实时点云数据采集步骤，通过激光雷达采集移动设备当前所在位置实时周边环境的点云数据；

S2、实时点云数据多边形构建步骤；基于采集到的周边环境的实时点云数据，利用点云距离信息，把点云中的各点按照角度分辨率从大到小的顺序依次相连，形成多边形；

S3、实时点云数据筛选处理步骤，对实时点云数据进行筛选处理，将认为是障碍物的点云数据删除，形成凸多边形；删除障碍物的点云数据的方法如下：对于实时点云数据多边形构建步骤中所构建的多边形，依次检查多边形相邻的两条边，如果相邻的两条边所形成的外角小于180度，则将两条边的交点从多边形中删除，再将这两条边剩余的两个点相连，形成一个新的多边形；重复以上步骤，直至得到的多边形的两两相邻的边形成的外角均不小于180°；

S4、实时点云数据分割步骤，对凸多边形的每一条边进行等距分割，得到定位分割点云数据，对定位分割点云数据采样，得到定位分割点云数据集合；

S5、障碍物像素点的点云数据采集步骤，根据移动设备的位姿，基于移动设备可能所在区域的已知地图，对该区域的障碍物像素点的点云数据进行采样；

S6、障碍物像素点的点云数据转换步骤，将障碍物像素点的点云数据转换为基于距离信息的点云数据；

S7、障碍物像素点点云数据多边形构建步骤，基于转换后得到的障碍物点云数据，利用点云距离信息，把点云中的各点按照角度分辨率从大到小的顺序依次相连，形成多边形；障碍物像素点点云数据分割步骤，对障碍物像素点点云数据多边形构建步骤得到的多边形的每条边进行等距分割，得到障碍物分割点云数据，对障碍物分割点云数据采样，得到障碍物分割点云数据集合；

S8、定位计算步骤，采用ICP算法对定位分割点云数据集合和障碍物分割点云数据集合进行配对计算，得到当前移动设备的真实位置坐标和修正后移动设备的位姿数据，完成移动设备的定位。

2.根据权利要求1所述的基于激光雷达点云数据的定位方法，其特征在于：实时点云数据筛选处理步骤中，在删除障碍物的点云数据之前，还包括干扰物删除步骤，该步骤如下：对实时点云数据多边形构建步骤中所构建的多边形，如果多边形相邻的两个点之间的距离大于设定阈值，则将这些相邻的点删除，对于剩下的点按照角度分辨率从大到小的顺序依次相连。

3.根据权利要求1或2所述的基于激光雷达点云数据的定位方法，其特征在于：实时点云数据分割步骤和障碍物像素点点云数据分割步骤中的分割长度相等。

Images