TEB算法详解（TebLocalPlannerROS::computeVelocityCommands（3））

第一章主要分析了teb算法的准备条件，包括获取当前位姿与速度、对全局路径的裁减以获取局部路径等以及局部地图的获取等。第二章主要讲述了在获取前置条件后，如何根据前置条件进行位姿优化，teb的路径优化主要是调用了g2o优化算法，以全局路径的点以及理论上点与点之间的运动时间作为g2o的优化顶点，以距离障碍物的距离、机器人最大运动速度、点与点之间的最短路径等约束条件作为g2o的边对局部路径规划的姿态进行了优化。这一章则是主要看一下算法怎么从优化后的点位到运动输出之间的关系。

假设算法已经获得了g2o优化后的结果，则算法在发布速度前会经历以下几步：

1、判断结果是否发散

//这里似乎对最后一个点进行了评价，如果最后优化的结果超过最大可接受Mahalanobis距离则返回true，说明优化结果发散。
  if (planner_->hasDiverged())
  {
    cmd_vel.twist.linear.x = cmd_vel.twist.linear.y = cmd_vel.twist.angular.z = 0;

    // Reset everything to start again with the initialization of new trajectories.
    // 重置所有变量，再次开始新轨迹的初始化
    planner_->clearPlanner();
    ROS_WARN_THROTTLE(1.0, "TebLocalPlannerROS: the trajectory has diverged. Resetting planner...");

    ++no_infeasible_plans_; // increase number of infeasible solutions in a row
    time_last_infeasible_plan_ = ros::Time::now();
    last_cmd_ = cmd_vel.twist;
    return mbf_msgs::ExePathResult::NO_VALID_CMD;
  }

在获取g2o优化的位姿结果后，算法第一步是对结果进行了收敛性判断。判断的依据应该是：判断局部路径规划的最后一个点的优化前后的曼哈顿距离是否在满足的阈值内，如果不满足则说明优化的结果发散了。

2、是否动态更新footprint

	// Check feasibility (but within the first few states only)检查路径是否可行
  //动态更新footprint_spec_，如果为true，则在检查轨迹可行性之前更新足迹
  if(cfg_.robot.is_footprint_dynamic)
  {
    // Update footprint of the robot and minimum and maximum distance from the center of the robot to its footprint vertices.
    // 更新机器人的足迹以及从机器人中心到足迹顶点的最小和最大距离，这个默认为false，好像是跟机器人模型相关？
    // teb初始化的时候执行过一次getRobotFootprint以及calculateMinAndMaxDistances，如果模型没有变化这里应该是不用重新计算的。
    footprint_spec_ = costmap_ros_->getRobotFootprint();
    //calculateMinAndMaxDistances函数对车体尺寸进行建模，对不同形状的车体进行外壳的位置计算。
    costmap_2d::calculateMinAndMaxDistances(footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius);
  }

这个过程是由参数cfg_.robot.is_footprint_dynamic决定的，机器人的footprint也就是机器人的模型一般来说是固定的，footprint的获取会在teb的初始化函数中获取一次，如果不改变的话后面算法会一直使用这个footprint，但是如果说机器人的模型会动态变化的话这会在这个位置进行一次更新。

3、轨迹可行性判读

bool feasible = planner_->isTrajectoryFeasible(costmap_model_.get(), footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius, cfg_.trajectory.feasibility_check_no_poses);

前面全局路径规划得到一条路径，这条路径是确认可以到达终点的，但是因为这里的局部路径规划优化后的轨迹会与全局路径规划有所区别，同时还有上面的可能存在的机器人模型的变化，所以对于优化后的轨迹我们需要重新判断它的可行性。

if (i<look_ahead_idx)
    {//查看两个点之间的角度以及距离
      double delta_rot = g2o::normalize_theta(g2o::normalize_theta(teb().Pose(i+1).theta()) -
                                              g2o::normalize_theta(teb().Pose(i).theta()));
      Eigen::Vector2d delta_dist = teb().Pose(i+1).position()-teb().Pose(i).position();
      //如果两个点之间的角度差超过阈值，或者点与点之间的距离超过一个机器人的身位(说明只遍历点是无法确认路径是否有效的，中间有盲区)
      if(fabs(delta_rot) > cfg_->trajectory.min_resolution_collision_check_angular || delta_dist.norm() > inscribed_radius)
      {
        //计算需要插入几个点
        int n_additional_samples = std::max(std::ceil(fabs(delta_rot) / cfg_->trajectory.min_resolution_collision_check_angular), 
                                            std::ceil(delta_dist.norm() / inscribed_radius)) - 1;
        PoseSE2 intermediate_pose = teb().Pose(i);
        //计算每个中间点的坐标
        for(int step = 0; step < n_additional_samples; ++step)
        {
          intermediate_pose.position() = intermediate_pose.position() + delta_dist / (n_additional_samples + 1.0);
          intermediate_pose.theta() = g2o::normalize_theta(intermediate_pose.theta() + 
                                                           delta_rot / (n_additional_samples + 1.0));
          //验证每个坐标点上的机器人模型是否会与障碍物重合
          if ( costmap_model->footprintCost(intermediate_pose.x(), intermediate_pose.y(), intermediate_pose.theta(),
            footprint_spec, inscribed_radius, circumscribed_radius) == -1 )
          {
            if (visualization_) 
            {
              visualization_->publishInfeasibleRobotPose(intermediate_pose, *robot_model_);
            }
            return false;
          }
        }
      }
    }

判断的方式也很简单：

首先对于局部路径规划中的每个点，将机器人模型投影到这个点上去判断这个点是否会跟障碍物i模型相重合，如果存在重合就说明这条路径有问题需要重新规划。

其次，判断局部路径上每个点之间的距离以及朝向角度差，这个距离不能超过机器人的长度，如果超过了机器人的长度则点上的模型不能完全覆盖机器人路径，可能会存在说两个姿态点都是没有问题的但是点与点之间存在障碍物的问题，这样仅仅对点进行判断是不能完全保证路径可行的。对于这个问题的处理方式也很简单，对路径按照机器人长度进行差值，直到模型覆盖整条路径为止。角度的问题也是一样的。

4、计算速度

//teb的速度比较简单，就是受cfg_.trajectory.control_look_ahead_poses影响，取向前多少个位姿点的姿态。然后用当前点的位姿与目标点做差
  //速度的话就是两点间距离/前面计算过的点与点之间理想时间和
  //在前面运动优化部分有计算过每个位姿之间的理论运动时间。例如这里如果要取前向第五个点，那么时间就是前面计算过的点1-2-3-4-5的时间和，距离就是点1与点5的距离差
  //角度的话也是简单明了的两点之间角度差/时间
  if (!planner_->getVelocityCommand(cmd_vel.twist.linear.x, cmd_vel.twist.linear.y, cmd_vel.twist.angular.z, cfg_.trajectory.control_look_ahead_poses))
  {
    planner_->clearPlanner();
    ROS_WARN("TebLocalPlannerROS: velocity command invalid. Resetting planner...");
    ++no_infeasible_plans_; // increase number of infeasible solutions in a row
    time_last_infeasible_plan_ = ros::Time::now();
    last_cmd_ = cmd_vel.twist;
    message = "teb_local_planner velocity command invalid";
    return mbf_msgs::ExePathResult::NO_VALID_CMD;
  }

重头戏！！！！！teb的用处是什么？？？不就是为了求机器人的速度吗？弯弯绕绕走了这么久终于算速度了。点进去看一下。

emmm,其实还是比较简单的哈。注意到传入了一个参数：cfg_.trajectory.control_look_ahead_poses。这个参数是用来判断机器人使用前向第几个姿态点作为速度计算的终点。这里有个细节，就是对于靠近终点附近的点位，剩余点的数量可能会少于这个参数，那就直接用终点了。然后我们就有了一个起点一个终点。要怎么计算运动过去的速度呢？往下看：

 //计算两个点的距离，得到x、y两个方向的距离
  Eigen::Vector2d deltaS = pose2.position() - pose1.position();
  //差速轮，没有y方向的速度
  if (cfg_->robot.max_vel_y == 0) // nonholonomic robot
  {
    //
    Eigen::Vector2d conf1dir( cos(pose1.theta()), sin(pose1.theta()) );
    // translational velocity
    //这边是在求方向？
    double dir = deltaS.dot(conf1dir);
    //deltaS.norm()返回的是斜边长度，也就是两点之间的距离
    //g2o::sign(dir)函数，当dir>0返回1，dir<0返回-1
    vx = (double) g2o::sign(dir) * deltaS.norm()/dt;
    vy = 0;
  }
  else // holonomic robot
  {
    // transform pose 2 into the current robot frame (pose1)
    // for velocities only the rotation of the direction vector is necessary.
    // (map->pose1-frame: inverse 2d rotation matrix)
    double cos_theta1 = std::cos(pose1.theta());
    double sin_theta1 = std::sin(pose1.theta());
    double p1_dx =  cos_theta1*deltaS.x() + sin_theta1*deltaS.y();
    double p1_dy = -sin_theta1*deltaS.x() + cos_theta1*deltaS.y();
    vx = p1_dx / dt;
    vy = p1_dy / dt;    
  }
  
  // rotational velocity
  //求旋转的角速度
  double orientdiff = g2o::normalize_theta(pose2.theta() - pose1.theta());
  omega = orientdiff/dt;

首先deltaS 里面存储的是xy两个方向的距离，dir获取了机器人需要的方向，计算方式使用的是xy方向的距离乘以角度的三角函数。g2o::sign(dir)函数对速度进行归一化，当dir>0返回1，dir<0返回-1。然后具体的速度大小则是距离/时间

这里的时间是来自于点与点之间的理论时间和。在前面运动优化部分有计算过每个位姿之间的理论运动时间。例如这里如果要取前向第五个点，那么时间就是前面计算过的点1-2-3-4-5的时间和，距离就是点1与点5的距离差：

for(int counter = 0; counter < look_ahead_poses; ++counter)
  {
    //计算一个运动时间，从当前点到前向多少个点的理论时间和
    dt += teb_.TimeDiff(counter);
    //dt_ref为两个点之间允许的最大时间，如果超过这个时间会进行点的插入
    if(dt >= cfg_->trajectory.dt_ref * look_ahead_poses)  // TODO: change to look-ahead time? Refine trajectory?
    {
        look_ahead_poses = counter + 1;
        break;
    }
  }

同样的，角速度也是两点之间的角度差/时间和。

5、速度限定

  //速度硬约束，保证计算结果不超过设定值
  // Saturate velocity, if the optimization results violates the constraints (could be possible due to soft constraints).
  saturateVelocity(cmd_vel.twist.linear.x, cmd_vel.twist.linear.y, cmd_vel.twist.angular.z,
                   cfg_.robot.max_vel_x, cfg_.robot.max_vel_y, cfg_.robot.max_vel_trans, cfg_.robot.max_vel_theta, 
                   cfg_.robot.max_vel_x_backwards);

这里就是速度进行最后的判断了，上述计算出来的速度是否满足算法设定的最大值，如果超过这个值的话会被限定到范围内。

6、非原地转向车辆的角速度修正

//如果需要，将rot-vel转换为转向角（carlike robot）。min_turning_radius允许稍小，因为它是一个软约束，而不是其他不受penalty_epsilon影响的约束。用户可以为参数本身添加安全裕度。
  if (cfg_.robot.cmd_angle_instead_rotvel)
  {
    cmd_vel.twist.angular.z = convertTransRotVelToSteeringAngle(cmd_vel.twist.linear.x, cmd_vel.twist.angular.z,
                                                                cfg_.robot.wheelbase, 0.95*cfg_.robot.min_turning_radius);
    if (!std::isfinite(cmd_vel.twist.angular.z))
    {
      cmd_vel.twist.linear.x = cmd_vel.twist.linear.y = cmd_vel.twist.angular.z = 0;
      last_cmd_ = cmd_vel.twist;
      planner_->clearPlanner();
      ROS_WARN("TebLocalPlannerROS: Resulting steering angle is not finite. Resetting planner...");
      ++no_infeasible_plans_; // increase number of infeasible solutions in a row
      time_last_infeasible_plan_ = ros::Time::now();
      message = "teb_local_planner steering angle is not finite";
      return mbf_msgs::ExePathResult::NO_VALID_CMD;
    }
  }

这个函数是只针对类似于我们小汽车之类的无法原地转向的机器人的，则需要设定最小转弯半径。这里暂时就不展开看了。

7、发布信息

  last_cmd_ = cmd_vel.twist;
  // 可视化障碍物，路过点，全局路径
  // Now visualize everything    
  planner_->visualize();
  visualization_->publishObstacles(obstacles_);
  visualization_->publishViaPoints(via_points_);
  visualization_->publishGlobalPlan(global_plan_);
  return mbf_msgs::ExePathResult::SUCCESS;

最后，算法会发布机器人的速度以及轨迹，到此，算法的整体流程就完成了。