4. 目标运动模型与状态估计基础

目标跟踪任务规划的第一步，不是选传感器，也不是排航路，而是回答一个更基础的问题：目标接下来会出现在哪里。无人系统（无人机、无人车、无人艇）对目标进行跟踪时，传感器给出的只是离散时刻的位置量测，而规划器真正需要的是连续、平滑、可外推的目标状态——位置、速度，必要时还有加速度和转弯率。运动模型负责描述“目标如何运动”，卡尔曼滤波负责把带噪声的量测与模型预测融合成状态估计。二者共同构成跟踪规划的“预测底座”。

一、状态向量：跟踪规划真正需要什么

在二维平面跟踪中，最常用的状态向量取位置与速度：x = [px, py, vx, vy]ᵀ。若目标机动明显，可扩展为匀加速模型 x = [px, py, vx, vy, ax, ay]ᵀ；若目标是飞机、舰船等做圆弧机动，则用协同转弯（CT, Coordinated Turn）模型，状态中增加转弯率 ω：x = [px, py, vx, vy, ω]ᵀ。状态维数越高，模型表达能力越强，但对量测质量和滤波稳定性要求也越高。规划器通常关心未来 1～5 秒的预测位置，因此速度与转弯率的估计精度往往比瞬时位置更关键。

二、三类典型运动模型

匀速（CV）模型假设目标速度恒定、无加速度，是最简单也最鲁棒的模型。其离散状态转移为 p(k+1) = p(k) + v(k)·T，v(k+1) = v(k)，其中 T 为采样周期。CV 模型对直线巡航的固定翼无人机、公路行驶车辆效果良好，但在目标转弯或加减速时会出现明显滞后。

匀加速（CA）模型在状态中引入加速度，转移关系为 p(k+1) = p(k) + v(k)T + ½a(k)T²，v(k+1) = v(k) + a(k)T。它能跟踪加速、减速和抛物线机动，但加速度不可直接观测，只能从位置量测的二阶差分中间接估计，噪声放大严重，因此过程噪声需要设置得比 CV 模型更大。

协同转弯（CT）模型描述目标以恒定速率和恒定转弯率做圆弧运动。设转弯率为 ω，则速度分量按旋转矩阵演化：vx(k+1) = vx(k)cos(ωT) − vy(k)sin(ωT)，vy(k+1) = vx(k)sin(ωT) + vy(k)cos(ωT)，位置由速度积分得到。CT 模型对盘旋侦察机、绕圈巡逻艇等目标非常贴合，但 ω 本身是待估参数，模型呈非线性，通常需要扩展卡尔曼滤波（EKF）或无迹卡尔曼滤波（UKF）处理。

三、卡尔曼滤波：预测与更新的闭环

卡尔曼滤波把跟踪问题写成两个交替步骤。预测步用运动模型把上一时刻状态外推到当前时刻：x̂⁻(k) = F·x̂(k−1)，P⁻(k) = F·P(k−1)·Fᵀ + Q。其中 F 由所选模型决定，Q 是过程噪声协方差，代表“模型有多不可信”。更新步用量测 z(k) 修正预测：先算新息 y = z(k) − H·x̂⁻(k)，再算卡尔曼增益 K = P⁻Hᵀ(HP⁻Hᵀ + R)⁻¹，最后 x̂(k) = x̂⁻(k) + K·y，P(k) = (I − KH)P⁻(k)。H 是量测矩阵，R 是量测噪声协方差。

对规划而言，滤波输出的不只是估计值，还有协方差 P。P 的主对角元反映位置、速度估计的不确定度，它直接决定预测轨迹的“可信半径”。当 P 增大，说明目标状态不确定，规划器应缩短预测时域、增大安全间隔或提高传感器更新率。

四、模型选择与参数设置的实际做法

实际工程中很少只用单一模型。常见做法是交互式多模型（IMM）：并行运行 CV、CA、CT 三个滤波器，按各模型似然计算权重并加权融合。这样目标直线飞行时 CV 权重升高，进入盘旋时 CT 权重自动上升，无需人工切换。

参数设置上，采样周期 T 应与传感器帧率一致，例如雷达 10 Hz 对应 T = 0.1 s。过程噪声 Q 的取值可用“最大预期加速度”反推：若目标最大加速度约 3 m/s²，则位置过程噪声谱密度可取 q ≈ (3)²·T³/3 量级作为起点，再根据新息序列是否白噪声来微调。若新息持续偏大且同号，说明 Q 偏小、模型跟不上机动；若估计轨迹抖动剧烈，说明 Q 偏大或 R 偏小。

五、滤波结果如何进入任务规划

跟踪规划需要的是“未来一段时间的目标位置分布”。卡尔曼滤波给出的 x̂(k) 与 P(k) 正好支撑这一点：用 F 外推 n 步得到预测状态 x̂(k+n|k)，同时外推协方差 P(k+n|k)，即可得到带不确定椭圆的预测航迹。规划器据此完成三件事：一是计算跟踪平台的期望航向与速度，使传感器视场覆盖预测位置；二是当预测不确定度超过阈值时触发重捕或缩小预测步长；三是在多目标场景中按预测协方差分配跟踪资源，把精度高的传感器优先分给机动性强、P 增长快的目标。

需要强调的是，模型与滤波不是一次性设定，而是随任务阶段动态调整的。远距离搜索阶段可用 CV 模型保证稳定；确认目标并进入近距跟踪后，若观测到持续转弯，应切换到 CT 或启用 IMM；目标做剧烈规避时，可临时增大 Q 提高响应速度，代价是估计噪声上升。理解这一权衡，才能在跟踪规划中把“预测得准”与“预测得稳”平衡好。