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}$。
4. 数据关联
当有多个检测（$D$ 个障碍物）和多个 tracker（$I$ 个跟踪目标）时，需决定匹配关系：
- 马氏距离：$\Omega_{d,i} = (o_d - \hat{o}_i)^T \sigma_i^{-1} (o_d - \hat{o}_i)$。越小越接近。
- 置信指标：$H_{d,i} = 1 - \frac{2}{\pi} \arctan(\Omega_{d,i})$，映射到 (0,1) 区间。
- 匈牙利算法：构建 $D \times I$ 代价矩阵，寻找最优匹配组合。若分数小于阈值 $t_{h_{min}}$，拒绝匹配。
5. 静态局部地图输出
获取当前帧动态点集 $W P_{dyn,k}$ 并从总点云 $W P_k$ 中移除，得到静态点云 $W P_{sta,k} = W P_k \setminus W P_{dyn,k}$。静态点云包含自由空间/占用空间完整信息，用于构建占用栅格地图避障。
流程：
- 更新 I-KD-Tree（插入新帧、删除过期帧）。
- 运行 DBSCAN 得到簇集合。
- 计算簇中心并匹配历史簇中心确定同一物体。
- 根据 $T_1, T_2$ 分类簇（Static / Moving / Unknown）。
- 对 Moving cluster 进行数据关联与 KF 更新。
- 用 $W P_{sta,k}$ 更新占用栅格地图。
自适应估计与预测
当目标突然加速或转弯时，固定协方差的卡尔曼滤波预测会滞后。FAPP 引入自适应过程噪声协方差 $Q_k$ 调整机制。
原理： 利用创新项（Innovation）$\gamma_k = Z_k - \hat{Z}{k|k-1}$ 的统计特性反解 $Q$。理论上，创新理论协方差 $S_k$ 应与经验协方差 $C{\gamma,k}$ 一致。
推导： $$ S_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} $$ 代入得： $$ 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$ 估计值，取对角线并截断保证正定： $$ Q_k(i,i) = \operatorname{clip}(\max(0, Q_e(i,i)), Q_{\min}, Q_{\max}) $$
步骤：
- 卡尔曼预测标准步骤。
- 计算创新 $\gamma_k$ 并存入滑动窗口（长度 $W$）。
- 计算经验创新协方差 $C_{\gamma,k}$。
- 计算理论项 $T$。
- 反解 $Q_k$ 并限制范围。
- 使用新 $Q_k$ 继续 KF 更新。
动态碰撞代价
对 $I$ 个被跟踪目标求和计算动态碰撞代价： $$ G_d = \sum_{i=1}^I \max\left{ (D^d_i - | p_l(t_\tau) - p^b_i(t_\tau)|^2), 0 \right}^3 $$ 其中安全间隔 $D^d_i = r_0 + r_i + e_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$ 的传播结果。
