TEB算法详解(TebLocalPlannerROS::computeVelocityCommands(2))

这篇具有很好参考价值的文章主要介绍了TEB算法详解(TebLocalPlannerROS::computeVelocityCommands(2))。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

上一章主要研究了一下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、设定是否允许机器人以非0速度到达终点

	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函数的流程就完成了,但是这里其实只是对轨迹进行了优化,这个轨迹该如何转化成机器人所需要的速度呢?下一章详细再看一下这部分。

参考:文章来源地址https://www.toymoban.com/news/detail-418741.html

https://blog.csdn.net/flztiii/article/details/121545662

到了这里,关于TEB算法详解(TebLocalPlannerROS::computeVelocityCommands(2))的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • TEB算法实现步骤

    以下是TEB算法的基本步骤: 初始化机器人的起始姿态和目标姿态。 根据机器人的动态约束和环境中的障碍物生成一系列候选路径。 对于每个候选路径,计算其路径长度和与目标姿态的距离。 根据路径长度和距离,对候选路径进行排序,选择最优路径。 对最优路径进行平滑

    2024年02月01日
    浏览(32)
  • 机器人控制算法——局部规划器TEB算法原理及C++可视化仿真

    1.背景介绍 最近一段时间,由于项目需要,一直在做TEB算法的工程化的工作,于是就考虑写下一篇系统些的文章,作为笔记,后续自己看也方便,TEB的英文名Time elastic band”,是一种局部规划器,它的核心思想是将路径规划问题转化为一个带有时间弹性的优化问题,通过对时间弹

    2024年02月04日
    浏览(44)
  • ROS-melodic:源码安裝teb_local_planner算法、替换DWA算法

    源码下载地址:GitHub - rst-tu-dortmund/teb_local_planner: An optimal trajectory planner considering distinctive topologies for mobile robots based on Timed-Elastic-Bands (ROS Package)  注意选择对应ROS版本的代码。  放在navigation目录下(或者自己创建一个): 安装缺失依赖(在teb_local_planner目录下打开终端):

    2024年02月08日
    浏览(41)
  • 机器人控制算法——TEB算法—Obstacle Avoidance and Robot Footprint Model(避障与机器人足迹模型)

    1.1处罚条款 避障是作为整体轨迹优化的一部分来实现的。显然,优化涉及到找到指定成本函数(目标函数)的最小成本解(轨迹)。简单地说:如果一个计划的(未来)姿势违反了与障碍物的期望分离,那么成本函数的成本必须增加。理想情况下,在这些情况下,成本函数值

    2024年02月06日
    浏览(49)
  • move_base代码解析(四)局部路径规划:TrajectoryPlannerROS::computeVelocityCommands

    在第三章中讲述了executeCycle的总体作用,可以看到这个函数的作用主要是将全局路径规划的路径给到局部路径规划,并判断机器人是否到位,如果没有到位就调用computeVelocityCommands函数计算机器人的速度。这里也就是move_base的局部路径规划的所在之处。这章简单张开看一下m

    2024年02月13日
    浏览(31)
  • 拓扑排序详解(包含算法原理图解、算法实现过程详解、算法例题变式全面讲解等)

    在图论中,如果一个有向图无法从某个顶点出发经过若干条边回到该点,则这个图是一个有向无环图(DAG图)。 如图所示。 对于一个有向图,若x点指向y点,则称x点为y点的入度。 对于一个有向图,若x点指向y点,则称y点为x点的出度。 队列是一种特殊的线性表,特殊之处在

    2024年02月07日
    浏览(52)
  • 【算法】—贪心算法详解

    ①贪心算法的概念 : 贪心算法就是不断选择 在当前看来最好的选择,也就是说贪心算法并不从整体最优考虑,它所作出的选择只是在某种意义上的局部最优选择 虽然贪心算法不能对所有问题都得到整体最优解,但在一些情况下,即使贪心算法 不一定能得到整体最优解 ,其

    2024年02月04日
    浏览(44)
  • 【算法】最直接的算法——穷举法详解

    穷举法又称为枚举法或者蛮力法,是一种简单直接解决问题的方法,常常是基于问题的直接描述去编写程序,比如说求n的阶乘,那么就直接一个循环n次的for循环。 穷举法依赖的基本技术是遍历,也就是采用一定策略依次处理待求解问题的所有元素。对于穷举法自身的优化,

    2024年02月08日
    浏览(39)
  • prim算法(普里姆算法)详解

    了解了什么是最小生成树后,本节为您讲解如何用普里姆(prim)算法查找连通网(带权的连通图)中的最小生成树。 普里姆算法查找最小生成树的过程,采用了贪心算法的思想。对于包含 N 个顶点的连通网,普里姆算法每次从连通网中找出一个权值最小的边,这样的操作重

    2024年01月16日
    浏览(41)
  • 辐射神经场算法——NeRF算法详解

    NeRF(Neural Radiance Fields)是2020年ECCV会议上的Best Paper,一石激起千层浪,在此之后的两三年的各大顶会上相关文章层出不穷,其影响力可见一斑,NeRF通过隐式表达的方式将新视角合成任务(Novel View Synthesis Task)推向了一个新的高度。那么,什么是“新视角合成任务”呢?什么

    2024年02月06日
    浏览(47)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包