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

权利要求书

1.一种三维激光定位方法，其特征在于，包括如下步骤：

步骤S1：根据编码器获取的数据预测机器人某一时刻相对上一时刻的预测位姿；

步骤S2：在所述预测位姿附近布置多个粒子，对每个粒子进行观测，将观测到的环境信息数据与体素距离地图作对比，根据对比的相似度计算每个粒子的系数；

步骤S3：根据系数滤除掉系数不优的粒子，多次循环滤除直至找出系数最优的粒子；

步骤S4：对系数最优的粒子对应的位姿进行平滑滤波处理，输出所述机器人的最终位姿。

2.根据权利要求1所述的三维激光定位方法，其特征在于：所述粒子的系数与所述机器人运动的速度有关，且是变量。

3.根据权利要求1或2所述的三维激光定位方法，其特征在于：所述粒子的系数包括权重或方差或所述权重和方差的组合中的任意一种。

4.根据权利要求3所述的三维激光定位方法，其特征在于：所述粒子的系数为权重时，多次循环滤除掉权重小的粒子。

5.根据权利要求4所述的三维激光定位方法，其特征在于：所述粒子的权重，是由查表法获取激光点落在对应体素栅格的距离信息得出。

6.根据权利要求3所述的三维激光定位方法，其特征在于：所述粒子的系数为方差时，多次循环滤除掉方差大的粒子。

7.根据权利要求3所述的三维激光定位方法，其特征在于：所述粒子的系数为权重和方差的组合时，多次循环滤除掉权重小且方差大的粒子。

8.根据权利要求1所述的三维激光定位方法，其特征在于：所述平滑滤波处理的方法为：所述机器人低速运行时，置信所述编码器输出的预测位姿，当所述机器人高速行驶时，置信所述系数最优的粒子对应的位姿。

9.根据权利要求8所述的三维激光定位方法，其特征在于：所述输出机器人的最终位姿时，当置信所述编码器输出的预测位姿时，所述预测位姿的权重大于所述系数最优的粒子对应位姿的权重；当置信所述系数最优的粒子对应的位姿时，所述预测位姿的权重小于所述系数最优的粒子对应位姿的权重。

10.一种三维激光定位系统，其特征在于：包括：

预测位姿模块，用于根据编码器获取的数据预测机器人某一时刻相对上一时刻的预测位姿；

测量模块，用于在所述预测位姿附近布置多个粒子，对每个粒子进行观测，将观测到的环境信息数据与体素距离地图作对比，根据对比的相似度计算每个粒子的系数；

重采样模块，用于根据系数滤除掉系数不优的粒子，多次循环滤除直至找出系数最优的粒子；

处理模块，用于对系数最优的粒子对应的位姿进行平滑滤波处理，输出所述机器人的最终位姿。