FAPP：面向动态杂乱环境的 UAV 感知与规划

这里的 $o_i = (o_{ix}, o_{iy}, o_{iz})$ 是当前帧检测到的簇中心，$\Delta o_i = o_i^{(t)} - o_i^{(t-1)}$ 是相邻帧簇中心差分，可以近似看成速度观测。这个做法不花哨，但很实用：比起只喂位置，加入帧间差分后，KF 收敛会快一些。

KF 更新步骤就是标准形式。先算残差 $y = Z - H \hat X_{pred}$，再算卡尔曼增益 $K = P_{pred} H^T (H P_{pred} H^T + R)^{-1}$，最后更新：

$$ \hat X = \hat X_{pred} + K y, \quad P = (I-KH)P_{pred} $$

3. 数据关联

当检测数 $D$ 和 tracker 数 $I$ 都不止一个时，就得决定谁对应谁。

先用马氏距离衡量检测和预测之间的接近程度：

$$ \Omega_{d,i} = (o_d - \hat{o}_i)^T \sigma_i^{-1} (o_d - \hat{o}_i) $$

• $o_d$ 是检测中心；$\hat{o}_i$ 是第 $i$ 个 tracker 预测中心；$\sigma_i$ 是 KF 的预测协方差。
• $\Omega_{d,i}$ 越小，越像同一个目标。

然后把它映射成一个置信指标：

$$ H_{d,i} = 1 - \frac{2}{\pi} \arctan(\Omega_{d,i}) $$

这个函数本质上是个经验映射，把距离压到 $(0,1)$ 区间。接下来构建 $D \times I$ 的代价矩阵，用匈牙利算法求最优匹配；如果匹配分数 $H_{d,i}$ 低于阈值 $t_{h_{min}}$，就直接拒绝。

4. 静态局部地图输出

每一帧的处理顺序基本是这样的：

1. 读取局部点云 $B P_k$ 和位姿 $pose ^W_{B_T}$，变换到全局坐标系得到 $W P_k$。
2. 更新 I-KD-Tree：插入 $W P_k$，删除过期帧。
3. 对 $W P_k$ 做 DBSCAN，得到簇集合 ${C_1 \dots C_m}$。
4. 再用欧式聚类把点云分成若干簇 $\xi = {C_k}_{k=1}^M$。
5. 计算每个簇的几何中心 $o_k = \frac{1}{|C_k|} \sum_{p_i \in C_k} p_i$。
6. 做簇匹配：当前簇中心和上一帧所有簇中心找最近邻，小于阈值就认为是同一簇。
7. 对簇中每个点查询 I-KD-Tree 的最近距离 $d_n$，计算 $T_1, T_2$，并分类为 Static / Moving / Unknown。
8. 把 Moving 簇合并成动态点云 $W P_{dyn,k}$。
9. 静态点云定义为 $W P_{sta,k} = W P_k \setminus W P_{dyn,k}$；Unknown 是否并入静态，取决于具体策略。
10. 对每个 Moving 簇：

• 能匹配 tracker 的，用匈牙利分配；
• 匹配上的 tracker 用测量 $Z$ 更新 KF；
• 没匹配上的检测则新建 tracker，初始协方差设大一些。

11. 用 $W P_{sta,k}$ 更新占用栅格地图或体素地图。

输出就是静态局部地图和所有 tracker 的状态 $\hat X_i$。这一步把'看见了什么'和'谁在动'分开了，后面的规划才不会被动态障碍拖着跑。

5. 自适应估计与预测

当目标突然加速、急转弯，或者运动模型和真实轨迹差太多时，KF 的预测会明显滞后，创新项也会变大。这个时候固定的过程噪声 $Q$ 往往不够用，需要动态调大。

核心思路不复杂：理想情况下，创新应该是零均值，协方差为

$$ S_k = H_k P_{k|k-1} H_k^T + R_k $$

如果模型不匹配，理论协方差和经验协方差就会偏开。于是可以拿经验协方差 $C_{\gamma,k}$ 去反推 $Q$。

推导过程如下：

$$ S_k = \operatorname{Cov}(\gamma_k) = H_k P_{k|k-1} H_k^T + R_k $$ $$ P_{k|k-1} = A_{k-1} P_{k-1|k-1} A_{k-1}^T + Q_{k-1} $$

代入后得到：

$$ S_k = H_k (A_{k-1} P_{k-1|k-1} A_{k-1}^T + Q_{k-1}) H_k^T + R_k $$

再用经验值 $C_{\gamma,k}$ 近似 $S_k$，可得：

$$ H_k Q_{k-1} H_k^T \approx C_{\gamma,k} - H_k A_{k-1} P_{k-1|k-1} A_{k-1}^T H_k^T - R_k $$ $$ Q_k = C_{\gamma,k} - A_{k-1} P_{k-1|k-1} A_{k-1}^T - R_k $$

这个式子算出来的 $Q$ 不能直接照搬，实际实现里通常还要做几道保险：

• 取对角：$Q_e = \operatorname{diag}(\hat Q_k)$。
• 逐元素截断：$Q_k(i,i) = \max(0, Q_e(i,i))$。
• 限制范围：$Q_k(i,i) = \operatorname{clip}(Q_k(i,i), Q_{\min}, Q_{\max})$。

说白了，这一步是在给滤波器'松绑'或'收紧'，让它在目标运动变化大时别太死板。

6. 动态碰撞代价

最后是规划里的动态碰撞代价。对 $I$ 个被跟踪目标求和：

$$ G_d = \sum_{i=1}^I \max\big{(D^d_i - | p_l(t_\tau) - p^b_i(t_\tau)|^2),,0\big}^3 $$

其中安全间隔

$$ D^d_i = r_0 + r_i + e_i $$

$r_i$ 是目标半径，$e_i$ 是因为位置不确定性带来的额外缓冲：

$$ e_i = \sqrt{ \sigma_{i\tau}(1,1)^2 + \sigma_{i\tau}(2,2)^2 + \sigma_{i\tau}(3,3)^2 } $$

这里的 $\sigma_{i\tau} = A_\tau \sigma^i_k A_\tau^T$ 表示第 $i$ 个目标的协方差从时间 $t_k$ 传播到 $t_\tau$。这个代价项的作用很直接：目标越近、越不确定，规划就越要绕开。