一种轮式机器人定位方法及系统技术方案

本发明专利技术涉及一种轮式机器人定位方法及系统，所述方法包括：获取栅格地图，接收激光点云数据、里程计数据；全局重定位获取初始位姿；位姿跟踪，包括：里程计预测得第一定位位姿；对第一定位位姿的定位质量进行评估，并记录平移评价值和旋转评价值；进行异常环境检测，根据检测结果对位姿跟踪目标函数的权重参数进行优化调节；局部缩放窗口暴力匹配：根据平移评价值和旋转评价值设置第一定位位姿的缩放窗口，确定第二定位位姿，并对位姿跟踪目标函数的权重参数进行优化调节；以第二定位位姿为初始解求解位姿跟踪目标函数，获得最优定位位姿。本发明专利技术提高了定位精度和定位的稳定性。发明专利技术提高了定位精度和定位的稳定性。发明专利技术提高了定位精度和定位的稳定性。

【技术实现步骤摘要】
一种轮式机器人定位方法及系统


[0001]本专利技术涉及机器人定位
，尤其是一种轮式机器人定位方法及系统。

技术介绍

[0002]位姿是描述某个对象在指定坐标系下的位置和姿态。机器人常用位姿来描述其在空间坐标系中的位置与姿态，随着机器人技术越来越成熟，各行各业的应用场景也越来越丰富，通过识别周围环境，进而计算出机器人在当前场景的准确位姿，才能完成移动作业。因此如何获得机器人当前实时位姿成为关键。实现机器人在已有栅格地图上的准确定位，目前常用的技术包括GPS法、在室内应用场景中添加辅助设备定位法、以及基于概率的定位方法。通过GPS可实时给机器人定位，但是在室内场景下GPS无法完全覆盖到，定位会失效。在室内应用场景中安装反光板、磁钉或者二维码辅助计算机器人位姿，该方法需要添加辅助设备，需要对原场景进行改造，并且当场景较大时，施工不便，不适用于通用场景。
[0003]基于概率的定位方法成为目前服务机器人定位的研究热点，它主要将机器人定位问题转化为贝叶斯估计问题，首先给定初始位姿，通过运动模型进行预测，得到下一时刻大致的机器人位姿，再通过传感器的观测模型对预测的位姿进行校正得到比较准确地机器人的位姿。经典的基于贝叶斯估计的定位方法有高斯滤波、直方图滤波、粒子滤波等，其中粒子滤波也称为蒙特卡罗定位，传统的蒙特卡罗定位可以解决机器人定位问题，但存在计算量大、粒子耗散(退化)，以及无先验(初始)位姿不移动时无法全局重定位、机器人遇到典型环境激光失效时的存在定位丢失等问题，导致计算效果较差。

技术实现思路
/>[0004]针对现有技术的不足，本专利技术提供一种轮式机器人定位方法及系统，能够提高定位精度和稳定性。
[0005]本专利技术采用的技术方案如下：
[0006]本申请提供一种轮式机器人定位方法，包括：
[0007]获取栅格地图，接收激光点云数据、里程计数据，并对激光点云数据进行栅格滤波；
[0008]全局重定位，判断是否存在初始位姿，若不存在则通过激光点云匹配栅格地图输出当前机器人的初始位姿；
[0009]位姿跟踪，包括：
[0010]里程计预测：基于所述初始位姿通过里程计增量预测第一定位位姿；
[0011]对所述第一定位位姿的定位质量进行评估，并记录平移评价值translation
delta
和旋转评价值rotation
delta
；
[0012]进行异常环境检测，根据检测结果对位姿跟踪目标函数的权重参数进行优化调节，所述位姿跟踪目标函数的权重参数包括激光匹配权重w1、平移权重w2和旋转权重w3，分别代表激光点云在全局栅格地图中的位姿匹配、第二定位位姿与最优定位位姿之间的位
移、旋转角度对最优定位位姿计算的影响程度；
[0013]局部缩放窗口暴力匹配，根据所述平移评价值translation
delta
和旋转评价值rotation
delta
设置第一定位位姿的缩放窗口，枚举窗口内所有可能出现的位姿，对各位姿的激光点云与栅格地图进行匹配度评估，根据评估情况确定所述第二定位位姿，并对位姿跟踪目标函数的权重参数进行优化调节；
[0014]以所述第二定位位姿为初始解求解位姿跟踪目标函数，获得最优定位位姿；
[0015]将所述最优定位位姿作为下一次里程计预测的初始位姿进行发布。
[0016]进一步技术方案为：
[0017]所述对第一定位位姿的定位质量进行评估，包括：
[0018]将第一定位位姿转化至原始全局栅格地图坐标系下，选取比原始全局栅格地图粗一层分辨率的地图作为评估地图，计算当前激光点云落在所述评估地图上的总概率作为匹配度，根据匹配度判断第一定位位姿是否准确。
[0019]根据匹配度判断第一定位位姿是否准确，包括：
[0020]若匹配度大于第一阈值min_score1，将平移评价值translation
delta
和旋转评价值rotation
delta
归零，则判断第一定位位姿准确；
[0021]若匹配度小于第一阈值min_score1，对平移评价值translation
delta
和旋转评价值rotation
delta
进行累计计数，并判断是否小于各自的设定阈值，若平移评价值translation
delta
和旋转评价值rotation
delta
均小于各自的设定阈值，则判断第一定位位姿准确，若平移评价值translation
delta
和旋转评价值rotation
delta
中至少一个超出其设定阈值，则判断第一定位位姿错误，需再次进行全局重定位和里程计预测，直到获得准确的第一定位位姿。
[0022]所述异常环境检测包括长走廊检测：将激光点云中的点分成N组，对每组的点按下式计算：
[0023][0024]式中，point
xi
、point
yi
分别为点云中第i个点的横坐标和纵坐标，m为每组中点的个数，A为经验值，为常数；
[0025]N组点云中满足上式的组数为n，若n/N大于等于设定的百分比，视为检测到长走廊环境；
[0026]针对长走廊环境，将平移权重w2提高，将激光匹配权重w1、旋转权重w3降低，使w2>w1>w3。
[0027]所述异常环境检测包括激光异常遮挡检测：
[0028]判断激光点云不足设定值，或者小于机器人自身半径的点云测距数量所占比例超过设定值时，视为检测到激光异常遮挡环境；
[0029]针对激光异常遮挡环境，将激光匹配权重w1设为零，将平移权重w2、旋转权重w3降低。
[0030]根据所述平移评价值translation
delta
和旋转评价值rotation
delta
设置第一定位位姿的缩放窗口，包括：
[0031]根据处理器性能和里程计的精度设定搜索平移步长和旋转步长以及初始搜索窗口边长length
init
，初始搜索窗口角度theta
init
；
[0032]按照下式设置优化的搜索窗口边长length
final
和优化的搜索窗口角度theta
final
：
[0033]length
final
＝length
init
+translation
delta
*a
[0034]theta
final
＝theta
init
+rotation
delta
*b
[0035]式中，a、b分别为经验常数。
[0036]所述对各位姿的激光点云与栅格地图进行匹配度评估，根据评估情况确定第二定位位姿，并对本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种轮式机器人定位方法，其特征在于，包括：获取栅格地图，接收激光点云数据、里程计数据，并对激光点云数据进行栅格滤波；全局重定位，判断是否存在初始位姿，若不存在则通过激光点云匹配栅格地图输出当前机器人的初始位姿；位姿跟踪，包括：里程计预测：基于所述初始位姿通过里程计增量预测第一定位位姿；对所述第一定位位姿的定位质量进行评估，并记录平移评价值translation
delta
和旋转评价值rotation
delta
；进行异常环境检测，根据检测结果对位姿跟踪目标函数的权重参数进行优化调节，所述位姿跟踪目标函数的权重参数包括激光匹配权重w1、平移权重w2和旋转权重w3，分别代表激光点云在全局栅格地图中的位姿匹配、第二定位位姿与最优定位位姿之间的位移、旋转角度对最优定位位姿计算的影响程度；局部缩放窗口暴力匹配，根据所述平移评价值translation
delta
和所述旋转评价值rotation
delta
设置第一定位位姿的缩放窗口，枚举窗口内所有可能出现的位姿，对各位姿的激光点云与栅格地图进行匹配度评估，根据评估情况确定所述第二定位位姿，并对位姿跟踪目标函数的权重参数进行优化调节；以所述第二定位位姿为初始解求解位姿跟踪目标函数，获得最优定位位姿；将所述最优定位位姿作为下一次里程计预测的初始位姿进行发布。2.根据权利要求1所述的轮式机器人定位方法，其特征在于，所述对第一定位位姿的定位质量进行评估，包括：将第一定位位姿转化至原始全局栅格地图坐标系下，选取比原始全局栅格地图粗一层分辨率的地图作为评估地图，计算当前激光点云落在所述评估地图上的总概率作为匹配度，根据匹配度判断第一定位位姿是否准确。3.根据权利要求2所述的轮式机器人定位方法，其特征在于，根据匹配度判断第一定位位姿是否准确，包括：若匹配度大于第一阈值min_score1，将平移评价值translation
delta
和旋转评价值rotation
delta
归零，则判断第一定位位姿准确；若匹配度小于第一阈值min_score1，对平移评价值translation
delta
和旋转评价值rotation
delta
进行累计计数，并判断是否小于各自的设定阈值，若平移评价值translation
delta
和旋转评价值rotation
delta
均小于各自的设定阈值，则判断第一定位位姿准确，若平移评价值translation
delta
和旋转评价值rotation
delta
中至少一个超出其设定阈值，则判断第一定位位姿错误，需再次进行全局重定位和里程计预测，直到获得准确的第一定位位姿。4.根据权利要求1所述的轮式机器人定位方法，其特征在于，所述异常环境检测包括长走廊检测：将激光点云中的点分成N组，对每组的点按下式计算：式中，point
xi
、point
yi
分别为点云中第i个点的横、纵坐标，m为每组中点的个数，A为经
验值，为常数；N组点云中满足上式的组数为n，若n/N大于等于设定的百分比，视为检测到长走廊环境；针对长走廊环境，将平移权重w2提高，将激光匹配权重w1、旋转权重w3降低，使w2>w1>w3。5.据权利要求1所述的轮式机器人定位方法，其特征在于，所述异常环境检测包括激光异常遮挡检测：判断激光点云不足设定值，或者小于机器人自身半径的点云测距数量所占比例超过设定值时，视为检测到激光异常遮挡环境；针对激光异常遮挡环境，将激光匹配权重w1设为零，将平移权重w2、旋转权重w3降低。6.根据权利要求1所述的轮式机器人定位方法，其特征在于，根据所述平移评价值translation
delta
和所述旋转评价值rotation
delta
设置第一定位位姿的缩放窗口，包括：根据处理器性能和里程计的精度设定搜索平移步长和旋转步长以及初始搜索窗口边长length
init
，初始搜索窗口角度theta
init
；按照下式设置优化的搜索窗口边长length
final
和优化的搜索窗口角度theta
final
：length
final
＝length
init
+translation
delta
*atheta
final
＝theta
init
+rotation
delta
*b式中，a、b分别为经验常数。7.根据权利要求1所述的轮式机器人定位方法，其特征在于，所述对各位姿的激光点云与栅格地图进行匹配度评估，根据评估情况确定第二定...

【专利技术属性】
技术研发人员：王运志，
申请(专利权)人：中科南京软件技术研究院，
类型：发明
国别省市：