本篇博客属于网上收集的笔记，大部分内容不是原创

所有的参考的文章都会在下面贴出来啦

我只是进行收集、总结、运用，后期可能会加入自己调试参数的效果

目录

 

一、ROS – teb_local_planner 参数总结

TEB 的参数调试：

二、TEB Local Planner的特性与参数调试技巧总结

TEB规划器的性能问题的总结参考

TEB Local Planner的重要特点和缺点

TEB Local Planner的参数调试方法

惩罚权重配置

TEB中的恢复措施

Costmap Converter

Local Costmap 配置指导

关于多路径并行规划(Parallel Planning in distinctive Topologies)

三、teb_local_planner安装及使用

四、ROS下阿克曼转向结构车型导航应用实践（teb_local_planner）

修改teb_local_planner_params.yaml中的内容

---

一、ROS – teb_local_planner 参数总结

原文链接：ROS – teb_local_planner 参数总结

这篇文章主要是参照官网的教程进行调试和总结的：

安装teb_local_planner：

sudo apt-get install ros-kinetic-teb-local-planner
sudo apt-get install ros-kinetic-teb-local-planner-tutorials

观察单个轨迹的仿真（观察参数对于路径规划上的影响），启动节点和rviz：

rosparam set /test_optim_node/enable_homotopy_class_planning True
roslaunch teb_local_planner test_optim_node.launch

启动参数调节器：

rosrun rqt_reconfigure rqt_reconfigure

汽车模型机器人的仿真：

roslaunch teb_local_planner_tutorials robot_carlike_in_stage.launch 

TEB 的参数调试：

欧几里得距离：

欧几里得距离，又称欧氏距离，是最常见的距离度量，衡量的是多维空间中各个点之间的绝对距离。在二维和三维空间中的欧式距离的就是两点之间的距离，二维的公式是 d = sqrt((x1-x2)^+(y1-y2)^) 三维的公式是 d=sqrt(x1-x2)^+(y1-y2)^+(z1-z2)^)。在计算相似度(比如人脸识别)的场景下，欧几里得距离是比较直观、比较常见的一种相似度算法。欧氏距离越小，相似度越大；欧氏距离越大，相似度越小。

二、TEB Local Planner的特性与参数调试技巧总结

原文链接：TEB Local Planner的特性与参数调试技巧总结

• ROS导航中的局部规划器TEB Local Planner在实际车辆上的应用条件和参数调试方法
• 讨论与之相关的局部耗费地图（local costmap）和耗费地图转换器(costmap converter)

TEB规划器的性能问题的总结参考

• 关闭多路径并行规划（效果非常显著）
• 使用Costmap Converter （非常显著）
• 降低迭代次数（no_inner/outer_iterations） （显著）
• 降低 max_lookahead_distance （一般）
• 减小局部耗费地图的大小 （显著）
• 增大规划周期和控制周期 （影响效果）
• 使用单点footprint，配合最小障碍物距离约束 （不太显著且影响效果）

TEB Local Planner的重要特点和缺点

• TEB规划器可以处理阿克曼底盘车辆的运动学约束，base local planner只能处理万向轮或差速转向的车辆模型
• TEB是时间最短的最优控制器。在最大速度和加速度等参数约束的条件下，TEB规划器将规划出时间最短路径和速度指令。路径上可以表现为明显的切弯、靠近障碍物；速度指令上将会表现为速度和方向指令的快速振荡。
• TEB基于姿态搜索最优方案。TEB的基本思路是在车辆附近空间内散布大量的姿态，然后通过搜索树搜索最优方案。因此，车辆轮廓的准确描述对于TEB的工作性能有显著影响。
• TEB规划器对电机和舵机的动态响应性能要求很高。使用直流电机时，需要认真调试PID参数才能实现比较好的效果。
• TEB规划器计算量很高。若开启不同拓扑结构路径的同时规划（homotopy class planning），规划周期甚至可达20秒以上，完全无法使用。
• TEB规划器的稳定性一般。由于不断重新规划路径，且实际车辆的定位测速不可能没有误差，在车辆静止时可以观察到舵机抖动。尽管降低规划频率(此频率由move_base的配置确定)有助于改善这一问题，但这同样导致车辆动态避障的性能大大降低。

TEB Local Planner的参数调试方法

下面的这些参数会在上面的表格中蓝色字体处理：

• max_vel_x和acc_lim_x 这是速度与加速度约束
• 注意加速度同样约束减速过程。若电机性能良好且速度不快可以立即刹车，可直接将acc_lim_x设置为0，表示没有约束。若电机不能承受阶跃输入或者响应时间很长，则应当设置加速度限制。
• max_vel_x_backwards 最大倒车速度
• 将此速度设置为0或者负数将导致错误。禁止倒车应在penalty部分将前向行驶的权重设置得极高。
• max_vel_y和acc_lim_y y轴方向，即垂直于车辆正对方向的速度。对阿克曼底盘的车辆没有意义。
• wheelbase和cmd_angle_instead_rotvel 后者设置为true时，ROS话题（rostopic） cmd_vel/angular/z 内的数据是舵机角度，其值满足katex is not defined 此时应设置wheelbase即前后轮距离。前后轮距离d与速度、角速度和舵机角度之间的关系是：katex is not defined（注：车辆运动学中心是后轮中点，速度除以角速度所得半径也是以这里为准的）
• min_turning_radius 最小转弯半径
• 为避免规划出不可能实现的移动路径，请动手测量实际车辆的转弯半径。此参数事实上约束了舵机的最大转角。有些车辆转向性能不佳，前轮实际转过角度小于舵机角度，则应当给指令转角乘上一增益后再控制舵机，否则车辆将总是不能实现设置的最小转弯半径。请注意此项应当与最大角速度配合设置。
• max_vel_theta和acc_lim_theta 角速度和角加速度约束
• 正确配置下最小转弯半径应在低速时生效，而角速度限制应作用在车辆高速行驶时。角速度约束可以防止高重心转弯过快翻车，角加速度限制可以避免车轮打滑，车辆失控。对于低速平稳运行的车辆可以不约束此两项，将二者设为一个足够大的值即可。
• xy_goal_tolerance和yaw_goal_tolerance 目标跟踪误差
• 根据车辆运行精度设置。例如，笔者使用一台攀爬RC车构建导航系统，这种车辆转向间隙特性很大，则不应设置严格的航向限制。
• free_goal_vel 自由目标速度
• 设为False时，车辆到达终点时的目标速度为0。我们已经指出，TEB是时间最优规划器。缺少目标速度约束将导致车辆“全速冲线”，即使前方有一堵墙也是如此（因为撞墙的时刻不在规划器考虑范围内了）。不过此选项在竞速比赛中比较有用。
• dt_ref和dt_hysteresis 最优路径上的两个相邻姿态（即位置、速度、航向信息，可通过TEB可视化在rivz中看到）的默认距离
• 此距离不固定，规划器自动根据速度大小调整这一距离，速度越大，相邻距离自然越大。笔者认为，将dt_ref设置为与车辆长度大致是可行的。较小的值理论上可提供更高精度，但笔者并未进行验证。对于一般的模型车而言，不需要改变默认配置。当相邻姿态距离和dt_ref的差超过正负dt_hysteresis时，规划器将改变这一距离。
• global_plan_overwrite_orientation 覆盖全局路径的方向是预留给3D路径规划使用的
• 对于车辆的2D规划，可以设置为False，可实现对全局路径的更好跟踪。
• global_plan_viapoint_sep 从全局规划路径中提取的路点的相互距离
• 默认设置下为Disable，即只沿全局规划向前寻找，找到的离开局部规划器规划范围前的最后一个点作为局部目标。若全局规划特殊，在无障碍空间仍不走直线（例如，工作中的扫地机器人），则需要将此值设置为设置为一个小的正数。
• max_global_plan_lookahead_dist 最大向前看距离
• 此距离1.应随车辆最大速度的增大而增大 2.不应超过激光雷达等传感器的可靠测量范围 3. 不应超过局部耗费地图的大小，即不能要求TEB对局部耗费地图以外的部分进行规划。
• include_costmap_obstacles 必须设置为True后才能规避实时探测到的、建图时不存在的障碍物。
• min_obstacle_dist 最小障碍物距离。若设置了车辆的轮廓，则不建议使用此参数。使用车辆轮廓配置footprint_model配合膨胀距离即可实现较好效果。障碍物惩罚权重很高，TEB不会违背这一最小距离约束，因此会把可以通过的缝隙视作不能通过，并终止运行。请注意，此处设置的机器人轮廓必须与局部耗费地图中的一致，后文将继续讨论。
• inflation_dist 障碍物膨胀距离
• 此膨胀只是降低通过这些区域的优先级，不应当用此距离迫使车辆远离障碍物。

惩罚权重配置

在TEB规划器中，除了撞击障碍物不被允许外，其余的约束在没有可行方案时可被打破（如规划出事实上不可行的转弯半径）。对于权重配置，向读者介绍以下几种策略：

• 提高penalty_epsilon。此项为速度等约束提供类似膨胀层的缓冲效果。接近限速将产生一定的惩罚值。
• 大幅提高weight_optimaltime（最优时间权重）。提高至3-5时，车辆在直道上快速加速，并靠近路径边缘沿切线过弯。
• 大幅降低weight_optimaltime，设置较大的inflation_dist，并稍微增加weight_inflation。时间权重降低至0.3以下时，车辆速度明显平稳，不会向最高速度加速。膨胀距离大时将远离障碍物行驶。
• 大幅增加weight_kinematics_forward_drive（>100），禁止规划倒车。如仍规划倒车，说明找不到可行的前向行驶路径。若路径确实存在，可以检查转弯半径、各种footprint是否正确，膨胀层是否合理。

TEB中的恢复措施

恢复措施可以尝试将卡在杂物中的机器人或路径规划错误的机器人恢复至正常状态。TEB Local Planner实现了由move_base规定的振荡恢复方法。但是，实测中控制器将高频率振荡速度指令（>10Hz），读者应当评估自己的电动机能否承受。（可在 move_base配置中关闭）

TEB提供路径规划不可行（plan not feasible）时的恢复措施，称为shrink horizon backup。此时TEB将以更近的点作为规划目标，尝试重新规划出可行路径。调试时可关闭，以在可视化界面上观察原出错路径。

Costmap Converter

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

ROS WIKI：costmap_converter

Local Costmap 配置指导

Costmap的一般配置方法请参考相应官方文档。本文着重介绍为TEB规划器配置时的注意事项。

footprint配置：配置车辆/机器人的多边形外形。此外形应与TEB中的外形一致，或者至少在大小上没有太大的差别。注意costmap中的footprint设置是有默认值的，因此读者必须手动设置此配置为合适的值。

costmap layers: 局部耗费地图需要有来自地图的静态层和来自传感器的动态层，缺一不可。笔者在最初尝试配置时忽略了静态层，此时TEB将无视全局地图中的障碍。许多传感器，例如2D激光雷达，不可能探测到实时障碍物（地图中没有的）背后的静态障碍物（地图中有的），TEB规划器可能规划出绕过前方障碍物，并穿过当前没有见到的障碍物的错误路径。速度较快时可能来不及在发现障碍物时改正。鉴于TEB已有障碍物膨胀功能，此处可以不再添加膨胀层。

局部耗费地图的更新频率应当不低于规划器和costmap_conveter的转换频率，否则显然是没有意义的。同时，局部耗费地图应当开启滑动窗口（rolling window）模式，窗口大小也不应大于传感器的探测距离。

关于多路径并行规划(Parallel Planning in distinctive Topologies)

此功能将严重影响规划器性能。当路径中存在大量不连续分布的障碍物（如锥桶）并产生大量可行路径时，此功能有良好的效果。相反，若在单一路径上运行，则没有必要时用此功能。具体配置笔者并未做更多研究，还留待读者自行探索。

三、teb_local_planner安装及使用

转载文章链接：teb_local_planner安装及使用

1、安装依赖：（ros：kenetic）

rosdep install teb_local_planner

2、github上下载源码：

https://github.com/rst-tu-dortmund/teb_local_planner

3、放入工作空间的/src文件夹中

4、（单独编译teb_local_planner）编译需要较长的编译时间

catkin_make -DCATKIN_WHITELIST_PACKAGES=“源码存放的文件夹名字”

5、查看teb_local_planner的plugin是否安装完成

rospack plugins --attrib=plugin nav_core

如果能查询到teb_local_planner，则表明以及准备就绪。值得一提的是，teb_local_planner源码中关于plugin的配置文件均已写好，直接编译源码就能完成plugin的注册及插入，非常方便。

6、新建teb_local_planner配置文件teb_local_planner_params.yaml

  teb_local_planner使用方法和base_local_planner一样

git clone https://github.com/rst-tu-dortmund/teb_local_planner_tutorials.git 

注意：由于使用机器人odom为差动模式的，故选取diff_drive中的。（这个是原文说明，但是我们的是阿克曼小车）

7、将teb_local_planner_params.yaml和navigation中其他的配置文件（如costmap_common_params.yaml等）放在一起，方便查阅调用。

8、在启动navigation的launch文件中load上述配置文件

<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
     ...
    <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
     ...
</node>
 teb_local_planner将navigation里的base_local_planner替换，故其作用机制和base_local_planner一样为ros的plugin机制

9、启动navigation，在rviz中将三条path调出来，即可观察机器人导航过程中规划路径的变化。

可以通过rosrun rqt_reconfigure rqt_reconfigure动态调节参数使机器人导航性能达到最佳。关于参数调节的方法请参考其ros官方文档。实际使用的经历来看，采用从teb_local_planner_tutorials下载下来的默认参数达到的性能已经远优于原始navigation中的路径规划算法的性能。

四、ROS下阿克曼转向结构车型导航应用实践（teb_local_planner）

原文链接：ROS下阿克曼转向结构车型导航应用实践（teb_local_planner）

ROS下的导航中local planner多数都针对两轮差速结构设计的，阿克曼转向结构由于结构的特点会有最小转向半径的限制，所以常见的local planner例如base_local_planner，dwa_local_planner等在这里都不适用里，这里使用的是teb_local_planner
它提供了一种car-like的动力学模型（即有最小转向半径限制），这样他所规划的路径也就符合了阿克曼结构的运动特点了

官网提供的教程：teb_local_planner/Tutorials

其中：Planning for car-like robots：teb_local_planner/Tutorials/Planning for car-like robots

跑下来效果还可以，直接上车测试，把Tutorials中类车结构下的配置文件移植到我的车上

主要修改下面几点：

• costmap_common_params.yaml中的footprint为实体机器人的实际尺寸信息
• global_costmap_params.yaml中的robot_base_frame为你机器人的基座标名称
• local_costmap_params.yaml中的robot_base_frame为你机器人的基座标名称
• local_costmap_params.yaml中的width和height这个参数默认设置为int

修改teb_local_planner_params.yaml中的内容

主要要设置车的速度，角速度，转向半径和车外轮廓安全尺寸

两个坑：

1. cmd_angle_instead_rotvel这个参数，这个参数的作用是在发布的cmd_vel（geometry_msgs/Twist）话题中将Z轴角速度的数值替换为车体的转向角度，个人理解这个纯粹是为了兼容Stage仿真器设置的，有点不伦不类的感觉，我的机器人是可以接受geometry_msgs/Twist或者ackermann_msgs/AckermannDrive类型消息，唯独不接受这种奇怪的消息，所以这个参数要设置为False以发布正常的geometry_msgs/Twist消息
2. footprint_model设置：去官网上找了一下具体的含义：wiki    

问题出现：

这个是博主的问题，但是我也遇到过！！！

[ WARN] [1567946341.717090426, 13.700000000]: Control loop missed its desired rate of 5.0000Hz... the loop actually took 1.0000 seconds
[ WARN] [1567946341.718131483, 13.700000000]: Map update loop missed its desired rate of 5.0000Hz... the loop actually took 0.7000 seconds

根据博主的测试：

这个链接是：Tutorials Wiki

长答案我总结一下：

本地costmap_2d配置（强烈建议使用滚动窗口！）：

• weight/height：局部成本图的大小：表示最大轨迹长度以及要考虑的占用单元数（对计算时间有重大影响，但是如果太小：较短的预测/规划范围会降低自由度，例如，为了避开障碍物）
• resolution：解析局部成本图：精细的解析度（较小的值）意味着存在许多需要优化的障碍（对计算时间的重大影响）

障碍物/所述的Costmap参数teb_local_planner：

通过注册/激活costmap_converter插件，也可以将costmap障碍物预处理移到另一个线程中。这些插件旨在将成本图单元（许多点障碍）转换为几何图元（点，线，多边形）。也可以过滤冗余单元或障碍物内部的单元。但是到目前为止，可用的转换插件仍处于试验阶段，并且有许多更有效的方法可以预处理成本图。请参阅本教程。如果有人有兴趣贡献力量，可以使用pluginlib轻松集成更多插件。

今天的文章网上收集关于Teb Local Planner的调试使用分享到此就结束了，感谢您的阅读，如果确实帮到您，您可以动动手指转发给其他人。