4. 感知与状态估计基础

碰撞避免系统的第一道防线不是避让算法，而是“我现在在哪里、别人在哪里、我们各自以多快速度朝哪个方向运动”这一组状态量的可信程度。如果相对位置误差达到数米、速度估计滞后数百毫秒，再精巧的避让律也可能做出错误决策。本节围绕多源传感器融合、相对定位、速度估计与不确定性传播四条主线，说明它们如何共同决定碰撞避免的可用边界。

一、坐标系与状态量的约定

讨论相对定位前必须先固定参照系。本课程统一采用：世界坐标系 W 为东北天（ENU）右手系，原点取任务区域基准点；机体坐标系 B 为前-右-下（FRD）；相对状态统一表达为“他机相对本机”的矢量，即 p_rel = p_j - p_i，速度同理 v_rel = v_j - v_i。距离用 d = ||p_rel||，接近率用 v_c = -d(d)/dt，v_c > 0 表示正在接近。全文与图示均遵循该约定，切换参照系时必须显式声明。

二、多源传感器融合：把互补的“短板”拼成可用状态

单一传感器无法同时满足精度、频率、覆盖与抗干扰。GNSS 提供绝对位置但易受遮挡与欺骗，更新率通常 5～10 Hz；IMU 提供高频（100～400 Hz）角速度与比力，但积分会漂移；视觉/激光雷达提供相对几何与障碍信息，但受光照、纹理、视场限制；UWB 或 V2X 通信可给出机间测距或他机广播状态，但存在时延与丢包。融合的本质是用互补性压制各自的失效模式。

工程上常用两级结构：第一级做时间同步与空间标定，把各传感器量测统一到同一时刻与同一坐标系；第二级用误差状态卡尔曼滤波（ESKF）或因子图优化估计状态。以 ESKF 为例，状态量取 x = [p, v, q, b_a, b_g]，即位置、速度、姿态四元数、加速度计零偏、陀螺零偏。预测步由 IMU 驱动：

p ← p + v·Δt + 0.5·(R·(a_m - b_a) + g)·Δt²
v ← v + (R·(a_m - b_a) + g)·Δt
q ← q ⊗ Δq(ω_m - b_g, Δt)

更新步用 GNSS 位置、UWB 测距或视觉相对观测做量测修正。关键参数是各量测的噪声阵 R 与过程噪声 Q：R 设得过大，滤波器会忽视有效观测；设得过小，异常值会把状态拉偏。实践中常配合卡方检验剔除野值，例如对 UWB 测距残差做 r^T S^{-1} r > χ²(3, 0.99) 判定。

三、相对定位：从“各自绝对”到“机间一致”

碰撞避免真正需要的是相对量。两条技术路线各有取舍：其一是“绝对定位相减”，即各机用 GNSS/RTK 得到自身位置后通过通信交换，p_rel = p_j - p_i。优点是实现简单，缺点是误差叠加——若两机各自水平误差 0.5 m，则相对误差可达约 0.7 m（独立时按方差相加），且通信时延会引入动态误差。其二是“直接相对观测”，如 UWB 到达时间差、视觉共视特征、激光雷达点云配准，直接测量机间几何，误差不随绝对定位漂移累积，但受视距与视场约束。

实际系统多采用松耦合互补：正常时以绝对定位相减为主，进入 GNSS 受限区或高密度编队时切换到 UWB/视觉相对观测，并在滤波器中同时保留两类量测。判断切换是否成功的依据是相对位置协方差是否收敛、残差是否持续在门限内，而不是简单地看“有没有数据”。

四、速度估计：差分噪声与滤波滞后之间的取舍

速度估计有两条常见路径。其一是位置差分：v ≈ (p_k - p_{k-1})/Δt，实现简单，但噪声被 1/Δt 放大——若位置噪声 0.5 m、Δt = 0.1 s，速度噪声可达 5 m/s，完全不可用。其二是把速度作为滤波状态由 IMU 预测、位置量测修正，这也是 ESKF 的天然输出。第三条路径是多普勒观测，如 GNSS 测速或雷达径向速度，精度高但只给径向分量。

对碰撞避免而言，速度估计的关键指标不是绝对精度，而是滞后。若滤波器等效时间常数 0.3 s，而两机接近率 10 m/s，则 0.3 s 内相对位移已达 3 m，足以跨越一个安全间隔。因此高动态场景应提高 IMU 权重、缩短更新窗口，或引入前向预测：用当前 p_rel 与 v_rel 外推 τ 秒后的最近距离 d_pred = ||p_rel + v_rel·τ||，以 d_pred 而非当前 d 做判定。

五、不确定性传播：把“误差”变成“安全裕度”

不确定性不会停留在估计器内部，它会沿决策链传播。设相对位置协方差为 P_p，则距离的方差近似为 σ_d² ≈ u^T P_p u，其中 u = p_rel/||p_rel|| 为单位方向矢量。速度方差同理。若两机状态独立，相对协方差为各自协方差之和；若共享同一 GNSS 基准或同一通信链路，误差相关，简单相加会高估，需要保留互协方差项。

工程上最直接的做法是“膨胀保护半径”：把判定门限从固定 R_safe 改为 R_safe + k·σ_d，k 取 2～3 对应约 95%～99.7% 置信。这样做的代价是可用空域被压缩，因此必须同时控制 σ_d 的增长速率。可操作的判据是：设定观测中断容忍时间 T_loss，要求在该时间内 σ_d 不超过门限预算，否则降级为保守避让或悬停。

六、对碰撞避免的实际影响与检查要点

把上述链条落到系统上，可形成一份简明的检查清单：

• 时间对齐：各传感器时间戳是否统一到同一时钟？残余时延是否小于 20 ms？
• 外参标定：IMU 与 GNSS 天线杆臂、相机与机体外参是否标定并写入配置？
• 噪声阵合理性：Q、R 是否与实际残差统计一致？是否做过新息白化检验？
• 野值抑制：是否启用卡方检验或鲁棒核函数？剔除率是否被监控？
• 协方差一致性：σ_d 是否随观测中断按预期增长？是否出现过度自信（协方差远小于实际误差）？
• 降级策略：观测中断超过 T_loss 时，系统是否自动扩大保护半径或转入保守模式？

其中“协方差一致性”最容易被忽视却最危险：若滤波器报告 σ_d = 0.3 m 而实际误差 2 m，保护半径会被严重低估，碰撞判定形同虚设。验证方法是把估计轨迹与差分 GPS 或全站仪真值比对，统计归一化误差平方（NEES）是否落在理论卡方区间内。

综上，多源融合决定状态是否可得，相对定位决定机间几何是否一致，速度估计决定动态预判是否及时，不确定性传播则把前三者的残余误差转化为可执行的安全裕度。四者共同构成碰撞避免的“感知地基”，任何一环的偏差都会直接反映为避让门限的误设。