路径规划与避障算法(五)---DWA算法流程之一

最新推荐文章于 2025-06-20 01:54:51 发布

原创

最新推荐文章于 2025-06-20 01:54:51 发布 · 1w 阅读

68 ·

CC 4.0 BY-SA版权

本文介绍了DWA算法在路径规划与避障中的应用，详细讲解了算法流程，包括读入地图、截取本地地图、更新costmap参数和寻找最优轨迹等步骤，特别是如何生成速度和轨迹采样空间。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

版权声明：本文为博主原创文章，转载请联系博主。https://mp.youkuaiyun.com/mdeditor/84305617
DWA算法流程图如下:
在这里插入图片描述

1.读入地图

  base_planner_.setPlan(orig_global_traj);

std::vector<geometry_msgs::PoseStamped> global_plan_;//头文件里定义 global_plan_变量

bool setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan) {
   
   

  global_plan_.clear();//reset the global plan

  global_plan_ = orig_global_plan;

  return true;
}

2.截取本地地图（local_plan）

/****
将参考路径根据costmap大小进行截取,获得适配costmap大小的参考路径地图
****/
base_planner_.getLocalPlan(pose1, transformed_plan);

3.根据获得的地图更新costmap参数

dwa_planner_->updatePlanAndLocalCosts(pose1, transformed_plan);

4.寻找最优轨迹

/**
* @brief  find best trajectory in the generated trajectories
* @pose1  current position of vehicle, including x,y,orientation(yaw/heading)
* @pose2  curr