CN114779264A

CN114779264A - 一种基于icp和kd树的激光雷达定位方法

Info

Publication number: CN114779264A
Application number: CN202210254094.XA
Authority: CN
Inventors: 曾庆喜; 阚宇超; 陶晓东
Original assignee: Nanjing University of Aeronautics and Astronautics
Current assignee: Nanjing University of Aeronautics and Astronautics
Priority date: 2022-03-15
Filing date: 2022-03-15
Publication date: 2022-07-22

Abstract

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

Description

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

技术领域

本发明涉及导航定位技术领域，具体而言涉及一种基于ICP和KD树的激光雷达定位方法。

背景技术

基于激光雷达的定位方法是目前AGV定位系统应用的主流技术之一，根据VDI2510标准指导手册，AGV是一种“由非接触式引导系统自动控制和引导的在地面运行的自主行驶的运输工具”。

目前来说，激光雷达定位系统主要分为基于人工信标的定位系统和基于自然特征的的定位系统两种，基于人工信标的激光雷达定位系统主要依靠的是具有高反射强度的反光板与三点定位法来实现的，通过将当前位置激光雷达扫描到的反光板与事先存储的全局地图中的反光板进行匹配，由于环境内所有反光板坐标已知，就可以根据匹配结果获得当前位置激光雷达扫描到的反光板的全局坐标，当获得的全局坐标至少有三个时，就可以再根据激光雷达与每个反光板的距离通过三点定位法求得激光雷达当前的全局坐标；而基于自然特征的定位方法主要是结合SLAM技术完成，包括二维SLAM的GMapping、Hector和Cartographer，以及三维SLAM的LOAM算法，考虑到2DLidar与3DLidar的生产成本，基于二维SLAM的AGV定位技术应用更加广泛。

基于SLAM算法的AGV定位技术适用于几何特征丰富的场景，尤其是角点特征，而类似于长廊、仓库等几何特征稀少的场景SLAM算法并不适用；而基于人工信标的AGV定位方法精度更高，但是需要对AGV的使用场景进行一定的改造，并且对人工信标在场景中的布置有一定限制，在使用过程中要求激光雷达能够检测到至少三块发光板。

发明内容

本发明的目的在于提供一种基于ICP和KD树的激光雷达定位方法，以解决现有技术中的问题。

为实现上述目的，本发明提供如下技术方案：

一种基于ICP和KD树的激光雷达定位方法，利用搭载有激光雷达的移动终端对目标区域内的各个固定目标对象进行连续扫描，获得各固定目标对象所对应的各个激光束，进一步得到各激光束分别所对应的点云集合，所述点云集合中包括反光板点云集合和墙壁点云集合，将反光板作为固定目标对象，基于点云集合，通过以下步骤A至步骤E确定搭载有激光雷达的移动终端的最终位姿：

步骤A、分别针对各个固定目标对象，进一步针对固定目标对象所对应的各个激光束，获得各激光束对应点云集合中反光板点云集合中的各个单一点云，进一步基于各个单一点云分别所对应的反射强度、以及入射角，确定反射强度阈值，利用反射强度阈值对各个单一点云进行筛选，获得固定目标对象所包含的各个反光板表面的各个单一点云，分别针对各个反光板表面，利用最小二乘法对该反光板表面的单一点云进行拟合，获得拟合面、以及拟合面的中心位置和拟合内径，即获得该固定目标对象对应的各个反光板表面所对应的拟合面；

步骤B、分别针对各个固定目标对象，基于固定目标对象对应点云集合中墙壁点云集合中的各个单一点云，获得单一点云分别接收不同激光束分别在其位置处的深度变化率，确定各个反光板表面分别对应预设距离范围内的各个墙壁边缘点；同时基于墙壁点云集合计算各个单一点云的曲率，确定各个墙壁角点，基于墙壁边缘点和墙壁角点分别对应的单一点云，利用最小二乘法对墙壁点云集合进行拟合，获得拟合的墙壁位置，即获得各个反光板表面所对应的拟合墙壁位置，随后进入步骤C；

步骤C、基于反光板集合，以反光板中心位置作为三角形顶点构建反光板三角形集合，针对连续两帧点云帧集中的反光板三角形集合，进行反光板三角形的边长比对，进一步获得反光板集合的点云匹配对集合，随后进入步骤D；

步骤D、针对反光板的点云匹配对集合中的各个点云匹配对，将点云匹配对按照其对应的反光板集合进行分类，获得匹配分类集合，利用奇异值分解计算匹配分类集合的位姿矩阵，利用ICP算法对位姿矩阵进行迭代更新，当位姿矩阵的变化量小于预设变化阈值时，获得搭载有激光雷达的移动终端的位姿变化量，确定移动终端的粗略位姿，随后进入步骤E；

步骤E、基于移动终端的粗略位姿，结合反光板中心位置，构建反光板中心位置对应的KD树，利用BBF算法搜索与粗略位姿距离最近的多个反光板中心位置并构建KD树三角形集合，在反光板集合中选取三个最近的反光板中心位置与KD树三角形集合比对，进一步获得KD树点云匹配集合，针对KD树点云匹配集合，利用奇异值分解确定移动终端的最终位姿。

进一步地，前述的步骤B中，具体包括以下步骤：

步骤B1、分别针对目标区域内的各个固定目标对象，根据以下公式：

获得点云集合中不同激光束在相同单一点云处的深度变化率ξ，其中，CP_E、CP_W分别为经过同一单一点云的两束激光束的长度，当深度变化率ξ大于预设变化率阈值时，该点云所在位置为墙壁边缘点；

步骤B2、分别针对各个固定目标对象其所对应的墙壁点云集合，根据公式：

获得单一点云的曲率c，其中，T为墙壁点云帧集中的单一点云个数，V_(k,i)为经过反光板集合中第k个反光板表面所对应的第i个单一点云处的激光束长度，V_(k,j)为经过反光板集合中第k个反光板表面所对应的第j个单一点云所代表的激光束长度，当该单一点云的曲率大于预设曲率阈值并且为墙壁点云集合中最大曲率时，该单一点云所对应的位置为墙壁角点；

步骤B3、利用最小二乘法对墙壁所对应的墙壁点云集合进行拟合，获得拟合的墙壁位置，基于墙壁位置和步骤A中获得的拟合内径对拟合面进行平移修正，获得修正后的拟合面的中心位置。

进一步地，前述的步骤C中所述的三角形构建过程中，反光板集合中包含的反光板中心位置数量大于三时，构建以反光板中心位置为顶点的反光板三角形，当反光板集合中包含的反光板中心位置数量小于三时，将墙壁边缘点和墙壁角点添加至反光板集合中，以反光板中心位置、墙壁边缘点或墙壁角点为顶点构建反光板三角形。

进一步地，当各个匹配三角形边长一一比对成功，获得反光板表面的点云匹配对，判断该点云匹配对是否存在于点云匹配对集合中，当点云匹配对不存在于点云匹配集合中，将该点云匹配对保存至点云匹配集合中，并将该点云匹配对的得分置为1，当点云匹配对存在于点云匹配集合中，将点云匹配对的得分加1并更新得分。

进一步地，构建反光板三角形以任意反光板中心位置为基础按照顺时针的顺序进行构建。

进一步地，前述的步骤D中，将匹配分类集合的位姿矩阵作为ICP算法的初值对各个单一点云进行迭代转换，获得各个单一点云之间的距离，把距离最小的两个单一点云作为最近点对，进一步获得新的位姿矩阵并更新原位姿矩阵，直至更新次数达到预设阈值或位姿矩阵的变化量小于预设变化阈值时，获得目标对象的位姿变化，并对更新过程中得到的各个位姿矩阵变化累加，获得累加位姿矩阵，由此确定移动终端的粗略位姿。

进一步地，前述的步骤E具体包括以下步骤：

步骤E1、分别针对测量所得的所有反光板中心位置，基于反光板表面的点云匹配对，利用二维KD树构建出包含所有反光板中心位置的KD树；

步骤E2、针对累加位姿矩阵得到的移动终端的粗略位姿，在KD树中搜索距离该位姿最近的三个反光板中心位置，作为第一分组，同时获得该KD树的各个叶节点；

步骤E3、分别针对各个叶节点，以叶节点为起始进行回溯搜索，以激光雷达位姿为圆心，该位姿到最近节点的距离为半径画圆，如果这个圆与KD树节点划分的平面相交，则转移至该平面代表的分支内进行最近点搜索，重复该过程直到圆与KD树节点划分的平面相交的区域内没有其他节点，此时该KD树节点代表的反光板中心位置为最近反光板中心位置，将最近反光板中心位置所对应的节点从KD树中删除；

步骤E4、重复步骤E2与E3的过程，获得三个最近反光板中心位置，作为第二分组，基于第一分组中的三个最近反光板中心位置坐标，通过三角形边长匹配获得由第一分组与第二分组中的三个最近反光板中心位置构成的匹配三角形，利用步骤D中所描述过程，针对第一分组和第二分组分别所对应的匹配正三角形利用奇异值分解移动终端在KD树坐标系中的位姿变换，获得移动终端的最终位姿。

进一步地，步骤E1中构建包含所有反光板中心位置的KD树，以各个反光板中心位置的横坐标作为KD树的奇数层，以各个反光板中心位置的纵坐标作为KD树的偶数层，对KD树划分维度，将小于划分维度中位数的反光板中心位置放置在KD树左分支，剩余的反光板中心位置放置在右分支，直到节点只有一个元素；

步骤E2中根据当前KD树层的划分维度来比较该位姿与KD树节点的大小，即奇数层比较横坐标，偶数层比较纵坐标，如果位姿小于节点，则进入节点左侧分支继续进行搜索，反之进入右侧，直至所比较的节点为叶节点，并且不再有分支，此时该节点为最近节点；

步骤E3在删除节点时，如果被删除节点的两个子树都为空，则用空树来代替，否则，根据当前层的划分维度选择被删除节点的左子树的最右节点或者右子树的最左节点作为替代的节点。

本发明所述一种基于ICP和KD树的激光雷达定位方法，采用以上技术方案与现有技术相比，具有以下技术效果：

本发明结合了自然特征与人工地标进行激光雷达的定位，不仅使得定位系统在走廊仓库等缺少特征点的环境下能保证良好的定位效果，而且在无法检测到反光板的路段仍然能保持一定的定位精度，大大提高了定位系统的鲁棒性。并且为ICP算法提供了一个较为精确的初值，减少了之后的迭代次数，有效提升定位系统的实时性。最后通过KD树的搜索去除了ICP算法的累积误差。

附图说明

图1为本发明示例性实施例激光雷达定位方法的流程示意图；

图2(a)为本发明示例性实施例的反光板表面点云坐标示意图；

图2(b)为本发明实施例反光板表面拟合的示意图；

图3为本发明示例性实施例的反光板构造示意图；

图4为本发明示例性实施例中反光板表面点云中所含拖尾点的示意图；

图5为本发明示例性实施例的确定墙壁边界示意图；

图6为本发明示例性实施例的拟合墙壁曲线示意图；

图7为本发明示例性实施例的坐标修正过程示意图；

图8为本发明示例性实施例的反光板表面修正拟合示意图；

图9为本发明示例性实施例的反光板匹配示意图；

图10为本发明示例性实施例的三角形构建的示意图；

图11为本发明示例性实施例的KD树构建的示意图。

具体实施方式

为了更了解本发明的技术内容，特举具体实施例并配合所附图式说明如下。

在本公开中参照附图来描述本发明的各方面，附图中示出了许多说明的实施例。本公开的实施例不必定义在包括本发明的所有方面。应当理解，上面介绍的多种构思和实施例，以及下面更加详细地描述的那些构思和实施方式可以以很多方式中任意一种来实施，这是因为本发明所公开的构思和实施例并不限于任何实施方式。另外，本发明公开的一些方面可以单独使用，或者与本发明公开的其他方面的任何适当组合来使用。

结合图1本发明示例性实施例基于ICP和KD树的激光雷达定位方法的流程示意，

用搭载有激光雷达的移动终端对目标区域内的各个固定目标对象进行连续扫描，获得各固定目标对象所对应的各个激光束，进一步得到各激光束分别所对应的点云集合，所述点云集合中包括反光板点云集合和墙壁点云集合，基于点云集合，通过以下步骤A至步骤E确定搭载有激光雷达的移动终端的最终位姿：

实施例

利用步骤A中过程，从Lidar中获取固定目标对象，即反光板的点云集合，根据点云反射强度在第i+1点云帧中提取反光板表面点云坐标，反光板包括但不仅局限于本发明示例性实施例所提供的圆柱体形反光板，如图2(a)所示为激光雷达扫描反光板所得到的反光板表面点云坐标及其周边一定数量的点云数据集合S＝{S_i(x_i,y_i,f_i)|i∈{1,2,...,n}}，其中(x_i,y_i)为点云的坐标，而f_i表示的是该点云的反射强度值，设置反射强度阈值f_t，把f_i>f_t的点判断为反光板表面的点云，图2(a)中实心的点为根据反射强度信息所提取出的反光板表面的点云，设第k个反光板的点云的集合为R_k＝{R_rk(x_rk,y_rk)|r∈{1,2,...,m}}，其中(x_rk,y_rk)表示第k个反光板的第r个点云的坐标；

根据这些坐标使用最小二乘法来拟合反光板横截面圆心坐标，通过最小化式的误差函数可得拟合出的圆心坐标，如图2(b)所示的拟合结果；

反光板的构造如图3所示，其包括圆柱体形的载体以及覆盖在载体表面的反光膜，反光膜的反射强度与入射角的关系如下表1所示，根据点云信息的入射角可以确定相应的反射强度阈值。

表1反光膜的反射强度与入射角的关系

结合图4中“×”点表示拖尾点，根据连续向量的旋转角度来正负判断是否为拖尾点，如拖尾点A与圆心C形成的向量

的方向逆时针旋转到点B与圆心C形成的向量

的方向，而点D不是拖尾点，其与圆心C形成向量

的方向是由向量

的方向顺时针旋转，通过以下公式从反光板点的相邻点A开始逐点计算圆心C到该点形成的向量：

接着计算相邻点之间向量的夹角，根据公式：

如果连续的三个点都是拖尾点，那么向量之间夹角的符号都一样，反之向量之间的夹角符号相反，因此计算∠BCA*∠DCB，如果∠BCA*∠DCB<0证明夹角符号相反，则点B为拖尾点的最后一点，由此可以确定拖尾点的第一个点和最后一个点。

根据步骤B中描述的过程，获得反光板紧靠着的墙壁的位置，确立墙壁的边界，一种是墙壁的边缘点，另一种是墙壁的角点，边缘点定义为墙壁边缘和其他物体的交界处，而角点定义为在一定范围内曲率最大的点，如图5所示，在边缘点附近，连续的激光光束读取到的深度信息会有一个突变的过程，因此可以根据深度的变化率来判断该点是否为边缘点，以图5中连续的两束激光束CP_E和CP_W为例，计算深度的变化率，根据以下公式：

其中函数max表示去两束激光束的长度值中最大的那一个值，当变化率大于设定的阈值后可将该点，如图5中的点P_E，判定为边缘点；

确定墙壁两侧的边缘点后，为了保证拟合的墙壁是一条直线，需要判断边缘点所确定的墙壁范围内是否有角点，获得单一点云的曲率，根据公式：

式中的|T|表示用第i个被检测点周围|T|个来计算该点曲率，‖V_(k,i)‖表示第k个反光板中的第i个点云所代表激光束的长度，‖∑_{j∈|T|,j≠i}(V_(k,i)-V_(k,j))‖为第i个被检测点周围|T|个点与被检测点形成的向量的和，当曲率大于所设定的阈值且是所有点中曲率最大的点时就可以将该点判定为角点；

通过遍历两侧拖尾点相邻的一定数量的点后可以确定墙壁的点云范围，得到第k个反光板周围的墙壁点云集合W_k＝{W_tk(x_tk,y_tk)|t∈{1,2,...,o}}，根据墙壁点云集合可以使用最小二乘法拟合出用一条直线表示的墙壁的位置，根据以下公式进行拟合，拟合结果如图6所示：

y＝ax+b

已知反光板半径，并且反光板紧靠墙壁摆放，则反光板圆心与墙壁的距离应当等于反光板半径，因此根据拟合的墙壁位置，将拟合的反光板圆心沿着圆心与坐标系原点形成的向量的方向平移至与墙壁距离为半径的地方，该点就为修正后的反光板坐标，如图7所示，反光板坐标的修正可以转化为让点C沿着向量CQ平移CC’长度的距离，获得反光板圆心修正需要平移的距离，得到修正后的反光板坐标C′(x_c′,y_c′)，修正效果如图8所示。

应用步骤C所描述过程，分别针对各个反光板表面，如果所含反光板数量大于等于三则以反光板中心位置作为三角形顶点构建反光板三角形集合，如果只有一个或两个反光板则将角点、边缘点信息添加到匹配集合中再构建，以图9的两帧点云信息为例，在第一帧点云坐标系YOX下形成的三角形集合可用反光板标号来表示为：

Tri＝{hij,hik,hil,hig,hjk,hjl,hjg,hkl,hkg,hlg,ijk,ijl,ijg,ikl,ikg,ilg,jkl,jkg,jlg,klg}

由于测量误差以及反光板安装误差的存在，类似于BCD、EFG这样理论上共线的三个反光板点也仍然可以构成三角形，在第二帧点云坐标系Y’O’X’下形成的三角形集合可用反光板标号来表示为：

Tri′

＝{bcd,bce,bcf,bca,bde,bdf,bda,bef,bea,bfa,cde,cdf,cda,cef,cea,cfa,def,dea,dfa,efa}

这两个集合中的每个三角形标识都储存了这个标识里三个反光板形成的三条线段的长度，在构建三角形集合Tri和Tri′时是从离Lidar最近的反光板h或c开始构建，原因是AGV的移动速度限制了相邻两帧点云的位姿变换，第二帧中离Lidar最近的反光板匹配成功的概率也就最高，从该最近点开始构建三角形集合能够加速之后的匹配过程。但在实际使用过程中很可能出现反光板被遮挡的情况，因此结合反光板周围墙壁来弥补不足三个反光板时的定位误差。

如图10所示，当检测到的反光板只有一个或两个时，将每个反光板两侧的角点或者边缘点加入到原始反光板匹配点集中，如果没有角点或是边缘点，则选择墙壁直线上和反光板有一定距离的点加入到原始匹配点集中。

获得两帧点云中反光板构成的三角形集合后，从中的第一个元素开始，与每个元素的三个边长信息进行比对，由于构建三角形时都是按照顺时针的顺序，因此比对时最多只需要三次，以BCD三块反光板，即中的bcd比对中的为例：

首先按照顺序比对三条边长，分别是bc和hk、cd和kg以及db和gh，由于长度不匹配所以此次对比失败；接下来比对bc和kg、cd和gh以及db和hk，仍然失败；最后对比bc和gh、cd和hk以及db和kg，这时就可以比对成功。若三次比对完成后还未成功即可判定为不匹配，不会存在还需要比对bc和gh、cd和kg以及db和hk的情况。

三角形比对成功后就可以获得三个匹配的反光板对，b和g、c和h以及d和k，如果在之前的三角形匹配中没有这样的匹配对，则创建这样的匹配对并保存在匹配对集合Pairs中并将该对得分置为1，如果这样的匹配对已经存在，则给该对的得分加一，即bcd与hkg成功匹配后，集合Pairs会变为Pairs＝{(b,g,1),(c,h,1),(d,k,1)}；之后继续匹配中的第二个元素bce，同样的按照边长匹配方法可以成功匹配中的hlg，获得三个匹配的反光板对，b和g、c和h以及e和l。由于b和g、c和h在之前的匹配中已经创建过，因此此次匹配将Pairs中的这两个匹配对的得分加一，而e和l并未存在于Pairs中，则创建该对并将得分置为1，此时Pairs变为Pairs＝{(b,g,2),(c,h,2),(d,k,1),(e,l,1)}。以此类推，遍历完所有最近反光板b参与构成的三角形后就可以得到一个初步的反光板对集合，如果遍历完所有反光板b参与构成的三角形后没有成功的匹配，则再按顺序匹配剩下的所有三角形以完成匹配。

在匹配过程中仍然可能会出现误匹配的现象，例如三边长度相近的BCF与EFC，在匹配过程中表现为为的bcd和的hjk的匹配。采用上述的匹配方法匹配成功后会出现b和j、c和k以及d和h三个反光板匹配对，在上述的匹配中已经获得了中的b是与中的g匹配的，而误匹配则出现了b和j匹配的情况，匹配过程中，当前帧的某个反光板只能唯一对应前一帧中的某个反光板，当出现了一个反光板匹配多个或是多个匹配一个的情况就证明出现了误匹配现象，此时就可以根据匹配对的得分来进行筛选，在匹配过程中获得的对得分为4，而误匹配的对得分仅为1，选取得分更高的匹配对就可以有效地筛除误匹配带来的错误匹配对，去除后获得最终的反光板对集合Pairs′＝{(b,g,4),(c,h,4),(d,k,4),(e,l,4)}。

根据步骤D中过程，获得反光板之间的匹配关系，可以看做获得了两个点集之间点与点的对应关系，采用奇异值分解来计算反光板构成的点集间的位姿，以得到的集合Pairs′为例，首先根据匹配结果将Pairs′分为两个点集，P＝{P₁(x_b,y_b),P₂(x_c,y_c),P₃(x_d,y_d),P₄(x_e,y_e)}和X＝{X₁(x_g,y_g),X₂(x_h,y_h),X₃(x_k,y_k),X₄(x_l,y_l)}，计算集合P和X的中心，根据以下公式：

计算集合P和X的协方差矩阵，根据公式：

然后对协方差矩阵W_px进行SVD分解，根据公式：

其中σ₁...σ_n为协方差矩阵W_px的奇异值，矩阵U的列为

的特征向量，矩阵V的列为

的特征向量，经过SVD分解后可得旋转矩阵R′和平移矩阵t′的表达式为：R′＝VU^T、t′＝u_x-R′u_p；

这里的R′就是使图9中的XOY坐标系旋转至与X′O′Y′坐标平行的旋转矩阵，而t′就是图9中点O到点O’的平移矩阵。

在获得R′和t′后，将它们作为ICP算法初值然后进行ICP算法迭代计算,首先是根据R′和t′对两帧点云中的第一帧进行转换，然后根据转换后的第一帧点云计算两帧点云间的点与点的距离，把与第二帧中点云距离最近的第一帧中点云作为一个最近点对，在获得所有的最近点对后计算一组新的旋转矩阵和平移矩阵，不断重复上述过程直到迭代达到一定次数或是位姿矩阵的变化量小于一定阈值，最终获得了系统的位姿变换R和t,每一次计算出的帧间位姿变化累加起来就可以得到系统的当前位置的粗略值。

根据步骤E中过程，获得系统的当前位置的粗略值之后，就可以在反光板中心位置KD树中搜索距离该粗略值最近的三个反光板中心位置。以反光板集合{(6,9),(4,1),(1,5),(11,6),(3,7),(2,5),(8,3)}为例，首先以横坐标为划分维度，将所有横坐标从小到大排序后顺序为{1,2,3,4,6,8,11}，中位数为4，所以KD树的根节点为点(4,1)，左分支的点为横坐标小于等于4的点，即{(1,5),(2,5),(3,7)}，右分支的点为横坐标大于4的点，即{(6,9),(8,3),(11,6)}。接下来根据纵坐标对两个分支再进行划分，左分支纵坐标的中位数为5，则第二层的第一个节点为(2,5)，第三层第一个节点为(1,5)，第三层第二个节点为(3,7)。同理，第二层的第二个节点为(11,6)，第三层第三个节点为(8,3)，第三层第四个节点为(6,9)。最终构建的反光板中心位置KD树如图11所示。

之后根据系统的当前位置的粗略值就可以对KD树中已储存的距离激光雷达最近的三个反光板中心位置进行搜索。假设激光雷达根据ICP算法计算出的当前坐标(X,Y)为(7,4)，检测到的反光板数量为3，此时就可以将问题转化为在构建好的反光板中心位置KD树中找到距离点(7,4)最近的三个点。在第一层的比较中，点(7,4)的横坐标大于(4,1)的横坐标，因此进入右分支搜索。第二层是根据纵坐标进行划分，点(7,4)的纵坐标小于(11,6)的纵坐标，因此最终搜索到了叶节点(8,3)。假设首次搜索到的叶节点为最近点，然后根据搜索路径逆序向上回溯。回溯过程中根据公式计算每个节点(x_i,y_i)与目标点(X,Y)的欧式距离d。

如果某节点比当前最近点离目标点更近，则将该节点更新为最近点。当前最近点一定存在于该节点一个子节点对应的区域，检查当前最近点的另一子节点对应的区域是否有更近的点。由于KD树按照相应的维度对数据集合进行了切分，所以计算另一子节点对应的划分出的区域是否和以目标点为圆心，以目标点与最近点间的距离为半径的圆相交。如果相交，可能在另一个子节点对应的区域内存在距目标点更近的点，移动到另一个子节点，继续递归地向下进行最近邻搜索；如果不相交，则向上回溯。当回溯到根节点时，回溯过程结束，最后的最近点即为目标点的最近邻点。根据上述方法就可以找到点(7,4)的第一个最近点为(8,3)。

在寻找第二个最近点时，将搜索到的第一个最近点从KD树中删除。由于最近点不一定是叶节点，因此采取以下方法删除节点。如果被删除节点的两个子树都为空，则用空树来代替。否则，根据当前层的划分维度选择被删除节点的左子树的最右节点或者右子树的最左节点作为替代的节点。再在新的KD树中按照搜索第一个最近邻点的方法去搜索第二个最近邻点，最终得到第二个最近邻点为(4,1)。重复上述过程，可得第三个最近邻点为(11,6)。根据获得三个KD树种距离激光雷达位置粗略值最近的反光板中心位置，与激光雷达当前检测到的反光板中心位置进行边长匹配获得相应的反光板匹配对集合，最后与步骤D类似通过SVD分解获得了系统在KD树坐标系种的位姿变换，也就是移动终端的最终位置。

虽然本发明已以较佳实施例揭露如上，然其并非用以限定本发明。本发明所属技术领域中具有通常知识者，在不脱离本发明的精神和范围内，当可作各种的更动与润饰。因此，本发明的保护范围当视权利要求书所界定者为准。

Claims

1.一种基于ICP和KD树的激光雷达定位方法，其特征在于，利用搭载有激光雷达的移动终端对目标区域内的各个固定目标对象进行连续扫描，获得各固定目标对象所对应的各个激光束，进一步得到各激光束分别所对应的点云集合，所述点云集合中包括反光板点云集合和墙壁点云集合，基于点云集合，通过以下步骤A至步骤E确定搭载有激光雷达的移动终端的最终位姿：

2.根据权利要求1所述的一种基于ICP和KD树的激光雷达定位方法，其特征在于，所述步骤B具体包括以下步骤：

3.根据权利要求1所述的一种基于ICP和KD树的激光雷达定位方法，其特征在于，所述步骤C中所述的三角形构建过程中，反光板集合中包含的反光板中心位置数量大于三时，构建以反光板中心位置为顶点的反光板三角形，当反光板集合中包含的反光板中心位置数量小于三时，将墙壁边缘点和墙壁角点添加至反光板集合中，以反光板中心位置、墙壁边缘点或墙壁角点为顶点构建反光板三角形。

4.根据权利要求3所述的一种基于ICP和KD树的激光雷达定位方法，其特征在于，当各个匹配三角形边长一一比对成功，获得反光板表面的点云匹配对，判断该点云匹配对是否存在于点云匹配对集合中，当点云匹配对不存在于点云匹配集合中，将该点云匹配对保存至点云匹配集合中，并将该点云匹配对的得分置为1，当点云匹配对存在于点云匹配集合中，将点云匹配对的得分加1并更新得分。

5.根据权利要求4所述的一种基于ICP和KD树的激光雷达定位方法，其特征在于，构建反光板三角形以任意反光板中心位置为基础按照顺时针的顺序进行构建。

6.根据权利要求5所述的一种基于ICP和KD树的激光雷达定位方法，其特征在于，所述步骤D中，将匹配分类集合的位姿矩阵作为ICP算法的初值对各个单一点云进行迭代转换，获得各个单一点云之间的距离，把距离最小的两个单一点云作为最近点对，进一步获得新的位姿矩阵并更新原位姿矩阵，直至更新次数达到预设阈值或位姿矩阵的变化量小于预设变化阈值时，获得目标对象的位姿变化，并对更新过程中得到的各个位姿矩阵变化累加，获得累加位姿矩阵，由此确定移动终端的粗略位姿。

7.根据权利要求6所述的一种基于ICP和KD树的激光雷达定位方法，其特征在于，所述步骤E具体包括以下步骤：

8.根据权利要求7所述的一种基于ICP和KD树的激光雷达定位方法，其特征在于，所述步骤E1中构建包含所有反光板中心位置的KD树，以各个反光板中心位置的横坐标作为KD树的奇数层，以各个反光板中心位置的纵坐标作为KD树的偶数层，对KD树划分维度，将小于划分维度中位数的反光板中心位置放置在KD树左分支，剩余的反光板中心位置放置在右分支，直到节点只有一个元素；

所述步骤E2中根据当前KD树层的划分维度来比较该位姿与KD树节点的大小，即奇数层比较横坐标，偶数层比较纵坐标，如果位姿小于节点，则进入节点左侧分支继续进行搜索，反之进入右侧，直至所比较的节点为叶节点，并且不再有分支，此时该节点为最近节点；

所述步骤E3在删除节点时，如果被删除节点的两个子树都为空，则用空树来代替，否则，根据当前层的划分维度选择被删除节点的左子树的最右节点或者右子树的最左节点作为替代的节点。

9.根据权利要求1-8任意一项所述的一种基于ICP和KD树的激光雷达定位方法，其特征在于，所述固定目标对象为反光板。