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

这篇具有很好参考价值的文章主要介绍了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;

最后,算法会发布机器人的速度以及轨迹,到此,算法的整体流程就完成了。文章来源地址https://www.toymoban.com/news/detail-416191.html

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

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

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

相关文章

  • TEB算法实现步骤

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

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

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

    2024年02月04日
    浏览(43)
  • 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日
    浏览(38)
  • 机器人控制算法——TEB算法—Obstacle Avoidance and Robot Footprint Model(避障与机器人足迹模型)

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

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

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

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

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

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

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

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

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

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

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

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

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

    2024年02月06日
    浏览(44)

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

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

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

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

二维码1

领取红包

二维码2

领红包