想象一下，你和朋友在不同的城市，各自拿着一张没有经纬度、只有相对位置的地图，你们如何描述“我在你东边500米”？这就是多机器人坐标系对齐要解决的核心问题：让每个机器人用同一把“尺子”和“方向”来描述自己的位置。

单台机器人的坐标变换链是线性的，从传感器到本体再到世界。但在多机系统中，这个链条变成了一个网络，每个机器人都是网络中的一个节点。对齐的本质，就是确定网络中任意两个节点之间相对位姿的变换关系。

多机对齐的三大核心挑战

上图清晰地展示了核心矛盾：每个机器人可能都定义了自己的世界坐标系（/world_A, /world_B），但它们之间没有已知的变换关系。对齐就是要解决这个“？”。这带来了三个主要挑战：

• 初始位姿未知：机器人启动时，彼此在公共地图中的位置是未知的。
• 观测受限：机器人可能无法直接“看到”对方，或者观测数据带有噪声。
• 通信与一致性：如何将各自估计的变换关系可靠地分发给所有机器人，并保持一致。

主流对齐方法对比

根据对齐的时机和依赖的信息，主要有三种方法。选择哪种，取决于你的硬件条件和任务需求。

方法	核心思想	所需条件	优点	缺点	典型场景
共享全局地标	所有机器人观测同一组预先定义或已知的全局特征点（如二维码、UWB基站）。	环境中部署可观测的全局标志物。	原理简单，对齐精度高，初始化快。	依赖外部基础设施，环境部署成本高。	仓库分拣、固定工站协作。
相对观测对齐	机器人之间互相观测（通过视觉、激光、UWB），直接估计彼此相对位姿。	机器人具备相互探测的传感器。	无需外部设施，自主性强。	依赖视距，初始未“见面”时无法对齐。	机器人编队、探索集群。
地图融合对齐	各机器人先独立建图，然后通过匹配地图的公共部分（如重叠区域）来求解相对变换。	机器人具备SLAM能力，环境有可匹配的特征。	完全自主，无需预先部署或直接观测。	算法复杂，计算量大，需要足够的重叠区域。	多机协同探索未知环境。

在实际项目中，我参与过一个AGV集群项目，初期尝试了相对观测对齐，但AGV货架太高经常遮挡视线，导致对齐不稳定。后来我们改为在仓库立柱上贴二维码（共享全局地标），虽然部署麻烦了点，但系统鲁棒性大幅提升。大部分室内多机系统，如果环境允许，我推荐采用“共享全局地标”作为基础方案。

实现：基于共享地标的对齐流程

我们以最典型的“共享全局地标”方法为例，拆解其实现步骤。假设我们在环境中固定位置放置了多个已知全局坐标的二维码（ArUco标记）。

这个过程的关键在于，每个二维码的全局坐标（在/world系下）是预先测量并存储在系统中的。当机器人摄像头看到二维码时，它能计算出“二维码相对于摄像头”的位姿T_camera_marker。由于T_world_marker已知，我们就可以解算出机器人在世界坐标系中的位姿：T_world_camera = T_world_marker * (T_camera_marker)^-1。

下面是一个简化的代码片段，演示了在ROS2中，一个机器人节点如何根据观测到的二维码计算并发布自身到世界坐标系的变换。这里假设已经有了从图像中识别并解算出的T_camera_marker（类型为geometry_msgs.msg.TransformStamped）。

// 假设已知：
// 1. marker_id: 观测到的二维码ID
// 2. T_camera_marker: 二维码在相机坐标系下的变换
// 3. world_marker_pose_map: 一个字典，存储每个二维码ID在世界坐标系下的位姿 (geometry_msgs.msg.Pose)

#include <geometry_msgs/msg/transform_stamped.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2/LinearMath/Transform.h>

void publishWorldTransform(int marker_id,
                           const geometry_msgs::msg::TransformStamped& T_camera_marker) {
  
  // 1. 获取该二维码在世界系下的位姿
  auto it = world_marker_pose_map.find(marker_id);
  if (it == world_marker_pose_map.end()) {
    // 错误处理：未知的二维码ID
    return;
  }
  geometry_msgs::msg::Pose world_marker_pose = it->second;
  
  // 2. 将世界系下的二维码位姿转换为tf2::Transform格式
  tf2::Transform T_world_marker;
  tf2::fromMsg(world_marker_pose, T_world_marker);
  
  // 3. 将相机系下的变换消息转换为tf2::Transform格式
  tf2::Transform T_camera_marker_tf;
  tf2::fromMsg(T_camera_marker.transform, T_camera_marker_tf);
  
  // 4. 核心计算：T_world_camera = T_world_marker * inverse(T_camera_marker)
  tf2::Transform T_world_camera = T_world_marker * T_camera_marker_tf.inverse();
  
  // 5. 假设已知相机到机器人底盘的固定变换 T_base_camera (例如从URDF加载)
  tf2::Transform T_base_camera = ...; // 从URDF或标定获得
  
  // 6. 计算机器人底盘在世界系下的位姿
  tf2::Transform T_world_base = T_world_camera * T_base_camera;
  
  // 7. 创建并发布TF消息
  geometry_msgs::msg::TransformStamped T_world_base_msg;
  T_world_base_msg.header.stamp = this->now();
  T_world_base_msg.header.frame_id = "world"; // 父坐标系
  T_world_base_msg.child_frame_id = "robot1/base_link"; // 子坐标系
  T_world_base_msg.transform = tf2::toMsg(T_world_base);
  
  tf_broadcaster_->sendTransform(T_world_base_msg);
}

这段代码的核心是第4步的矩阵运算。它体现了坐标系对齐的基本数学原理：通过一个已知的“桥梁”（二维码），将局部观测与全局知识连接起来。

多机TF树的结构设计

当多个机器人都完成对齐后，整个系统的TF树应该如何组织？一个好的结构能简化数据的使用。我推荐采用“星型”拓扑。

在ROS2的TF2系统中，无论采用哪种拓扑，你都可以使用tf2_ros::Buffer::lookupTransform函数，直接查询任意两个坐标系之间的变换，库会自动计算最短路径。但星型拓扑在概念理解和调试上更具优势。