CN114694013B - 一种分布式多机协同视觉slam方法与*** - Google Patents

Abstract

本发明公开了一种分布式多机协同视觉SLAM方法与***,包括以下步骤：对机器人运行视觉SLAM程序，通过相机获取图像，基于所述图像提取NetVLAD全局描述子；基于所述NetVLAD全局描述子利用欧式距离判断，满足通讯范围要求则机器人间进行通讯并且确定机器人间闭环；基于确定机器人间闭环的机器人，利用DGS1算法进行分布式图优化，确定多个机器人间协同定位。本发明对错误闭环有效剔除，采取分布式位姿图优化实现多机间协同定位，增强定位能力且对光照具有鲁棒性，数据传输量小，优化效率高。

Description

一种分布式多机协同视觉SLAM方法与***

技术领域

本发明属于多机协同移动机器人的同步定位与地图构建技术领域，尤其涉及一种分布式多机协同视觉SLAM方法与***。

背景技术

同步定位与地图构建(Simultaneous localization and mapping，简称为SLAM)，是指搭载特定传感器的载体，在未知环境于运动过程中进行自身定位，并建立环境的地图。SLAM能够有效解决GPS信号缺失下的定位问题，在无人车辆、机器人等领域得到了广泛的应用。目前基于单机的视觉SLAM方法已经具有较为成熟的解决方案，但随着作业环境的扩大，单机SLAM效率较低，估计时间增长，估计精度下降，往往难以达到良好的效果。为了解决这一问题，多机协同的SLAM方案引起了学术界和工业界的广泛关注。多机协同视觉SLAM一般指群体移动机器人，在未知环境中，搭载相机获取环境信息，通过数据交换，估计、优化自身的定位信息，并建立环境的地图。其中，机器人间的数据关联往往采用相似场景辨识确定多机间闭合环路并计算相对位姿变换矩阵实现的。

目前，多机协同视觉SLAM的基本框架包含前端机间闭环检测以及后端优化两个部分。现有多机协同SLAM的框架主要分为集中式和分布式两种，分布式由于其良好的隐私性以及较为宽松的计算资源要求而得到关注，本发明亦采用分布式的框架。现有多机协同SLAM的前端主要通过词袋模型对图像输入进行处理，并应用于场景辨识以寻找机间闭环。由于基于视觉的场景辨识往往受到光照明暗变化的影响，本发明采用一种基于卷积神经网络的全局图像描述子NetVLAD进行机间闭环检测，可以有效减小光线对于场景辨识的影响，增强***的鲁棒性。

通过计算描述子之间的欧式距离以判断机器人是否经过了相似场景形成多机间闭环，这种方法能够以轻量级的数据传输量实现场景辨识功能。利用PNP算法进行几何验证，可以初步筛选闭环候选集。此后，利用两两一致性检测方法(Pairwise consistentmeasurement set maximization，简称PCM算法)，可以进一步剔除潜在的错误闭环，避免出现错误优化，影响***精度。目前被广泛使用的多机协同SLAM后端优化方案是分布式高斯赛德尔算法(Distributed Gauss-Seidel,简称DGS算法)。但是其算法对于第一步所估计的旋转向量精度要求较高，估计不准将导致收敛速度变慢。因此本发明在其基础上进行了改进，在不增加数据传输量的基础上提升了估计精度和估计效率。

发明内容

本发明的目的在于提出一种分布式多机协同视觉SLAM方法与***，对错误闭环有效剔除，采取分布式位姿图优化实现多机间协同定位，增强定位能力且对光照具有鲁棒性，数据传输量小，优化效率高。

一方面为实现上述目的，本发明提供了一种分布式多机协同视觉SLAM方法，包括以下步骤：

对机器人运行视觉SLAM程序，通过相机获取图像，基于所述图像提取NetVLAD全局描述子；

基于所述NetVLAD全局描述子利用欧式距离判断，满足通讯范围要求则机器人间进行通讯并且确定机器人间闭环；

基于确定机器人间闭环的机器人，利用DGS1算法进行分布式图优化，确定多个机器人间协同定位。

可选的，基于所述图像提取NetVLAD全局描述子的过程包括：***运行前，基于机器人使用场景训练相关图像获取NetVLAD测试数据集；***运行后，当机器人间通讯范围不在满足范围之内时，基于相机获取图像进行视觉SLAM程序加载，根据训练好的NetVlad测试数据集，每个机器人提取图像的NetVLAD全局描述子。

可选的，确定机器人间闭环的过程包括：当机器人间通讯范围在满足范围之内时，基于每个机器人提取图像的NetVLAD全局描述子，计算所述NetVLAD全局描述子并判断不同图像帧描述子间的欧式距离，确定机器人闭环并获取机器人闭环候选集。

可选的，基于所述机器人闭环候选集判断满足优化间隔过近情况包括：基于机器人闭环候选集，获取图像帧，基于图像帧数量判断相邻两次闭环间是否优化间隔过近并确定是否选择进行多个机器人间闭环图像优化。

可选的，基于PNP算法进行几何验证提取闭环候选集并估计多机间的相对位姿变换：利用OpenCV中的solvePnpRansac函数计算图像帧，获取产生闭环匹配的两机器人间的相对位姿变换矩阵，返回内点；当内点数量足够大，则判断为闭环候选帧；设定内点数量阈值进行筛选闭环候选帧，获取视觉特征，确定机器人间的相对位姿变换。

可选的，通过PCM算法，基于闭环候选集剔除错误闭环，确定优化闭环集，包括：基于几何验证提取的闭环候选集利用PCM算法进行两两一致性检测，通过设置PCM离群值阈值，剔除错误闭环，获取闭环优化候选集。

可选的，利用DGS1算法进行分布式图优化，确定多个机器人间协同定位包括：机器人间相互传输产生闭环的图像帧下的位姿信息，基于优化闭环候选集，通过DGS1算法进行分布式位姿图优化，对筛选后的闭环候选集进行旋转变量估计，优化平移变量，基于旋转变量和平移变量利用SOR算法求解位姿优化问题，反复交换机器人间的闭合环路的姿态，直到形成最优轨迹，获得优化后的旋转分量和平移分量，确定机器人定位。

另一方面为实现上述目的，本发明提供了一种分布式多机协同视觉SLAM***，包括：

获取数据模块：对机器人运行视觉SLAM程序，通过相机获取图像，基于所述图像提取NetVLAD全局描述子；

闭环识别模块：基于所述NetVLAD全局描述子，满足通讯范围要求则机器人间进行通讯，交换描述子信息并利用欧式距离判断不同机器人是否经过相同场景，以此确定机器人间闭环；

定位模块：基于确定机器人间闭环的机器人，利用DGS1算法进行分布式图优化，确定多个机器人间协同定位。

本发明技术效果：本发明公开了一种分布式多机协同视觉SLAM方法与***，解决了大规模作业环境下的机器人自主定位问题，首先本发明所描述的前端利用NetVLAD全局描述子实现了高效的机间闭环检测，解决了多机间的数据关联问题，较少的数据交换量减轻了多机***对于带宽的限制；其次本发明所描述的错误闭环剔除方法能够在不增加数据交换量的前提下实现精准的闭环提取，以避免错误闭环所带来的隐患；最后本发明通过优化的DGS1方法对多机交换的数据进行优化，在保护了数据传输隐私性的前提下，实现了每台机器人自身定位精度以及优化效率的提高。

附图说明

构成本申请的一部分的附图用来提供对本申请的进一步理解，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。在附图中：

图1为本发明实施例分布式多机协同视觉SLAM方法的流程示意图；

图2为本发明实施例一中通过机间闭环实现数据关联示意图；

图3为本发明实施例一中多机间闭环检测流程示意图；

图4为本发明实施例一中PCM离群值剔除算法原理示意图；

图5为本发明实施例二中分布式多机协同视觉SLAM***的结构示意图。

具体实施方式

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本申请。

需要说明的是，在附图的流程图示出的步骤可以在诸如一组计算机可执行指令的计算机***中执行，并且，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。

实施例一

如图1-4所示，本实施例中提供一种分布式多机协同视觉SLAM方法，包括以下步骤：

对机器人运行视觉SLAM程序，通过相机获取图像，基于所述图像提取NetVLAD全局描述子；

基于所述NetVLAD全局描述子，满足通讯范围要求则机器人间进行通讯，交换描述子信息并利用欧式距离判断不同机器人是否经过相同场景，以此确定机器人间闭环；

基于确定机器人间闭环的机器人，利用DGS1算法进行分布式图优化，确定多个机器人间协同定位。

进一步优化方案，基于所述图像提取NetVLAD全局描述子的过程包括：***运行前，基于机器人使用场景训练相关图像获取NetVLAD测试数据集；***运行后，当机器人间通讯范围不在满足范围之内时，基于相机获取图像进行视觉SLAM程序加载，根据训练好的NetVlad测试数据集，每个机器人提取图像的NetVLAD全局描述子。

进一步优化方案，确定机器人间闭环的过程包括：当机器人间通讯范围在满足范围之内时，基于每个机器人提取图像的NetVLAD全局描述子，计算所述NetVLAD全局描述子判断欧式距离，确定机器人闭环并获取机器人闭环候选集。

进一步优化方案，基于所述机器人闭环候选集判断满足优化间隔过近情况包括：基于机器人闭环候选集，获取图像帧，基于图像帧数量判断相邻两次闭环间是否优化间隔过近并确定是否选择进行多个机器人间闭环图像优化。

进一步优化方案，基于PNP算法进行几何验证提取闭环候选集并估计多机间的相对位姿变换：利用OpenCV中的solvePnpRansac函数计算图像帧，获取产生闭环匹配的两机器人间的相对位姿变换矩阵，返回内点；当内点数量足够大，则判断为闭环候选帧；设定内点数量阈值进行筛选闭环候选帧，获取视觉特征，确定机器人间的相对位姿变换。

进一步优化方案，通过PCM算法，基于闭环候选集剔除错误闭环，确定优化闭环集，包括：基于几何验证提取的闭环候选集利用PCM算法进行两两一致性检测，通过设置PCM离群值阈值，剔除错误闭环，获取闭环优化候选集。

进一步优化方案，利用DGS1算法进行分布式图优化，确定多个机器人间协同定位包括：机器人间相互传输产生闭环的图像帧下的位姿信息，基于优化闭环候选集，通过DGS1算法进行分布式位姿图优化，对筛选后的闭环候选集进行旋转变量估计，优化平移变量，基于旋转变量和平移变量利用SOR算法求解位姿优化问题，反复交换机器人间的闭合环路的姿态，直到形成最优轨迹，获得优化后的旋转分量和平移分量，确定机器人定位。

实施例二

如图5所示，本实施例中提供一种分布式多机协同视觉SLAM***，包括：

获取数据模块：对机器人运行视觉SLAM程序，通过相机获取图像，基于所述图像提取NetVLAD全局描述子；

闭环识别模块：基于所述NetVLAD全局描述子，满足通讯范围要求则机器人间进行通讯，交换描述子信息并利用欧式距离判断不同机器人是否经过相同场景，以此确定机器人间闭环；

定位模块：基于确定机器人间闭环的机器人，利用DGS1算法进行分布式图优化，确定多个机器人间协同定位。

步骤1：基于NetVlad全局描述子的视觉SLAM前端。首先根据应用场景训练NetVlad测试数据集并在SLAM开始前进行加载。当机器人间未发生相遇，即机器人间不在通讯范围内时，每个机器人独立运行视觉SLAM程序，对采集的图像进行NetVLAD全局描述子提取。

以下为具体过程：

基于机器人的使用场景训练NetVlad测试数据集，并导入到每个机器人的处理器中，在SLAM开始前进行加载。例如，使用从Oxford RobotCar数据集中的若干序列进行随机特征提取，训练NetVLAD测试数据集。当机器人未发生相遇时，每个机器人利用相机所采集环境的图像信息，独立运行视觉SLAM程序。例如，采用开源视觉SLAM方案ORB-SLAM2作为单个机器人的视觉SLAM方案。根据训练好的NetVlad测试数据集，每个机器人提取当前帧图像的全局描述子并存储。

步骤2：基于NetVLAD全局描述子实现机间闭环检测，利用PNP几何验证和PCM算法剔除潜在错误闭环。当机器人间发生相遇，即机器人间距离满足通讯范围时，机器人间传输上一次相遇后的所有图像描述子，并根据计算描述子的欧式范数确定闭环候选集。通过相邻两次闭环优化间所经历的图像帧数量进行判断是否出现相遇过近的情况，以决定是否进行此次优化。

发生相遇的机器人互相传输上一次相遇之后所有的图像描述子，并计算欧式范数初步判断是否曾到达过相似场景，产生闭环候选集，如式所示。其中

为机器人α的i帧图像的NetVLAD描述子，

为机器人β在j帧图像的NetVLAD描述子，τ_NetVLAD为人为设置的距离阈值，例如可以设置为0.15，阈值越大，闭环条件越宽松，反之越严格。注意，如果距离上一次机间闭环优化过近，将放弃此次优化，以避免计算负担过重以及产生病态图优化。以上一次进行机间闭环优化到本次闭环的图像帧数量进行判断是否出现相遇时间过近现象，例如设置最小图像帧数量为50帧，

对于产生机间闭环的图像帧，提取视觉特征以及相应的三维位置估计，利用OpenCV中的solvePnpRansac函数计算得到产生闭环匹配的两机器人间的相对位姿变换矩阵，同时返回一组内点。如果内点的数量足够大，可以认为该次闭环是比较成功的。通过设定内点数量阈值可以初步筛选部分闭环。此步可以求解两机器人间的相对位姿变换矩阵，因此***允许初始状态机器人随机摆放，无需事先测量机器人之间的位置关系。

利用PCM算法，在闭环候选集中进一步剔除潜在的错误闭环，确定最终用于优化的闭环集，增强***的鲁棒性，其算法原理如图3所示。PCM用于检查机间闭环的一致性，与此度量闭环是否正确。式中||·||_Σ代表马氏距离，

表示机器人α由j帧到i帧的相对位姿变换矩阵，其他符号具有类似的含义，γ则代表PCM检测阈值，例如可以设置为0.01。阈值越高表明对于闭环的要求越宽松，反之越严格，

步骤3：由步骤2可以获得具有良好机间闭环的机器人编号，具有机间闭环的机器人均将执行以下分布式图优化过程。机器人间相互传输产生闭环的图像帧下的位姿信息，基于DGS1算法进行分布式位姿图优化。由DGS算法，多机协同SLAM有以下模型，对具有机间闭环约束的α_i和β_j，有相对位姿变换矩阵：

机器人间相互传输产生闭环的图像帧下的位姿信息，

其中

表示机器人α在i帧下的旋转矩阵，R_ò和t_ò是测量噪声。

进行DGS1算法的第一步，由式计算优化后的旋转向量，式中

为旋转噪声。代价函数由两部分组成，其中第一部分为本次相遇所产生的的机间闭环约束＜α_i,β_j＞等。第二部分为虚拟闭环约束，对于存在不止一个机间闭环匹配的机器人α，例如有约束＜α_i,β_j＞、＜α_k,χ_l＞，可以构成单机相对位姿变换矩阵约束＜α_i,α_k＞。对于只存在单个机间闭环匹配的机器人，利用上一次相遇过程中的闭环约束，可以构成第二项约束，且无需额外计算。虚拟闭环约束的引入有利于增强***的估计的全局一致性，进一步利用历史闭环信息，提升分布式图优化中第一步旋转估计的精度，从而引导更快的收敛速度，

进行DGS1算法的第二步，代入优化后的旋转变量，初步优化平移变量。DGS算法的第二步，利用第一步所得的旋转估计初始化旋转变量，并采用SOR算法求解全姿态优化问题。SOR的迭代收敛速度与初始优化顺序的选择有关，从加快算法收敛角度考虑，本发明对SOR算法进行改进，将最速下降(Steep Descent，SD算法)和SOR算法进行结合，利用SD算法为SOR算法提供有效的搜索方向，加快收敛速度，提升算法性能。SD算法可以快速接近最优解，但是在最优解附近的收敛速度将会减慢，与SOR算法互补，因此将SD算法与SOR算法结合有利于提升算法效率。DGS1第二步在第一步优化的旋转变量的基础上，采用最速下降法求解经过初步优化的平移变量，优化函数为式，其中

为平移噪声，

进行DGS1算法的第三步，代入第一步所得的初始旋转估计以及第二步所得的初始平移估计，用SOR算法解决全姿态优化问题，反复地交换机器人间的闭合环路所涉及的姿态，直到它们对最优轨迹估计达成共识，以提升机器人的定位精度，得到优化后的旋转分量和平移分量。

以上所述，仅为本申请较佳的具体实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应该以权利要求的保护范围为准。

Claims (8)

1.一种分布式多机协同视觉SLAM方法，其特征在于，包括以下步骤：

对机器人运行视觉SLAM程序，通过相机获取图像，基于所述图像提取NetVLAD全局描述子；

基于所述NetVLAD全局描述子利用欧式距离判断，满足通讯范围要求则机器人间进行通讯并且确定机器人间闭环；

基于确定机器人间闭环的机器人，利用DGS1算法进行分布式图优化，确定多个机器人间协同定位；

机器人间相互传输产生闭环的图像帧下的位姿信息，基于DGS1算法进行分布式位姿图优化；多机协同SLAM有以下模型，对具有机间闭环约束的α_i和β_j，有相对位姿变换矩阵：

机器人间相互传输产生闭环的图像帧下的位姿信息

其中

表示机器人α在i帧下的旋转矩阵，R_∈和t_∈是测量噪声；

进行DGS1算法的第一步，由下列公式计算优化后的旋转向量

式中

为旋转噪声；代价函数由两部分组成，其中第一部分为本次相遇所产生的机间闭环约束＜α_i,β_j＞；第二部分为虚拟闭环约束，对于存在不止一个机间闭环匹配的机器人α，有约束＜α_i,β_j＞、＜α_k,χ_l＞，构成单机相对位姿变换矩阵约束＜α_i,α_k＞；对于只存在单个机间闭环匹配的机器人，利用上一次相遇过程中的闭环约束，构成第二项约束，且无需额外计算；虚拟闭环约束的引入有利于增强***的估计的全局一致性，进一步利用历史闭环信息，提升分布式图优化中第一步旋转估计的精度，从而引导更快的收敛速度；

进行DGS1算法的第二步，代入优化后的旋转变量，初步优化平移变量；利用第一步所得的旋转估计初始化旋转变量，并采用SOR算法求解全姿态优化问题；SOR的迭代收敛速度与初始优化顺序的选择有关，从加快算法收敛角度考虑，对SOR算法进行改进，将最速下降SD算法和SOR算法进行结合，利用最速下降SD算法为SOR算法提供有效的搜索方向，加快收敛速度，提升算法性能；最速下降SD算法快速接近最优解，但是在最优解附近的收敛速度将减慢，与SOR算法互补，因此将最速下降SD算法与SOR算法结合有利于提升算法效率；DGS1第二步在第一步优化的旋转变量的基础上，采用最速下降SD算法求解经过初步优化的平移变量，优化函数为下式，其中

为平移噪声，

进行DGS1算法的第三步，代入第一步所得的初始旋转估计以及第二步所得的初始平移估计，用SOR算法解决全姿态优化问题，反复地交换机器人间的闭合环路所涉及的姿态，直到它们对最优轨迹估计达成共识，以提升机器人的定位精度，得到优化后的旋转分量和平移分量。

2.如权利要求1所述的分布式多机协同视觉SLAM方法，其特征在于，基于所述图像提取NetVLAD全局描述子的过程包括：***运行前，基于机器人使用场景训练相关图像获取NetVLAD测试数据集；***运行后，当机器人间通讯范围不在满足范围之内时，基于相机获取图像进行视觉SLAM程序加载，根据训练好的NetVLAD测试数据集，每个机器人提取图像的NetVLAD全局描述子。

3.如权利要求2所述的分布式多机协同视觉SLAM方法，其特征在于，确定机器人间闭环的过程包括：当机器人间通讯范围在满足范围之内时，基于每个机器人提取图像的NetVLAD全局描述子，计算所述NetVLAD全局描述子判断欧式距离，确定机器人闭环并获取机器人闭环候选集。

4.如权利要求3所述的分布式多机协同视觉SLAM方法，其特征在于，基于所述机器人闭环候选集判断满足优化间隔过近情况包括：基于机器人闭环候选集，获取图像帧，基于图像帧数量判断相邻两次闭环间是否优化间隔过近并确定是否选择进行多个机器人间闭环图像优化。

5.如权利要求4所述的分布式多机协同视觉SLAM方法，其特征在于，基于PNP算法进行几何验证提取闭环候选集并估计多机间的相对位姿变换：利用OpenCV中的solvePnpRansac函数计算图像帧，获取产生闭环匹配的两机器人间的相对位姿变换矩阵，返回内点；当内点数量足够大，则判断为闭环候选帧；设定内点数量阈值进行筛选闭环候选帧，获取视觉特征，确定机器人间的相对位姿变换。

6.如权利要求5所述的分布式多机协同视觉SLAM方法，其特征在于，通过PCM算法，基于闭环候选集剔除错误闭环，确定优化闭环集，包括：基于几何验证提取的闭环候选集利用PCM算法进行两两一致性检测，通过设置PCM离群值阈值，剔除错误闭环，获取闭环优化候选集。

7.如权利要求6所述的分布式多机协同视觉SLAM方法，其特征在于，利用DGS1算法进行分布式图优化，确定多个机器人间协同定位包括：机器人间相互传输产生闭环的图像帧下的位姿信息，基于优化闭环候选集，通过DGS1算法进行分布式位姿图优化，对筛选后的闭环候选集进行旋转变量估计，优化平移变量，基于旋转变量和平移变量利用SOR算法求解位姿优化问题，反复交换机器人间的闭合环路的姿态，直到形成最优轨迹，获得优化后的旋转分量和平移分量，确定机器人定位。

8.一种分布式多机协同视觉SLAM***，其特征在于，包括：

获取数据模块：对机器人运行视觉SLAM程序，通过相机获取图像，基于所述图像提取NetVLAD全局描述子；

闭环识别模块：基于所述NetVLAD全局描述子，满足通讯范围要求则机器人间进行通讯，交换描述子信息并利用欧式距离判断不同机器人是否经过相同场景，以此确定机器人间闭环；

定位模块：基于确定机器人间闭环的机器人，利用DGS1算法进行分布式图优化，确定多个机器人间协同定位；机器人间相互传输产生闭环的图像帧下的位姿信息，基于DGS1算法进行分布式位姿图优化；多机协同SLAM有以下模型，对具有机间闭环约束的α_i和β_j，有相对位姿变换矩阵：

机器人间相互传输产生闭环的图像帧下的位姿信息

其中

表示机器人α在i帧下的旋转矩阵，R_∈和t_∈是测量噪声；

进行DGS1算法的第一步，由下列公式计算优化后的旋转向量

式中

为旋转噪声；代价函数由两部分组成，其中第一部分为本次相遇所产生的机间闭环约束＜α_i,β_j＞；第二部分为虚拟闭环约束，对于存在不止一个机间闭环匹配的机器人α，有约束＜α_i,β_j＞、＜α_k,χ_l＞，构成单机相对位姿变换矩阵约束＜α_i,α_k＞；对于只存在单个机间闭环匹配的机器人，利用上一次相遇过程中的闭环约束，构成第二项约束，且无需额外计算；虚拟闭环约束的引入有利于增强***的估计的全局一致性，进一步利用历史闭环信息，提升分布式图优化中第一步旋转估计的精度，从而引导更快的收敛速度；

进行DGS1算法的第二步，代入优化后的旋转变量，初步优化平移变量；利用第一步所得的旋转估计初始化旋转变量，并采用SOR算法求解全姿态优化问题；SOR的迭代收敛速度与初始优化顺序的选择有关，从加快算法收敛角度考虑，对SOR算法进行改进，将最速下降SD算法和SOR算法进行结合，利用最速下降SD算法为SOR算法提供有效的搜索方向，加快收敛速度，提升算法性能；最速下降SD算法快速接近最优解，但是在最优解附近的收敛速度将减慢，与SOR算法互补，因此将最速下降SD算法与SOR算法结合有利于提升算法效率；DGS1第二步在第一步优化的旋转变量的基础上，采用最速下降SD算法求解经过初步优化的平移变量，优化函数为下式，其中

为平移噪声，

进行DGS1算法的第三步，代入第一步所得的初始旋转估计以及第二步所得的初始平移估计，用SOR算法解决全姿态优化问题，反复地交换机器人间的闭合环路所涉及的姿态，直到它们对最优轨迹估计达成共识，以提升机器人的定位精度，得到优化后的旋转分量和平移分量。

Images