[发明专利]机器人定位方法、装置、计算机可读存储介质及机器人

权利要求书

1.一种机器人定位方法，其特征在于，包括：

使用粒子滤波算法对机器人进行定位，得到第一定位位姿；

使用牛顿迭代优化算法对所述第一定位位姿进行矫正计算，得到矫正的位姿增量；判断所述位姿增量是否大于预设的阈值；

若所述位姿增量大于预设的阈值，则将所述第一定位位姿确定为所述机器人的定位结果；

若所述位姿增量小于或等于所述阈值，则根据所述第一定位位姿和所述位姿增量计算第二定位位姿，并将所述第二定位位姿确定为所述机器人的定位结果；

所述根据所述第一定位位姿和所述位姿增量计算第二定位位姿包括：

根据下式计算所述第二定位位姿：

ξ′＝ξ+Δξ

其中，ξ为所述第一定位位姿，Δξ为所述位姿增量，ξ′为所述第二定位位姿。

2.根据权利要求1所述的机器人定位方法，其特征在于，所述使用粒子滤波算法对机器人进行定位，得到第一定位位姿包括：

获取对所述机器人进行定位的粒子集合；

根据预设的运动模型对所述粒子集合中的各个粒子的状态分别进行更新，得到各个粒子的更新状态；

获取激光雷达的观测数据；

根据所述观测数据和各个粒子的更新状态分别计算各个粒子的权重；

根据各个粒子的权重进行重采样，得到更新后的粒子集合；

根据所述更新后的粒子集合对所述机器人进行定位，得到所述第一定位位姿。

3.根据权利要求2所述的机器人定位方法，其特征在于，所述根据各个粒子的权重进行重采样，得到更新后的粒子集合包括：

根据目标粒子的权重计算所述目标粒子的采样概率，所述目标粒子为所述粒子集合中的任意一个粒子，且所述目标粒子的采样概率与所述目标粒子的权重正相关；

根据所述目标粒子的采样概率对所述目标粒子进行采样，并将采样结果添加入所述更新后的粒子集合中。

4.根据权利要求2所述的机器人定位方法，其特征在于，所述使用牛顿迭代优化算法对所述第一定位位姿进行矫正计算，得到矫正的位姿增量包括：

根据下式计算所述位姿增量：

Δξ＝H^-1dTr

其中，ξ为所述第一定位位姿，i为所述观测数据中的各个激光点的序号，1≤i≤n，n为激光点的数目，S_i(ξ)为第i个激光点变换到栅格地图坐标系下的坐标，M(S_i(ξ))为S_i(ξ)被占据的概率，Δξ为所述位姿增量。

5.根据权利要求4所述的机器人定位方法，其特征在于，还包括：

根据下式计算第i个激光点变换到栅格地图坐标系下的坐标：

其中，(p_x,p_y)为所述第一定位位姿中的位置坐标，为所述第一定位位姿中的姿态角，为第i个激光点的位置坐标。

6.一种机器人定位装置，其特征在于，包括：

粒子滤波模块，用于使用粒子滤波算法对机器人进行定位，得到第一定位位姿；

牛顿迭代优化模块，用于使用牛顿迭代优化算法对所述第一定位位姿进行矫正计算，得到矫正的位姿增量；判断所述位姿增量是否大于预设的阈值；

第一定位结果确定模块，用于若所述位姿增量大于预设的阈值，则将所述第一定位位姿确定为所述机器人的定位结果；

第二定位结果确定模块，用于若所述位姿增量小于或等于所述阈值，则根据所述第一定位位姿和所述位姿增量计算第二定位位姿，并将所述第二定位位姿确定为所述机器人的定位结果；

所述根据所述第一定位位姿和所述位姿增量计算第二定位位姿包括：

根据下式计算所述第二定位位姿：

ξ′＝ξ+Δξ

其中，ξ为所述第一定位位姿，Δξ为所述位姿增量，ξ′为所述第二定位位姿。