xtea算法python解法 teb算法解析

上一章主要研究了一下teb算法中局部路径规划之前的处理，包括了局部地图的处理、初始位姿、机器人当前速度以及从全局路径中如何提取出局部路径等内容。这一章继续看一下teb算法中对于局部路径规划的运动部分处理，看一下在已知上述先验的条件下算法是如何计算出一个合适的轨迹的。

teb算法的速度计算主要函数入口在：

bool success = planner_->plan(transformed_plan, &robot_vel_, cfg_.goal_tolerance.free_goal_vel);

这个函数在TebOptimalPlanner类中，它主要用来实现下述功能：

1、initTrajectoryToGoal

该函数只有在初始化时执行一次，它的作用主要是将起点终点固定，并计算每两个点之间的理想时间：距离/最大速度。另外还处理了下点的方向，对每个点的方向也进行了一定的平滑处理

2、updateAndPruneTEB

这个函数用于清理路径上已经走过的点，它的思路是：从局部路径规划的点位起点开始找到一个离当前位置最近的点。如果最近的点是第一个点，则会遍历所有点位，如果是其中某个点，则从第一个点开始往这个点遍历的时候距离会逐渐减小，每次记录更小的值，当值开始比上一个值更大时跳出循环：

double dist_cache = (new_start->position()- Pose(0).position()).norm();
    double dist;
    int lookahead = std::min<int>( sizePoses()-min_samples, 10); // satisfy min_samples, otherwise max 10 samples
    int nearest_idx = 0;
    for (int i = 1; i<=lookahead; ++i)
    {
      dist = (new_start->position()- Pose(i).position()).norm();
      if (dist<dist_cache)
      {
        dist_cache = dist;
        nearest_idx = i;
      }
      else break;
    }

然后根据记录的点的ID，将容器中前面的点清理掉。然后将终点记录在BackPose中

if (new_goal && sizePoses()>0)
  {
    BackPose() = *new_goal;
  }

3、设定初始速度

if (start_vel)
    setVelocityStart(*start_vel);

根据odom返回的当前速度决定teb算法当前初始速度。

4、设定是否允许机器人以非０速度到达终点

if (free_goal_vel)
    setVelocityGoalFree();
  else
    vel_goal_.first = true;

这个由参数cfg_.goal_tolerance.free_goal_vel决定

5、TebOptimalPlanner::optimizeTEB

optimizeTEB进行最后的优化，它的主要作用包括了：

5.1、resize路径

这个功能由参数trajectory.teb_autosize决定，用于优化前是否再次优化一下路径。这里的优化是对路径点的删除与增加，受五个参数影响：

cfg_->trajectory.dt_ref
cfg_->trajectory.dt_hysteresis
cfg_->trajectory.min_samples
cfg_->trajectory.max_samples
cfg_->obstacles.include_dynamic_obstacles

dt_ref与dt_hysteresis联用，前面记得我们曾经计算过两个点之间的运动时间，这里根据时间来确定是否增加或者删除一些点，当两点间时间超过dt_ref+dt_hysteresis，会进行点的增加；当两点间时间小于dt_ref-dt_hysteresis，会进行点的删除；

min_samples与max_samples作为两个限制，总的点数不能超过这两者之间的区间。

include_dynamic_obstacles用来确定是只更新一个点就退出还是更新到上述条件不满足情况下退出。

5.2、构建图

teb算法的核心算法是通过g2o算法实现的，所以这里需要构建图优化需要的一些参数，图的构建包括了以下几个部分：
1）、设定优化器的属性，决定是否记录中间信息和统计信息

optimizer_->setComputeBatchStatistics(cfg_->recovery.divergence_detection_enable);

2）、添加顶点

// add TEB vertices
  // 添加位姿态和时间间隔顶点 
  AddTEBVertices();

teb的边包含了两个方面：姿态以及时间。前面我们计算以及增删了一部分点位并对点位的朝向进行了修改，以及计算了位姿之间的运动时间。这里会将这两者都作为g2o优化的顶点信息添加进去。

for (int i=0; i<teb_.sizePoses(); ++i)
  {
    teb_.PoseVertex(i)->setId(id_counter++);
    optimizer_->addVertex(teb_.PoseVertex(i));
    if (teb_.sizeTimeDiffs()!=0 && i<teb_.sizeTimeDiffs())
    {
      teb_.TimeDiffVertex(i)->setId(id_counter++);
      optimizer_->addVertex(teb_.TimeDiffVertex(i));
    }
    iter_obstacle->clear();
    (iter_obstacle++)->reserve(obstacles_->size());
  }

3）、添加约束
在添加完顶点后，算法继续添加约束需要的边。这里的边包含了几个部分：

首先添加的是障碍物边，

// 添加障碍物边
  if (cfg_->obstacles.legacy_obstacle_association)
    AddEdgesObstaclesLegacy(weight_multiplier);
  else
    AddEdgesObstacles(weight_multiplier);

这里的添加方式有两种，具体使用哪一种由参数obstacles.legacy_obstacle_association决定。以AddEdgesObstacles为例，函数首先判断是否进行了障碍物膨胀，之后定义了一个创建边的函数。之后，遍历每一个坐标点，找到离坐标点距离小于阈值的障碍物，以及左侧和右侧最近的障碍物，构建EdgeObstacle对象，作为图的障碍物边。边误差的计算为坐标点到障碍物的距离，再过一个激活函数。

然后是添加动态障碍物边：

//添加动态障碍物边
  if (cfg_->obstacles.include_dynamic_obstacles)
    AddEdgesDynamicObstacles();

基本和添加障碍物类似，只是障碍物随时间变化。

接下来添加经过路径点边：

//添加经过路径点边
  AddEdgesViaPoints();

AddEdgesViaPoints函数中首先遍历每一个路径点，计算与当前路径点最近的坐标点，构建路径点边，类型为 EdgeViaPoint，边的误差计算就是欧氏距离。

下一个是添加速度与加速度约束边：

//添加速度边 
  AddEdgesVelocity();
  //添加加速度边
  AddEdgesAcceleration();

添加速度与加速度约束目的是防止线速度和角速度超过给定阈值。

再下一个是添加时间约束边：

//添加时间约束边
  AddEdgesTimeOptimal();

时间最优化边，用于优化时间

以及一个最短路径边：

//添加最短路径边
  AddEdgesShortestPath();

目的是优化轨迹长度

最后还有一个运动学约束：

//添加运动学约束边
  if (cfg_->robot.min_turning_radius == 0 || cfg_->optim.weight_kinematics_turning_radius == 0)
    AddEdgesKinematicsDiffDrive(); // we have a differential drive robot
  else
    AddEdgesKinematicsCarlike(); // we have a carlike robot since the turning radius is bounded from below.
  //添加旋转约束边
  AddEdgesPreferRotDir();

AddEdgesKinematicsDiffDrive函数的作用是用于添加动力学限制边，目的是让生成的轨迹满足运动学约束。AddEdgesPreferRotDir用于使机器人在避障过程中倾向左侧还是右侧。

至此，图优化需要的顶点与约束关系就添加完成了。

5.3、优化图

前面我们构建了一张图，这里则是对图进行优化：

//优化图
    success = optimizeGraph(iterations_innerloop, false);
    if (!success) 
    {
        clearGraph();
        return false;
    }
    optimized_ = true;

代码本身是比较简单的，主要内容包含了三行代码：

optimizer_->setVerbose(cfg_->optim.optimization_verbose);
  optimizer_->initializeOptimization();
  int iter = optimizer_->optimize(no_iterations);

前面两行是对优化器属性设置以及初始化优化器，第三行是调用优化器进行no_iterations迭代。

5.4、计算cost

这个步骤在外循环的最后一次完成时进行计算：

//在最后一次循环时计算当前cost
    if (compute_cost_afterwards && i==iterations_outerloop-1) // compute cost vec only in the last iteration
      computeCurrentCost(obst_cost_scale, viapoint_cost_scale, alternative_time_cost);

cost包含了两个部分：

第一个部分是时间成本：

if (alternative_time_cost)
  {
    cost_ += teb_.getSumOfAllTimeDiffs();
    // TEST we use SumOfAllTimeDiffs() here, because edge cost depends on number of samples, which is not always the same for similar TEBs,
    // since we are using an AutoResize Function with hysteresis.
  }

第二个是障碍物成本：

for (std::vector<g2o::OptimizableGraph::Edge*>::const_iterator it = optimizer_->activeEdges().begin(); it!= optimizer_->activeEdges().end(); it++)
  {
    double cur_cost = (*it)->chi2();

    if (dynamic_cast<EdgeObstacle*>(*it) != nullptr
        || dynamic_cast<EdgeInflatedObstacle*>(*it) != nullptr
        || dynamic_cast<EdgeDynamicObstacle*>(*it) != nullptr)
    {
      cur_cost *= obst_cost_scale;
    }
    else if (dynamic_cast<EdgeViaPoint*>(*it) != nullptr)
    {
      cur_cost *= viapoint_cost_scale;
    }
    else if (dynamic_cast<EdgeTimeOptimal*>(*it) != nullptr && alternative_time_cost)
    {
      continue; // skip these edges if alternative_time_cost is active
    }
    cost_ += cur_cost;
  }

最后算法会返回一个当前总的成本。

至此整个plan函数的流程就完成了，但是这里其实只是对轨迹进行了优化，这个轨迹该如何转化成机器人所需要的速度呢