ROS源码阅读---局部路径规划之DWAPlannerROS分析

1 体系结构

(1)主要成员
base_local_planner::LocalPlannerUtil planner_util_; 用来存储运动控制参数以及costmap2d、tf等,会被传入dp_
costmap_2d::Costmap2DROS* costmap_ros_;
base_local_planner::OdometryHelperRos odom_helper_; 用来辅助获取odom信息,会被传入dp_
boost::shared_ptr dp_; 正常的dwa运动控制类
base_local_planner::LatchedStopRotateController latchedStopRotateController_; 到达目标点后的停止旋转运动控制类

(2)主要接口
void initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros)—初始化
bool setPlan(const std::vector& orig_global_plan)—设置全局路径
bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel)—计算控制速度
bool isGoalReached()—判断是否达到目标点

2 总体流程

在move base控制循环中会在规划出新的路径时,将新的全局路径利用setPlan传给DWAPlannerROS,然后调用DWAPlanner::setPlan将路径传递给DWAPlanner(同时复位latchedStopRotateController_中的锁存相关的标志位)。

在运动规划之前,move_base先利用DWAPlannerROS::isGoalReached(内部利用LatchedStopRotateController::isGoalReached函数实现)判断是否已经到达了目标点(即位置在xy_goal_tolerance以内,朝向在yaw_goal_tolerance以内,且速度小于trans_stopped_velocity,rot_stopped_velocity),如果是则控制结束,即发送0速,且复位move_base的相关控制标记,并返回action成功的结果。否则,利用DWAPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)计算局部规划速度。

在DWAPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)中,会先将全局路径映射到局部地图中,并更新对应的打分项(参考DWAPlanner::updatePlanAndLocalCosts函数,和具体的打分项更新)。然后,利用LatchedStopRotateController::isPostionReached函数判断是否已经到达目标位置,如果是,则利用LatchedStopRotateController::computeVelocityCommandsStopRotate函数计算对应的减速停止或者旋转至目标朝向的速度指令(取决于是否机器人已经停稳,进而决定实现减速停止或者旋转至目标朝向),否则利用DWAPlannerROS::dwaComputeVelocityCommands(tf::Stamped &global_pose, geometry_msgs::Twist& cmd_vel)计算DWA局部路径速度。

对于停止时的处理逻辑,即LatchedStopRotateController的所有逻辑,参考“停止”章节。

对于DWAPlannerROS::dwaComputeVelocityCommands(tf::Stamped &global_pose, geometry_msgs::Twist& cmd_vel)函数,直接利用DWAPlanner::findBestPath(
tf::Stamped global_pose,
tf::Stamped global_vel,
tf::Stamped& drive_velocities,
std::vector footprint_spec)获取最优局部路径对应的速度指令。

在DWAPlanner::findBestPath函数中,先利用SimpleTrajectoryGenerator::initialise(
const Eigen::Vector3f& pos,
const Eigen::Vector3f& vel,
const Eigen::Vector3f& goal,
base_local_planner::LocalPlannerLimits* limits,
const Eigen::Vector3f& vsamples,
bool discretize_by_time = false)初始化轨迹产生器,即产生速度空间。
然后,利用SimpleScoredSamplingPlanner::findBestTrajectory(Trajectory& traj, std::vector* all_explored = 0)查找最优的局部路径。在该函数中,先调用每个打分项的prepare函数进行准备,参考“打分”章节。然后,利用SimpleScoredSamplingPlanner里面存储的TrajectorySampleGenerator轨迹产生器列表(目前,只有SimpleTrajectoryGenerator一个产生器)分别进行轨迹产生和打分,并根据打分选出最优轨迹。对于每个轨迹的打分,调用SimpleScoredSamplingPlanner::scoreTrajectory执行,在该函数中,遍历各个打分项进行打分并进行求和,如果某个打分项打分小于0,则将该负值作为路径打分项进行立刻返回。在对每个打分项打分之前,先获取对应的scale参数,如果scale为0证明该打分项无效,则跳过。如果获取到正常的打分项打分后(利用打分项的scoreTrajectory函数),将打分项乘以对应的scale作为最终的打分项打分。

具体的速度采样和轨迹规划参考“运动规划”章节。

接着在DWAPlanner::findBestPath中,根据使能参数,选择发布轨迹相关的可视化数据。
然后,如果最优轨迹有效,则利用最优轨迹更新摆动打分项的标志位。
最后,在返回最优规划轨迹之前,判断最优轨迹代价,如果代价小于0,则代表最优轨迹无效,即没有有效轨迹,则将返回速度指令,设置为0。

最后,如果DWAPlannerROS::dwaComputeVelocityCommands会判断得到的最优轨迹代价值,如果小于0,则该函数会返回false,从而引起move_base进行进一步的处理,如重新规划、recovery等。

至此,完成了整个dwa local planner的流程。

3 运动规划

在每个控制周期内,DWAPlanner::findBestPath会查找最优局部轨迹。

在DWAPlanner::findBestPath中,首先调用SimpleTrajectoryGenerator::initialise进行速度采样,然后利用SimpleScoredSamplingPlanner::findBestTrajectory根据速度采样空间进行轨迹产生、打分、筛选,从而得到最优局部轨迹。

(1)采样
速度采样在SimpleTrajectoryGenerator::initialise中进行,总体思路是先获取速度空间边界,然后根据采样个数参数在空间内进行采样。

在获取空间边界时,根据use_dwa参数,采用两套策略。
如果use_dwa==false,首先用当前位置与目标位置的距离处理仿真时间sim_time(模拟仿真时间内匀减速到0,刚好到达目标点的情景)得到max_vel_x和max_vel_y边界,然后基于acc_lim * sim_time得到一种边界(上边界),还有设置的速度极限参数(max_vel_xxx,min_vel_xxx)作为一种边界,然后选取三种边界中空间较小的边界。这种策略,能够获得较大的采样空间(因为用了sim_time)。
如果use_dwa==true,则直接用acc_lim * sim_period得到边界,然后还有设置的速度极限参数作为边界,然后选取两种边界中空间较小的边界。

得到速度空间边界后,根据x,y,theta三个采样个数进行插补,进而组合出整个速度采样空间。

(2)获取最优轨迹
获取最优轨迹在SimpleScoredSamplingPlanner::findBestTrajectory中进行,在该函数中,首先调用各个打分项的prepare函数进行准备。然后遍历std::vector

4 路径

(1)存储全局路径
路径类型为std::vector。
在move base控制循环中会在规划出新的路径时,将新的全局路径利用setPlan传给DWAPlannerROS,然后调用DWAPlanner::setPlann将路径传递给DWAPlanner(同时复位latchedStopRotateController_中的锁存相关的标志位)。
在DWAPlanner::setPlann中,会先将OscillationCostFunction中的摆动标志位复位,然后利用planner_util_->setPlan将路径传入planner_util_,直接保存为global_plan_。
此时的路径时相对于全局地图的全局坐标系的(通常为”map”)。

(2)局部路径映射及存储
在computeVelocityCommands计算速度前,会先将全局路径映射到局部地图坐标系下(通常为“odom”),通过LocalPlannerUtil::getLocalPlan(tf::Stamped& global_pose, std::vector& transformed_plan)实现。
在getLocalPlan中,先将较长的全局路径映射并截断到局部地图内(即坐标系转换为局部地图,且范围完全在局部地图内,超出地图的则抛弃)。然后,裁减全局路径和局部路径(与机器人当前位置距离超过1m的旧的路径会被裁减掉)。

裁减过的全局路径还保存在planner_util_中,映射并经过裁减后的局部路径会在computeVelocityCommands函数中传入dp_中(利用DWAPlanner::updatePlanAndLocalCosts函数)。
在updatePlanAndLocalCosts中,会将局部路径保存到dp_的global_plan_中(对于dp_来说,局部地图中映射的全局路径就是全局的,而dp_自己规划的路径是局部的)。然后,将该路径信息传入各个打分对象,具体参考打分章节。

5 打分

打分对象一共6个,分别为:
base_local_planner::OscillationCostFunction oscillation_costs_(摆动打分)
base_local_planner::ObstacleCostFunction obstacle_costs_(避障打分)
base_local_planner::MapGridCostFunction path_costs_(路径跟随打分)
base_local_planner::MapGridCostFunction goal_costs_(指向目标打分)
base_local_planner::MapGridCostFunction goal_front_costs_(前向点指向目标打分)
base_local_planner::MapGridCostFunction alignment_costs_(对齐打分)

其中,摆动打分和避障打分项是独立的类型,后四个打分项是同一类型。

打分计算过程中出现的负的值可以认为是错误代码(用于指示具体的出错原因),而不是得分,该说明对下面所有的打分描述有效。
如果轨迹为空(在产生轨迹时出错,例如不满足最大最小速度要去),则各个打分项对应的得分都为0。
(1)打分对象初始化及更新
oscillation_costs_
在DWAPlanner的构造函数中,利用oscillation_costs_.resetOscillationFlags()复位摆动标志位(侧移、旋转、前进方向的相关参数strafe_pos_only_,strafe_neg_only_,strafing_pos_,strafing_neg_,rot_pos_only_,rot_neg_only_,rotating_pos_,rotating_neg_,forward_pos_only_,forward_neg_only_,forward_pos_,forward_neg_)。
然后将oscillation_costs传入打分项列表(也会加入其它打分项),并将打分项列表std::vector

6 停止

对于DWAPlannerROS的停止处理逻辑,由LatchedStopRotateController类完成,主要包括了停止判断、加速停止、旋转至目标朝向三个大的部分。

(1)停止判断
在move_base的每个控制周期,都会利用利用DWAPlannerROS::isGoalReached(内部利用LatchedStopRotateController::isGoalReached函数实现)判断是否已经到达了目标点(即位置在xy_goal_tolerance以内,朝向在yaw_goal_tolerance以内,且速度小于trans_stopped_velocity,rot_stopped_velocity)。

对于LatchedStopRotateController::isGoalReached函数,分为锁存目标和不锁存目标两种处理逻辑。锁存目标,即如果机器人在行驶过程中出现过位置满足xy_goal_tolerance的条件时,则设置xy_tolerance_latch_标志位,代表已经达到过目标,不用考虑最终是否超出目标点,直接进行停止、旋转至目标朝向即可。
如果不锁存,则必须始终满足机器人当前位置是否满足xy_goal_tolerance的条件,满足则代表到达了目标位置。

在到达目标位置的前提下,还要判断机器人朝向是否满足目标朝向yaw_goal_tolerance的需求,如果也满足,则判断当前速度是否满足停止条件,即x和y的速度小于trans_stopped_velocity,theta速度小于rot_stopped_velocity。

只有到达位置、达到朝向、速度满足停止三个条件都满足的情况下,才算机器人到达了目标位姿,isGoalReached函数才会返回true。

(2)加速停止
在DWAPlannerROS::computeVelocityCommands的每个计算周期内,都会先利用LatchedStopRotateController::isPostionReached函数判断是否已经到达目标位置,如果是,则利用LatchedStopRotateController::computeVelocityCommandsStopRotate函数计算对应的减速停止或者旋转至目标朝向的速度指令,否则才会使用DWAPlanner计算最优轨迹对应的速度命令。

对于isPostionReached的判断,和isGoalReached中判断达到位置的逻辑一样,即如果启动了锁存,且xy_tolerance_latch_标志位被标记(即满足过xy_goal_tolerance条件),则认为已经到达;如果未启用锁存,则需要基于当前位置是否满足xy_goal_tolerance确定是否到达了位置。

在LatchedStopRotateController::computeVelocityCommandsStopRotate函数中,会假设用户想要进行旋转停止了,即已经到达位置了,因此会先判断当前朝向是否满足yaw_goal_tolerance要求,如果满足,则将速度指令设置为0(这种做法太过粗略)。
如果朝向没有满足要求,则会判断是要进行加速停止还是旋转至目标点。

如果机器人还未处于旋转停止状态(通过rotating_to_goal标记),并且速度没有满足停止要求,则会调用stopWithAccLimits进行加速停止处理,否则代表应进入了旋转停止状态,则会调用rotateToGoal进行旋转至目标朝向处理,同时设置rotating_to_goal标志位true。

对于stopWithAccLimits函数,对于利用acc_lim参数进行最快的减速命令计算,然后利用DWAPlanner::checkTrajectory函数计算出轨迹,并进行轨迹打分。在计算轨迹时利用了SimpleTrajectoryGenerator::generateTrajectory(
Eigen::Vector3f pos,
Eigen::Vector3f vel,
Eigen::Vector3f sample_target_vel,
base_local_planner::Trajectory& traj),如果速度不满足最小速度要求(min_rot_vel、min_trans_vel)则轨迹为空。
在对轨迹进行打分时,利用了SimpleScoredSamplingPlanner::scoreTrajectory(Trajectory& traj, double best_traj_cost),如果轨迹为空,则打分为0,此时虽然checkTrajectory返回true,但对应的速度可能过小(目前判断没有太大影响)。
如果checkTrajecotry返回true(轨迹打分大于等于0)则代表路径有效,stopWithAccLimits计算出的加速停止速度命令有效,否则设置速度命令为0。

(3)旋转至目标朝向
旋转至目标朝向有rotateToGoal函数计算,首先将x和y速度设置为0,对于theta速度,利用当前朝向与目标朝向的差的比例控制(系数为1)产生,然后用加速能力(acc_lim*sim_period)、速度限幅(max_rot_vel,min_rot_vel)、减速能力(v^2=2as公式)进行速度限幅。接着利用DWAPlanner::checkTrajectory进行轨迹打分检测,逻辑同前。

转自 http://www.cnblogs.com/sakabatou/p/8297479.html

 介绍如何为机器人整合导航包,实现有效控制和自主导航等功能 目录:  ROSnavigation 教程-目录  ROSnavigation 教程-设置机器人使用 TF  ROSnavigation 教程-基本导航调试指南  ROSnavigation 教程-安装和配置导航包  ROSnavigation 教程-结合 RVIZ 与导航包  ROSnavigation 教程-发布里程计消息  ROSnavigation 教程-发布传感器数据  ROSnavigation 教程-编写自定义全局路径规划ROSnavigation 教程-stage 仿真  ROSnavigation 教程-示例-激光发布(C++)  ROSnavigation 教程-示例-里程发布(C++)  ROSnavigation 教程-示例-点云发布(C++)  ROSnavigation 教程-示例-机器人 TF 设置(C++)  ROSnavigation 教程-示例-导航目标设置(C++)  ROSnavigation 教程-turtlebot-整合导航包简明指南  ROSnavigation 教程-turtlebot-SLAM 地图构建  ROSnavigation 教程-turtlebot-现有地图的自主导航  ROSnavigation 教程-map_server 介绍  ROSnavigation 教程-move_base 介绍  ROSnavigation 教程-move_base_msgs 介绍  ROSnavigation 教程-fake_localization 介绍  ROSnavigation 教程-voel_grid 介绍  ROSnavigation 教程-global_planner 介绍  ROSnavigation 教程-base_local_planner 介绍2  ROSnavigation 教程-carrot_planner 介绍  ROSnavigation 教程-teb_local_planner 介绍  ROSnavigation 教程-dwa_local_planner(DWA)介绍  ROSnavigation 教程-nav_core 介绍  ROSnavigation 教程-robot_pose_ekf 介绍  ROSnavigation 教程-amcl 介绍  ROSnavigation 教程-move_slow_and_clear 介绍  ROSnavigation 教程-clear_costmap_recovery 介绍  ROSnavigation 教程-rotate_recovery 介绍  ROSnavigation 教程-costmap_2d 介绍  ROSnavigation 教程-costmap_2d-range_sensor_layer 介绍  ROSnavigation 教程-costmap_2d-social_navigation_layers 介绍  ROSnavigation 教程-costmap_2d-staticmap 介绍  ROSnavigation 教程-costmap_2d-inflation 介绍  ROSnavigation 教程-obstacle 层介绍  ROSnavigation 教程-Configuring Layered Costmaps
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值