CN115290098A

CN115290098A - 一种基于变步长的机器人定位方法和系统

Info

Publication number: CN115290098A
Application number: CN202211210142.1A
Authority: CN
Inventors: 谌文亲; 陈启明; 陈鑫华; 郑鑫
Original assignee: Chengdu Puwei Technology Co ltd
Current assignee: Chengdu Puwei Technology Co ltd
Priority date: 2022-09-30
Filing date: 2022-09-30
Publication date: 2022-11-04
Anticipated expiration: 2042-09-30
Also published as: CN115290098B

Abstract

本发明提供了一种基于变步长的机器人定位方法和系统，包括，S1：基于第一最优位姿和第一搜索步长，确定多个搜索位姿；S2：确定所述多个搜索位姿的得分值，并确定最优得分值和第二最优位姿；S3：判断所述第二最优位姿是否比所述第一最优位姿更符合机器人的当前位置和姿态；若是，则将所述第二最优位姿更新为所述第一最优位姿，并重复S1‑S3；S4：若否，则确定第二搜索步长；S5：判断所述第二搜索步长是否小于最小步长阈值；若否，则将所述第二搜索步长更新为所述第一搜索步长，并重复S1‑S5；S6：若是，则将所述第一最优位姿作为机器人的当前位姿；以实现机器人的精确定位。

Description

一种基于变步长的机器人定位方法和系统

技术领域

本发明涉及机器人定位技术领域，具体而言，涉及一种基于变步长的机器人定位方法和系统。

背景技术

目前各种类型的智能移动机器人已经走进千家万户和各行各业，包括但不限于扫地机器人、送餐机器人、酒店配送/贩售机器人、移动消杀机器人、仓储机器人、工业移动AGV等。这些可自主移动的机器人的智能化都以精准可靠的机器人定位技术为基础。因此，机器人定位技术是一项基础且重要的技术。但是现有机器人的定位方式不精准，使得机器人无法完成既定的行为。

有鉴于此，本说明书提出了一种基于变步长的机器人定位方法和系统。以实现机器人的精确定位。

发明内容

本发明的目的在于提供一种基于变步长的机器人定位方法，包括，S1：基于第一最优位姿和第一搜索步长，确定多个搜索位姿；所述第一最优位姿是指最符合机器人当前位置和姿态的位姿；S2：确定所述多个搜索位姿的得分值，并确定最优得分值和第二最优位姿；所述第二最优位姿是指搜索位姿中最符合机器人当前位置和姿态的位姿；S3：判断所述第二最优位姿是否比所述第一最优位姿更符合机器人的当前位置和姿态；若是，则将所述第二最优位姿更新为所述第一最优位姿，并重复S1-S3；S4：若否，则确定第二搜索步长：所述第二搜索步长小于所述第一搜索步长；S5：判断所述第二搜索步长是否小于最小步长阈值；若否，则将所述第二搜索步长更新为所述第一搜索步长，并重复S1-S5；S6：若是，则将所述第一最优位姿作为机器人的当前位姿。

进一步的，所述确定多个搜索位姿，包括，使用参数(X,Y,H)对机器人的位姿进行描述；其中，XY表示机器人的二维坐标，H表示机器人的二维朝向；以步长

在X轴上生成三个横轴搜索位置

、

和

；以步长

在Y轴上生成三个纵轴搜索位置

、

和

；以步长

在朝向上生成三个搜索姿态

、

和

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

进一步的，所述确定第二搜索步长，包括，按比例缩小所述步长

、所述步长

和所述步长

，得到新的步长

、步长

和步长

。

进一步的，所述判断所述第二搜索步长是否小于最小步长阈值，包括，判断所述步长

、所述步长

和所述步长

是否均小于各自的最小步长阈值。

进一步的，还包括，获取机器人在所述当前位姿下的激光点云数据；所述确定所述多个搜索位姿的得分值，包括，基于所述多个搜索位姿，将所述激光点云数据分别映射到代价地图上；基于所述激光点云数据分别在所述代价地图上的映射范围，确定所述多个搜索位姿的得分值。

进一步的，所述代价地图通过模糊化占据栅格地图得到，所述模糊化包括，获取占据栅格；所述占据栅格是指在所述占据栅格地图中处于占据状态的栅格；获取待模糊栅格；所述待模糊栅格是指在所述占据栅格地图中距所述占据栅格不超过10个栅格距离且处于自由和未知状态的栅格；基于所述待模糊栅格与所述占据栅格的距离，确定所述待模糊栅格的代价值；所述代价值与距离成反比。

进一步的，所述基于所述激光点云数据分别在所述代价地图上的映射范围，确定所述多个搜索位姿的得分值，包括，将所述映射范围内所有栅格的代价值的和作为所述搜索位姿的得分值；所述确定最优得分值和第二最优位姿，包括，将得分值最大的搜索位姿作为所述第二最优位姿，该最大得分值为所述最优得分值。

进一步的，当所述第一最优位姿为初始位姿时，直接将所述第二最优位姿更新为所述第一最优位姿。

进一步的，还包括；获取当前激光雷达数据，当接收到所述当前激光雷达数据后，执行步骤S1-S6；当得到机器人的所述当前位姿后，获取下一帧激光雷达数据，并将所述当前位姿作为所述初始位姿。

本发明的目的在于提供一种基于变步长的机器人定位系统，包括搜索位姿确定模块、第二最优位姿确定模块、第一判断模块、第一最优位姿确定模块、第二搜索步长确定模块、第二判断模块、第一搜索步长确定模块和当前位姿确定模块；所述搜索位姿确定模块用于基于第一最优位姿和第一搜索步长，确定多个搜索位姿；所述第一最优位姿是指最符合机器人当前位置和姿态的位姿；所述第二最优位姿确定模块用于确定所述多个搜索位姿的得分值，并确定最优得分值和第二最优位姿；所述第二最优位姿是指搜索位姿中最符合机器人当前位置和姿态的位姿；所述第一判断模块用于判断所述第二最优位姿是否比所述第一最优位姿更符合机器人的当前位置和姿态；所述第一最优位姿确定模块用于当所述第二最优位姿比所述第一最优位姿更符合机器人的当前位置和姿态时，将所述第二最优位姿更新为所述第一最优位姿；所述第二搜索步长确定模块用于当所述第一最优位姿比所述第二最优位姿更符合机器人的当前位置和姿态时，确定第二搜索步长：所述第二搜索步长小于所述第一搜索步长；所述第二判断模块用于判断所述第二搜索步长是否小于最小步长阈值；所述第一搜索步长确定模块用于当所述第二搜索步长大于所述最小步长阈值时，将所述第二搜索步长更新为所述第一搜索步长；所述当前位姿确定模块用于当所述第二搜索步长小于所述最小步长阈值时，将所述第一最优位姿作为机器人的当前位姿。

本发明实施例的技术方案至少具有如下优点和有益效果：

本说明书中的一些实施例通过激光雷达数据和机器人占据栅格地图之间配准来实现定位，并通过变步长搜索方法，当在较大步长的迭代搜索中出现目标值不收敛的情况时，就缩小步长进行搜索，直到步长小于自行设定的最小步长阈值。使得机器人的定位不依赖于IMU、也不依赖于精准的轮速里程计，在机器人驱动轮打滑时，依然可以保持稳定精确的定位输出，且满足不同场景和定位精度需求，方法的适用性广。

本说明书中的一些实施例通过变步长搜索方法，当在较大步长的迭代搜索中出现目标值不收敛的情况时，就缩小步长进行搜索。使得避免了高斯牛顿法可能出现的算法不收敛的问题，增强了定位的稳定性。并且搜索步长缩小的次数和长度都可控，可以保证每次输出非常接近或者等于最优解的机器人位姿，提高了定位精度。

本说明书中的一些实施例使用了迭代的方式，因此通过多次迭代并且设置较小的最小步长阈值的情况下可以逼近最优解或者等于最优解，大大提高了机器人的定位精度。

附图说明

图1为本发明一些实施例提供的一种基于变步长的机器人定位方法的示例性流程图；

图2为本发明一些实施例提供的确定搜索位姿的得分值的示例性流程图；

图3为本发明一些实施例提供的确定代价地图的示例性流程图；

图4为本发明一些实施例提供的一种基于变步长的机器人定位系统的示例性模块图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。

图1为本发明一些实施例提供的一种基于变步长的机器人定位方法的示例性流程图。在一些实施例中，流程100可以由系统400执行。如图1所示，流程100包括以下步骤：

S1：基于第一最优位姿和第一搜索步长，确定多个搜索位姿。

第一最优位姿是指最符合机器人当前位置和姿态的位姿。当前位置和姿态可以是指机器人发出并接收该帧激光雷达时的位姿和姿态。第一搜索步长可以用于确定搜索范围。例如，搜索范围可以是距离第一最优位姿第一搜索步长的范围的位置和姿态。搜索位姿可以是指用于搜索机器人当前位置和姿态的位姿。

在一些实施例中，可以使用参数(X,Y,H)对机器人的位姿进行描述；其中，XY表示机器人的二维坐标， H表示机器人的二维朝向。以步长

在X轴上生成三个横轴搜索位置

、

和

；以步长

在Y轴上生成三个纵轴搜索位置

、

和

；以步长

在朝向上生成三个搜索姿态

、

和

；组合横轴搜索位置、纵轴搜索位置和搜索姿态，得到多个搜索位姿。例如，从当前最优位姿为起点（如果是第一轮迭代，则为接近于真实位置0.25米范围内的初始位姿），从X、Y、H三个维度产生27个搜索位姿。

以

为搜索步长可在X轴上产生三个位置的集合，同理

，

均可在各自维度上产生三个位置/ 朝向集合，如我们设定三个步长初值分别为

，

，

。三个组合自由组合又可以产生27个位姿，这27个位姿即表示27个搜索位姿。

S2：确定多个搜索位姿的得分值，并确定最优得分值和第二最优位姿。

得分值可以用于表示搜索位姿与机器人当前位姿的符合程度。第二最优位姿是指搜索位姿中最符合机器人当前位置和姿态的位姿。最优得分值可以是指第二最优位姿的得分值。在一些实施例中，得分值越高代表搜索位姿与机器人的当前位姿越符合，反之则相反。关于确定搜索位姿的得分值的更多内容，参见图2及其相关描述。

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

在一些实施例中，当第二最优位姿的得分值高于第一最优位姿的得分值时，可以认为第二最优位姿比第一最优位姿更符合机器人的当前位置和姿态，反之则相反。

在一些实施例中，当第一最优位姿为初始位姿时，直接将第二最优位姿更新为第一最优位姿。初始位姿可以是指机器人刚开始工作时的位姿或发出并接收到每一帧激光雷达时的位姿。例如，当机器人刚开始工作时，初始位姿可以是机器人存储的初始位姿，该初始位姿可以通过各种可行的方式确定。又例如，当机器人每发出并接收一帧雷达时，可以基于获取的雷达数据更新机器人的初始位姿，该初始位姿可以为还未经过校正的位姿，该初始位姿可以为上一帧激光雷达的当前位姿。

S4：若否，则确定第二搜索步长。

第二搜索步长小于第一搜索步长。在一些实施例中，可以按比例缩小步长

、步长

和步长

，得到新的步长

、步长

和步长

。例如，如果第二最优位姿的得分值低于第一最优位姿的得分值，则表示如果继续以第一搜索步长进行搜索，得分值已经不能收敛，此时则需要减小三个维度上的步长，以得到更符合机器人当前位姿的数据。使得，

，

，

，即，将三个维度上的步长均减小到当前步长的 0.6倍。

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

最小步长阈值可以是指第一搜索步长所能减小到的步长。在一些实施例中，最小步长阈值可以根据经验或实际需求设置。在一些实施例中，当步长

、步长

和步长

均小于各自的最小步长阈值时，可以认为第二搜索步长小于最小步长阈值。例如，各自的最小步长阈值均可以根据对定位精度的需求进行设定。如果多个减小后的步长中任意一个大于其对应的最小步长阈值时，则返回步骤S1。

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

如果多个减小后的步长都已经小于各自最小步长阈值，说明该第一最优位姿已经达我们要求的定位精度，则输出该第一最优位姿为这一帧激光雷达数据的当前位姿。

在一些实施例中，还包括获取当前激光雷达数据，当接收到当前激光雷达数据后，执行步骤S1-S6；当得到机器人的当前位姿后，获取下一帧激光雷达数据，并将当前位姿作为初始位姿，以实现根据实时输入的激光雷达数据进行跟踪定位。其中，激光雷达测量的数据包括障碍物距离雷达的距离和角度等。在一些实施例中，激光雷达数据可以为单线激光雷达数据。

图2为本发明一些实施例提供的确定搜索位姿的得分值的示例性流程图。如图2所示，流程200包括以下步骤：

步骤210，获取机器人在当前位姿下的激光点云数据。

在一些实施例中，激光点云数据可以通过分析处理获取的激光雷达数据得到。激光点云数据中至少可以包括机器人距离障碍物的距离。

步骤220，基于多个搜索位姿，将激光点云数据分别映射到代价地图上。

代价地图可以用于表示占据栅格地图中障碍物与其他栅格的位置关系。在一些实施例中，可以将搜索位姿的所在点作为机器人的当前位姿，并将激光点云数据以该搜索位姿映射到代价地图上。

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

当将激光点云数据映射到代价地图上时，可以基于激光点云数据中包含的距离障碍物的距离信息，以该搜索位姿为中心，获取多个距离该搜索位姿在该距离范围内的多个栅格的代价值。在一些实施例中，可以对代价地图进行双线性插值以计算每个雷达数据点和代价值。在一些实施例中，可以将映射范围内所有栅格的代价值的和作为搜索位姿的得分值。依次遍历27个搜索位姿，计算每个搜索位姿对应的得分值，并将得分值最大的搜索位姿作为第二最优位姿，该最大得分值为最优得分值。

图3为本发明一些实施例提供的确定代价地图的示例性流程图。如图3所示，流程300包括以下步骤：

在一些实施例中，确定代价地图的步骤可以在机器人定位开始的时候执行一次，后续不需要再重复执行。

步骤310，获取占据栅格。

占据栅格是指在占据栅格地图中处于占据状态的栅格。在一些实施例中，可以获取机器人工作场景的占据栅格地图，然后通过识别占据栅格地图中的栅格的状态确定占据栅格。占据栅格地图就是把环境划分成一系列栅格，其中每一栅格可能有三个状态：占据、自由、未知。其中占据状态表示这个栅格存在障碍物。自由状态表示不存在障碍物。未知的状态表示建图的时候未扫描到该区域，该区域是否有障碍物是未知的。占据栅格地图可以通过各种可行的方式得到，包括但不限于网络获取、提取数据库等。

步骤320，获取待模糊栅格。

待模糊栅格是指在占据栅格地图中距占据栅格不超过10个栅格距离且处于自由和未知状态的栅格。在一些实施例中，可以通过识别占据栅格地图中的栅格的状态确定占据栅格。

步骤330，基于待模糊栅格与占据栅格的距离，确定待模糊栅格的代价值。

代价值与距离成反比。在一些实施例中，代价值的计算公式可以为：

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

例如，占据栅格地图的分辨率为0.025米/栅格，则向障碍物所在栅格周围0.25米半径内进行了模糊化。遍历每个障碍物栅格，对其周围0.25米范围内进行模糊，如果有重复被模糊的栅格，则保留最大的模糊值，未被模糊到的自由状态栅格和未知状态栅格的代价值均设置为0。经过对栅格地图进行模糊化后，生成了一张尺寸和分辨率与占据栅格地图一致的代价地图。代价地图中距离障碍物距离越近的栅格,代价值越接近于1，距离障碍物越远的栅格，代价值越接近于0。

图4为本发明一些实施例提供的一种基于变步长的机器人定位系统的示例性模块图。如图4所示，系统400包括搜索位姿确定模块410、第二最优位姿确定模块420、第一判断模块430、第一最优位姿确定模块440、第二搜索步长确定模块450、第二判断模块460、第一搜索步长确定模块470和当前位姿确定模块480。

搜索位姿确定模块410用于基于第一最优位姿和第一搜索步长，确定多个搜索位姿；第一最优位姿是指最符合机器人当前位置和姿态的位姿。关于搜索位姿确定模块410的更多内容，参见图1及其相关描述。

第二最优位姿确定模块420用于确定多个搜索位姿的得分值，并确定最优得分值和第二最优位姿；第二最优位姿是指搜索位姿中最符合机器人当前位置和姿态的位姿。关于第二最优位姿确定模块420的更多内容，参见图1及其相关描述。

第一判断模块430用于判断第二最优位姿是否比第一最优位姿更符合机器人的当前位置和姿态。关于第一判断模块430的更多内容，参见图1及其相关描述。

第一最优位姿确定模块440用于当第二最优位姿比第一最优位姿更符合机器人的当前位置和姿态时，将第二最优位姿更新为第一最优位姿。关于第一最优位姿确定模块440的更多内容，参见图1及其相关描述。

第二搜索步长确定模块450用于当第一最优位姿比第二最优位姿更符合机器人的当前位置和姿态时，确定第二搜索步长：第二搜索步长小于第一搜索步长。关于第二搜索步长确定模块450的更多内容，参见图1及其相关描述。

第二判断模块460用于判断第二搜索步长是否小于最小步长阈值。关于第二判断模块460的更多内容，参见图1及其相关描述。

第一搜索步长确定模块470用于当第二搜索步长大于最小步长阈值时，将第二搜索步长更新为第一搜索步长。关于第一搜索步长确定模块470的更多内容，参见图1及其相关描述。

当前位姿确定模块480用于当第二搜索步长小于最小步长阈值时，将第一最优位姿作为机器人的当前位姿。关于当前位姿确定模块480的更多内容，参见图1及其相关描述。

以上仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims

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

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

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

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

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

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

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

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

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

以步长

在X轴上生成三个横轴搜索位置

、

和

；

以步长

在Y轴上生成三个纵轴搜索位置

、

和

；

以步长

在朝向上生成三个搜索姿态

、

和

；

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

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

按比例缩小所述步长

、所述步长

和所述步长

，得到新的步长

、步长

和步长

。

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

判断所述步长

、所述步长

和所述步长

是否均小于各自的最小步长阈值。

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

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

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

基于所述多个搜索位姿，将所述激光点云数据分别映射到代价地图上；

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

6.根据权利要求5所述的基于变步长的机器人定位方法，其特征在于，所述代价地图通过模糊化占据栅格地图得到，所述模糊化包括，

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

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

基于所述待模糊栅格与所述占据栅格的距离，确定所述待模糊栅格的代价值；所述代价值与距离成反比。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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