多无人系统协同视觉SLAM算法_魏强.pdf

多无人系统协同视觉 算法魏 强，张冬梅，范勇生（中国空间技术研究院，北京；中国东方红卫星股份有限公司，北京；上海宇航系统工程研究所，上海）摘要：针对传统同时定位与地图构建（）方法受计算能力、存储能力的限制无法在大范围场景下作业，无法建立大范围场景的全局一致性地图等问题，基于 系统框架建立了一种中心式的多无人系统协同视觉 算法。设计了地图大小限制策略，以保证无人系统不受机载设备算力和存储能力的影响，基于字典机制和 转换构建了地图融合优化算法，以完成全局地图构建与优化。最后进行了数据集测试，结果表明：所提出算法能够完成多无人系统协同，相对传统 算法具有明显优势。关键词：视觉；无人系统协同；地图融合；状态估计中图分类号：文献标识码：文章编号：（）收稿日期：；修回日期：第一作者：魏强，男，硕士，高级工程师，研究方向为航天管理科学与工程。：，（，；，；，）：，：；引言 近年来，随着机器人领域技术的不断发展，以地面无人车辆和无人机为代表的智能无人系统在军事、民用等领域中的应用逐渐成为了研究热点，智能无人系统不仅能够完成浇灌、测绘、巡检等民用任务，还能有效完成侦察、打击、追踪等作战任务。然而随着任务需求的增高，对无人系统能力的需求也愈来愈高。导航系统是无人系统功能实现的关键组成部分，负责向无人系统提供其在参考坐标系下的位置、速度、姿态等关键信息，引导无人系统按照既第 卷 第 期 年 月 载 人 航 天 DOI:10.16329/ki.zrht.2023.01.006定路线行进，导航系统的精确程度和稳定性直接影响了后续任务规划和控制的能力。传统的导航系统多依赖于全球导航卫星系统（，）和机载惯性导航元件之间的组合导航，然而，在室内环境或复杂室外环境中 系统会受高大障碍物遮挡以及多路径效应的影响，导致信号解析错误或信号中断。同时定位与建图技术（，）使智能体获得自定位的能力，以保证在 拒止的情况下无人系统能够完成自主导航，同时 技术能够建立一个全局一致表征的环境地图，可用于环境探索等任务，因此，技术是在 拒止情况下，使无人系统能够完成自主导航的关键技术之一。按照使用的传感器不同，技术主要可以分为激光雷达 和视觉 种：激光雷达误差模型简单，对于距离估计更为准确，并且不受弱光照等环境因素的影响，但是价格昂贵，重量大；相较之下，视觉传感器价格低廉，设备轻便，应用前景更加广泛。目前小范围环境下的 问题研究较为透彻，但单一无人系统在大范围场景下进行探索时效率较低，并且由于地图数据量的不断增长，计算时间和内存占用都会对机载计算机的能力提出严苛的要求。多机协同 可以将问题分而治之，将整个任务的计算 存储压力以及计算量分摊到参与协同的各个无人系统上，大幅提高任务效率。因此，多机协同 逐渐成为了当前的研究重点。目前多机协同视觉 系统多数为中心式架构，中心式架构将智能体分为子端和中心端，子端和中心端直接进行信息交互，子端智能体执行部分 任务，完成状态估计并建立局部地图；中心端利用子端传输的数据，对地图进行融合和优化，创建全局一致性表示的全局地图。年，等在 等的基础上使用了 个单目视觉传感器，利用 个子端产生的关键帧共同完成地图构建。基于相似的思路，等提出了，将多视觉传感器协同的数量规模扩张到了 个。然而随着参与协同的传感器数目增多，的算力压力较大，并且对通信系统的带宽具有较高的要求。等通过引入云端服务器，使子端将关键帧上传至云端，在云端服务器上完成局部地图的构建和子端地图融合，改善了系统的计算压力和带宽压力。等使用了减少数据传输量的方法来缓解通讯压力，只发送检测出的特征点和关键帧帧间的相对位姿。通过比较子端和中心端计算的关键帧位姿之间的差异来恢复绝对位姿。本文提出了一种大范围场景下的多无人系统协同定位与建图方法，利用多个无人系统进行探索，同时每个子端进行同时建图与定位过程，建立稀疏点云地图。之后，根据区域重叠等公共信息进行局部地图的融合，建立全局的稀疏点云地图，完成协同定位与建图功能，在提升定位精度与效率的基础上建立一个全局一致的环境地图表示，以提升无人系统集群的智能程度。多无人系统协同定位与建图系统.系统整体框架 采用中心式架构，将系统分为两部分：子端和中心端。每个子端搭载一台单目相机、通信单元和机载计算机；中心端和子端进行数据交互，接收子端传输的关键帧，地图点等数据，完成回环检测，地图融合，全局优化等非实时操作；并将优化后的数据发送回子端，指导子端的导航系统。整体的系统结构如图 所示。图 算法框架图 ）所有智能体子端都运行一个独立的前端视觉里程计，维持一个规模较小的局部地图并将关键帧和地图点发送给中心端，中心端接收各子端信息，形成各子端的历史地图。）中心端通过从所有子端处获得的关键帧增量式构建了一个关键帧数据库。用于存储所有载 人 航 天第 卷接收到的关键帧的信息，中心端还建立了一个包含了所有子端局部地图的地图堆栈，不同子端地图的构建独立进行，互不干扰。重叠区域检测过程不断进行，新接收的关键帧通过基于字典机制的相似度检测算法不断检测子端是否位于过去到达过的区域，重叠区域包括该子端过去到达过的区域和其他子端到达过的区域。子端内部重叠，则进行回环检测过程；子端之间重叠，则通过匹配的关键帧建立两子端局部地图间的数据关联，进一步进行地图融合，地图融合后这 个地图被合并，原有的局部地图从地图堆栈中删除，而由它们的融合产生的一个新的全局地图将被添加到地图堆栈中，并与 个子端直接关联。最后根据得到的全局地图进行全局优化。）在全局优化结束后，中心端会将优化后的相机位姿发送回对应的子端，子端会更新位姿并将其进行局部优化，从而更精确的完成协同 过程，并获得更准确的导航结果。.子端算法 子端的主要功能是运行视觉里程计，并将跟踪的路标点进行存储，组成局部地图，按照设定好的数据结构将信息发送给中心端。这部分基于进行开发，如图 所示。由于子端局部地图的大小会随着运行时间而增长，大范围协同 一般都需要长时间运行，这就对子端的存储能力提出了挑战。由于中心端的地图堆栈中包含了每个子端的完整地图，因此在通信系统畅通的情况下，子端无需保留过多地图信息。本方法对各子端建立的局部地图中关键帧的数量进行限制，以降低在大范围探索任务中子端机载处理器的存储压力和计算压力。设定子端局部地图中关键帧数目的上限为，一旦关键帧数目超过，就去除旧的关键帧，同时将新创建的关键帧添加进局部地图，为了防止出现中心端没有接收到被删除的关键帧而造成数据丢失的现象，在删除关键帧之前，子端会和中心端进行一次数据交互，确认中心端是否接收到该关键帧，确认后再进行删除，流程如图 所示。.中心端算法 不同子端局部地图之间的参考坐标系关系如图 所示，假设有子端，子端，子端 的参考坐标系为，子端 的参考坐标系为，图 跟踪线程框架图 图 子端局部建图流程 为子端 创建的一个关键帧，它在子端 的局部地图中的位姿为，它在子端 的局部地图中的第 期 魏 强，等 多无人系统协同视觉 算法图 参考坐标系转换关系 位姿 。因此要完成子端，之间的地图融合，就需要得知从 到 的位姿转换关系。如果子端 和子端 在观测过一处公共区域，那么 个局部地图之间就存在着重叠区域，可以利用其完成地图融合与全局优化，然而，若 个子端没有观测过公共区域，那么在没有其他信息的情况下无法完成两地图之间的数据融合。由于局部地图之间重叠部分的地图点所对应的特征描述子之间相似度较高。而系统创建了一个关键帧数据库，其中存储了所有子端发送的关键帧，因此，可以为关键帧数据库建立一个字典树，在整个地图堆栈中检索有无关键帧匹配，检测到匹配后利用匹配关系解算地图间的相对位姿关系，实现全局地图融合与优化。下面介绍基于重叠区域检测的地图融合优化算法。首先，进行重叠区域检测。遍历新关键帧 的所有共视关键帧，计算 与每个共视关键帧的词袋向量相似度，并得到当前帧与其共视关键帧相似度的最低分，在关键帧数据库中寻找与 相似度超过 的关键帧，称为候选匹配关键帧，接下来在候选匹配关键帧中检测具有连续性的候选帧，连续次数大于 则认为成功匹配，将所有符合条件的关键帧组合起来，就得到了候选关键帧组。然后，利用求解出的匹配关系进行地图之间相对位姿的计算。遍历匹配候选关键帧集合，筛选出与当前帧的匹配特征点大于 的候选帧集合；每一个候选帧都构造一个位姿求解器进行迭代求解；本文的位姿求解算法使用三维相似变换算法（，），常用于求取 组匹配点之间的位姿变换关系。利用 转换通过匹配的地图点之间的关系求解出两关键帧间的相似变换关系，假设当前帧到匹配帧的坐标变换矩阵为，逆变换矩阵为，当前关键帧坐标系下的地图点 对应的图像像素点为，同理，有候选帧坐标系下与地图点 匹配的地图点 及其对应的图像像素点，则重投影误差为：（）设置两个重投影误差阈值分别为 和，若 且 ，则该组匹配构建的三维点被称为内点。当内点数量满足要求时，视为该候选帧匹配成功。当有一个候选帧匹配成功，成功解算出相对位姿关系后，为了优化这个初步解算出的位姿转换关系，通过将匹配关键帧以及其相连关键帧的地图点都投影到当前关键帧进行匹配，以构造更多的匹配关系来进一步优化 变换，直到优化后的内点数目符合要求后，则视为相似变换解算成功。最后，进行地图融合与优化。由于上一步中计算 转换改变了当前关键帧的地图点，因此需要根据共视关系更新当前帧与其他关键帧之间的连接以及其他关键帧的地图点，对冲突的地图点进行替换或填补。然后将匹配关键帧组的地图点投影到局部关键帧组上进行地图点检查与替换，更新关键帧之间的共视相连关系，得到因匹配时地图点融合而新得到的连接关系。接下来进行本质图优化，优化因匹配而更改的连接关系，最后添加当前帧与闭环匹配帧之间的边，进行全局优化，基于地图点到关键帧的投影关系，优化所有的地图点和关键帧的位姿。算法流程图如图 所示。实验结果.数据集和测试指标 由于目前少有针对多机 的测试数据集，因此常应用、等单系统于单一环境以不同轨迹收集的多个数据集应用于多机协同 的性能测试。是 领域常用的测试数据集，常用于、等系统的性能测试。中包含一个在小型工业厂房中收载 人 航 天第 卷图 基于重叠区域检测的地图融合优化算法流程 集的数据集，该厂房如图 所示，厂房结构较复杂，特征丰富，适合用来对 算法进行性能测试。该数据集中、和 是由无人机在光照条件较为良好的情况下进行低速飞行采集到的 组数据集，算法测试难度适中。由于厂房面积较小，因此这 组数据集存在大量的重叠关系，为多机协同 的地图融合创造了条件。本文使用，和 数据集作为 个无人系统子端对系统进行测试，用 来进行数据可视化。系统性能测试中常用的精度指标为绝对轨迹误差（，），本文选取绝对轨迹误差 作为评测指标。.实验结果 本文首先建立系统中心端和 个无人系统子端，分别接收，等 组数据集发送的图像数据，如图 所示。其中绿色的轨迹和点云是 中相机的轨迹估计结果和构建的点云，白色的轨迹和点云是 中相机的轨迹估计结果和构建的点云，蓝色的轨迹和点图 厂房图 云是 中相机的轨迹估计结果和构建的点云。可以发现，组独立的飞行过程通过本文的协同 系统完成了数据关联，得到了各自轨迹的相对位姿关系，并共同构建了一个全局一致性表征的稀疏点云地图。最终的轨迹估计结果如图 所示。图 结果图 图 轨迹图 为了对比本文提出的协同 方法的性能，以 作为基准方法，对比协同情况下轨迹估计值的误差和单独进行 情况下轨迹估计值的误差。使用 测评工具 测试了轨迹跟踪情况，使用 单独进行 过程的轨迹跟踪结果和应用本文方法进行协同 过程的轨迹跟踪结果如图 所示，其中图（），图（）是在 数据集上的轨迹跟第 期 魏 强，等 多无人系统协同视觉 算法图 本方法和 对各数据集的轨迹结果 踪结果，图（），图（）是在 数据集上的轨迹跟踪结果，图（），图（）是在 数据集上的轨迹跟踪结果，轨迹跟踪情况较好。算法和本方法在 组数据集上的绝对轨迹误差对比结果如表 表 所示。由表可知，种算法在数据集中的误差都较低，在 数 据 集 中 的 误 差 均 值 为.，本方法的误差均值为.，与 持平，在 数据集中 的误差