ROS与navigation教程-teb_local_planner

ROS与navigation教程-teb_local_planner

说明：

• 介绍了teb_local_planner的概念和参数

参考代码

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

概要

• teb_local_planner包是2D导航功能包中base_local_planner的插件实现。

• Timed Elastic Band 基础方法在轨迹执行时间，障碍物分离以及运行时遵守kinodynamic约束方面，局部优化了机器人的轨迹。

• 该软件包实现了一种在线优化的局部轨迹规划器，作为ROS导航包的插件用于导航和控制移动机器人。

• 在运行时，优化由全局路径规划器生成的初始轨迹，以便最小化轨迹执行时间（时间最优目标），与障碍物分离，并遵守诸如满足最大速度和加速度的动力学约束。

• 目前的实现符合非全向轮机器人（差速驱动机器人和四驱驱动机器人）的运动学。 （在Kinetic版本开始，该包支持全向轮机器人）

• 通过求解sparse scalarized multi-objective优化问题有效地获得了最优轨迹。 用户可以为优化问题设置权重，以便在目标冲突的情况下指定操作。

• “Timed-Elastic-Band”方法在：

  • Rösmann C., Feiten W., Wösch T., Hoffmann F. and Bertram. T.: Trajectory modification considering dynamic constraints of autonomous robots. Proc. 7th German Conference on Robotics, Germany, Munich, 2012, pp 74–79.

  • Rösmann C., Feiten W., Wösch T., Hoffmann F. and Bertram. T.: Efficient trajectory optimization using a sparse model. Proc. IEEE European Conference on Mobile Robots, Spain, Barcelona, 2013, pp. 138–143.

• 局部路径规划器如Timed Elastic Band 经常被卡住在局部最优轨迹上，因为他们无法跨越障碍物。所以它进行了扩展，并行优化不同拓扑的可通过轨迹的子集。局部规划器能够切换到候选集中的当前全局最优轨迹，同时通过利用同源/同伦类的概念获得不同的拓扑。

• 将会很快提供更多有关此扩展的信息。 相关文章已发表： Rösmann C., Hoffmann F. and Bertram T.: Planning of Multiple Robot Trajectories in Distinctive Topologies, Proc. IEEE European Conference on Mobile Robots, UK, Lincoln, Sept. 2015

• Use GitHub to report bugs or submit feature requests. View active issues

Video

• 以下视频介绍了该软件包的功能，并显示了仿真和真实机器人情况下的示例。

• youtube 视频链接：

• 以下视频介绍了0.2版本的功能(支持四驱驱动机器人和costmap转换):

• youtube 视频链接：

Node API

( 1 ) Topics

( 1.1 ) Published Topics

~<name>/global_plan ([nav_msgs/Path][4]) 

• Global plan that the local planner is currently attempting to follow.

• 主要用于可视化。

~<name>/local_plan ([nav_msgs/Path][5]) 

• teb_local_planner优化和跟踪的局部路径规划或轨迹。

• 主要用于可视化。

~<name>/teb_poses ([geometry_msgs/PoseArray][6]) 

• 当前局部路径规划的（discrete pose list）离散位姿列表（SE2）。

• 主要用于可视化。

~<name>/teb_markers ([visualization_msgs/Marker][7]) 

• teb_local_planner通过具有不同命名空间的标记来提供规划场景的附加信息。

• 命名空间 PointObstacles 和 PolyObstacles : 可视化在优化时当前考虑的所有点和多边形障碍物。

• 可视化替代拓扑中所有找到的和优化的轨迹（仅当启用并行计划时）。

• 将会发布更多的信息，如优化足迹模型。

~<name>/teb_feedback ([teb_local_planner/FeedbackMsg][10]) 

• 反馈消息含有包括速度曲线和时间信息以及障碍物列表的已规划轨迹。

• 主要用于评估和调试。

• 需启用 ~/publish_feedback 参数。

( 1.2 ) Subscribed Topics

~<name>/odom ([nav_msgs/Odometry][11]) 

• 给局部路径规划器提供机器人当前速度的里程计信息。

• 可以通过重新映射或修改参数〜 / odom_topic来更改此话题。

~<name>/obstacles ([teb_local_planner/ObstacleMsg][12]) 

• 提供自定义的障碍物如点，线或多边形（另外或可代替costmap的障碍）。

( 2 ) Parameters

• teb_local_planner包允许用户设置参数以便自定义机器人的行为。

• 这些参数分为几类：机器人配置，目标公差，轨迹配置，障碍，优化，特殊拓扑中的规划和其他参数。

• 其中一些参数和base_local_planner包中的参数是一样的，许多（但不是全部）参数可以在运行时使用rqt_reconfigure进行修改。

( 2.1 ) Robot Configuration Parameters

~<name>/acc_lim_x (double, default: 0.5) 

• 机器人的最大线加速度，单位为 meters/sec^2 。

~<name>/acc_lim_theta (double, default: 0.5) 

• 机器人的最大角加速度，单位为 radians/sec^2 。

~<name>/max_vel_x (double, default: 0.4) 

• 机器人的最大平移速度，单位为 meters/sec

~<name>/max_vel_x_backwards (double, default: 0.2) 

• 当向后移动时，机器人的最大绝对平移速度，单位为 meters/sec 。

• See optimization parameter weight_kinematics_forward_drive.

~<name>/max_vel_theta (double, default: 0.3) 

• 机器人的最大角速度，单位为 radians/sec 。

以下参数适用于四驱驱动机器人：

~<name>/min_turning_radius (double, default: 0.0) 

• 四驱驱动机器人的最小转向半径（差速驱动机器人设置为零）。

~<name>/wheelbase (double, default: 1.0) 

• 后驱动轴与前驱动轴之间的距离。

• The value might be negative for back-wheeled robots （仅当〜 / cmd_angle_instead_rotvel设置为true时才需要）。

~<name>/cmd_angle_instead_rotvel (bool, default: false) 

• Substitute the rotational velocity in the commanded velocity message by the corresponding steering angle [-pi/2,pi/2]. Note, changing the semantics of yaw rate depending on the application is not preferable. Here, it just complies with the inputs required by the stage simulator. Datatypes in ackermann_msgs are more appropriate, but are not supported by move_base. The local planner is not intended to send commands by itself.

以下参数适用于全向轮机器人:

• Note, reduce ~/weight_kinematics_nh significantly in order to adjust the tradeoff between usual driving and strafing.

~<name>/max_vel_y (double, default: 0.0) 

• Maximum strafing velocity of the robot (should be zero for non-holonomic robots!)

~<name>/acc_lim_y (double, default: 0.5) 

• Maximum strafing acceleration of the robot

以下是与用于优化的足迹模型相关的参数：

• (see Tutorial Obstacle Avoidance and Robot Footprint Model). New in version 0.3

~<name>/footprint_model/type (string, default: "point") 

• 指定用于优化的机器人足迹模型的类型。

• 有如“点”，“圆形”，“线”，“two_circles”和“多边形”的不同类型。

• 但其模型的类型对所需的计算时间有显著的影响。

~<name>/footprint_model/radius (double, default: 0.2) 

• 该参数是仅用于“圆形“类型的模型，其包含了圆的半径。

• 圆的中心位于机器人的旋转轴（axis of rotation）上。

~<name>/footprint_model/line_start (double[2], default: [-0.3, 0.0]) 

• 该参数是仅用于“线“类型的模型，其包含了里线段的起始坐标。

~<name>/footprint_model/front_offset (double, default: 0.2) 

• 该参数是仅用于“two_circles“类型的模型，其描述了前圆（front circle）的中心沿机器人的x轴移动了多少。假设机器人的旋转轴（axis of rotation）位于[0,0]处。

~<name>/footprint_model/front_radius (double, default: 0.2) 

• 该参数是仅用于“two_circles“类型的模型，其包含了前圆（front circle）的半径。

~<name>/footprint_model/rear_offset (double, default: 0.2) 

• 该参数是仅用于“two_circles“类型的模型，其描述了后圆（rear circle）的中心沿机器人的负x轴移动多少。假设机器人的旋转轴（axis of rotation）位于[0,0]处。

~<name>/footprint_model/rear_radius (double, default: 0.2) 

• 该参数是仅用于“two_circles“类型的模型，其包含了后圆（rear circle）的半径。

~<name>/footprint_model/vertices (double[], default: [ [0.25,-0.05], [...], ...]) 

• 该参数是仅用于“多边形“类型的模型，其包含了多边形顶点列表（每个2d坐标）。

• 多边形总是闭合的：不要在最后重复第一个顶点。

( 2.2 ) Goal Tolerance Parameters

~<name>/xy_goal_tolerance (double, default: 0.2) 

• 允许的机器人到目标位置的最终欧氏距离（euclidean distance），单位为 meters 。

~<name>/yaw_goal_tolerance (double, default: 0.2)

• Allowed final orientation error in radians

~<name>/free_goal_vel (bool, default: false) 

• 去除目标速度的约束，让机器人可以以最大速度到达目标。

• Remove the goal velocity constraint such that the robot can arrive at the goal with maximum speed.

( 2.3 ) Trajectory Configuration Parameters

~<name>/dt_ref (double, default: 0.3) 

• Desired temporal resolution of the trajectory (the trajectory is not fixed to dt_ref since the temporal resolution is part of the optimization, but the trajectory will be resized between iterations if dt_ref +-dt_hysteresis is violated.

~<name>/dt_hysteresis (double, default: 0.1) 

• Hysteresis for automatic resizing depending on the current temporal resolution, usually approx. 10% of dt_ref is recommended .

~<name>/min_samples (int, default: 3) 

• 最小样本数（始终大于2）

~<name>/global_plan_overwrite_orientation (bool, default: true) 

• 覆盖由全局规划器提供的局部子目标的方向（因为它们通常仅提供2D路径）

~<name>/global_plan_viapoint_sep (double, default: -0.1 (disabled)) 

• 如果为正值，则通过点（via-points ）从全局计划（路径跟踪模式）展开。

• 该值确定参考路径的分辨率（沿着全局计划的每两个连续通过点之间的最小间隔，if negative: disabled）。

• 可以参考参数weight_viapoint来调整大小。New in version 0.4.

• If positive, via-points are extrected from the global plan (path-following mode). The value determines the resolution of the reference path (min. separation between each two consecutive via-points along the global plan, if negative: disabled). Refer to parameter weight_viapoint for adjusting the intensity. New in version 0.4

~<name>/max_global_plan_lookahead_dist (double, default: 3.0) 

• Specify the maximum length (cumulative Euclidean distances) of the subset of the global plan taken into account for optimization. The actual length is than determined by the logical conjunction of the local costmap size and this maximum bound. Set to zero or negative in order to deactivate this limitation.

~<name>/force_reinit_new_goal_dist (double, default: 1.0) 

• Reinitialize the trajectory if a previous goal is updated with a separation of more than the specified value in meters (skip hot-starting)

~<name>/feasibility_check_no_poses (int, default: 4) 

• Specify up to which pose on the predicted plan the feasibility should be checked each sampling interval.

~<name>/publish_feedback (bool, default: false) 

• 发布包含完整轨迹和动态障碍的列表的规划器反馈（应仅用于测试或调试启用）。 查看上面发布者的列表。

~<name>/shrink_horizon_backup (bool, default: true) 

• 允许规划器在自动检测到问题(e.g. infeasibility)的情况下临时缩小horizon（50％）。参考参数shrink_horizon_min_duration。

• Allows the planner to shrink the horizon temporary (50%) in case of automatically detected issues (e.g. infeasibility). Also see parameter shrink_horizon_min_duration.

~<name>/allow_init_with_backwards_motion (bool, default: false) 

• If true, underlying trajectories might be initialized with backwards motions in case the goal is behind the start within the local costmap (this is only recommended if the robot is equipped with rear sensors).

~<name>/exact_arc_length (bool, default: false

• 如果为真，则规划器在速度，加速度和角速度计算中使用确切的弧长度（ - >增加cpu运算时间），否则使用Euclidean approximation。

~<name>/shrink_horizon_min_duration (double, default: 10.0) 

• Specify minimum duration for the reduced horizon in case an infeasible trajectory is detected (refer to parameter shrink_horizon_backup in order to activate the reduced horizon mode).

( 2.4 ) Obstacle Parameters

~<name>/min_obstacle_dist (double, default: 0.5) 

• 与障碍的最小期望距离，单位 meters 。

~<name>/include_costmap_obstacles (bool, default: true) 

• 指定应否考虑到局部costmap的障碍，被标记为障碍物的每个单元格被认为是点障碍物（point-obstacle）。

• 不要选择非常小的costmap分辨率，因为它增加了计算时间。

• 在未来的版本中，这种情况将会得到解决，并为动态障碍提供额外的API。

~<name>/costmap_obstacles_behind_robot_dist (double, default: 1.0) 

• Limit the occupied local costmap obstacles taken into account for planning behind the robot (specify distance in meters).

~<name>/obstacle_poses_affected (int, default: 30) 

• Each obstacle position is attached to the closest pose on the trajectory in order to keep a distance. Additional neighbors can be taken into account as well.

• 注意：在将来的版本中可能会删除该参数，因为障碍物关联算法已经在 kinetic+ 中被修改。

• 可以参考参数 legacy_obstacle_association 的描述。

~<name>/inflation_dist (double, default: pre kinetic: 0.0, kinetic+: 0.6) 

• Buffer zone around obstacles with non-zero penalty costs (should be larger than min_obstacle_dist in order to take effect).

• 可以参考参数 weight weight_inflation 。

<name>/legacy_obstacle_association (bool, default: false) 

• 连接轨迹位姿与优化障碍的策略已被修改（参考changelog）。

• 参数设置为true，既是切换到旧的的策略。 旧战略：对于每个障碍，找到最近的TEB位姿; 新策略：对于每个teb位姿，只找到"relevant" obstacles。

~<name>/obstacle_association_force_inclusion_factor (double, default: 1.5) 

• 在优化过程中，非传统障碍物关联策略试图仅将相关障碍与离散轨迹相连接。 但是，特定距离内的所有障碍都被迫被包含（作为min_obstacle_dist的倍数）。

• 例如。 选择2.0以便在2.0 * min_obstacle_dist的半径范围内强制考虑障碍。 [只有在参数legacy_obstacle_association为false时才使用此参数]

~<name>/obstacle_association_cutoff_factor (double, default: 5) 

• 请参考参数barriers_association_force_inclusion_factor，但超出[value] * min_obstacle_dist的倍数时，优化过程中将忽略所有障碍。

• 首先处理参数barrier_association_force_inclusion_factor。 [只有参数legacy_obstacle_association为false时才使用此参数]

以下参数适用于需要costmap_converter插件（参见教程）的情况下：

~<name>/costmap_converter_plugin (string, default: "") 

• 定义插件名称，用于将costmap的单元格转换成点/线/多边形。

• 若设置为空字符，则视为禁用转换，将所有点视为点障碍。

~<name>/costmap_converter_spin_thread (bool, default: true) 

• 如果为true，则costmap转换器将以不同的线程调用其回调队列。

~<name>/costmap_converter_rate (double, default: 5.0) 

• 定义costmap_converter插件处理当前costmap的频率（该值不高于costmap更新率）,单位为 Hz 。

( 2.5 ) Optimization Parameters

~<name>/no_inner_iterations (int, default: 5) 

• 在每个外循环迭代中调用的实际求解器迭代次数。

• 参考参数 no_outer_iterations。

• Number of actual solver iterations called in each outerloop iteration. See param no_outer_iterations.

~/no_outer_iterations (int, default: 4)

• 每个外循环自动根据期望的时间分辨率dt_ref调整轨迹大小，并调用内部优化器（执行参数no_inner_iterations）。 因此，每个规划周期中求解器迭代的总数是两个值的乘积。

• Each outerloop iteration automatically resizes the trajectory according to the desired temporal resolution dt_ref and invokes the internal optimizer (that performs no_inner_iterations). The total number of solver iterations in each planning cycle is therefore the product of both values.

~<name>/penalty_epsilon (double, default: 0.1) 

• 为硬约束近似的惩罚函数添加一个小的安全范围。

• Add a small safety margin to penalty functions for hard-constraint approximations .

~<name>/weight_max_vel_x (double, default: 2.0) 

• 满足最大允许平移速度的优化权重。

~<name>/weight_max_vel_theta (double, default: 1.0) 

• 满足最大允许角速度的优化权重。

~<name>/weight_acc_lim_x (double, default: 1.0) 

• 满足最大允许平移加速度的优化权重。

~<name>/weight_acc_lim_theta (double, default: 1.0) 

• 满足最大允许角加速度的优化权重。

~<name>/weight_kinematics_nh (double, default: 1000.0) 

• 用于满足 non-holonomic 运动学的优化权重（由于运动学方程构成等式约束，该参数必须很高）

• Optimization weight for satisfying the non-holonomic kinematics (this parameter must be high since the kinematics equation constitutes an equality constraint, even a value of 1000 does not imply a bad matrix condition due to small 'raw' cost values in comparison to other costs).

~<name>/weight_kinematics_forward_drive (double, default: 1.0) 

• 强制机器人仅选择正向（正的平移速度）的优化权重。

• 重量小的机器人仍然可以向后移动。

~<name>/weight_kinematics_turning_radius (double, default: 1.0) 

• 采用最小转向半径的优化权重（仅适用于四驱驱动机器人）

~<name>/weight_optimaltime (double, default: 1.0) 

• Optimization weight for contracting the trajectory with regard to transition/execution time .

~<name>/weight_obstacle (double, default: 50.0) 

• 保持与障碍物的最小距离的优化权重。

~<name>/weight_viapoint (double, default: 1.0) 

• 最小化到通过点的距离(resp. reference path)的优化权重。New in version 0.4.

~<name>/weight_inflation (double, default: 0.1) 

• 膨胀区惩罚的优化权重（值应该很小）。

~<name>/weight_adapt_factor (double, default: 2.0) 

• Some special weights (currently weight_obstacle) are repeatedly scaled by this factor in each outer TEB iteration (weight_new = weight_old*factor). Increasing weights iteratively instead of setting a huge value a-priori leads to better numerical conditions of the underlying optimization problem.

( 2.6 ) Parallel Planning in distinctive Topologies

~<name>/enable_homotopy_class_planning (bool, default: true) 

• 在不同的拓扑里激活并行规划（因为一次优化多个轨迹，所以需要占用更多的CPU资源）

• Activate parallel planning in distinctive topologies (requires much more CPU resources, since multiple trajectories are optimized at once)

~<name>/enable_multithreading (bool, default: true) 

• 激活多个线程，以便在不同的线程中规划每个轨迹。

~<name>/max_number_classes (int, default: 4) 

• 指定考虑到的不同轨迹的最大数量（限制计算量）。

~<name>/selection_cost_hysteresis (double, default: 1.0) 

• Specify how much trajectory cost must a new candidate have w.r.t. a previously selected trajectory in order to be selected (selection if new_cost < old_cost*factor).

~<name>/selection_obst_cost_scale (double, default: 100.0) 

• Extra scaling of obstacle cost terms just for selecting the 'best' candidate.

~<name>/selection_viapoint_cost_scale (double, default: 1.0) 

• Extra scaling of via-point cost terms just for selecting the 'best' candidate. New in version 0.4.

~<name>/selection_alternative_time_cost (bool, default: false) 

• If true, time cost (sum of squared time differences) is replaced by the total transition time (sum of time differences).

~<name>/roadmap_graph_no_samples (int, default: 15) 

• 指定为创建路线图而生成的样本数。

~<name>/roadmap_graph_area_width (double, default: 6) 

• 指定该区域的宽度。

• 在开始和目标之间的矩形区域中采样随机关键点/航点(keypoints/waypoints)。

~<name>/h_signature_prescaler (double, default: 1.0) 

• 缩放用于区分同伦类的内部参数（H-signature）。

• 警告：只能减少此参数，如果在局部costmap中遇到太多障碍物的情况，请勿选择极低值，否则无法将障碍物彼此区分开（0.2 < value <= 1）。

~<name>/h_signature_threshold (double, default: 0.1)

• Two H-signatures are assumed to be equal, if both the difference of real parts and complex parts are below the specified threshold.

~<name>/obstacle_heading_threshold (double, default: 1.0) 

• Specify the value of the scalar product between obstacle heading and goal heading in order to take them (obstacles) into account for exploration.

~<name>/visualize_hc_graph (bool, default: false) 

• 可视化创建的图形，用于探索不同的轨迹（在rviz中检查标记消息）

~<name>/viapoints_all_candidates (bool, default: true) 

• If true, all trajectories of different topologies are attached to the set of via-points, otherwise only the trajectory sharing the same topology as the initial/global plan is connected with them (no effect on test_optim_node). New in version 0.4

( 2.7 ) Miscellaneous Parameters

~<name>/odom_topic (string, default: "odom") 

• odometry消息的主题名称，由机器人驱动程序或仿真器提供。

~<name>/map_frame (string, default: "odom") 

• 全局规划坐标系（在静态地图的情况下，此参数通常必须更改为“/ map”。）。

Roadmap

• 目前为将来所计划的一些功能和改进。 欢迎大家来贡献出自己的一分力！

  • 在不可避免的障碍（例如障碍物十分靠近目标位置）的情况下，增加和改进安全性能。

  • 实施适当的逃离行为。

  • 改进或解决规划器在多个局部最佳解决方案之间的选择问题(不是在拓扑学基础上，而是由于出现干扰等)。

参考资料

• http://wiki.ros.org/teb_local_planner