现实环境中,关于Teb Local Planner 参数调试总结

1.问题一:针对阿克曼底盘控制的小车,方向控制“摆头”严重!
在Teb中,主要由几个参数决定:

根据de_ref参考两个位姿之间的时间差,dt_hysteresis为滞后时间,也就是为时间差加一个误差限,一般为dt_ref的10%。
经过调整滞后想要达到的目的是,每个时间差都在dt_ref左右。

 # ********************** Carlike robot parameters ********************
 min_turning_radius: 0.5        # Min turning radius of the carlike robot (compute value using a model or adjust with rqt_reconfigure manually)
 wheelbase: 0.4                 # Wheelbase of our robot
 cmd_angle_instead_rotvel: True # stage simulator takes the angle instead of the rotvel as input (twist message)
 # ********************************************************************

在车体模型参数中,轴距和转弯半径都会对其造成影响,cmd_angle_instead_rotvel会将控制命令转换成线速度和角度的方式,当轴距设置较大时,再经过反正切函数计算角度时,会将其角度的波段范围缩小,会有效的降低“摆头”情况,但是治标不治本。
通过设置de_ref也可以有效的降低“摆头”情况。

2.问题二:局部路径目标点设置错误,导致并不会严格按照全局路径寻迹,直接贯穿地图。
原因:在Teb中,切割全局路径时,将CostMap的一半作为阈值,如果当前位置在局部地图边界之外,则把全局地图的最后一个点设置为局部规划的目标点。
在Teb的transformGlobalPlan函数中:

    //we'll discard points on the plan that are outside the local costmap
    double dist_threshold = std::max(costmap.getSizeInCellsX() * costmap.getResolution() / 2.0,
                                     costmap.getSizeInCellsY() * costmap.getResolution() / 2.0);

以上确定了距离的阈值,接下来计算全局路径中符合阈值的路径点,这里全局地图和局部地图都是在Map坐标系下,其实并没有转换。如果满足阈值要求,则加入到局部路径transformed_plan中去。

 //now we'll transform until points are outside of our distance threshold
    while(i < (int)global_plan.size() && sq_dist <= sq_dist_threshold && (max_plan_length<=0 || plan_length <= max_plan_length))
    {
      const geometry_msgs::PoseStamped& pose = global_plan[i];
      tf::poseStampedMsgToTF(pose, tf_pose);
      tf_pose.setData(plan_to_global_transform * tf_pose);
      tf_pose.stamp_ = plan_to_global_transform.stamp_;
      tf_pose.frame_id_ = global_frame;
      tf::poseStampedTFToMsg(tf_pose, newer_pose);
      transformed_plan.push_back(newer_pose);

      double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x;
      double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y;
      sq_dist = x_diff * x_diff + y_diff * y_diff;
      
      // caclulate distance to previous pose
      if (i>0 && max_plan_length>0)
        plan_length += distance_points2d(global_plan[i-1].pose.position, global_plan[i].pose.position);
      ++i;
    }

如果没有满足要求的全局路径点,就会发生将全局路径最后一个点设置为局部路径的当前目标点,此时便会发生横穿地图的情况:

    if (transformed_plan.empty())
    {
      tf::poseStampedMsgToTF(global_plan.back(), tf_pose);
      tf_pose.setData(plan_to_global_transform * tf_pose);
      tf_pose.stamp_ = plan_to_global_transform.stamp_;
      tf_pose.frame_id_ = global_frame;
      tf::poseStampedTFToMsg(tf_pose, newer_pose);

      transformed_plan.push_back(newer_pose);
      
      // Return the index of the current goal point (inside the distance threshold)
      if (current_goal_idx) *current_goal_idx = int(global_plan.size())-1;
    }

否则,将从全局路径中找到的符合要求的最后一个点,设置为局部路径的当前目标点current_goal_idx

    else
    {
      // Return the index of the current goal point (inside the distance threshold)
      if (current_goal_idx) *current_goal_idx = i-1; // subtract 1, since i was increased once before leaving the loop
    }

因为在实际需求中,给定固定的全局路径,因此如果要避免这种情况,就要将局部地图CostMap的范围设大,此时会影响计算的效率,否则要时小车的当前位置靠近全局路径。如果参数设置不当,小车较容易偏离全局路径,如果此时不更新全局路径的话,就会直接奔向全局地图的最后一个点。
附上Teb代码解析博客 https://blog.csdn.net/windxf/article/details/110039280

在后续的调试中,发现是全局路径剪切问题,导致无法找到最近点,即参数global_plan_prune_distance: 0.6

double dist_thresh_sq = dist_behind_robot*dist_behind_robot;
    
    // iterate plan until a pose close the robot is found
    std::vector<geometry_msgs::PoseStamped>::iterator it = global_plan.begin();
    std::vector<geometry_msgs::PoseStamped>::iterator erase_end = it;
    while (it != global_plan.end())
    {
      double dx = robot.getOrigin().x() - it->pose.position.x;
      double dy = robot.getOrigin().y() - it->pose.position.y;
      double dist_sq = dx * dx + dy * dy;
      if (dist_sq < dist_thresh_sq)
      {
         erase_end = it;
         break;
      }
      ++it;
    }

通过参数擦除之前经过的全局路径点,默认全局路径中第一个点即最近点,因此并不会往后去寻找。
因为在一般规划中,全局路径会不断地更新,所以全局路径中第一个点即为最近点。
而在本次需求下,需要按照事先规定的全局路径,而此时巡逻开始的位置并不一定在全局路径的起点。

3.问题三:在Teb计算控制命令时,计算时间较长,发送频率过低导致控制不平滑,频繁刹车。
参考文章:https://www.it610.com/article/1295841594758471680.htm

TEB规划器的性能问题的总结参考
关闭多路径并行规划(效果非常显著)
使用Costmap Converter (非常显著)
降低迭代次数(no_inner/outer_iterations) (显著)
降低 max_lookahead_distance (一般)
减小局部耗费地图的大小 (显著)
增大规划周期和控制周期 (影响效果)
使用单点footprint,配合最小障碍物距离约束 (不太显著且影响效果)

Costmap Converter(ROS WIKI:costmap_converter)
TEB默认情况下不使用Costmap Converter。事实上,此插件可以在复杂场景下极大提高运算效率,尤其是处理激光雷达分散的测量数据时。因为将障碍物视为系列孤立点效率极低。

#当使用costmap_converter插件时
costmap_converter_plugin  #(字符串,默认值:" ")
#定义插件名称,以及将成本单元格转换成点/线/多边形。设置一个空字符串以禁用转换,以便将所有单元格都视为点障碍
costmap_converter_spin_thread #(bool,默认值:true)
#如果设置为true,costmap转换器将在另一个线程中调用其回调队列
costmap_converter_rate  #(double,默认值:5.0)
#用于定义插件处理当前成本图的频率

本地costmap_2d配置(强烈建议使用滚动窗口!):
weight/height:局部成本图的大小:表示最大轨迹长度以及要考虑的占用单元数(对计算时间有重大影响,但是如果太小:较短的预测/规划范围会降低自由度,例如,为了避开障碍物)
resolution:解析局部成本图:精细的解析度(较小的值)意味着存在许多需要优化的障碍(对计算时间的重大影响)

teb_local_planner/Tutorials/Frequently Asked Questions中有更多关于如何设置优化参数来降低计算时间。
Wiki链接:http://wiki.ros.org/teb_local_planner/Tutorials/Frequently%20Asked%20Questions

*关于Teb的博客:
ROS - teb_local_planner 参数总结
帮助路径规划理解

4.问题四:路径规划的过程中,小车原地转圈,但是全局路径正常
检查local cost map参数,heightwidth是否太小!
如果参数设置过小的话,局部路径无法生成就会原地打转!

  • 18
    点赞
  • 80
    收藏
  • 打赏
    打赏
  • 5
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
©️2022 CSDN 皮肤主题:博客之星2021 设计师:Hiro_C 返回首页
评论 5

打赏作者

秃头队长

你的鼓励将是我创作的最大动力

¥2 ¥4 ¥6 ¥10 ¥20
输入1-500的整数
余额支付 (余额:-- )
扫码支付
扫码支付:¥2
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、C币套餐、付费专栏及课程。

余额充值