Covins多机器人单目协同建图实战：基于Euroc数据集的全流程解析

老读者应该还记得我之前写过Covins相关的记录这次是2021年11月23日又一次把整套流程重新跑通之后的更新。先说结论Covins这套多机器人单目协同建图框架跑Euroc数据集来验证依然是目前最直观、最容易上手的方案。它本质上是把VINS-Mono从“单机版”扩展成“多机版”让每台机器人各自算自己的视觉惯性里程计同时通过一个共享服务器做闭环检测和协同优化最终输出一个全局一致的轨迹和地图。这篇文章我会把这次更新的完整过程、环境配置、数据集处理、启动命令和排坑经验都写出来给正在做多机器人SLAM、想拿公开数据集评估算法效果的朋友一个可以直接参考的实操记录。1. 先弄清楚Covins到底解决什么问题1.1 单机SLAM的天然瓶颈我们平时用的单机SLAM说白了就是一台机器人自己跑自己的摄像头采集图像IMU给运动约束后端做一个优化最后输出一条轨迹和一张地图。单机跑没问题可一旦到了多台机器人协作的场景问题马上就来了每台机器人的地图坐标系是独立的你只知道“我在哪”不知道“相对于队友我在哪”。有人会说那让每台机器人把轨迹发回来不就行了实际操作过就会发现没那么简单。通信带宽有限图像帧动辄几百KB多台机器人的数据全汇聚到中心算力很快就撑不住更关键的是各台机器人之间的数据关联怎么做谁来发现“A机器人看到的走廊其实就是B机器人十分钟前走过的走廊”这件事单机SLAM根本没有答案。Covins就是冲着这个痛点去的。我当时第一次看到这个项目名字还以为是某个相机标定工具后来认真读了代码才发现它把多机器人协同定位建图做成了一个可以拿来跑数据集的完整系统这对我们做研究或者做产品验证的人来说太友好了。1.2 Covins的架构每台机器人一个VINS再配一个共享服务器Covins的架构很好理解它把系统分成两块。第一块是前端也就是每台机器人本地跑的那套东西直接基于VINS-Mono改进。每台机器人用自己的单目相机和IMU做视觉惯性里程计输出局部位姿估计。这里有个很关键的设计本地里程计不依赖任何其他机器人所以单台机器人的定位是实时且自包含的。第二块是后端一个集中式的服务器叫做Covins Server。所有机器人的关键帧信息会以紧凑描述子的形式发上去Server在统一的数据库里做闭环检测一旦发现某台机器人的当前帧和另一台机器人的历史帧有共视关系就建立一个跨机器人的闭环约束然后把约束广播给相关机器人让它们在本地位姿图里加入这条边并做优化。这样的好处是全局地图不需要集中式地重算一遍每个机器人只在需要的时候接收和优化自己相关的约束通信量大大降低服务器压力也小很多。实测下来单目图像的关键帧压缩成描述子之后每条消息可能只有几百字节这和直接传输原始图像完全不是一个量级。原理解释清楚了你就能明白为什么Covins特别适合拿Euroc这种公开数据集来做验证它不挑平台只要你有多个有重叠视野的序列就能模拟多机器人协同建图的完整流程。1.3 单目方案为什么比双目和激光更难既然要做协同为什么偏偏选单目其实这是Covins的一个亮点也正因为它选了单目整个系统才更有工程参考价值。单目相机只有一个摄像头每个像素只提供方向信息没有深度。单机状态下单目SLAM还能靠多帧三角化恢复结构但尺度是未知的轨迹会漂移。为了恢复尺度Covins在每台机器人身上都加了IMU用视觉惯性紧耦合把尺度确定下来这步其实就是VINS-Mono的老本行。但到了多机协同阶段新的问题出现了每台机器人的IMU不同初始化好坏不同各自的局部尺度可能不完全一致。要让多台机器人协同建图必须先解决不同局部坐标系之间的一致性这就需要跨机器人的闭环约束来统一。Euroc数据集里同一场景下的不同序列正好能提供这种共视用来验证系统能不能把不同机器人的轨迹拉到一个全局一致坐标系里。这也就是为什么单目协同建图跑通之后再去做多机器人路径规划会顺畅很多因为底层的全局一致地图已经有了。2. 环境准备这次更新后比VINS-Mono多踩了几个坑2.1 系统版本和ROS选型先说我的运行环境一台ThinkPad P52i7-8750H16G内存GTX 1050 Ti系统是Ubuntu 18.04 LTSROS版本是Melodic。这套组合是保守路线也是最稳的路线。我个人不建议直接用Ubuntu 20.04加Noetic去跑Covins不是说完全跑不了而是老代码对Noetic的一些接口兼容性不太好编译过程容易在cv_bridge和Eigen版本上卡住。需要准备的依赖项有ROS Melodic建议装desktop-full版本OpenCV 3.x注意必须是3.xCovins很多接口和OpenCV 4不兼容Ceres Solver版本推荐1.14.0Eigen 3.3.xDBoW2这是闭环检测依赖的词袋库我这次重跑之前写了个脚本把这些依赖统一检查了一遍发现Ceres之前被我升级过和Covins的接口对不上花了一点时间降级才解决。所以如果你之前装过其他SLAM项目强烈建议先确认依赖版本再开始编译Covins不然报错会非常痛苦。2.2 编译顺序和这次更新遇到的编译问题Covins是用catkin构建的推荐用catkin_tools而不是catkin_make因为多包管理起来方便很多。我创建了工作空间之后先把代码clone到src目录下然后执行catkin init catkin build covins_msgs catkin build先编译covins_msgs这个自定义消息包再编译整个工作空间这个顺序很重要不然会出现消息头文件找不到的错误。这次更新之后我注意到默认分支对Ceres版本的要求更严格了如果你在编译covins_optimizer或者covins_map时看到和Ceres相关的报错大概率是版本问题。另一个高频编译错误是OpenCV版本的冲突。如果你系统里同时装过OpenCV 3和OpenCV 4CMake有可能找错版本导致一些函数符号对不上。我检查过Covins在OpenCV 3下跑是最顺的尤其是在特征提取和描述子计算这一步。2.3 没有GPU也能跑但要有心理预期Covins的特征提取部分用的是学习型描述子我记得是基于SuperPoint那一类网络训练出来的模型这也是它通信量小的关键之一。很多第一次接触Covins的人默认这种网络必须得用GPU跑实际上我在CPU上也能跑通只是实时性会稍微差一点。没有CUDA的机器编译时不要开GPU相关的选项默认走CPU路径就行。Euroc数据集每个序列也就两三分钟离线跑完全没问题我就是全程CPU跑的。如果你要上真机实时跑那确实需要一块还不错的GPU或者一个支持推理加速的算力平台。在这里我多说一句学习型特征虽然计算量比ORB大但在跨视角、跨场景的匹配鲁棒性上优势明显。多机器人协同最怕的就是明明看到了同一个地方但因为视角差异太大匹配不上Covins用学习型特征正是为了减少这种漏匹配。3. 数据集准备Euroc不是拿来就能直接跑的3.1 Euroc数据集选序列的逻辑Euroc数据集合集了很多无人机在室内飞行时的数据分两组一组是Machine HallMH01到MH05一组是Vicon RoomV101、V102等。每组序列都包含了单目图像、IMU数据和groundtruth轨迹非常适合做SLAM评估。关键来了要想用Euroc模拟多机器人协同建图一定不能随手选一个MH序列和一个V序列这样两个场景完全没有视觉重叠Covins的闭环检测模块根本找不到跨机器人共视协同也就无从谈起。我这次选的是MH01、MH02和MH03这三个序列它们都在同一个Machine Hall里飞行轨迹不同但存在大量公共区域这样Server才能发现不同“机器人”之间的闭环关系。下载的时候直接选择rosbag格式省去转换的麻烦。序列长度都还可以MH01大约180秒MH02大约150秒跑起来不会太占时间。如果你一开始不熟悉流程建议先用MH01和MH02两个序列做最小验证跑通了再加第三个。3.2 用bag构造多机器人的输入在单台电脑上模拟多台机器人本质上是让多个Covins节点同时运行每个节点对应一个agent读取一个bag文件共享同一个Covins Server。实际操作时要处理好topic的重映射。Euroc官方bag里的图像话题通常是/cam0/image_rawIMU话题通常是/imu0。而Covins每个agent内部需要监听自己命名空间下的话题所以需要用remap参数把bag里的原始话题映射到agent的命名空间下。我自己的做法是写一个launch文件把每个agent的namespace参数、话题映射和对应的bag路径都固定下来这样每次启动就一行命令不用反复手动配置。如果你只是临时测试也可以直接用roslaunch的remap参数硬映射但写launch会省事很多。这里有个容易忽略的细节每个agent对应一个独立的配置yaml里面有相机内参、IMU噪声参数、话题名和agent_id。MH序列的相机是全局快门内参在Euroc官方的文档里都能找到填进去就行。如果内参不准跑出来的轨迹会非常飘而且这个问题后面很难排查。3.3 时间轴对齐是伪分布式的关键单机模拟多机器人最大的坑就是时间轴的对齐。真实系统里每台机器人都有自己的硬件时钟各跑各的没问题但在模拟环境里所有数据来自你本地电脑如果你先播放了bag1再播放bag2那bag1的机器人已经把场景飞完了bag2的机器人才刚开始它们在时间上根本没有交集Server自然检测不到协同。所以启动多个bag时尽量让它们同时开始播放。我是开了三个终端在终端里输入几乎一样的rosbag play命令快速依次执行让三个bag基本同步播放。如果你想更精确也可以在所有bag播放命令里加上--clock参数配合统一的ROS时间源但实测下来只要同场景序列同时启动协同效果就不差。另外Euroc的MH01到MH05虽然在同一个大厅但飞行的起始位置和方向是固定的所以前几十秒三个“机器人”大概率都在原地初始化这个阶段看不到太多跨机闭环不用慌等它们各自飞到公共区域Server的闭环检测就会陆续触发。4. 实操记录一步步启动Covins4.1 启动Covins Server整个系统的入口是Server。先启动Server等于先把共享数据库准备好了后续各个agent才能把关键帧描述子送上来。在终端里执行source devel/setup.bash roslaunch covins covins_server.launchServer启动后会打印出监听的topic和一些参数信息这时候终端会持续挂着不要关闭它。我习惯另开一个终端用rostopic list确认Server相关的topic已经注册比如/covins_server/keyframe这类确保Server没有问题再往下走。有朋友把Server挂掉后忘了重启就直接启动agent结果agent节点找不到Server一直报连接失败。这个坑很容易踩记住启动顺序一定是Server先行。4.2 启动多个agent机器人接下来启动agent每个agent代表一台机器人。为了方便区分我给三个agent分别起了不同的命名空间。假设我们在工作空间根目录source devel/setup.bash roslaunch covins covins_agent.launch agent_id:0 namespace:agent0 config:src/covins/config/euroc/mh01.yaml roslaunch covins covins_agent.launch agent_id:1 namespace:agent1 config:src/covins/config/euroc/mh02.yaml roslaunch covins covins_agent.launch agent_id:2 namespace:agent2 config:src/covins/config/euroc/mh03.yaml具体launch文件的路径和参数名第一次用之前建议先去launch目录里看一眼每个版本的Covins命名规则略有差异这次更新后一些默认参数也调整过直接照抄网上老教程可能会找不到参数。核心思路就是让每个agent有独立的agent_id这样才能在Server端区分不同机器人的关键帧。启动完agent之后不要急着回放bag。先用rostopic list检查每个agent的topic是否已经出现在对应命名空间下比如/agent0/vins_estimator/camera_pose这个topic有输出说明agent已经拿到了图像和IMU。4.3 回放bag数据确认agent都就绪后开始回放三个bag。我开的三个终端分别执行rosbag play --pause MH_01_easy.bag rosbag play --pause MH_02_easy.bag rosbag play --pause MH_03_easy.bag先全部用--pause暂停三行命令都启动后再分别回到每个终端按一下空格让它们同时开始播放。这个方法比一个一个rosbag play要准很多能最大限度保证三个序列的同步性。bag播放起来之后Covins Server的终端会开始刷日志你会看到类似“keyframe received”或者“loop closure detected”这样的输出。当你看到跨机器人的闭环检测日志出现时恭喜协同建图真正开始工作了。4.4 在Rviz里看效果Covins自己带了一个Rviz配置文件启动Rviz后加载即可roslaunch covins rviz.launch在Rviz里你会看到三台机器人的轨迹分别以不同颜色显示。刚启动的时候三条轨迹各画各的坐标系互不相干看起来像三条时空错乱的线。等跨机器人闭环约束加入并优化之后三条轨迹会逐渐被拉到同一个全局框架下公共区域的轨迹会重叠在一起那个瞬间的效果非常直观。我记得我第一次跑通的时候看到MH01和MH02的轨迹在走廊转角处对齐时真的有点激动因为这意味着系统真正理解了“这两台机器人看到的是同一个世界”。这种验证是你只看写代码永远感受不到的。5. 结果评估与调试心得5.1 用evo量化协同建图效果跑通只是第一步如果你写论文或者做算法对比还需要量化建图效果。我推荐用evo工具来评估轨迹精度。先把Covins每个agent输出的轨迹保存成TUM格式一般rostopic echo的方式太原始Covins的launch里通常会开启轨迹记录生成的轨迹文件可能在~/.ros或者你启动roslaunch的目录下。拿到轨迹文件之后用evo和Euroc的groundtruth对比evo_ape euroc MH_01_gt.csv agent0_traj.csv -aevo_ape算绝对位姿误差-a参数是自动对齐轨迹这个对齐尤其重要因为不同agent的局部坐标系和groundtruth世界坐标系没有对齐。如果你想看多机器人协同前后的误差变化分别关掉Server和打开Server各跑一次对比同一agent的APE差异会非常明显。协同之前的轨迹误差会随着时间累积跑到后面越来越大协同之后由于跨机器人闭环的约束误差被拉回来曲线会明显改善。我自己测下来MH01在协同后的RMSE降幅是很可观的当然不同序列、不同参数效果会有差异这个需要自己实验。5.2 常见问题速查表这次重跑包括之前的几轮测试我攒了一些高频问题整理成表格方便你快速排查。问题现象可能原因解决办法编译时报Ceres相关错误Ceres版本不匹配降到1.14.0重新编译编译时报OpenCV函数找不到系统存在多个OpenCV版本指定OpenCV 3.x路径Server启动后无任何反应Server和agent没有连接到同一ROS master检查ROS_MASTER_URI确保在同一个网络agent启动后轨迹几乎没有输出bag话题名和配置不一致用rostopic list确认图像和IMU话题名Server检测不到闭环选择的序列没有公共视野换成同一场景下多个序列三个轨迹完全不重合bag同步性差用--pause同时回放或重试轨迹整体漂移严重相机内参或IMU外参配置错误核对Euroc官方标定参数5.3 我个人踩过的几个典型坑第一个坑是IMU外参。Euroc数据集每个序列的IMU和相机外参在原始配置里有细微差别如果你只是把MH01的yaml复制成MH02和MH03用那MH02和MH03的初始轨迹就会离谱得不像话。正确做法是每个序列单独建一个yaml把对应的外参填进去。第二个坑是时间戳。Euroc的bag如果播放速度太快单目视觉的帧率跟不上特征跟踪容易丢VINS初始化也会失败。我第一次跑的时候图省事直接rosbag play --rate2.0结果三个agent一个都没初始化成功全在那转圈。慢一点按照默认1倍速播放反而最稳。第三个坑是资源占用。三个agent加上一个Server再加上Rviz16G内存其实有点紧张尤其是当三个agent同时做特征提取和位姿图优化的时候。如果电脑配置一般建议只跑两个agent验证流程或者把Rviz的可视化频率调低一点别问我是怎么知道的。6. 从协同建图到多机器人路径规划6.1 全局一致地图的真正意义跑通Covins拿到一个全局一致的轨迹和地图到底是什么级别的意义我自己的理解是它把多机器人系统从“各干各的”变成了“干同一件事”。你有了全局一致的地图后面才有可能在这个地图上做多机器人路径规划否则你规划出来的路径在A机器人的坐标系里可能直接穿墙或者和B机器人的路径重叠冲突。有了协同建图作为底图多机器人路径规划才能展开比如在共享地图上做避障、做任务分配、做编队协同。所以Covins这套系统本质上是在为更上层的多机器人协同决策提供基础支撑。这也是我为什么一直推荐做多机器人方向的朋友先从Covins入手因为它是前面那一块又难又重要但又相对容易验证的拼图。6.2 后续扩展的方向跑通Covins之后如果你还想往深处走可以试试两条路。第一条是把这个协同框架接到真实的路径规划算法上比如在Server输出的全局地图上用图搜索算法做多机路径协调这需要把Covins的地图数据转换成适合规划的栅格或者拓扑地图。第二条是改进Covins本身比如把集中式的Server改成完全分布式的架构。Covins虽然通信量已经很小但Server仍然是中心节点一旦Server挂了系统就瘫了这个问题在很多需要高可靠性的场景里是不能接受的。对我个人来说Covins在Euroc数据集上跑通是我多机器人方向最里程碑的一次实操。它让我从“看论文觉得分布式协同很美好”跨越到“亲手看到不同机器人的轨迹被闭环约束拉到同一个世界坐标系下”这种体验对理解的帮助特别大。如果你也在折腾多机器人SLAM建议直接下载Euroc的MH01到MH03按我上面的步骤跑一遍尤其是那些第一次接触Covins的同学这套流程足够你静下心来跑一整个下午。你已经看到全部的坑了剩下的就是动手。