一种基于3D点云配准的定位方法技术

本发明专利技术公开了一种基于3D点云的定位方法，包括：3DSLAM、数据采集、特征点提取与匹配优化、路径规划。3DSLAM过程使用移动机器人建立完整工作环境的点云地图。在导航时，使用多线激光雷达获取实时的点云数据，对其进行预处理和特侦点提取。通过帧间匹配得到激光里程计数据，再对激光里程计数据进行优化。使用最邻近搜索，搜索与当前帧点云邻近的点云数据。使用icp匹配算法计算出当前机器人位姿与点云地图中机器人位姿的相对转换关系，将其作为约束添加到位姿图中对激光里程计进行优化，得到准确的机器人位姿信息。使用navigation工具实现机器人的自主导航。本发明专利技术提高了移动机器人导航时定位的精度，同时提高了定位的稳定性和可靠性。性。性。

【技术实现步骤摘要】
一种基于3D点云配准的定位方法


[0001]本专利技术涉及机器人自主定位导航
，尤其涉及一种基于3D点云配准的定位方法。

技术介绍

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

技术实现思路

[0004]本专利技术为了克服已有技术的不足之处，提供一种基于3D点云配准的定位方法，用以提高定位精度，增强对特殊环境的适用性和鲁棒性。
[0005]该方法使用多线激光雷达，将实时扫描得到的机器人环境点云经过处理后与通过3DSLAM技术获取的三维环境点云地图进行匹配，得到机器人在工作环境中的估计位姿。将该数据与惯性导航传感器和轮式编码器的数据进行融合计算出更为准确的机器人位姿信息。将定位模块得到的位姿信息作为输入数据，通过move_base算法和teb算法规划机器人的行驶路径实现机器人的导航功能。
[0006]一种基于3D点云配准的定位方法，包括以下步骤：
[0007]步骤1：传感器外参标定；将惯性导航传感器安装在机器人的底座上，将该位置作为基坐标系的原点；通过手动测量激光雷达与惯性导航传感器的相对位置关系计算出雷达坐标系和基坐标系的相对位姿关系；在工控机上使用ROS(机器人操作系统)的通讯机制发
布雷达坐标系和基坐标系的TF转换关系话题；
[0008]步骤2：3D建图；控制机器人在工作环境中移动；通过3DSLAM技术获取机器人工作环境的3D点云地图并对点云进行降采样，3D点云地图以八叉树的数据格式进行保存；提取出点云地图设定高度范围的点云，将其投影到机器人底盘坐标系原点的水平面上，形成二维的栅格地图；
[0009]步骤3：特征点提取与匹配；
[0010]步骤3
‑
1：点云预处理；使用3D激光雷达获取实时点云数据；选取一个起始点和最末点，两个点的角度保持在Π与3Π之间，将一帧点云的角度控制在起始点和最末点的范围之内；将接收到的点云信息保存为二维数组的形式，数组的第一个索引表示点所在的线数，第二个索引表示点是在该线上的第几个点，数组保存的数据表示该点与激光雷达的距离；根据点云的距离剔除距离过远或者过近的点云；计算多线激光雷达中水平下方的激光束中每个点与其前一个点的水平夹角，将两点之间的水平夹角小于一定阈值的两个点标记为地面点；使用点云聚类算法将满足聚类数量的点云聚类为多个点云集合，无法形成聚类的点云作为噪声点剔除；
[0011]步骤3
‑
2：特征点提取；计算除地面点外的所有聚类点的曲率值；计算公式为：
[0012][0013]其中r
i
表示第i个点云的距离值，S表示计算点云的数量；将一帧的点云根据方位分为多个区域，在每个区域中按照每个点的曲率对点云进行排序，将曲率值大于一定阈值的点标记为角点，将曲率小于一定阈值的点标记为平面点；曲率最大的几个点和最小的几个点标记为大角点和大平面点；
[0014]步骤3
‑
3：帧间匹配；使用点云匹配算法分别将当前点云与前一帧点云对角点和平面点进行匹配计算；将当前帧点云中的大角点与前一帧点云中的角点进行点对线匹配，得到机器人位姿中的俯仰角、翻滚角和z轴变化信息；以俯仰角、翻滚角和z轴变化为约束将当前帧点云中的大平面点与前一帧点云中的平面点进行点对面匹配计算得到机器人位姿中的x、y和偏航角变化信息；假设激光雷达的运动是匀速的；计算一帧数据终止点相对于起始点的转换矩阵后可以对这一帧数据中的任意点按照其获得时相对于起始点的时间进行插值，获得每一个点的位姿；插值的公式如下所示：
[0015][0016]其中t
i
表示一帧激光点云数据中第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、本专利技术是基于多线激光雷达获取环境信息，相较于单线激光雷达，多线激光雷达探测的的范围远大于单线激光雷达，并且可以采集垂直空间的点云信息。能够得到更为丰富的环境本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于3D点云配准的定位方法，其具体步骤如下：步骤1：传感器外参标定；将惯性导航传感器安装在机器人的底座上，将该位置作为基坐标系的原点；通过手动测量激光雷达与惯性导航传感器的相对位置关系计算出雷达坐标系和基坐标系的相对位姿关系；在工控机上使用机器人操作系统ROS的通讯机制发布雷达坐标系和基坐标系的TF转换关系话题；步骤2：3D建图；控制机器人在工作环境中移动；通过3DSLAM技术获取机器人工作环境的3D点云地图并对点云进行降采样，3D点云地图以八叉树的数据格式进行保存；提取出点云地图设定高度范围的点云，将其投影到机器人底盘坐标系原点的水平面上，形成二维的栅格地图；步骤3：特征点提取与匹配；步骤3
‑
1：点云预处理；使用3D激光雷达获取实时点云数据；选取一个起始点和最末点，两个点的角度保持在Π与3Π之间，将一帧点云的角度控制在起始点和最末点的范围之内；将接收到的点云信息保存为二维数组的形式，数组的第一个索引表示点所在的线数，第二个索引表示点是在该线上的第几个点，数组保存的数据表示该点与激光雷达的距离；根据点云的距离剔除距离过远或者过近的点云；计算多线激光雷达中水平下方的激光束中每个点与其前一个点的水平夹角，将两点之间的水平夹角小于一定阈值的两个点标记为地面点；使用点云聚类算法将满足聚类数量的点云聚类为多个点云集合，无法形成聚类的点云作为噪声点剔除；步骤3
‑
2：特征点提取；计算除地面点外的所有聚类点的曲率值；计算公式为：其中r
i
表示第i个点云的距离值，S表示计算点云的数量；将一帧的点云根据方位分为多个区域，在每个区域中按照每个点的曲率对点云进行排序，将曲率值大于一定阈值的点标记为角点，将曲率小于一定阈值的点标记为平面点；曲率最大的几个点和最小的几个点标记为大角点和大平面点；步骤3
‑
3：帧间匹配；使用点云匹配算法分别将当前点云与前一帧点云对角点和平面点进行匹配计算；将当前帧点云中的大角点与前一帧点云中的角点进行点对线匹配，得到机器人位姿中的俯仰角、翻滚角和z轴变化信息；以俯仰角、翻滚角和z轴变化为约束将当前帧点云中的大平面点与前一帧点云中的平面点进行点对面匹配计算得到机器人位姿中的x、y和偏航角变化信息；假设激光雷达的运动是匀速的；计算一帧数据终止点相对于起始点的转换矩阵后可以对这一帧数据中的任意点按照其获得时相对于起始点的时间进行插值，获得每一个点的位姿；插值的公式如下所示：其中t
i
表示一帧激光点云数据中第i个IMU的数据的时间，...

【专利技术属性】
技术研发人员：禹鑫燚，蔡传烩，钱学成，欧林林，叶超，
申请(专利权)人：浙江工业大学，
类型：发明
国别省市：