TEB (Timed Elastic Band)

最新推荐文章于 2024-07-26 15:48:53 发布

gxt_kt

最新推荐文章于 2024-07-26 15:48:53 发布

阅读量290

点赞数 1

文章标签： teb 路径规划

本文链接：https://blog.csdn.net/my_id_kt/article/details/132679899

版权

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年版的，功能也都很完善了。

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

输入输出

输入：

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

输出：

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

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

大概计算步骤

准备好相关数据
- 起始点位姿和终点（目标点）位姿
- 准备好障碍物信息（没有可以不添加）
根据当前是否有路径进行选择
- 第一次优化可能没有起始路径，就直接从起点到终点插值（不考虑障碍物碰撞）。一个起始点，一个终点，会补充到min_sample（超参，3）个点，也就是中间差值一个
- 如果已经有了路径，就只需要对已有路径插值就好了。
- 如果路径崩了（比如曲线优化不了了），就直接清除路径，和第一次优化一样
构建问题，添加约束
优化问题，再回到第二步循环

相关细节

关于插值

**保证每相邻两个点之间的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的约束中，只添加了核心必要约束，满足需求使用即可。

关于倒车问题

和三个变量相关：

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

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));
    }
}