FAPP 概述
FAPP (Fast and Adaptive Perception and Planning) 是一种针对无人机在动态杂乱环境中进行快速感知与规划的方法。该方法采用几何聚类结合运动估计的点云分割策略，无需依赖 GPU 即可区分快速动态与静态目标。
动态环境感知
1. 点云预处理与 I-KD Tree
- 将点云从传感器局部坐标系 $B$ 变换到全局坐标系 $W$。
- 建立增量 KD 树（I-KD Tree）来维护最近 $F$ 帧的点集合 $\xi$。点集合通过增删操作动态更新：
- $\xi \subseteq { W P_j }_{j \in [k-F, k-1]}$
- 使用 I-KD Tree 可使 $\xi$ 保持合适的大小并在 3D 空间上较均匀分布，从而较好地表示最近几帧扫描得到的障碍物空间分布信息。
设计考量：
- 均匀分布：如果历史点云过密（如静止墙重复扫描），KD 树查询距离几乎为 0，会掩盖其他动态物体；如果过稀，则动态检测不稳定。
- 多帧保存：保存多帧而非仅上一帧，让'静态结构'在时间上积累密度，而'动态物体'因移动形成稀疏或拖尾现象。
- KD Tree 作用：作为空间索引结构，用于快速做最近邻搜索。I-KD Tree 支持增量更新，保留历史点云结构信息，便于判断新出现点或移动物体。
2. 动态/静态点云分类
对当前帧点云 $W P_k$ 使用 DBSCAN 聚类，得到 $m$ 个簇 $C_k = { C_1, \dots, C_m }$。对每个簇计算两个统计量：
- 平均最近距离 $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 + \epsilon}$（衡量相对离散程度，加入 $\epsilon$ 防止分母过小）
根据阈值 $h_1, h_2$ 判定三类情况：
- Case 1（持续移动物体）：$T_1 > h_1$ 且 $T_2 < h_2$。当前点到历史点云距离大，且投影一致，判断为连续移动。
- Case 2（静态物体）：$T_1 < h_1$。点在历史帧中出现且未移动，距离在测量误差范围内。
- Case 3（未知物体）：$T_1 > h_1$ 且 $T_2 > h_2$。可能是被遮挡后显现的静态物体或新进入视野的物体，距离分布不均匀。
参数选择：
- $\varepsilon$ 和
min_samples决定 DBSCAN 聚类颗粒度。 - $h_1$ 设为传感器噪声/定位误差的倍数（如 2–3 倍标准差）。
- $h_2$ 为经验值，需交叉验证。
3. 动态目标跟踪
使用离散线性系统与卡尔曼滤波器（KF）估计状态。若某簇为新出现，新建一个 KF；若能关联到已有 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$ 假定为块对角单位阵形式。
- 过程噪声 $\nu \sim \mathcal{N}(0, Q)$，测量噪声 $\omega \sim \mathcal{N}(0, R)$。
- 观测向量 $Z_i = [o_{ix}, o_{iy}, o_{iz}, \Delta o_{ix}, \Delta o_{iy}, \Delta o_{iz}]^T$，其中 $\Delta o$ 近似瞬时速度的观测。
KF 更新公式： $$ \hat{X} = \hat{X}{pred} + K y, \quad P = (I - KH)P{pred} $$ 其中 $K = P_{pred} H^T (H P_{pred} H^T + R)^{-1}$。
