CN115446834B - 一种基于占据栅格配准的车底巡检机器人单轴重定位方法 - Google Patents

Abstract

本发明公开了一种基于占据栅格配准的车底巡检机器人单轴重定位方法，具体为：机器人再部署期间进行模板采集；机器人运行期间，机器人依据重定位点编号，从SQL数据库中取出采集配置，根据采集配置中的采集时间累积当前点云数据，并对点云数据进行直通滤波；取出模板点云数据，对当前点云与模板点云进行下采样与统计学滤波，并使用单轴配准算法计算当前点云与模板点云之间在x轴方向的平移变换关系；再将当前点云与模板点云的平移变换关系叠加到模板位姿上，使用该新位姿重置机器人里程计。本发明提升了在复杂车底点云下点云相似程度计算效率，降低了在重复的检修地沟特征下算法陷入局部最优的失效概率，提升了算法在多个场景下的适用性。

Description

一种基于占据栅格配准的车底巡检机器人单轴重定位方法

技术领域

本发明属于机器人领域，具体涉及一种基于占据栅格配准的车底巡检机器人单轴重定位方法。

背景技术

车底巡检机器人工作在狭窄的检修地沟内前后运动，根据预设的项点，对地沟上方的车辆底部的关键零部件(如螺栓、轮组、滤网、管线等)进行图像信息与特征信息的采集，并通过分析系统的处理，给出车辆是否需要进行维护的建议。车底巡检机器人提升了巡检质量与巡检效率，降低了工人的工作强度。

车底巡检机器人的重定位主要有如下两个任务：第一个，由于机器人在检修地沟内的移动距离往往达到几十甚至数百米，里程计会产生累积误差，使得机器人定位不准，无法准确地回到原点，此时需要重定位来修正机器人的里程计累积误差；第二个，由于车辆每次在检修地沟上方的停车位置不同，因此车辆坐标系与巡检地沟坐标系的变换关系是不定的，而巡检导航点的位姿需要基于这一变换关系计算，此时需要亚厘米级的重定位来检测车辆的停车位置。对于传统巡检机器人而言，重定位往往依赖于在运用现场安装反光板、二维码等外设，部署成本高，较新的方法中则考虑为巡检机器人配置3D激光雷达，从而使重定位转化为点云配准问题。S.Wanayuth等人于2013年在文章"Under-vehicle inspectionutilizing a mobile robot with a LRF sensor"中提出对于车底巡检机器人，可使用最近点迭代(ICP)算法进行点云配准，从而检测车辆的停车位置；Q.Tian等人于2019年在文章"A Novel Global Relocalization Method Based on Hierarchical Registration of3D Point Cloud Map for Mobile Robot"中提出对于服务机器人，可通过对正态分布转换(NDT)算法与ICP算法的联合使用(NDT+ICP)，进行服务机器人全局定位误差的修正。然而上述算法均未考虑到特殊巡检对象车底点云复杂性以及检修地沟环境的特殊性，对于类似列车转向架的复杂点云，其所需要的处理时间过长，精度不高；而对于检修地沟这一具有大量重复墙面与立柱特征的场景，其优化迭代的计算方式使得存在陷入局部最优的配准错误的风险。同时，现有算法均在具有良好的迭代初值的前提下，才能具有准确的计算结果，而重定位场景下，对于良好的迭代初值的取得是较为困难的。

因此，有必要针对该场景研究车底巡检机器人在检修地沟内单自由度运动时的重定位实施策略与高效率高精度的配准算法，以达成车底巡检机器人重定位的主要目标。

发明内容

为在检修地沟内高精度、高效率地重定位任务，进而保证可以安全且高效的完成车辆的车底巡检作业。本发明根据车底巡检机器人在巡检地沟内的单自由度工作场景，基于车底特征点云与检修沟特征点云的特性，提供了一种基于占据栅格配准的车底巡检机器人单轴重定位方法。

本发明的一种基于占据栅格配准的车底巡检机器人单轴重定位方法，包括机器人部署期间模板采集和运行期间基于模板重定位两部分。

机器人部署期间模板采集具体步骤为：

步骤A：选取重定位点，生成重定位点编号，并基于外部准确测量(如卷尺、激光测距仪等)将机器人移动到巡检地沟中对应位置上。

步骤B：根据现实情况确定点云累积时间、点云所需空间区域与点云下采样分辨率，根据编号生成采集配置；使用3D激光雷达累积点云数据，根据编号生成模板点云数据，同时将外部准确测量距离结果根据编号生成模板位姿，以重定位点编号为主键，插入到SQL数据库中。

步骤C：在机器人的业务流程中加入重定位部分，并给定重定位点编号，使得机器人自行从SQL数据库中读取采集配置、模板点云数据与模板位姿，并根据实际状况运行定位修正。

机器人运行期间基于模板重定位具体步骤为：

步骤1：机器人根据业务流程，依据重定位点编号，从SQL数据库中取出重定位点模板位姿，并以里程计定位信息作为参考，行驶到里程计认为已到达模板位姿的位置上停车，准备开始采集当前点云。

步骤2：机器人依据重定位点编号，从SQL数据库中取出采集配置，并调用3D激光雷达，根据采集配置中的采集时间累积当前点云数据，并根据采集配置中的所需空间区域对点云数据进行直通滤波，使其仅保留所需的部分。

步骤3：考虑到机器人在巡检地沟内仅具有前后行驶的单自由度，将机器人运行方向定义为x轴正方向，从SQL数据库中取出模板点云数据，对当前点云与模板点云进行下采样与统计学滤波，并使用单轴配准算法计算当前点云与模板点云之间在x轴方向的平移变换关系。

步骤4：将当前点云与模板点云的平移变换关系叠加到模板位姿上，使用该新位姿重置机器人里程计。

进一步的，步骤3中的单轴配准算法步骤具体为：

步骤3.1：生成x方向上的平移搜索空间，平移搜索空间表示为：

式中，d_x为搜索间隔，为x轴负方向上的最大搜索范围，即代数值最小的搜索距离，/>为x轴正方向上的最大搜索范围，即代数值最大的搜索距离，n为搜索空间的大小，表示为：

从而搜索空间中第i项对应的x轴上的平移表示为：

步骤3.2：枚举平移搜索空间T_x，将当前点云P_c根据t_i变换为搜索点云P_t，点云P_t中的每一个点表示为：

其中，为当前点云P_c中的每一个点。

步骤3.3：将搜索点云P_t与模板点云P_m以一定分辨率转换为八叉树空间占据栅格M_t与M_m，从而可以查询M_m中标记为占据的栅格，其对应栅格在M_t中是否为占据状态，而这一查询是在栅格编号层面连续的。

步骤3.4：定义测度函数E(P_t,P_m)，用于评估搜索点云与模板点云的相似程度，考虑到点云均已被转换为八叉树空间占据栅格，因此测度函数表示为：

而配准问题则转化为了求函数最大值的问题，表示为：

使用平移空间搜索的方式，即可计算出当测度函数E(P_t,P_m)达到最大值时的平移变换t_r，t_r即为输出的配准结果。

本发明的有益技术效果为：

本发明提出的方法与现有方法相比，其优点在于：1)与传统巡检机器人重定位方法相比，该方法利用部署现场固有特征即可进行重定位，不需要安装反光板等附件，降低了部署成本。2)方法中的配准算法将离散的点云数据转换为连续的八叉树空间栅格占据查询，采用占据栅格的空间与运算作为测度函数，大大提升了在复杂车底点云下点云相似程度计算效率。3)方法中的配准算法采用平移空间搜索枚举整个平移空间，大大降低了在重复的检修地沟特征下算法陷入局部最优的失效概率。4)方法中的配准算法只需要指定搜索间隔与搜索起止点即可得到精确的配准结果，不需要为算法提供良好的迭代初值，大大提升了算法在多种重定位尺度下的鲁棒性。

附图说明

图1为巡检机器人在检修地沟中的工作图示；

图2为该方法在运行阶段的重定位完整流程框图。

具体实施方式

下面结合附图和具体实施例对本发明做进一步详细说明。

一种在地铁检修地沟内运行的轮式巡检机器人，结合图1所示，机器人在检修地沟内直线前后运行，安装有：3D激光雷达、电机轮速计等，基于ROS机器人操作系统，使用改进的LOAM雷达里程计，机器人在巡检地沟内自主定位导航，使用重定位修正里程计累积误差、检测车辆停车位置，步骤如下：

里程计累积误差修正重定位部署：

步骤1：选取重定位点，生成重定位点编号，并基于外部准确测量(如卷尺、激光测距仪等)，将机器人移动到巡检地沟中对应位置上，此处选择卷尺测量距离原点2.5m处作为重定位点。

步骤2：根据现实情况确定点云累积时间、点云所需空间区域与下采样分辨率，根据编号生成采集配置；使用3D激光雷达累积点云数据，根据编号生成模板点云数据，同时将外部准确测量距离结果根据编号生成模板位姿，以重定位点编号为主键，插入到SQL数据库中。此处累积时间设置为5s；空间区域设置为以雷达原点为基准，前后0m到5m，左右-0.8m到0.8m，上下-0.8m到0.4m，上下范围的取值使得仅有检修沟内固定的方柱特征存在于点云数据中，除去了车辆车底特征，以排除停车位置差异对配准结果的影响；下采样分辨率设置为0.01m。

步骤3：在机器人的业务流程中加入重定位部分，并给定重定位点编号，使得机器人可以自行从SQL数据库中读取采集配置、模板点云数据与模板位姿，并根据实际状况运行定位修正。此处在机器人完成巡检任务，依据里程计定位数据认为已到达存储的模板位姿处停车后加入重定位过程。

里程计累积误差修正重定位执行：

结合图2所示，具体包括以下步骤：

步骤1：机器人根据业务流程，依据重定位点编号，从SQL数据库中取出重定位点模板位姿，并以里程计定位信息作为参考，行驶到里程计认为已到达模板位姿的位置上停车，准备开始采集当前点云。

步骤2：机器人依据重定位点编号，从SQL数据库中取出采集配置，并调用3D激光雷达，根据采集配置中的采集时间累积当前点云数据，并根据采集配置中的所需空间区域对点云数据进行直通滤波，使其仅保留所需的部分。此处累积时间为5s，且点云数据仅保留检修沟内固定的方柱特征。

步骤3：从SQL数据库中取出模板点云数据，对当前点云与模板点云进行下采样与统计学滤波，并使用单轴配准算法计算当前点云与模板点云之间在x轴方向的平移变换关系。

步骤3.1：生成x方向上的平移搜索空间，平移搜索空间可表示为：

式中，d_x为搜索间隔，为x轴负方向上的最大搜索范围，即代数值最小的搜索距离，/>为x轴正方向上的最大搜索范围，即代数值最大的搜索距离，n为搜索空间的大小，可表示为：

从而搜索空间中第i项对应的x轴上的平移可表示为：

步骤3.2：枚举平移搜索空间T_x，将当前点云P_c根据t_i变换为搜索点云P_t，点云P_t中的每一个点可表示为：

其中，为当前点云P_c中的每一个点。

步骤3.3：将搜索点云P_t与模板点云P_m以一定分辨率转换为八叉树空间占据栅格M_t与M_m，从而可以查询M_m中标记为占据的栅格，其对应栅格在M_t中是否为占据状态，而这一查询是在栅格编号层面连续的。此处转换分辨率取0.001m。

步骤3.4：定义测度函数E(P_t,P_m)，用于评估搜索点云与模板点云的相似程度，考虑到点云均已被转换为八叉树空间占据栅格，因此测度函数可表示为：

而配准问题则转化为了求函数最大值的问题，可表示为：

使用平移空间搜索的方式，即可计算出当测度函数E(P_t,P_m)达到最大值时的平移变换t_r，t_r即为输出的配准结果。

步骤4：将当前点云与模板点云的平移变换关系叠加到模板位姿上，使用该新位姿重置机器人里程计，并向机器人发送导航至x＝0m的指令，机器人精确回到原点。

本实施例中，取d_x＝0.005m、可达到精度与效率的平衡。为测试重定位配准算法精度，以5s的累积时间，在距离重定位点0.2m、0.4m、0.6m、0.8m以及1.0m处各按照相同的采集配置采集点云数据，以模拟机器人里程计产生累积误差时导致的导航到点误差，并对五组数据进行测试，结果表1所示，在较短的处理时间内，可以达到亚厘米级的重定位性能。

表1实施例1测试结果

测试组	配准结果tr(m)	处理时间(s)	误差(％)
				组1(0.2m)	0.200m	2.081s	0.000％
组2(0.4m)	0.400m	1.508s	0.000％
				组3(0.6m)	0.600m	1.596s	0.000％
组4(0.8m)	0.805m	2.092s	0.625％
				组5(1.0m)	0.995m	2.135s	0.500％

车辆停车位置检测重定位部署：

步骤1：选取重定位点，生成重定位点编号，并基于外部准确测量(如卷尺、激光测距仪等)，将机器人移动到巡检地沟中对应位置上，此处选择卷尺测量距离部署时停放的地铁列车每一节车厢第一个转向架中心点负方向2.5m处在检修地沟坐标系中对应的点作为重定位点；

步骤2：根据现实情况确定点云累积时间、点云所需空间区域与下采样分辨率，根据编号生成采集配置；使用3D激光雷达累积点云数据，根据编号生成模板点云数据，同时将外部准确测量距离结果根据编号生成模板位姿，以重定位点编号为主键，插入到SQL数据库中。此处累积时间设置为5s；空间区域设置为以雷达原点为基准，前后0m到5m，左右-0.8m到0.8m，上下0.4m到0.95m，上下范围的取值使得仅有车辆车底特征(此处为转向架)存在于点云数据中，除去了检修地沟中固定的方柱特征，以排除固定特征对动态特征配准结果的影响；下采样分辨率设置为0.01m；

步骤3：在机器人的业务流程中加入重定位部分，并给定重定位点编号，使得机器人可以自行从SQL数据库中读取采集配置、模板点云数据与模板位姿，并根据实际状况运行定位修正。此处在机器人到达每一节车厢准备开始巡检前，依据里程计定位数据认为已到达存储的模板位姿处停车后加入重定位过程。

车辆停车位置检测重定位执行：

结合图2所示，具体包括以下步骤：

步骤1：机器人根据业务流程，依据重定位点编号，从SQL数据库中取出重定位点模板位姿，并以里程计定位信息作为参考，行驶到里程计认为已到达模板位姿的位置上停车，准备开始采集当前点云；

步骤2：机器人依据重定位点编号，从SQL数据库中取出采集配置，并调用3D激光雷达，根据采集配置中的采集时间累积当前点云数据，并根据采集配置中的所需空间区域对点云数据进行直通滤波，使其仅保留所需的部分。此处累积时间为5s，且点云数据仅保留列车车底转向架特征；

步骤3：从SQL数据库中取出模板点云数据，并使用配准算法计算当前点云与模板点云之间在x轴方向的平移变换关系；

步骤3.1：生成x方向上的平移搜索空间，平移搜索空间可表示为：

式中，d_x为搜索间隔，为x轴负方向上的最大搜索范围，即代数值最小的搜索距离，/>为x轴正方向上的最大搜索范围，即代数值最大的搜索距离，n为搜索空间的大小，可表示为：

从而搜索空间中第i项对应的x轴上的平移可表示为：

步骤3.2：枚举平移搜索空间T_x，将当前点云P_c根据t_i变换为搜索点云P_t，点云P_t中的每一个点可表示为：

其中，为当前点云P_c中的每一个点。

步骤3.3：将搜索点云P_t与模板点云P_m以一定分辨率转换为八叉树空间占据栅格M_t与M_m，从而可以查询M_m中标记为占据的栅格，其对应栅格在M_t中是否为占据状态，而这一查询是在栅格编号层面连续的。此处转换分辨率取0.001m。

步骤3.4：定义测度函数E(P_t,P_m)，用于评估搜索点云与模板点云的相似程度，考虑到点云均已被转换为八叉树空间占据栅格，因此测度函数可表示为：

而配准问题则转化为了求函数最大值的问题，可表示为：

使用平移空间搜索的方式，即可计算出当测度函数E(P_t,P_m)达到最大值时的平移变换t_r，t_r即为输出的配准结果。

步骤4：使用当前点云与模板点云的平移变换关系重置列车车底坐标系与检修地沟坐标系之间的变换关系，将以列车车底坐标系为基准的巡检导航点位姿转换到检修地沟坐标系中来，驱动机器人到达巡检导航点，对车底零部件进行巡检。

本实施例中，取d_x＝0.005m、可达到精度与效率的平衡。为测试重定位配准算法精度，以5s的累积时间，在距离重定位点0.2m、0.4m、0.6m、0.8m以及1.0m处各按照相同的采集配置采集点云数据，以模拟列车停车位置的差异，并对五组数据进行测试，结果表2所示。

表2实施例2测试结果

测试组	配准结果tr(m)	处理时间(s)	误差(％)
				组1(0.2m)	0.200m	3.605s	0.000％
组2(0.4m)	0.400m	4.770s	0.000％
				组3(0.6m)	0.600m	4.525s	0.000％
组4(0.8m)	0.800m	3.978s	0.000％
				组5(1.0m)	0.995m	4.312s	0.500％

可以看出本实施例在较短的处理时间内，可以达到亚厘米级的重定位性能。

Claims (1)

1.一种基于占据栅格配准的车底巡检机器人单轴重定位方法，其特征在于，包括机器人部署期间模板采集和运行期间基于模板重定位两部分；

机器人部署期间模板采集具体步骤为：

步骤A：选取重定位点，生成重定位点编号，并基于外部准确测量将机器人移动到巡检地沟中对应位置上；

步骤B：根据现实情况确定点云累积时间、点云所需空间区域与点云下采样分辨率，根据编号生成采集配置；使用3D激光雷达累积点云数据，根据编号生成模板点云数据，同时将外部准确测量距离结果根据编号生成模板位姿，以重定位点编号为主键，插入到SQL数据库中；

步骤C：在机器人的业务流程中加入重定位部分，并给定重定位点编号，使得机器人自行从SQL数据库中读取采集配置、模板点云数据与模板位姿，并根据实际状况运行定位修正；

机器人运行期间基于模板重定位具体步骤为：

步骤1：机器人根据业务流程，依据重定位点编号，从SQL数据库中取出重定位点模板位姿，并以里程计定位信息作为参考，行驶到里程计认为已到达模板位姿的位置上停车，准备开始采集当前点云；

步骤2：机器人依据重定位点编号，从SQL数据库中取出采集配置，并调用3D激光雷达，根据采集配置中的采集时间累积当前点云数据，并根据采集配置中的所需空间区域对点云数据进行直通滤波，使其仅保留所需的部分；

步骤3：考虑到机器人在巡检地沟内仅具有前后行驶的单自由度，将机器人运行方向定义为x轴正方向，从SQL数据库中取出模板点云数据，对当前点云与模板点云进行下采样与统计学滤波，并使用单轴配准算法计算当前点云与模板点云之间在x轴方向的平移变换关系；

单轴配准算法步骤具体为：

步骤3.1：生成x方向上的平移搜索空间，平移搜索空间表示为：

式中，d_x为搜索间隔，为x轴负方向上的最大搜索范围，即代数值最小的搜索距离，为x轴正方向上的最大搜索范围，即代数值最大的搜索距离，n为搜索空间的大小，表示为：

从而搜索空间中第i项对应的x轴上的平移表示为：

步骤3.2：枚举平移搜索空间T_x，将当前点云P_c根据t_i变换为搜索点云P_t，点云P_t中的每一个点表示为：

其中，为当前点云P_c中的每一个点；

步骤3.3：将搜索点云P_t与模板点云P_m以一定分辨率转换为八叉树空间占据栅格M_t与M_m，从而可以查询M_m中标记为占据的栅格，其对应栅格在M_t中是否为占据状态，而这一查询是在栅格编号层面连续的；

步骤3.4：定义测度函数E(P_t,P_m)，用于评估搜索点云与模板点云的相似程度，考虑到点云均已被转换为八叉树空间占据栅格，因此测度函数表示为：

而配准问题则转化为了求函数最大值的问题，表示为：

使用平移空间搜索的方式，即可计算出当测度函数E(P_t,P_m)达到最大值时的平移变换t_r，t_r即为输出的配准结果；

步骤4：将当前点云与模板点云的平移变换关系叠加到模板位姿上，使用该新位姿重置机器人里程计。