Fast-Planner项目中的SDF地图构建技术解析

概述

本文深入分析Fast-Planner项目中fast_planner/plan_env/src/sdf_map.cpp文件实现的有符号距离场(SDF)地图构建技术。SDF地图是无人机快速路径规划的核心组件，它不仅能表示障碍物位置，还能提供障碍物距离信息，为规划算法提供关键的环境感知数据。

SDF地图初始化

参数配置

SDF地图初始化时从ROS参数服务器读取多项配置参数：

node_.param("sdf_map/resolution", mp_.resolution_, -1.0);
node_.param("sdf_map/map_size_x", x_size, -1.0);
node_.param("sdf_map/map_size_y", y_size, -1.0);
node_.param("sdf_map/map_size_z", z_size, -1.0);

主要参数包括：

• 地图分辨率(resolution)
• 地图尺寸(map_size_x/y/z)
• 局部更新范围(local_update_range)
• 障碍物膨胀系数(obstacles_inflation)
• 相机内参(fx/fy/cx/cy)
• 深度滤波参数(depth_filter_*)

地图数据结构

地图使用三维体素网格表示，核心数据结构包括：

// 占据概率对数表示
md_.occupancy_buffer_ = vector<double>(buffer_size, mp_.clamp_min_log_ - mp_.unknown_flag_);

// 膨胀占据图
md_.occupancy_buffer_inflate_ = vector<char>(buffer_size, 0);

// 距离场数据
md_.distance_buffer_ = vector<double>(buffer_size, 10000);
md_.distance_buffer_neg_ = vector<double>(buffer_size, 10000);
md_.distance_buffer_all_ = vector<double>(buffer_size, 10000);

传感器数据处理

深度图像处理

系统支持两种数据输入方式：

1. 深度图像+位姿同步数据
2. 点云+里程计独立数据

深度图像处理核心流程：

void SDFMap::projectDepthImage() {
  // 将深度图像像素投影到世界坐标系
  proj_pt(0) = (u - mp_.cx_) * depth / mp_.fx_;
  proj_pt(1) = (v - mp_.cy_) * depth / mp_.fy_;
  proj_pt(2) = depth;
  proj_pt = camera_r * proj_pt + md_.camera_pos_;
  
  // 应用深度滤波
  if (depth < mp_.depth_filter_mindist_) continue;
  else if (depth > mp_.depth_filter_maxdist_) depth = mp_.max_ray_length_ + 0.1;
}

光线投射

将投影点转换为体素网格中的射线，更新沿线的占据概率：

void SDFMap::raycastProcess() {
  // 对每个投影点进行光线追踪
  for (int i = 0; i < md_.proj_points_cnt; ++i) {
    // 计算射线方向
    // 更新射线经过的体素
  }
}

有符号距离场计算

3D距离变换

采用高效的ESDF(欧几里得符号距离场)算法：

void SDFMap::updateESDF3d() {
  // 正向距离场计算
  fillESDF([&](int z) { /* 获取值 */ }, 
           [&](int z, double val) { /* 设置值 */ },
           min_esdf[2], max_esdf[2], 2);
  
  // 负向距离场计算
  fillESDF([&](int z) { /* 获取负向值 */ },
           [&](int z, double val) { /* 设置负向值 */ },
           min_esdf[2], max_esdf[2], 2);
  
  // 合并正负距离场
  md_.distance_buffer_all_[idx] = md_.distance_buffer_[idx];
  if (md_.distance_buffer_neg_[idx] > 0.0)
    md_.distance_buffer_all_[idx] += (-md_.distance_buffer_neg_[idx] + mp_.resolution_);
}

二维填充算法

使用优化的二维扫描算法计算距离场：

template <typename F_get_val, typename F_set_val>
void SDFMap::fillESDF(F_get_val f_get_val, F_set_val f_set_val, int start, int end, int dim) {
  // 实现高效的一维ESDF计算
  // 通过三次扫描(沿x,y,z轴)完成三维ESDF计算
}

地图更新机制

定时更新

系统通过ROS定时器定期更新占据图和ESDF：

occ_timer_ = node_.createTimer(ros::Duration(0.05), &SDFMap::updateOccupancyCallback, this);
esdf_timer_ = node_.createTimer(ros::Duration(0.05), &SDFMap::updateESDFCallback, this);
vis_timer_ = node_.createTimer(ros::Duration(0.05), &SDFMap::visCallback, this);

局部更新

为提升效率，系统支持局部区域更新：

void SDFMap::resetBuffer(Eigen::Vector3d min_pos, Eigen::Vector3d max_pos) {
  // 只重置指定区域内的缓冲区
  for (int x = min_id(0); x <= max_id(0); ++x)
    for (int y = min_id(1); y <= max_id(1); ++y)
      for (int z = min_id(2); z <= max_id(2); ++z) {
        // 重置局部区域数据
      }
}

可视化

系统提供多种可视化主题：

map_pub_ = node_.advertise<sensor_msgs::PointCloud2>("/sdf_map/occupancy", 10);
map_inf_pub_ = node_.advertise<sensor_msgs::PointCloud2>("/sdf_map/occupancy_inflate", 10);
esdf_pub_ = node_.advertise<sensor_msgs::PointCloud2>("/sdf_map/esdf", 10);

技术亮点

1. 高效ESDF计算：使用优化的三维距离变换算法，大幅提升计算效率
2. 局部更新机制：只更新变化区域，减少计算量
3. 深度滤波：多种滤波策略提高深度数据质量
4. 多传感器支持：灵活支持深度相机和激光雷达输入
5. 实时性能：通过定时器和回调机制保证实时性

总结

Fast-Planner中的SDF地图实现展示了如何高效构建和维护三维有符号距离场，为无人机在复杂环境中的实时运动规划提供了关键的环境表示能力。其设计充分考虑了计算效率和内存使用的平衡，是机器人环境感知的优秀实践。