[发明专利]一种移动机器人运动中三维激光测距数据动态补偿方法

摘要

本发明提供了一种面向动态过程中的三维激光数据动态补偿方法，属于移动机器人自主环境感知技术领域；本发明最大的创新点在于改善了在运动过程中三维激光数据采集方式，且通过合理的数据精简方法，提高算法的执行效率；为此本发明采用动态数据匹配方法，矫正由里程计引入的机器人位姿误差，提高动态过程中获得三维激光数据的精度，另一方面，采用平面提取方法滤除杂乱的激光数据，降低匹配激光点数量，使得所提方法可以在线实时运行；本发明可用在移动机器人结构化场景重构等领域。

主权项

一种移动机器人运动中三维激光测距数据动态补偿方法，其特征在于：先获取三维环境的点云作为匹配模板；对平面点云进行栅格化处理并计算各栅格内点云形状参数，构建含有形状参数的向量，基于最小二乘法计算两幅点云之间的变换关系，校正机器人位姿，进而对三维激光测距数据进行补偿；具体步骤如下：a)移动机器人在初始位姿利用360度全景三维激光测距传感器对环境进行扫描，获取三维环境的激光测距数据，通过坐标变换将激光测距数据变换到全局坐标系下构成点云作为匹配模板；b)在移动机器人运动过程中，设旋转云台旋转半周时间为一个周期，将每个周期分割成5等份，在每一小段时间内利用移动机器人的里程计数据推算机器人位姿，并利用预测的机器人位姿对运动过程中构建的点云进行初步校正；c)定义一个k×k的邻域，将邻域内的激光点进行平面拟合，取k>3；拟合的目标是使得取得最小值，其中pi＝[x,y,z]T为一个激光点，m为激光点重心，n为拟合后平面的单位化法向量，i＝1，2，...，N，N为邻域内激光点个数；引入邻域点集的协方差矩阵得到D＝nTAn，欲使D最小，引入拉格朗日乘子λ，使用拉格朗日乘子法得由此可得An＝λn，从而得到D＝nTλn＝λnTn＝λ，D能达到的最小值，恰恰是协方差矩阵A的最小特征值，而使D能取得该最小值的法向量n，就是A的最小特征值所对应的特征向量；设定一个阈值Pthresh，如果求出A的最小特征值λmin小于Pthresh，则认为平面拟合成功，否则，认为拟合失败，删除局部邻域拟合平面失败的点云；d)对保留的点云进行栅格化处理，计算栅格内的形状参数P为栅格内点云平面相似度，λ1，λ2，λ3为适应点云二次曲面形状的参数的三个特征值且λ1≤λ2≤λ3，其中M为栅格激光点的数目，qi为栅格内第i个激光点，μ为栅格内点云重心，i＝1，2，...，M；为每个栅格构建含有其形状参数的向量其中ν1是S特征值最小的特征向量，即平面法向量，α用来修正栅格内点云重心位置和局部形状之间的权重；基于向量间的欧几里得距离最小准则完成栅格匹配，结合配准约束CT(‑[μ]×δr+δt)，其中[μ]×表示μ的斜对称矩阵，δr为旋转分量，δt为平移分量；利用最小二乘方法求出两幅点云之间的旋转平移关系，校正机器人位姿，进而校正三维激光测距数据。