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

权利要求书

1.一种机器人定位方法，其特征在于，所述机器人搭载有激光雷达，所述方法包括：

对机器人的位置进行一次定位以得到第一位姿；

将所述第一位姿作为所述机器人的初始位姿，利用所述初始位姿将当前激光点云在栅格地图中的坐标转换到机器人坐标系下的坐标以得到所述激光点云的转换坐标；

基于所述初始位姿和所述转换坐标对相应激光点云的得分优化函数进行梯度寻优以求解所述激光雷达的扫描数据与所述栅格地图的匹配位姿，所述匹配位姿作为所述机器人的第二位姿；其中，所述得分优化函数根据激光点云的转换坐标及激光点云在栅格地图中所在栅格的高斯分布信息构建；

在所述第二位姿满足预设匹配条件的情况下，将所述第二位姿作为所述机器人当前的最终位姿，在不满足所述预设匹配条件的情况下，将所述第一位姿作为所述机器人当前的最终位姿。

2.根据权利要求1所述的机器人定位方法，其特征在于，所述第二位姿满足预设匹配条件，包括：

所述第二位姿下所述激光点云的得分大于或等于置信度阈值，并且所述第二位姿和所述第一位姿之间的差值小于或等于预设误差阈值。

3.根据权利要求1或2所述的机器人定位方法，其特征在于，所述基于所述初始位姿和所述转换坐标对相应激光点云的得分优化函数进行梯度寻优以求解所述激光雷达扫描的数据与所述栅格地图的匹配位姿，包括：

基于所述转换坐标对所述激光点云的得分优化函数利用高斯牛顿迭代算法求解出对应的梯度和黑森矩阵；

将所有激光点云的所述梯度和所述黑森矩阵叠加以计算出当前的位姿增量；

将所述初始位姿与所述当前的位姿增量相加得到第二位姿，其中，在所述第二位姿下激光雷达的扫描数据与所述栅格地图的匹配程度最高。

4.根据权利要求1所述的机器人定位方法，其特征在于，求解所述机器人的第二位姿之前，所述方法还包括：

对所述机器人预先加载的地图进行栅格化，得到由若干个栅格构成的栅格地图，以及根据所述栅格地图中障碍物的位置计算出每个栅格的高斯分布的均值和方差。

5.根据权利要求1所述的机器人定位方法，其特征在于，所述对机器人的位置进行一次定位以得到第一位姿，包括：

在所述机器人的初始位置周围随机生成相应数量的粒子，每个粒子具有各自的初始位姿及相同的初始权重；

根据移动指令使所述机器人在所述栅格地图中移动，并在每次移动后更新各个粒子的位姿，得到各个粒子的更新位姿；

根据所述粒子的更新位姿和所述机器人通过搭载的激光雷达测量得到的观测数据对对应粒子进行权重更新，得到各个粒子的更新权重；

按照所有粒子的所述更新权重的分布进行粒子重采样以得到重采样的所述数量的粒子，返回上述的移动步骤进行预设次数的迭代；

根据最终迭代得到的各个粒子的更新位姿和更新权重计算出所述机器人的第一位姿。

6.根据权利要求1所述的机器人定位方法，其特征在于，所述坐标转换的转换公式为：

其中，(x_i，y_i)表示第i个激光点云在栅格地图中的坐标；X′_i表示第i个激光点云在转换后的转换坐标；(T_x，T_y)表示所述机器人的位置，T_θ表示所述机器人的姿态角。

7.根据权利要求6所述的机器人定位方法，其特征在于，所述第i个激光点云的得分优化函数的表达式为：

其中，q_i和∑_i分别表示第i个激光点云所在栅格的高斯分布的均值和方差。