作者：最爱喝兽奶 | 来源：3DCV
1、导读
在多机器人系统中，准确的定位和建图对于机器人之间的协作和任务执行至关重要。然而，由于环境的复杂性和传感器的限制，单个机器人的定位和建图可能存在误差。因此，本文提出了一种名为CoVOR-SLAM的方法，通过结合视觉里程计和距离测量数据，实现了多机器人系统的协作定位和建图。该方法通过数据融合和优化算法，提高了定位和建图的准确性，并降低了计算复杂度。通过实验验证，CoVOR-SLAM在减小视觉里程计误差和实现高精度定位方面取得了显著的效果。
2、研究思路
本文的研究思路是通过合作式同时定位与地图构建（Cooperative SLAM）方法，利用视觉里程计（Visual Odometry）和距离测量数据来解决多机器人系统中的定位和地图构建问题。具体而言，本研究提出了一种名为CoVOR-SLAM的方法，该方法利用视觉里程计和距离测量数据进行数据融合，以提高定位的准确性和地图的完整性。
3、贡献
包括以下几点：
- 引入了距离测量数据：传统的视觉里程计方法在定位时存在尺度模糊的问题，而本研究通过引入距离测量数据，可以减小尺度模糊的影响，提高定位的准确性。
- 利用多机器人的合作：本研究通过多机器人之间的合作，利用彼此之间的距离测量数据进行数据融合，从而提高整个系统的定位和地图构建的准确性。
- 降低计算和通信负载：本研究的方法在数据融合过程中需要的计算和通信负载相对较低，适用于计算和通信能力有限的多机器人系统。
4、研究内容
本篇文章的研究内容是关于多机器人系统中的协作SLAM（同时定位与地图构建）问题。文章提出了一种名为CoVOR-SLAM的方法，该方法利用视觉里程计和距离测量数据来实现多机器人系统的定位和地图构建。CoVOR-SLAM通过融合视觉里程计和距离测量数据，可以减少视觉里程计的尺度模糊问题，并且在计算复杂度上具有较低的要求。文章通过实验数据验证了CoVOR-SLAM方法的有效性，并与其他方法进行了比较分析。
5、创新性
本研究的创新性主要体现在以下几个方面：
- 提出了一种基于视觉里程计和距离测量的多机器人协作
SLAM方法。传统的SLAM方法通常只使用视觉信息或者距离测量信息，而本研究将两者结合起来，通过多机器人之间的协作，实现更准确和鲁棒的环境建模和定位。 - 引入了基于范围测量的协作因子图优化。通过使用范围测量信息，可以更好地约束机器人之间的相对位置关系，从而提高整个系统的定位精度和鲁棒性。
- 提出了一种增量式的姿态估计方法。通过将估计的姿态增量转换为矩阵形式，并使用
Levenberg-Marquardt算法进行优化，可以实现姿态的增量更新，从而提高定位的准确性和效率。 - 在实验数据上验证了
CoVOR-SLAM的性能。通过在实际场景中进行实验，本研究证明了CoVOR-SLAM相比于传统的视觉里程计方法具有更高的定位精度和更低的计算复杂度。
6、CoVOR-SLAM
CoVOR-SLAM是一种使用视觉里程计和测距数据的协作式SLAM方法。它可以用于多机器人系统中，每个机器人通过视觉里程计估计自己的姿态和地图点，并通过交换消息来进行数据融合。 CoVOR-SLAM的框架如图所示。
每个机器人使用视觉里程计在本地参考框架中估计自己的姿态和地图点。数据融合模块通过交换三种类型的消息（KeyframeMsg、InterRangeMsg和AnchorRangeMsg）进行多机器人数据融合。这些消息包含机器人的姿态和测距数据。
6.1、系统模型
多智能体系统模型如图所示，图中使用了两个智能体来解释模型。智能体 <math xmlns="http://www.w3.org/1998/Math/MathML"> k k </math>k的导航坐标系被表示为 <math xmlns="http://www.w3.org/1998/Math/MathML"> L k L_k </math>Lk，它是一个固定的坐标系，智能体用它来表示自己的轨迹。使用视觉里程计（VO），每个机器人逐步估计其机载相机 <math xmlns="http://www.w3.org/1998/Math/MathML"> C k C_k </math>Ck的6自由度姿态（3D旋转和平移）在其本地参考坐标系中。时间 <math xmlns="http://www.w3.org/1998/Math/MathML"> n n </math>n时刻在 <math xmlns="http://www.w3.org/1998/Math/MathML"> L k L_k </math>Lk坐标系中的无误差相机姿态表示为：
其中 <math xmlns="http://www.w3.org/1998/Math/MathML"> R L K C K n ∈ S O ( 3 ) , t L K C K n ∈ R 3 \mathrm {\mathbf{R} }{\mathrm{L}{\mathrm{K}}\mathrm{C}{\mathrm{K}}}^{n}\in\mathrm{SO}(3),\mathrm {\mathbf{t}}{\mathrm{L}{\mathrm{K}}\mathrm{C}{\mathrm{K}}}^{n}\in \mathbb{R}^{3} </math>RLKCKn∈SO(3),tLKCKn∈R3 。
对于搭载单目相机的智能体，由于视觉里程计存在全局尺度模糊，平移向量 <math xmlns="http://www.w3.org/1998/Math/MathML"> t L K C K n \mathrm {\mathbf{t}}{\mathrm{L}{\mathrm{K}}\mathrm{C}{\mathrm{K}}}^{n} </math>tLKCKn只能以全局尺度模糊的方式进行估计。因此，在 <math xmlns="http://www.w3.org/1998/Math/MathML"> L k L_k </math>Lk坐标系中的姿态也是按比例缩放的。 在这个示例设置中，将智能体1的参考坐标系设置为全局参考坐标系（即 <math xmlns="http://www.w3.org/1998/Math/MathML"> L 1 = G L_1=G </math>L1=G）。因此，智能体1的带有尺度模糊的6自由度本地姿态 <math xmlns="http://www.w3.org/1998/Math/MathML"> T L 1 C 1 n \mathrm {\mathbf{T}}{\mathrm{L}{\mathrm{1}}\mathrm{C}{\mathrm{1}}}^{n} </math>TL1C1n可以通过简单地添加一个尺度因子 <math xmlns="http://www.w3.org/1998/Math/MathML"> s 1 s_1 </math>s1转换为相机状态在全局坐标系中的7自由度表示 <math xmlns="http://www.w3.org/1998/Math/MathML"> S G C 1 n \mathrm {\mathbf{S}}{\mathrm{G}\mathrm{C}{\mathrm{1}}}^{n} </math>SGC1n，这个转换可以用矩阵形式表示：
当我们使用单目相机估计每个机器人的姿态时， <math xmlns="http://www.w3.org/1998/Math/MathML"> s 1 s_1 </math>s1表示单目相机轨迹估计的绝对尺度。当使用立体相机估计姿态时，我们可以简单地初始化 <math xmlns="http://www.w3.org/1998/Math/MathML"> s 1 s_1 </math>s1=1，因为立体视觉里程计没有全局尺度模糊（使用已知的立体相机基线长度估计轨迹的绝对尺度）。然而，立体相机的相对尺度估计误差也会随时间累积。
在全局坐标系G中，智能体2的姿态 <math xmlns="http://www.w3.org/1998/Math/MathML"> S G C 2 n \mathrm {\mathbf{S}}{\mathrm{G}\mathrm{C}{\mathrm{2}}}^{n} </math>SGC2n（使用Sim(3)中的7自由度矩阵参数化）可以从其本地姿态 <math xmlns="http://www.w3.org/1998/Math/MathML"> T L 2 C 2 n \mathrm {\mathbf{T}}{\mathrm{L}{\mathrm{2}}\mathrm{C}_{\mathrm{2}}}^{n} </math>TL2C2n（带有尺度模糊）转换得到：
其中 <math xmlns="http://www.w3.org/1998/Math/MathML"> S L 1 L 2 \mathrm {\mathbf{S}}{\mathrm{L}{\mathrm{1}}\mathrm{L}{\mathrm{2}}} </math>SL1L2表示两个本地参考坐标系之间的相似变换， <math xmlns="http://www.w3.org/1998/Math/MathML"> S G L 1 \mathrm {\mathbf{S}}{\mathrm{G}\mathrm{L}{\mathrm{1}}} </math>SGL1将姿态从 <math xmlns="http://www.w3.org/1998/Math/MathML"> L 1 L_1 </math>L1坐标系转换到全局坐标系 <math xmlns="http://www.w3.org/1998/Math/MathML"> G G </math>G，如之前方程中所定义。可以使用已知模式（如ArUco标记和棋盘格）在任务初始化阶段估计 <math xmlns="http://www.w3.org/1998/Math/MathML"> S L 1 L 2 \mathrm {\mathbf{S}}{\mathrm{L}{\mathrm{1}}\mathrm{L}{\mathrm{2}}} </math>SL1L2。如果两个机器人在初始化期间在同一个2D平面上移动，还可以使用其他方法估计 <math xmlns="http://www.w3.org/1998/Math/MathML"> S L 1 L 2 \mathrm {\mathbf{S}}{\mathrm{L}{\mathrm{1}}\mathrm{L}_{\mathrm{2}}} </math>SL1L2。在公共全局坐标系G中，所有K个智能体的7自由度姿态被定义为要估计的系统状态变量：
不同智能体的时间戳{ <math xmlns="http://www.w3.org/1998/Math/MathML"> n 0 , . . . , n N n_0, . . . , n_N </math>n0,...,nN}可以是异步的。
6.2、测量模型
测量模型包括两种类型的测量：机器人之间的测距和机器人与锚点之间的测距。
机器人之间的测距（Inter-agent range measurements）：这些测量是通过机器人上的测距传感器（天线）测量的，表示为:
机器人与锚点之间的测距（Anchor-to-agent range measurements）：这些测量是通过机器人上的测距标签模块与锚点之间的真实距离加上一个误差项表示的，表示为:
6.3、多机器人数据融合
CoVOR-SLAM是一种使用视觉里程计和距离测量进行多机器人系统的协作SLAM方法。它通过融合视觉里程计和距离测量来估计多个机器人的姿态和地图点。在CoVOR-SLAM中，每个机器人使用视觉里程计来估计其相对于本地参考框架的姿态和地图点。为了实现多机器人数据融合，系统通过三种类型的消息进行信息交换：关键帧消息（KeyframeMsg）、机器人间距离消息（InterRangeMsg）和锚点到机器人的距离消息（AnchorRangeMsg）。关键帧消息包含机器人的姿态信息，用于在数据融合模块中进行姿态估计。机器人间距离消息和锚点到机器人的距离消息用于融合距离测量，进一步提高姿态估计的准确性。CoVOR-SLAM的数据融合模块可以在每个机器人的处理器上实现，使得系统可以以分散的方式运行。通过融合视觉里程计和距离测量，CoVOR-SLAM能够准确估计机器人的姿态，即使在机器人之间的通信能力有限的情况下也能够工作。 总之，CoVOR-SLAM通过视觉里程计和距离测量的协作，实现了多机器人系统的SLAM。它通过数据融合来提高姿态估计的准确性，并且可以在通信能力有限的情况下工作。
7、CoVOR-SLAM的优势
- 使用稀疏的测距数据可以准确估计多个机器人的姿态，即使只使用单目相机（不使用其他传感器，如
IMU和激光雷达）。其他传感器可以很容易地集成到基于图优化的框架中。 - 在
CoVOR-SLAM的框架中，我们将机器人的运动参数化为7自由度（DoF）的相似变换，因此可以在视觉-测距数据融合过程中考虑视觉定位的尺度误差（并因此减小融合中的尺度误差）。 - 它需要比机器人之间的环路闭合更少的通信能力，在通信能力有限时更有用。此外，该算法可以在每个机器人上运行，因为它不需要大量的计算能力。因此，它对于分散式群体智能系统可能更有用。
- 它不限制机器人的移动方式，只要可以测量到距离，即使在两个模块之间使用的是无线电信号，也通常可以在较大距离上进行测量。
- 机器人的平台选择可以灵活，因为在
CoVOR-SLAM中，每个群体元素不需要具有相似的视角来检测机器人之间的环路。 - 该算法不需要同时连接到多个锚定站。即使其中一个漫游器只连接到一个单独的锚定站，它也可以显著减小群体系统的姿态估计误差。
CoVOR-SLAM已经在仿真和实验数据上进行了系统测试，并取得了良好的结果。具体的测试结果可以在论文中找到。
8、实验
本研究采用了实验方法来验证CoVOR-SLAM算法的性能。实验分为两个部分，分别在足球场和碎石场进行。 在足球场实验中，使用了由德国航空航天中心通信与导航研究所开发的两个机器人。其中一个机器人（Rover-1）配备了前置摄像头、UWB传感器、GNSS RTK和惯性导航系统，用于获取真实位置信息。另一个机器人（Rover-2）作为锚点固定在一个位置。通过UWB传感器获取机器人之间的测距数据。实验数据包括在足球场上的行驶时间、行驶距离和轨迹维度等信息。 在碎石场实验中，同样使用了上述的两个机器人。实验数据包括在碎石场上的行驶时间、行驶距离和轨迹维度等信息。 通过这些实验数据，研究人员验证了CoVOR-SLAM算法在减小视觉里程计估计误差、提高定位精度和降低计算复杂度方面的有效性。同时，还对UWB测距数据的误差模型进行了建模，并通过实验数据生成了测距数据。通过对比实验结果和地面真实轨迹，验证了CoVOR-SLAM算法的性能。
9、结论
本文提出了一种名为CoVOR-SLAM的协作式同时定位与地图构建方法，用于多机器人系统中的相对定位。该方法通过交换机器人之间的姿态估计、估计不确定性和相互测距信息来实现定位。与传统的基于视觉特征的方法相比，CoVOR-SLAM不需要进行复杂的特征匹配和地图融合，从而大大减少了计算和通信负载。实验结果表明，CoVOR-SLAM能够准确估计机器人的姿态，并且所需的计算和通信资源较少。因此，CoVOR-SLAM在资源受限的环境下具有很大的应用潜力。