沉默杀手
(四)configureBackupModes 2 FailureDetector和振荡恢复
|路径规划TEB算法|
Word count: 1.2k|Reading time: 5 min

configureBackupModes 第二部分

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
// detect and resolve oscillations
if (cfg_.recovery.oscillation_recovery)
{
    double max_vel_theta;
    // 车一般不能倒退，所以基本是 max_vel_x
    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();
    // 距离上次振荡过了多久，若小于配置值(10秒)，说明最近也振荡了
    bool recently_oscillated = (ros::Time::now()-time_last_oscillation_).toSec() < cfg_.recovery.oscillation_recovery_min_duration;
    // 如果现在是振荡状态
    if (oscillating)
    {
        if (!recently_oscillated)    //  最近的状态不是振荡
        {
            // save current turning direction
            // 以下其实都是对 last_preferred_rotdir_ 赋值
            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();
        // 对 prefer_rotdir_ 赋值，最终影响 AddEdgesPreferRotDir
        planner_->setPreferredTurningDir(last_preferred_rotdir_);
    }
    // 现在和最近都没有振荡   clear recovery behavior
    else if (!recently_oscillated && last_preferred_rotdir_ != RotType::none) 
    {
        last_preferred_rotdir_ = RotType::none;
        planner_->setPreferredTurningDir(last_preferred_rotdir_);
        ROS_INFO("TebLocalPlannerROS: oscillation recovery disabled/expired.");
    }
}

但是如果机器人确实stucked，最终会在move_base里Abort

setPreferredTurningDir

设置期望的初始转弯方向 (也就是惩罚相反的方向)。A desired (initial) turning direction might be specified in case the planned trajectory oscillates between two solutions (in the same equivalence class!) with similar cost. 配置参数以调整惩罚的权重，Initial 说明惩罚只适用于trajectory的前几个位姿

参数dir可以是RotType::left, RotType::right, RotType::none

代码实际调用的是：

1
2
3
4
5
6
7
8
void HomotopyClassPlanner::setPreferredTurningDir(RotType  dir)
{
  // set preferred turning dir for all TEBs
  for (TebOptPlannerContainer::const_iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb)
  {
    (*it_teb)->setPreferredTurningDir(dir);
  }
}

结果还是TebOptimalPlanner的同名成员函数，赋值而已

1
virtual void setPreferredTurningDir(RotType dir) {  prefer_rotdir_ = dir;  }

赋值完了，prefer_rotdir_会在AddEdgesPreferRotDir里产生影响

AddEdgesPreferRotDir

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
void TebOptimalPlanner::AddEdgesPreferRotDir()
{
    /* roesmann 注释: these edges can result in odd predictions, in particular
    we can observe a substantional mismatch between open- and closed-loop planning
   leading to a poor control performance.
   目前这个功能只用于振荡恢复，短期使用这个约束不会有重大影响
   可能让机器人脱离振荡 */
  if (prefer_rotdir_ == RotType::none || cfg_->optim.weight_prefer_rotdir == 0)
        return;
    // 必须定义好转向
  if (prefer_rotdir_ != RotType::right && prefer_rotdir_ != RotType::left)
  {
    ROS_WARN("TebOptimalPlanner::AddEdgesPreferRotDir(): unsupported RotType selected. Skipping edge creation.");
    return;
  }
  // create edge for satisfiying kinematic constraints
  Eigen::Matrix<double,1,1> information_rotdir;
  information_rotdir.fill(cfg_->optim.weight_prefer_rotdir);
  // 目前只使用前三个旋转
  for (int i=0; i < teb_.sizePoses()-1 && i < 3; ++i)
  {
    EdgePreferRotDir* rotdir_edge = new EdgePreferRotDir;
    rotdir_edge->setVertex(0, teb_.PoseVertex(i));
    rotdir_edge->setVertex(1, teb_.PoseVertex(i+1));      
    rotdir_edge->setInformation(information_rotdir);
    
    if (prefer_rotdir_ == RotType::left)
        rotdir_edge->preferLeft();
    else if (prefer_rotdir_ == RotType::right)
        rotdir_edge->preferRight();
    
    optimizer_->addEdge(rotdir_edge);
  }
}

调用时在configureBackupModes第二部分，那里是对prefer_rotdir_赋值，

FailureDetector

类FailureDetector的代码在TEB的recovery_behaviors.cpp，FailureDetector检测机器人是否 stucked或者振荡，分析last N(buffer的大小)个速度命令，看这些线速度和角速度的平均值是否小于oscillation_v_eps 和 oscillation_omega_eps

构造函数和析构全是空。

buffer

1
2
3
// Set buffer length (measurement history)
// length: number of measurements to be kept
void setBufferLength(int length) {  buffer_.set_capacity(length);  }

调用：

1
2
failure_detector_.setBufferLength(std::round(
    cfg_.oscillation_filter_duration * controller_frequency ) );

默认是10*30，其实比较大，可以降低oscillation_filter_duration

1
2
// Clear buffer， 置为不振荡状态
void clear();

FailureDetector::update

先看调用，只有第一个参数是速度控制量，其他都是配置参数:

1
2
3
4
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();

源码其实也简单

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
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;
    /* 
    struct VelMeasurement
    {
        double v = 0;
        double omega = 0;
    };  */
    measurement.v = twist.linear.x; // 不考虑 y 方向速度
    measurement.omega = twist.angular.z;
    
    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_.push_back(measurement);
    // immediately compute new state
    detect(v_eps, omega_eps);
}

detect

  • v_eps: (0,1)内的normalized 线速度的平均值的阈值，默认0.1
  • omega_eps: (0,1)内的normalized 角速度的平均值，默认0.1

如果机器人stucked则返回true，否则false

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
bool detect(double v_eps, double omega_eps)
{
    oscillating_ = false;
    //至少有一半的速度数据才检测，也就是过一点时间才判断
    if (buffer_.size() < buffer_.capacity()/2)  
        return false;
    double n = (double)buffer_.size();
    // 计算buffer内线速度和角速度的均值
    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;
    }
    return oscillating_;
}

参数

我用的参数值如下：

1
2
3
4
5
6
7
shrink_horizon_backup = true;
shrink_horizon_min_duration = 10:   # 默认10
oscillation_recovery = true;
oscillation_v_eps = 0.1;     # 线速度和角速度设置0.1已经足够
oscillation_omega_eps = 0.1;
oscillation_recovery_min_duration = 5;
oscillation_filter_duration = 6   # 默认10，这个大了会使buffer太大

要出现振荡，最直观的就是车左右摇摆或者车前后小步行走，因为代码中没有绝对值，否则朝一个方向小幅行走也算振荡了。实际中当然是不允许这种奇怪的行为。