📚 课程目录第4章环境建模

4、环境建模：栅格地图、八叉树地图、点云地图的构建与处理

环境建模是四旋翼无人机避障路径设计的基石。无论采用何种规划算法（如A*、RRT*或人工势场法），其输入都必须是结构化、可查询的环境表示。本章聚焦三种主流建模方法：栅格地图（Grid Map）、八叉树地图（OctoMap）与点云地图（Point Cloud Map）。我们将从数据来源、构建原理、内存效率、查询复杂度及工程适用场景五个维度展开，帮助你在实际飞控与机载计算资源约束下做出合理选型。

4.1 核心原理：三种地图的数学本质与适用边界

栅格地图将连续空间离散化为固定尺寸的立方体（2D为正方形，3D为正方体）。每个栅格存储占据概率值（0~1），通常以对数几率（Log-Odds）形式更新以支持多帧融合。其数学表达为：设栅格集合 \(G = \{g_{i,j,k}\}\)，占据概率 \(P(g_{i,j,k}) \in [0,1]\)。栅格地图的优点是查询速度极快（直接索引），但内存消耗随分辨率呈三次方增长。例如，一个100m×100m×10m的空间，若分辨率为0.1m，则栅格数量为1000×1000×100 = 1亿个，每个栅格若用float32存储，需400MB内存，这对机载计算机是巨大负担。

八叉树地图采用递归空间分割：根节点代表整个空间，若某节点内部所有体素状态一致（全占据或全空闲），则不再细分；否则分裂为8个子节点。这种自适应结构使内存消耗与场景复杂度成正比，而非空间体积。OctoMap库（基于八叉树）支持概率更新、多分辨率查询和压缩序列化。其深度通常设为16层（对应0.01m~0.05m分辨率），内存占用比同分辨率栅格地图减少80%~95%。

点云地图直接存储传感器原始测量点（x,y,z,intensity），不进行体素化。其优势是精度无损、内存占用小（每点约16字节），但无法直接查询某位置是否被占据，必须借助KD-Tree或体素滤波（Voxel Grid Filter）进行近邻搜索。点云地图适合作为SLAM前端输出，但直接用于路径规划时需额外构建占据网格或进行碰撞检测。

维度	栅格地图	八叉树地图	点云地图
数据结构	均匀数组	递归树	无序点集
内存复杂度	O(N³)	O(占据体素数)	O(点数)
查询耗时	O(1)	O(logN)	O(logN)需KD-Tree
更新方式	Log-Odds累加	概率更新+剪枝	直接插入/删除
典型分辨率	0.05m~0.2m	0.01m~0.1m	原始精度
适用场景	2D导航、小场景	3D避障、大场景	SLAM、高精感知

4.2 构建流程与关键参数配置

三种地图的构建流程均遵循“传感器数据→坐标变换→模型更新”的通用链路，但具体操作差异显著。

栅格地图构建步骤

数据预处理：将激光雷达或深度相机数据从传感器坐标系变换到世界坐标系（需外参标定）。
射线投射：从传感器原点出发，沿光束方向遍历栅格，命中点标记为占据（+0.4），中间点标记为空闲（-0.1）。
概率更新：使用公式 \(L_{new} = L_{old} + L_{hit} - L_{prior}\)，其中 \(L_{prior}=0\)，\(L_{hit}=0.85\)，\(L_{miss}=0.4\)（对数几率值）。
阈值判定：当 \(P > 0.7\) 视为占据，\(P < 0.3\) 视为空闲，中间为未知区域。

// 栅格更新伪代码（C++风格）
void updateGrid(GridMap& map, const Ray& ray) {
    for (float t = 0; t < ray.length; t += map.resolution) {
        Point p = ray.origin + ray.direction * t;
        Cell& cell = map.getCell(p);
        if (t < ray.length - map.resolution) {
            cell.logOdds += 0.4;  // 空闲更新
        } else {
            cell.logOdds += 0.85; // 占据更新
        }
        cell.probability = 1.0 / (1.0 + exp(-cell.logOdds));
    }
}

八叉树地图构建要点

深度选择：深度16层对应0.01m分辨率（若空间为100m³），深度14层对应0.05m。机载场景建议14~15层。
剪枝策略：当某节点的8个子节点状态一致时，合并为父节点并删除子节点，可大幅压缩内存。
传感器模型：使用OctoMap库时，需设置激光的命中点、射线终点和最大范围，内部自动处理概率更新。

点云地图处理流程

体素滤波：使用PCL的VoxelGrid滤波器，设置leaf_size=0.05m，将密集点云降采样至每立方米约8000点。
离群点移除：StatisticalOutlierRemoval，设置邻居数50，标准差阈值1.0，剔除孤立噪声点。
KD-Tree构建：对滤波后的点云构建KD-Tree，用于后续最近邻查询和碰撞检测。

工程经验：在PX4或ROS环境中，推荐使用octomap_server包将点云直接转换为八叉树地图。其内部已集成射线投射和概率更新，只需配置resolution和sensor_model参数。实测在Jetson Xavier NX上，处理16线激光雷达数据（10Hz）时，CPU占用率低于15%。

4.3 案例拆解：基于八叉树地图的室内避障系统

背景与约束：某巡检无人机需在20m×15m×4m的仓库内自主飞行，障碍物包括货架、立柱和悬空管道。机载计算机为NVIDIA Jetson Orin NX（8GB内存），激光雷达为Livox Mid-360（非重复扫描，点云密度不均）。要求避障响应时间小于100ms，地图更新频率不低于5Hz。

初始现象与目标：初期直接使用栅格地图（分辨率0.1m），内存占用达1.2GB，导致系统频繁卡顿；改用点云地图后，碰撞检测需遍历全部点云（约30万点），单次查询耗时80ms，无法满足实时性。目标是将内存控制在500MB以内，查询耗时低于20ms。

关键参数与操作步骤：

地图选型：采用八叉树地图，深度15层（分辨率约0.03m），预期内存约200MB。
点云预处理：先对Livox点云进行体素滤波（leaf_size=0.05m），将点数降至8万点，再输入octomap_server。
传感器模型配置：设置sensor_model.max_range=10m，sensor_model.hit=0.9，sensor_model.miss=0.3，避免远处噪声点影响。
查询优化：在路径规划节点中，使用八叉树自带的光线追踪接口castRay()检测路径是否与占据体素碰撞，替代逐点查询。

结果对比：

指标	栅格地图	点云+KD-Tree	八叉树地图
内存占用	1.2GB	480MB	210MB
单次碰撞查询	0.5ms	80ms	3ms
地图更新延迟	120ms	200ms	45ms
系统稳定性	卡顿	偶发超时	流畅

风险与代价：八叉树地图在动态障碍物场景下需要额外处理“地图过期”问题，本案例通过设置occupancy_min_z=0.1m和occupancy_max_z=3.5m过滤地面和天花板反射噪声。复盘经验：对于非重复扫描雷达，必须做体素滤波，否则八叉树更新会因点云密度不均产生“空洞”伪影。

4.4 常见误区与总结复盘

误区1：盲目追求高分辨率。0.01m分辨率在100m³空间下，八叉树深度需18层，内存仍可能超过1GB。应根据无人机尺寸（如轴距250mm）选择0.05m~0.1m即可，过细分辨率反而增加计算噪声。

误区2：忽略坐标系对齐。若IMU与激光雷达外参标定误差超过2cm，地图会出现“重影”。务必使用tf2树检查各传感器坐标系变换是否连续。

误区3：点云地图直接用于规划。点云是稀疏的，无法直接判断两个点之间是否安全。必须转换为占据网格或使用FCL（Flexible Collision Library）进行有向距离场（SDF）计算。

总结复盘：环境建模的核心是“精度-内存-速度”三角权衡。栅格地图适合小范围、低维度场景；八叉树地图是3D避障的首选，兼顾内存与查询效率；点云地图则作为感知前端，需配合滤波和索引结构使用。在实际工程中，建议采用“点云输入→体素滤波→八叉树构建→路径规划”的流水线，并利用OctoMap的增量更新特性实现实时避障。最后，务必在仿真环境（如Gazebo）中验证地图构建参数，再移植到真机，避免因传感器噪声导致地图失真。