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

1.本发明属于多机器人协作的地图高效精确构建技术领域，具体涉及一种基于多机器人协作的移动机器人室内地图构建方法。


背景技术：

2.slam技术在室内环境地图构建上得到很好的应用，机器人通过携带传感器在运动过程中采集激光扫描数据和测量数据，通过对采集数据的处理实现对自己位姿的定位从而构建精确的地图。传统的地图构建往往使用摄像头或者激光扫描仪实现对环境特征点的采集，通过对特征点进行回环匹配，实现地图构建，计算开销很大，并且效率较低。在大型环境中可靠的slam仍然是机器人领域目前面临的一大挑战。
3.此外，单个机器人在大型环境的地图构建中效率受到明显限制，在大型环境中解决实际的建图问题较为困难，针对这一缺点，多机器人系统应运而生。相较于单个机器人在效率和能力方面的限制，多机器人系统可以通过群体之间协作的方式弥补单个机器人的缺点，能够很好的替代单个机器人实现大型环境地图的构建，提升工作效率。机器人搭载编码器可以得到自身的里程信息，里程计提供了机器人短时间内精确的位姿变化，但是长时间存在累计误差。基于多个机器人采集的多组uwb距离信息，通过非线性优化算法得到多机器人的位姿信息，融合非线性优化后的uwb定位信息能够纠正累积误差提供更精确的多机器人移动轨迹。每台机器人通过搭载激光雷达采集环境特征信息，机器人通过回环检测方式利用环境特征判定是否重新回到某一区域，从而对轨迹进一步约束，有效克服大型环境中环境距离的影响。在此基础上结合激光传感器数据进行精确的地图构建，解决了多机器人在大型环境下协作建图困难且精度不高的问题。


技术实现要素：

4.本发明的目的是为了解决多机器人在大型环境下协作建图困难且精度不高的问题，提出了一种基于多机器人协作的移动机器人室内地图构建方法。
5.本发明的技术方案是：一种基于多机器人协作的移动机器人室内地图构建方法包括以下步骤：
6.s1：采集多机器人的里程信息、uwb节点之间的距离信息和激光扫描数据；
7.s2：基于uwb节点之间的距离信息，利用非线性优化算法计算多机器人的相对位姿，融合里程计信息，得到优化后的多机器人轨迹数据；
8.s3：根据激光扫描数据构建激光闭环约束，结合步骤s2优化后的多机器人轨迹数据构建位姿图，通过图优化算法对多机器人的轨迹进一步优化，结合激光扫描数据构建地图。
9.进一步地，步骤s1中，采集多机器人的里程信息的具体方法为：将里程计搭载在机器人上采集里程信息；
10.采集uwb节点之间的距离信息的具体方法为：在每个机器人的不同位置搭载uwb标
签，利用uwb标签采集uwb节点之间的距离信息；
11.采集激光扫描数据的具体方法为：将激光传感器搭载在机器人上采集激光扫描数据。
12.进一步地，步骤s2中，通过高斯迭代得到最佳的多机器人相对位姿，使uwb节点之间距离测量值与距离估计值的误差最小，其多机器人的相对位姿的计算公式为：
[0013][0014]
其中，argmin(
·
)表示最小值运算，x表示机器人i和机器人j间的相对位姿，(x,y)表示机器人i相对机器人j的位置坐标，θ表示机器人i相对于机器人j的方向角度，表示在t时刻机器人i上uwb节点k和机器人j上uwb节点l之间的实际测量值，表示给定机器人i与机器人j相对位姿的情况下机器人i上uwb节点k和机器人j上uwb节点l之间的距离，表示机器人i上的uwb节点k的相对位置，表示机器人j上的uwb节点l的相对位置；
[0015]
步骤s2中，将机器人的位姿作为顶点，不同时刻的估计位姿作为约束，包括基于非线性优化后uwb的相对位姿约束和里程计中相邻时刻机器人的位姿变换约束，通过图优化算法求取使约束最小情况下的多机器人轨迹数据。
[0016]
进一步地，步骤s3包括以下子步骤：
[0017]
s31：根据激光传感器采集的激光扫描数据，利用正态分布变换配准算法构建激光闭环约束；
[0018]
s32：基于优化后的多机器人轨迹数据和激光闭环约束，构建位姿图；根据位姿图，利用图优化算法对多机器人轨迹进行再优化；并结合激光扫描数据构建室内栅格地图，完成基于多机器人协作的移动机器人室内地图构建。
[0019]
进一步地，步骤s31包括以下子步骤：
[0020]
s331：将激光扫描数据的点云划分为源点云q和参考点云r，并将参考点云r划分为若干立方体，并计算每个立方体内点云位置平均值和协方差矩阵v，其中，q＝{q1,q2,
…
,q
n
}，r＝{r1,r2…
r
n
}，q
i
,i＝1,2,...,n表示源点云数据，r
i
,i＝1,2,...,n表示源点云数据，n表示点云数据个数；
[0021]
s332：将源点云q旋转平移得到新的点云m＝{m1,m2,
…
m
n
}，确定新的点云m中点m
i
所在参考点云r对应的立方体，并计算点m
i
在参考点云r中的概率p(m
i
)，其计算公式为：
[0022][0023]
其中，m
i
,i＝1,2,...,n表示新的点云数据，exp[
·
]表示指数运算；
[0024]
s333：根据点m
i
在参考点云r中的概率p(m
i
)计算新的点云m中所有点在参考点云r中的概率密度之和s(p)，其计算公式为：
[0025][0026]
其中，p表示旋转平移矩阵，t(p,q
k
)表示当前扫描点q
k
经过旋转平移后得到的点m
k
；
[0027]
s334：利用高斯牛顿迭代法计算使s(p)最小的旋转平移矩阵将代入源点云q中，作为新的点云m，并返回步骤s32，直至新的点云m中所有点在参考点云r中的概率密度之和s(p)小于设定阈值，完成激光闭环约束构建。
[0028]
进一步地，步骤s32中，利用图优化算法对多机器人轨迹进行再优化的具体方法为：把多机器人的位姿作为顶点，不同时刻的约束条件作为边，并利用图优化算法计算约束条件最小情况下的位姿，其中，所述约束条件包括多机器人里程计约束、多组uwb距离数据非线性优化后的多机器人相对位姿约束和激光闭环约束；约束最小情况下的位姿x
*
的计算公式为：
[0029][0030]
其中，argmin(
·
)表示最小值运算，x表示机器人i和机器人j间的相对位姿，a∈[1,
…
,n]，n表示机器人个数，t表示时长，表示机器人a在t时刻基于里程计信息的位姿估计，表示机器人a在t
‑
1时刻的位姿，机器人a在t时刻的位姿，表示机器人a在t
‑
1到t时刻里程计相对测量位姿的预测值，表示机器人a在t
‑
1至t时刻里程计测量值的信息矩阵，表示多机器人的相对位姿，表示机器人i在t时刻的位姿，表示机器人j在t时刻的位姿，表示机器人i和机器人j间uwb测量位姿的预测值，表示机器人i和机器人j之间在t时刻uwb位姿估计的信息矩阵，表示机器人i在t
α
时刻和机器人j在t
β
时刻的激光点云ndt配准算法收敛后的得分值小于阈值γ，表示机器人i在t
α
时刻和机器人j在t
β
时刻基于ndt激光点云配准算法的位姿变换，表示机器人i在t
α
时刻的位姿，表示机器人j在t
β
时刻的位姿，表示机器人i在t
α
时刻和机器人j在t
β
时刻激光雷达ndt配准位姿的预测值，表示机器人i和机器人j之间在t
α
到t
β
时刻激光闭环估计的信息矩阵。
[0031]
本发明的有益效果是：
[0032]
(1)本发明提出了一种基于多机器人协作的地图高效精确构建方法，机器人通过编码器计算自身的里程信息；基于采集的多组uwb距离信息，通过非线性优化算法得到多机器人的相对位姿；融合里程计与uwb位姿估计数据提高定位精度；通过所携带的激光雷达对
环境进行扫描，构建激光闭环约束，通过图优化算法对轨迹进一步约束，实现多机器人间更精确的位姿估算。
[0033]
(2)本发明利用多组uwb数据可以解决机器人间的相对定位，但由于uwb数据受环境多径干扰，定位误差较大；里程计虽然长时间存在累积误差，但是在短时间内精度很高，因此融合短时间内的里程计信息可以提高定位精度。
[0034]
(3)每台机器人通过搭载激光雷达采集环境特征信息，机器人通过回环检测方式利用环境特征判定是否重新回到某一区域，从而对轨迹进一步约束，实现更高的定位精度。在此基础上结合激光传感器数据进行精确的地图构建，解决了多个机器人在大型环境中地图构建效率不高，精度较差的问题。
附图说明
[0035]
图1为移动机器人室内地图构建方法的流程图。
[0036]
图2为移动机器人室内地图构建方法的示意图。
具体实施方式
[0037]
下面结合附图对本发明的实施例作进一步的说明。
[0038]
在描述本发明的具体实施例之前，为使本发明的方案更加清楚完整，首先对本发明中出现的缩略语和关键术语定义进行说明：
[0039]
uwb：一种使用1ghz以上频率带宽的无线载波通信技术。
[0040]
如图1所示，本发明提供了一种基于多机器人协作的移动机器人室内地图构建方法，包括以下步骤：
[0041]
s1：采集多机器人的里程信息、uwb节点之间的距离信息和激光扫描数据；
[0042]
s2：基于uwb节点之间的距离信息，利用非线性优化算法计算多机器人的相对位姿，融合里程计信息，得到优化后的多机器人轨迹数据；
[0043]
s3：根据激光扫描数据构建激光闭环约束，结合步骤s2优化后的多机器人轨迹数据构建位姿图，通过图优化算法对多机器人的轨迹进一步优化，结合激光扫描数据构建地图。
[0044]
在本发明实施例中，步骤s1中，采集多机器人的里程信息的具体方法为：将里程计搭载在机器人上采集里程信息；
[0045]
采集uwb节点之间的距离信息的具体方法为：在每个机器人的不同位置搭载uwb标签，利用uwb标签采集uwb节点之间的距离信息；
[0046]
采集激光扫描数据的具体方法为：将激光传感器搭载在机器人上采集激光扫描数据。
[0047]
在本发明实施例中，步骤s2中，通过高斯迭代得到最佳的多机器人相对位姿，使uwb节点之间距离测量值与距离估计值的误差最小，其多机器人的相对位姿的计算公式为：
[0048]
[0049]
其中，argmin(
·
)表示最小值运算，x表示机器人i和机器人j间的相对位姿，(x,y)表示机器人i相对机器人j的位置坐标，θ表示机器人i相对于机器人j的方向角度，表示在t时刻机器人i上uwb节点k和机器人j上uwb节点l之间的实际测量值，表示给定机器人i与机器人j相对位姿的情况下机器人i上uwb节点k和机器人j上uwb节点l之间的距离，表示机器人i上的uwb节点k的相对位置，表示机器人j上的uwb节点l的相对位置；
[0050]
步骤s2中，里程计虽然长时间存在累积误差，但是在短时间内精度很高，因此融合短时间内的里程计信息可以提高定位精度。将机器人的位姿作为顶点，不同时刻的估计位姿作为约束，包括基于非线性优化后uwb的相对位姿约束和里程计中相邻时刻机器人的位姿变换约束，通过图优化算法求取使约束最小情况下的多机器人轨迹数据。
[0051]
uwb具有穿透力强、功耗低和测距精度高的优点，但是uwb无法测得目标的方位信息，因此单个uwb难以实现目标的定位。基于采集的多组uwb节点之间的距离信息，通过非线性优化算法得到机器人i和机器人j之间的相对位姿
[0052]
里程计虽然长时间存在累积误差，但是在短时间内精度很高，因此融合短时间内的里程计信息可以提高定位精度。把机器人的位姿作为顶点，不同时刻的估计位姿作为约束，包括基于非线性优化后uwb的位姿约束，里程计中相邻时刻机器人的位姿变换约束，通过图优化算法求取使约束最小情况下的位姿。
[0053]
在本发明实施例中，步骤s3包括以下子步骤：
[0054]
s31：根据激光传感器采集的激光扫描数据，利用正态分布变换配准算法构建激光闭环约束；
[0055]
s32：基于优化后的多机器人轨迹数据和激光闭环约束，构建位姿图；根据位姿图，利用图优化算法对多机器人轨迹进行再优化；并结合激光扫描数据构建室内栅格地图，完成基于多机器人协作的移动机器人室内地图构建。
[0056]
在本发明实施例中，步骤s31包括以下子步骤：
[0057]
s331：将激光扫描数据的点云划分为源点云q和参考点云r，并将参考点云r划分为若干立方体，并计算每个立方体内点云位置平均值r和协方差矩阵v，其中，q＝{q1,q2,
…
,q
n
}，r＝{r1,r2…
r
n
}，q
i
,i＝1,2,...,n表示源点云数据，r
i
,i＝1,2,...,n表示源点云数据，n表示点云数据个数；
[0058]
s332：将源点云q旋转平移得到新的点云m＝{m1,m2,
…
m
n
}，确定新的点云m中点m
i
所在参考点云r对应的立方体，并计算点m
i
在参考点云r中的概率p(m
i
)，其计算公式为：
[0059][0060]
其中，m
i
,i＝1,2,...,n表示新的点云数据，exp[
·
]表示指数运算；
[0061]
s333：根据点m
i
在参考点云r中的概率p(m
i
)计算新的点云m中所有点在参考点云r中的概率密度之和s(p)，其计算公式为：
[0062][0063]
其中，p表示旋转平移矩阵，t(p,q
k
)表示当前扫描点q
k
经过旋转平移后得到的点m
k
；
[0064]
s334：利用高斯牛顿迭代法计算使s(p)最小的旋转平移矩阵将代入源点云q中，作为新的点云m，并返回步骤s32，直至新的点云m中所有点在参考点云r中的概率密度之和s(p)小于设定阈值，完成激光闭环约束构建。
[0065]
激光闭环的检测采用ndt(正态分布变换)配准算法，因为其在配准过程中不需要搜索最邻近的匹配点，所以相对于icp(迭代最接近点)算法有更好的精度和收敛速度。通过高斯牛顿迭代法求解使目标函数s(p)最小的旋转平移矩阵其计算公式为：h(p)δp＝
‑
g(p)，其中，h(p)表示s(p)的海瑟矩阵，g(p)表示s(p)的梯度。通过给定初值，将旋转平移矩阵p代入源点云q中，得到新的一组点云m，再将m点云与参考点云r＝{r1,r2…
r
n
}匹配求解新的旋转平移矩阵。多次迭代后s(p)趋于收敛，得到此时的源点云和参考点云的位置变换关系。当s(p)小于阈值γ时，算法中认为检测到了闭环。
[0066]
在本发明实施例中，步骤s32中，对轨迹数据进行约束的具体方法为：根据激光扫描数据，利用回环检测方法确定多机器人重新回到的活动轨迹区域，完成对轨迹数据的约束；
[0067]
步骤s32中，利用图优化算法对多机器人轨迹进行再优化的具体方法为：把多机器人的位姿作为顶点，不同时刻的约束条件作为边，并利用图优化算法计算约束条件最小情况下的位姿，其中，所述约束条件包括多机器人里程计约束、多组uwb距离数据非线性优化后的多机器人相对位姿约束和激光闭环约束；约束最小情况下的位姿x
*
的计算公式为：
[0068][0069]
其中，argmin(
·
)表示最小值运算，x表示机器人i和机器人j间的相对位姿，a∈[1,
…
,n]，n表示机器人个数，t表示时长，表示机器人a在t时刻基于里程计信息的位姿估计，表示机器人a在t
‑
1时刻的位姿，机器人a在t时刻的位姿，表示机器人a在t
‑
1到t时刻里程计相对测量位姿的预测值，表示机器人a在t
‑
1至t时刻里程计测量值的信息矩阵，表示多机器人的相对位姿，表示机器人i在t时刻的位姿，表示机器人j在t时刻的位姿，表示机器人i和机器人j间uwb测量位姿的预测值，表示机器人i和机器人j之间在t时刻uwb位姿估计的信息矩阵，表示机器人i在t
α
时刻和机器人j在t
β
时刻的激光点云ndt配准算法收敛后的得分值小于阈值γ，表示机
器人i在t
α
时刻和机器人j在t
β
时刻基于ndt激光点云配准算法的位姿变换，表示机器人i在t
α
时刻的位姿，表示机器人j在t
β
时刻的位姿，表示机器人i在t
α
时刻和机器人j在t
β
时刻激光雷达ndt配准位姿的预测值，表示机器人i和机器人j之间在t
α
到t
β
时刻激光闭环估计的信息矩阵。
[0070]
uwb测距虽然不受环境纹理的影响，但是uwb测量距离有限，当机器人距离较远时，uwb难以得到机器人间的位姿关系。每台机器人通过搭载激光雷达采集环境特征信息，机器人通过回环检测方式利用环境特征判定是否重新回到某一区域，从而对轨迹进一步约束。基于s2优化后的多机器人轨迹数据，结合激光扫描匹配算法引入额外激光
‑
闭环约束，使用图优化算法通过优化位姿来使激光
‑
闭环约束带来的误差最小，得到更高精度的多机器人轨迹数据，完成栅格地图的构建。把机器人的位姿作为顶点，不同时刻的约束条件作为边，包括多机器人里程计约束，多组uwb距离数据非线性优化后的多机器人位姿约束，激光
‑
闭环约束，通过图优化算法求取使约束最小情况下的位姿。如图2所示，多机器人采用本发明的方法构建室内地图。
[0071]
本发明的工作原理及过程为：本发明涉及一种基于多机器人协作的大型环境地图高效精确构建方法，本发明包括uwb、里程计与激光数据采集模块、基于uwb机器人相对位姿估计、里程计与uwb位姿估计数据融合优化、激光闭环检测与位姿进一步优化。利用每个机器人搭载的里程计、多个uwb标签和激光雷达作为感知单元，实现里程计数据、uwb数据和激光数据的采集。
[0072]
本发明的有益效果为：
[0073]
(1)本发明提出了一种基于多机器人协作的地图高效精确构建方法，机器人通过编码器计算自身的里程信息；基于采集的多组uwb距离信息，通过非线性优化算法得到多机器人的相对位姿；融合里程计与uwb位姿估计数据提高定位精度；通过所携带的激光雷达对环境进行扫描，构建激光闭环约束，通过图优化算法对轨迹进一步约束，实现多机器人间更精确的位姿估算。
[0074]
(2)本发明利用多组uwb数据可以解决机器人间的相对定位，但由于uwb数据受环境多径干扰，定位误差较大；里程计虽然长时间存在累积误差，但是在短时间内精度很高，因此融合短时间内的里程计信息可以提高定位精度。
[0075]
(3)每台机器人通过搭载激光雷达采集环境特征信息，机器人通过回环检测方式利用环境特征判定是否重新回到某一区域，从而对轨迹进一步约束，实现更高的定位精度。在此基础上结合激光传感器数据进行精确的地图构建，解决了多个机器人在大型环境中地图构建效率不高，精度较差的问题。
[0076]
本领域的普通技术人员将会意识到，这里所述的实施例是为了帮助读者理解本发明的原理，应被理解为本发明的保护范围并不局限于这样的特别陈述和实施例。本领域的普通技术人员可以根据本发明公开的这些技术启示做出各种不脱离本发明实质的其它各种具体变形和组合，这些变形和组合仍然在本发明的保护范围内。