艺点意创官网,在线seo,wordpress个人博客主题2019,修改wordpress邮件在我之前的文章《ROS导航包Navigation中的 Movebase节点路径规划相关流程梳理》中已经介绍过Move_base节点调用局部路径规划器插件的接口函数是computeVelocityCommands#xff0c;接下来#xff0c;我们就从这个函数入手梳理一下teb_local_planner功能包的工作流程。 ☆注接下来我们就从这个函数入手梳理一下teb_local_planner功能包的工作流程。 ☆注因篇幅较长本部分内容分成了上和下两篇文章阅读完成本文的内容后想要阅读后续内容请前往下继续阅读。 下篇文章链接如下 《ROS局部路径规划器插件teb_local_planner流程梳理下》 一、规划前的准备工作 1、判断插件是否已完成初始化若没有则返回错误信息 if(!initialized_)2、获取机器人当前位姿及速度信息
costmap_ros_-getRobotPose(robot_pose);odom_helper_.getRobotVel(robot_vel_tf);robot_vel_.angular.z tf2::getYaw(robot_vel_tf.pose.orientation);3、对走过的全局路径部分进行修剪 由于机器人是实时运动的所以需要对已经走过的全局路径裁减处理通过调用pruneGlobalPlan函数完成裁剪处理其实现思路是从全局路径规划容器的起点开始一个个遍历计算其与机器人当前点的距离差直到找到距离小于我们设定的阈值的点将该点之前的全局路径点全部删除这样就可以将位于机器人后方距离超过一定阈值的点删除掉。 上述判断用的阈值使用的是外部参数global_plan_prune_distance。 //修改全局路径规划将已经走过的路径删除保留机器人前方的路径pruneGlobalPlan(*tf_, robot_pose, global_plan_, cfg_.trajectory.global_plan_prune_distance);4、 将全局路径转换到局部代价地图的坐标系下并从中选择合适的点作为局部路径点 调用transformGlobalPlan函数通过tf 树将全局路径信息转换到局部代价地图的坐标系下值得注意的是在这个函数中并不仅仅进行了坐标系的变换还对全局路径信息点进行了一些处理从中选出合适的点作为局部路径规划点。 其具体过程是依次对全局路径进行坐标变换同时计算其与当前位置的距离并统计已转换的全局路径点之间的距离和即已转换的全局路径的长度直到超过设定的长度阈值或者与当前点的距离大于设定的距离阈值则丢弃剩余的路径点。 其中长度阈值由外部参数max_global_plan_lookahead_dist决定距离阈值由局部代价地图的大小width和height以及系数0.85有关
transformGlobalPlan(*tf_, global_plan_, robot_pose, *costmap_, global_frame_, cfg_.trajectory.max_global_plan_lookahead_dist, transformed_plan, goal_idx, tf_plan_to_global)附距离阈值sq_dist_threshold 计算过程
double dist_threshold std::max(costmap.getSizeInCellsX() * costmap.getResolution() / 2.0,costmap.getSizeInCellsY() * costmap.getResolution() / 2.0);
dist_threshold * 0.85; // just consider 85% of the costmap size to better incorporate point obstacle that are
double sq_dist_threshold dist_threshold * dist_threshold;概括一下经过本步之后从全局路径信息中选出的局部路径点是在以机器人当前位置为圆心距离阈值sq_dist_threshold 半径的圆内的路径点且累积路径长度小于路径长度阈值的点的集合。 5、 对得到的局部路径规划点进行进一步的筛选 调用updateViaPointsContainer函数对上一步得到的局部路径规划点进行进一步的筛选其实现思路是循环遍历局部路径规划点从中选出距离上一个被选中的点的距离大于设定的阈值的点局部路径规划点中的第一个点为本步筛选中第一个被选中的点 这样处理可以将上一步得到的局部路径点稀疏化从而减少局部路径点的个数阈值由外部参数global_plan_viapoint_sep决定若该参数设置为负数则表示跳过本步处理不进行筛选
updateViaPointsContainer(transformed_plan, cfg_.trajectory.global_plan_viapoint_sep);第3~5步过程动态演示如下 ROS的TEB局部路径规划器全局路径处理过程示意 6、 判断机器人是否到达目标点 得到局部路径的状态点后先检查是否已经到达了全局目标点具体要检查以下内容 1当前点距离目标点的距离是否小于由外部参数设定的阈值xy_goal_tolerance 2当前点的姿态角与目标姿态角是否小于由外部参数设定的阈值yaw_goal_tolerance 3判断机器人是否完全到位由参数complete_global_plan以及via_points_.size()影响其中complete_global_plan主要作用是防止机器人在越过终点时提前结束路径 4有些版本中还会检测判断机器人当前速度是否停止跟参数theta_stopped_vel、trans_stopped_vel、free_goal_vel有关其中free_goal_vel由外部参数设定是否允许机器人以最大速度驶向目的地即是否允许机器人达到目标点的速度不为零参数heta_stopped_vel和trans_stopped_vel在melodic版本的包中并没有。
if(fabs(std::sqrt(dx*dxdy*dy)) cfg_.goal_tolerance.xy_goal_tolerance fabs(delta_orient) cfg_.goal_tolerance.yaw_goal_tolerance (!cfg_.goal_tolerance.complete_global_plan || via_points_.size() 0))7、 判断是否进入恢复行为模式 调用configureBackupModes函数判断是否进入恢复模式 1短时间内规划失败若机器人规划失败时没有到达目标点但是距离上次局部路径规划成功的时间间隔很小算法会缩小规划范围对局部路径点进行裁减将远处的点先裁减掉以更近的点作为规划目标尝试重新规划出可行路径。 2长时间的规划失败多次短期规划都失败后算法判断机器人是否长时间在某个地方发生振荡其具体思路是首先将机器人当前速度与角速度归一化到[0,1]然后对线速度与角速度进行判断若机器人在一段时间内的平均线速度与角速度都小于阈值并且前后两次采样过程中机器人的角速度相反则认为机器人陷入了振荡状态算法中创建了容器buffer_。用于面存储了一段时间内的机器人线速度与角速度 当机器人处于振荡状态同时如果这个时间超过阈值oscillation_recovery_min_duration则机器人会进行旋转旋转方向由机器人当前角速度反方向决定如果没有超过阈值则以当前机器人角速度方向进行旋转。
configureBackupModes(transformed_plan, goal_idx);8、调整局部目标点的角度 根据外部参数 global_plan_overwrite_orientation决定是否调整局部目标点的角度也就是经过上述处理后的路径的最后一个点的角度调整是通过调用estimateLocalGoalOrientation函数来实现的
if (cfg_.trajectory.global_plan_overwrite_orientation)robot_goal_.theta() estimateLocalGoalOrientation(global_plan_, transformed_plan.back(), goal_idx, tf_plan_to_global);当需要修改时会判断当前局部路径规划的终点与全局路径规划的终点是否接近如果两个点比较接近(就是同一个点或者下一个点就是终点)则直接使用当前局部路径规划中终点的方向作为规划的方向。如果当前局部路径规划的终点跟全局目标点之间还间隔很多个点的话会使用当前局部路径的终点以及向后两个点的角度进行平滑处理得到一个新的角度值。 当不需要修改时采用局部目标点的原有角度。 9、将机器人当前的位姿插入到局部路径点中 上一步完成了局部规划的目标点的确认这一步要完成局部规划的起始点的确认前面虽然根据全局路径规划中截取了局部路径规划的点但是这个容器的起点不一定是是机器人当前时刻所处的点因此需要将机器人当前位姿插入到前面得到的局部路径点中作为规划的起点。 transformed_plan.front() robot_pose; // update start 更新起始点10、更新障碍物信息 根据外部参数 costmap_converter决定如何更新障碍物信息若该参数不为空则调用costmap_converter插件进行地图信息更新若为空直接使用costmap的信息进行更新。 分别对应下面那个函数
updateObstacleContainerWithCostmapConverterupdateObstacleContainerWithCostmap若使用costmap_converter插件进行更新则在updateObstacleContainerWithCostmapConverter函数中调用了getObstacles()函数从代价地图转换器获得障碍物并对障碍物根据其形状圆、点、线、多边形进行分类将其转换成相应的障碍物类型然后将障碍物坐标存储到obstacles_容器中针对移动的障碍物还会设定其速度。 若使用costmap的信息进行更新根据外部参数 include_costmap_obstacles决定是否考虑costmap中的障碍物若考虑则遍历整个代价地图对于每一个代价地图单元格如果该单元格的代价为LETHAL_OBSTACLE代价值为100则将其视为障碍物并检测该障碍物是否足够引人注意(例如离机器人不远)如果障碍物在机器人后面且距离机器人的距离超过了外部参数决定的阈值 costmap_obstacles_behind_robot_dist则忽略该障碍物。最后将得到的障碍物添加到障碍物容器中 除此之外还要从ROS消息中获取障碍物信息然后将这些障碍物添加到当前的障碍物容器中。该过程通过函数updateObstacleContainerWithCustomObstacles完成 // also consider custom obstacles (must be called after other updates, since the container is not cleared)// 还要考虑custom障碍(必须在其他更新之后调用因为障碍容器没有被清除)updateObstacleContainerWithCustomObstacles();消息来源于
custom_obst_sub_ nh.subscribe(obstacles, 1, TebLocalPlannerROS::customObstacleCB, this);至此规划前的准备工作完成下面正式开始规划进行局部路径的规划 二、调用核心函数plan进行局部路径规划 bool success planner_-plan(transformed_plan, robot_vel_, cfg_.goal_tolerance.free_goal_vel);需要注意的是存在三个名为plan的函数根据传入的形参及其含义可知此处调用的是如下函数
bool TebOptimalPlanner::plan(const std::vectorgeometry_msgs::PoseStamped initial_plan, const geometry_msgs::Twist* start_vel, bool free_goal_vel)1、根据teb_.isInit()判断是否初始化从起点到目标点的路径 若是则调用initTrajectoryToGoal函数完成该操作若不是则采用热启动的方式此时需要进一步判断如果当前目标与上一个目标的位置差小于阈值force_reinit_new_goal_dist姿态角度差小于阈值force_reinit_new_goal_angular则调用updateAndPruneTEB函数清理路径上已经走过的点若不满足上述阈值则认为当前目标点与上一个目标点之间的距离足够远了需要重新调用initTrajectoryToGoal函数进行路径的初始化。 其中updateAndPruneTEB函数的具体实现思路是首先在局部路径规划点中寻找距离当前位置最近的点然后从局部路径规划点的第一个点开始往后搜索直至找到这个最近的点。然后删除这个最近的点之前的那些点。 调用initTrajectoryToGoal函数的语句如下 teb_.initTrajectoryToGoal(initial_plan, cfg_-robot.max_vel_x, cfg_-robot.max_vel_theta, cfg_-trajectory.global_plan_overwrite_orientation,cfg_-trajectory.min_samples, cfg_-trajectory.allow_init_with_backwards_motion);需要注意的是存在两个名为initTrajectoryToGoal的函数根据传入的形参及其含义可知此处调用的是如下函数
bool TimedElasticBand::initTrajectoryToGoal(const std::vectorgeometry_msgs::PoseStamped plan, double max_vel_x, double max_vel_theta, bool estimate_orient, int min_samples, bool guess_backwards_motion)其具体实现思路如下 1检查是否已经初始化路径如果没有则添加起始位置处的姿态点(包括机器人的位置和朝向)到局部路径点的容器中并设置起点优化中为固定不变的。 2设定后退标志位若外部参数 allow_init_with_backwards_motion为真即否允许在开始时想后退来执行轨迹且目标点在起始点的后方则将后退标志位backwards标志设置为true否则为false。 3从起始点开始遍历局部路径点容器中的每个点若外部参数 global_plan_overwrite_orientation为真即否允覆盖全局路径中局部路径点的朝向若允许则根据相邻两个局部路径点之间的连线的朝向利用atan2函数重新计算每个局部路径点的朝向否则保持每个局部路径姿态点的原有朝向不变。 在上述遍历中还会利用外部参数 max_vel_x和max_vel_theta即设定的最大线速度和角速度来预估相邻两个局部路径点之间要花费的时间dt即两点间位置差除以max_vel_x得到线运动时间两点间角度差除以max_vel_theta得到角运动时间然后取这两个时间中的较大值作为两点之间小路径的预估时间dt。 4判断是否需要强制添加局部路径点若当前的局部路径点的数量小于由外部参数设定的最小样本数 min_samples-1为了保证生成的路径点数量足够多则会强制添加一些局部路径点具体方式是不断在当前点与目标点之间取中点添加到局部路径点中当前点会变化直至局部路径点的数量等于min_samples-1。 static PoseSE2 average(const PoseSE2 pose1, const PoseSE2 pose2){return PoseSE2( (pose1._position pose2._position)/2 , g2o::average_angle(pose1._theta, pose2._theta) );}5将目标点添加到局部路径点的末尾处并将其设为固定即在优化中不能被改变。 至此局部路径的初始化完成可以这样理解以上过程当我们接收到新的目标点时新的目标点与上一个目标点的距离或姿态差大于设定的阈值需要重新调用initTrajectoryToGoal函数进行路径的初始化若目标点没有变化或者变化小于阈值则认为是机器人移动过程中针对同一目标点的多次规划此时只需要调用updateAndPruneTEB函数对已经走过的部分的路径点进行裁剪就可以了不需要重新进行路径初始化也就是所谓的热启动模式。 ☆注因篇幅较长本部分内容分成了上和下两篇文章阅读完成本文的内容后想要阅读后续内容请前往下继续阅读。 下篇文章链接如下 《ROS局部路径规划器插件teb_local_planner流程梳理下》
