CN113670290B - 一种基于多机器人协作的移动机器人室内地图构建方法 - Google Patents

一种基于多机器人协作的移动机器人室内地图构建方法 Download PDF

Info

Publication number
CN113670290B
CN113670290B CN202110741481.1A CN202110741481A CN113670290B CN 113670290 B CN113670290 B CN 113670290B CN 202110741481 A CN202110741481 A CN 202110741481A CN 113670290 B CN113670290 B CN 113670290B
Authority
CN
China
Prior art keywords
robot
pose
point cloud
uwb
laser
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.)
Active
Application number
CN202110741481.1A
Other languages
English (en)
Other versions
CN113670290A (zh
Inventor
刘冉
陈凯翔
张华�
肖宇峰
邓忠元
曹志强
秦正泓
霍建文
刘桂华
徐锋
张静
刘满禄
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Southwest University of Science and Technology
Original Assignee
Southwest University of Science and Technology
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Southwest University of Science and Technology filed Critical Southwest University of Science and Technology
Priority to CN202110741481.1A priority Critical patent/CN113670290B/zh
Publication of CN113670290A publication Critical patent/CN113670290A/zh
Application granted granted Critical
Publication of CN113670290B publication Critical patent/CN113670290B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G01C21/3833—Creation or updating of map data characterised by the source of data
    • G01C21/3841—Data obtained from two or more sources, e.g. probe vehicles
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G01C21/3807—Creation or updating of map data characterised by the type of data
    • G01C21/383—Indoor data
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种基于多机器人协作的移动机器人室内地图构建方法，包括以下步骤：S1：采集多机器人的里程信息、UWB节点之间的距离信息和激光扫描数据；S2：通过非线性优化算法得到多机器人的相对位姿，在此基础上融合里程计信息，得到优化后的多机器人轨迹数据；S3：构建激光闭环约束和构建位姿图，对多机器人的轨迹进一步优化，结合激光扫描数据构建地图。本发明中机器人通过编码器计算自身的里程信息；基于采集的多组UWB距离信息，通过非线性优化算法得到多机器人的相对位姿；融合里程计与UWB位姿估计数据提高定位精度；构建激光闭环约束，通过图优化算法对轨迹进一步约束，实现多机器人间更精确的位姿估算。

Description

一种基于多机器人协作的移动机器人室内地图构建方法
技术领域
本发明属于多机器人协作的地图高效精确构建技术领域，具体涉及一种基于多机器人协作的移动机器人室内地图构建方法。
背景技术
SLAM技术在室内环境地图构建上得到很好的应用，机器人通过携带传感器在运动过程中采集激光扫描数据和测量数据，通过对采集数据的处理实现对自己位姿的定位从而构建精确的地图。传统的地图构建往往使用摄像头或者激光扫描仪实现对环境特征点的采集，通过对特征点进行回环匹配，实现地图构建，计算开销很大，并且效率较低。在大型环境中可靠的SLAM仍然是机器人领域目前面临的一大挑战。
此外，单个机器人在大型环境的地图构建中效率受到明显限制，在大型环境中解决实际的建图问题较为困难，针对这一缺点，多机器人系统应运而生。相较于单个机器人在效率和能力方面的限制，多机器人系统可以通过群体之间协作的方式弥补单个机器人的缺点，能够很好的替代单个机器人实现大型环境地图的构建，提升工作效率。机器人搭载编码器可以得到自身的里程信息，里程计提供了机器人短时间内精确的位姿变化，但是长时间存在累计误差。基于多个机器人采集的多组UWB距离信息，通过非线性优化算法得到多机器人的位姿信息，融合非线性优化后的UWB定位信息能够纠正累积误差提供更精确的多机器人移动轨迹。每台机器人通过搭载激光雷达采集环境特征信息，机器人通过回环检测方式利用环境特征判定是否重新回到某一区域，从而对轨迹进一步约束，有效克服大型环境中环境距离的影响。在此基础上结合激光传感器数据进行精确的地图构建，解决了多机器人在大型环境下协作建图困难且精度不高的问题。
发明内容
本发明的目的是为了解决多机器人在大型环境下协作建图困难且精度不高的问题，提出了一种基于多机器人协作的移动机器人室内地图构建方法。
本发明的技术方案是：一种基于多机器人协作的移动机器人室内地图构建方法包括以下步骤：
S1：采集多机器人的里程信息、UWB节点之间的距离信息和激光扫描数据；
S2：基于UWB节点之间的距离信息，利用非线性优化算法计算多机器人的相对位姿，融合里程计信息，得到优化后的多机器人轨迹数据；
S3：根据激光扫描数据构建激光闭环约束，结合步骤S2优化后的多机器人轨迹数据构建位姿图，通过图优化算法对多机器人的轨迹进一步优化，结合激光扫描数据构建地图。
进一步地，步骤S1中，采集多机器人的里程信息的具体方法为：将里程计搭载在机器人上采集里程信息；
采集UWB节点之间的距离信息的具体方法为：在每个机器人的不同位置搭载UWB标签，利用UWB标签采集UWB节点之间的距离信息；
采集激光扫描数据的具体方法为：将激光传感器搭载在机器人上采集激光扫描数据。
进一步地，步骤S2中，通过高斯迭代得到最佳的多机器人相对位姿，使UWB节点之间距离测量值
Figure BDA0003141537950000021
与距离估计值
Figure BDA0003141537950000022
的误差最小，其多机器人的相对位姿
Figure BDA0003141537950000023
的计算公式为：
Figure BDA0003141537950000024
其中，argmin(·)表示最小值运算，x表示机器人i和机器人j间的相对位姿，(x,y)表示机器人i相对机器人j的位置坐标，θ表示机器人i相对于机器人j的方向角度，
Figure BDA0003141537950000031
表示在t时刻机器人i上UWB节点k和机器人j上UWB节点l之间的实际测量值，
Figure BDA0003141537950000032
表示给定机器人i与机器人j相对位姿的情况下机器人i上UWB节点k和机器人j上UWB节点l之间的距离，
Figure BDA0003141537950000033
表示机器人i上的UWB节点k的相对位置，
Figure BDA0003141537950000034
表示机器人j上的UWB节点l的相对位置；
步骤S2中，将机器人的位姿作为顶点，不同时刻的估计位姿作为约束，包括基于非线性优化后UWB的相对位姿约束和里程计中相邻时刻机器人的位姿变换约束，通过图优化算法求取使约束最小情况下的多机器人轨迹数据。
进一步地，步骤S3包括以下子步骤：
S31：根据激光传感器采集的激光扫描数据，利用正态分布变换配准算法构建激光闭环约束；
S32：基于优化后的多机器人轨迹数据和激光闭环约束，构建位姿图；根据位姿图，利用图优化算法对多机器人轨迹进行再优化；并结合激光扫描数据构建室内栅格地图，完成基于多机器人协作的移动机器人室内地图构建。
进一步地，步骤S31包括以下子步骤：
S331：将激光扫描数据的点云划分为源点云Q和参考点云R，并将参考点云R划分为若干立方体，并计算每个立方体内点云位置平均值
Figure BDA0003141537950000035
和协方差矩阵V，其中，Q＝{q1,q2,…,qn}，R＝{r1,r2…rn}，qi,i＝1,2,...,n表示源点云数据，ri,i＝1,2,...,n表示源点云数据，n表示点云数据个数；
S332：将源点云Q旋转平移得到新的点云M＝{m1,m2,…mn}，确定新的点云M中点mi所在参考点云R对应的立方体，并计算点mi在参考点云R中的概率p(mi)，其计算公式为：
Figure BDA0003141537950000041
其中，mi,i＝1,2,...,n表示新的点云数据，exp[·]表示指数运算；
S333：根据点mi在参考点云R中的概率p(mi)计算新的点云M中所有点在参考点云R中的概率密度之和s(P)，其计算公式为：
Figure BDA0003141537950000042
其中，P表示旋转平移矩阵，T(P,qk)表示当前扫描点qk经过旋转平移后得到的点mk；
S334：利用高斯牛顿迭代法计算使s(P)最小的旋转平移矩阵
Figure BDA0003141537950000043
将
Figure BDA0003141537950000044
代入源点云Q中，作为新的点云M，并返回步骤S32，直至新的点云M中所有点在参考点云R中的概率密度之和s(P)小于设定阈值，完成激光闭环约束构建。
进一步地，步骤S32中，利用图优化算法对多机器人轨迹进行再优化的具体方法为：把多机器人的位姿作为顶点，不同时刻的约束条件作为边，并利用图优化算法计算约束条件最小情况下的位姿，其中，所述约束条件包括多机器人里程计约束、多组UWB距离数据非线性优化后的多机器人相对位姿约束和激光闭环约束；约束最小情况下的位姿x*的计算公式为：
Figure BDA0003141537950000045
其中，argmin(·)表示最小值运算，x表示机器人i和机器人j间的相对位姿，a∈[1,…,N]，N表示机器人个数，T表示时长，
Figure BDA0003141537950000046
表示机器人a在t时刻基于里程计信息的位姿估计，
Figure BDA0003141537950000047
表示机器人a在t-1时刻的位姿，
Figure BDA0003141537950000048
机器人a在t时刻的位姿，
Figure BDA0003141537950000051
表示机器人a在t-1到t时刻里程计相对测量位姿的预测值，
Figure BDA0003141537950000052
表示机器人a在t-1至t时刻里程计测量值的信息矩阵，
Figure BDA0003141537950000053
表示多机器人的相对位姿，
Figure BDA0003141537950000054
表示机器人i在t时刻的位姿，
Figure BDA0003141537950000055
表示机器人j在t时刻的位姿，
Figure BDA0003141537950000056
表示机器人i和机器人j间UWB测量位姿的预测值，
Figure BDA0003141537950000057
表示机器人i和机器人j之间在t时刻UWB位姿估计的信息矩阵，
Figure BDA0003141537950000058
表示机器人i在tα时刻和机器人j在tβ时刻的激光点云NDT配准算法收敛后的得分值小于阈值γ，
Figure BDA0003141537950000059
表示机器人i在tα时刻和机器人j在tβ时刻基于NDT激光点云配准算法的位姿变换，
Figure BDA00031415379500000510
表示机器人i在tα时刻的位姿，
Figure BDA00031415379500000511
表示机器人j在tβ时刻的位姿，
Figure BDA00031415379500000512
表示机器人i在tα时刻和机器人j在tβ时刻激光雷达NDT配准位姿的预测值，
Figure BDA00031415379500000513
表示机器人i和机器人j之间在tα到tβ时刻激光闭环估计的信息矩阵。
本发明的有益效果是：
(1)本发明提出了一种基于多机器人协作的地图高效精确构建方法，机器人通过编码器计算自身的里程信息；基于采集的多组UWB距离信息，通过非线性优化算法得到多机器人的相对位姿；融合里程计与UWB位姿估计数据提高定位精度；通过所携带的激光雷达对环境进行扫描，构建激光闭环约束，通过图优化算法对轨迹进一步约束，实现多机器人间更精确的位姿估算。
(2)本发明利用多组UWB数据可以解决机器人间的相对定位，但由于UWB数据受环境多径干扰，定位误差较大；里程计虽然长时间存在累积误差，但是在短时间内精度很高，因此融合短时间内的里程计信息可以提高定位精度。
(3)每台机器人通过搭载激光雷达采集环境特征信息，机器人通过回环检测方式利用环境特征判定是否重新回到某一区域，从而对轨迹进一步约束，实现更高的定位精度。在此基础上结合激光传感器数据进行精确的地图构建，解决了多个机器人在大型环境中地图构建效率不高，精度较差的问题。
附图说明
图1为移动机器人室内地图构建方法的流程图。
图2为移动机器人室内地图构建方法的示意图。
具体实施方式
下面结合附图对本发明的实施例作进一步的说明。
在描述本发明的具体实施例之前，为使本发明的方案更加清楚完整，首先对本发明中出现的缩略语和关键术语定义进行说明：
UWB：一种使用1GHz以上频率带宽的无线载波通信技术。
如图1所示，本发明提供了一种基于多机器人协作的移动机器人室内地图构建方法，包括以下步骤：
S1：采集多机器人的里程信息、UWB节点之间的距离信息和激光扫描数据；
S2：基于UWB节点之间的距离信息，利用非线性优化算法计算多机器人的相对位姿，融合里程计信息，得到优化后的多机器人轨迹数据；
S3：根据激光扫描数据构建激光闭环约束，结合步骤S2优化后的多机器人轨迹数据构建位姿图，通过图优化算法对多机器人的轨迹进一步优化，结合激光扫描数据构建地图。
在本发明实施例中，步骤S1中，采集多机器人的里程信息的具体方法为：将里程计搭载在机器人上采集里程信息；
采集UWB节点之间的距离信息的具体方法为：在每个机器人的不同位置搭载UWB标签，利用UWB标签采集UWB节点之间的距离信息；
采集激光扫描数据的具体方法为：将激光传感器搭载在机器人上采集激光扫描数据。
在本发明实施例中，步骤S2中，通过高斯迭代得到最佳的多机器人相对位姿，使UWB节点之间距离测量值
Figure BDA0003141537950000071
与距离估计值
Figure BDA0003141537950000072
的误差最小，其多机器人的相对位姿
Figure BDA0003141537950000073
的计算公式为：
Figure BDA0003141537950000074
其中，argmin(·)表示最小值运算，x表示机器人i和机器人j间的相对位姿，(x,y)表示机器人i相对机器人j的位置坐标，θ表示机器人i相对于机器人j的方向角度，
Figure BDA0003141537950000075
表示在t时刻机器人i上UWB节点k和机器人j上UWB节点l之间的实际测量值，
Figure BDA0003141537950000076
表示给定机器人i与机器人j相对位姿的情况下机器人i上UWB节点k和机器人j上UWB节点l之间的距离，
Figure BDA0003141537950000077
表示机器人i上的UWB节点k的相对位置，
Figure BDA0003141537950000078
表示机器人j上的UWB节点l的相对位置；
步骤S2中，里程计虽然长时间存在累积误差，但是在短时间内精度很高，因此融合短时间内的里程计信息可以提高定位精度。将机器人的位姿作为顶点，不同时刻的估计位姿作为约束，包括基于非线性优化后UWB的相对位姿约束和里程计中相邻时刻机器人的位姿变换约束，通过图优化算法求取使约束最小情况下的多机器人轨迹数据。
UWB具有穿透力强、功耗低和测距精度高的优点，但是UWB无法测得目标的方位信息，因此单个UWB难以实现目标的定位。基于采集的多组UWB节点之间的距离信息，通过非线性优化算法得到机器人i和机器人j之间的相对位姿
Figure BDA0003141537950000079
里程计虽然长时间存在累积误差，但是在短时间内精度很高，因此融合短时间内的里程计信息可以提高定位精度。把机器人的位姿作为顶点，不同时刻的估计位姿作为约束，包括基于非线性优化后UWB的位姿约束，里程计中相邻时刻机器人的位姿变换约束，通过图优化算法求取使约束最小情况下的位姿。
在本发明实施例中，步骤S3包括以下子步骤：
S31：根据激光传感器采集的激光扫描数据，利用正态分布变换配准算法构建激光闭环约束；
S32：基于优化后的多机器人轨迹数据和激光闭环约束，构建位姿图；根据位姿图，利用图优化算法对多机器人轨迹进行再优化；并结合激光扫描数据构建室内栅格地图，完成基于多机器人协作的移动机器人室内地图构建。
在本发明实施例中，步骤S31包括以下子步骤：
S331：将激光扫描数据的点云划分为源点云Q和参考点云R，并将参考点云R划分为若干立方体，并计算每个立方体内点云位置平均值r和协方差矩阵V，其中，Q＝{q1,q2,…,qn}，R＝{r1,r2…rn}，qi,i＝1,2,...,n表示源点云数据，ri,i＝1,2,...,n表示源点云数据，n表示点云数据个数；
S332：将源点云Q旋转平移得到新的点云M＝{m1,m2,…mn}，确定新的点云M中点mi所在参考点云R对应的立方体，并计算点mi在参考点云R中的概率p(mi)，其计算公式为：
Figure BDA0003141537950000081
其中，mi,i＝1,2,...,n表示新的点云数据，exp[·]表示指数运算；
S333：根据点mi在参考点云R中的概率p(mi)计算新的点云M中所有点在参考点云R中的概率密度之和s(P)，其计算公式为：
Figure BDA0003141537950000082
其中，P表示旋转平移矩阵，T(P,qk)表示当前扫描点qk经过旋转平移后得到的点mk；
S334：利用高斯牛顿迭代法计算使s(P)最小的旋转平移矩阵
Figure BDA0003141537950000091
将
Figure BDA0003141537950000092
代入源点云Q中，作为新的点云M，并返回步骤S32，直至新的点云M中所有点在参考点云R中的概率密度之和s(P)小于设定阈值，完成激光闭环约束构建。
激光闭环的检测采用NDT(正态分布变换)配准算法，因为其在配准过程中不需要搜索最邻近的匹配点，所以相对于ICP(迭代最接近点)算法有更好的精度和收敛速度。通过高斯牛顿迭代法求解使目标函数s(P)最小的旋转平移矩阵
Figure BDA0003141537950000093
其计算公式为：H(P)ΔP＝-g(P)，其中，H(P)表示s(P)的海瑟矩阵，g(P)表示s(P)的梯度。通过给定初值，将旋转平移矩阵P代入源点云Q中，得到新的一组点云M，再将M点云与参考点云R＝{r1,r2…rn}匹配求解新的旋转平移矩阵。多次迭代后s(P)趋于收敛，得到此时的源点云和参考点云的位置变换关系。当s(P)小于阈值γ时，算法中认为检测到了闭环。
在本发明实施例中，步骤S32中，对轨迹数据进行约束的具体方法为：根据激光扫描数据，利用回环检测方法确定多机器人重新回到的活动轨迹区域，完成对轨迹数据的约束；
步骤S32中，利用图优化算法对多机器人轨迹进行再优化的具体方法为：把多机器人的位姿作为顶点，不同时刻的约束条件作为边，并利用图优化算法计算约束条件最小情况下的位姿，其中，所述约束条件包括多机器人里程计约束、多组UWB距离数据非线性优化后的多机器人相对位姿约束和激光闭环约束；约束最小情况下的位姿x*的计算公式为：
Figure BDA0003141537950000094
其中，argmin(·)表示最小值运算，x表示机器人i和机器人j间的相对位姿，a∈[1,…,N]，N表示机器人个数，T表示时长，
Figure BDA0003141537950000101
表示机器人a在t时刻基于里程计信息的位姿估计，
Figure BDA0003141537950000102
表示机器人a在t-1时刻的位姿，
Figure BDA0003141537950000103
机器人a在t时刻的位姿，
Figure BDA0003141537950000104
表示机器人a在t-1到t时刻里程计相对测量位姿的预测值，
Figure BDA0003141537950000105
表示机器人a在t-1至t时刻里程计测量值的信息矩阵，
Figure BDA0003141537950000106
表示多机器人的相对位姿，
Figure BDA0003141537950000107
表示机器人i在t时刻的位姿，
Figure BDA0003141537950000108
表示机器人j在t时刻的位姿，
Figure BDA0003141537950000109
表示机器人i和机器人j间UWB测量位姿的预测值，
Figure BDA00031415379500001010
表示机器人i和机器人j之间在t时刻UWB位姿估计的信息矩阵，
Figure BDA00031415379500001011
表示机器人i在tα时刻和机器人j在tβ时刻的激光点云NDT配准算法收敛后的得分值小于阈值γ，
Figure BDA00031415379500001012
表示机器人i在tα时刻和机器人j在tβ时刻基于NDT激光点云配准算法的位姿变换，
Figure BDA00031415379500001013
表示机器人i在tα时刻的位姿，
Figure BDA00031415379500001014
表示机器人j在tβ时刻的位姿，
Figure BDA00031415379500001015
表示机器人i在tα时刻和机器人j在tβ时刻激光雷达NDT配准位姿的预测值，
Figure BDA00031415379500001016
表示机器人i和机器人j之间在tα到tβ时刻激光闭环估计的信息矩阵。
UWB测距虽然不受环境纹理的影响，但是UWB测量距离有限，当机器人距离较远时，UWB难以得到机器人间的位姿关系。每台机器人通过搭载激光雷达采集环境特征信息，机器人通过回环检测方式利用环境特征判定是否重新回到某一区域，从而对轨迹进一步约束。基于S2优化后的多机器人轨迹数据，结合激光扫描匹配算法引入额外激光-闭环约束，使用图优化算法通过优化位姿来使激光-闭环约束带来的误差最小，得到更高精度的多机器人轨迹数据，完成栅格地图的构建。把机器人的位姿作为顶点，不同时刻的约束条件作为边，包括多机器人里程计约束，多组UWB距离数据非线性优化后的多机器人位姿约束，激光-闭环约束，通过图优化算法求取使约束最小情况下的位姿。如图2所示，多机器人采用本发明的方法构建室内地图。
本发明的工作原理及过程为：本发明涉及一种基于多机器人协作的大型环境地图高效精确构建方法，本发明包括UWB、里程计与激光数据采集模块、基于UWB机器人相对位姿估计、里程计与UWB位姿估计数据融合优化、激光闭环检测与位姿进一步优化。利用每个机器人搭载的里程计、多个UWB标签和激光雷达作为感知单元，实现里程计数据、UWB数据和激光数据的采集。
本发明的有益效果为：
(1)本发明提出了一种基于多机器人协作的地图高效精确构建方法，机器人通过编码器计算自身的里程信息；基于采集的多组UWB距离信息，通过非线性优化算法得到多机器人的相对位姿；融合里程计与UWB位姿估计数据提高定位精度；通过所携带的激光雷达对环境进行扫描，构建激光闭环约束，通过图优化算法对轨迹进一步约束，实现多机器人间更精确的位姿估算。
(2)本发明利用多组UWB数据可以解决机器人间的相对定位，但由于UWB数据受环境多径干扰，定位误差较大；里程计虽然长时间存在累积误差，但是在短时间内精度很高，因此融合短时间内的里程计信息可以提高定位精度。
(3)每台机器人通过搭载激光雷达采集环境特征信息，机器人通过回环检测方式利用环境特征判定是否重新回到某一区域，从而对轨迹进一步约束，实现更高的定位精度。在此基础上结合激光传感器数据进行精确的地图构建，解决了多个机器人在大型环境中地图构建效率不高，精度较差的问题。
本领域的普通技术人员将会意识到，这里所述的实施例是为了帮助读者理解本发明的原理，应被理解为本发明的保护范围并不局限于这样的特别陈述和实施例。本领域的普通技术人员可以根据本发明公开的这些技术启示做出各种不脱离本发明实质的其它各种具体变形和组合，这些变形和组合仍然在本发明的保护范围内。

Claims (3)

1.一种基于多机器人协作的移动机器人室内地图构建方法，其特征在于，包括以下步骤：
S1：采集多机器人的里程信息、UWB节点之间的距离信息和激光扫描数据；
S2：基于UWB节点之间的距离信息，利用非线性优化算法计算多机器人的相对位姿，融合里程计信息，得到优化后的多机器人轨迹数据；
S3：根据激光扫描数据构建激光闭环约束，结合步骤S2优化后的多机器人轨迹数据构建位姿图，通过图优化算法对多机器人的轨迹进一步优化，结合激光扫描数据构建地图；
所述步骤S3包括以下子步骤：
S31：根据激光传感器采集的激光扫描数据，利用正态分布变换NDT配准算法构建激光闭环约束；
S32：基于优化后的多机器人轨迹数据和激光闭环约束，构建位姿图；根据位姿图，利用图优化算法对多机器人轨迹进行再优化；并结合激光扫描数据构建室内栅格地图，完成基于多机器人协作的移动机器人室内地图构建；
所述步骤S31包括以下子步骤：
S331：将激光扫描数据的点云划分为源点云Q和参考点云R，并将参考点云R划分为若干立方体，并计算每个立方体内点云位置平均值r和协方差矩阵V，其中，Q＝{q1,q2,…,qn}，R＝{r1,r2…rn}，qi,i＝1,2,...,n表示源点云数据，ri,i＝1,2,...,n表示源点云数据，n表示点云数据个数；
S332：将源点云Q旋转平移得到新的点云M＝{m1,m2,…mn}，确定新的点云M中点mi所在参考点云R对应的立方体，并计算点mi在参考点云R中的概率p(mi)，其计算公式为：
Figure FDA0004169617730000021
其中，mi,i＝1,2,...,n表示新的点云数据，exp[·]表示指数运算；
S333：根据点mi在参考点云R中的概率p(mi)计算新的点云M中所有点在参考点云R中的概率密度之和s(P)，其计算公式为：
Figure FDA0004169617730000022
其中，P表示旋转平移矩阵，T(P,qk)表示当前扫描点qk经过旋转平移后得到的点mk；
S334：利用高斯牛顿迭代法计算使s(P)最小的旋转平移矩阵
Figure FDA0004169617730000023
将
Figure FDA0004169617730000024
代入源点云Q中，作为新的点云M，并返回步骤S32，直至新的点云M中所有点在参考点云R中的概率密度之和s(P)小于设定阈值，完成激光闭环约束构建；
所述步骤S32中，利用图优化算法对多机器人轨迹进行再优化的具体方法为：把多机器人的位姿作为顶点，不同时刻的约束条件作为边，并利用图优化算法计算约束条件最小情况下的位姿，其中，所述约束条件包括多机器人里程计约束、多组UWB距离数据非线性优化后的多机器人相对位姿约束和激光闭环约束；约束最小情况下的位姿x*的计算公式为：
Figure FDA0004169617730000031
其中，argmin(·)表示最小值运算，x表示机器人i和机器人j间的相对位姿，a∈[1,…,N]，N表示机器人个数，T表示时长，
Figure FDA0004169617730000032
表示机器人a在t时刻基于里程计信息的位姿估计，
Figure FDA0004169617730000033
表示机器人a在t-1时刻的位姿，
Figure FDA0004169617730000034
机器人a在t时刻的位姿，
Figure FDA0004169617730000035
表示机器人a在t-1到t时刻里程计相对测量位姿的预测值，
Figure FDA0004169617730000036
表示机器人a在t-1至t时刻里程计测量值的信息矩阵，
Figure FDA0004169617730000037
表示多机器人的相对位姿，
Figure FDA0004169617730000038
表示机器人i在t时刻的位姿，
Figure FDA0004169617730000039
表示机器人j在t时刻的位姿，
Figure FDA00041696177300000310
表示机器人i和机器人j间UWB测量位姿的预测值，
Figure FDA00041696177300000311
表示机器人i和机器人j之间在t时刻UWB位姿估计的信息矩阵，
Figure FDA00041696177300000312
表示机器人i在tα时刻和机器人j在tβ时刻的激光点云NDT配准算法收敛后的得分值小于阈值γ，
Figure FDA00041696177300000313
表示机器人i在tα时刻和机器人j在tβ时刻基于NDT激光点云配准算法的位姿变换，
Figure FDA00041696177300000314
表示机器人i在tα时刻的位姿，
Figure FDA00041696177300000315
表示机器人j在tβ时刻的位姿，
Figure FDA00041696177300000316
表示机器人i在tα时刻和机器人j在tβ时刻激光雷达NDT配准位姿的预测值，
Figure FDA00041696177300000317
表示机器人i和机器人j之间在tα到tβ时刻激光闭环估计的信息矩阵。
2.根据权利要求1所述的基于多机器人协作的移动机器人室内地图构建方法，其特征在于，所述步骤S1中，采集多机器人的里程信息的具体方法为：将里程计搭载在机器人上采集里程信息；
采集UWB节点之间的距离信息的具体方法为：在每个机器人的不同位置搭载UWB标签，利用UWB标签采集UWB节点之间的距离信息；
采集激光扫描数据的具体方法为：将激光传感器搭载在机器人上采集激光扫描数据。
3.根据权利要求1所述的基于多机器人协作的移动机器人室内地图构建方法，其特征在于，所述步骤S2中，通过高斯迭代得到最佳的多机器人相对位姿，使UWB节点之间距离测量值
Figure FDA0004169617730000041
与距离估计值
Figure FDA0004169617730000042
的误差最小，其多机器人的相对位姿
Figure FDA0004169617730000043
的计算公式为：
Figure FDA0004169617730000044
其中，argmin(·)表示最小值运算，x表示机器人i和机器人j间的相对位姿，(x,y)表示机器人i相对机器人j的位置坐标，θ表示机器人i相对于机器人j的方向角度，
Figure FDA0004169617730000045
表示在t时刻机器人i上UWB节点k和机器人j上UWB节点l之间的实际测量值，
Figure FDA0004169617730000046
表示给定机器人i与机器人j相对位姿的情况下机器人i上UWB节点k和机器人j上UWB节点l之间的距离，
Figure FDA0004169617730000047
表示机器人i上的UWB节点k的相对位置，
Figure FDA0004169617730000048
表示机器人j上的UWB节点l的相对位置；
所述步骤S2中，将机器人的位姿作为顶点，不同时刻的估计位姿作为约束，包括基于非线性优化后UWB的相对位姿约束和里程计中相邻时刻机器人的位姿变换约束，通过图优化算法求取使约束最小情况下的多机器人轨迹数据。
CN202110741481.1A 2021-06-30 2021-06-30 一种基于多机器人协作的移动机器人室内地图构建方法 Active CN113670290B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110741481.1A CN113670290B (zh) 2021-06-30 2021-06-30 一种基于多机器人协作的移动机器人室内地图构建方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110741481.1A CN113670290B (zh) 2021-06-30 2021-06-30 一种基于多机器人协作的移动机器人室内地图构建方法

Publications (2)

Publication Number Publication Date
CN113670290A CN113670290A (zh) 2021-11-19
CN113670290B true CN113670290B (zh) 2023-05-12

Family

ID=78538539

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110741481.1A Active CN113670290B (zh) 2021-06-30 2021-06-30 一种基于多机器人协作的移动机器人室内地图构建方法

Country Status (1)

Country Link
CN (1) CN113670290B (zh)

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105928505A (zh) * 2016-04-19 2016-09-07 深圳市神州云海智能科技有限公司 移动机器人的位姿确定方法和设备
CN110645974A (zh) * 2019-09-26 2020-01-03 西南科技大学 一种融合多传感器的移动机器人室内地图构建方法
CN110737271A (zh) * 2019-10-23 2020-01-31 西南科技大学 一种水面机器人自主巡航系统及方法
CN111337018A (zh) * 2020-05-21 2020-06-26 上海高仙自动化科技发展有限公司 定位方法及装置、智能机器人及计算机可读存储介质
CN112197770A (zh) * 2020-12-02 2021-01-08 北京欣奕华数字科技有限公司 一种机器人的定位方法及其定位装置
CN112230243A (zh) * 2020-10-28 2021-01-15 西南科技大学 一种移动机器人室内地图构建方法
CN112325872A (zh) * 2020-10-27 2021-02-05 上海懒书智能科技有限公司 一种基于多传感器耦合的移动设备的定位方法
CN112793629A (zh) * 2021-03-04 2021-05-14 上海申传电气股份有限公司 一种煤矿井下防爆蓄电池轨道电机车自主定位方法

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20170023659A1 (en) * 2015-05-08 2017-01-26 5D Robotics, Inc. Adaptive positioning system
US10801855B2 (en) * 2018-01-12 2020-10-13 Zhejiang Guozi Technology Co., Ltd. Method and system for creating map based on 3D laser

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105928505A (zh) * 2016-04-19 2016-09-07 深圳市神州云海智能科技有限公司 移动机器人的位姿确定方法和设备
CN110645974A (zh) * 2019-09-26 2020-01-03 西南科技大学 一种融合多传感器的移动机器人室内地图构建方法
CN110737271A (zh) * 2019-10-23 2020-01-31 西南科技大学 一种水面机器人自主巡航系统及方法
CN111337018A (zh) * 2020-05-21 2020-06-26 上海高仙自动化科技发展有限公司 定位方法及装置、智能机器人及计算机可读存储介质
CN112325872A (zh) * 2020-10-27 2021-02-05 上海懒书智能科技有限公司 一种基于多传感器耦合的移动设备的定位方法
CN112230243A (zh) * 2020-10-28 2021-01-15 西南科技大学 一种移动机器人室内地图构建方法
CN112197770A (zh) * 2020-12-02 2021-01-08 北京欣奕华数字科技有限公司 一种机器人的定位方法及其定位装置
CN112793629A (zh) * 2021-03-04 2021-05-14 上海申传电气股份有限公司 一种煤矿井下防爆蓄电池轨道电机车自主定位方法

Non-Patent Citations (8)

* Cited by examiner, † Cited by third party
Title
Cost-effective Mapping of Mobile Robot Based on the Fusion of UWB and Short-range 2D LiDAR;Ran Liu等;《IEEE/ASME Transactions on Mechatronics》;第27卷(第3期);第1-12页 *
Efficient Laser-Based 3D SLAM for Coal Mine Rescue Robots;MENGGANG LI等;《IEEE》;第14124-14138页 *
基于A*和TEB融合的行人感知无碰跟随方法;庞磊等;《航空学报》;第42卷(第4期);第1-10页 *
基于UWB定位技术的多移动机器人编队控制;郑亚君等;《智能科学与技术学报》;第1卷(第1期);地1-5页 *
基于多传感器融合的机器人位姿估计研究;罗耀耀;钟山;王锐;王楠;杨韵秋;;实验技术与管理(05);全文 *
基于多源传感器信息融合的移动机器人协同导航研究;王晨琳;《中国优秀硕士学位论文全文数据库 信息科技辑》;I140-376 *
基于联邦滤波的室内移动机器人定位;许坤;邹杰;陈谋;;电光与控制(09);全文 *
激光SLAM之NDT算法;叫我李先生;《https://blog.csdn.net/qq_40216084/article/details/107618766》;第1-5页 *

Also Published As

Publication number Publication date
CN113670290A (zh) 2021-11-19

Similar Documents

Publication Publication Date Title
CN112230243B (zh) 一种移动机器人室内地图构建方法
Nieto et al. Recursive scan-matching SLAM
CN102221688B (zh) 一种雷达系统误差估计方法
CN111536967A (zh) 一种基于ekf的多传感器融合温室巡检机器人跟踪方法
Du et al. CRCLoc: A crowdsourcing-based radio map construction method for WiFi fingerprinting localization
Fang et al. A real-time 3d perception and reconstruction system based on a 2d laser scanner
CN105208651A (zh) 基于地图结构的Wi-Fi位置指纹非监督训练方法
CN113763549B (zh) 融合激光雷达和imu的同时定位建图方法、装置和存储介质
Lu et al. Robot indoor location modeling and simulation based on Kalman filtering
Lin et al. Mobile robot self-localizationusing visual odometry based on ceiling vision
Zhao et al. An efficient radio map updating algorithm based on K-means and Gaussian process regression
CN113670290B (zh) 一种基于多机器人协作的移动机器人室内地图构建方法
CN111274529B (zh) 一种鲁棒的高斯逆威沙特phd多扩展目标跟踪算法
CN111275748B (zh) 动态环境下基于激光雷达的点云配准方法
CN102830391A (zh) 一种红外搜索与跟踪系统准确性指标计算方法
Wang et al. Indoor position algorithm based on the fusion of wifi and image
CN117109581A (zh) 移动机器人自定位导航方法、系统、电子设备及存储介质
Liu et al. Unmanned aerial vehicle positioning algorithm based on the secant slope characteristics of transmission lines
CN113554705B (zh) 一种变化场景下的激光雷达鲁棒定位方法
CN115639570A (zh) 融合激光强度和点云几何特征的机器人定位和建图方法
CN112305558B (zh) 一种利用激光点云数据的移动机器人轨迹确定方法及装置
CN111239761B (zh) 一种用于室内实时建立二维地图的方法
Guo et al. 3D Lidar SLAM Based on Ground Segmentation and Scan Context Loop Detection
Zhang et al. OW-LOAM: Observation-weighted LiDAR odometry and mapping
Xiang et al. Low data overlap rate graph-based slam with distributed submap strategy

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
GR01 Patent grant
GR01 Patent grant