CN111830985A - 一种多机器人定位方法、系统及集中式通信系统 - Google Patents
一种多机器人定位方法、系统及集中式通信系统 Download PDFInfo
- Publication number
- CN111830985A CN111830985A CN202010723850.XA CN202010723850A CN111830985A CN 111830985 A CN111830985 A CN 111830985A CN 202010723850 A CN202010723850 A CN 202010723850A CN 111830985 A CN111830985 A CN 111830985A
- Authority
- CN
- China
- Prior art keywords
- map
- local grid
- robot
- maps
- grid maps
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 45
- 238000004891 communication Methods 0.000 title claims abstract description 12
- 230000004927 fusion Effects 0.000 claims abstract description 38
- 239000011159 matrix material Substances 0.000 claims description 31
- 239000013598 vector Substances 0.000 claims description 25
- 238000004422 calculation algorithm Methods 0.000 claims description 22
- 230000009466 transformation Effects 0.000 claims description 19
- PXFBZOLANLWPMH-UHFFFAOYSA-N 16-Epiaffinine Natural products C1C(C2=CC=CC=C2N2)=C2C(=O)CC2C(=CC)CN(C)C1C2CO PXFBZOLANLWPMH-UHFFFAOYSA-N 0.000 claims description 17
- 230000003068 static effect Effects 0.000 claims description 13
- 238000013519 translation Methods 0.000 claims description 11
- 238000000605 extraction Methods 0.000 claims description 10
- 238000012545 processing Methods 0.000 claims description 9
- 238000007499 fusion processing Methods 0.000 claims description 5
- 238000010276 construction Methods 0.000 claims description 3
- 239000000284 extract Substances 0.000 abstract description 2
- 239000002245 particle Substances 0.000 description 25
- 230000003044 adaptive effect Effects 0.000 description 4
- 230000004807 localization Effects 0.000 description 4
- 230000033001 locomotion Effects 0.000 description 4
- 238000013507 mapping Methods 0.000 description 4
- 230000000694 effects Effects 0.000 description 3
- 238000009499 grossing Methods 0.000 description 3
- 238000011160 research Methods 0.000 description 3
- 238000012216 screening Methods 0.000 description 3
- 241000233855 Orchidaceae Species 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 2
- 238000001514 detection method Methods 0.000 description 2
- 230000010339 dilation Effects 0.000 description 2
- 239000000428 dust Substances 0.000 description 2
- 238000001914 filtration Methods 0.000 description 2
- 238000000746 purification Methods 0.000 description 2
- 244000291564 Allium cepa Species 0.000 description 1
- 238000012952 Resampling Methods 0.000 description 1
- 230000004888 barrier function Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000004883 computer application Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000003708 edge detection Methods 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 238000010191 image analysis Methods 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 238000007500 overflow downdraw method Methods 0.000 description 1
- 230000001902 propagating effect Effects 0.000 description 1
- 230000003252 repetitive effect Effects 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0234—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using optical markers or beacons
- G05D1/0236—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using optical markers or beacons in combination with a laser
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0238—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors
- G05D1/024—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors in combination with a laser
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Aviation & Aerospace Engineering (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Electromagnetism (AREA)
- Optics & Photonics (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Multimedia (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种多机器人定位方法、系统及集中式通信系统，系统内各独立机器人利用自身携带的激光雷达探测周围环境，完成各子区域环境地图的构建，并将子区域栅格地图发送给终端计算机。终端计算机接收到子区域栅格地图后，首先对子区域栅格地图进行空间关系特征的提取，确定子区域栅格地图之间的重合部分。其次，通过对子区域栅格地图重合部分的ORB特征进行提取匹配，计算最优匹配点，设定融合比例进行子区域栅格地图的融合，得到全局地图。然后，通过amcl自适应蒙特卡罗定位方法对机器人进行定位，最后，在原有地图基础上，加入特征地图层，为路径规划提供便利。
Description
技术领域
本发明涉及多机器人协同建图领域，特别是一种多机器人地图融合定位、定位与特征地图构建方法。
背景技术
在自主移动机器人领域，现阶段的研究热点是如何在GPS受到抑制的情况下进行同时定位与地图构建(Simultaneous Localization And Mapping，SLAM)，单机器人SLAM研究已经较为深入，然而当转移到多机器人系统时，SLAM又面临很多新的挑战。地图融合算法是解决多机器人SLAM的一个方向，它将多机器人系统中各机器人构建的局部地图(子地图)通过融合构建成一张全局地图，通常分为2步：1)各子地图之间对准信息的匹配；2)通过整合对准信息求得旋转矩阵和平移矢量，达到地图融合。一般通过重复区域或位姿对子地图之间的对准信息进行查找，从而解算出地图之间的转换关系。目前，多机器人地图融合主要从各机器人初始位姿、多机器人会合、机器人间相对定位和子地图间重复区域 4个方面进行研究。由于基于子地图间重复区域的地图融合算法不需要多机器人之间实时满足视线内观测条件，也不需要机器人会合，因此得到了广泛应用。
目前，多机器人协同建图方法还处于研究阶段，仍然存在一些问题有待解决，比如在地图融合过程中，基于子地图间重复区域的地图融合算法随着局部地图的增大，特征点增多，耗时也大幅增加，仍需占用较多的数据处理资源，且当子地图之间存在较少的重复区域时可能会失效。
在机器人导航过程中，机器人的定位是非常重要的，如果无法正确定位机器人当前位置，那么基于错误的起始点来进行后面规划的到达目的地的路径必定也是错误的，所以采用amcl定位方法来对机器人进行精准定位是非常必要的。同时，由于在实际的导航任务中，光有一张地图是不够的，机器人需要能动态的把障碍物加入，或者清楚已经不存在的障碍物，有些时候还要在地图上标出危险区域，所以在原有地图基础上加入特征地图，可以为之后的路径规划提供极大的便利。
申请号为201810184710.3的发明专利申请通过对图像进行特征提取匹配，同时求解机器人初始位姿，并进行位姿优化，根据优化后的位姿结合对应帧来生成点云地图，它只是对单机器人SLAM进行了优化，并未涉及多机器人地图融合与定位。申请号为201010262075.9的发明专利申请通过将待融合的三维几何地图投影成二维栅格地图，考虑三维地图的几何特征信息，结合二维地图融合和三维点集配准算法，完成了对多机器人三维地图融合的优化，但该专利申请并未考虑地图融合完成后的机器人定位问题。
发明内容
本发明所要解决的技术问题是，针对现有技术不足，提供一种多机器人定位方法、系统及集中式通信系统，实现多机器人的协同建图，缩短地图融合的整体时间，提高地图融合的准确率，对机器人进行精准定位。
为解决上述技术问题，本发明采用的技术方案是：一种多机器人定位方法，包括以下步骤：
1)构建集中式机器人通信系统，该通信系统中的多个机器人各自构建局部栅格地图；
2)提取各局部栅格地图的空间关系特征，确定两个相邻局部栅格地图的重合区域；
3)整合所有局部栅格地图的空间关系特征，并对局部栅格地图的重合区域进行ORB特征提取，对提取的ORB特征点进行匹配，得到匹配点对；
4)利用匹配点对获取两个局部栅格地图之间的旋转矩阵、尺度变量和平移矢量，计算最优偏仿射变换矩阵；
5)利用所述旋转矩阵、尺度变量、平移矢量和最优偏仿射变换矩阵，设置融合比例，对两个局部栅格地图进行融合；
6)重复步骤5)，直至融合完所有的局部栅格地图，得到全局地图；确定机器人在所述全局地图中的位置。
本发明通过对单机器人所构建的栅格地图进行融合，从而得到全局地图，实现多机器人的协同建图，在融合过程中使用基于空间关系特征的地图重合区域提取方法来对局部栅格地图的重合区域进行提取，从而缩短了地图融合的整体时间，提高了地图融合的准确率，并可以在得到全局地图后对机器人进行精准定位。
优选地，还包括：7)在所述全局地图的基础上加入特征地图，为路径规划提供了极大的便利。
所有的机器人均将局部栅格地图发送给终端计算机。
步骤1)中，所述机器人使用Hector-SLAM算法进行局部地图的构建。 Hector-SLAM算法使用高斯牛顿方法，不需要里程计数据，只根据激光雷达信息便可构建地图，所以在地面不平整的情况下仍然可以进行局部栅格地图的构建。
步骤2)中，通过对局部栅格地图的空间关系特征进行提取，从而确定两幅局部栅格地图的重合区域，为接下来的地图融合打下了基础，避免了子地图之间存在较少的重复区域时算法失效，提高了地图融合的准确率，其具体实现过程包括：
A)设有局部栅格地图m1和m2，根据局部栅格地图m1和m2的大小，将局部栅格地图m1和m2分别均匀划分为n个规则的子区域，分别对两个局部栅格地图的n个规则子区域进行空间关系特征提取，提取的特征向量分别为 F1,1,F1,2,…F1,n和F2,1,F2,2,…F2,n；
B)根据特征向量F1,1,F1,2,…F1,n和F2,1,F2,2,…F2,n对两个局部栅格地图的子区域进行匹配，根据匹配结果得到两个局部栅格地图的重合区域。
步骤3)中，获取匹配点对后，剔除Hanming距离大于第一设定阈值的ORB 特征点对，然后寻找两个局部栅格地图中距离差Δd相同的ORB特征点对，剔除距离差Δd大于第二设定阈值的ORB特征点对，最后利用RANSAC算法对剩余的匹配点对再次进行筛选，得到最优匹配点对。
使用三步提纯手段对获得的匹配点对进行筛选，减少了匹配效果较差的特征点对的同时，极大的提高了地图融合效果，从而得到较为准确的全局地图。
使用基于ORB特征的地图融合方法对两个局部栅格地图进行融合，ORB特征是目前最优秀的特征提取与匹配算法之一，它使用FAST算法来检测特征点，计算速度快，实时性高，采用BRIEF算法来计算特征点的描述子，节约了存储空间，缩短了匹配的时间。其具体实现过程包括：
i)将每两幅相邻的局部栅格地图分成一组，对每一组的两幅局部栅格地图重合区域的ORB特征进行提取，匹配两幅局部栅格地图的ORB特征，找到最优匹配点，并计算地图最优匹配点集的单应矩阵，找到最优匹配点集之间的最优仿射变换参数，根据尺度变量设置融合比例，从而在同一尺度下完成两幅局部栅格地图的融合；
ii)对于融合后的所有地图，重复步骤i)的融合过程，直至形成最终的全局地图。
最优偏仿射变换矩阵T的表达式为：
其中，φ代表局部栅格地图m1相较于地图局部栅格m2旋转的角度，s代表缩放比例，δx和δy代表局部栅格地图m1相较于局部栅格地图m2在x方向和y 方向上的偏移量。通过计算最优偏仿射变换矩阵来进行局部栅格地图的融合，提高了地图融合的准确率，保证了融合效果的有效性和鲁棒性。
步骤7)的具体实现过程包括：在全局地图的基础上加入静态地图层、障碍地图层和膨胀层，将所述全局地图拓展为多层结构，其中静态地图层为SLAM 建立完成的静态地图，障碍地图层用于动态的记录安装于机器人上的传感器感知到的障碍物信息，膨胀层用于处理机器人导航地图上的障碍物信息。通过特征地图的加入，使得地图上的障碍物信息可以动态更新，并且对这些障碍物进行了膨胀，避免了机器人与障碍物的碰撞，为之后的路径规划模块打下了基础，保证了机器人运动的安全性。
本发明还提供了一种集中式机器人通信系统，包括多个机器人、与所述多个机器人通信的终端计算机；所述终端计算机接收每个机器人构建的局部栅格地图后，进行如下处理：
1)提取各局部栅格地图的空间关系特征，确定两个相邻局部栅格地图的重合区域；
2)整合所有局部栅格地图的空间关系特征，并对局部栅格地图的重合区域进行ORB特征提取，对提取的ORB特征点进行匹配，得到匹配点对；
3)利用匹配点对获取两个局部栅格地图之间的旋转矩阵、尺度变量和平移矢量，计算最优偏仿射变换矩阵；
4)利用所述旋转矩阵、尺度变量、平移矢量和最优偏仿射变换矩阵，设置融合比例，对两个局部栅格地图进行融合；
5)重复步骤4)，直至融合完所有的局部栅格地图，得到全局地图；确定机器人在所述全局地图中的位置；
优选地，所述终端计算机在所述全局地图的基础上加入特征地图。
作为一个发明构思，本发明提供了一种多机器人定位系统，其包括计算机设备；所述计算机设备被配置或编程为执行本发明方法的步骤。
与现有技术相比，本发明所具有的有益效果为：本发明通过对单机器人所构建的栅格地图进行融合，从而得到全局地图，实现多机器人的协同建图，在融合过程中使用基于空间关系特征的地图重合区域提取方法来对局部栅格地图的重合区域进行提取，从而缩短了地图融合的整体时间，提高了地图融合的准确率。在得到全局地图后使用amcl定位方法对机器人进行精准定位，并在原有地图层的基础上加入特征地图层，为路径规划提供了极大的便利。
附图说明
图1为本发明实施例的实验场地图；
图2为本发明实施例的系统架构图；
图3为本发明实施例中两个机器人建立的局部栅格地图；
图4为基于ORB特征的地图融合算法流程图；
图5为本发明实施例中融合而成的全局地图；
图6为本发明实施例中机器人在地图中的定位；
图7为本发明实施例中加入特征地图层后的地图。
具体实施方式
本实施例由两台深兰福喜吸尘机器人和一台终端计算机组成，每台机器人均安装Ubuntu16.04系统和TIM561激光雷达，终端计算机安装Ubuntu16.04系统和开源图像处理库opencv，在如图1所示的场地中进行基于ORB特征的多机器人地图融合实验。
系统结构如图2所示，本发明包括以下步骤：
1)构建集中式的机器人通信系统，单个机器人使用Ubuntu16.04系统，终端计算机安装Ubuntu16.04系统和开源图像处理库opencv。
2)系统中的自主移动机器人采用漫游的形式，通过Hector-SLAM算法各自构建局部地图，并将数据传输到终端计算机。
3)当终端计算机接收到机器人所创建的局部栅格地图后，对局部栅格地图的空间关系特征进行提取，通过对提取的空间关系特征进行分析，找到两幅局部栅格地图的重合区域。
4)终端计算机整合各局部栅格地图的信息，并对局部栅格地图的重合区域进行ORB特征的提取，然后对提取的特征进行匹配。
5)在特征提取并找到匹配点对后，使用3步提纯手段对匹配点对进行筛选，得到优质匹配点对，即可求得两幅局部地图之间的旋转矩阵Rφ，尺度变量S和平移矢量t，之后根据特征点对计算最优偏仿射变换矩阵T。
6)利用步骤五中所求得的参数，通过设置融合比例，即可将两幅局部地图进行融合，得到全局地图。
7)使用AMCL(自适应蒙特卡洛定位)来确定机器人当前处于地图的什么位置，并将机器人实时显示在地图中。
8)在全局地图的基础上加入特征地图，即在原有全局地图层的基础上加入静态地图层，障碍地图层和膨胀层，从而使原有地图拓展为多层结构。
所述步骤1)中集中式机器人通信系统由两台深兰福喜吸尘机器人和一台笔记本电脑组成，每台机器人均安装Ubuntu16.04系统，ROS版本Kinetic和TIM561 激光雷达，笔记本电脑作为终端计算机，安装Ubuntu16.04系统，ROS版本Kinetic 和开源图像处理库opencv。使用终端计算机通过SSH网络协议远程登录机器人，向机器人发布指令，独立控制两个机器人。
步骤2)中，单个机器人使用Hector-SLAM算法进行局部地图的构建。Hector-SLAM算法使用高斯牛顿方法，不需要里程计数据，只根据激光信息便可构建地图。该算法假设地图模型为栅格地图，同时通过扫描匹配进行定位。在初始化阶段，激光传感器的第一帧数据直接进行构图，接下来的传感器数据与地图进行匹配，通过匹配推导出机器人最优位姿x,利用高斯-牛顿梯度法进行最优解的搜索求解(Kohlbrecher,Stefan,et al."A flexibleand scalable SLAM system with full 3D motion estimation."IEEE InternationalSymposium on Safety IEEE,2011.)。
两台机器人所建立的栅格地图m1和m2如图3所示。
所述步骤3)的具体步骤如下：
第一步，对局部栅格地图进行空间关系特征的提取，即根据栅格地图m1和 m2的大小将其均匀分为9个规则子区域，分别对两个局部栅格地图的9个规则子区域进行空间关系特征提取，提取的特征向量分别为F1,1,F1,2,…F1,9(栅格地图 m1)和F2,1,F2,2,…F2,9(栅格地图m2)。
子区域的特征向量可以通过检测二维小波变换的模极大值点确定，基本原理 (张宇伟.一种结合sobel算子和小波变换的图像边缘检测方法[J].计算机应用与软件,2017.)如下：
设有一副局部地图f(x,y)，在尺度为s时，其小波变换的两个分量定义为：
其中，θ(x,y)为高斯平滑函数，θ(x,y)的两个偏导数为：
二进制小波的定义为：
图像f(x,y)的平滑处理是由
完成的，信号平滑处理后得到的梯度矢量的两个分量，正好等于二维二进制小波变换的两个分量。
梯度矢量
的大小正比于：
A梯度矢量
的方向和水平方向的夹角为：
为了得到子区域的特征向量，可以沿着梯度矢量方向进行检测，由于梯度模局部极大值的方向和梯度矢量方向一致，最后得到的检测结果就是子区域的特征向量。
第二步，根据特征向量对两幅栅格地图所划分的9个规则子区域进行匹配，根据匹配结果得到两幅局部栅格地图的重合区域m3和m4。
步骤4)的具体步骤如下：
对m3和m4进行ORB特征的提取，得到oFAST特征点集合o1和o2，并将每个特征点依据描述子rBRIEF的方向向量化，用坐标表示为ki和kj。
其中基于ORB特征的地图融合算法的基本思路如下:首先提取栅格地图重合区域的ORB特征并找到最优的匹配点；然后计算地图最优匹配点集的单应矩阵，找到点集之间的最优仿射变换参数；最后完成子地图的融合。该算法在进行地图融合时，首先将各个机器人构建的局部地图每2幅分成一组进行融合，然后再将融合后的地图继续分组进行融合，直至形成最终的全局地图。
步骤5)中，由于ORB的rBRIEF描述子是基于BRIEF描述子改进后的二进制描述子，因此选择Hanming距离来匹配相似特征点。
当两个重合区域的特征点提取匹配完毕后，对提取的匹配特征点进行筛选。首先，对BruteForce匹配后的特征点进行提纯，采用基于阈值的方法，首先将 Hanming距离>30的特征点剔除，并找到2幅地图中距离差Δd相同的特征点对，定义为
为了求解2幅子地图的转换关系，即单应矩阵T，计算最终匹配的特征点对偏仿射变换，定义旋转矩阵Rφ、尺度变量S和平移矢量t分别如下:
t＝[δx δy]T
φ代表地图m1相较于地图m2旋转的角度，s代表缩放的比例，δx和δy代表地图m1相较于地图m2在x和y方向上的偏移量，使用estimateRigidTransform 函数对特征点对计算最优偏仿射变换矩阵T，如下：
本实例中旋转矩阵Rφ为：
平移矢量t＝[-0.020364 0.004739]
单应矩阵T为：
所述步骤6)的具体步骤如下：由于T中只有4个变量，只需输入2组匹配点对即可求解，因此使用最小二乘法求解最优变换，得到单应矩阵后，采用迭代方式计算最优解，之后根据得到的最优匹配点对将两幅局部地图联系起来，设定融合比例，将m1与m2融合，从而得到全局栅格地图，本实例中最后得到的全局地图如图5所示。
所述步骤7)的具体步骤如下：
首先运行机器人节点，并启动机器人驱动程序，发布激光雷达数据和里程计数据，然后对地图配置参数进行设置并对地图进行加载，最后启动amcl节点并运行rviz，地图和机器人就显示在了rviz中，如图6所示。
采用AMCL(adaptive Monte Carlo Localization)自适应蒙特卡洛定位方法来对机器人进行定位，amcl其实就是蒙特卡洛定位方法的一种升级版，使用自适应的KLD方法来更新粒子，而mcl(蒙特卡洛定位)法使用的是粒子滤波的方法来进行定位的。而粒子滤波就是一开始在地图空间很均匀的撒一把粒子，然后通过获取机器人的motion来移动粒子，比如机器人向前移动了一米，所有的粒子也就向前移动一米，不管现在这个粒子的位置对不对。使用每个粒子所处位置模拟一个传感器信息跟观察到的传感器信息(一般是激光)作对比，从而赋给每个粒子一个概率。之后根据生成的概率来重新生成粒子，概率越高的生成的概率越大。这样的迭代之后，所有的粒子会慢慢地收敛到一起，机器人的确切位置也就被推算出来了，其算法(Fox,Dieter,et al."Monte Carlo Localization:Efficient PositionEstimation for Mobile Robots."Of the National Conference on ArtificialIntelligence 1999.)步骤为：
第一步，在地图中对粒子群进行初始化，粒子可以有位置、方向或者其他需要估计的状态变量。每一个粒子都有一个权重(概率)，表示它与系统的实际状态匹配的可能性，并使用相同的权重初始化每个粒子。
第二步，利用系统模型模拟粒子运动，并在每一个粒子所处位置模拟一个传感器信息跟机器人所观察到的激光雷达信息作对比，其相似度越高，这个粒子与真实位置就越接近。
第三步，更新粒子的权重，使用测量数据来修正每个粒子的权值，保证与真实位置越接近的粒子获得的权值越大。
第四步，对粒子群进行重采样，舍弃权值小的粒子，保留权值大的粒子，并进行多次迭代。
第五步，经过多次迭代后，所有的粒子基本都收敛在一起，机器人的准确位置也就推算出来了。
步骤8)中，在原有地图基础上加入特征地图，也就是在原有地图基础上加入另外几层地图，分别为静态地图层(Static Map Layer)，障碍地图层(Obstacle Map Layer)和膨胀层(Inflation Layer)。其中静态地图层通常都是SLAM建立完成的静态地图，障碍地图层用于动态的记录传感器感知到的障碍物信息，膨胀层用于处理机器人导航地图上的障碍物信息膨胀(让地图上的障碍物比实际障碍物的大小更大一些)，尽可能使机器人更安全的移动。这三层组合成了master map (最终的特征地图)，供给路径规划模块使用。
特征地图采用栅格形式，每个网格在0～255范围内取值。网格分为三种状态：占用区域(occupied)、自由区域(free)和未知区域(unknown)。因此，在下层数据结构中仅需要3个值来表示每个网格的状态。并且特征地图以一定的周期进行地图更新。在每个周期内接收激光雷达数据后，在特征地图底层占用结构上执行标记和清除障碍操作。这种结构会被投影到特征地图并附上相应代价值。之后，对占用区域栅格执行障碍物膨胀操作，即从每个占用栅格向外传播代价值，直到达到定义的膨胀半径为止，从而对特征地图层进行更新。
其具体步骤如下：首先对Costmap初始化获得全局坐标系和机器人坐标系的转换(世界坐标系和机器人坐标系相对变化关系)，然后加载各个图层，也就是静态地图层(Static Map Layer)，障碍地图层(Obstacle Map Layer)和膨胀层 (Inflation Layer)，之后实例化一个Costmap2DPublisher来发布可视化数据，并开启动态参数配置服务，服务启动了更新地图的线程，最后的加入特征地图后的地图层如图7所示。
Claims (10)
1.一种多机器人定位方法，其特征在于，包括以下步骤：
1)构建集中式机器人通信系统，该通信系统中的多个机器人各自构建局部栅格地图；
2)提取各局部栅格地图的空间关系特征，确定两个相邻局部栅格地图的重合区域；
3)整合所有局部栅格地图的空间关系特征，并对局部栅格地图的重合区域进行ORB特征提取，对提取的ORB特征点进行匹配，得到匹配点对；
4)利用匹配点对获取两个局部栅格地图之间的旋转矩阵、尺度变量和平移矢量，计算最优偏仿射变换矩阵；
5)利用所述旋转矩阵、尺度变量、平移矢量和最优偏仿射变换矩阵，设置融合比例，对两个局部栅格地图进行融合；
6)重复步骤5)，直至融合完所有的局部栅格地图，得到全局地图；确定机器人在所述全局地图中的位置；
优选地，还包括：7)在所述全局地图的基础上加入特征地图。
2.根据权利要求1所述的多机器人定位方法，其特征在于，所有的机器人均将局部栅格地图发送给终端计算机。
3.根据权利要求1所述的多机器人定位方法，其特征在于，步骤1)中，所述机器人使用Hector-SLAM算法进行局部地图的构建。
4.根据权利要求1所述的多机器人定位方法，其特征在于，步骤2)的具体实现过程包括：
A)设有局部栅格地图m1和m2，根据局部栅格地图m1和m2的大小，将局部栅格地图m1和m2分别均匀划分为n个规则的子区域，分别对两个局部栅格地图的n个规则子区域进行空间关系特征提取，提取的特征向量分别为F1,1,F1,2,…F1,n和F2,1,F2,2,…F2,n；
B)根据特征向量F1,1,F1,2,…F1,n和F2,1,F2,2,…F2,n对两个局部栅格地图的子区域进行匹配，根据匹配结果得到两个局部栅格地图的重合区域。
5.根据权利要求1所述的多机器人定位方法，其特征在于，步骤3)中，获取匹配点对后，剔除Hanming距离大于第一设定阈值的ORB特征点对，然后寻找两个局部栅格地图中距离差Δd相同的ORB特征点对，剔除距离差Δd大于第二设定阈值的ORB特征点对，最后利用RANSAC算法对剩余的匹配点对再次进行筛选，得到最优匹配点对。
6.根据权利要求1所述的多机器人定位方法，其特征在于，融合两个局部栅格地图的具体实现过程包括：
i)将每两幅相邻的局部栅格地图分成一组，对每一组的两幅局部栅格地图重合区域的ORB特征进行提取，匹配两幅局部栅格地图的ORB特征，找到最优匹配点，并计算地图最优匹配点集的单应矩阵，找到最优匹配点集之间的最优仿射变换参数，根据尺度变量设置融合比例，从而在同一尺度下完成两幅局部栅格地图的融合；
ii)对于融合后的所有地图，重复步骤i)的融合过程，直至形成最终的全局地图。
7.根据权利要求1所述的多机器人定位方法，其特征在于，最优偏仿射变换矩阵T的表达式为：
其中，φ代表局部栅格地图m1相较于地图局部栅格m2旋转的角度，s代表缩放比例，δx和δy代表局部栅格地图m1相较于局部栅格地图m2在x方向和y方向上的偏移量。
8.根据权利要求1所述的多机器人定位方法，其特征在于，步骤7)的具体实现过程包括：在全局地图的基础上加入静态地图层、障碍地图层和膨胀层，将所述全局地图拓展为多层结构，其中静态地图层为SLAM建立完成的静态地图，障碍地图层用于动态的记录安装于机器人上的传感器感知到的障碍物信息，膨胀层用于处理机器人导航地图上的障碍物信息。
9.一种集中式机器人通信系统，其特征在于，包括多个机器人、与所述多个机器人通信的终端计算机；所述终端计算机接收每个机器人构建的局部栅格地图后，进行如下处理：
1)提取各局部栅格地图的空间关系特征，确定两个相邻局部栅格地图的重合区域；
2)整合所有局部栅格地图的空间关系特征，并对局部栅格地图的重合区域进行ORB特征提取，对提取的ORB特征点进行匹配，得到匹配点对；
3)利用匹配点对获取两个局部栅格地图之间的旋转矩阵、尺度变量和平移矢量，计算最优偏仿射变换矩阵；
4)利用所述旋转矩阵、尺度变量、平移矢量和最优偏仿射变换矩阵，设置融合比例，对两个局部栅格地图进行融合；
5)重复步骤4)，直至融合完所有的局部栅格地图，得到全局地图；确定机器人在所述全局地图中的位置；
优选地，所述终端计算机在所述全局地图的基础上加入特征地图。
10.一种多机器人定位系统，其特征在于，包括计算机设备；所述计算机设备被配置或编程为执行权利要求1～8之一所述方法的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010723850.XA CN111830985A (zh) | 2020-07-24 | 2020-07-24 | 一种多机器人定位方法、系统及集中式通信系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010723850.XA CN111830985A (zh) | 2020-07-24 | 2020-07-24 | 一种多机器人定位方法、系统及集中式通信系统 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN111830985A true CN111830985A (zh) | 2020-10-27 |
Family
ID=72925402
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010723850.XA Pending CN111830985A (zh) | 2020-07-24 | 2020-07-24 | 一种多机器人定位方法、系统及集中式通信系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111830985A (zh) |
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113084822A (zh) * | 2021-04-30 | 2021-07-09 | 广东美房智高机器人有限公司 | 机器人位置显示方法、系统、设备及介质 |
| CN114383622A (zh) * | 2021-12-27 | 2022-04-22 | 广州视源电子科技股份有限公司 | 机器人定位方法、机器人及计算机可读存储介质 |
| CN115014352A (zh) * | 2022-06-01 | 2022-09-06 | 浙江大学 | 一种基于建议分布地图的室内全局定位方法 |
| CN115139300A (zh) * | 2022-07-01 | 2022-10-04 | 北京盈迪曼德科技有限公司 | 云端服务器、机器人、多机管理系统及多机管理方法 |
| CN117723048A (zh) * | 2023-12-18 | 2024-03-19 | 哈尔滨工业大学 | 一种通信受限下的多机器人压缩通信协同建图方法及系统 |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104180799A (zh) * | 2014-07-15 | 2014-12-03 | 东北大学 | 一种基于自适应蒙特卡罗定位的机器人定位方法 |
| CN108227717A (zh) * | 2018-01-30 | 2018-06-29 | 中国人民解放军陆军装甲兵学院 | 基于orb特征的多移动机器人地图融合方法及融合平台 |
| CN108303986A (zh) * | 2018-03-09 | 2018-07-20 | 哈工大机器人（昆山）有限公司 | 一种激光slam导航的临时障碍物处理方法 |
| CN109682382A (zh) * | 2019-02-28 | 2019-04-26 | 电子科技大学 | 基于自适应蒙特卡洛和特征匹配的全局融合定位方法 |
-
2020
- 2020-07-24 CN CN202010723850.XA patent/CN111830985A/zh active Pending
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104180799A (zh) * | 2014-07-15 | 2014-12-03 | 东北大学 | 一种基于自适应蒙特卡罗定位的机器人定位方法 |
| CN108227717A (zh) * | 2018-01-30 | 2018-06-29 | 中国人民解放军陆军装甲兵学院 | 基于orb特征的多移动机器人地图融合方法及融合平台 |
| CN108303986A (zh) * | 2018-03-09 | 2018-07-20 | 哈工大机器人（昆山）有限公司 | 一种激光slam导航的临时障碍物处理方法 |
| CN109682382A (zh) * | 2019-02-28 | 2019-04-26 | 电子科技大学 | 基于自适应蒙特卡洛和特征匹配的全局融合定位方法 |
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113084822A (zh) * | 2021-04-30 | 2021-07-09 | 广东美房智高机器人有限公司 | 机器人位置显示方法、系统、设备及介质 |
| CN114383622A (zh) * | 2021-12-27 | 2022-04-22 | 广州视源电子科技股份有限公司 | 机器人定位方法、机器人及计算机可读存储介质 |
| CN114383622B (zh) * | 2021-12-27 | 2024-04-19 | 广州视源电子科技股份有限公司 | 机器人定位方法、机器人及计算机可读存储介质 |
| CN115014352A (zh) * | 2022-06-01 | 2022-09-06 | 浙江大学 | 一种基于建议分布地图的室内全局定位方法 |
| CN115139300A (zh) * | 2022-07-01 | 2022-10-04 | 北京盈迪曼德科技有限公司 | 云端服务器、机器人、多机管理系统及多机管理方法 |
| CN117723048A (zh) * | 2023-12-18 | 2024-03-19 | 哈尔滨工业大学 | 一种通信受限下的多机器人压缩通信协同建图方法及系统 |
| CN117723048B (zh) * | 2023-12-18 | 2024-07-19 | 哈尔滨工业大学 | 一种通信受限下的多机器人压缩通信协同建图方法及系统 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111830985A (zh) | 一种多机器人定位方法、系统及集中式通信系统 | |
| Chang et al. | Kimera-multi: a system for distributed multi-robot metric-semantic simultaneous localization and mapping | |
| JP6896077B2 (ja) | 車両自動パーキングシステム及び方法 | |
| Cunningham et al. | Fully distributed scalable smoothing and mapping with robust multi-robot data association | |
| Weng et al. | Pole-based real-time localization for autonomous driving in congested urban scenarios | |
| WO2017028653A1 (zh) | 一种移动机器人室内自建地图的方法和系统 | |
| CN109186606B (zh) | 一种基于slam和图像信息的机器人构图及导航方法 | |
| Wang et al. | A simple and parallel algorithm for real-time robot localization by fusing monocular vision and odometry/AHRS sensors | |
| CN103389699B (zh) | 基于分布式智能监测控制节点的机器人监控及自主移动系统的运行方法 | |
| Sudhakara et al. | Trajectory planning of a mobile robot using enhanced A-star algorithm | |
| CN111609852A (zh) | 语义地图构建方法、扫地机器人及电子设备 | |
| CN106548486A (zh) | 一种基于稀疏视觉特征地图的无人车位置跟踪方法 | |
| CN111679661A (zh) | 基于深度相机的语义地图构建方法及扫地机器人 | |
| CN113778096B (zh) | 室内机器人的定位与模型构建方法及系统 | |
| Garrote et al. | 3D point cloud downsampling for 2D indoor scene modelling in mobile robotics | |
| CN112857370A (zh) | 一种基于时序信息建模的机器人无地图导航方法 | |
| Gimenez et al. | Optimization methodology to fruit grove mapping in precision agriculture | |
| Vutetakis et al. | An autonomous loop-closure approach for simultaneous exploration and coverage of unknown infrastructure using mavs | |
| Pang et al. | A Low-Cost 3D SLAM System Integration of Autonomous Exploration Based on Fast-ICP Enhanced LiDAR-Inertial Odometry | |
| Achakir et al. | Non-Model-Based approach for complete digitization by TLS or mobile scanner | |
| Liu et al. | Processed RGB-D slam using open-source software | |
| Sun et al. | Online map fusion system based on sparse point-cloud | |
| Baligh Jahromi et al. | Layout slam with model based loop closure for 3d indoor corridor reconstruction | |
| Szendy et al. | Simultaneous localization and mapping with TurtleBotII | |
| Li et al. | Partial computing offloading assisted cloud point registration in multi-robot slam |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20201027 |
|
| RJ01 | Rejection of invention patent application after publication |