UAV 动态环境下快速自适应感知与规划

统计量定义： 对簇中每个点 $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$ 个跟踪目标）时，需决定匹配关系。

1. 马氏距离（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}$ 越小越接近，越有可能是同一物体。

2. 转换成置信指标： $$ H_{d,i} = 1 - \frac{2}{\pi} \arctan(\Omega_{d,i}) $$

   • 这是一个经验函数，把距离映射到 $(0,1)$ 区间。

构建 $D \times I$ 的代价矩阵，然后用匈牙利算法寻找最优匹配组合；若匹配分数 $H_{d,i}$ 小于阈值 $t_{h_{min}}$，则拒绝匹配。

4. 静态局部地图输出

每帧 $k$：

1. 获取局部点云 $B P_k$ 与 pose $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. 簇匹配：在当前簇 $o_k$ 与上一帧所有簇中心集合中找最近邻，若距离小于阈值则认为同一簇。
7. 分类：对簇中每个点查询 I-KD-Tree 最近距离 $d_n$，计算 $T_1, T_2$，分类为 Static / Moving / Unknown。
8. 动态簇集合 $W P_{dyn,k}$ = union of Moving clusters。
9. 静态点云 $W P_{sta,k} = W P_k \setminus W P_{dyn,k}$（对于 Unknown，按策略决定是否加入）。
10. 对每个 Moving cluster：
    • 若可与某 tracker 匹配，用匈牙利分配。
    • 对匹配 tracker：用测量 $Z$ 更新该 tracker 的 KF。
    • 对未匹配检测：新建 tracker（初始协方差设大）。
11. 用 $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$ 的传播。