ROS——Teb算法的优化

一、简介

 “TEB”全称Time Elastic Band（时间弹性带）Local Planner，该方法针对全局路径规划器生成的初始轨迹进行后续修正(modification)，从而优化机器人的运动轨迹，属于局部路径规划。

关于eletic band（橡皮筋）的定义：连接起始、目标点，并让这个路径可以变形，变形的条件就是将所有约束当做橡皮筋的外力。

二、说明

局部路径规划之Teb

起始点、目标带你状态由全局规划，中间插入N个控制节点来改变橡皮筋的状态控制点，在点与点之间定义运动时间Time。

这个路径可以变形，变形的条件就是将所有约束当做橡皮筋的外力

注意每个目标函数只与几个连续状态有关，而非整条band。

 当我们设置目标时，小车的目的地是靠近障碍物的，但是如果我们小于我们的障碍物距离最小距离，我们就需要往外拉，这就是我们的类橡皮筋过程

约束目标函数：

 

 

 

 优化问题：

Teb优化问题实质上是一个优化问题，大多数目标是基于局部的，只与一小部分参数相关，因为他们只依赖于几个连续的机器人

TEB生成的局部轨迹由一系列带有时间信息的离散位姿组成，g2o算法优化的目标就是这些离散的位姿，同时设计一条时间最短，距离最短，远离障碍物等目标，同时限制速度与加速度使轨迹满足机器人运动学。

整体的规划为：

全局路径————加入约束————g2o优化————速度指令

参数：

# Trajectory

teb_autosize: True #优化期间允许改变轨迹时域长度

dt_ref: 0.3 #局部路径规划解析度(0.01~1.0) 默认为0.3 (两个相邻位姿之间的时间_时间分辨率)

(可以设置0.45，其他保持默认)

dt_hysteresis: 0.1 #允许浮动范围

global_plan_overwrite_orientation: True #覆盖全局路 径中局部路点朝向

max_global_plan_lookahead_dist: 3.0 #全局优化子集最大长度

feasibility_check_no_poses: 2 # 检测位姿可达到的时间间隔

# Robot _ 下面的这些参数就会切实的影响规划

max_vel_x: 0.7  #最大前进速度 x  (可以设置成0.5尝试)
 max_vel_x_backwards: 0.3 #最大后退速度 x 这个值不能为0或者负数，否则会错误

(无法阻止倒车，就算比例很大，在迫不得已的情况下)
 max_vel_y: 0.0 # y方向最大速度 阿克曼形是没有的
 max_vel_theta: 0.50 #最大转向角速度

acc_lim_x: 0.15 # 最大加速度

acc_lim_theta: 0.20 #最大角加速度，不建议很大，会导致震荡

min_turning_radius: 0.35 #最小转弯半径

这个参数的设置是非常影响转弯时的路径规划的

 footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
   type: "line" # 设置机器人模型——可以为上面几种
   line_start: [0.05, 0.0] # for type "line" #线的起点
   line_end: [0.10, 0.0] # for type "line" #线的终点
(这种线的模式适用于阿克曼小车，设置线的起点和终点)

# GoalTolerance 目标容忍度

前两个比较重要，但不介意设置的很小

 xy_goal_tolerance: 0.2 #xy目标偏移度
 yaw_goal_tolerance: 0.2 #目标角度偏移容忍度
 free_goal_vel: True #允许机器人以最大速度前往目的地，True的话就会在可以加速的过程中加速
 complete_global_plan: True #完成目标点 

 # Obstacles 障碍物
    
 min_obstacle_dist: 0.30 # 与障碍物最小距离(整个线的头 中间 尾)

这个参数的设置决定了和障碍物约束的距离

测试之后不建议设置的很小，因为我们的车模本身就是小的，而且地面有凸起，所以离得远点

 inflation_dist: 0.30 # 障碍物膨胀距离
 include_costmap_obstacles: True # 局部地图中的实时障碍物是否考虑
 costmap_obstacles_behind_robot_dist: 0.3 #代价地图考虑后方的障碍物(因为是有倒车的)
 obstacle_poses_affected: 7 #障碍物姿态影响，影响不是特别大
 
 dynamic_obstacle_inflation_dist: 0.6 #动态障碍物膨胀范围
 include_dynamic_obstacles: True #是否将为速度模型
 
 costmap_converter_plugin: "" #一般不使用这个插件
 costmap_converter_spin_thread: True
 costmap_converter_rate: 5

#Optimization 优化参数

 no_inner_iterations: 5 # 被外循环调用后内循环执行次数
 no_outer_iterations: 4 # 执行外循环优化次数
 optimization_activate: True # 激活优化过程
 optimization_verbose: False # 打印优化过程

 penalty_epsilon: 0.1 # 对硬约束近似

这个参数会为速度约束，也就是达到最大速度前，会有个惩罚，会让他提前减速达到缓冲效果
 obstacle_cost_exponent: 4
 weight_max_vel_x: 2 # 最大速度权重
 weight_max_vel_theta: 1 # 最大角速度权重
 weight_acc_lim_x: 1 # 最大加速度权重
 weight_acc_lim_theta: 1 # 最大角速度权重

(上面的参数主要起综合作用，确定是高速还是低速)

 weight_kinematics_nh: 1000 
 weight_kinematics_forward_drive: 500 #抑制倒车的权重，正常设置1
 weight_kinematics_turning_radius: 1 #最小转弯半径，我们没必要最小转弯
 weight_optimaltime: 500 #优化时间参数，让小车多走直线和内道
 weight_shortest_path: 0
 weight_obstacle: 50 # 优化过程中和障碍物最小距离权重
 weight_inflation: 0.2 # 膨胀区域权重
 weight_dynamic_obstacle: 10 # 动态障碍物最小距离权重
 weight_dynamic_obstacle_inflation: 0.2 # 动态障碍物膨胀区域权重
 
 weight_viapoint: 1 #路径采样点距离权重

上面的这些参数是我们修改比较多，也是影响比较大的参数

注意点：

1.当小车在某些位置卡住不敢走（还没撞）时，考虑膨胀半径是否应该调小，和weight_optimaltime是否应该调大。

2.刚开始调，可以速度调慢点，max_global_plan_lookahead_dist（向前规划距离）调小，可以使得局部路径较贴近全局路径并完成跑图，再慢慢调大找最佳值