TEB算法5- 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