[发明专利]一种移动机器人定位方法及系统

权利要求书

1.一种移动机器人定位方法，其特征在于，包括以下步骤：

步骤S1、获取机器人移动时生成的二维占用单元格地图，将所述二维占用单元格地图转换为三维单元格地图，获取所述三维单元格地图中每个单元格的能量值，以粒子表示机器人的位姿，初始化粒子对应的位姿，确定粒子集X_t；

步骤S2、采集机器人的运动信息，根据所述运动信息采集粒子，并更新粒子的位姿；

步骤S3、获取粒子所在的单元格，将所在单元格对应的能量值赋值给粒子，得到粒子的预测能量值，获取机器人的真实能量值根据所述预测能量值和真实能量值确定粒子的权重，根据粒子的位姿和粒子的权重，获取机器人的当前位姿；

步骤S4、根据粒子的权重在粒子集X_t中进行粒子采样，得到粒子集X_L，根据每个单元格的能量进行粒子采样，得到粒子集X_G，由粒子集X_L和粒子集X_G组成新的粒子集；

步骤S5、根据新的粒子集，重复执行步骤S2-S4，得到机器人在移动中的位姿。

2.根据权利要求1所述的移动机器人定位方法，其特征在于，将所述二维占用单元格地图转换为三维单元格地图，具体包括，将方向空间等分为K＝360·N/β个区间，将x、y两个维度构成的二维占用单元格地图，转换为由x、y、θ三个维度构成的三维单元格地图，其中，K为空间中的区间个数，β为机器人上的激光雷达的测量角度，N为激光雷达的测量数据量。

3.根据权利要求2所述的移动机器人定位方法，其特征在于，获取所述三维单元格地图中每个单元格的能量值，具体包括，根据公式

获取所述三维单元格地图中每个单元格的能量值，其中，E_i为第i个单元格的能量值，θ_i为单元格i对应的θ坐标，d_n为预测机器人到达单元格对应位姿时的激光雷达测量值，d_max为激光雷达测量值中的最大值。

4.根据权利要求1所述的移动机器人定位方法，其特征在于，根据所述运动信息采集粒子，具体包括，根据均值Δμ和协方差矩阵R的多维高斯分布对粒子进行采样，其中，Δμ为机器人相对于上一时刻运动的差值，R为采集机器人运动信息时里程计误差的协方差矩阵。

5.根据权利要求1所述的移动机器人定位方法，其特征在于，所述更新粒子的位姿，具体包括，根据公式

更新粒子的位姿，其中，为第n个粒子的位姿，p(·)表示概率分布，x_t表示本次位姿矢量，x_t-1上一次的位姿矢量，μ_t为机器人的控制矢量。

6.根据权利要求3所述的移动机器人定位方法，其特征在于，所述获取机器人的真实能量值具体包括，根据公式

所述获取机器人的真实能量值其中，为激光雷达对周围障碍物的产生的距离数据。

7.根据权利要求1所述的移动机器人定位方法，其特征在于，所述根据所述预测能量值和真实能量值确定粒子的权重，具体包括，计算所述预测能量值与真实能量值差的绝对值，以所述差的绝对值作为自变量，以均值为0、以方差为σ_err，获取高斯函数的函数值作为粒子的权重，其中，σ_err为常数。

8.根据权利要求1所述的移动机器人定位方法，其特征在于，根据粒子的位姿和粒子的权重，获取机器人的当前位姿，具体包括，根据公式

获取机器人的当前位姿，x_t为机器人当前位姿，为第n个粒子的位姿，为第n个粒子的权重。