当前位置:   article > 正文

TEB算法5- teb recovery机制

teb 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
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9

备份模式

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 mode
    if (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());
        else
            goal_idx += horizon_reduction; // this should not happen, but safety first ;-)
    }


    // detect and resolve oscillations
    if (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 );
        else
            max_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 recently

        if (oscillating)
        {
            if (!recently_oscillated)
            {
                // save current turning direction
                if (robot_vel_.angular.z > 0)
                    last_preferred_rotdir_ = RotType::left;
                else
                    last_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.");
        }
    }

}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76

震荡检测流程

在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;
    //这里仅记录速度x
    measurement.v = twist.linear.x; // just consider linear velocity in x-direction in the robot frame for now
    measurement.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 state
    detect(v_eps, omega_eps);
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26

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 half
        return false;

    double n = (double)buffer_.size();
            
    // compute mean for v and omega
    double 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_;
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/weixin_40725706/article/detail/697917
推荐阅读
相关标签
  

闽ICP备14008679号