hc

最新推荐文章于 2024-01-26 15:45:06 发布

Octavia_Kong

最新推荐文章于 2024-01-26 15:45:06 发布

阅读量683

点赞数

本文链接：https://blog.csdn.net/m0_37923410/article/details/98628306

版权

以前整理的

teb_local_planner
teb_local_planner如果enable_homotopy_class_planning，则plan时调用HomotopyClassPlanner（Parallel planning in distinctive topologies enabled），否则调用TebOptimalPlanner。HomotopyClassPlanner用的是拓扑学的理论（不太懂）优化，TebOptimalPlanner用的是g2o优化。作者在论文中说hc可以更好的处理动态障碍，论文中hc+teb性能表现对比也比纯teb强。
主要思想
通过graph_search确定homotopy classes, 然后作为初始化来用teb优化轨迹，然后根据cost选择最好的轨迹。
规划步骤

bool HomotopyClassPlanner::plan(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::Twist* start_vel, bool free_goal_vel)
{
  ROS_ASSERT_MSG(initialized_, "Call initialize() first.");

  // Update old TEBs with new start, goal and velocity
  updateAllTEBs(&start, &goal, start_vel);

  // Init new TEBs based on newly explored homotopy classes
  exploreEquivalenceClassesAndInitTebs(start, goal, cfg_->obstacles.min_obstacle_dist, start_vel);
  // update via-points if activated
  updateReferenceTrajectoryViaPoints(cfg_->hcp.viapoints_all_candidates);
  // Optimize all trajectories in alternative homotopy classes
  optimizeAllTEBs(cfg_->optim.no_inner_iterations, cfg_->optim.no_outer_iterations);
  // Select which candidate (based on alternative homotopy classes) should be used
  selectBestTeb();

  initial_plan_ = nullptr; // clear pointer to any previous initial plan (any previous plan is useless regarding the h-signature);
  return true;
}

（另外这个没看懂，All steps are repeated in the subsequent sampling interval with the exception, that already planned (optimized) trajectories are preferred against new path initilizations in order to improve the hot-starting capability.所有步骤都会在随后的采样间隔中重复，除了已经计划好的(优化的)轨迹与新的路径初始化相反，以提高热启动能力。）

相关参数

simple_exploration
explore hcs时用的graph search,如果simple_exploration为true，则用lrKeyPointGraph，否则用ProbRoadmapGraph。作用：Create a graph and sample points in the global frame that can be used to explore new possible paths between start and goal.
lrKeyPointGraph：places a keypoint on the left and right side of each obstacle w.r.t to the goal heading.
ProbRoadmapGraph：samples keypoints in a predefined area (config) in the current frame between start and goal.
然后都是用的深度优先搜索寻找可行路径，作者觉得lrKeyPointGraph works very well for small point obstacles，而ProbRoadmapGraph适合complex, non-point or huge obstacles。
max_number_classes：
其中在exploreEquivalenceClassesAndInitTebs中判断tebs_.size() >= cfg_->hcp.max_number_classes时，则不再addAndInitNewTeb。
selection_cost_hysteresis
表示一个滞后作用，如果上一个best_teb仍是一个重要的candidate

// check if last best_teb is still a valid candidate
    if (best_teb_ && std::find(tebs_.begin(), tebs_.end(), best_teb_) != tebs_.end())
    {
        // get cost of this candidate
        min_cost_last_best = best_teb_->getCurrentCost() * cfg_->hcp.selection_cost_hysteresis; // small hysteresis
        last_best_teb_ = best_teb_;
    }
    else
    {
      last_best_teb_.reset();
    }

selection_obst_cost_scale
selection_viapoint_cost_scale
计算cost时的缩放因子，为1即可

void TebOptimalPlanner::computeCurrentCost(double obst_cost_scale, double viapoint_cost_scale, bool alternative_time_cost)
{ 
  // check if graph is empty/exist  -> important if function is called between buildGraph and optimizeGraph/clearGraph
  bool graph_exist_flag(false);
  if (optimizer_->edges().empty() && optimizer_->vertices().empty())
  {
    // here the graph is build again, for time efficiency make sure to call this function 
    // between buildGraph and Optimize (deleted), but it depends on the application
    buildGraph();	
    optimizer_->initializeOptimization();
  }
  else
  {
    graph_exist_flag = true;
  }
  
  optimizer_->computeInitialGuess();
  
  cost_ = 0;

  if (alternative_time_cost)
  {
    cost_ += teb_.getSumOfAllTimeDiffs();
    // TEST we use SumOfAllTimeDiffs() here, because edge cost depends on number of samples, which is not always the same for similar TEBs,
    // since we are using an AutoResize Function with hysteresis.
  }
  
  // now we need pointers to all edges -> calculate error for each edge-type
  // since we aren't storing edge pointers, we need to check every edge
  for (std::vector<g2o::OptimizableGraph::Edge*>::const_iterator it = optimizer_->activeEdges().begin(); it!= optimizer_->activeEdges().end(); it++)
  {
    double cur_cost = (*it)->chi2();

    if (dynamic_cast<EdgeObstacle*>(*it) != nullptr
        || dynamic_cast<EdgeInflatedObstacle*>(*it) != nullptr
        || dynamic_cast<EdgeDynamicObstacle*>(*it) != nullptr)
    {
      cur_cost *= obst_cost_scale;
    }
    else if (dynamic_cast<EdgeViaPoint*>(*it) != nullptr)
    {
      cur_cost *= viapoint_cost_scale;
    }
    else if (dynamic_cast<EdgeTimeOptimal*>(*it) != nullptr && alternative_time_cost)
    {
      continue; // skip these edges if alternative_time_cost is active
    }
    cost_ += cur_cost;
  }

  // delete temporary created graph
  if (!graph_exist_flag) 
    clearGraph();
}

selection_alternative_time_cost
同样是在计算cost中调用，好像是设为true，才是时间最优

  if (alternative_time_cost)
  {
    cost_ += teb_.getSumOfAllTimeDiffs();
    // TEST we use SumOfAllTimeDiffs() here, because edge cost depends on number of samples, which is not always the same for similar TEBs,
    // since we are using an AutoResize Function with hysteresis.
  }

roadmap_graph_no_samples
graph search时的采样个数，仅在simple_exploration为false（即用ProbRoadmapGraph）起效，
roadmap_graph_area_width
在起始点、目标点和指定的宽度之间采样，同上仅在simple_exploration为false起效
obstacle_heading_threshold
障碍航向和目标航向之间归一化标量乘积的数值 [0，1]

   Eigen::Vector2d start_orient_vec( cos(start.theta()), sin(start.theta()) ); // already normalized
          // check angle
          if (start_orient_vec.dot(keypoint_dist) <= obstacle_heading_threshold)
          {
            ROS_DEBUG("createGraph() - deleted edge: limit_obstacle_heading");
            continue;
          }