[发明专利]一种基于变步长的机器人定位方法和系统

权利要求书

1.一种基于变步长的机器人定位方法，其特征在于，包括，

S1：基于第一最优位姿和第一搜索步长，确定多个搜索位姿；所述第一最优位姿是指最符合机器人当前位置和姿态的位姿；

S2：确定所述多个搜索位姿的得分值，并确定最优得分值和第二最优位姿；所述第二最优位姿是指搜索位姿中最符合机器人当前位置和姿态的位姿；

S3：判断所述第二最优位姿是否比所述第一最优位姿更符合机器人的当前位置和姿态；若是，则将所述第二最优位姿更新为所述第一最优位姿，并重复S1-S3；

S4：若否，则确定第二搜索步长：所述第二搜索步长小于所述第一搜索步长；

S5：判断所述第二搜索步长是否小于最小步长阈值；若否，则将所述第二搜索步长更新为所述第一搜索步长，并重复S1-S5；

S6：若是，则将所述第一最优位姿作为机器人的当前位姿；

还包括，

获取机器人在所述当前位姿下的激光点云数据；

所述确定所述多个搜索位姿的得分值，包括，

基于所述多个搜索位姿，将所述激光点云数据分别映射到代价地图上；所述代价地图通过模糊化占据栅格地图得到，所述模糊化包括：

获取占据栅格；所述占据栅格是指在所述占据栅格地图中处于占据状态的栅格；

获取待模糊栅格；所述待模糊栅格是指在所述占据栅格地图中距所述占据栅格不超过10个栅格距离且处于自由和未知状态的栅格；

基于所述待模糊栅格与所述占据栅格的距离，确定所述待模糊栅格的代价值；所述代价值与距离成反比；代价值的计算公式为：

其中，(x,y)指以占据栅格为原点，得到的待模糊栅格的坐标；

基于所述激光点云数据分别在所述代价地图上的映射范围，确定所述多个搜索位姿的得分值。

2.根据权利要求1所述的基于变步长的机器人定位方法，其特征在于，所述确定多个搜索位姿，包括，

使用参数(X,Y,H)对机器人的位姿进行描述；其中，XY表示机器人的二维坐标， H表示机器人的二维朝向；

以步长在X轴上生成三个横轴搜索位置、和；

以步长在Y轴上生成三个纵轴搜索位置、和；

以步长在朝向上生成三个搜索姿态、和；

组合所述横轴搜索位置、纵轴搜索位置和搜索姿态，得到所述多个搜索位姿。

3.根据权利要求2所述的基于变步长的机器人定位方法，其特征在于，所述确定第二搜索步长，包括，

按比例缩小所述步长、所述步长和所述步长，得到新的步长、步长和步长。

4.根据权利要求3所述的基于变步长的机器人定位方法，其特征在于，所述判断所述第二搜索步长是否小于最小步长阈值，包括，

判断所述步长、所述步长和所述步长是否均小于各自的最小步长阈值。

5.根据权利要求1所述的基于变步长的机器人定位方法，其特征在于，所述基于所述激光点云数据分别在所述代价地图上的映射范围，确定所述多个搜索位姿的得分值，包括，

将所述映射范围内所有栅格的代价值的和作为所述搜索位姿的得分值；

所述确定最优得分值和第二最优位姿，包括，

将得分值最大的搜索位姿作为所述第二最优位姿，该最大得分值为所述最优得分值。

6.根据权利要求1所述的基于变步长的机器人定位方法，其特征在于，当所述第一最优位姿为初始位姿时，直接将所述第二最优位姿更新为所述第一最优位姿。

7.根据权利要求6所述的基于变步长的机器人定位方法，其特征在于，还包括；

获取当前激光雷达数据，当接收到所述当前激光雷达数据后，执行步骤S1-S6；

当得到机器人的所述当前位姿后，获取下一帧激光雷达数据，并将所述当前位姿作为所述初始位姿。

8.一种基于变步长的机器人定位系统，其特征在于，包括搜索位姿确定模块、第二最优位姿确定模块、第一判断模块、第一最优位姿确定模块、第二搜索步长确定模块、第二判断模块、第一搜索步长确定模块和当前位姿确定模块；

所述搜索位姿确定模块用于基于第一最优位姿和第一搜索步长，确定多个搜索位姿；所述第一最优位姿是指最符合机器人当前位置和姿态的位姿；

所述第二最优位姿确定模块用于确定所述多个搜索位姿的得分值，并确定最优得分值和第二最优位姿；所述第二最优位姿是指搜索位姿中最符合机器人当前位置和姿态的位姿；

所述第一判断模块用于判断所述第二最优位姿是否比所述第一最优位姿更符合机器人的当前位置和姿态；

所述第一最优位姿确定模块用于当所述第二最优位姿比所述第一最优位姿更符合机器人的当前位置和姿态时，将所述第二最优位姿更新为所述第一最优位姿；

所述第二搜索步长确定模块用于当所述第一最优位姿比所述第二最优位姿更符合机器人的当前位置和姿态时，确定第二搜索步长：所述第二搜索步长小于所述第一搜索步长；

所述第二判断模块用于判断所述第二搜索步长是否小于最小步长阈值；

所述第一搜索步长确定模块用于当所述第二搜索步长大于所述最小步长阈值时，将所述第二搜索步长更新为所述第一搜索步长；

所述当前位姿确定模块用于当所述第二搜索步长小于所述最小步长阈值时，将所述第一最优位姿作为机器人的当前位姿；

还包括，

获取机器人在所述当前位姿下的激光点云数据；

所述确定所述多个搜索位姿的得分值，包括，

基于所述多个搜索位姿，将所述激光点云数据分别映射到代价地图上；所述代价地图通过模糊化占据栅格地图得到，所述模糊化包括：

获取占据栅格；所述占据栅格是指在所述占据栅格地图中处于占据状态的栅格；

获取待模糊栅格；所述待模糊栅格是指在所述占据栅格地图中距所述占据栅格不超过10个栅格距离且处于自由和未知状态的栅格；

基于所述待模糊栅格与所述占据栅格的距离，确定所述待模糊栅格的代价值；所述代价值与距离成反比；代价值的计算公式为：

其中，(x,y)指以占据栅格为原点，得到的待模糊栅格的坐标；

基于所述激光点云数据分别在所述代价地图上的映射范围，确定所述多个搜索位姿的得分值。