CN113310484B

CN113310484B - 一种移动机器人定位方法和系统

Info

Publication number: CN113310484B
Application number: CN202110592577.6A
Authority: CN
Inventors: 余子衿; 张汉烨
Original assignee: Hangzhou Amy Ronotics Co ltd
Current assignee: Hangzhou Amy Ronotics Co ltd
Priority date: 2021-05-28
Filing date: 2021-05-28
Publication date: 2022-06-24
Anticipated expiration: 2041-05-28
Also published as: CN113310484A

Abstract

本发明公开了一种移动机器人定位方法和系统，所述方法包括如下步骤：确定定位场景，在所述定位场景内贴装视觉定位标识符，建立视觉定位标识符地图；在所述定位场景同时建立激光定位地图；获取视觉标识符地图定位数据和激光定位地图的定位数据，并处理两种定位数据，形成融合定位数据；根据所述融合定位数据进行处理获取或更新定位结果。所述方法和系统采用视觉标识符和激光里程计进行数据融合的方式进行机器人定位，可以有效地提高机器人定位的精度，避免单纯的激光里程计扫描存在地图边缘缺失的问题。

Description

一种移动机器人定位方法和系统

技术领域

本发明涉及移动机器人定位领域，特别涉及一种移动机器人定位方法和系统。

背景技术

目前使用激光里程计作为移动机器人定位的方式已成为一大主流，其算法执行效率高，定位精度也较高，但是也存在高度相似的几何环境定位丢失的风险，大地图下全局自主重定位计算速度慢甚至无法准确定位。目前视觉传感器价钱低廉，成本底，定位精度也较高，也是用于定位的常用手段。但一般基于图像特征追踪的方式在大场景下需要维护很大的地图，且对计算平台有很高的要求，目前并不适用于使用嵌入式计算平台的移动机器人。

比如现有技术中专利申请号为CN201811085808的中国专利公开了一种基于无人机视觉SLAM的辅助地面机器人导航方法及装置，该专利用视觉特征去弥补激光建图时由于扫描高度限制而导致的地图边缘信息缺失的问题，但并没有有效的提高移动机器人的定位精度。因为本来激光应该看不到的地方还是看不到，由于建图时考虑了不同高度的障碍物，并将其反映在地图中，反而使得定位时做scan-map的匹配时无法正确的匹配。

发明内容

本发明其中一个发明目的在于提供一种移动机器人定位方法和系统，所述方法和系统采用视觉标识符和激光里程计进行数据融合的方式进行机器人定位，可以有效地提高机器人定位的精度，避免单纯的激光里程计扫描存在地图边缘缺失的问题。

本发明其中一个发明目的在于提供一种移动机器人定位方法和系统，所述方法和系统可以在日常工作中进行视觉标识符地图精度的自优化，从而使得环境定位精度的影响较小。

本发明其中一个发明目的在于提供一种移动机器人定位方法和系统，所述方法和系统采用记录三维视觉标识符的地图结构，可以和传统的激光地图同步，从而使得机器人可以适应相似度极高的复杂环境。

本发明其中一个发明目的在于提供一种移动机器人定位方法和系统，所述方法和系统对机器人本体和机器人自身的机构部件采用视觉标识进行定位，用于检测机器人在全局地图中的状态，从而可以有效地提高对机器人定位精度，使得机器人可以在几何复杂度较高的环境中执行相关的定位操作。

本发明其中一个发明目的在于提供一种移动机器人定位方法和系统，所述方法和系统结合视觉标识符和激光里程计无需全场景粘贴，只需要粘贴至需要工作的场景，可以降低定位成本，并且可以实现无人化的自动定位。

为了实现至少一个上述发明目的，本发明进一步提供一种移动机器人定位方法，所述方法包括如下步骤：

确定定位场景，在所述定位场景内贴装视觉标识符，建立视觉标识符地图；

在所述定位场景同时建立激光定位地图；

获取视觉标识符地图定位数据和激光定位地图的定位数据，并处理两种定位数据，形成融合定位数据；

根据所述融合定位数据进行处理获取或更新定位结果。

根据本发明其中一个较佳实施例，所述定位方法包括视觉标识符地图的初始化步骤：获取机器人的当前位置，通过激光里程计建立临时地图，在所述临时地图内选取当前移动机器人未到达过的栅格点作为目标点进行移动，直到检测到所述视觉标识符地图中贴装好的视觉标识符停止。

根据本发明另一个较佳实施例，通过检测到的觉定位标识符计算移动机器人在定位地图中的全局位姿：

T_wr＝T_wt(T_raT_at)^-1

其中T_wt表示视觉标识符在全局地图中的位姿，T_wr表示激光里程计定位移动机器人在全局地图中的位姿，T_ra表示从相机安装位姿到机器人中心的坐标变化关系，T_at表示相机坐标系下视觉标识符的位姿，所述相机安装于机器人本体和运动机构。

根据本发明另一个较佳实施例，所述视觉标识符地图采用结构化数据存储，所述结构化数据包括：视觉标识符编号id-视觉标识符位置xyz-视觉标识符姿态(四元数形式)Q-迭代优化次数N-最近一次优化或检测的时间t。

根据本发明另一个较佳实施例，所述定位方法还包括在对视觉标识符定位精度优化检测方法：

e_i＝ln(T_it ^-1T_i ^-1T_t)^∨＝ln(exp((-ξ_it)^∧)exp((-ξ_i)^∧)exp(ξ_t ^∧))^∨

其中，ξ表示待优化的变量为李代数形式下表示的视觉表示符，用于表示移动机器人或移动机器人机构的空间位姿；i是索引标号，指代第i次对视觉标识符的检测；e_i表示的是设计的误差函数；T_it表示的是从视觉标识符到移动机器人或移动机器人机构的位姿变换关系，ξ_it是T_it的李代数表示形式，T_i表示的是移动机器人或者移动机器人机构检测到视觉标识符在地图上的空间位姿，ξ_i是T_i的李代数表示形式，T_t表示的是视觉标识符在地图上的空间位姿，ξ_t是T_t的李代数表示形式，ρ表示的是核函数，用来抑制检测到的外点对优化系统的影响。

根据本发明另一个较佳实施例，所述视觉标识符定位精度优化方法包括：

设置最大检测次数N_max；

记录当前检测次数为N，若N＜N_max，则优化完后N＝N+1；

直到当前检测次数N≥N_max。

根据本发明另一个较佳实施例，所述觉标识符定位精度优化方法包括：

周期性遍历所述视觉标识符地图，记录检测时移动机器人对应的地图点；当满足

T_now-t＜t_τ

时，删除记录的地图点；其中T_now表示的是当前的时间，t是在视觉标识符地图中记录的上次访问或修改此时间对应的视觉标识符的时间，t_τ表示时间间隔。

根据本发明另一个较佳实施例，所述融合定位数据处理方法包括：

获取n个位姿的检测结果，在k时刻估计的检测值为：

其中Z_k＝[z_1，k z_2，k … z_n，k]^T，表示对应n个位姿检测结果；

W_k＝[ω_1，k ω_2，k … ω_n，k]^T；表示对应n个检测结果的权重系数，其中所述权重系数满足

误差函数为

根据本发明另一个较佳实施例，所述融合定位数据处理方法包括构建非线性检测结果的优化：

使用梯度下降法获得W_k的更新：

其中

表示W_k的更新结果；

其中，

设置激光检测误差为e_L，k；当前误差为e_i，k，若e_L，k＜＜e_i，k，i≠L，则将本次融合定位数据中的激光检测数据丢弃。

为了实现至少一个上述发明目的，本发明进一步提供一种移动机器人定位系统，所述定位系统采用上述一种机器人移动定位方法。

本发明提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序可被处理器执行上述一种移动机器人定位系统。

附图说明

图1显示的是本发明一种移动机器人定位方法的流程示意图。

图2显示的是本发明中移动机器人结构示意图。

其中

相机a-10，相机b-20，激光雷达-30

具体实施方式

以下描述用于揭露本发明以使本领域技术人员能够实现本发明。以下描述中的优选实施例只作为举例，本领域技术人员可以想到其他显而易见的变型。在以下描述中界定的本发明的基本原理可以应用于其他实施方案、变形方案、改进方案、等同方案以及没有背离本发明的精神和范围的其他技术方案。

可以理解的是，术语“一”应理解为“至少一”或“一个或多个”，即在一个实施例中，一个元件的数量可以为一个，而在另外的实施例中，该元件的数量可以为多个，术语“一”不能理解为对数量的限制。

请参考图1，本发明公开了一种移动机器人定位方法的流程示意图，所述方法主要包括两个部分，首先是建立精确的视觉标识符地图，进一步在机器人移动过程中采用激光里程计获取的定位数据融合所述视觉标识符的定位数据，用于对移动机器人执行精确的定位。

所述视觉标识符地图的建立方法包括如下步骤：

选定移动机器人的工作场景，在所述工作场景内粘贴所述视觉标识符，用于建立视觉标识符地图；

在所述机器人本体和机器人机构上安装定位相机，所述定位相机用于获取拍摄视野内的视觉标识符，所述机器人机构包括但不仅限于机械臂、叉齿、摇臂等，定位相机在拍摄到所述视觉标识符后将该视觉标识符保存至所述视觉标识符地图中，所述移动机器人还设有激光里程计，所述激光里程机用于计算激光地图中的定位数据，通过融合所述视觉标识符地图和激光地图的定位数据对所述移动机器人进行精确定位。由于在所述机器人机构上安装了定位相机，因此在融合所述视觉标识符和激光的定位数据，建立融合定位数据后，可以精确定位所述移动机器人在三维控制中的位姿，从而可以适应复杂几何环境下的作业任务。

具体的，本发明所述的融合定位数据包括：机器人在移动过程中对视觉标识符在全局地图中的位姿，所述全局地图表示地图中所有数据的总和，其中所述全局地图包括激光地图和视觉标识符地图，所述激光地图记录了空间中由激光里程计记录的所有环境结构的点在全局地图中的位置，所述视觉标识符地图记录了所有视觉标识符在全局地图中的位置。进一步计算所述移动机器人对视觉标识符在所述全局地图中的位姿：

T_wt＝T_wrT_raT_at；

其中T_wt表示视觉标识符在全局地图中的位姿，T_wr表示激光里程计定位移动机器人在全局地图中的位姿，T_ra表示从相机安装位姿到机器人中心的坐标变化关系，T_at表示相机坐标系下视觉标识符的位姿。进一步将检测到的视觉标识符在所述全局地图中的位姿采用如下结构化数据进行保存：

视觉标识符编号id-视觉标识符位置xyz-视觉标识符姿态(四元数形式)Q-迭代优化次数N-最近一次优化或检测的时间t。

由于所述移动机器人在工作环境下受到各种因素的影响，从而导致定位精度下降的情况，本发明进一步对日常工作中的移动机器人的定位进行优化，用于提高移动机器人的定位精度，定位精度的优化方法包括：

其中ξ表示待优化的变量为李代数形式下表示的视觉表示符，argmin表示

值最小时i的取值，表示移动机器人或移动机器人机构的空间位姿；∧是合取符号，∨为析取符号，i是索引标号，指代第i次对视觉标识符的检测；e_i表示的是设计的误差函数；T_it表示的是从视觉标识符到移动机器人或移动机器人机构的位姿变换关系，ξ_it是其李代数表示形式，T_i表示的是移动机器人或者其机构检测到视觉标识符在地图上的空间位姿，ξ_i是其李代数表示形式，T_t表示的是视觉标识符在地图上的空间位姿，ξ_t是其李代数表示形式，ρ表示的是核函数，用来抑制检测到的外点对优化系统的影响。也就是说，通过计算上述检测数据，并设置误差函数和核函数可以有效地提高移动机器人的定位精度，并且可以做到实时定位的技术效果。

本发明在使用激光里程计建立新的地图，所述新的地图是临时地图，通过所述机器人自身的建图模块建立虚拟的具有栅格点的临时地图，需要说明的是，所述临时地图的原点为机器人当前在工作场景中的位置。并通过移动机器人的不断移动来扩大机器人当前所在的场景。

具体而言：所述移动机器人获取当前位置，并通过所述临时地图寻找到最接近机器人当前位置的栅格点，并判断该栅格点是否经历过，若否则将该栅格点作为所述移动机器人的目标点靠近该栅格点进行移动，若检测到的最近的栅格点为已经经历过的点，则再寻找其他未经历的最接近的栅格点作为目标点进行靠近移动。到达未经历的栅格点后重复执行临时地图的建立和目标点的寻找，直到所述移动机器人通过所述定位相机检测到场景中的已经记录的视觉标识符。

进一步计算初始化状态下的位姿检测结果：若所述移动机器人检测到场景中的视觉标识符，则根据所述视觉标识符计算所述移动机器人已经构建完毕的全局图中的位姿：

T_wr＝T_wt(T_raT_at)^-1；

其中T_wt表示视觉标识符在全局地图中的位姿，T_wr表示激光里程计定位移动机器人在全局地图中的位姿，T_ra表示从相机安装位姿到机器人中心的坐标变化关系，T_at表示相机坐标系下视觉标识符的位姿。需要说明的是，T_ra是根据机器人自身的坐标系建立的转换关系，也就是说可以根据机器人的几何结构获取运动中心，根据运动中心设置三维立体坐标系，在所述移动机器人身上设置不同的相机，根据所述移动机器人运动中心建立的三维立体坐标系使得每个相机都有一个坐标参数，该坐标参数为从相机安装位姿到机器人中心的坐标变化关系。其中所述相机自身的拍摄界面也设有一个拍摄界面坐标系，该拍摄界面坐标系以传统的拍摄界面左上角为拍摄坐标系原点，以拍摄界面左方向为拍摄坐标系x轴正轴延伸方向，以拍摄界面下方向为拍摄坐标系y轴延伸方向，以拍摄界面朝里的方向为拍摄坐标系的z轴。当所述相机拍摄到视觉标识符后就可以获取所述视觉标识符在所述拍摄界面坐标系下的位姿，需要说明的是，拍摄相机可以设置成多个，从而使得不同相机拍摄坐标系下视觉标识符的位姿不同。

在获取上述至少一个工作地图的全局位姿后，删除建立的临时地图，并根据上述全局位姿进行移动机器人的融合定位，所述融合定位采用了视觉标识符的多检测数据和激光里程计定位数据的融合定位，在执行融合定位计算时，需要加载所述移动机器人中正常保存的激光地图和视觉标识定位地图，并将机器人的初始位置设置为上述全局地图位姿计算结果下的机器人所在的位置。

所述视觉标识符可以采用包括但不仅限于Apriltag，ARtag等开源稳定方案，解算的结果需要包括物体在三维空间中六个自由度的表达。

值得一提的是，在本发明其中一个较佳实施例中，所述融合定位方法包括如下步骤：设置定位标识符的个数为n-1，n＝0，1，2……，在k时刻估计的位姿结果测量值为：

其中Z_k＝[z_1，k z_2，k … z_n，k]^T；z_n，k表示第n个位姿的检测结果，Z_k表示k时刻n个位姿的检测结果的集合，Z_k包含视觉标识符的位姿检测结果和激光里程计的位姿检测结果，通过上述位姿检测结果可以使得激光定位数据和视觉标识符定位数据进行融合，从而使得定位结果的精度更好。

设置k时刻所述位姿检测结果的权重系数W_k，其中W_k＝[ω_1，k ω_2，k … ω_n，k]^T；ω_n，k表示对第n个位姿检测结果的数据设置的权重系数，将所述位姿检测结果权重系数W_k和位姿的检测结果进行融合，生成所述k时刻估计的测量值

进一步根据估计的位姿结果测量值

和实际位姿检测结果Z_k计算误差函数

上述权重系数满足

T上标表示转置矩阵。

进一步构造非线性优化更新任务：

采用梯度下降法对位姿检测结果权重系数W_k进行更新：

其中，

在实际的检测和更新过程中需要对异常数据进行处理，异常数据的处理过程包括如下步骤：设置误差阈值e_L，k，测量当前误差为e_i，k；若当前误差远大于所述误差阈值，即e_L，k＜＜e_i，k，i≠L，则认为激光测量数据在高度特征重复的环境下发生定位丢失，本次数据融合将激光测量数据舍弃。

在本发明另一较佳实施例中，需要对移动过程中的机器人进行全局地图的姿态检测，移动状态下检测结果可以如下表示：

T_t＝T_wt＝T_wrT_raT_at；

其中，T_wt是表示视觉标识符在全局地图中的位姿，T_wr表示激光里程计定位移动机器人在全局地图中的位姿，T_ra表示从相机安装位姿到机器人中心的坐标变化关系，T_at表示相机坐标系下视觉标识符的位姿。

进一步根据位姿检测结果建立结构化数据存储，所述结构化数据包括：视觉标识符编号id-视觉标识符位置xyz-视觉标识符姿态(四元数形式)Q-迭代优化次数N-最近一次优化或检测的时间t。

本发明进一步对融合定位的优化更新进行管理，优化更新方法包括如下步骤：

若检测到的视觉标识符编号为初次检测，将本次检测的结果以上述设计的数据结构存储于文件中。

在机器人的移动过程中，若检测到视觉标识符满足如下条件，则执行上述的视觉标识符优化更新策略：

若当前优化次数为N＜N_max；

则执行当前的优化更新策略，优化完成后更新N＝N+1，其中，N是视觉标识符地图中记录的检测到视觉标识符的次数。N_max表示的设计的最大检测次数，可以根据具体应用场景灵活调整。周期性的对记录的视觉标识符地图进行遍历检测，若满足：

T_now-t＜t_τ；

其中，T_now表示的是当前的时间，t是在视觉标识符地图中记录的上次访问或修改此时间对应的视觉标识符的时间。t_τ表示时间间隔，可以根据具体应用场景灵活调整。

在本发明其中一个较佳实施例中可选取优化的最大次数N_max＝200，优化选取的核函数为Huber核函数，具体如下：

其中，ε的取值可以设置为0.25。

在管理视觉标识符地图策略中，t_τ选取可以为2592000s。

为了更好地解释本发明，本发明以电商仓库作为本发明具体实施实例举例说明：该电商仓库的长约120米，宽约80，高约10米，仓库内重复的复杂集合环境繁多，多为相同式样的货架和廊柱。选择在其几何结构重复度高的区域，例如长走廊中间或者交叉十字路口处的地面上张贴Apriltag视觉标识符。选择需要移动机器人叉腿搬运货物的货架或附近的墙面上，张贴Apriltag视觉标识符。张贴的角度无严格要求，方便张贴布置为主。选择使用的Apriltag视觉标识符的样式为tag_36_11，大小为0.2m×0.2m。

实施实例中采用的移动机器人如附图2所示：移动机器人有安装一个激光雷达30和两个RGB相机(相机a10和相机b20)，其中相机a10固定在本体上，相机b20固定在叉腿上，相机b跟随叉腿进行上下移动。

本实施中如若是第一次部署运用则需要构建激光里程计构建仓库环境的二维激光栅格地图和视觉标识符地图，建图方式为人工遥控移动机器人，移动机器人初步构建带有边界且任务点信息描述正确的初步地图。

特别地，根据本发明公开的实施例，上文参考流程图描述的过程可以被实现为计算机软件程序。例如，本公开的实施例包括一种计算机程序产品，其包括承载在计算机可读介质上的计算机程序，该计算机程序包含用于执行流程图所示的方法的程序代码。在这样的实施例中，该计算机程序可以通过通信部分从网络上被下载和安装，和/或从可拆卸介质被安装。在该计算机程序被中央处理单元(CPU)执行时，执行本申请的方法中限定的上述功能。需要说明的是，本申请上述的计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质或者是上述两者的任意组合。计算机可读存储介质例如可以是但不限于电、磁、光、电磁、红外线段、或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子可以包括但不限于：具有一个或多个导线段的电连接、便携式计算机磁盘、硬盘、随机访问存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本申请中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。而在本申请中，计算机可读的信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读的信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于：无线段、电线段、光缆、RF等等，或者上述的任意合适的组合。

附图中的流程图和框图，图示了按照本发明各种实施例的系统、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段、或代码的一部分，该模块、程序段、或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个接连地表示的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或操作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。

本领域的技术人员应理解，上述描述及附图中所示的本发明的实施例只作为举例而并不限制本发明，本发明的目的已经完整并有效地实现，本发明的功能及结构原理已在实施例中展示和说明，在没有背离所述原理下，本发明的实施方式可以有任何变形或修改。