ros DWA局部规划模块

2024-06-01 13:36
文章标签 规划 模块 ros 局部 dwa

本文主要是介绍ros DWA局部规划模块，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

ROS-DWA模块

    • 主要流程
    • DWAPlannerROS::computeVelocityCommands
      • DWAPlannerROS::dwaComputeVelocityCommands
        • DWAPlanner::findBestPath
          • SimpleScoredSamplingPlanner::findBestTrajectory
    • 调参技巧
      • DWA被目标点过度吸引，且不听全局规划器指挥
    • 消融实验
      • goal_front_costs_
      • alignment_costs_
      • path_costs_
      • goal_costs_
      • 评价

在之前的学习中我们了解到，设定机器人的导航目标位置后，会在 MoveBase::executeCb函数中执行全局路径规划，并通过while循环反复执行 executeCycle函数控制机器人跟踪全局路径，这里控制机器人跟踪就是我们常说的局部规划，常用的局部规划方法有dwa、teb、pid、mpc、pure pursuit等，这一节对dwa算法进行研究。

主要流程

  bool DWAPlannerROS::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan) {if (! isInitialized()) {ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");return false;}//when we get a new plan, we also want to clear any latch we may have on goal toleranceslatchedStopRotateController_.resetLatching();ROS_INFO("Got new plan");return dp_->setPlan(orig_global_plan);}

这里调用的是dp_的setPlan()，dp_是指向DWAPlanner的指针。

  bool DWAPlanner::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan) {oscillation_costs_.resetOscillationFlags();return planner_util_->setPlan(orig_global_plan);    // 将orig_global_plan设置给LocalPlannerUtil的global_plan_}

接着就是调用tc_->computeVelocityCommands(cmd_vel)进行局部规划。

DWAPlannerROS::computeVelocityCommands

bool DWAPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) {// dispatches to either dwa sampling control or stop and rotate control, depending on whether we have been close enough to goal// 获取当前位姿if ( ! costmap_ros_->getRobotPose(current_pose_)) {ROS_ERROR("Could not get robot pose");return false;}// 调用planner_util_的getLocalPlan方法，以当前位姿为起点，// 获取局部路径，并将其存储在transformed_plan中。如果失败，则返回false。std::vector<geometry_msgs::PoseStamped> transformed_plan;if ( ! planner_util_.getLocalPlan(current_pose_, transformed_plan)) {ROS_ERROR("Could not get local plan");return false;}//if the global plan passed in is empty... we won't do anythingif(transformed_plan.empty()) {ROS_WARN_NAMED("dwa_local_planner", "Received an empty transformed plan.");return false;}ROS_DEBUG_NAMED("dwa_local_planner", "Received a transformed plan with %zu points.", transformed_plan.size());// update plan in dwa_planner even if we just stop and rotate, to allow checkTrajectorydp_->updatePlanAndLocalCosts(current_pose_, transformed_plan, costmap_ros_->getRobotFootprint());// 使用latchedStopRotateController_来检查是否到达了目标位置。如果到达了，则执行停止和旋转的逻辑。if (latchedStopRotateController_.isPositionReached(&planner_util_, current_pose_)) {//publish an empty plan because we've reached our goal positionstd::vector<geometry_msgs::PoseStamped> local_plan;std::vector<geometry_msgs::PoseStamped> transformed_plan;publishGlobalPlan(transformed_plan);publishLocalPlan(local_plan);base_local_planner::LocalPlannerLimits limits = planner_util_.getCurrentLimits();return latchedStopRotateController_.computeVelocityCommandsStopRotate(cmd_vel,limits.getAccLimits(),dp_->getSimPeriod(),&planner_util_,odom_helper_,current_pose_,boost::bind(&DWAPlanner::checkTrajectory, dp_, _1, _2, _3));} else {// 计算DWA规划器的速度命令bool isOk = dwaComputeVelocityCommands(current_pose_, cmd_vel);if (isOk) {publishGlobalPlan(transformed_plan);} else {ROS_WARN_NAMED("dwa_local_planner", "DWA planner failed to produce path.");std::vector<geometry_msgs::PoseStamped> empty_plan;publishGlobalPlan(empty_plan);}return isOk;}}

主要的几个函数是：
1、
通过DWAPlannerROS::dwaComputeVelocityCommands计算施加在机器人上的控制速度，在该函数内部调用了dp_->findBestPath, 机器人控制命令通过drive_cmds拿到。

DWAPlannerROS::dwaComputeVelocityCommands

  bool DWAPlannerROS::dwaComputeVelocityCommands(geometry_msgs::PoseStamped &global_pose, geometry_msgs::Twist& cmd_vel) {// dynamic window sampling approach to get useful velocity commandsif(! isInitialized()){ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");return false;}// 获取odom速度 geometry_msgs::PoseStamped robot_vel;odom_helper_.getRobotVel(robot_vel);/* For timing uncommentstruct timeval start, end;double start_t, end_t, t_diff;gettimeofday(&start, NULL);*///compute what trajectory to drive alonggeometry_msgs::PoseStamped drive_cmds;drive_cmds.header.frame_id = costmap_ros_->getBaseFrameID();// call with updated footprintbase_local_planner::Trajectory path = dp_->findBestPath(global_pose, robot_vel, drive_cmds);//ROS_ERROR("Best: %.2f, %.2f, %.2f, %.2f", path.xv_, path.yv_, path.thetav_, path.cost_);//pass along drive commandscmd_vel.linear.x = drive_cmds.pose.position.x;cmd_vel.linear.y = drive_cmds.pose.position.y;cmd_vel.angular.z = tf2::getYaw(drive_cmds.pose.orientation);//if we cannot move... tell someonestd::vector<geometry_msgs::PoseStamped> local_plan;if(path.cost_ < 0) {ROS_DEBUG_NAMED("dwa_local_planner","The dwa local planner failed to find a valid plan, cost functions discarded all candidates. This can mean there is an obstacle too close to the robot.");local_plan.clear();publishLocalPlan(local_plan);return false;}ROS_DEBUG_NAMED("dwa_local_planner", "A valid velocity command of (%.2f, %.2f, %.2f) was found for this cycle.", cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z);return true;}
DWAPlanner::findBestPath
  base_local_planner::Trajectory DWAPlanner::findBestPath(const geometry_msgs::PoseStamped& global_pose,const geometry_msgs::PoseStamped& global_vel,geometry_msgs::PoseStamped& drive_velocities) {//make sure that our configuration doesn't change mid-runboost::mutex::scoped_lock l(configuration_mutex_);Eigen::Vector3f pos(global_pose.pose.position.x, global_pose.pose.position.y, tf2::getYaw(global_pose.pose.orientation));Eigen::Vector3f vel(global_vel.pose.position.x, global_vel.pose.position.y, tf2::getYaw(global_vel.pose.orientation));geometry_msgs::PoseStamped goal_pose = global_plan_.back();Eigen::Vector3f goal(goal_pose.pose.position.x, goal_pose.pose.position.y, tf2::getYaw(goal_pose.pose.orientation));base_local_planner::LocalPlannerLimits limits = planner_util_->getCurrentLimits();// prepare cost functions and generators for this rungenerator_.initialise(pos,vel,goal,&limits,vsamples_);result_traj_.cost_ = -7;// find best trajectory by sampling and scoring the samplesstd::vector<base_local_planner::Trajectory> all_explored;scored_sampling_planner_.findBestTrajectory(result_traj_, &all_explored);// debrief stateful scoring functionsoscillation_costs_.updateOscillationFlags(pos, &result_traj_, planner_util_->getCurrentLimits().min_vel_trans);//if we don't have a legal trajectory, we'll just command zeroif (result_traj_.cost_ < 0) {drive_velocities.pose.position.x = 0;drive_velocities.pose.position.y = 0;drive_velocities.pose.position.z = 0;drive_velocities.pose.orientation.w = 1;drive_velocities.pose.orientation.x = 0;drive_velocities.pose.orientation.y = 0;drive_velocities.pose.orientation.z = 0;} else {drive_velocities.pose.position.x = result_traj_.xv_;drive_velocities.pose.position.y = result_traj_.yv_;drive_velocities.pose.position.z = 0;tf2::Quaternion q;q.setRPY(0, 0, result_traj_.thetav_);tf2::convert(q, drive_velocities.pose.orientation);}return result_traj_;}

generator_是轨迹生成器base_local_planner::SimpleTrajectoryGenerator，在DWAPlanner的构造函数中，可以看到，generator_被放置到了generator_list，然后传入了base_local_planner::SimpleScoredSamplingPlanner的构造函数构造出了scored_sampling_planner_。

std::vector<base_local_planner::TrajectoryCostFunction*> critics;
critics.push_back(&oscillation_costs_); // discards oscillating motions (assisgns cost -1)
critics.push_back(&obstacle_costs_); // discards trajectories that move into obstacles
critics.push_back(&goal_front_costs_); // prefers trajectories that make the nose go towards (local) nose goal
critics.push_back(&alignment_costs_); // prefers trajectories that keep the robot nose on nose path
critics.push_back(&path_costs_); // prefers trajectories on global path
critics.push_back(&goal_costs_); // prefers trajectories that go towards (local) goal, based on wave propagation
critics.push_back(&twirling_costs_); // optionally prefer trajectories that don't spin// trajectory generators
std::vector<base_local_planner::TrajectorySampleGenerator*> generator_list;
generator_list.push_back(&generator_);scored_sampling_planner_ = base_local_planner::SimpleScoredSamplingPlanner(generator_list, critics);

因此，DWAPlanner::findBestPath中下面这部分代码，就是对scored_sampling_planner_中所使用的轨迹生成器进行初始化。

generator_.initialise(pos,vel,goal,&limits,vsamples_);

vsamples_表示采样的数量。
接着调用了SimpleScoredSamplingPlanner::findBestTrajectory函数，dwa的最优轨迹通过参数traj返回，而参数all_explored则保存了DWA的所有搜索轨迹，下面来重点看这个函数。。

SimpleScoredSamplingPlanner::findBestTrajectory
  bool SimpleScoredSamplingPlanner::findBestTrajectory(Trajectory& traj, std::vector<Trajectory>* all_explored) {Trajectory loop_traj;Trajectory best_traj;double loop_traj_cost, best_traj_cost = -1;bool gen_success;int count, count_valid;// 检查所有的TrajectoryCostFunction是否准备好for (std::vector<TrajectoryCostFunction*>::iterator loop_critic = critics_.begin(); loop_critic != critics_.end(); ++loop_critic) {TrajectoryCostFunction* loop_critic_p = *loop_critic;if (loop_critic_p->prepare() == false) {ROS_WARN("A scoring function failed to prepare");return false;}}for (std::vector<TrajectorySampleGenerator*>::iterator loop_gen = gen_list_.begin(); loop_gen != gen_list_.end(); ++loop_gen) {count = 0;count_valid = 0;TrajectorySampleGenerator* gen_ = *loop_gen;// 遍历轨迹生成器生成的全部轨迹  while (gen_->hasMoreTrajectories()) {gen_success = gen_->nextTrajectory(loop_traj);if (gen_success == false) {// TODO use this for debuggingcontinue;}// 对该轨迹进行打分  loop_traj_cost = scoreTrajectory(loop_traj, best_traj_cost);if (all_explored != NULL) {loop_traj.cost_ = loop_traj_cost;all_explored->push_back(loop_traj);}if (loop_traj_cost >= 0) {count_valid++;if (best_traj_cost < 0 || loop_traj_cost < best_traj_cost) {best_traj_cost = loop_traj_cost;   // 更新最佳得分best_traj = loop_traj;    // 更新最佳轨迹  }}count++;if (max_samples_ > 0 && count >= max_samples_) {break;}        }// 最佳轨迹的得分是合法的，说明找到了最佳轨迹 ，将最佳轨迹信息交给trajif (best_traj_cost >= 0) {traj.xv_ = best_traj.xv_;traj.yv_ = best_traj.yv_;traj.thetav_ = best_traj.thetav_;traj.cost_ = best_traj_cost;traj.resetPoints();double px, py, pth;for (unsigned int i = 0; i < best_traj.getPointsSize(); i++) {best_traj.getPoint(i, px, py, pth);traj.addPoint(px, py, pth);}}ROS_DEBUG("Evaluated %d trajectories, found %d valid", count, count_valid);if (best_traj_cost >= 0) {// do not try fallback generatorsbreak;}}return best_traj_cost >= 0;}

调参技巧

DWA被目标点过度吸引，且不听全局规划器指挥

如下图：
在这里插入图片描述
这是由于目标吸引的权重过大导致的，应该提高轨迹对齐的权重降低目标吸引的权重，如下，将goal_distance_bias的权重由24下降到5，机器人就能按照全局轨迹走而不是被目标吸引的卡住不动。
在这里插入图片描述

在这里插入图片描述

消融实验

goal_front_costs_

在这里插入图片描述
在这里插入图片描述
轨迹歪歪扭扭的，还喜欢拼命往墙上靠，感觉这个cost就是瞎搞的，还不如不要。。

alignment_costs_

在这里插入图片描述

在这里插入图片描述
轨迹平滑了很多，也不会往墙上靠，但是，变得很慢，而且到目的地后原地转圈。。

path_costs_

在这里插入图片描述
在这里插入图片描述

不按全局规划的路径走，原地转圈，怀疑原地转圈是属于恢复行为，但是速度为0，应该是由于该cost计算的是当前位姿和规划路径的偏移，而速度为0时偏移最小。

goal_costs_

在这里插入图片描述

在这里插入图片描述
按照对这个cost的理解，应该是始终的往离目的地最近的地方开，但是实际轨迹却歪歪扭扭，原地转圈，撞墙。

评价

ros dwa_local_planner包原生的这几个cost的实现各有各的问题，不是很好的实现，最好的解决办法就是直接抛弃重写。。。

这篇关于ros DWA局部规划模块的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！


http://www.chinasem.cn/article/1021157。

相关文章

Python AST 模块实战演示

Python AST 模块实战演示

《PythonAST模块实战演示》Python的ast模块提供了一种处理Python代码的强大工具,通过解析代码生成抽象语法树（AST）,可以进行代码分析、修改和生成,接下来通过本文给大家介绍Py... 目录 什么是抽象语法树（AST）️ ast 模块的核心用法1. 解析代码生成 AST2. 查看 AST
阅读更多...
Python sys模块的使用及说明

Python sys模块的使用及说明

《Pythonsys模块的使用及说明》Pythonsys模块是核心工具,用于解释器交互与运行时控制,涵盖命令行参数处理、路径修改、强制退出、I/O重定向、系统信息获取等功能,适用于脚本开发与调试,需... 目录python sys 模块详解常用功能与代码示例获取命令行参数修改模块搜索路径强制退出程序标准输入
阅读更多...
Python pickle模块的使用指南

Python pickle模块的使用指南

《Pythonpickle模块的使用指南》Pythonpickle模块用于对象序列化与反序列化,支持dump/load方法及自定义类,需注意安全风险,建议在受控环境中使用,适用于模型持久化、缓存及跨... 目录python pickle 模块详解基本序列化与反序列化直接序列化为字节流自定义对象的序列化安全注
阅读更多...
python pymodbus模块的具体使用

python pymodbus模块的具体使用

《pythonpymodbus模块的具体使用》pymodbus是一个Python实现的Modbus协议库,支持TCP和RTU通信模式,支持读写线圈、离散输入、保持寄存器等数据类型,具有一定的参考价值... 目录一、详解1、 基础概念2、核心功能3、安装与设置4、使用示例5、 高级特性6、注意事项二、代码示例
阅读更多...
Python中logging模块用法示例总结

Python中logging模块用法示例总结

《Python中logging模块用法示例总结》在Python中logging模块是一个强大的日志记录工具,它允许用户将程序运行期间产生的日志信息输出到控制台或者写入到文件中,：本文主要介绍Pyt... 目录前言一. 基本使用1. 五种日志等级2.  设置报告等级3. 自定义格式4. C语言风格的格式化方法
阅读更多...
Python 基于http.server模块实现简单http服务的代码举例

Python 基于http.server模块实现简单http服务的代码举例

《Python基于http.server模块实现简单http服务的代码举例》Pythonhttp.server模块通过继承BaseHTTPRequestHandler处理HTTP请求,使用Threa... 目录测试环境代码实现相关介绍模块简介类及相关函数简介参考链接测试环境win11专业版python
阅读更多...
Nginx添加内置模块过程

Nginx添加内置模块过程

《Nginx添加内置模块过程》文章指导如何检查并添加Nginx的with-http_gzip_static模块：确认该模块未默认安装后,需下载同版本源码重新编译,备份替换原有二进制文件,最后重启服务验... 目录1、查看Nginx已编辑的模块2、Nginx官网查看内置模块3、停止Nginx服务4、Nginx
阅读更多...
python urllib模块使用操作方法

python urllib模块使用操作方法

《pythonurllib模块使用操作方法》Python提供了多个库用于处理URL,常用的有urllib、requests和urlparse（Python3中为urllib.parse）,下面是这些... 目录URL 处理库urllib 模块requests 库urlparse 和 urljoin编码和解码
阅读更多...
浅谈MySQL的容量规划

浅谈MySQL的容量规划

《浅谈MySQL的容量规划》进行MySQL的容量规划是确保数据库能够在当前和未来的负载下顺利运行的重要步骤,容量规划包括评估当前资源使用情况、预测未来增长、调整配置和硬件资源等,感兴趣的可以了解一下... 目录一、评估当前资源使用情况1.1 磁盘空间使用1.2 内存使用1.3 CPU使用1.4 网络带宽二、
阅读更多...
创建springBoot模块没有目录结构的解决方案

创建springBoot模块没有目录结构的解决方案

《创建springBoot模块没有目录结构的解决方案》2023版IntelliJIDEA创建模块时可能出现目录结构识别错误,导致文件显示异常,解决方法为选择模块后点击确认,重新校准项目结构设置,确保源... 目录创建spChina编程ringBoot模块没有目录结构解决方案总结创建springBoot模块没有目录
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2