CN112034851B - 集群机器人建图系统及方法 - Google Patents

Abstract

本发明公开了一种集群机器人建图系统及方法，扫地机器人利用激光雷达构建栅格地图，在基于路由器搭建的局域网通信模块中实时发送局部子地图到控制中心，最后在已知机器人初始相对位姿基础上，通过地图坐标系转换融合为完整地图。本发明基于局域网的集群扫地机器人实时建图方法，可以解决在大面积室内环境下建图时间长的问题，同时既可以提高建图质量，又可以实时观察建图效果。

Description

集群机器人建图系统及方法

技术领域

本发明涉及集群扫地机器人建图领域，特别是涉及一种基于局域网的集群机器人实时建图系统。

背景技术

机器人逐渐代替人类完成基本工作。最为常见的是扫地机器人，它可以帮助我们打扫卫生，深受广大人民的喜爱。但是随着清扫环境的变化，比如面积变大，结构更加复杂，单个扫地机器人任务完成效率逐渐降低，甚至对于环境的感知也随着长时间的游走而产生的一定的误差，影响工作效率。因此集群机器人工作成为研究热点，现有的特征点匹配融合方法需要很大重叠区域，不能实时融合，同时误匹配特征点比较多，从而无法得出正确的转换关系。

发明内容

本发明所要解决的技术问题是，针对现有技术不足，提供一种集群机器人建图系统，实时传送地图数据，防止数据丢失，实时融合地图。

为解决上述技术问题，本发明所采用的技术方案是：一种集群机器人建图系统，包括：

集群机器人，各机器人构建局部环境地图，并将局部环境地图经通信网络发送至控制中心；

通信网络，包括由多个路由器搭建的局域网；

控制中心，用于对接收到的局部环境地图解压，并利用各机器人初始位姿和局部环境地图解压数据融合局部环境地图，得到全局地图。

本发明局域网(通信网络)可以实时传送地图数据，并且不会有数据丢失情况，同时根据初始位姿融合可以实时融合地图。

所述通信网络传输局部环境地图的通信格式为(height，width，d1，a[]，origini)；其中(height，width)为局部环境地图的高度和宽度，d1为局部环境地图数据长度，a[]为局部环境地图数据，origini＝(ox_i,oy_i)为局部环境地图左上角在机器人坐标系中坐标值，i为机器人编号。本发明只传送必要数据，减少原始地图无关数据，缩短数据长度。

各机器人并排设置。机器人并排设置可以固定初始相互位姿，避免每次修改初始位姿参数。

所述控制中心获得全局地图的具体实现过程包括：

S1、对于机器人A和机器人B，将机器人B的左上角真实坐标值转化为机器人A坐标系中的坐标，其中机器人A坐标系和机器人B坐标系分别以机器人初始位置为原点，前进方向为Y轴负方向，原地右转前进方向为X轴正方向。将机器人B的坐标在xy方向上分别加上真实初始位姿坐标值，从而将机器人A和机器人B的局部环境地图左上角坐标值转化为机器人A坐标系下的坐标值；其中，机器人A和机器人B相邻；

S2、在机器人A坐标系下，根据机器人A和机器人B的左上角坐标点位置，确定两个坐标值中沿x，y方向距离最大的值为全局地图原点在机器人A坐标系下坐标(ox，oy)；

S3、获取机器人A和机器人B局部环境地图右下角在机器人A坐标系中坐标，将机器人A和机器人B局部环境地图右下角在机器人A坐标系中坐标值的x和y值分别进行比较，分别选择两个坐标值中x方向和y方向最大值作为全局地图的右下角x和y坐标值，计算出全局地图大小(heigh，width)；

S4、创建两个全局地图大小的空白地图，根据机器人A和机器人B局部环境地图左上角坐标与全局地图原点坐标(ox，oy)的相互关系，推导出机器人A和机器人B的局部环境地图和全局地图的转换关系，即转换矩阵分别为T₁和T₂，利用T₁和T₂分别对机器人A和机器人B的局部环境地图对应进行转换；

S5、根据融合准则融合转换后的两幅地图，得到全局地图。

以上步骤S1—S5根据地图左上角坐标进行地图坐标系转换，可以实时地图融合，并且随着单机器人建图回环检测左上角坐标会动态优化，从而减少误差。

步骤S4中，转换矩阵为，其中(ox₁，oy₁)为机器人A局部环境地图在机器人A坐标系下的左上角坐标，(ox₁₂，oy₁₂)为机器人B局部环境地图在机器人A坐标系下的左上角坐标，

转换矩阵只包含坐标系直接的x和y方向转换，无旋转变换，计算简单，节省时间。

步骤S5中，融合准则为，

s1＝障碍，s2＝空闲，ss＝障碍；

s1＝障碍，s2＝未知，ss＝障碍；

s1＝空闲，s2＝未知，ss＝空闲；

s1＝空闲，s2＝障碍，ss＝障碍；

s1＝未知，s2＝障碍，ss＝障碍；

s1＝未知，s2＝空闲，ss＝空闲；

其中，s1为机器人A局部环境地图转换后得到的地图中的任一位置的像素点状态，s2为机器人B局部环境地图转换后得到的地图对应s1的位置的像素点状态，ss为融合后得到的地图对应s1的位置的像素点状态。

本发明的融合准则可以简单方便的对于同一位置的不同状态进行判断。

本发明还提供了一种集群机器人建图方法，包括以下步骤：

1)各机器人构建局部环境地图；

2)利用各机器人初始位姿和局部环境地图解压数据融合局部环境地图，得到全局地图。

步骤2)的具体实现过程包括：

S1、对于机器人A和机器人B，将机器人B的左上角真实坐标值转化为机器人A坐标系中的坐标，，其中机器人A坐标系和机器人B坐标系分别以机器人初始位置为原点，前进方向为Y轴负方向，原地右转前进方向为X轴正方向。将机器人B的坐标在xy方向上分别加上真实初始位姿坐标值，从而将机器人A和机器人B的局部环境地图左上角坐标值转化为机器人A坐标系下的坐标值；其中，机器人A和机器人B相邻；

S2、在机器人A坐标系下，根据机器人A和机器人B的左上角坐标点位置，确定两个坐标值中沿x，y方向距离最大的值为全局地图原点在机器人A坐标系下坐标(ox，oy)；

S3、获取机器人A和机器人B局部环境地图右下角在机器人A坐标系中坐标，将机器人A和机器人B局部环境地图右下角在机器人A坐标系中坐标值的x和y值分别进行比较，分别选择两个坐标值中x方向和y方向最大值作为全局地图的右下角x和y坐标值，计算出全局地图大小(heigh，width)；

S4、创建两个全局地图大小的空白地图，根据机器人A和机器人B局部环境地图左上角坐标与全局地图原点坐标(ox，oy)的相互关系，推导出机器人A和机器人B的局部环境地图和全局地图的转换关系，即转换矩阵分别为T₁和T₂，利用T₁和T₂分别对机器人A和机器人B的局部环境地图对应进行转换；

S5、根据融合准则融合转换后的两幅地图，得到全局地图。

步骤S5中，融合准则为，

s1＝障碍，s2＝空闲，ss＝障碍；

s1＝障碍，s2＝未知，ss＝障碍；

s1＝空闲，s2＝未知，ss＝空闲；

s1＝空闲，s2＝障碍，ss＝障碍；

s1＝未知，s2＝障碍，ss＝障碍；

s1＝未知，s2＝空闲，ss＝空闲；

其中，s1为机器人A局部环境地图转换后得到的地图中的任一位置的像素点状态，s2为机器人B局部环境地图转换后得到的地图对应s1的位置的像素点状态，ss为融合后得到的地图对应s1的位置的像素点状态。

与现有技术相比，本发明所具有的有益效果为：

1、本发明在面积大结构复杂的环境下，可以协同工作，共同完成环境建图。

2、集群扫地机器人协同工作节省时间，提高工作效率，建图质量也有较大提升。

3、本发明可以实时传送地图数据，并且不会有数据丢失情况，同时根据初始位姿融合可以实时融合地图。

附图说明

图1为集群扫地机器人框架图；

图2为单个扫地机器人局域网通信结构；

图3为实施例1机器人真实环境单独建立的三副局部子地图(局部环境地图)；

图4为实施例1实验结果展示图。

具体实施方式

本发明提供了一种基于局域网的群扫地机器人实时建图系统，包含多个扫地机器人，每个扫地机器人携带有路由器和单轴激光雷达；所述扫地机器人利用激光雷达构建二维平面地图，在搭建的局域网环境下传输地图数据；所述控制中心利用初始位姿和地图左上角坐标进行地图融合，最后将全局地图发送到每个扫地机器人。

进一步地，通信网络是由各个路由器相互桥接搭建的局域网基础下，利用socket通信机制中的TCP协议，将栅格地图一维数据和相关位置信息进行实时传送。

扫地机器人随机漫游在环境中，利用Cartographer建立栅格平面地图。

本发明传输地图数据消息格式为(height，width，d1，a[]，origini),其中(height，width)为地图大小，d1为地图数据长度，a[]为一维栅格地图数据，origini＝(ox_i,oy_i)为地图左下角在机器人坐标系中坐标值(i为机器人编号)。

本发明中，已知扫地机器人初始位姿，以机器人初始位姿为原点构建机器人坐标系，通过初始位姿关系推导出地图转换关系，从而融合成全局地图。对于大小不同的地图，利用地图左上角在扫地机器人坐标系中坐标值(ox_i,oy_i)和扫地机器人之间的相互位置，其中机器人2与机器人1的真实相对位置为(tx，ty)，计算出子地图之间的转换关系。

我们将机器人2的左上角真实坐标值转化为机器人1坐标系中坐标，将机器人2的坐标在xy方向上分别加上真实初始位姿的值，便可以将两个地图左上角坐标值化为统一坐标系。

origin1＝(ox₁,oy₁) (1)

origin2＝(ox₂,oy₂) (2)

origin12＝(ox₁₂,oy₁₂)＝(ox₂+tx,oy₂+ty) (3)

在机器人1坐标系下，根据两个左上角坐标点位置(两个坐标点数值为负)，可以确定两个坐标值中沿x，y方向距离最大的值为全局地图原点坐标(ox，oy)。

ox＝min(ox₁,ox₁₂) (4)

oy＝min(oy₁,oy₁₂) (5)

获得全局原点坐标后，可以求得每个子地图右下角在机器人A坐标系中坐标，将两个坐标值比较，选择每个方向最大值作为全局地图的右下角坐标值，由此计算出全局地图大小(heigh，width)。创建两个全局地图大小的空白地图。在同一坐标系下，由于我们建立的是二维平面栅格地图，根据全局地图原点坐标与机器人局部地图左上角坐标，我们可以推导出机器人局部子地图到全局地图的像素坐标平移矩阵。

实施例1的系统由两台深兰福喜吸尘机器人和一台终端计算机组成，在大面积区域内协同建图。每台扫地机器人均安装一个路由器和一个TIM561激光雷达，终端计算机安装Ubuntu16.04系统。集群扫地机器人系统框架见图1，本发明主要步骤如下：

步骤一：两个扫地机器人在环境中随机游走，利用cartographer包建立栅格地图，此功能包会实时将地图数据传送到话题上。

步骤二：扫地机器人通过局域网将地图数据和位姿数据传送到控制中心。

步骤三：控制中心收到地图数据后，利用所述地图融合方法，根据扫地机器人初始位姿，我们可以融合局部子地图，从而得到全局地图。控制中心将全局地图再发送到每个扫地机器人。

在步骤一中，cartographer包订阅激光雷达数据，将每一帧扫描数据(laserscan)构建子地图(submap)，每隔一段时间进行以此所有submap数据图优化，最后输出栅格地图以及地图左上角在机器人坐标系中坐标数据。

在步骤二中，每个扫地机器人携带路由器，通过路由器之间的桥接，可以使得所有扫地机器人在同一局域网下，控制中心上位机通过wife信号加入局域网，形成一个通信网络。其整个数据传送过程如下：控制中心作为客户端首先循环向每个扫地机器人请求发送地图数据；扫地机器人作为服务端，首先响应客户端的要求，通过订阅ROS环境中的/map话题的Occupancygrid消息获得实时栅格地图数据，将数据和其数据长度压缩为一个包一起发送到服务端；控制中心接收到数据后，将压缩包进行恢复获得其实际数据，再经过相应数据处理成ROS话题所对应的消息类型，在发送到/map(id)话题上。

在步骤二中，控制中心接受到地图相关数据中，包含地图大小以及栅格数据，其对应的Occupancygrid消息内包含有以下参数：消息名称，地图大小，地图分辨率，地图一维栅格数据。我们将受到的数据封装成此消息格式，发送到对应话题下。

在步骤二中，控制中心接受到地图相关数据中，包含地图左上角在机器人坐标系中坐标，我们由此需设计一个坐标消息将其发送到相应话题上，坐标消息中我们设置两个int类型数据值，分别为x和y坐标值。

在步骤三中，我们已知机器人的初始位姿，由此可以通过初始位姿推导出机器人局部子地图之间的转换关系。在我们机器人发布到地图话题的地图数据中，主要包含栅格地图数据以及地图左上角像素坐标在真实环境下机器人坐标系的坐标位置。我们主要利用这个左上角坐标来进行地图转换。

实施例1地图转换过程如下：机器人1构建map1，其左上角实际坐标为origin1；机器人2构建map2，其左上角实际坐标为origin2，机器人2与机器人1的真实相对位置为(tx，ty)。

两个机器人创建的地图大小可能会有不一致的情况，所以首先需要考虑全局地图的原点位置以及地图大小。首先我们将机器人2的左上角真实坐标值转化为机器人1坐标系中坐标，将机器人2的坐标在xy方向上分别加上真实初始位姿的值，便可以将两个地图左上角坐标值化为统一坐标系。

origin1＝(ox₁,oy₁) (1)

origin2＝(ox₂,oy₂) (2)

origin21＝(ox₁₂,oy₁₂)＝(ox₂+tx,oy₂+ty) (3)

在机器人1坐标系下，根据两个左上角坐标点位置(两个坐标点数值为负)，可以确定两个坐标值中沿x，y方向距离最大的值为全局地图原点坐标(ox，oy)。

ox＝min(ox₁,ox₁₂) (4)

oy＝min(oy₁,oy₁₂) (5)

获得全局原点坐标后，可以求得每个子地图右下角在机器人A坐标系中坐标，将两个坐标值比较，选择每个方向最大值作为全局地图的右下角坐标值，由此计算出全局地图大小(heigh，width)。创建两个全局地图大小的空白地图。在同一坐标系下，由于我们建立的是二维平面栅格地图，根据全局地图原点坐标与机器人局部地图左上角坐标，我们可以推导出机器人局部子地图到全局地图的像素坐标平移矩阵T₁和T₂(每个像素格子大小为0.05m)：

我们将平移矩阵乘以局部子地图每个像素坐标值，可以得到在全局地图上的像素坐标位置，创建两个全局地图坐标系下的局部子地图，我们比较某点在两幅不同地图中的像素值，当两点像素值不一样时按照以下准则来计算此点状态：

(1)s1＝障碍，s2＝空闲，ss＝障碍

(2)s1＝障碍，s2＝未知，ss＝障碍

(3)s1＝空闲，s2＝未知，ss＝空闲

(4)s1＝空闲，s2＝障碍，ss＝障碍

(5)s1＝未知，s2＝障碍，ss＝障碍

(6)s1＝未知，s2＝空闲，ss＝空闲

其中s1是此点在map1中的像素值，s2是此点在map1中的像素值，ss是我们计算得到的像素值。

Claims (7)

1.一种集群机器人建图系统，其特征在于，包括：

集群机器人，各机器人构建局部环境地图，并将局部环境地图经通信网络发送至控制中心；

通信网络，包括由多个路由器搭建的局域网；

控制中心，用于对接收到的局部环境地图解压，并利用各机器人初始位姿和局部环境地图解压数据融合局部环境地图，得到全局地图；

所述控制中心获得全局地图的具体实现过程包括：

S1、对于机器人A和机器人B，将机器人B的左上角真实坐标值转化为机器人A坐标系中的坐标，其中机器人A坐标系和机器人B坐标系均分别以机器人初始位置为原点，前进方向为Y轴负方向，原地右转前进方向为X轴正方向；将机器人B的坐标在xy方向上分别加上真实初始位姿坐标值，从而将机器人A和机器人B的局部环境地图左上角坐标值转化为机器人A坐标系下的坐标值；其中，机器人A和机器人B相邻；

S2、在机器人A坐标系下，根据机器人A和机器人B的左上角坐标点位置，确定两个坐标值中沿x，y方向距离最大的值为全局地图原点在机器人A坐标系中的坐标(ox，oy)；

S3、获取机器人A和机器人B局部环境地图右下角在机器人A坐标系下坐标，将机器人A和机器人B局部环境地图右下角在机器人A坐标系中坐标值的x和y值分别进行比较，分别选择两个坐标值中x方向和y方向最大值作为全局地图的右下角x和y坐标值，计算出全局地图大小(heigh，width)；

S4、创建两个全局地图大小的空白地图，根据机器人A和机器人B局部环境地图左上角坐标与全局地图原点坐标(ox，oy)的相互关系，推导出机器人A和机器人B的局部环境地图和全局地图的转换关系，即转换矩阵分别为T₁和T₂，利用T₁和T₂分别对机器人A和机器人B的局部环境地图对应进行转换；

S5、根据融合准则融合转换后的两幅地图，得到全局地图。

2.根据权利要求1所述的集群机器人建图系统，其特征在于，所述通信网络传输局部环境地图的通信格式为(height，width，d1，a[]，origini)；其中(height，width)为局部环境地图的高度和宽度，d1为局部环境地图数据长度，a[]为局部环境地图数据，origini＝(ox_i,oy_i)为局部环境地图左上角在机器人坐标系中坐标值，i为机器人编号。

3.根据权利要求1所述的集群机器人建图系统，其特征在于，各机器人并排设置。

4.根据权利要求1所述的集群机器人建图系统，其特征在于，步骤S4中，转换矩阵为：

其中(ox₁，oy₁)为机器人A局部环境地图在机器人A坐标系下的左上角坐标，(ox₁₂，oy₁₂)为机器人B局部环境地图在机器人A坐标系下的左上角坐标。

5.根据权利要求1所述的集群机器人建图系统，其特征在于，步骤S5中，

融合准则为，

s1＝障碍，s2＝空闲，ss＝障碍；

s1＝障碍，s2＝未知，ss＝障碍；

s1＝空闲，s2＝未知，ss＝空闲；

s1＝空闲，s2＝障碍，ss＝障碍；

s1＝未知，s2＝障碍，ss＝障碍；

s1＝未知，s2＝空闲，ss＝空闲；

其中，s1为机器人A局部环境地图转换后得到的地图中的任一位置的像素点状态，s2为机器人B局部环境地图转换后得到的地图对应s1的位置的像素点状态，ss为融合后得到的地图对应s1的位置的像素点状态。

6.一种集群机器人建图方法，其特征在于，包括以下步骤：

1)各机器人构建局部环境地图；

2)利用各机器人初始位姿和局部环境地图解压数据融合局部环境地图，得到全局地图；

步骤2)的具体实现过程包括：

S1、对于机器人A和机器人B，将机器人B的左上角真实坐标值转化为机器人A坐标系中的坐标，其中机器人A坐标系和机器人B坐标系分别以机器人初始位置为原点，前进方向为Y轴负方向，原地右转前进方向为X轴正方向；将机器人B的坐标在xy方向上分别加上真实初始位姿坐标值，从而将机器人A和机器人B的局部环境地图左上角坐标值转化为机器人A坐标系下的坐标值；其中，机器人A和机器人B相邻；

S2、在机器人A坐标系下，根据机器人A和机器人B的左上角坐标点位置，确定两个坐标值中沿x，y方向距离最大的值为全局地图原点在机器人A坐标系中的坐标(ox，oy)；

S3、获取机器人A和机器人B局部环境地图右下角在机器人A坐标系中的坐标，将机器人A和机器人B局部环境地图右下角在机器人A坐标系中坐标值的x和y值分别进行比较，分别选择两个坐标值中x方向和y方向最大值作为全局地图的右下角x和y坐标值，计算出全局地图大小(heigh，width)；

S4、创建两个全局地图大小的空白地图，根据机器人A和机器人B局部环境地图左上角坐标与全局地图原点坐标(ox，oy)的相互关系，推导出机器人A和机器人B的局部环境地图和全局地图的转换关系，即转换矩阵分别为T₁和T₂，利用T₁和T₂分别对机器人A和机器人B的局部环境地图对应进行转换；

S5、根据融合准则融合转换后的两幅地图，得到全局地图。

7.根据权利要求6所述的集群机器人建图方法，其特征在于，步骤S5中，融合准则为，

s1＝障碍，s2＝空闲，ss＝障碍；

s1＝障碍，s2＝未知，ss＝障碍；

s1＝空闲，s2＝未知，ss＝空闲；

s1＝空闲，s2＝障碍，ss＝障碍；

s1＝未知，s2＝障碍，ss＝障碍；

s1＝未知，s2＝空闲，ss＝空闲；

其中，s1为机器人A局部环境地图转换后得到的地图中的任一位置的像素点状态，s2为机器人B局部环境地图转换后得到的地图对应s1的位置的像素点状态，ss为融合后得到的地图对应s1的位置的像素点状态。

Images