Teb安装以及调参技巧（适合小白）

最新推荐文章于 2024-06-06 09:54:17 发布

努力glow .

最新推荐文章于 2024-06-06 09:54:17 发布

阅读量4.7k

点赞数 44

文章标签：算法机器学习人工智能自动驾驶开源 c++

本文链接：https://blog.csdn.net/qq_52383168/article/details/135869404

版权

Teb安装以及调参技巧

一. 安装teb

1. 二进制安装

安装teb_local_planner(替换成自己的ros版本)
```
sudo apt-get install ros-melodic-teb-local-planner
sudo apt-get install ros-melodic-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
```

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

2.源码安装

安装依赖
```
rosdep install teb_local_planner
```

github上下载源码

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

放入工作空间/src文件下
单独编译teb
```
catkin_make -DCATKIN_WHITELIST_PACKAGES=“源码存放的文件夹名字”
```
==注意：==有时候功能包之间有联系，注意是一些第三方库的依赖，就必须删除原来的build，devel重新整体编译。
查看teb_local_planner的plugin是否安装完成
```
rospack plugins --attrib=plugin nav_core
```
如果能查询到teb_local_planner，则表明以及准备就绪。值得一提的是，teb_local_planner源码中关于plugin的配置文件均已写好，直接编译源码就能完成plugin的注册及插入，非常方便。
启动navigation，在rviz中将三条path调出来，即可观察机器人导航过程中规划路径的变化。
新建teb_local_planner配置文件teb_local_planner_params.yaml
```
git clone https://github.com/rst-tu-dortmund/teb_local_planner_tutorials.git 
```
如果克隆失败，尝试把https改成http重新克隆，如果还是不成功，直接进入链接地址下载即可。

可以参考我上传的附件中的参数配置文件，重新配置参数即可

二. 参数

参数	类型	含义	最小	默认	最大
teb_autosize	bool	优化期间允许改变轨迹的时域长度	False	True	True
dt_ref	double	局部路径规划的解析度	0.01	0.3	1.0
dt_hysteresis	double	允许改变的时域解析度的浮动范围，一般为 dt_ref 的 10% 左右;	0.002	0.1	0.5
global_plan_overwrite_orientation	bool	覆盖全局路径中局部路径点的朝向，有些全局规划者在开始和全局目标之间没有考虑局部子目标的方向，因此自动确定	False	True	True
allow_init_with_backwards_motion	bool	允许在开始时想后退来执行轨迹，如果为true，则在目标在本地成本图中落后于起点的情况下，可以使用向后运动来初始化基础轨迹（仅在机器人配备了后部传感器的情况下才建议这样做）	False	False	True
max_global_plan_lookahead_dist	double	考虑优化的全局计划子集的最大长度（累积欧几里得距离）（如果为0或负数：禁用；长度也受本地Costmap大小的限制）	0.0	3.0	50.0
force_reinit_new_goal_dist	double	如果上一个目标的间隔超过指定的米数（跳过热启动），则强制规划器重新初始化轨迹	0.0	1.0	10.0
feasibility_check_no_poses	int	检测位姿可到达的时间间隔	0	5	50
exact_arc_length	bool	如果为真，规划器在速度、加速度和转弯率计算中使用精确的弧长[->增加的CPU时间]，否则使用欧几里德近似	False	False	True
publish_feedback	bool	发布包含完整轨迹和活动障碍物列表的规划器反馈	False	False	True
visualize_with_time_as_z_axis_scale	double	如果该值大于0，则使用该值缩放的Z轴的时间在3D中可视化轨迹和障碍物。最适用于动态障碍。	0.0	0.0	1.0
global_plan_viapoint_sep	double	从全局计划中提取的每两个连续通过点之间的最小间隔[如果为负：禁用]	-0.1	-0.1	5.0
via_points_ordered	bool	如果为真，规划器遵循存储容器中通过点的顺序。	False	False	True
max_vel_x	double	最大x前向速度	0.01	0.4	100.0
max_vel_x_backwards	double	最大x后退速度	0.01	0.2	100.0
max_vel_theta	double	最大转向叫速度	0.01	0.3	100.0
acc_lim_x	double	最大x加速度	0.01	0.5	100.0
acc_lim_theta	double	最大角速度	0.01	0.5	100.0
is_footprint_dynamic	bool	是否footprint 为动态的，如果为true，则在检查轨迹可行性之前更新覆盖区	False	False	True
min_turning_radius	double	车类机器人的最小转弯半径，小型机器人的最小转弯半径（差速驱动机器人：零）	0.0	0.0	50.0
wheelbase	double	驱动轴和转向轴之间的距离（仅适用于启用了“Cmd_angle_而不是_rotvel”的Carlike机器人）；对于后轮式机器人，该值可能为负值！	-10.0	1.0	10.0
cmd_angle_instead_rotvel	bool	将收到的角速度消息转换为操作上的角度变化。	False	False	True
max_vel_y	double	最大y方向速度，机器人的最大起步速度（对于非完整机器人，应为零！）	0.0	0.0	100.0
acc_lim_y	double	最大y向加速度，机器人的最大加速度	0.01	0.5	100.0
xy_goal_tolerance	double	目标 xy 偏移容忍度，到目标位置的最终欧几里得距离	0.001	0.2	10.0
yaw_goal_tolerance	double	目标角度偏移容忍度	0.001	0.1	3.2
free_goal_vel	bool	允许机器人以最大速度驶向目的地，出于计划目的，允许机器人的速度不为零（机器人可以以最大速度到达目标）	False	False	True
min_obstacle_dist	double	和障碍物最小距离	0.0	0.5	10.0
inflation_dist	double	障碍物膨胀距离，惩罚成本不为零的障碍物周围的缓冲区（应大于min_obstacle_dist才能生效）	0.0	0.6	15.0
dynamic_obstacle_inflation_dist	double	动态障碍物的膨胀范围，动态障碍物的预测位置周围的缓冲区的惩罚成本非零（应大于min_obstacle_dist才能生效）	0.0	0.6	15.0
include_dynamic_obstacles	bool	是否将动态障碍物预测为速度模型，指定是否动态障碍物的运动应该被一个恒速模型预测(这也改变了同伦类搜索)。如果为false，则认为所有的障碍都是静态的。	False	False	True
include_costmap_obstacles	bool	costmap 中的障碍物是否被直接考虑	False	True	True
legacy_obstacle_association	bool	是否严格遵循局部规划出来的路径，如果为true，则使用旧的关联策略（对于每个障碍，找到最近的TEB姿势），否则使用新的关联策略（对于每个teb姿势，仅找到“相关”障碍）。	False	False	True
obstacle_association_force_inclusion_factor	double	non-legacy障碍关联技术只尝试连接相关障碍与离散轨迹优化、specifed距离内的所有障碍被迫被包括(如min_obstacle_dist的倍数),如选择2.0为了考虑在半径为2.0 * min_obstacle_dist障碍。	0.0	1.5	100.0
obstacle_association_cutoff_factor	double	参见obstacle_association_force_inclusion_factor，但如果超过[value]*min_obstacle_dist的倍数，优化过程中所有障碍都将被忽略。首先处理obstacle_association_force_inclusion_factor。	1.0	5.0	100.0
costmap_obstacles_behind_robot_dist	double	限制在机器人后面规划时考虑到的占用的本地成本图障碍(指定距离，单位为米)	0.0	1.5	20.0
obstacle_poses_affected	int	障碍物位置与轨迹上最接近的姿态相连，以减少计算量，但同时也考虑了许多相邻的障碍物	0	30	200
no_inner_iterations	int	被外循环调用后内循环执行优化次数	1	5	100
no_outer_iterations	int	执行的外循环的优化次数	1	4	100
optimization_activate	bool	激活优化	False	True	True
optimization_verbose	bool	打印优化过程详情	False	False	True
penalty_epsilon	double	对于硬约束近似，在惩罚函数中添加安全范围	0.0	0.1	1.0
weight_max_vel_x	double	最大x速度权重	0.0	2.0	1000.0
weight_max_vel_y	double	最大y速度权重	0.0	2.0	1000.0
weight_max_vel_theta	double	最大加速度权重	0.0	1.0	1000.0
weight_acc_lim_x	double	最大x 加速度权重	0.0	1.0	1000.0
weight_acc_lim_y	double	最大y 加速度权重	0.0	1.0	1000.0
weight_acc_lim_theta	double	最大角速度权重	0.0	1.0	1000.0
weight_kinematics_nh	double	满足非完整运动学的优化权值	0.0	1000.0	10000.0
weight_kinematics_forward_drive	double	优化过程中，迫使机器人只选择前进方向，差速轮适用	0.0	1.0	1000.0
weight_kinematics_turning_radius	double	优化过程中，车型机器人的最小转弯半径的权重	0.0	1.0	1000.0
weight_optimaltime	double	优化过程中，基于轨迹的时间上的权重	0.0	1.0	1000.0
weight_obstacle	double	优化过程中，和障碍物最小距离的权重	0.0	50.0	1000.0
weight_inflation	double	优化过程中，膨胀区的权重	0.0	0.1	10.0
weight_dynamic_obstacle	double	优化过程中，和动态障碍物最小距离的权重	0.0	50.0	1000.0
weight_dynamic_obstacle_inflation	double	优化过程中，和动态障碍物膨胀区的权重	0.0	0.1	10.0
weight_viapoint	double	优化过程中，和全局路径采样点距离的权重	0.0	1.0	1000.0
weight_adapt_factor	double	在每次外部TEB迭代中，一些特殊权重（当前为“ weight_obstacle”）会以此因子重复缩放（weight_new：weight_old * factor）；迭代地增加权重而不是设置较大的先验值会导致底层优化问题的更好的数值条件。	1.0	2.0	100.0
enable_multithreading	bool	允许多线程并行处理	False	True	True
max_number_classes	int	允许的线程数	1	5	100
selection_cost_hysteresis	double	指定新候选人必须有多少轨迹成本先前选择的轨迹以进行选择（如果new_cost	0.0	1.0	2.0
selection_prefer_initial_plan	double	为初始计划的等价类中的轨迹指定在区间(0,1)中的成本降低。	0.0	0.95	1.0
selection_obst_cost_scale	double	为了选择“最佳”候选人，障碍成本项的额外缩放(new_obst_cost: obst_cost*factor)	0.0	100.0	1000.0
selection_viapoint_cost_scale	double	仅为了选择“最佳”候选人，就可以额外增加通点成本条款。（new_viapt_cost：viapt_cost * factor）	0.0	1.0	100.0
selection_alternative_time_cost	bool	如果为true，则将时间成本替换为总过渡时间。	False	False	True
switching_blocking_period	double	指定允许切换到新的等效类之前需要终止的持续时间（以秒为单位）	0.0	0.0	60.0
roadmap_graph_no_samples	int	如果simple_exploration被turend关闭，请指定为创建路线图图而生成的示例数量	1	15	100
roadmap_graph_area_width	double	指定采样区域在开始和目标[m]之间的宽度(高度等于开始和目标的距离)	0.1	5.0	20.0
roadmap_graph_area_length_scale	double	矩形区域的长度取决于起点和目标之间的距离。此参数进一步缩放距离，以使几何中心保持相等！）	0.5	1.0	2.0
h_signature_prescaler	double	缩放障碍物数值，允许设置大量障碍物。不要选得太低，否则无法区分障碍物(0.2	0.2	1.0	1.0
h_signature_threshold	double	如果实部和复部的差都低于指定的阈值，则假设两个h-符号相等	0.0	0.1	1.0
obstacle_heading_threshold	double	指定障碍物航向和目标航向之间的标准化标量积的值，以便将它们(障碍物)考虑到探索中)	0.0	0.45	1.0
viapoints_all_candidates	bool	如果为true，则将不同拓扑的所有轨迹附加到该组通孔点，否则仅附加与初始/全局计划共享同一轨迹的轨迹（在test_optim_node中无效）。	False	True	True
visualize_hc_graph	bool	可视化为探索新同伦类而创建的图	False	True	True
shrink_horizon_backup	bool	当规划器检测到系统异常，允许缩小时域规划范围	False	True	True
oscillation_recovery	bool	尝试检测和解决振荡，尝试检测并解决相同等价类中多个解决方案之间的振荡（机器人经常在左/右/前/后退之间切换）。	False	True	True

三. 重要参数调试

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 障碍物膨胀距离
此膨胀只是降低通过这些区域的优先级，不应当用此距离迫使车辆远离障碍物。

1. 惩罚权重配置

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

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

2. TEB中的恢复措施

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

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

3. Costmap Converter

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

在这讲一下如何安装此插件：

插件激活

rospack plugins --attrib=plugin costmap_converter

配置参数文件

在原来的teb参数文件的目标下，新建costmap_converter_params.yaml

cd xxx
touch costmap_converter_params.yaml  //新建文件

tip：在上面的teb源码安装的第七步下载的文件中也有这个插件配置文件。

如果没有，文件内容如下：

###########################################################################################
## NOTE: Costmap conversion is experimental. Its purpose is to combine many point        ##
## obstales into clusters, computed in a separate thread in order to improve the overall ## 
## efficiency of local planning. However, the implemented conversion algorithms are in a ##
## very early stage of development. Contributions are welcome!                           ##
###########################################################################################

TebLocalPlannerROS:

  ## Costmap converter plugin   
  # costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH"
  costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSRANSAC"
  #costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSMCCH"
  #costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSConcaveHull"
  costmap_converter_spin_thread: True
  costmap_converter_rate: 5
 
 
  ## Configure plugins (namespace move_base/costmap_to_lines or move_base/costmap_to_polygons)
  ## costmap_converter/CostmapToLinesDBSRANSAC, costmap_converter/CostmapToLinesDBSMCCH, costmap_converter/CostmapToPolygonsDBSMCCH
  costmap_converter/CostmapToLinesDBSRANSAC:
    cluster_max_distance: 0.4
    cluster_min_pts: 2
    ransac_inlier_distance: 0.15
    ransac_min_inliers: 10
    ransac_no_iterations: 1500
    ransac_remainig_outliers: 3
    ransac_convert_outlier_pts: True
    ransac_filter_remaining_outlier_pts: False
    convex_hull_min_pt_separation: 0.1

在launch文件中增加costmap_converter启动项（准确来说是加载move_base节点的launch文件下）下面文件不完整，只是举个例子

<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">

    <!-- Load all costmap_2d and planner parameters here -->

    <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />


    <!-- LOAD COSTMAP_CONVERTER PARAMETERS HERE -->
    <rosparam file="$(find 自己的路径)/costmap_converter_params.yaml" command="load" />

</node>

通过注册/激活costmap_converter插件，也可以将costmap障碍物预处理移到另一个线程中。这些插件旨在将成本图单元（许多点障碍）转换为几何图元（点，线，多边形）。

4. 上车测试

直接上车测试，把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

5. 修改`teb_local_planner_params.yaml`中的内容

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

两个坑：

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

在这里插入图片描述

多边形会非常耗费算力，我选用的是line。

6. TEB规划器的性能问题

在本文的最后一部分，将总结一些可以提高TEB规划器性能的参数调整策略，供遇到与我类似问题的读者参考。

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

四. 问题解决

1. 算力不够的问题

[ 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

这里先看一下其他博主的解决办法

在这里插入图片描述

这里提一下 max_number_classes这个是多路径一般设置为2(规避障碍物左右两条路)，或者直接不开启，规划的路径越多越耗费算力。

这里放一下我总结的方案：(字比较丑，请担待)
在这里插入图片描述

2.原地转圈

路径规划的过程中，小车原地转圈，但是全局路径正常

解决方案：

检查local cost map参数，height与width是否太小！如果参数设置过小的话，局部路径无法生成就会原地打转。（注意：height和width是int型，很多人都会忽略这一点）

3.代价地图问题

本地代价地图的分辨率和大小（请参阅local_costmap_params.yaml）会显着影响优化的性能，因为每个占用的代价地图单元都被视为单点障碍。对于大量障碍，由于数值不稳定性，在独特拓扑中的并行规划可能会失败。在这种情况下，尝试增加分辨率或减小大小，直到修复或绕过此问题（否则可能会尝试稍微减小h_signature_prescaler的值）。