机器人的相互定位在多机器人合作中起着关键作用。本项目提出了一种新型的专注于多机器人六自由度(DOF)相对姿态的估计系统 CREPES。
CREPES硬件结构设计紧凑，其硬件结构包含红外LED，鱼眼相机，超宽带测距模块（UWB）和惯性测量单元（IMU）。 通过利用红外光通信，实现相机视觉检测和UWB测距之间的数据关联。 通过相机解算的方向信息与UWB测距数据的融合可以获取CREPES相对的3-DOF位置数据。 结合相邻CREPES设备的相对位置和IMU重力方向约束，我们可以从一帧传感器测量数据中估计出6-DOF的相对姿势。
此外，CREPES包含一个基于误差状态卡尔曼滤波器（ESKF）的估计器，以提高系统的准确性和稳健性。当有多个邻居时，引入姿势图优化（PGO）算法，以进一步提高系统精度。 我们进行了大量的实验来证明CREPES在双机协作、集群编队应用中的准确性，以及在复杂定位环境下的性能。
CREPES的硬件系统包含一个惯性测量单元（IMU），一个超宽带测距模块（UWB），一个工业相机和一组红外LED。 相对位姿的直接检测算法采集相邻设备的ID、位置向量（视觉检测）、相对距离（来自UWB）和惯性检测单元IMU数据，用于解算相邻设备位姿的原始估计。 然后，ESKF算法模块使用惯性单元IMU数据对原始估计位姿进行滤波。当周围有多个邻居CREPES时，将采用PGO模块进一步提高位姿估计性能。
为了使设备更加便携，减少硬件输出端口，CREPES的硬件经历了四次迭代优化。 从CREPES1到CREPES4，最终CREPES4设备的重量、体积和通信距离列示如下。
双机在静止状态下进行定位。测试距离分别为：2.5米，5米和10米。 位置误差的中位数小于0.1米。 姿态误差的中位数小于0.4度。
双机在运动状态下进行定位。双机运动距离范围在5米至10米。 位置误差的中位数小于0.1米。 姿态误差的中位数小于2度。
在非视距和遮挡情况下，如何实现设备的位姿估计和定位是一个巨大的挑战。 该实验中，我们使用五个机器人在存在遮挡的复杂场景下进行定位。下列视频展示了实验中NOKOV动捕系统真值输出和CREPES系统相对定位输出的数据对比。
地面车辆搭载CREPES设备，并通过遥控控制。无人飞行器搭载相同CREPES进行定位，并跟踪地面车辆进行绕环轨迹运动。
三台无人机搭载CREPES获取相对定位，环绕车辆进行跟踪。
无人机搭载雷达和CREPES设备，运行SLAM算法，作为系统的领导者。无人车仅搭载CREPES设备获取相对位资，在无人机引导下完成环境探索。
@misc{xun2023crepes,
title={CREPES: Cooperative RElative Pose Estimation System},
author={Zhiren Xun and Jian Huang and Zhehan Li and Zhenjun Ying and Yingjian Wang and Chao Xu and Fei Gao and Yanjun Cao},
year={2023},
archivePrefix={IROS}
}