一种基于ICP和KD树的激光雷达定位方法技术

本发明专利技术公开了一种基于ICP和KD树的激光雷达定位方法，涉及导航定位技术领域。利用搭载有激光雷达移动终端对目标区域内的各个固定目标对象进行扫描，获得包含各固定目标对象分别所对应的一帧点云集合，所述点云集合中包括反光板连续点云和墙壁连续点云，分别针对目标区域内各个固定目标对象，基于单个点云帧中各个单一点云的反射强度，提取固定目标对象的坐标并利用ICP算法确定搭载有激光雷达的移动终端位姿。通过本发明专利技术的技术方案，为ICP算法与KD树搜索提供初值，提高了实时性与鲁棒性，并去除累积误差。除累积误差。除累积误差。

A lidar location method based on ICP and KD tree

【技术实现步骤摘要】
一种基于ICP和KD树的激光雷达定位方法


[0001]本专利技术涉及导航定位
，具体而言涉及一种基于ICP和KD树的激光雷达定位方法。

技术介绍

[0002]基于激光雷达的定位方法是目前AGV定位系统应用的主流技术之一，根据VDI2510标准指导手册，AGV是一种“由非接触式引导系统自动控制和引导的在地面运行的自主行驶的运输工具”。
[0003]目前来说，激光雷达定位系统主要分为基于人工信标的定位系统和基于自然特征的的定位系统两种，基于人工信标的激光雷达定位系统主要依靠的是具有高反射强度的反光板与三点定位法来实现的，通过将当前位置激光雷达扫描到的反光板与事先存储的全局地图中的反光板进行匹配，由于环境内所有反光板坐标已知，就可以根据匹配结果获得当前位置激光雷达扫描到的反光板的全局坐标，当获得的全局坐标至少有三个时，就可以再根据激光雷达与每个反光板的距离通过三点定位法求得激光雷达当前的全局坐标；而基于自然特征的定位方法主要是结合SLAM技术完成，包括二维SLAM的GMapping、Hector和Cartographer，以及三维SLAM的LOAM算法，考虑到2DLidar与3DLidar的生产成本，基于二维SLAM的AGV定位技术应用更加广泛。
[0004]基于SLAM算法的AGV定位技术适用于几何特征丰富的场景，尤其是角点特征，而类似于长廊、仓库等几何特征稀少的场景SLAM算法并不适用；而基于人工信标的AGV定位方法精度更高，但是需要对AGV的使用场景进行一定的改造，并且对人工信标在场景中的布置有一定限制，在使用过程中要求激光雷达能够检测到至少三块发光板。

技术实现思路

[0005]本专利技术的目的在于提供一种基于ICP和KD树的激光雷达定位方法，以解决现有技术中的问题。
[0006]为实现上述目的，本专利技术提供如下技术方案：
[0007]一种基于ICP和KD树的激光雷达定位方法，利用搭载有激光雷达的移动终端对目标区域内的各个固定目标对象进行连续扫描，获得各固定目标对象所对应的各个激光束，进一步得到各激光束分别所对应的点云集合，所述点云集合中包括反光板点云集合和墙壁点云集合，将反光板作为固定目标对象，基于点云集合，通过以下步骤A至步骤E确定搭载有激光雷达的移动终端的最终位姿：
[0008]步骤A、分别针对各个固定目标对象，进一步针对固定目标对象所对应的各个激光束，获得各激光束对应点云集合中反光板点云集合中的各个单一点云，进一步基于各个单一点云分别所对应的反射强度、以及入射角，确定反射强度阈值，利用反射强度阈值对各个单一点云进行筛选，获得固定目标对象所包含的各个反光板表面的各个单一点云，分别针对各个反光板表面，利用最小二乘法对该反光板表面的单一点云进行拟合，获得拟合面、以
及拟合面的中心位置和拟合内径，即获得该固定目标对象对应的各个反光板表面所对应的拟合面；
[0009]步骤B、分别针对各个固定目标对象，基于固定目标对象对应点云集合中墙壁点云集合中的各个单一点云，获得单一点云分别接收不同激光束分别在其位置处的深度变化率，确定各个反光板表面分别对应预设距离范围内的各个墙壁边缘点；同时基于墙壁点云集合计算各个单一点云的曲率，确定各个墙壁角点，基于墙壁边缘点和墙壁角点分别对应的单一点云，利用最小二乘法对墙壁点云集合进行拟合，获得拟合的墙壁位置，即获得各个反光板表面所对应的拟合墙壁位置，随后进入步骤C；
[0010]步骤C、基于反光板集合，以反光板中心位置作为三角形顶点构建反光板三角形集合，针对连续两帧点云帧集中的反光板三角形集合，进行反光板三角形的边长比对，进一步获得反光板集合的点云匹配对集合，随后进入步骤D；
[0011]步骤D、针对反光板的点云匹配对集合中的各个点云匹配对，将点云匹配对按照其对应的反光板集合进行分类，获得匹配分类集合，利用奇异值分解计算匹配分类集合的位姿矩阵，利用ICP算法对位姿矩阵进行迭代更新，当位姿矩阵的变化量小于预设变化阈值时，获得搭载有激光雷达的移动终端的位姿变化量，确定移动终端的粗略位姿，随后进入步骤E；
[0012]步骤E、基于移动终端的粗略位姿，结合反光板中心位置，构建反光板中心位置对应的KD树，利用BBF算法搜索与粗略位姿距离最近的多个反光板中心位置并构建KD树三角形集合，在反光板集合中选取三个最近的反光板中心位置与KD树三角形集合比对，进一步获得KD树点云匹配集合，针对KD树点云匹配集合，利用奇异值分解确定移动终端的最终位姿。
[0013]进一步地，前述的步骤B中，具体包括以下步骤：
[0014]步骤B1、分别针对目标区域内的各个固定目标对象，根据以下公式：
[0015][0016]获得点云集合中不同激光束在相同单一点云处的深度变化率ξ，其中，CP
E
、CP
W
分别为经过同一单一点云的两束激光束的长度，当深度变化率ξ大于预设变化率阈值时，该点云所在位置为墙壁边缘点；
[0017]步骤B2、分别针对各个固定目标对象其所对应的墙壁点云集合，根据公式：
[0018][0019]获得单一点云的曲率c，其中，T为墙壁点云帧集中的单一点云个数，V
(k,i)
为经过反光板集合中第k个反光板表面所对应的第i个单一点云处的激光束长度，V
(k,j)
为经过反光板集合中第k个反光板表面所对应的第j个单一点云所代表的激光束长度，当该单一点云的曲率大于预设曲率阈值并且为墙壁点云集合中最大曲率时，该单一点云所对应的位置为墙壁角点；
[0020]步骤B3、利用最小二乘法对墙壁所对应的墙壁点云集合进行拟合，获得拟合的墙壁位置，基于墙壁位置和步骤A中获得的拟合内径对拟合面进行平移修正，获得修正后的拟
合面的中心位置。
[0021]进一步地，前述的步骤C中所述的三角形构建过程中，反光板集合中包含的反光板中心位置数量大于三时，构建以反光板中心位置为顶点的反光板三角形，当反光板集合中包含的反光板中心位置数量小于三时，将墙壁边缘点和墙壁角点添加至反光板集合中，以反光板中心位置、墙壁边缘点或墙壁角点为顶点构建反光板三角形。
[0022]进一步地，当各个匹配三角形边长一一比对成功，获得反光板表面的点云匹配对，判断该点云匹配对是否存在于点云匹配对集合中，当点云匹配对不存在于点云匹配集合中，将该点云匹配对保存至点云匹配集合中，并将该点云匹配对的得分置为1，当点云匹配对存在于点云匹配集合中，将点云匹配对的得分加1并更新得分。
[0023]进一步地，构建反光板三角形以任意反光板中心位置为基础按照顺时针的顺序进行构建。
[0024]进一步地，前述的步骤D中，将匹配分类集合的位姿矩阵作为ICP算法的初值对各个单一点云进行迭代转换，获得各个单一点云之间的距离，把距离最小的两个单一点云作为最近点对，进一步获得新的位姿矩阵并更新原位姿矩阵，直至更新次数达到预设阈值或位姿矩阵的变化量小于预设变化阈值时，获得目标对象的位姿变化，并对更新过程本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于ICP和KD树的激光雷达定位方法，其特征在于，利用搭载有激光雷达的移动终端对目标区域内的各个固定目标对象进行连续扫描，获得各固定目标对象所对应的各个激光束，进一步得到各激光束分别所对应的点云集合，所述点云集合中包括反光板点云集合和墙壁点云集合，基于点云集合，通过以下步骤A至步骤E确定搭载有激光雷达的移动终端的最终位姿：步骤A、分别针对各个固定目标对象，进一步针对固定目标对象所对应的各个激光束，获得各激光束对应点云集合中反光板点云集合中的各个单一点云，进一步基于各个单一点云分别所对应的反射强度、以及入射角，确定反射强度阈值，利用反射强度阈值对各个单一点云进行筛选，获得固定目标对象所包含的各个反光板表面的各个单一点云，分别针对各个反光板表面，利用最小二乘法对该反光板表面的单一点云进行拟合，获得拟合面、以及拟合面的中心位置和拟合内径，即获得该固定目标对象对应的各个反光板表面所对应的拟合面；步骤B、分别针对各个固定目标对象，基于固定目标对象对应点云集合中墙壁点云集合中的各个单一点云，获得单一点云分别接收不同激光束分别在其位置处的深度变化率，确定各个反光板表面分别对应预设距离范围内的各个墙壁边缘点；同时基于墙壁点云集合计算各个单一点云的曲率，确定各个墙壁角点，基于墙壁边缘点和墙壁角点分别对应的单一点云，利用最小二乘法对墙壁点云集合进行拟合，获得拟合的墙壁位置，即获得各个反光板表面所对应的拟合墙壁位置，随后进入步骤C；步骤C、基于反光板集合，以反光板中心位置作为三角形顶点构建反光板三角形集合，针对连续两帧点云帧集中的反光板三角形集合，进行反光板三角形的边长比对，进一步获得反光板集合的点云匹配对集合，随后进入步骤D；步骤D、针对反光板的点云匹配对集合中的各个点云匹配对，将点云匹配对按照其对应的反光板集合进行分类，获得匹配分类集合，利用奇异值分解计算匹配分类集合的位姿矩阵，利用ICP算法对位姿矩阵进行迭代更新，当位姿矩阵的变化量小于预设变化阈值时，获得搭载有激光雷达的移动终端的位姿变化量，确定移动终端的粗略位姿，随后进入步骤E；步骤E、基于移动终端的粗略位姿，结合反光板中心位置，构建反光板中心位置对应的KD树，利用BBF算法搜索与粗略位姿距离最近的多个反光板中心位置并构建KD树三角形集合，在反光板集合中选取三个最近的反光板中心位置与KD树三角形集合比对，进一步获得KD树点云匹配集合，针对KD树点云匹配集合，利用奇异值分解确定移动终端的最终位姿。2.根据权利要求1所述的一种基于ICP和KD树的激光雷达定位方法，其特征在于，所述步骤B具体包括以下步骤：步骤B1、分别针对目标区域内的各个固定目标对象，根据以下公式：获得点云集合中不同激光束在相同单一点云处的深度变化率ξ，其中，CP
E
、CP
W
分别为经过同一单一点云的两束激光束的长度，当深度变化率ξ大于预设变化率阈值时，该点云所在位置为墙壁边缘点；步骤B2、分别针对各个固定目标对象其所对应的墙壁点云集合，根据公式：
获得单一点云的曲率c，其中，T为墙壁点云帧集中的单一点云个数，V
(k,i)
为经过反光板集合中第k个反光板表面所对应的第i个单一点云处的激光束长度，V
(k,j)
为经过反光板集合中第k个反光板表面所对应的第j个单一点云所代表的激光束长度，当该单一点云的曲率大于预设曲率阈值并且为墙壁点云集合中最大曲率时，该单一点云所对应的位置为墙壁角点；步骤B3、利用最小二乘法对墙壁所对应的墙壁点云集合进行拟合，获得拟合的墙壁位置，基于墙壁位置和步骤A中获得的拟合内径对拟合面进行平移修正，获得修正后的拟合面的中心位置。3.根据权利要求1所述的一种基于ICP和KD树的激光雷达定位方法，其特征在于，所述步骤C中所述的三角形...

【专利技术属性】
技术研发人员：曾庆喜，阚宇超，陶晓东，
申请(专利权)人：南京航空航天大学，
类型：发明
国别省市：