3. 无人机运动学与观测模型

多无人机协同定位的精度上限，首先由单机的运动学模型与观测模型决定。协同算法只是把各机信息“拼”在一起，如果单机模型本身写错，融合结果只会更自信地错。本节从坐标系约定出发，依次建立典型运动模型、四类传感器观测方程，以及噪声与偏差的建模方式。

一、坐标系与状态量约定

全文统一采用右手系：世界系 W 为东北天（ENU），机体系 B 为前-右-下（FRD）。状态量取 x = [p, v, q, b_a, b_g]，其中 p 为世界系位置，v 为世界系速度，q 为机体系到世界系的旋转四元数，b_a、b_g 分别为加速度计与陀螺仪的零偏。约定重力矢量在世界系为 [0, 0, -g]，g ≈ 9.81 m/s²。所有观测方程中的噪声均定义为加性零均值高斯白噪声，偏差定义为缓变随机游走。

二、典型运动模型

工程中最常用的是恒速（CV）模型与恒加速度（CA）模型，二者都是线性模型，便于卡尔曼滤波直接使用。CV 模型假设加速度为零均值扰动：

p_{k+1} = p_k + v_k·Δt + 0.5·w_a·Δt²
v_{k+1} = v_k + w_a·Δt

其中 w_a 为过程噪声加速度，典型取值 0.5～2 m/s²（示例值，需按机型标定）。CA 模型把加速度也作为状态，适合急加减速的巡检或穿越场景。对于多旋翼，更贴合的是微分平坦模型：以位置、偏航及其导数为平坦输出，推力与姿态由平坦输出解析求得，因此协同定位中常直接对位置-速度-加速度建模，而不显式建模姿态动力学。

当无人机做协调转弯时，可用协同转弯（CT）模型，状态中加入角速度 ω，位置更新为圆弧形式。选择原则是：机动弱、采样快用 CV；机动强、需预测未来轨迹用 CA 或 CT。模型阶数越高，对观测频率与噪声统计的准确性要求越高。

三、四类观测方程

IMU 观测方程（机体系）：

a_m = R(q)ᵀ·(a_W - g_W) + b_a + n_a
ω_m = ω_B + b_g + n_g

IMU 提供高频（100～1000 Hz）的角速度与比力，是状态递推的主力，但单独积分会因零偏产生立方级漂移，必须由其他传感器约束。

GNSS 观测方程（世界系）：

z_gnss = p + n_p ,  n_p ~ N(0, σ_p²)

单点定位水平精度约 1.5～3 m（示例），RTK 可达厘米级。GNSS 提供绝对位置，抑制长期漂移，但更新率低（1～10 Hz）且易受遮挡与多径影响。

视觉观测方程：对已知世界坐标的地标 L_i，机体系投影为

u_i = f_x·(X_c/Z_c) + c_x + n_u
v_i = f_y·(Y_c/Z_c) + c_y + n_v
[X_c,Y_c,Z_c]ᵀ = R(q)ᵀ·(L_i - p)

视觉提供高精度相对/绝对位姿，但存在尺度不确定（单目）、光照敏感与计算延迟问题。

雷达观测方程：以距离-方位-俯仰表示，对目标 T：

r = ‖T - p‖ + n_r
θ = atan2(Δy, Δx) + n_θ
φ = asin(Δz / r) + n_φ

雷达（含 UWB 测距）抗光照、可穿透烟尘，适合机间相对观测，但角分辨率通常低于视觉。

四、噪声与偏差建模

IMU 误差分两类：白噪声（角度随机游走、速度随机游走）与零偏。白噪声用连续时间谱密度描述，离散化时按 σ_d = σ_c / √Δt 折算；零偏建模为随机游走 ḃ = n_b，其驱动噪声 n_b 决定零偏变化快慢。典型 MEMS 器件示例：陀螺零偏稳定性 10 °/h，加计零偏 0.02 mg，需通过 Allan 方差标定。

GNSS 噪声除高斯项外，还须处理野值与多径：可用卡方检验剔除新息超阈值的量测，或采用 Huber 鲁棒核降低大残差权重。视觉噪声与像素坐标相关，重投影误差近似服从高斯分布，但需注意深度方向误差随距离平方增长。雷达测距噪声通常为厘米级，角度噪声为度级，远距离时角度误差会放大为显著的位置误差。

五、协同定位中的模型一致性

多机协同时，各机必须使用同一世界系与同一时间基准。若 A 机用 ENU、B 机用 NED，相对观测方程会整体翻转，融合必然发散。时间上需将各传感器时间戳对齐到统一时钟，IMU 递推到观测时刻再做更新。此外，机间相对观测（UWB/雷达）的观测方程应写成两机状态的函数，例如测距 r_ij = ‖p_i − p_j‖ + n_r，这样协同滤波才能同时约束两机位置，而不是把邻居位置当作已知常量。

实际调试时，可先单独跑 IMU+GNSS 看轨迹是否平滑、零偏是否收敛；再接入视觉检查重投影残差是否在 1 像素量级；最后加入机间测距，观察相对位置协方差是否下降。若融合后轨迹出现周期性抖动，优先检查观测时间戳对齐与噪声参数是否被低估。