FAPP: Fast and Adaptive Perception and Planning for UAVs in Dynamic Cluttered Environments
FAPP (Fast and Adaptive Perception and Planning) 采用几何聚类结合运动估计的点云分割方法，无需依赖 GPU 即可区分快速动态与静态目标。
1. 动态环境感知
1.1 坐标系变换与增量 KD 树
- 将点云从传感器局部坐标系 $B$ 变换到全局坐标系 $W$。
- 建立增量 KD 树（I-KD Tree）来维护最近 $F$ 帧的点集合 $\xi$。点集合通过增删操作动态更新：
- $\xi \subseteq { W P_j }_{j \in [k-F, k-1]}$
- I-KD Tree 使 $\xi$ 保持合适大小并在 3D 空间上较均匀分布，较好地表示最近几帧扫描得到的障碍物空间分布信息。
为什么要让点在 $\xi$ 中'均匀分布'？
- 如果历史点云过密（如静止墙被重复扫描），KD 树查询距离几乎为 0，会掩盖其他动态物体；
- 如果过稀，则动态检测不稳定。
为什么用 KD Tree？
- KD Tree 是空间索引结构，用于快速做最近邻搜索。查询时给定一点 $p$，快速找到 $\xi$ 中离 $p$ 最近的点。
- I-KD Tree 是增量式 KD 树，新帧到来时不重新建树，而是增量更新，保留历史点云的结构信息，用于判断点是否新出现或物体是否移动。
为什么要保存多帧（而不是只上一帧）？
- 保存多帧让'静态结构'在时间上积累密度，而'动态物体'因移动形成稀疏或拖尾现象。
KD Tree 中的内容注意不是 $\xi$？
- $\xi$ 中包含多个时间戳的点云（混在一起），而非分开存储。
- 若这几帧点云合在一起形成稠密点簇，KD Tree 查询的最近距离 $d_n \approx 0$，说明'它在过去多帧都出现过 → 静态'。
1.2 区分动态/静态点云
对当前帧点云 $W P_k$ 使用 DBSCAN 聚类，得到 $m$ 个簇 $C_k = {C_1, \dots, C_m}$。对每个簇，根据阈值 $h_1, h_2$ 判定三类状态：
- Case 1（持续移动物体）：$T_1 > h_1$ 且 $T_2 < h_2$。
- 真正移动的物体，当前点到历史点云的最近距离较大（大 $T_1$），且投影相对一致，$d_n$ 分布较均匀（小 $T_2$）。
- Case 2（静态物体）：$T_1 < h_1$。
- 曾在历史帧中出现且未移动，点到历史点云的最近距离应在传感器测量误差范围内（小 $T_1$）。
- Case 3（未知物体）：$T_1 > h_1$ 且 $T_2 > h_2$。
- 包括之前被移动物体遮挡的静态物体，或新进入视野的物体。点到历史点的距离不均匀（有些近有些远），$T_2$ 较大。
参数选择：
- $\varepsilon$（邻域半径）与
min_samples决定聚类颗粒度。 - $h_1$：设为与传感器噪声/定位误差相关的倍数（例如噪声标准差的 2–3 倍）。太小误判移动为静态，太大误判静态为移动。
- $h_2$：判断分布是否'均匀'的阈值，需经验交叉验证。
- 当 $T_1$ 很小时分母数值问题，建议用 $T_1^2 + \epsilon$ 做分母保护。
统计量定义： 对簇中每个点 $p_n$（共 $N$ 个点），计算其到历史点集 $\xi$ 的全局最近距离 $d_n$。 $$ T_1 = \frac{1}{N}\sum_{n=1}^{N} d_n $$ $$ T_2 = \frac{1}{N}\sum_{n=1}^{N}\frac{(d_n-T_1)^2}{T_1^2} $$
2. 动态目标跟踪
使用离散线性系统与卡尔曼滤波器（KF）估计。若某簇是新出现的，新建一个 KF；若能关联到已有第 $i$ 个 tracker，将该簇的几何中心与帧差作为测量向量 $Z_i$ 进入 KF 更新。
状态向量： $$ X_i = [x_i, y_i, z_i, \dot{x}_i, \dot{y}_i, \dot{z}_i]^T $$
系统模型：
- 状态转移矩阵 $A_k$ 与测量矩阵 $H_k$。
- 过程噪声与测量噪声假定为零均值高斯噪声，协方差分别为 $Q$ 与 $R$。 $$ X_i^{k+1} = A_k X_i^k + \nu_i $$ $$ Z_i^k = H_k X_i^k + \omega_i $$ $$ A_k = \begin{bmatrix} I_{3\times3} & \Delta t\cdot I_{3\times3} \ 0_{3\times3} & I_{3\times3} \end{bmatrix}, \quad H_k = \begin{bmatrix} I_{3\times3} & 0_{3\times3} \ 0_{3\times3} & I_{3\times3} \end{bmatrix} $$
含义：
- $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$ 个跟踪目标）时，需决定匹配关系。
-
马氏距离（Mahalanobis distance）： $$ \Omega_{d,i} = (o_d - \hat{o}_i)^T \sigma_i^{-1} (o_d - \hat{o}_i) $$
- $o_d$：检测的中心；$\hat{o}_i$：第 $i$ 个跟踪器预测的中心；$\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. 静态局部地图输出
每帧 $k$：
- 获取局部点云 $B P_k$ 与 pose $pose ^W_{B_T}$，变换为全局 $W P_k$。
- 更新 I-KD-Tree：插入 $W P_k$、删除过期帧。
- 对 $W P_k$ 运行 DBSCAN -> 得到簇集合 ${C_1 \dots C_m}$。
- 欧式聚类将点云分为若干簇 $\xi = {C_k}_{k=1}^M$。
- 计算每个簇的几何中心 $o_k = \frac{1}{|C_k|} \sum_{p_i \in C_k} p_i$。
- 簇匹配：在当前簇 $o_k$ 与上一帧所有簇中心集合中找最近邻，若距离小于阈值则认为同一簇。
- 分类：对簇中每个点查询 I-KD-Tree 最近距离 $d_n$，计算 $T_1, T_2$，分类为 Static / Moving / Unknown。
- 动态簇集合 $W P_{dyn,k}$ = union of Moving clusters。
- 静态点云 $W P_{sta,k} = W P_k \setminus W P_{dyn,k}$（对于 Unknown，按策略决定是否加入）。
- 对每个 Moving cluster：
- 若可与某 tracker 匹配，用匈牙利分配。
- 对匹配 tracker：用测量 $Z$ 更新该 tracker 的 KF。
- 对未匹配检测：新建 tracker（初始协方差设大）。
- 用 $W P_{sta,k}$ 更新占用栅格地图（或体素地图）。
输出： 静态局部地图 + 所有 tracker 的状态 $\hat X_i$。
5. 自适应估计与预测
当目标突然加速或转弯时，模型不再准确，卡尔曼滤波的预测会滞后，创新变大。此时需自适应调整过程噪声协方差 $Q_k$。
原理：
- 理想情况下，创新应该是零均值、协方差为 $S_k = H_k P_{k|k-1} H_k^T + R_k$。
- 若模型不匹配，检测到模型不匹配，据此调整模型不确定性。
- 理论上创新的理论协方差 $S_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$ 的估计： $$ Q_k = C_{\gamma,k} - A_{k-1} P_{k-1|k-1} A_{k-1}^T - R_k $$
保证正定与数值稳定：
- 取对角：$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$ 的传播。