2. 多无人机系统坐标系与状态表示

多无人机协同定位的第一步，不是写算法，而是把“谁在哪里、朝向如何、这个结论是在什么时刻、相对哪个原点得到的”用统一的数学语言固定下来。如果每架无人机各自使用自己的局部坐标，融合时就会出现位置对不上、航向差一个旋转、时间戳错位等问题。本节围绕全局/局部坐标系、位姿表示、状态向量定义以及时间戳与坐标变换基础展开，目标是让读者能够为一个小型多无人机系统写出可直接用于代码的状态定义与变换关系。

一、全局坐标系与局部坐标系的分工

在多无人机系统中，通常约定一个全局坐标系（World/Global Frame），记为 W，用于统一表达所有无人机的位置与姿态。全局系的原点一般选在任务区域的某个固定参考点，例如起飞点、地面站天线相位中心或标定的场地角点；X_W 指向东、Y_W 指向北、Z_W 指向上，构成右手系。这样约定后，任意一架无人机 i 在全局系下的位置可写为 p_i^W = [x_i, y_i, z_i]^T，单位统一为米。

局部坐标系（Body/Local Frame）则固连在无人机机体上，记为 B_i。原点通常取机体质心或IMU安装中心，X_B 指向机头，Y_B 指向右，Z_B 指向下（航空常用）或向上（部分室内系统）。局部系的作用是描述机载传感器（相机、激光雷达、UWB标签）的观测，因为这些观测天然是相对机体的。全局系与局部系之间通过位姿变换连接：局部系中的一点 p^B 转换到全局系为 p^W = R_B^W · p^B + t_B^W，其中 R_B^W 是旋转矩阵，t_B^W 是局部系原点在全局系中的位置。

二、位姿表示：旋转矩阵、欧拉角与四元数

位姿（Pose）由位置和姿态共同构成。姿态的表示方式直接影响计算稳定性与存储开销。常见三种表示：

• 旋转矩阵 R：3×3正交矩阵，det(R)=1。优点是可直接用于向量旋转，无奇异；缺点是9个参数存在6个约束，数值积分后需重新正交化。
• 欧拉角：如滚转 φ、俯仰 θ、偏航 ψ。直观易读，但存在万向节死锁，且旋转顺序必须声明（如Z-Y-X）。
• 四元数 q = [q_w, q_x, q_y, q_z]^T：4个参数，单位模长。无奇异、插值平滑，是多无人机状态估计中最常用的姿态表示。

以四元数为例，从机体到全局的旋转可写为 R(q)。若某无人机偏航 ψ=30°、俯仰和滚转为0，则对应四元数约为 q = [cos15°, 0, 0, sin15°] ≈ [0.966, 0, 0, 0.259]。这个数值可直接用于后续坐标变换，避免每次从欧拉角重算。

三、状态向量定义

单架无人机的完整状态通常包括位置、速度、姿态和角速度。以15维状态为例：

状态向量 x_i (15维):
  p^W  = [x, y, z]        位置 (m)
  v^W  = [vx, vy, vz]     速度 (m/s)
  q_BW = [qw, qx, qy, qz] 姿态四元数 (机体→全局)
  ω^B  = [wx, wy, wz]     机体角速度 (rad/s)
  b_a  = [bax, bay, baz]  加速度计零偏 (m/s²)
  b_g  = [bgx, bgy, bgz]  陀螺仪零偏 (rad/s)

实际系统中，零偏常作为增广状态在线估计，因此状态维数可能达到15或更高。多无人机协同定位时，全局状态由各机状态拼接而成：X = [x_1, x_2, ..., x_N]。若采用集中式融合，还需加入无人机间的相对观测；若采用分布式，则每架无人机维护自己的状态和邻居状态的副本。

四、时间戳与坐标变换基础

多无人机系统中，不同传感器的采样时刻不同：IMU可能200Hz，相机30Hz，UWB测距10Hz。每条观测都必须带时间戳 t，且所有时间戳应统一到同一时钟源（如GPS秒或PTP同步后的系统时间）。若观测时刻与状态估计时刻不一致，需要做时间对齐：

• 插值：对高频IMU做预积分，得到两个时刻之间的相对运动。
• 外推：用运动模型把状态推到观测时刻，再计算残差。
• 时间偏移估计：把传感器时间偏移作为待估参数，在线标定。

坐标变换的链式关系是：传感器系 S → 机体系 B → 全局系 W。若已知外参 T_S^B（传感器到机体）和位姿 T_B^W（机体到全局），则传感器观测 z^S 在全局系中的表达为 z^W = T_B^W · T_S^B · z^S。这里的 T 是4×4齐次变换矩阵，包含旋转和平移。实际编码时，通常把旋转和平移分开存储，避免频繁构造齐次矩阵。

五、一个可落地的状态定义示例

假设一个三机编队，每架无人机搭载IMU、单目相机和UWB标签。我们可以这样定义单机状态和观测：

单机状态 (15维):
  x_i = [p^W(3), v^W(3), q_BW(4), ω^B(3), b_a(3), b_g(3)]

外参:
  T_cam^B: 相机到机体 (标定得到)
  T_uwb^B: UWB标签到机体 (机械安装确定)

观测:
  IMU: 角速度 ω_m = ω^B + b_g + n_g
       比力   a_m = R_BW^T (a^W - g^W) + b_a + n_a
  相机: 特征点像素坐标，经投影模型转到全局
  UWB: 机间距离 d_ij = ||p_i^W - p_j^W|| + n_d

时间戳:
  t_imu, t_cam, t_uwb 统一到系统时钟
  融合时按 t 对齐，IMU预积分提供高频递推

在这个定义下，协同定位的残差可以写成：UWB测距残差 r_uwb = d_ij - ||p_i^W - p_j^W||，视觉重投影残差 r_cam = z_cam - π(T_cam^B · T_B^W · p^W)，IMU预积分残差 r_imu。三者按时间戳对齐后进入优化或滤波。判断结果是否合理，可以看残差是否接近零均值、协方差是否收敛、机间距离估计是否与UWB原始测距一致。

总结来说，多无人机系统的状态表示需要先固定全局系与局部系的定义，再选择稳定的姿态表示（推荐四元数），然后明确状态向量的维度和各分量的物理含义，最后用统一时间戳和链式变换把传感器观测接入融合框架。这套约定一旦建立，后续的协同定位算法、通信协议和可视化都将在同一套坐标与状态语义下运行，避免因参照系不一致而产生的隐性错误。