TEB (Timed Elastic Band)

TEB (Timed Elastic Band)

源码：https://github.com/gxt-kt/teb_local_planner

移植了官方的teb源码，实现了普通优化和多路径优化。

移植参考ros源码。无需ros框架即可运行，并使用opencv进行显示。

并用 google 的 ceres-solver 替换 g2o 进行单路径优化。

主要用来解决局部路径规划问题，和DWA属于同一级别

使用到了Ceres/g2o等优化库进行优化，使得输出的曲线更加平滑，通过加入机器人运动学约束，从而生成更加符合机器人动力学特性的轨迹。

---

关于论文

主要论文有两篇：

• 2012年：主要是提出TEB算法核心思想，存在约束过少或者不成熟的问题，另外单路径会陷入局部最优值

• 2017年：基于2012年，改进了约束，添加了很多新约束，让路径更加平滑和可用。另外提出了一种绕开障碍的多路径方法，解决了单路径陷入局部最优值问题。（但实际计算仍然是多个问题多次优化，计算会更加耗时一点）

TEB的ROS实现源码是2017年版的，功能也都很完善了。

在源码部分分为单路径和多路径。但是其核心的优化部分都是一样的，无非多了一个路径探索。

---

输入输出

输入：

• 起始点位姿和终点位姿
• 途径经过点
• 障碍物位置和类型

输出：

• 中间路径上的每个点的位姿

当然其中还有许多超参是可以调整的，包括机器人模型啥的。

---

大概计算步骤

1. 准备好相关数据

   • 起始点位姿和终点（目标点）位姿
   • 准备好障碍物信息（没有可以不添加）

2. 根据当前是否有路径进行选择

   • 第一次优化可能没有起始路径，就直接从起点到终点插值（不考虑障碍物碰撞）。一个起始点，一个终点，会补充到min_sample（超参，3）个点，也就是中间差值一个
   • 如果已经有了路径，就只需要对已有路径插值就好了。
   • 如果路径崩了（比如曲线优化不了了），就直接清除路径，和第一次优化一样

3. 构建问题，添加约束

4. 优化问题，再回到第二步循环

相关细节

关于插值

**保证每相邻两个点之间的timediff小于0.4和大于0.2，并路径上的总点数小于500。**这一部分在函数TimedElasticBand::autoResize

• timediff根据两个位姿的距离/max_vel （max_vel是给超参，代表机器人最大线速度）
• 大于0.4就在中间差值
• 小于0.2就把点删了

所以插值就理解上分成两种：

• 相邻两个点距离太远了，就需要在中间插值
• 相邻两个点距离太近了，就需要删掉点

关于具体怎么插：位置就是直接取两点中点，朝向就是取向量中间值，下面是角度取中间值的代码：

inline double average_angle(double theta1, double theta2) {
  double x, y;

  x = std::cos(theta1) + std::cos(theta2);
  y = std::sin(theta1) + std::sin(theta2);
  if(x == 0 && y == 0)
    return 0;
  else
    return std::atan2(y, x);
}

---

关于障碍物的添加

TEB其实给了很多障碍物类型：

• 点类型，最常用
• 圆形障碍物
• 椭圆形障碍物
• 线障碍物
• 多边形障碍物
• 动态障碍物（有待进一步验证）

原本TEB源码是在ROS中的，所以这一部分会订阅代价地图相关，然后给到TEB的障碍物

问题：拿到的代价地图是怎么转换成障碍物容器的

有一个配置 cfg_.obstacles.costmap_converter_plugin

• 如果为空，就是不启用插件，这样激光雷达的点就是障碍物，都是point类型的

• 如果启用，就用运行对应属性的插件，把costmap转换成障碍物

  teb默认是没有启用插件的，也就是全都是point类型障碍物

TEB在ROS中添加障碍物的源码 ：起始也就是遍历地图所有栅格，然后先确实是障碍物，然后把机器人前进相反方向并且间隔一定距离的给筛掉，剩下的就确定是障碍物，push_back添加就行了

void TebLocalPlannerROS::updateObstacleContainerWithCostmap() {
  // Add costmap obstacles if desired
  if (cfg_.obstacles.include_costmap_obstacles) {
    Eigen::Vector2d robot_orient = robot_pose_.orientationUnitVec();

    for (unsigned int i = 0; i < costmap_->getSizeInCellsX() - 1; ++i) {
      for (unsigned int j = 0; j < costmap_->getSizeInCellsY() - 1; ++j) {
        if (costmap_->getCost(i, j) == costmap_2d::LETHAL_OBSTACLE) {
          Eigen::Vector2d obs;
          costmap_->mapToWorld(i, j, obs.coeffRef(0), obs.coeffRef(1));

          // check if obstacle is interesting (e.g. not far behind the robot)
          Eigen::Vector2d obs_dir = obs - robot_pose_.position();
          if (obs_dir.dot(robot_orient) < 0 &&
              obs_dir.norm() >
                  cfg_.obstacles.costmap_obstacles_behind_robot_dist)
            continue;

          obstacles_.push_back(ObstaclePtr(new PointObstacle(obs)));
        }
      }
    }
  }
}

---

关于顶点和约束

下面是以图优化中的顶点和边的角度来讲的，也就是基于原本的g2o的，其实使用ceres也差不多，只是约束添加部分不一样

添加顶点：交替添加位姿和时间差，比如先添加位姿id=0，添加时间差id=1，在添加位姿id=2，再添加时间差id=3，由于位姿数刚好比时间差多一个，所以最后添加的一个为位姿（id=128）

添加约束：

• 添加经过点的边：对于每个经过点，找到和它最近的顶点，还要确保找到的顶点不会是开头和结束的点，然后把经过点约束到最相邻顶点的边

• 添加机器人速度边约束：约束两个相邻顶点的速度，多元边，顶点是两个相邻位姿和对应的时间差，如果位姿数是65个，这里的边就是添加64条

• 添加机器人加速度约束：

  • 如果有初始速度vel_start_.first，也把初始速度加入约束
  • 然后根据三个相邻姿态点添加加速度约束，如果位姿数是65个，这里的边就是添加63条
  • 如果有最终目标点速度vel_end_.first，也把初始速度加入约束

• 添加时间约束：如果时间数是64个，这里的边就是添加64条

• 添加最短路径约束：和相邻两个位姿有关，让两个位姿距离最小，如果位姿数是65个，这里的边就是添加64条

• 添加差动驱动机器人的运动学约束：运动学约束，和相邻两个位姿有关，如果位姿数是65个，这里的边就是添加64条

• 添加障碍物的约束：

  • 源码中有两种关于障碍物的算法（我们用的新的）（新的和旧的遍历对象不同）
    • 旧的：对于每个障碍物，查找最近的 TEB 位姿
    • 新的：对于每个 teb 位姿，仅查找"相关"障碍物
  • 去除第一个和最后一个点，然后计算障碍物到顶点的距离
  • 如果距离很近，就认为是相关的障碍物边
  • 如果距离较远，就直接不考虑
  • 普通距离也会考虑一个最相邻左边和一个最相邻右边的障碍物
  • 然后添加相关障碍物边和一个最左边和最右边

• 动态障碍物约束：暂时跳过（大家自行研究）

以上就是g2o版本的约束添加，在ceres的约束中，只添加了核心必要约束，满足需求使用即可。

---

关于倒车问题

和三个变量相关：

1. max_vel_x_backwards 最大倒车速度，这个值不能设置为0或者负数，否则不收敛，倒车问题不能通过这个变量解决

2. weight_kinematics_forward_drive 用于强制机器人仅选择前进方向的优化权重，可用于解决倒车问题，加大此变量，会加大倒车惩罚。实测加到500-1500左右比较合适

   下面展示了关于weight_kinematics_forward_drive具体计算残差的代码（非连续）

   information_kinematics(1, 1) = cfg_->optim.weight_kinematics_forward_drive;
   
   const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
   const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
   
   Eigen::Vector2d deltaS = conf2->position() - conf1->position();
   
   // non holonomic constraint
   _error[0] = fabs( ( cos(conf1->theta())+cos(conf2->theta()) ) * deltaS[1] - ( sin(conf1->theta())+sin(conf2->theta()) ) * deltaS[0] );
   
   // positive-drive-direction constraint
   Eigen::Vector2d angle_vec ( cos(conf1->theta()), sin(conf1->theta()) );	   
   _error[1] = penaltyBoundFromBelow(deltaS.dot(angle_vec), 0,0);
   
   inline double penaltyBoundFromBelow(const double& var, const double& a,const double& epsilon)
   {
       if (var >= a+epsilon)
       {
           return 0.;
       }
       else
       {
           return (-var + (a+epsilon));
       }
   }

3. 和最优化时间weight_optimaltime相关

   如果把这个值设置成0，就会原地打转，再移动。但事实上改动这个值肯定不太现实，我们的时间还是又要求的

---

启用多路径

本仓库已经将多路径源码移了出来。

根据enable_homotopy_class_planning变量进行选择HomotopyClassPlanner

---