基于FLISP的大规模隧道空地协作：快速LiDAR-IMU同步路径规划器 Large-Scale Tunnel Air-Ground Collaboration With FLISP: Fast LiDAR-IMU Synchronized Path Planner

FLISP的完整流程包含五个主要步骤。步骤1：UGV基础走廊生成。首先通过点云采样估计隧道壁法向量$\vec{v}$，计算偏航角偏差$\theta = \pi - \arccos([1\ 0\ 0] \cdot \vec{v})$，融合到姿态四元数$q_d = q_b \otimes [\cos(\theta/2), 0, 0, \sin(\theta/2)]$实现完整状态估计。然后根据yaw角$\theta$动态调整步长$\Delta s$，将点云分箱$B_j = \{(x_i, y_i, z_i) \in P | (j-1)\Delta s \leq x_i < j\Delta s\}$，提取每个箱的左右边界点$p_{Lj} = \arg\min_{y} (x,y,z) \in B_j$和$p_{Rj} = \arg\max_{y} (x,y,z) \in B_j$。通过分析局部斜率序列$K = \{k_i | k_i = \frac{y_{i+\delta}-y_i}{x_{i+\delta}-x_i}, i=1,2,...,n-\delta\}$的方差$\sigma_K^2$区分直线和弯道，应用多级拟合模型推断完整边界：$y = \begin{cases} \beta_0 + \beta_1 x, & \sigma_K^2 \leq \tau_{\text{curve}} \\ \sum_{j=0}^{2m} \alpha_j x^j, & \sigma_K^2 > \tau_{\text{curve}} \end{cases}$。初始路径为$P_{\text{initial}} = \frac{p_{Li} + p_{Ri}}{2}$。步骤2：路径鲁棒化和细化。对每个点$p_i$，基于邻居构造前向预测$\hat{y}_{i,\text{prev}} = y_{i-1} + \frac{y_{i-1}-y_{i-2}}{x_{i-1}-x_{i-2}}(x_i-x_{i-1})$和后向预测$\hat{y}_{i,\text{next}} = y_{i+1} + \frac{y_{i+1}-y_{i+2}}{x_{i+1}-x_{i+2}}(x_i-x_{i+1})$，合并为$\hat{y}_i = \frac{\hat{y}_{i,\text{prev}} + \hat{y}_{i,\text{next}}}{2}$。计算测量误差$\varepsilon_i = y_i - \hat{y}_i$，建模为高斯分布：$p(\varepsilon_i|S_n) = \frac{1}{\sigma_n\sqrt{2\pi}}e^{-\frac{\varepsilon_i^2}{2\sigma_n^2}}$（正常点）和$p(\varepsilon_i|S_o) = \frac{1}{\sigma_o\sqrt{2\pi}}e^{-\frac{\varepsilon_i^2}{2\sigma_o^2}}$（离群点）。根据贝叶斯定理计算后验概率$P(S_o|\varepsilon_i) = \frac{p(\varepsilon_i|S_o)P(S_o)}{p(\varepsilon_i|S_n)P(S_n) + p(\varepsilon_i|S_o)P(S_o)}$，超过阈值则修正点$p_i \leftarrow (x_i, \hat{y}_i)$。步骤3：动态安全和避障。沿路径构建安全走廊，检测盒$B_i = \{p | R_i^T(p-c_i) \in D_i\}$定义为旋转后的轴对齐边界框$D_i = [-L_i, L_i] \times [-r, r] \times [z_{\min}, z_{\max}]$，其中$L_i$为段长、$r$为横向安全半径、$[z_{\min}, z_{\max}]$为垂直限制。使用空间分区策略将检测域离散化为路径对齐的1D网格，通过快速整数索引($O(1)$)将点映射到单元，仅对路径占据单元中的点进行精确几何包含检查（窄相），避免暴力迭代($O(N \cdot M)$)的延迟。检测到障碍物后，用增强型FA优化局部避障路径，约束在隧道物理边界内：$y_{\min} = -R_{\text{pipe}} + \frac{W_v}{2} + M_s$、$y_{\max} = R_{\text{pipe}} - \frac{W_v}{2} - M_s$（$R_{\text{pipe}}$为隧道半径、$W_v$为车辆宽度、$M_s$为安全缓冲），并施加最大倾角约束$\frac{|y|}{R_{\text{pipe}}} \leq \theta_{\max}$防止翻滚。路径点质量由亮度$I(y) = d_{\text{obstacle}} - w_{\text{tilt}} \cdot \frac{|y|}{R_{\text{pipe}}} - w_{\text{center}} \cdot |y|$评价，萤火虫位置更新为$y_i^{(t+1)} = y_i^{(t)} + \beta_0 e^{-\gamma r_{ij}^2}(y_j^{(t)} - y_i^{(t)}) + \alpha(t) \cdot \text{rand}(-0.5, 0.5) \cdot (y_{\max} - y_{\min})$，随机参数$\alpha(t)$几何衰减$\alpha(t+1) = 0.95\alpha(t)$。步骤4：最终路径平滑和姿态校正。为减轻翻滚风险，应用姿态校正机制，首先用收缩因子衰减测量角度$\theta_{\text{adjusted}} = \theta_{\text{original}} \cdot f_{\text{shrink}}$。自适应计算过渡点数量$n_{\text{transition}} = \min\left(n_{\text{total}}, \frac{n_{\text{total}}}{1+e^{-|\theta_{\text{yaw}}|}}\right)$，车辆姿态在这些点上线性插值$\theta_{\text{yaw,roll}}(i) = \theta_{\text{yaw,roll}}\left(1 - \frac{i}{n_{\text{transition}}}\right)$，最终变换使用四元数$\vec{p}_{\text{global}}(i) = \begin{cases} q_{\theta(i)} \otimes \vec{p}_{\text{local}}(i), & i < n_{\text{transition}} \\ q_0 \otimes \vec{p}_{\text{local}}(i), & \text{otherwise} \end{cases}$得到可执行UGV路径$\phi$。步骤5：UAV路径规划。以精炼的UGV路径$\phi$为参考，首先生成最低安全高度路径$P_{\text{lowest}}$和期望高度路径$P_h$（从隧道中心线投影），在每个$P_{\text{lowest}}$点上构建通信安全约束，初始UAV路径$\Gamma_{\text{initial}}$确保$P_h$中每个点$\phi_i$满足约束：$\gamma_i = \begin{cases} \phi_i, & \|\phi_i - p_{li}\| \leq r \\ p_{li} + r \cdot \frac{\phi_i - p_{li}}{\|\phi_i - p_{li}\|}, & \text{otherwise} \end{cases}$。初始路径需要优化平滑性和避障，使用动态采样迭代方法。对每个路点，在其邻域内用极坐标策略生成候选点$\zeta_i$：$\zeta_i = \zeta_0 + r_{sp} \begin{bmatrix} \cos \theta_i \\ \sin \theta_i \\ 0 \end{bmatrix}$。用多目标代价函数评价每个候选：$J(\zeta) = \alpha J_1(\zeta) + \beta J_2(\zeta) - \eta J_3(\zeta) + \delta J_4(\zeta)$，其中$J_1$为安全代价（对圆柱安全走廊内的障碍施加二次惩罚）、$J_2$为平滑代价（惩罚偏离直线）、$J_3$为前进奖励（奖励向目标前进）、$J_4$为高度一致性代价（惩罚不必要的高度变化）。求解优化问题$\zeta^* = \arg\min_{\zeta \in Z} J(\zeta)$得到最优点，最终优化UAV路径$\epsilon = \{\zeta^*_0, \zeta^*_1, ..., \zeta^*_n\}$。

FLISP的技术新颖性体现在四个方面：一是首次提出UGV主导的异构协作无地图规划范式，将路径生成的计算复杂度从$O(N \cdot M)$降低到$O(1)$，实现了三个数量级的速度提升（Method II延迟6492.61 ms → FLISP 7.05 ms）；二是通过分层多项式拟合和贝叶斯离群点检测，在稀疏点云下仍能推断完整边界，具有天然抗噪声能力，避免了SLAM漂移；三是针对隧道场景定制的平台特定求解器，UGV的增强型FA引入动态加权代价函数（$w_{\text{tilt}}$非线性增长）、UAV的动态迭代优化采用多目标代价函数（$\alpha, \beta, \eta, \delta$权重调节），相比通用RRT*和A*在效率和质量上都有显著提升；四是通过长视距几何拟合（50米前瞻）避免局部盲点导致的死锁和翻滚风险，实验中成功避免了TEB局部规划器的屋顶阴影陷阱和拓扑死锁。此外，论文还揭示了基于地图方法的计算瓶颈（80%延迟用于网格栅格化和C-Space生成），以及采样方法在非凸盲弯中的收敛问题，为后续研究提供了重要见解。

FLISP存在以下具体弱点和改进方向：1) 线性结构假设限制：当前框架无法处理隧道分支、交汇或环路等复杂拓扑。改进方向是引入拓扑决策层（作者已提出），结合全局地图信息进行复杂场景的路径选择和切换。2) 依赖局部里程计：在极端泥泞、积水或车轮打滑时，局部里程计失效可能影响跟踪精度。改进方向是融合视觉里程计或里程计滑移检测机制，在里程计失效时切换到纯LiDAR模式或降低跟踪速度。3) 传感器鲁棒性：强反射（水面）或全遮挡（浓烟）会导致LiDAR数据退化。改进方向是融合多模态传感器（如毫米波雷达、热成像）或在传感器退化时触发安全停止模式。4) UGV机械约束：慢响应底盘无法执行急转弯，在连续窄弯中可能失败。改进方向是优化机械设计（提高转向带宽）或采用分段策略（先前进再倒车调整）。5) 缺乏动态预测：对移动人员等动态障碍仅通过重规划响应，效率较低。改进方向是引入轨迹预测模块（如卡尔曼滤波、社交力模型），预测障碍运动并提前规划。6) 异构协调不足：UAV和UGV之间缺乏任务级协调（如动态分配检测区域）。改进方向是引入任务调度层，根据环境复杂度和传感器覆盖自动调整平台分工。

作者提出的未来工作方向包括：1) 扩展框架超越线性结构，通过集成拓扑决策层处理复杂分支和交汇；2) 与并行开发的控制势垒函数（CBF）共识跟踪控制器集成，在物理平台上评估组合性能。基于成果可延伸的方向包括：1) 多团队协同：将框架扩展到多UGV-UAV团队，实现更大规模隧道的并行检测；2) 自适应参数调节：根据隧道几何特征（曲率、宽度）动态调整多项式阶数、FA参数和代价函数权重，提升自适应性；3) 在线建图与规划结合：在无地图规划基础上选择性构建关键区域地图（如闸门、弯道），提供长期记忆和重访能力；4) 能量优化：在路径规划中集成UAV续航模型，优化飞行高度和速度以最大化检测范围；5) 故障检测与恢复：引入异常检测机制（如点云密度异常、IMU漂移），在传感器或执行器故障时触发安全模式；6) 人机协同：集成远程操作接口，在自动化系统失效时允许人工接管，同时保留自主规划能力。

论文的复现性评估如下：开源情况方面，作者已在GitHub发布源代码（https://github.com/ArchibaldGuo/FLISP.git）和代表性LiDAR-IMU数据集，包含隧道环境中的关键几何退化场景，便于其他研究者复现和对比。数据方面，论文使用真实1.2公里水电隧道数据，包含多种代表性场景（入口闸门、弯道、直道、闸门穿越、淤泥积累等），并详细记录实验参数（表3：物理参数、FLISP规划视距50米、Method II最大规划时间8秒、Method III网格分辨率0.1米等），便于复现。算力方面，系统运行在UGV搭载的工业计算机（Intel i7-12700 CPU、NVIDIA GTX 4060-8G GPU、32 GB RAM），实时性能良好，但GPU未充分利用（规划主要在CPU上），研究者可在更低配置上验证核心算法。难度方面，论文提供详细算法描述（Algorithm 1、Algorithm 2）和参数设置，但部分实现细节（如多项式拟合的阶数选择、FA参数$\beta_0, \gamma$的调节策略、多目标代价函数权重$\alpha, \beta, \eta, \delta$的确定）未完全公开，可能影响精确复现。实地实验依赖特定硬件（Ouster OS1-64 LiDAR、DJI Mavic 3T UAV、自制UGV）和真实隧道环境，完全复现实地实验难度较大，但仿真实验可通过Gazebo环境复现。总体而言，论文在开源和数据共享方面表现良好，核心算法可在中等算力平台上复现，但完全复现实地实验需要特定硬件和环境条件。