一种基于3D点云配准的定位方法与流程

一种基于3d点云配准的定位方法
技术领域
1.本发明涉及机器人自主定位导航技术领域，尤其涉及一种基于3d点云配准的定位方法。


背景技术：

2.随着计算机技术的发展，移动机器人取得了广泛的关注，机器人的自主导航是移动机器人的基础功能，机器人的定位是实现机器人自主导航的前提。机器人通过自身携带的传感器模块可以获取自身与环境的信息。现有的定位技术常用的传感器有惯性导航传感器(imu)、轮式编码器、gps、激光雷达等。其中gps适用于室外环境的定位与导航，在信号不佳的室内环境中，gps精度难以达到要求甚至无法正常工作。
3.目前，激光雷达结合惯性导航传感器和轮式编码器的定位技术广泛的应用于室内的移动机器人中。其中轮式编码器结合惯性导航传感器(imu)可以获得机器人的位置信息以及姿态信息。在机器人移动时，对轮式编码器和惯性导航传感器的原始数据进行解算可以获得机器人的运动速度以及加速度等运动信息。将机器人的姿态信息和运动信息结合，得到机器人的里程计信息。通过激光雷达扫描到的当前环境信息与保存的二维地图进行匹配，得到的机器人在环境中的估计位姿。单线激光雷达的最大探测范围为10～20m，在空旷的大型室内环境中无法探测到足够的环境信息，导致机器人定位失败。多线的激光雷达的探测范围可达100m以上，现有的激光雷达定位方案中将多线激光雷达的三维激光点云数据转换为二维的激光数据来代替单线激光雷达，有效的提升激光数据的有效范围，提高定位的精度。但是在特征点稀疏的环境中使用仅仅包含平面信息的激光数据与二维的地图进行配准得到机器人的位姿信息并不准确，因为平面的激光数据无法准确的描述机器人所处的环境信息，二维的地图也无法完整的保存环境信息。因此，在某些特殊的使用场景，比如长走廊中，机器人的定位可能会出现较大的误差。


技术实现要素：

4.本发明为了克服已有技术的不足之处，提供一种基于3d点云配准的定位方法，用以提高定位精度，增强对特殊环境的适用性和鲁棒性。
5.该方法使用多线激光雷达，将实时扫描得到的机器人环境点云经过处理后与通过3dslam技术获取的三维环境点云地图进行匹配，得到机器人在工作环境中的估计位姿。将该数据与惯性导航传感器和轮式编码器的数据进行融合计算出更为准确的机器人位姿信息。将定位模块得到的位姿信息作为输入数据，通过move_base算法和teb算法规划机器人的行驶路径实现机器人的导航功能。
6.一种基于3d点云配准的定位方法，包括以下步骤：
7.步骤1：传感器外参标定；将惯性导航传感器安装在机器人的底座上，将该位置作为基坐标系的原点；通过手动测量激光雷达与惯性导航传感器的相对位置关系计算出雷达坐标系和基坐标系的相对位姿关系；在工控机上使用ros(机器人操作系统)的通讯机制发
布雷达坐标系和基坐标系的tf转换关系话题；
8.步骤2：3d建图；控制机器人在工作环境中移动；通过3dslam技术获取机器人工作环境的3d点云地图并对点云进行降采样，3d点云地图以八叉树的数据格式进行保存；提取出点云地图设定高度范围的点云，将其投影到机器人底盘坐标系原点的水平面上，形成二维的栅格地图；
9.步骤3：特征点提取与匹配；
10.步骤3-1：点云预处理；使用3d激光雷达获取实时点云数据；选取一个起始点和最末点，两个点的角度保持在π与3π之间，将一帧点云的角度控制在起始点和最末点的范围之内；将接收到的点云信息保存为二维数组的形式，数组的第一个索引表示点所在的线数，第二个索引表示点是在该线上的第几个点，数组保存的数据表示该点与激光雷达的距离；根据点云的距离剔除距离过远或者过近的点云；计算多线激光雷达中水平下方的激光束中每个点与其前一个点的水平夹角，将两点之间的水平夹角小于一定阈值的两个点标记为地面点；使用点云聚类算法将满足聚类数量的点云聚类为多个点云集合，无法形成聚类的点云作为噪声点剔除；
11.步骤3-2：特征点提取；计算除地面点外的所有聚类点的曲率值；计算公式为：
[0012][0013]
其中ri表示第i个点云的距离值，s表示计算点云的数量；将一帧的点云根据方位分为多个区域，在每个区域中按照每个点的曲率对点云进行排序，将曲率值大于一定阈值的点标记为角点，将曲率小于一定阈值的点标记为平面点；曲率最大的几个点和最小的几个点标记为大角点和大平面点；
[0014]
步骤3-3：帧间匹配；使用点云匹配算法分别将当前点云与前一帧点云对角点和平面点进行匹配计算；将当前帧点云中的大角点与前一帧点云中的角点进行点对线匹配，得到机器人位姿中的俯仰角、翻滚角和z轴变化信息；以俯仰角、翻滚角和z轴变化为约束将当前帧点云中的大平面点与前一帧点云中的平面点进行点对面匹配计算得到机器人位姿中的x、y和偏航角变化信息；假设激光雷达的运动是匀速的；计算一帧数据终止点相对于起始点的转换矩阵后可以对这一帧数据中的任意点按照其获得时相对于起始点的时间进行插值，获得每一个点的位姿；插值的公式如下所示：
[0015][0016]
其中ti表示一帧激光点云数据中第i个imu的数据的时间，表示在接收到第k帧激光点云数据后接收到第i个imu数据时激光里程计的位姿；使用一个旋转矩阵r和一个平移量t表示这一帧数据中的点和上一帧数据中点的对应关系：
[0017][0018]
对旋转矩阵进行求导得到点到线和面的距离，并获得用于优化的误差函数：
[0019][0020]
其中f的每一行代表一个特征点，通过牛顿迭代法对点云帧间匹配得到的位姿信
息进行优化得到激光里程计数据并通过ros话题的方式发布出去；
[0021]
步骤3-4：点云地图匹配；使用kd-tree最邻近搜索算法将当前点云与步骤2-1建立的点云地图进行匹配搜索；将符合阈值条件的地图点云保存为历史关键帧点云用于匹配；使用点云匹配算法计算历史关键帧点云与当前点云的相对转换关系得到最终的位姿转换关系；以步骤3-2中计算得到的激光里程计作为优化目标，在位姿图中添加匹配得到的位姿约束对优化目标进行优化最终得到准确的机器人位姿数据；
[0022]
步骤4：将步骤3-3的机器人位姿信息和点云数据转换形成二维激光数据作为输入数据，将步骤2-1的2d栅格地图作为全局导航地图；使用ros导航算法进行路径规划实现机器人的自主导航；
[0023]
本发明的优点和积极效果是：
[0024]
1、本发明对地图点云进行降采样压缩，在不影响定位精度的前提下减小了点云地图中点云的数量，提高了运算的速度。
[0025]
2、本发明是基于多线激光雷达获取环境信息，相较于单线激光雷达，多线激光雷达探测的的范围远大于单线激光雷达，并且可以采集垂直空间的点云信息。能够得到更为丰富的环境信息。
[0026]
3、本发明使用聚类的方法将点云信息进行分割，过滤。将分别使用相同属性的点云进行配准，既提高了配准的精度，又降低了配准的计算量，提高了计算速度。
[0027]
4、本发明是基于3d点云配准的方法，将实时的激光点云数据与建立完成的环境点云地图进行配准，减少了特征点稀疏对定位的影响，提高了定位的精度。
[0028]
5、本发明根据实际使用需求对多线激光点云数据进行投影得到合适的二维激光数据，提高了导航的灵活性。
附图说明
[0029]
图1为本发明的流程图。
[0030]
图2为建立的环境点云地图。
[0031]
图3为实时采集的环境点云信息。
[0032]
图4a～图4b是点云分割后的点云信息示意图，其中图4a是角点与平面点示意图，图4b是地面点示意图。
[0033]
图5三维点云数据转换得到的二维激光数据。
具体实施方式
[0034]
以下结合附图对本发明做进一步详述。
[0035]
一种基于3d点云的定位方法，其工作流程框图如图1所示，该方法由3dslam、数据采集、特征点提取与匹配优化、路径规划四个步骤组成。图2显示的是通过3dslam技术构建的环境点云地图。其中的点云包含位置信息以及属性信息。图3是通过多线激光雷达采集到的当前环境的实时点云数据。图4为经过点云分割后的点云数据，包括角点、平面点以及地面点。在导航的过程中使用图5所示的二维激光数据实现避障。
[0036]
结合图1-5，本发明专利的具体实施方式如下：
[0037]
步骤1：传感器外参标定。将惯性导航传感器安装在机器人的底座上作为基坐标系
的原点。将robosense16线激光雷达安装在惯性导航传感器的上方1m处，工控机采用英伟达的tx2开发板，开发板装有ubuntu16.04操作系统 ros kinetic pcl gtsam作为软件平台。通过手动测量激光雷达与惯性导航传感器的相对位置关系计算出雷达坐标系和基坐标系的相对位姿关系。使用ros(机器人操作系统)的通讯机制发布雷达坐标系和基坐标系的tf转换关系话题。
[0038]
步骤2：3d建图。通过logic遥控手柄控制机器人在工作环境中移动，保证激光雷达可以扫描到各个方位。在机器人移动的过程中，将惯性导航传感器的imu数据和激光雷达的点云数据作为输入数据，运行lego-loam3dslam算法对实验环境进行点云地图构建，使用点云压缩算法对点云进行压缩降采样。在机器人建图完成后使用pcl的点云工具对点云地图进行保存。提取出点云地图1-2m高度范围的点云，将其投影到机器人底盘坐标系原点的水平面上，形成二维的栅格地图。
[0039]
步骤3：特征点提取与匹配。
[0040]
步骤3-1：点云预处理。在机器人导航时使用多线雷达对周围环境进行实时的扫描，接收到的点云数据进行预处理。创建ros的话题用于接收激光点云的数据，将激光点云数据转换为ros的格式。将激光雷达运行一周接收到的点云数据视为一帧点云数据，一帧中接收到的第一个点云数据视为起始点，接收到的最后一个点云数据视为最末点。以x轴的负轴为基准，计算起始点与最末点的水平角度，如果起始点角度与最末点角度的差大于3π则将最末点的角度减去2π。如果起始点角度和最末点角度小于π，则最末点角度加上2π。通过以上方法，使得起始点与最末点的角度差保持在π与3π之间。将所有的点云数据投影到一个二维的数组上，数组的第一个索引表示点所在的线数，第二个索引表示点是在该线上的第几个点，数组保存的数据表示该点与激光雷达的距离。去除点云中距离过远或者过近的点云。本发明使用的激光雷达共有16根激光束，机器人运行时的工作地面可视为水平面，因此只有自下而上的8根线有可能扫描到地面。遍历8根激光束所有的点，找到与当前点在二维数组中第一个索引值相邻，第二个索引值相同的中点，即与当前点水平角度相同并且激光束相邻的点。计算当前点云和相邻点的水平角度差，若角度差小于10
°
，两个点均标记地面点。对地面点之外的点云使用广度优先搜索的方法进行标记，即取一个核心点，计算核心点八邻域的各个点云与核心点的距离，若距离小于阈值则视为处于一个聚类，若大于阈值则不再同一个聚类。以八邻域中处于同一个聚类的点作为核心点，计算该点与其八邻域中未筛选过的点的距离，大于阈值则不处于一个聚类，小于阈值则视为在一个聚类。以此类推，直到所有的点的八邻域中为筛选过的点都不在一个聚类中则结束一个聚类的搜索。若一个聚类中点云的数量大于30个，将该聚类中的点标记可用的聚类点。若一个聚类中的点云数量少于30个，则该聚类中的点云均标记离散点。通过聚类的方法可以有效的剔除激光点云数据中的噪声点。将处理后的点云数据通过ros话题的方式发布出去。
[0041]
步骤3-2：特征点提取。创建一个ros节点接收步骤3-1发布的点云话题。使用惯性导航传感器和轮式编码器的融合数据对处理后的点云数据进行插值处理去除运动畸变。遍历除地面之外的每一个点云，计算每个点与前后相邻的五个的偏差作为曲率值c，计算公式为：
[0042][0043]
其中ri表示第i个点云的距离值，s表示计算点云的数量，在本发明中s为10。将一圈点云均分为6个扇形区域，在每个区域中按照每个点的曲率对点云进行排序，曲率值大于阈值的视为角点，曲率值小于阈值的为平面点。角点中曲率最大的2个点标记为大角点，平面点中曲率最小的10个点标记为大平面点。通过点对点匹配找到两帧点云之间的位姿变换关系。用当前帧点云的大角点集合与前一帧点云的角点集合进行匹配，当前帧点云的大平面点集合与前一帧点云的平面点集合进行匹配，这种匹配方式减小了匹配的点云数量，提高了匹配的精度。
[0044]
步骤3-3：帧间匹配。为了进一步提高匹配的精度，采用了点到线以及点到面的匹配方法。角点到角点线的对应，首先找到角点i对应的上一帧点云数据中距离最近的两个点j和l，然后判断这两个点是否是角点，j和l必须是不同的线上中的点，因为一次一个线在某一段中最多有一个边缘点。平面点对应上一帧点云中的平面。找到上一帧点云数据最近点，并在该线扫上找到另一点，再在相邻扫描线上找一点，这样就可以保证三个点不在一条线上组成一个平面。有了点到线和点到面的对应关系，接下来就要求点到线和点到面的距离。首先求点到线段的距离dε，公式如下所示：
[0045][0046]
其中表示第k帧点第i个点，则表示第k 1帧的第i个点的点云到第k帧第j个点的点云的向量。该公式的分子是两个向量的叉乘，而叉乘后求模就变成了求两个向量构成的三角形的面积。公式的分母是向量的模相当于三角形底边的长。三角形面积除以一条边就可以求出该边到对应顶点的距离，也就是边角点到边角线的距离。
[0047]
接下来求平面点到对应平面的距离dh，公式如所示：
[0048][0049]
公式中分子的上部分计算得到一个向量，表示立方体的高，下部分计算得到一个向量，两个向量叉乘再取模表示的是三角形面积，两者相乘就表示立方体体积。而分母表示立方体底面三角形的面条，得到的高就是平面点到平面的距离。假设激光雷达的运动是匀速的。计算一帧数据终止点相对于起始点的转换矩阵后可以对这一帧数据中的任意点按照其获得时相对于起始点的时间进行插值，获得每一个点的位姿。插值的公式如下所示：
[0050][0051]
其中ti表示一帧激光点云数据中第i个imu的数据的时间，表示在接收到第k帧激光点云数据后接收到第i个imu数据时激光里程计的位姿。使用一个旋转矩阵r和一个平
移量t表示这一帧数据中的点和上一帧数据中点的对应关系：
[0052][0053]
对旋转矩阵进行求导得到点到线和面的距离，并获得用于优化的误差函数：
[0054][0055]
其中f的每一行代表一个特征点，通过牛顿迭代法对点云帧间匹配得到的位姿信息进行优化得到激光里程计数据并通过ros话题的方式发布出去。
[0056]
步骤3-4：点云地图匹配。为了减少匹配的计算量，对当前点云进行降采样。使用kd-tree最邻近搜索算法将当前点云与点云地图进行匹配搜索。将匹配距离小于0.5的地图点云保存为历史关键帧点云用于匹配。使用icp匹配算法计算历史关键帧点云与当前点云的相对转换关系得到最终的位姿转换关系。以步骤3-3中计算得到的激光里程计作为优化目标，使用gtsam在位姿图中添加匹配得到的位姿约束，使用图优化算法对优化目标进行优化最终得到准确的机器人位姿数据。
[0057]
步骤4：使用navigation导航工具包进行导航，该工具包需要两个输入数据：二维的全局地图以及二维激光数据。其中二维的全局环境地图由步骤2获得，二维激光数据通过将多线激光雷达第5到第10根线获取的点云数据投影到一个二维的水平面上形成。使用步骤3-4计算得到的机器人位姿代替自适应蒙特卡洛算法得到的机器人位姿数据。通过move_base算法规划全局的行驶路径，将二维激光膨胀处理后形成局部代价地图，使用teb算法进行局部路径规划，实现机器人的自主导航。