CN115965673B - 基于双目视觉的集中式多机器人定位方法 - Google Patents

Abstract

本发明公开了基于双目视觉的集中式多机器人定位方法，以双目图像作为基本输入，使用双目视觉的SLAM算法完成集中式多机器人的定位建图。客户端接收到双目图像之后，会在跟踪线程以及局部建图线程进行客户端位姿的计算，同时将数据通过交流线程发送给服务端。服务端会开启融合检测线程，在这个线程会对客户端之间的地图进行匹配融合，融合完成之后会进行全局的优化，得到全局一致的客户端位姿。本发明解决了单目集中式多机SLAM框架中尺度不固定，鲁棒性较差，跟踪稳定性较差的问题。

Description

基于双目视觉的集中式多机器人定位方法

技术领域

本发明涉及基于双目视觉的集中式多机器人定位方法，属于同步定位与建图技术(SLAM)领域，具体涉及一种基于双目视觉的集中式多机器人SLAM算法。

背景技术

近年来，由于机器人技术的不断发展，对于多机器人的同步定位与建图技术得到了学术界和工业界极大的关注。在基于视觉的集中式多机器人的定位中，机器人需要通过传感器获取相关的视觉信息完成自身的定位。目前相关研究采用的大多是单目相机作为机器人的传感器。然而，单目相机具有无法计算场景的真实深度、尺度漂移等缺点，这极大的降低了系统的稳定性、鲁棒性。因此，通过设计基于新的传感器的多机器人定位与建图算法赋予多机器人系统更好的性能是十分重要的。

发明内容

有鉴与此，本发明提供了基于双目视觉的集中式多机器人定位方法，能够在机器人定位中提供真实的深度信息，同时由于双目的尺度确定，使得定位结果不会出现尺度漂移的问题，极大的提升了定位结果的稳定性与鲁棒性。

实现本发明的技术方案如下：

基于双目视觉的集中式多机器人定位方法，多机器人的个数为N个，i＝1，2，3…，N，该方法的步骤包括：

步骤1，使用机器人i的双目相机获取灰度图像，并提取灰度图像的特征点，采用视差测距的方式计算特征点的深度，并根据计算得到的特征点的深度生成地图点，所有的地图点集合为地图i；

步骤2，将步骤1生成的地图i中的地图点投影到机器人i的双目相机的当前帧，并对当前帧进行基于双目视觉的位姿优化，筛选位姿优化后的当前帧作为关键帧，在完成关键帧的筛选时会生成新的地图点A，从生成的新的地图点A中选出符合要求的地图点加入到步骤1生成的地图i中；

步骤3，利用步骤2中生成的关键帧的共视关系生成新的地图点B，并将生成的新的地图点B加入到步骤1的地图i中，然后优化新的地图点B和关键帧的位姿，并进行地图i中关键帧与地图点的冗余删除操作；

步骤4，客户端将加入了地图点A和地图点B的地图i发送到服务端，服务端对接收到的地图i进行sim3变换，服务端根据sim3变换的结果将地图1、地图2、…地图i、地图N进行融合，得到全局地图，并对得到的全局地图进行基于双目相机的全局优化，优化地图中所有的关键帧以及地图点，完成基于双目视觉的集中式多机器人定位。

所述的步骤1中，提取特征点并计算特征点深度的方法如下：

客户端读入使用机器人i的双目相机获取的灰度图像，然后利用网格化的方法提取特征，并进行双目的匹配，计算深度之前要求成功匹配的特征点数大于600，随后利用双目的视差公式(1)完成特征点深度的计算：

其中，Z为特征点的深度，u_L-u_R为特征点的视差，f_x为相机的焦距，b为双目的基线长度；u_L为特征点在左目相机中的像素横坐标，u_R为特征点在右目相机中的像素横坐标；

所述的步骤1中，生成地图点时利用双目匹配算法以及三角化直接生成具有绝对尺度的地图点；

所述的步骤1中，生成地图点的方法如下：

已知特征点的深度，根据相机投影公式(2)计算地图点在相机坐标系下的坐标，相机投影公式为：

Zp＝KP

其中，P表示相机坐标系下的地图点坐标，Z表示相机坐标系下地图点的深度，K表示相机的内参矩阵，p表示该地图点的像素坐标；

上述公式变换为：

其中，X、Y、Z为地图点在相机坐标系下的坐标，u、v分别是在像素平面的横坐标与纵坐标，f_x、f_y、c_x和c_y为相机内参，通过该公式，便能够求出地图点在相机坐标系下的坐标[X Y Z]^T；

所述的步骤2中，对当前帧进行基于双目视觉的位姿优化的方法为：

对当前帧F在优化函数中加入右目坐标的优化约束，增加优化目标维度，对当前帧F位姿的优化公式为：

其中，ρ为Huber损失函数，x^j表示第j个特征点在当前帧F以及对应右目中的像素坐标，X^j表示对应的第j个地图点在世界坐标系下的坐标，R_cw表示世界坐标系到当前帧F的旋转变换，t_cw表示世界坐标系到当前帧F的平移变换，f定义如下：

所述的步骤2中，筛选位姿优化后的当前帧作为关键帧的方法为：关键帧筛选需要满足的条件为：距离上次筛选出关键帧已经过去mMaxFrames个普通帧或者属于当前帧的地图点数量少于上一个关键帧拥有地图点数量的75％；

所述的步骤2中，从生成的新的地图点A中选出符合要求的地图点的方法为：去除深度过大的地图点，具体方法为：把地图点A按照深度值升序排列，并设定判别阈值，超过该阈值的地图点会在地图中去除，对应的判别公式如下：

其中，j为地图点的序列号，g(j)为第j个地图点是否采纳的标志位，g(j)＝1时表示采纳，g(j)＝0时表示不采纳；

所述的步骤3中，将生成的新的地图点B加入到步骤1的地图i中的方法为：

在双目传感器的前提下，采取利用12个共视关键帧来生成新的地图点，同时如果关键帧距离小于相机的基线，放弃这个关键帧，最后检验双目相机的重投影误差去剔除一些外点，这里的误差变量也包含了右目坐标的误差，所以用卡方检验有三个自由度、置信度为0.95，选定阈值为7.81，判断标准为：

其中，e_x1为左目图像像素坐标误差，e_y1为右目图像像素坐标误差，σ为方差，大小与特征点所处的平面有关；

所述的步骤3中，优化新的地图点B和关键帧的位姿的方法为：

进行关键帧与地图点的局部优化的公式如下所示:

其中,为具有共视关系的关键帧集合,R_c、t_c为共识关键帧的旋转平移变换，Xⁱ为共识关键帧所拥有的地图点，/>为关键帧生成的地图点B，/>为看到/>但是不属于的其他关键帧,R_m表示第m个关键帧从世界坐标系到当前关键帧的旋转变换，t_m为第m个关键帧从世界坐标系到当前关键帧的平移变换，E_mj为第j个地图点在第m个关键帧的重投影误差；

在基于双目的优化中，不仅仅考虑左目图像的像素坐标，也会考虑对应的特征点的右目坐标，优化的参考值会变成三维，自由度变为三；

所述的步骤3中，进行地图i中关键帧与地图点的冗余删除操作的方法为：

统计每个关键帧所拥有的地图点被观测到的次数，如果某一个关键帧拥有的85％的地图点被看见了超过四次，那么该关键帧是冗余的并执行关键帧删除操作，判别公式如下：

其中，F(m)为第m个关键帧删除的标志位，为第m个关键帧的所有地图点数，第m个关键帧的被观测次数超过四次的地图点数，计算公式如下：

其中，表示在第m个关键帧中的第j个地图点是否为被多次观测地图点；

在基于双目的关键帧删除操作中，对地图点距离相机的距离进行判定，如果地图点距离相机超过的45倍的基线，则忽略该地图点，不会参与关键帧的删除操作，当关键帧被删除之后，对应地图点的观测次数需要修改，判定该地图点是否在删除的关键帧的右目中也被观测到，如果被观测到，那么该地图点的观测次数会减去2，否则观测次数减去1；

对新添加的地图点进行删除操作：删除一些外点来保证轨迹位姿估计的准确性，对于双目传感器设定观测阈值为4，当一个地图点被少于四个关键帧观测到的时候从全局地图中删除该地图点，判断公式如下：

其中，obs(j)为第j个地图点的被观测次数，f(j)为是否删除地图点的标志。

所述的步骤4中，客户端将加入了地图点A和地图点B的地图i发送到服务端的方法为：

机器人i将相关的关键帧、地图点信息转换为rosmsg格式，通过rostopic进行发送，通常在基于双目的多机器人中，客户端需要转换的信息包括地图点的观测等基本信息以及每个关键帧的右目坐标的信息、对应特征点的深度信息以及双目相机的基线，同时服务端在接受到了客户端发来的消息之后，将相关的rosmsg信息再此逆向转换为服务端能够识别的信息，此外，重新统计地图点观测，对于双目系统，对于地图点的观测统计会考虑他在右目中是否会存在，对于右目中也存在的地图点，该地图点的观测次数设置为2，判别公式如下：

其中，为第j个地图点在第m个关键帧的观测次数，/>为第j个地图点在第m个关键帧的右目中的横坐标；

通过该处的转换，服务端得到了机器人i发送的地图i；

所述的步骤4中，进行地图的融合的方法为：

检验地图M^a、地图M^b之间是否有重叠区域，如果有重叠区域计算该区域的sim3变换，同时利用双目相机尺度固定特性，把尺度因子置1，将问题转化为SE3求解，求出客户端之间的变换之后，进行地图之间的融合，并且各客户端之间具有相同的尺度，初始化一个全新的地图M^S，该地图M^S会和两个客户端的地图M^a与M^b中的一个完全重合，不妨设M^a与M^S重合，则相似变换为且旋转量/>平移量/>尺度而通过SE3计算的变换为/>由于双目的尺度固定，所以/>同时保存该变换，之后客户端b估计出自身第m个关键帧位姿/>与第j个地图点位置/>的位置经过变换/>计算出在地图M^S下的关键帧与地图点位置，形成全局一致的地图，转换公式如下所示：

所述的步骤4中，进行全局优化的方法为：对全局的地图点以及关键帧进行优化，对于双目相机而言，优化6个自由度的变量，同时，设置的鲁棒核函数的属性为三个自由度。

有益效果

(1)相比于使用单目相机的传感器集中式多机SLAM算法，双目相机不仅会在局部建图线程中依靠共识关键帧生成新的地图点，在追踪线程中生成新的关键帧的时候也会直接依靠深度生成更多的地图点，有效的提高了跟踪的稳定性。

(2)由于双目相机能够直接计算出特征点的有效深度，所以在初始化阶段能够直接生成地图点完成系统的初始化，提高了系统初始化的快速性。

(3)由于双目相机具有绝对尺度，所以在运行过程中不会出现尺度漂移问题，同时能够还原出真实世界的尺度，提高了系统的鲁棒性以及实用性。

(4)本发明公开了基于双目视觉的集中式多机器人定位方法，以双目图像作为基本输入，使用双目视觉的SLAM算法完成集中式多机器人的定位建图。客户端接收到双目图像之后，会在跟踪线程以及局部建图线程进行客户端位姿的计算，同时将数据通过交流线程发送给服务端。服务端会开启融合检测线程，在这个线程会对客户端之间的地图进行匹配融合，融合完成之后会进行全局的优化，得到全局一致的客户端位姿。本发明解决了单目集中式多机SLAM框架中尺度不固定，鲁棒性较差，跟踪稳定性较差的问题。

附图说明

图1为跟踪过程效果图；

图2为多机地图融合未全局优化效果图；

图3为多机地图融合全局优化效果图；

图4为精度效果图。

具体实施方式

下图结合附图并举实例，对本发明进行详细描述。

本发明提供了基于双目视觉的集中式多机器人定位方法，其详细实施方式如下：

步骤1：客户端输入双目图像，完成客户端初始化；

所述的步骤1中，提取特征点并计算特征点深度方法如下：

客户端读入双目图像，然后利用网格化的方法提取特征，并进行双目的匹配，计算深度之前要求成功匹配的特征点数大于600，随后利用双目的视差公式完成地图点深度的计算：

其中，Z为特征点的深度，u_L-u_R为特征点的视差，f_x为相机的焦距、b为双目的基线长度；u_L为特征点在左目相机中的像素横坐标，u_L为特征点在右目相机中的像素横坐标；

所述的步骤1中，生成地图点的方法如下：

已知特征点的深度，根据相机投影公式计算地图点在相机坐标系下的坐标，相机投影公式为：

Zp＝KP

其中，P表示相机坐标系下的地图点坐标，Z表示相机坐标系下地图点的深度，K表示相机的内参矩阵，p表示该地图点的像素坐标；

上述公式变换为：

其中，X、Y、Z为地图点在相机坐标系下的坐标，u、v分别是在像素平面的横坐标与纵坐标，f_x、f_y、c_x和c_y为相机内参，通过该公式，便能够求出地图点在相机坐标系下的坐标[X Y Z]^T；

步骤2：开启跟踪线程，进行基于双目视觉的位姿优化以及关键帧的地图点的生成；

所述的步骤2中，进行基于双目视觉位姿优化的方法为：

在跟踪线程中，对当前帧F在优化函数中加入右目坐标的优化约束，增加优化目标维度，对当前帧F位姿的优化公式为：

其中，ρ为Huber损失函数，x^j表示第j个特征点在当前帧F以及对应右目中的像素坐标，X^j表示对应的第j个地图点在世界坐标系下的坐标，R_cw表示世界坐标系到当前帧F的旋转变换、t_cw表示世界坐标系到当前帧F的平移变换，f定义如下：

所述步骤2中，进行关键帧筛选及新建地图点方法如下：

关键帧筛选需要满足的条件为：距离上次筛选出关键帧已经过去mMaxFrames个普通帧或者属于当前帧的地图点数量少于上一个关键帧拥有地图点数量的75％。采取步骤1所述方法生成新地图点，同时去除深度过大的地图点，把地图点按照深度值升序排列，并设定判别阈值，超过该阈值的地图点会在地图中去除，对应的判别公式如下：

其中，i为地图点的序列号，g(i)为第i个地图点是否采纳的标志位，g(i)＝1时表示采纳，g(i)＝0时表示不采纳；

跟踪过程如图1所示，其中灰色的点为成功三角化的地图点对应当前帧中的特征点；

步骤3，客户端与服务端开启局部建图线程，利用共视关系生成新的地图点B，进行关键帧位姿以及地图点位置的优化并删除地图i中冗余的关键帧与地图点；

步骤3.1，利用当前关键帧的共视关键帧三角化生成更多的地图点B，在双目传感器的前提下，采取利用12个共视关键帧来生成新的地图点，同时如果关键帧距离小于相机的基线，放弃这个关键帧。最后检验双目相机的重投影误差去剔除一些外点，这里的误差变量也包含了右目坐标的误差，所以用卡方检验有三个自由度，置信度为0.95所以选定阈值为7.81。判断标准为：

其中，e_x1为左目图像像素坐标误差，e_y1为右目图像像素坐标误差，σ为方差，大小与特征点所处的平面有关；

步骤3.2，随后进行关键帧与地图点的局部优化，公式如下所示:

其中,为具有共视关系的关键帧集合,/>为关键帧生成的地图点B，/>为看到/>但是不属于/>的其他关键帧。

在基于双目的优化中，不仅仅考虑左目图像的像素坐标，也会考虑对应的特征点的右目坐标，优化的参考值会变成三维，自由度变为三。

步骤3.3，进行关键帧的删除操作，统计每个关键帧所拥有的地图点被观测到的次数，如果某一个关键帧拥有的85％的地图点被看见了超过四次，那么认为该关键帧是冗余的并执行关键帧删除操作，判别公式如下：

其中，F(m)为第m个关键帧删除的标志位，为第m个关键帧的所有地图点数，第m个关键帧的被观测次数超过四次的地图点数，计算公式如下：

其中，表示在第m个关键帧中的第k个地图点是否为被多次观测地图点。

在基于双目的关键帧删除操作中，对地图点距离相机的距离进行判定，如果地图点距离相机超过的45倍的基线，则忽略该地图点，不会参与关键帧的删除操作。当关键帧被删除之后，对应地图点的观测次数需要修改，判定该地图点是否在删除的关键帧的右目中也被观测到。如果被观测到，那么该地图点的观测次数会减去2，否则观测次数减去1。

步骤3.4：对新添加的地图点进行删除操作，删除一些外点来保证轨迹位姿估计的准确性，对于双目传感器设定观测阈值为4，当一个地图点被少于四个关键帧观测到的时候从全局地图中删除该地图点，判断公式如下：

其中，obs(i)为第n个地图点的被观测次数，f(n)为是否删除地图点的标志。

步骤4，开启融合检测线程，客户端接收机器人i的地图i，进行不同客户端地图的匹配融合以及全局的优化。

步骤4.1：机器人i将相关的关键帧、地图点信息转换为rosmsg格式，通过rostopic进行发送，通常在基于双目的多机器人中，客户端需要转换的信息包括地图点的观测等基本信息以及每个关键帧的右目坐标的信息、对应特征点的深度信息以及双目相机的基线，同时服务端在接受到了客户端发来的消息之后，将相关的rosmsg信息再此逆向转换为服务端能够识别的信息。此外，重新统计地图点观测，对于双目系统，对于地图点的观测统计会考虑他在右目中是否会存在，对于右目中也存在的地图点，该地图点的观测次数设置为2，判别公式如下：

其中，为第k个地图点在第m个关键帧的观测次数，/>为第k个地图点在第m个关键帧的右目中的横坐标；

通过该处的转换，服务端得到了机器人i发送的地图i。

步骤4.2：进行地图的融合，检验地图M^a、地图M^b之间是否有重叠区域，如果有重叠区域计算该区域的SIM3变换，同时利用双目相机尺度固定特性，把尺度因子置1，将问题转化为SE3求解。求出客户端之间的变换之后，进行地图之间的融合，并且各客户端之间具有相同的尺度。初始化一个全新的地图M^S，该地图会和两个客户端的地图M^a与M^b中的一个完全重合，不妨设M^a与M^S重合，则相似变换为且旋转量/>平移量尺度/>而通过SE3计算的变换为/>由于双目的尺度固定，所以/>同时保存该变换，之后客户端b估计出自身第i个关键帧位姿/>与第j个地图点位置/>的位置经过变换/>可计算出在地图M^S下的关键帧与地图点位置，形成全局一致的地图，转换公式如下所示：

步骤4.3，随后我们会对全局的地图点以及关键帧进行优化，对于双目相机而言，优化6个自由度的变量。同时，设置的鲁棒核函数的属性为三个自由度。

最后对基于双目视觉的多机器人SLAM定位算法进行数据集以及实际场景的测试。

在实际场景测试中，我们利用两个客户端、一个服务端进行定位建图。如图2为客户端融合之后的示意图，其中左边为客户端A的轨迹与地图点，右边为客户端B的轨迹与地图点，可以看出这是一个全局一致的稀疏点云图，并且两个客户端的关键帧与轨迹在同一个世界坐标系下，不过此时的全局地图没有经过优化操作。如图3是经过优化的全局地图，相较于图2，两个客户端经过相同墙体的点云与轨迹更接近同一个水平面。

在数据集的测试中，利用三个客户端和一个服务端在EuRoc数据集上进行定位建图。如图4所示，最终的位姿最小误差在0.006m，同时该算法的均值误差在0.04m左右，基本满足多机器人定位的精度要求。

通过真实场景与数据集的测试，可以说明，使用基于双目视觉的集中式多机器人SLAM算法，能够在保证鲁棒性与稳定性的前提下准确的完成多机器人的定位，可以为多机器人的定位导航提供准确的位置信息。

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.基于双目视觉的集中式多机器人定位方法，其特征在于该方法的步骤包括：

步骤1，使用机器人i的双目相机获取灰度图像，并提取灰度图像的特征点，采用视差测距的方式计算特征点的深度，并根据计算得到的特征点的深度生成地图点，所有的地图点集合为地图i；多机器人的个数为N个，i＝1，2，3…，N；

步骤2，将步骤1生成的地图i中的地图点投影到机器人i的双目相机的当前帧，并对当前帧进行基于双目视觉的位姿优化，筛选位姿优化后的当前帧作为关键帧，在完成关键帧的筛选时会生成新的地图点A，从生成的新的地图点A中选出符合要求的地图点加入到步骤1生成的地图i中；

步骤3，利用步骤2中生成的关键帧的共视关系生成新的地图点B，并将生成的新的地图点B加入到步骤1的地图i中，然后优化新的地图点B和关键帧的位姿，并进行地图i中关键帧与地图点的冗余删除操作；

步骤4，客户端将加入了地图点A和地图点B的地图i发送到服务端，服务端对接收到的地图i进行sim3变换，服务端根据sim3变换的结果将地图1、地图2、…地图i、地图N进行融合，得到全局地图，并对得到的全局地图进行基于双目相机的全局优化，优化地图中所有的关键帧以及地图点，完成基于双目视觉的集中式多机器人定位；

所述的步骤2中，从生成的新的地图点A中选出符合要求的地图点的方法为：把地图点A按照深度值升序排列，并设定判别阈值，超过该阈值的地图点会在地图中去除，对应的判别公式如下：

其中，j为地图点的序列号，g(j)为第j个地图点是否采纳的标志位，g(j)＝1时表示采纳，g(j)＝0时表示不采纳；

所述的步骤3中，将生成的新的地图点B加入到步骤1的地图i中的方法为：

在双目传感器的前提下，采取利用12个共视关键帧来生成新的地图点，同时如果关键帧距离小于相机的基线，放弃这个关键帧，最后检验双目相机的重投影误差去剔除外点，判断标准为：

其中，e_x1为左目图像像素坐标误差，e_y1为右目图像像素坐标误差，σ为方差，大小与特征点所处的平面有关；

所述的步骤4中，进行地图的融合的方法为：

检验地图M^a、地图M^b之间是否有重叠区域，如果有重叠区域计算该区域的sim3变换，同时利用双目相机尺度固定特性，把尺度因子置1，将问题转化为se3求解，求出客户端之间的变换之后，进行地图之间的融合，并且各客户端之间具有相同的尺度，初始化一个全新的地图M^S，该地图M^S会和两个客户端的地图M^a与M^b中的一个完全重合，不妨设M^a与M^S重合，则相似变换为且旋转量/>平移量/>尺度/>而通过SE3计算的变换为/>由于双目的尺度固定，所以/>同时保存该变换，之后客户端b估计出自身第m个关键帧位姿/>与第j个地图点位置/>的位置经过变换计算出在地图M^S下的关键帧与地图点位置，形成全局一致的地图，转换公式如下所示：

进行全局优化的方法为：对全局的地图点以及关键帧进行优化，对于双目相机而言，优化6个自由度的变量，同时，设置的鲁棒核函数的属性为三个自由度。

2.根据权利要求1所述的基于双目视觉的集中式多机器人定位方法，其特征在于：

所述的步骤1中，提取特征点并计算特征点深度的方法如下：

客户端读入使用机器人i的双目相机获取的灰度图像，然后利用网格化的方法提取特征，并进行双目的匹配，利用双目的视差公式(1)完成特征点深度的计算：

其中，Z为特征点的深度，u_L-u_R为特征点的视差，f_x为相机的焦距，b为双目的基线长度；u_L为特征点在左目相机中的像素横坐标，u_R为特征点在右目相机中的像素横坐标。

3.根据权利要求1或2所述的基于双目视觉的集中式多机器人定位方法，其特征在于：

所述的步骤1中，生成地图点的方法如下：

已知特征点的深度，根据相机投影公式(2)计算地图点在相机坐标系下的坐标，相机投影公式为：

Zp＝KP

其中，P表示相机坐标系下的地图点坐标，Z表示相机坐标系下地图点的深度，K表示相机的内参矩阵，p表示该地图点的像素坐标；

上述公式变换为：

其中，X、Y、Z为地图点在相机坐标系下的坐标，u、v分别是在像素平面的横坐标与纵坐标，f_x、f_y、c_x和c_y为相机内参，通过该公式，便能够求出地图点在相机坐标系下的坐标[X YZ]^T。

4.根据权利要求1所述的基于双目视觉的集中式多机器人定位方法，其特征在于：

所述的步骤2中，对当前帧进行基于双目视觉的位姿优化的方法为：

对当前帧F在优化函数中加入右目坐标的优化约束，增加优化目标维度，对当前帧F位姿的优化公式为：

其中，ρ为Huber损失函数，x^j表示第j个特征点在当前帧F以及对应右目中的像素坐标，X^j表示对应的第j个地图点在世界坐标系下的坐标，R_cw表示世界坐标系到当前帧F的旋转变换，t_cw表示世界坐标系到当前帧F的平移变换，f定义如下：

所述的步骤2中，筛选位姿优化后的当前帧作为关键帧的方法为：关键帧筛选需要满足的条件为：距离上次筛选出关键帧已经过去5个普通帧或者属于当前帧的地图点数量少于上一个关键帧拥有地图点数量的75％。

5.根据权利要求1所述的基于双目视觉的集中式多机器人定位方法，其特征在于：

所述的步骤3中，优化新的地图点B和关键帧的位姿的方法为：

进行关键帧与地图点的局部优化的公式如下所示:

其中,为具有共视关系的关键帧集合,R_c、t_c为共识关键帧的旋转平移变换，Xⁱ为共识关键帧所拥有的地图点，/>为关键帧生成的地图点B，/>为看到/>但是不属于/>的其他关键帧,R_m表示第m个关键帧从世界坐标系到当前关键帧的旋转变换，t_m为第m个关键帧从世界坐标系到当前关键帧的平移变换，E_mj为第j个地图点在第m个关键帧的重投影误差。

6.根据权利要求1所述的基于双目视觉的集中式多机器人定位方法，其特征在于：

所述的步骤3中，进行地图i中关键帧与地图点的冗余删除操作的方法为：

对新添加的关键帧进行删除操作：统计每个关键帧所拥有的地图点被观测到的次数，如果某一个关键帧拥有的85％的地图点被看见了超过四次，那么该关键帧是冗余的并执行关键帧删除操作，判别公式如下：

其中，F(m)为第m个关键帧删除的标志位，为第m个关键帧的所有地图点数，/>第m个关键帧的被观测次数超过四次的地图点数，计算公式如下：

其中，表示在第m个关键帧中的第j个地图点是否为被多次观测地图点；

在基于双目的关键帧删除操作中，对地图点距离相机的距离进行判定，如果地图点距离相机超过的45倍的基线，则忽略该地图点，不会参与关键帧的删除操作，当关键帧被删除之后，对应地图点的观测次数需要修改，判定该地图点是否在删除的关键帧的右目中也被观测到，如果被观测到，那么该地图点的观测次数会减去2，否则观测次数减去1；

对新添加的地图点进行删除操作：删除一些外点来保证轨迹位姿估计的准确性，对于双目传感器设定观测阈值为4，当一个地图点被少于四个关键帧观测到的时候从全局地图中删除该地图点，判断公式如下：

其中，obs(j)为第j个地图点的被观测次数，f(j)为是否删除地图点的标志。

7.根据权利要求1所述的基于双目视觉的集中式多机器人定位方法，其特征在于：

所述的步骤4中，客户端将加入了地图点A和地图点B的地图i发送到服务端的方法为：

机器人i将相关的关键帧、地图点信息转换为rosmsg格式，通过rostopic进行发送，在基于双目的多机器人中，客户端需要转换的信息包括地图点的观测基本信息以及每个关键帧的右目坐标的信息、对应特征点的深度信息以及双目相机的基线，同时服务端在接受到了客户端发来的消息之后，将相关的rosmsg信息再此逆向转换为服务端能够识别的信息，此外，重新统计地图点观测，对于双目系统，对于地图点的观测统计会考虑他在右目中是否会存在，对于右目中也存在的地图点，该地图点的观测次数设置为2，判别公式如下：

其中，为第j个地图点在第m个关键帧的观测次数，/>为第j个地图点在第m个关键帧的右目中的横坐标。