一、簡(jiǎn)介

?“TEB”全稱Time Elastic Band（時(shí)間彈性帶）Local Planner，該方法針對(duì)全局路徑規(guī)劃器生成的初始軌跡進(jìn)行后續(xù)修正(modification)，從而優(yōu)化機(jī)器人的運(yùn)動(dòng)軌跡，屬于局部路徑規(guī)劃。

關(guān)于eletic band（橡皮筋）的定義：連接起始、目標(biāo)點(diǎn)，并讓這個(gè)路徑可以變形，變形的條件就是將所有約束當(dāng)做橡皮筋的外力。

二、說(shuō)明

局部路徑規(guī)劃之Teb

起始點(diǎn)、目標(biāo)帶你狀態(tài)由全局規(guī)劃，中間插入N個(gè)控制節(jié)點(diǎn)來(lái)改變橡皮筋的狀態(tài)控制點(diǎn)，在點(diǎn)與點(diǎn)之間定義運(yùn)動(dòng)時(shí)間Time。

這個(gè)路徑可以變形，變形的條件就是將所有約束當(dāng)做橡皮筋的外力

注意每個(gè)目標(biāo)函數(shù)只與幾個(gè)連續(xù)狀態(tài)有關(guān)，而非整條band。

?當(dāng)我們?cè)O(shè)置目標(biāo)時(shí)，小車(chē)的目的地是靠近障礙物的，但是如果我們小于我們的障礙物距離最小距離，我們就需要往外拉，這就是我們的類橡皮筋過(guò)程

約束目標(biāo)函數(shù)：

?

?

?

?優(yōu)化問(wèn)題：

Teb優(yōu)化問(wèn)題實(shí)質(zhì)上是一個(gè)優(yōu)化問(wèn)題，大多數(shù)目標(biāo)是基于局部的，只與一小部分參數(shù)相關(guān)，因?yàn)樗麄冎灰蕾囉趲讉€(gè)連續(xù)的機(jī)器人

TEB生成的局部軌跡由一系列帶有時(shí)間信息的離散位姿組成，g2o算法優(yōu)化的目標(biāo)就是這些離散的位姿，同時(shí)設(shè)計(jì)一條時(shí)間最短，距離最短，遠(yuǎn)離障礙物等目標(biāo)，同時(shí)限制速度與加速度使軌跡滿足機(jī)器人運(yùn)動(dòng)學(xué)。

整體的規(guī)劃為：

全局路徑————加入約束————g2o優(yōu)化————速度指令

參數(shù)：

# Trajectory

teb_autosize: True #優(yōu)化期間允許改變軌跡時(shí)域長(zhǎng)度

dt_ref: 0.3 #局部路徑規(guī)劃解析度(0.01~1.0) 默認(rèn)為0.3 (兩個(gè)相鄰位姿之間的時(shí)間_時(shí)間分辨率)

(可以設(shè)置0.45，其他保持默認(rèn))

dt_hysteresis: 0.1 #允許浮動(dòng)范圍

global_plan_overwrite_orientation: True #覆蓋全局路 徑中局部路點(diǎn)朝向

max_global_plan_lookahead_dist: 3.0 #全局優(yōu)化子集最大長(zhǎng)度

feasibility_check_no_poses: 2 # 檢測(cè)位姿可達(dá)到的時(shí)間間隔

# Robot _ 下面的這些參數(shù)就會(huì)切實(shí)的影響規(guī)劃

max_vel_x: 0.7? #最大前進(jìn)速度 x? (可以設(shè)置成0.5嘗試)
?max_vel_x_backwards: 0.3?#最大后退速度 x 這個(gè)值不能為0或者負(fù)數(shù)，否則會(huì)錯(cuò)誤

(無(wú)法阻止倒車(chē)，就算比例很大，在迫不得已的情況下)
?max_vel_y: 0.0 # y方向最大速度 阿克曼形是沒(méi)有的
?max_vel_theta: 0.50?#最大轉(zhuǎn)向角速度

acc_lim_x: 0.15?# 最大加速度

acc_lim_theta: 0.20?#最大角加速度，不建議很大，會(huì)導(dǎo)致震蕩

min_turning_radius: 0.35?#最小轉(zhuǎn)彎半徑

這個(gè)參數(shù)的設(shè)置是非常影響轉(zhuǎn)彎時(shí)的路徑規(guī)劃的

?footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
? ?type: "line" # 設(shè)置機(jī)器人模型——可以為上面幾種
? ?line_start: [0.05, 0.0] # for type "line" #線的起點(diǎn)
? ?line_end: [0.10, 0.0] # for type "line" #線的終點(diǎn)
(這種線的模式適用于阿克曼小車(chē)，設(shè)置線的起點(diǎn)和終點(diǎn))

# GoalTolerance 目標(biāo)容忍度

前兩個(gè)比較重要，但不介意設(shè)置的很小

?xy_goal_tolerance: 0.2?#xy目標(biāo)偏移度
?yaw_goal_tolerance: 0.2 #目標(biāo)角度偏移容忍度
?free_goal_vel: True #允許機(jī)器人以最大速度前往目的地，True的話就會(huì)在可以加速的過(guò)程中加速
?complete_global_plan: True #完成目標(biāo)點(diǎn)?

?# Obstacles 障礙物
? ??
?min_obstacle_dist: 0.30?# 與障礙物最小距離(整個(gè)線的頭 中間 尾)

這個(gè)參數(shù)的設(shè)置決定了和障礙物約束的距離

測(cè)試之后不建議設(shè)置的很小，因?yàn)槲覀兊能?chē)模本身就是小的，而且地面有凸起，所以離得遠(yuǎn)點(diǎn)

?inflation_dist: 0.30?# 障礙物膨脹距離
?include_costmap_obstacles: True # 局部地圖中的實(shí)時(shí)障礙物是否考慮
?costmap_obstacles_behind_robot_dist: 0.3?#代價(jià)地圖考慮后方的障礙物(因?yàn)槭怯械管?chē)的)
?obstacle_poses_affected: 7?#障礙物姿態(tài)影響，影響不是特別大
?
?dynamic_obstacle_inflation_dist: 0.6 #動(dòng)態(tài)障礙物膨脹范圍
?include_dynamic_obstacles: True #是否將為速度模型
?
?costmap_converter_plugin: "" #一般不使用這個(gè)插件
?costmap_converter_spin_thread: True
?costmap_converter_rate: 5

#Optimization 優(yōu)化參數(shù)

?no_inner_iterations: 5 # 被外循環(huán)調(diào)用后內(nèi)循環(huán)執(zhí)行次數(shù)
?no_outer_iterations: 4 # 執(zhí)行外循環(huán)優(yōu)化次數(shù)
?optimization_activate: True # 激活優(yōu)化過(guò)程
?optimization_verbose: False # 打印優(yōu)化過(guò)程

?penalty_epsilon: 0.1 # 對(duì)硬約束近似

這個(gè)參數(shù)會(huì)為速度約束，也就是達(dá)到最大速度前，會(huì)有個(gè)懲罰，會(huì)讓他提前減速達(dá)到緩沖效果
?obstacle_cost_exponent: 4
?weight_max_vel_x: 2 # 最大速度權(quán)重
?weight_max_vel_theta: 1 # 最大角速度權(quán)重
?weight_acc_lim_x: 1 # 最大加速度權(quán)重
?weight_acc_lim_theta: 1 # 最大角速度權(quán)重

(上面的參數(shù)主要起綜合作用，確定是高速還是低速)

?weight_kinematics_nh: 1000?
?weight_kinematics_forward_drive: 500 #抑制倒車(chē)的權(quán)重，正常設(shè)置1
?weight_kinematics_turning_radius: 1 #最小轉(zhuǎn)彎半徑，我們沒(méi)必要最小轉(zhuǎn)彎
?weight_optimaltime: 500 #優(yōu)化時(shí)間參數(shù)，讓小車(chē)多走直線和內(nèi)道
?weight_shortest_path: 0
?weight_obstacle: 50 # 優(yōu)化過(guò)程中和障礙物最小距離權(quán)重
?weight_inflation: 0.2 # 膨脹區(qū)域權(quán)重
?weight_dynamic_obstacle: 10 # 動(dòng)態(tài)障礙物最小距離權(quán)重
?weight_dynamic_obstacle_inflation: 0.2 # 動(dòng)態(tài)障礙物膨脹區(qū)域權(quán)重
?
?weight_viapoint: 1 #路徑采樣點(diǎn)距離權(quán)重

上面的這些參數(shù)是我們修改比較多，也是影響比較大的參數(shù)

注意點(diǎn)：

1.當(dāng)小車(chē)在某些位置卡住不敢走（還沒(méi)撞）時(shí)，考慮膨脹半徑是否應(yīng)該調(diào)小，和weight_optimaltime是否應(yīng)該調(diào)大。

2.剛開(kāi)始調(diào)，可以速度調(diào)慢點(diǎn)，max_global_plan_lookahead_dist（向前規(guī)劃距離）調(diào)小，可以使得局部路徑較貼近全局路徑并完成跑圖，再慢慢調(diào)大找最佳值

?