4. 坐标系、时间同步与状态估计基础

航迹跟踪与自主导航的第一道“地基”不是控制器，而是坐标与时间。同一架无人机的位置、速度、姿态，如果分别用惯性系、机体系和导航系描述，数值完全不同；GPS、IMU、视觉三路传感器如果时间戳没有对齐，融合出来的状态甚至会比单传感器更差。本节围绕三件事展开：坐标系如何定义与转换、多源时间如何对齐、状态估计接口如何组织。

一、三个坐标系：定义与转换

工程上最常用的是右手坐标系，本节统一约定如下，后续所有公式与图示均保持一致。

• 惯性系（i系）：原点在地心，坐标轴指向恒星固定方向，近似不随地球自转。它用于描述牛顿力学下的绝对运动，是IMU陀螺与加速度计积分的理论参考。
• 机体系（b系）：原点在机体质心，x轴指向机头，y轴指向右翼，z轴指向机腹（前右下）。IMU、相机、机载天线等载荷的安装位置都以机体系描述。
• 导航系（n系）：原点在无人机所在位置，x轴指东、y轴指北、z轴指天（东北天，ENU）。航迹规划、航点坐标、地面站显示通常都在n系下表达。

三者之间的转换由旋转矩阵完成。设无人机在n系下的姿态为横滚φ、俯仰θ、偏航ψ，则n系到b系的旋转矩阵为：

R_nb = Rz(ψ) * Ry(θ) * Rx(φ)

Rx(φ) = [1, 0, 0; 0, cosφ, sinφ; 0, -sinφ, cosφ]
Ry(θ) = [cosθ, 0, -sinθ; 0, 1, 0; sinθ, 0, cosθ]
Rz(ψ) = [cosψ, sinψ, 0; -sinψ, cosψ, 0; 0, 0, 1]

v_b = R_nb * v_n
v_n = R_nb^T * v_b

注意方向：R_nb把n系向量转到b系，其转置把b系向量转回n系。若把方向写反，速度符号会整体翻转，这是初学者最常见的错误。举例来说，无人机在n系下速度v_n = [5, 0, 0]（向东5 m/s），偏航角ψ=90°（机头朝北），则v_b = R_nb * v_n = [0, -5, 0]，即机体系下速度指向机腹方向，符合“机头朝北、向东运动”的物理直觉。

惯性系与导航系之间还涉及地球自转与重力。对于中小型无人机、飞行时间几分钟到几十分钟的任务，通常把n系近似为惯性系，忽略地球自转带来的科氏项（约10⁻⁴量级），但重力矢量必须保留，且方向为n系-z轴。若任务涉及长航时或高精度惯导，则需引入地球模型与经纬度，本节不展开。

二、时间同步：三路传感器如何对齐

GPS、IMU、视觉的采样率差异巨大：IMU通常200 Hz，视觉30 Hz，GPS仅1 Hz。若直接把最近一帧GPS位置与当前IMU积分结果相减，误差里会混入最多0.5 s的运动偏差——以10 m/s飞行速度计算，就是5 m的位置误差，足以让融合结果发散。

时间对齐分两步。第一步是硬件触发与时间戳统一：所有传感器使用同一时钟源，通常以IMU的采样中断为基准，GPS的PPS秒脉冲用于校准时钟漂移，视觉帧在曝光时刻打上硬件时间戳。第二步是软件插值：把低频观测插值到高频IMU时间轴上。对GPS位置，采用线性插值即可；对视觉位姿，若帧间运动较大，需用IMU预积分结果做运动补偿，而不是简单线性插值。

一个可操作的检查方法是：静止放置无人机5分钟，记录GPS位置与IMU积分位置的差值。若差值随时间线性漂移，说明时间戳存在固定偏移；若差值随机跳动，说明时钟抖动或插值策略不当。固定偏移可通过互相关估计，典型值在10～50 ms之间。

三、状态估计接口：输入输出与协方差

状态估计器的接口设计决定了后续航迹跟踪能否稳定运行。以扩展卡尔曼滤波（EKF）为例，状态向量通常取15维：

x = [p_n(3), v_n(3), q_nb(4), b_a(3), b_g(3)]^T

p_n  : n系位置 (m)
v_n  : n系速度 (m/s)
q_nb : n系到b系四元数
b_a  : 加速度计零偏 (m/s²)
b_g  : 陀螺零偏 (rad/s)

预测步由IMU驱动：用陀螺积分更新姿态，用加速度计去除重力后积分更新速度与位置，同时零偏按随机游走模型传播。更新步由GPS和视觉触发：GPS提供n系位置与速度观测，视觉提供相对位姿或地标观测。每次更新后输出状态均值与协方差矩阵P，P的对角线元素直接反映各状态量的不确定度。

接口使用中有三个关键判断。第一，协方差是否收敛：静止时位置标准差应小于0.5 m，姿态标准差小于0.5°；若持续增大，说明观测未正确接入或时间戳错位。第二，零偏估计是否合理：消费级IMU的加速度计零偏通常在0.02～0.1 m/s²，陀螺零偏在0.001～0.01 rad/s，超出这个范围需检查标定。第三，更新频率匹配：GPS更新时若IMU预测已推进超过0.5 s，应限制更新或先做状态回滚，避免线性化误差过大。