TEB算法5- teb recovery机制

teb recovery机制

• recovery概述
• Recovery参数说明
• 备份模式
• 震荡检测流程

recovery概述

在调用teb中的computeVelocityCommands()函数计算速度时,会不断的调用configureBackupModes()函数检查是否进入备份模式并进行相关的设置。
configureBackupModes()主要实现

• 判断规划是否异常，如果检测到规划异常超出设定阈值，则缩小时域规划范围,TEB将以更近的点作为规划目标，尝试重新规划出可行路径;
• 检查机器人是否震荡，如果震荡，选择一个方向作为路径规划的优先方向；

Recovery参数说明

#*******************************************************************************
#当规划器检测到系统异常，允许缩小时域规划范围,TEB将以更近的点作为规划目标，尝试重新规划出可行路径;
shrink_horizon_backup: false       
shrink_horizon_min_duration: 10  #如果检测到不可行的轨迹，激活缩小的水平线后备模式，本参数为其最短持续时间。
oscillation_recovery: True      #尝试检测和解决振荡
oscillation_v_eps: 0.1          #(0,1)内的 normalized 线速度的平均值的阈值，判断机器人是否运动异常
oscillation_omega_eps: 0.1      #(0,1)内的 normalized 角速度的平均值，判断机器人是否运动异常
oscillation_recovery_min_duration: 10  #在这个时间内，是否再次发生FailureDetector检测的振荡
oscillation_filter_duration: 10  #failure_detector_中buffer容器的大小为oscillation_filter_duration * controller_frequency

备份模式

teb_local_planner_ros.cpp
如果检测到规划异常超出设定阈值，则缩小时域规划范围,TEB将以更近的点作为规划目标，尝试重新规划出可行路径;

void TebLocalPlannerROS::configureBackupModes(std::vector<geometry_msgs::PoseStamped>& transformed_plan,  int& goal_idx)
{ros::Time current_time = ros::Time::now();// shrink_horizon_backup:当规划器检测到系统异常，允许缩小时域规划范围,TEB将以更近的点作为规划目标，尝试重新规划出可行路径;// reduced horizon backup modeif (cfg_.recovery.shrink_horizon_backup &&goal_idx < (int)transformed_plan.size()-1 && // we do not reduce if the goal is already selected (because the orientation might change -> can introduce oscillations)(no_infeasible_plans_>0 || (current_time - time_last_infeasible_plan_).toSec() < cfg_.recovery.shrink_horizon_min_duration )) // keep short horizon for at least a few seconds{ROS_INFO_COND(no_infeasible_plans_==1, "Activating reduced horizon backup mode for at least %.2f sec (infeasible trajectory detected).", cfg_.recovery.shrink_horizon_min_duration);// Shorten horizon if requested// reduce to 50 percent:int horizon_reduction = goal_idx/2;//连续10次探测到不可行的轨迹,horizon_reduction进一步缩小。if (no_infeasible_plans_ > 9){ROS_INFO_COND(no_infeasible_plans_==10, "Infeasible trajectory detected 10 times in a row: further reducing horizon...");horizon_reduction /= 2;}// we have a small overhead here, since we already transformed 50% more of the trajectory.// But that's ok for now, since we do not need to make transformGlobalPlan more complex// and a reduced horizon should occur just rarely.int new_goal_idx_transformed_plan = int(transformed_plan.size()) - horizon_reduction - 1;goal_idx -= horizon_reduction;if (new_goal_idx_transformed_plan>0 && goal_idx >= 0)transformed_plan.erase(transformed_plan.begin()+new_goal_idx_transformed_plan, transformed_plan.end());elsegoal_idx += horizon_reduction; // this should not happen, but safety first ;-)}// detect and resolve oscillationsif (cfg_.recovery.oscillation_recovery){double max_vel_theta;double max_vel_current = last_cmd_.linear.x >= 0 ? cfg_.robot.max_vel_x : cfg_.robot.max_vel_x_backwards;if (cfg_.robot.min_turning_radius!=0 && max_vel_current>0)max_vel_theta = std::max( max_vel_current/std::abs(cfg_.robot.min_turning_radius),  cfg_.robot.max_vel_theta );elsemax_vel_theta = cfg_.robot.max_vel_theta;//检查机器人是否震荡failure_detector_.update(last_cmd_, cfg_.robot.max_vel_x, cfg_.robot.max_vel_x_backwards, max_vel_theta,cfg_.recovery.oscillation_v_eps, cfg_.recovery.oscillation_omega_eps);bool oscillating = failure_detector_.isOscillating();//限定时间周期内(oscillation_recovery_min_duration)是否已经检测到过震荡行为bool recently_oscillated = (ros::Time::now()-time_last_oscillation_).toSec() < cfg_.recovery.oscillation_recovery_min_duration; // check if we have already detected an oscillation recentlyif (oscillating){if (!recently_oscillated){// save current turning directionif (robot_vel_.angular.z > 0)last_preferred_rotdir_ = RotType::left;elselast_preferred_rotdir_ = RotType::right;ROS_WARN("TebLocalPlannerROS: possible oscillation (of the robot or its local plan) detected. Activating recovery strategy (prefer current turning direction during optimization).");}time_last_oscillation_ = ros::Time::now();planner_->setPreferredTurningDir(last_preferred_rotdir_);}else if (!recently_oscillated && last_preferred_rotdir_ != RotType::none) // clear recovery behavior{last_preferred_rotdir_ = RotType::none;planner_->setPreferredTurningDir(last_preferred_rotdir_);ROS_INFO("TebLocalPlannerROS: oscillation recovery disabled/expired.");}}}

震荡检测流程

在teb_local_planner/recovery_behaviors.cpp中定义了FailureDetector类。
其中的主要函数包括update() 和detect ()两个函数。
具体的update函数：
实现更新机器人速度信息，并将线速度和角速度归一化到[0,1]区间，存入buffer中

// ============== FailureDetector Implementation ===================void FailureDetector::update(const geometry_msgs::Twist& twist, double v_max, double v_backwards_max, double omega_max, double v_eps, double omega_eps)
{if (buffer_.capacity() == 0)return;VelMeasurement measurement;//这里仅记录速度xmeasurement.v = twist.linear.x; // just consider linear velocity in x-direction in the robot frame for nowmeasurement.omega = twist.angular.z;//归一化速度到(0,1)区间if (measurement.v > 0 && v_max>0)measurement.v /= v_max;else if (measurement.v < 0 && v_backwards_max > 0)measurement.v /= v_backwards_max;if (omega_max > 0)measurement.omega /= omega_max;// 存入buffer_中buffer_.push_back(measurement);// immediately compute new statedetect(v_eps, omega_eps);
}

detect（）函数判断是否震荡

//检查机器人是否震荡
bool FailureDetector::detect(double v_eps, double omega_eps)
{oscillating_ = false;if (buffer_.size() < buffer_.capacity()/2) // we start detecting only as soon as we have the buffer filled at least halfreturn false;double n = (double)buffer_.size();// compute mean for v and omegadouble v_mean=0;double omega_mean=0;int omega_zero_crossings = 0;for (int i=0; i < n; ++i){v_mean += buffer_[i].v;omega_mean += buffer_[i].omega;//前后两个角速度方向是否一致if ( i>0 && g2o::sign(buffer_[i].omega) != g2o::sign(buffer_[i-1].omega) )++omega_zero_crossings;}v_mean /= n;omega_mean /= n;//如果线速度和角速度均值小于阈值，且方向震荡，则判定机器人处于震荡状态if (std::abs(v_mean) < v_eps && std::abs(omega_mean) < omega_eps && omega_zero_crossings>1 ) {oscillating_ = true;}
//     ROS_INFO_STREAM("v: " << std::abs(v_mean) << ", omega: " << std::abs(omega_mean) << ", zero crossings: " << omega_zero_crossings);return oscillating_;
}

本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！