FAR Planner代码解析之动态障碍物更新

原创已于 2025-07-31 11:52:11 修改 · 1.1k 阅读

21 ·

CC 4.0 BY-SA版权

文章标签：

#机器人 #自动驾驶 #算法

于 2025-07-30 16:41:12 首次发布

机器人&自动驾驶专栏收录该内容

4 篇文章

订阅专栏

将之前整理的一些笔记放出来一些，顺便做个备份～

1. 基本map信息

全局网格地图：
world_obs_cloud_grid_
world_free_cloud_grid_
最大尺寸1000* 1000* 100m
分辨率5* 5* 2m
原点：以机器人初始位置坐标作为网格中心，从而对应的网格左下角世界坐标初始时才更新

处理网格图列表：对应网格图的所有网格

global_visited_induces_
util_remove_check_list_
util_obs_modified_list_
util_free_modified_list_

局部尺寸：
与terrain_height_grid_尺寸对应
水平尺寸范围：
neibor_Lum = senor_range*2/cell_length +1
注意：该值网格化后的数值尺寸要转为奇数

高度尺寸范围：
neighbor_Hum = 5
这两个值是更新机器人当前位置的时候
更新neibor网格索引（neighbor_free_indices_ neighbor_obs_indices_）的时候用的

2. TerrainCallBack（）

动态障碍物更新

PC—PrcocessCloud---->temp_cloud_ptr_—CropBoxCloud---->temp_cloud_ptr_----ExtractFreeAndObsCloud---->temp_free_ptr_&temp_obs_ptr_

到目前为止，temp_cloud_ptr_ temp_free_ptr_ temp_obs_ptr_都是当前帧的点云数据
其中temp_cloud_ptr_ 是经过预处理和滤波后的点云数据，包含打到障碍物和地面的
temp_free_ptr_ ，temp_obs_ptr_分别对应打地面的和非地面的点云数据

经过FARUtil::RemoveOverlapCloud(temp_obs_ptr_, FARUtil::stack_dyobs_cloud_, true)，这时候
temp_obs_ptr_中去除了与stack_dyobs_cloud_重叠的部分
将此时的temp_obs_ptr_ temp_obs_ptr_更新到全局地图中

UpdateObsCloudGrid(temp_obs_ptr_)
UpdateFreeCloudGrid(temp_free_ptr_)

将原temp_obs_ptr_中满足网格条件的更新到world_obs_cloud_grid_
并按照实际更新的点云数据重新记录temp_obs_ptr_
至此，world_obs_cloud_grid_中已经包含了当前帧的障碍物数据，注意这里的障碍物数据中是去除了动态障碍物（一定时间内的）的
注意 obs与free在更新的时候是不一样的 temp_obs_ptr_更新的时候需要检验局部和全局地图范围
free只需要满足全局即可
这里的temp_obs_ptr_ temp_free_ptr_都是实际用到的点云数据
不一样的是更新范围不同

一会重点关注一下，这个动态障碍物是如何更新的

此时temp_obs_ptr_中是去除了动态障碍物的
注意这里FARUtil::surround_obs_cloud_中是当前帧更新前的局部障碍物点云数据
提取只有temp_obs_ptr_中有但是surround_obs_cloud_中没有的点云数据放入FARUtil::cur_new_cloud_
所以这里，cur_new_cloud_中目前只有当前帧的静态障碍物信息
FARUtil::ExtractNewObsPointCloud(temp_obs_ptr_,
FARUtil::surround_obs_cloud_,
FARUtil::cur_new_cloud_);

到这里FARUtil::surround_free_cloud_ FARUtil::surround_obs_cloud_中是包含了当前帧的的点云数据的
将free部分用于更新地形

// extract surround free cloud & update terrain height
map_handler_.GetSurroundFreeCloud(FARUtil::surround_free_cloud_);
map_handler_.UpdateTerrainHeightGrid(FARUtil::surround_free_cloud_, terrain_height_ptr_);
// update surround obs cloud
map_handler_.GetSurroundObsCloud(FARUtil::surround_obs_cloud_);

FARUtil::cur_scan_cloud_中存的是预处理的当前帧点云数据（是在ScanCallBack中更新的注意这里的点云更新频次更高更接近当前的情况）
FARUtil::surround_obs_cloud_ FARUtil::surround_free_cloud_是包含了当前帧点云数据的局部点云
将FARUtil::cur_scan_cloud_中非FARUtil::surround_free_cloud_的部分作为静态障碍物点云
这里有个问题：这次不是地面上次是地面的点云在这里是不是也被扣除去了？？？
只要曾经是地面的地方都被扣除了？
并对被cur_scan_cloud_点云击穿部分点云数据且存在于FARUtil::surround_obs_cloud_中的数据进行提取存入FARUtil::cur_dyobs_cloud_（待消除的点云）
这样的话，就是从来没有被扫到过地面？并且被当前帧点云击穿的部分的点云才是可以被消除的~~~
这样处理避免误将静态障碍物消掉

FARUtil::cur_dyobs_cloud_->clear();
this->ExtractDynamicObsFromScan(FARUtil::cur_scan_cloud_,
FARUtil::surround_obs_cloud_,
FARUtil::surround_free_cloud_,
FARUtil::cur_dyobs_cloud_);

s6：

待消除点云达到一定数量：
对点云数据进行一定的膨胀后进行全局去重和局部去重
至此，world_obs_cloud_grid_与FARUtil::surround_obs_cloud_中都去除了动态障碍物的点云

FARUtil::InflateCloud(FARUtil::cur_dyobs_cloud_, master_params_.voxel_dim, 1, true);
map_handler_.RemoveObsCloudFromGrid(FARUtil::cur_dyobs_cloud_);
FARUtil::RemoveOverlapCloud(FARUtil::surround_obs_cloud_, FARUtil::cur_dyobs_cloud_);

s7：

将当前帧动态障碍物点云数据添加到FARUtil::cur_new_cloud_ 中，至此，FARUtil::cur_new_cloud_中既包含了当前帧新增和待删除的点云数据？？？
FARUtil::FilterCloud(FARUtil::cur_dyobs_cloud_, master_params_.voxel_dim);
// update new cloud
*FARUtil::cur_new_cloud_ += *FARUtil::cur_dyobs_cloud_;
FARUtil::FilterCloud(FARUtil::cur_new_cloud_, master_params_.voxel_dim);

将当前帧击穿的点云按照时间更新到FARUtil::stack_dyobs_cloud_中
也就是这里存的时候一定时间内的消失的障碍物（可理解为会动的）
用于与下一帧对比
比如，存在一个会动的障碍物 t0时刻没有该障碍物 t1时刻有 t2时刻消失一半 t3时刻消失

t0时刻环境不变 stack_dyobs_cloud_是空的
t1时刻新增障碍物信息当前时刻障碍物全部被更新到环境中此时stack_dyobs_cloud_还是空
t2时刻障碍物被击穿一部分 stack_dyobs_cloud_中有该障碍物一部分的信息
t3时刻此时如果再次打到该障碍物上次被击穿的部分会被剔除（重叠部分）障碍物被击穿全部 stack_dyobs_cloud_中有整个障碍物信息
也就是上次（在一定时间内积累的）被击穿的这次又扫到了也不更新
只要被击穿一次就代表会动那么在一定时间内积累的被击穿部分的数据都不会更新避免重影问题
但是被遮挡部分无法消除（无法击穿）

// update world dynamic obstacles
FARUtil::StackCloudByTime(FARUtil::cur_dyobs_cloud_, FARUtil::stack_dyobs_cloud_, FARUtil::kObsDecayTime);

// create and update kdtrees
// 将kNewDecayTime时间间隔内的当前帧新增和被击穿的障碍物点云提取出来到stack_new_cloud_ FARUtil::StackCloudByTime(FARUtil::cur_new_cloud_, FARUtil::stack_new_cloud_, FARUtil::kNewDecayTime);
FARUtil::UpdateKdTrees(FARUtil::stack_new_cloud_);

//处理动态障碍物 
void FARMaster::TerrainCallBack(const sensor_msgs::PointCloud2ConstPtr& pc) {
  if (!is_odom_init_) return;
  // update map grid robot center 全局的地图原点只更新一次
  // 局部实时更新
  map_handler_.UpdateRobotPosition(FARUtil::robot_pos);
  if (!is_stop_update_) {
    this->PrcocessCloud(pc, temp_cloud_ptr_);
    FARUtil::CropBoxCloud(temp_cloud_ptr_, robot_pos_, Point3D(master_params_.terrain_range,
                                                               master_params_.terrain_range,
                                                               FARUtil::kTolerZ));
    //当前帧点云分成temp_obs_ptr_ temp_free_ptr_
    FARUtil::ExtractFreeAndObsCloud(temp_cloud_ptr_, temp_free_ptr_, temp_obs_ptr_);
    
    //在当前帧点云数据中去除与之前stack_dyobs_cloud_中相同的点云，得到当前帧新增的点云temp_obs_ptr_
    //stack_dyobs_cloud_初始的时候是空的
    if (!master_params_.is_static_env) {
      FARUtil::RemoveOverlapCloud(temp_obs_ptr_, FARUtil::stack_dyobs_cloud_, true);
    }
    
    //将新增点云更新到全局障碍点云中 保留实际被更新进去的点云（在nobor范围内的）
    //对更新的点云体素单元进行滤波
    map_handler_.UpdateObsCloudGrid(temp_obs_ptr_);
    map_handler_.UpdateFreeCloudGrid(temp_free_ptr_);
    
    
    // extract new points
    //参考surround_obs_cloud_提取仅temp_obs_ptr_中有的点云放入cur_new_cloud_
    //就是当前帧新增的点云
    FARUtil::ExtractNewObsPointCloud(temp_obs_ptr_,
                                     FARUtil::surround_obs_cloud_,
                                     FARUtil::cur_new_cloud_);
  } else { // stop env update
    temp_cloud_ptr_->clear();
    FARUtil::cur_new_cloud_->clear();
  }
  
  // extract surround free cloud & update terrain height
  map_handler_.GetSurroundFreeCloud(FARUtil::surround_free_cloud_);
  map_handler_.UpdateTerrainHeightGrid(FARUtil::surround_free_cloud_, terrain_height_ptr_);
  
  // update surround obs cloud
  map_handler_.GetSurroundObsCloud(FARUtil::surround_obs_cloud_);
  // extract dynamic obstacles
  FARUtil::cur_dyobs_cloud_->clear();
  if (!master_params_.is_static_env && !is_stop_update_) {
    //基于当前帧点云数据 参考全局neighbor障碍物和非障碍物信息提取当前动态障碍物信息：
    //先把当前帧障碍物点云中的free部分去掉
    //surround_obs_cloud_中现在是free的，也就是被击穿的存入cur_dyobs_cloud_
    this->ExtractDynamicObsFromScan(FARUtil::cur_scan_cloud_, 
                                    FARUtil::surround_obs_cloud_, 
                                    FARUtil::surround_free_cloud_, 
                                    FARUtil::cur_dyobs_cloud_);
    //点数达到一定阈值
    if (FARUtil::cur_dyobs_cloud_->size() > FARUtil::kDyObsThred) {
      if (FARUtil::IsDebug) ROS_WARN("FARMaster: dynamic obstacle detected, size: %ld", FARUtil::cur_dyobs_cloud_->size());
      FARUtil::InflateCloud(FARUtil::cur_dyobs_cloud_, master_params_.voxel_dim, 1, true);
      //world_obs_cloud_grid_中涉及到的网格数据都进行去重叠
      //全局去重
      map_handler_.RemoveObsCloudFromGrid(FARUtil::cur_dyobs_cloud_);
      //局部去重 surround_obs_cloud_是投影用的点云 这里去除了动态点云 是为啥呢
      FARUtil::RemoveOverlapCloud(FARUtil::surround_obs_cloud_, FARUtil::cur_dyobs_cloud_);
      FARUtil::FilterCloud(FARUtil::cur_dyobs_cloud_, master_params_.voxel_dim);
      
      // update new cloud 这里存的是啥呢
      *FARUtil::cur_new_cloud_ += *FARUtil::cur_dyobs_cloud_;
      FARUtil::FilterCloud(FARUtil::cur_new_cloud_, master_params_.voxel_dim);
    }
    // update world dynamic obstacles
    FARUtil::StackCloudByTime(FARUtil::cur_dyobs_cloud_, FARUtil::stack_dyobs_cloud_, FARUtil::kObsDecayTime);
  }
  
  // create and update kdtrees
  // 将cur_new_cloud_中在kNewDecayTime时间间隔内的新增点云提取出来到stack_new_cloud_
  FARUtil::StackCloudByTime(FARUtil::cur_new_cloud_, FARUtil::stack_new_cloud_, FARUtil::kNewDecayTime);
  //所以stack_new_cloud_中是一定时间内的动态障碍物
  FARUtil::UpdateKdTrees(FARUtil::stack_new_cloud_);

  if (!FARUtil::surround_obs_cloud_->empty()) is_cloud_init_ = true;

  /* visualize clouds */
  planner_viz_.VizPointCloud(new_PCL_pub_, FARUtil::stack_new_cloud_);
  planner_viz_.VizPointCloud(dynamic_obs_pub_, FARUtil::cur_dyobs_cloud_);
  planner_viz_.VizPointCloud(surround_free_debug_, FARUtil::surround_free_cloud_);
  planner_viz_.VizPointCloud(surround_obs_debug_,  FARUtil::surround_obs_cloud_);
  planner_viz_.VizPointCloud(terrain_height_pub_, terrain_height_ptr_);
  // visualize map grid
  PointStack neighbor_centers, occupancy_centers;
  map_handler_.GetNeighborCeilsCenters(neighbor_centers);
  map_handler_.GetOccupancyCeilsCenters(occupancy_centers);
  planner_viz_.VizMapGrids(neighbor_centers, occupancy_centers, map_params_.cell_length, map_params_.cell_height);
  // DBBUG visual raycast grids
  if (!master_params_.is_static_env) {
    scan_handler_.GridVisualCloud(scan_grid_ptr_, GridStatus::RAY);
    planner_viz_.VizPointCloud(scan_grid_debug_, scan_grid_ptr_);
  }
}