[发明专利]三维激光定位方法及系统

说明书

本发明涉及一种三维激光定位方法及系统,包括根据编码器获取的数据预测机器人某一时刻相对上一时刻的预测位姿；在所述预测位姿附近布置多个粒子，对每个粒子进行观测，将观测到的环境信息数据与体素距离地图作对比，根据对比的相似度计算每个粒子的系数；根据系数滤除掉系数不优的粒子，多次循环滤除直至找出系数最优的粒子；对系数最优的粒子对应的位姿进行平滑滤波处理，输出所述机器人的最终位姿。本发明既可以提升查询速度，还可以提高定位精度。

技术领域

本发明涉及机器人定位的技术领域，尤其是指一种三维激光定位方法及系统。

背景技术

定位是移动机器人实现自主能力的最基本环节，是完成导航、路径规划等后续任务的前提，其目的就是确定移动机器人在工作环境中相对于世界坐标系的位姿(包括位置和姿态)。目前定位方法主要分为如下三类：相对定位技术、绝对定位技术、组合定位技术。

在定位过程中，由于感知传感器本身存在着性能限制，运行环境中各种不确定因素干扰，移动机器人的定位研究开始依靠基于概率的模型对不确切的运动和感知信息进行建模，实现机器人的自定位。目前常用的概率定位方法包括扩展卡尔曼滤波、马尔科夫定位、多假设跟踪以及蒙特卡罗定位。而若应用在变电站环境中，由于场景范围大，且特征较为稀疏，对定位算法的时效性和精度都带来了很大的挑战，具体地体现为：观测噪声大、定位时效性、环境特征稀少，很难达到较高的定位精度。

为了克服上述问题，现有采用粒子滤波定位算法，所述粒子滤波定位算法是应用粒子集表示定位后验概率分布的蒙特卡洛方法，因此又被称为蒙特卡洛定位算法(MCL)。在移动机器人领域，粒子滤波定位就是利用机器人的输入数据，传感器的观测数据，结合运动学模型和观测模型来迭代，估计当前机器人位姿信度的最优化问题，即Bayesian滤波器问题的变体，主要思想就是用一个随机采样获得的具有权重的样本集合表示并估计后验概率密度。现有的粒子滤波定位算法框架中，在计算每个粒子权重时，需要对当前帧激光点查询已知地图中的最近邻，但是过程是整个算法中最耗时的过程，不但速度低，而且定位精度差。

发明内容

为此，本发明所要解决的技术问题在于克服现有技术中效率低，且定位精度差的问题，从而提供一种效率高，且有效提高定位精度的三维激光定位方法及系统。

为解决上述技术问题，本发明的一种三维激光定位方法，包括如下步骤：根据编码器获取的数据预测机器人某一时刻相对上一时刻的预测位姿；在所述预测位姿附近布置多个粒子，对每个粒子进行观测，将观测到的环境信息数据与体素距离地图作对比，根据对比的相似度计算每个粒子的系数；根据系数滤除掉系数不优的粒子，多次循环滤除直至找出系数最优的粒子；对系数最优的粒子对应的位姿进行平滑滤波处理，输出所述机器人的最终位姿。

在本发明的一个实施例中，所述粒子的系数与所述机器人运动的速度有关，且是变量。

在本发明的一个实施例中，所述粒子的系数包括权重或方差或所述权重和方差的组合中的任意一种。

在本发明的一个实施例中，所述粒子的系数为权重时，多次循环滤除掉权重小的粒子。

在本发明的一个实施例中，所述粒子的权重，是由查表法获取激光点落在对应体素栅格的距离信息得出。

在本发明的一个实施例中，所述粒子的系数为方差时，多次循环滤除掉方差大的粒子。

在本发明的一个实施例中，所述粒子的系数为权重和方差的组合时，多次循环滤除掉权重小且方差大的粒子。

在本发明的一个实施例中，所述平滑滤波处理的方法为：所述机器人低速运行时，置信所述编码器输出的预测位姿，当所述机器人高速行驶时，置信所述系数最优的粒子对应的位姿。