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

技术特征：
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的数据的时间，表示在接收到第k帧激光点云数据后接收到第i个imu数据时激光里程计的位姿；使用一个旋转矩阵r和一个平移量t表示这一帧数据中的点和上一帧数据中点的对应关系：
对旋转矩阵进行求导得到点到线和面的距离，并获得用于优化的误差函数：其中f的每一行代表一个特征点，通过牛顿迭代法对点云帧间匹配得到的位姿信息进行优化得到激光里程计数据并通过ros话题的方式发布出去；步骤3-4：点云地图匹配；使用kd-tree最邻近搜索算法将当前点云与步骤2-1建立的点云地图进行匹配搜索；将符合阈值条件的地图点云保存为历史关键帧点云用于匹配；使用点云匹配算法计算历史关键帧点云与当前点云的相对转换关系得到最终的位姿转换关系；以步骤3-2中计算得到的激光里程计作为优化目标，在位姿图中添加匹配得到的位姿约束对优化目标进行优化最终得到准确的机器人位姿数据；步骤4：将步骤3-3的机器人位姿信息和点云数据转换形成二维激光数据作为输入数据，将步骤2-1的2d栅格地图作为全局导航地图；使用ros导航算法进行路径规划实现机器人的自主导航。2.如权利要求1所述的一种基于3d点云配准的定位方法，其特征在于：步骤1中采用英伟达的tx2开发板作为硬件平台，开发板装有ubuntu16.04操作系统 ros kinetic pcl gtsam作为软件平台。3.如权利要求1所述的一种基于3d点云配准的定位方法，其特征在于：步骤2所述的提取出点云地图设定高度范围是1～2米；构建3d点云地图的算法为lego-loam slam算法。4.如权利要求1所述的一种基于3d点云配准的定位方法，其特征在于：步骤3-1所述的3d激光雷达共有16跟激光束，机器人运行时的工作地面可视为水平面且激光雷达水平安装，因此只有自下而上的8根线有可能扫描到地面；判断地面点云的角度阈值为5
°
；判断聚类点云是否满足要求的阈值为40个。5.如权利要求1所述的一种基于3d点云配准的定位方法，其特征在于：步骤3-2所述的将一圈点云分割的扇形区域的数量为6个；在点云中提取的大角点数量为3个，大平面点的数量为15个。6.如权利要求1所述的一种基于3d点云配准的定位方法，其特征在于：步骤4所述的将多线激光雷达投影到一个二维的水平面形成二维激光数据时采用的激光束为第5根到第10根。

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


技术研发人员：禹鑫燚 蔡传烩 钱学成 欧林林 叶超
受保护的技术使用者：浙江工业大学
技术研发日：2021.09.18
技术公布日：2022/2/6