ROS导航局部路径规划算法
DWA 是一种基于。
·
在导航功能包中，局部路径规划器的位置如图红框所示。它接受全局路径规划器生成的全局路径以及里程计信息和局部代价地图，最后输出速度控制信息
一、DWA（Dynamic Window Approach）
原理介绍
DWA中文名称动态窗口方法。窗口指的是机器人运动控制中线路轨迹和运动速度的可选择的空间(由机器人的最大加减速能力和当前速度限制，即速度区间)。此算法是通过在当前速度区间内采样多个速度组合（线速度和角速度），对每个采样的(v, ω)组合，假设机器人在短时间内（通常为几秒）保持该速度不变，通过运动学模型预测这些速度在短时间内生成的轨迹，然后根据评价函数选择最优轨迹。 “在短时间内（通常为几秒）保持该速度不变”意味着如果预测轨迹路径太长，轨迹可能变成闭合圆弧
当全局规划器规划出导航路线后(紫线)，地图中出现了障碍物
接下来机器人可能会走出如下图所示黄线的轨迹，其中撞到障碍物的轨迹(红线)会被淘汰，剩下的可能的轨迹以及在轨迹上可能的速度变化，就是所谓的窗口。DWA算法就是生成这样的一系列轨迹和速度的方案，从中挑选最合适的轨迹，然后按照它来驱动机器人运动。这里涉及两个内容，一个是生成轨迹，一个是挑选轨迹。生成轨迹是以机器人当前的速度为基础，规划未来一段时间机器人的运动状态和移动线路
机器人底盘的速度是由矢量速度和旋转速度两个分量组成的，给这两个分量取不同的值，然后组合起来就能让机器人走出不一样的弧线。DWA的轨迹生成就是用这种方法实现的，其中对速度分量的取值操作称为采样，速度的取值会综合考虑底盘加速度的限制、与障碍物保持安全刹车距离以及尽快运动到轨迹终点这三个因素
最终从多条轨迹中选出最合适的轨迹，选择标准有三个：运动轨迹和全局轨迹的贴合程度，轨迹末端和目标点的距离，轨迹路线和障碍物的距离。
DWA的轨迹实际上是平滑的弧线，如下所示，白色为所有预测的轨迹，绿色为选择的最优的那一条，可以通过调节参数sim_time来增加轨迹预测长度，但是设置过大轨迹会闭合为一个圆而原地打转
其一般步骤如下：
-
动态窗口生成：根据机器人的动力学约束（最大速度、加速度(均为可调参数)）生成一个可达的速度范围（
v, ω）。仅考虑在当前控制周期内能安全停止的速度组合（避免碰撞）。可调参数包括机器人的最大/最小线速度和角速度，基于电机限制的瞬时加速度，能在碰撞前停止的速度。 -
轨迹模拟：对每个速度候选（vv, ωω），模拟机器人在未来短时间（通常3~5秒）内的运动轨迹。
-
代价函数评估：对每条轨迹打分，考虑以下因素：
-
选择最优轨迹
从所有候选（vv, ωω）中选择得分最高的组合，发送给机器人执行。
DWA可以做到每周期（通常100ms）重新计算，适应动态环境。但是可能陷入局部最优（如U型障碍），需依赖全局规划器或恢复行为。
局部最优解
局部最优解：指算法在有限的搜索空间或时间内，找到了一个在当前局部范围内看起来最优的解决方案，但这个方案从全局角度看并非最佳，甚至可能导致任务失败（如无法到达目标）。在算法当前的搜索范围内（如动态窗口内的速度组合），某个解（如一条轨迹）比其他解更优（如得分最高），但若扩大搜索范围，可能存在更优的解。也就是说视野太窄而找不到全局的最优解
机器人进入U型障碍后，DWA的短视特性（仅评估几秒内的轨迹）可能导致它不断优化朝向障碍内壁的运动，而无法“后退”寻找出口，甚至会使机器人在障碍内来回振荡或原地打转。并且多个移动障碍物暂时包围机器人，DWA的瞬时避障策略可能无法找到全局逃生路径。
之所以DWA会陷入局部最优解，原因如下：DWA的“动态窗口”仅考虑当前速度附近的可达速度组合，而非所有可能性。若全局最优解需要先减速或反向运动（如倒车），可能被窗口过滤掉。在非凸障碍物（如凹多边形）环境中，全局路径可能需要先远离目标再接近，而DWA的贪心策略会拒绝暂时远离目标的路径。DWA作为局部规划器，仅依赖局部代价地图。若全局路径穿过动态障碍物，而局部地图未更新，可能导致机器人卡住。
除此之外，DWA对快速移动的障碍物（如行人、其他机器人）反应可能不及时。这是因为速度采样范围有限（受动态窗口约束），可能无法生成足够激进的避障速度，并且障碍物预测基于当前瞬时状态，未显式建模动态障碍物的运动趋势（对比TEB或MPC）。
参数说明
在调参时可以在终端运行rosrun rqt_reconfigure rqt_reconfigure启动如下调参工具，可以一边运行一边调参(动态调参)，调出想要的参数后，再将参数值写到调参文件，下次启动就可以实现较好的效果。
#Description:
# dwa_local_planner提供一个能够驱动底座的控制器,该控制器连接了路径规划器和机器人.
# 使用地图,规划器产生从起点到目标点的运动轨迹,在移动时,规划器在机器人周围产生一个函数,
# 用网格地图表示。控制器的工作就是利用这个函数来确定发送给机器人的速度dx, dy, dtheta
#
# >> DWA算法的基本思想 <<
#DWA算法的核心思想是通过在速度空间中采样一组可能的速度组合（即线速度和角速度），并对这些速度组合进行前向模拟，生成多条可能的轨迹。然后，DWA会根据一定的评分标准（如距离目标点的远近、是否避障、是否接近全局路径等）对这些轨迹进行评分，最终选择得分最高的轨迹对应的速度发送给机器人。
# 1.在机器人控制空间离散采样(dx, dy, dtheta)
# 2.对每一个采样的速度进行前向模拟,看看在当前状态下,使用该采样速度移动一小段时间后会发生什么.
# 3.评价前向模拟得到的每个轨迹,是否接近障碍物,是否接近目标,是否接近全局路径以及速度等等.舍弃非法路径
# 4.选择得分最高的路径,发送对应的速度给底座
#
# DWA与Trajectory Rollout的区别主要是在机器人的控制空间采样差异.Trajectory Rollout采样点来源于整个
# 前向模拟阶段所有可用速度集合,而DWA采样点仅仅来源于一个模拟步骤中的可用速度集合.这意味着相比之下
# DWA是一种更加有效算法,因为其使用了更小采样空间;然而对于低加速度的机器人来说可能Trajectory Rollout更好,
# 因为DWA不能对常加速度做前向模拟。
# * publish_cost_grid:是否发布规划器在规划路径时的代价网格.如果设置为true,
# 那么就会在~/cost_cloud话题上发布sensor_msgs/PointCloud2类型消息.
# 每个点云代表代价网格,并且每个单独的评价函数都有一个字段及其每个单元
# 的总代价，并考虑评分参数.
#
# * oscillation_reset_dist:机器人运动多远距离才会重置振荡标记.
#
# * prune_plan:机器人前进是是否清楚身后1m外的轨迹.
latch_xy_goal_tolerance: true
DWAPlannerROS:
# Robot Configuration Parameters - stdr robot
#-----------------------------------------------------------------------
#-----------------------------------------------------------------------
#----------------------机器人运动限制参数----------------------------------
#-----------------------------------------------------------------------
#-----------------------------------------------------------------------
# 这些参数确保机器人在运动时不会超出其物理能力，避免失控或损坏。
# 分别定义了机器人在 x 方向、y 方向和旋转方向的最大加速度。
acc_lim_x: 1.2 # 1.2
acc_lim_y: 0.4 # 0.4
acc_lim_th: 1.4
# 定义了机器人在 x 和 y 方向的最大和最小速度。
max_vel_x: 1.5 # 1.5
min_vel_x: -0.8
max_vel_y: 0.5 #diff drive robot,don't need set vel_y # 0.5
min_vel_y: -0.2
# 定义了机器人旋转的最大和最小角速度。
max_vel_theta: 3.0 #choose slightly less than the base's capability
min_vel_theta: -3.0 #this is the min angular velocity when there is negligible translational velocity
# 定义了机器人停止时的平移速度和角速度阈值,当机器人的平移速度（即线速度）旋转速度（即角速度）低于这个值时，规划器会认为机器人已经停止。这个参数的设置是为了避免机器人因为微小的速度波动而被误认为仍在移动。
trans_stopped_vel: 0.1
theta_stopped_vel: 0.2
# 定义了机器人平移的最大和最小速度的绝对值。定义了机器人平移速度的最值，即机器人在平面内移动时的最大合成速度（不考虑方向）。max_vel_x、min_vel_x、max_vel_y、min_vel_y这些参数是分方向的速度限制，分别控制机器人在 x 和 y 方向的速度范围。对于差速驱动机器人（如两轮差速机器人），通常只需要设置 max_vel_x 和 min_vel_x，因为它们在 y 方向没有速度（max_vel_y 和 min_vel_y 为 0）。而对于全向移动机器人（如麦克纳姆轮机器人），则需要设置 x 和 y 方向的速度限制。
max_vel_trans: 1.5
min_vel_trans: 0.0
#-----------------------------------------------------------------------
#-----------------------------------------------------------------------
#---------------------------目标容差参数----------------------------------
#-----------------------------------------------------------------------
#-----------------------------------------------------------------------
# 这些参数控制机器人到达目标点时的精度。
# Goal Tolerance Parameters
yaw_goal_tolerance: 0.1 # 0.1 rad = 5.7 degree 定义了机器人到达目标点时，偏航角的允许误差（单位：弧度）。
xy_goal_tolerance: 0.1 # 定义了机器人到达目标点时，在 xy 平面内与目标点的距离误差。如果你希望机器人在到达目标点时更加准确，可以通过减小 yaw_goal_tolerance 和 xy_goal_tolerance 的值来实现。但如果 yaw_goal_tolerance 设置得过小，机器人可能会因为无法达到精度要求而陷入振荡或无法完成任务。
latch_xy_goal_tolerance: true #如果设置为 true，当机器人进入容错距离后，即使旋转时跑出容错距离，也会继续旋转直到达到目标方向。
#-----------------------------------------------------------------------
#-----------------------------------------------------------------------
#---------------------------前向仿真参数----------------------------------
#-----------------------------------------------------------------------
#-----------------------------------------------------------------------
# 这些参数影响规划器生成轨迹的精度和计算复杂度
# Forward Simulation Parameters
sim_time: 1.5 # 2.0 定义了规划器向前仿真轨迹的时间长度。sim_time = 1.5，规划器会生成机器人在未来 1.5 秒内的运动轨迹.sim_time 较大：规划器会生成更长的轨迹，能够更好地预测机器人的未来运动。适合机器人需要长距离规划的场景（例如空旷环境）。但计算量会增加，可能会导致规划器响应变慢。sim_time 较小：规划器生成的轨迹较短，计算量较小，响应更快。适合机器人需要快速调整路径的场景（例如狭窄或动态环境）。但可能会因为轨迹太短而无法有效避开障碍物或接近目标。
sim_granularity: 0.05 # 定义了轨迹上采样点之间的距离，影响轨迹的密集程度。规划器在生成轨迹时，会将轨迹离散化为多个采样点，每个采样点之间的距离由 sim_granularity 决定。通过调整 sim_granularity，可以控制轨迹的精度和计算量。sim_granularity = 0.05，轨迹上每 0.05 米会有一个采样点。
# vx_samples、vy_samples 和 vth_samples 是用于控制速度空间采样的参数。它们决定了在速度空间中采样的点数，进而影响规划器生成轨迹的精度和计算复杂度。速度空间采样点数是指在DWA算法的第一步中，在速度空间中采样的速度组合的数量。采样点数越多，生成的速度组合越多，轨迹的多样性也越强，但计算量也会相应增加。
vx_samples: 20 # default 3
vy_samples: 1 # diff drive robot, there is only one sample # 1
vth_samples: 40 # 20
controller_frequency: 15.0
#-----------------------------------------------------------------------
#-----------------------------------------------------------------------
#---------------------------轨迹评分参数----------------------------------
#-----------------------------------------------------------------------
#-----------------------------------------------------------------------
# 用于定义如何对生成的轨迹进行评分。这些参数决定了规划器在评估轨迹时的权重和优先级，从而影响机器人最终选择的运动轨迹。
# Trajectory Scoring Parameters
path_distance_bias: 45.0 # 30.0 该参数定义了轨迹与全局路径的接近程度的权重。当 path_distance_bias 值较大时，规划器会更倾向于选择与全局路径接近的轨迹。这个参数用于确保机器人尽可能沿着全局规划器生成的路径移动，而不是偏离太远。如果机器人经常偏离全局路径，可以适当增加该值。如果机器人过于死板地跟随全局路径而无法避障，可以适当减小该值。
goal_distance_bias: 30.0 # 24.0 该参数定义了轨迹与局部目标点接近程度的权重。当 goal_distance_bias 值较大时，规划器会更倾向于选择能够更快到达目标点的轨迹。这个参数用于确保机器人能够高效地朝着目标点移动。如果机器人到达目标点的速度较慢，可以适当增加该值。如果机器人过于追求快速到达目标点而忽略了避障或路径平滑性，可以适当减小该值。goal_distance_bias 参数定义的是轨迹与局部目标点接近程度的权重，而局部目标点通常是全局路径规划器（如 global_planner 或 navfn）生成的路径上的某个点，或者是路径的终点（即最终目标点）。这个参数的作用是让 DWA 局部规划器在评估轨迹时，优先选择那些能够更快接近目标点的轨迹。
occdist_scale: 1.0 # 0.01 该参数定义了轨迹避障的权重。当 occdist_scale 值较大时，规划器会更倾向于选择远离障碍物的轨迹。如果机器人经常撞到障碍物，可以适当增加该值。如果机器人过于保守，总是远离障碍物而导致路径不够平滑，可以适当减小该值。
forward_point_distance: 0.325 # 0.325 该参数定义了在轨迹前方多远的位置放置一个额外的评分点。通过在轨迹前方放置一个额外的评分点，规划器可以更好地评估轨迹的前方状态，从而避免机器人进入危险区域。如果机器人经常进入危险区域，可以适当增加该值。如果机器人过于保守，导致路径不够高效，可以适当减小该值。
stop_time_buffer: 0.2 # 0.2 该参数定义了机器人在碰撞前必须提前停止的时间长度。
scaling_speed: 0.25 # 0.25 这个参数用于提高机器人在高速运动时的安全性。
max_scaling_factor: 0.2 # 0.2 这个参数用于确保机器人在高速运动时有足够的安全距离。
publish_cost_grid: false # 该参数定义了是否发布规划器在规划路径时的代价网格。如果设置为 true，规划器会在 ~/cost_cloud 话题上发布代价网格信息，通常用于调试和可视化。代价网格显示了规划器在评估轨迹时对每个网格单元的评分。
#-----------------------------------------------------------------------
#-----------------------------------------------------------------------
#---------------------------振荡预防参数----------------------------------
#-----------------------------------------------------------------------
#-----------------------------------------------------------------------
# Oscillation Prevention Parameters
oscillation_reset_dist: 0.1 # default 0.05 定义了机器人运动多远距离才会重置振荡标记。防止机器人在某些情况下反复振荡。
hdiff_scale: 1.0 # 1.0 是一个与 航向角（heading）偏差(航向角是指机器人当前朝向与目标方向之间的角度偏差。)相关的参数。它用于控制机器人在路径规划时对目标方向偏差的敏感度。值越大，机器人越倾向于调整航向角以减少偏差。值越小，机器人对航向角偏差的敏感度越低。
heading_points: 2 # 1参数定义了在路径规划时，机器人需要考虑的路径点的数量，用于计算方向.heading_points 决定了机器人在规划路径时，会考虑多少个路径点来计算其目标方向。设置为 1 表示机器人只考虑下一个路径点的方向。增加这个值会使机器人在规划时考虑更远的路径点，从而可能使路径更加平滑。
# Global Plan Parameters
#-----------------------------------------------------------------------
#-----------------------------------------------------------------------
#---------------------------全局路径参数----------------------------------
#-----------------------------------------------------------------------
#-----------------------------------------------------------------------
prune_plan: true # 如果设置为 true，机器人会在前进时清除身后 1 米外的轨迹，减少不必要的路径点。
#-----------------------------------------------------------------------
#-----------------------------------------------------------------------
#---------------------------其他参数-------------------------------------
#-----------------------------------------------------------------------
#-----------------------------------------------------------------------
publish_traj_pc: false # 是否发布规划器在规划路径时的代价网格。
publish_cost_grid_pc: false # 这个参数控制是否发布规划器在规划路径时的代价网格点云（cost grid point cloud）。
global_frame_id: map # 定义了全局坐标系，通常为 map。
二，TEB（Timed Elastic Band）
TEB不属于ROS官方的导航组件，需要使用sudo apt install ros-noetic-teb-local-planner另行安装
对于二维路径的描述，提出了一个有趣的方法，叫做Elastic Band(橡皮筋)。简而言之，就是连接起始点和目标点的一条橡皮筋，并且这个橡皮筋可以变形，变形的条件就是将所有的约束当作橡皮筋的外力。橡皮筋的起始点和目标点状态由用户/全局规划器指定，中间插入N个控制橡皮筋形状的的控制点(机器人姿态)，为了显示轨迹的运动学信息，在点与点(相邻点之间)之间定义运动时间time。于是，这个方法就叫做Timed Elastics Band，下面进行详细解释。
首先起始点和目标点状态由用户/全局规划器指定好理解，就是从全局规划器获得目标点位置，如下图所示小车的起始点Current robot pose，小车的目标点Current goal就是从全局规划器或者是我们指定要小车移动到哪的目标点。黑色的虚线是从全局路径规划器获得的全局路径，蓝色的五边形就是障碍物Obstacle。TEB算法得到全局路径以后会在路径中插入n个点(控制点，内容就是机器人的姿态pose,包括xy坐标和航向)，如何小车真的在这条黑线上走，TEB会经过计算把小车在这n个点上的位置算出来，用这一系列的姿态点来代表这条路径。每两个姿态点之间会定义一个固定的时间(时间分辨率,dt_ref)，有了两个点之间的时间和姿态信息，就可以算出这两个点之间的距离，进而算出小车在这一小段路程里的速度，速度再进行微分就可以求出加速度，这样就可以知道小车的所有运动学信息。
前面说过变形的条件就是将所有的约束当作橡皮筋的外力，这里我们以障碍物的约束进行讲解。在运动时我们不希望小车碰到障碍物，所以在这里我们给小车和障碍物之间的距离划定一个最小值(dist)，小车与障碍物之间所划定的这个最小距离就是一个约束。假如现在人为的把障碍物往小车那边推，为了满足最小距离的约束，最靠近障碍物的点就被迫往下面远离障碍物的方向移动，这样看起来就像是障碍物使了外力将路径像橡皮筋一样推开。除了障碍物的约束之外，每两个相邻点之间也有运动学相关的约束，比如其中一个点被障碍物推到了下方，那它相邻的点也不可能在原处不动，因为速度、加速度、角速度、角加速度都是由最大值的，所以两个点之间不能有特别大的姿态跳变，这样就满足一个比较圆滑的过渡路径
注意，每个目标函数只与elastic band中的某几个连续状态有关，而非整条band。这里所说的目标函数其实就是约束条件，只是说我们把一些约束量化成了函数形式。也就是说每一种约束其实它只影响几个连续的状态，并不是说单一的路径会对整一条路径都会产生影响。
如下图所示，紫色为全局路径，绿色为teb局部规划路径，红色为未来连续几个时间单位机器人将会到达的位置，局部导航路径不断变化，引导机器人到达全局路线的终点
TEB（Timed Elastic Band，时间弹性带）是ROS导航中一种基于弹性带优化的局部路径规划算法，专门为动态环境设计，尤其适合非完整约束（如差速驱动、汽车模型）的机器人。相比DWA，TEB通过全局优化轨迹和时间自由度，能够更有效地规避局部最优解问题。
TEB有以下优点：
1，全局优化：不是只看下一步，而是优化整条路线，避免钻进死胡同。
2， 时间弹性：快慢自由调节，遇到障碍物自动减速，空旷路段加速。
3，多方案备选同时生成多条候选路线（比如左绕、右绕、倒车），选最好的
相比来说，DWA只规划“下一步”速度，像蒙着眼走路，容易卡死。而TEB规划“整条路径+速度变化”，像边走边看地图，还能倒车绕路。
约束目标函数
以下是常见的四种约束：
- 跟随约束+避障：这种约束主要有两个目标，跟随已知的全局规划路径和避障。这两种约束写成的目标函数很相似，所以归为一类。跟随路径施力将elastic bands拉向全局路径，而避障约束施力使得elastic bands远离障碍物
- 速度/加速度约束：由于小车的性能约束，速度和加速度都是由最大最小值的。可以在参数里调节最大最小值限制
- 运动学约束：这里是希望生成的轨迹是比较光滑的。控制量一般只有车速和转角，对于阿克曼结构还有最小的转弯半径.为了满足这个约束生成的路径一般是由若干段圆弧组成的光滑轨迹
- 最快路径约束：主要是希望机器人能够获得最快的路径。这里的最快和传统的最短路径其实是不一样的，最短的路径不一定是最快的路径
优化
TEB的优化是一种局部的优化，被表述为一个多目标优化问题，大多数的目标都是局部的，只与一小部分参数相关，因为它只依赖于几个连续的机器人的状态(每一种约束只影响几个连续的状态)。
TEB用到的优化算法是开源的框架g2o，这个优化算法涉及到两个概念，一个是点(node)一个是边(edge)。在我们这个小车情景下所有的姿态点 时间间隔以及障碍物等等都被描述为点，而约束条件被描述为边，通过约束条件这些边来把各个点连接起来，经过这样的处理以后，后面就调用图优化法进行优化
总的来说，TEB生成的局部轨迹由一系列带有时间信息的离散位姿组成，g2o算法优化的目标即这些离散的位姿，使最终由这些离散位姿组成的轨迹能达到时间最短 距离最短 远离障碍物等目标，同时限制速度与加速度使轨迹满足机器人的运动学。
简单来说工作流程就是：全局路径-->加入约束-->g2o优化-->速度指令
参数说明
在调参时可以在终端运行rosrun rqt_reconfigure rqt_reconfigure启动如下调参工具，可以一边运行一边调参(动态调参)，调出想要的参数后，再将参数值写到调参文件，下次启动就可以实现较好的效果。
TebLocalPlannerROS:
robot_type: "omni"
odom_topic: odom
map_frame: odom
# 要调的参数后加 #@
#-----------------------------------------------------------------------
#-----------------------------------------------------------------------
#---------------------------轨迹配置参数----------------------------------
#-----------------------------------------------------------------------
#-----------------------------------------------------------------------
teb_autosize: true # 当设置为True时，TEB(Time Elastic Band)算法会自动优化时间分配(时间间隔dt)
# 算法会根据路径的曲率和速度限制动态调整时间步长，允许路径上的不同点有不同的时间间隔
# 建议保持True以获得更好的性能，如果设为False，则需要手动设置固定时间步长，这在复杂环境中可能导致性能下降
dt_ref: 0.3 #@ # 两个姿态控制点间的参考时间间隔，当teb_autosize=True时，dt_ref作为优化的初始值
# 影响轨迹优化的精度和计算效率较小的dt_ref提高轨迹精度，但会增加计算负担
# 建议设为车长/最大速度（如车长0.5m，速度1m/s → 0.5）
dt_hysteresis: 0.1 # 允许的dt_ref波动范围，防止时间步长在临界值附近频繁振荡。
# 只有当新计算的时间步长与当前值的差异超过此阈值时才会更新。通常保持默认值0.1s即可
max_samples: 5 # 定义了优化过程中保留的候选轨迹的最大数量。
# 在每次优化迭代中，TEB算法会生成并评估多个候选轨迹，max_samples决定了算法保留的最佳轨迹数量上限。
# TEB算法核心是通过同时优化多条候选轨迹来找到最优解，这些候选轨迹代表不同的时间分配和路径形状
# 更多样本意味着更大的搜索空间，可能找到更好的解，但也会显著增加计算负担
global_plan_overwrite_orientation: false # 控制是否覆盖全局路径中点的朝向（orientation）
# 当设置为True时，TEB会忽略全局路径提供的点朝向，根据局部优化重新计算每个点的朝向
# 当设置为False时，会尽量保持全局路径提供的原始朝向
global_plan_viapoint_sep: 0.3 # 定义从全局路径中提取航点(viapoints)的间隔距离
# 算法会沿着全局路径每隔global_plan_viapoint_sep距离选取一个航点，这些航点作为优化目标点，引导TEB路径朝向全局路径
# 较小值：生成更多航点，路径更紧密跟随全局路径
# 较大值：航点稀疏，路径更平滑。可能偏离全局路径较远
# -0.1表示禁用
allow_init_with_backwards_motion: false # 控制是否允许初始轨迹包含后退运动
# 当设置为True时，如果有利于达到目标，TEB可能生成包含后退移动的初始轨迹
# 当设置为False时，强制初始轨迹只包含前进运动
max_global_plan_lookahead_dist: 1.0 #@ # 局部规划的前视距离，表示从机器人当前位置开始，全局路径上向前看的最大距离，超出此距离的全局路径部分将被忽略
# 该参数控制局部优化的"视野范围"
# 应随车辆最大速度的增大而增大
# 不应超过激光雷达等传感器的可靠测量范围 16m
# 不应超过局部耗费地图的大小
# 决定局部规划初始轨迹的最大长度，实际调试发现此参数无需过大，因为局部轨迹在每个控制周期都被更新，实际执行的指令仅是轨迹上第一个点的速度值
global_plan_prune_distance: 0.8 #@ # 当机器人沿着全局路径前进时，global_plan_prune_distance会删除机器人后方超过该距离的路径点。影响全局路径的内存占用和更新频率
# 较小值（0.5-1米）：快速释放内存，但可能导致路径更新更频繁，适合需要精确路径跟踪的场景
# 较大值（2-3米）：减少计算开销，但可能略微增加内存使用，适合大范围导航
complete_global_plan: false # 是否要求严格跟踪完整全局路径到终点
exact_arc_length: false # 如果为真，规划器在速度、加速度和转弯率计算中使用精确的弧长[->增加的CPU时间]，
# 否则使用欧几里德近似
feasibility_check_no_poses: 2 #@ # 该参数定义了在局部轨迹优化过程中，每隔多少个位姿（poses）进行一次运动学/动力学可行性检查（如速度、加速度限制、碰撞检测等）。
# 值越小：检查频率越高，轨迹更安全但计算量增大。
# 值越大：检查频率越低，计算更快但可能忽略中间状态的违规（如急转弯或动态障碍物突然出现）。
# TEB优化器生成候选轨迹后，会检查轨迹的可行性
# 算法选取轨迹上连续的feasibility_check_no_poses个点
# 如果任何一点违反约束，则拒绝该轨迹。不影响优化过程本身，但影响最终接受的轨迹
publish_feedback: false # 布尔值参数，控制是否发布TEB优化过程的反馈信息
# 启用后会发布/move_base/TebLocalPlannerROS/teb_feedback话题
shrink_horizon_backup: false # 允许规划者在自动检测到问题（例如不可行性）的情况下暂时缩小视野 （50%）。
# 另请参阅参数 shrink_horizon_min_duration。
shrink_horizon_min_duration: 10.0 # 指定在检测到不可行轨迹的情况下减小的水平的最小持续时间
#（请参阅参数 shrink_horizon_backup以激活缩小的水平模式）。
#-----------------------------------------------------------------------
#----------------------------------------------------------------------
#---------------------------机器人约束参数-------------------------------
#-----------------------------------------------------------------------
#-----------------------------------------------------------------------
holonomic_robot: true # 明确声明为全向机器人
max_vel_x: 0.5 #@ # 机器人的最大平移速度（米/秒）
max_vel_x_backwards: 0.3 #@ # 机器人向后行驶时的最大绝对平移速度，以米/秒为单位。
max_vel_y: 0.4 #@ # 机器人的最大横向速度(仅全向移动机器人需要调整)
max_vel_theta: 0.8 #@ # 机器人的最大角速度（以弧度/秒为单位）
acc_lim_x: 0.8 #@ # 机器人的最大平移加速度（米/秒^2）
acc_lim_y: 0.6 #@ # 机器人的最大横向加速度(仅全向移动机器人需要调整)
acc_lim_theta: 1.0 #@ # 机器人的最大角加速度（以弧度/秒为单位^2）
scaling_speed: 0.3 # 当距离障碍物 < min_obstacle_dist 时，速度按比例降低
use_proportional_saturation: true # 控制是否对机器人的速度/加速度命令应用比例饱和（proportional saturation）策略
# 一种优化约束处理方式，影响TEB如何将优化结果转换为实际控制命令
# true（比例饱和）：当机器人的速度/加速度接近限制值时，TEB会按比例缩放所有控制输入，使其平滑过渡到饱和状态
# false（硬截断）：直接截断超出限制的值（例如直接将超速设为 max_vel_x），可能导致不连续的加速度。
# ********************** Carlike robot parameters ********************
# 以下三个参数仅当小车为阿克曼结构时需要调整
min_turning_radius: 0 # carlike 机器人的最小转弯半径（对于差速机器人和全向移动机器人，设置为零）。
wheelbase: 0 #@ # 后轴和前轴之间的距离
cmd_angle_instead_rotvel: false # 控制TEB输出转向命令的形式
# true：发布转向角（steering angle）命令
# false：发布旋转速度（rotational velocity）命令
# 全向移动机器人设置为false
#-----------------------------------------------------------------------
#-----------------------------------------------------------------------
#---------------------------机器人模型参数-------------------------------
#-----------------------------------------------------------------------
#-----------------------------------------------------------------------
# costmap_common_params中的footprint用于全局/局部代价地图的障碍物检测，防止机器人碰撞静态/动态障碍物
# TEB中的footprint_model仅用于TEB规划器，局部轨迹优化时动态避障，考虑机器人形状与轨迹可行性（如窄通道通过性）
# 两者定义的顶点坐标应完全一致
footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
type: "polygon"
# radius: 0.36 # for type "circular"
# line_start: [-0.3, 0.0] # for type "line" 线模型起始坐标
# line_end: [0.3, 0.0] # for type "line" 线模型尾部坐标
# front_offset: 0.2 # for type "two_circles" 前圆心坐标
# front_radius: 0.2 # for type "two_circles" 前圆半径
# rear_offset: 0.2 # for type "two_circles" 后圆心坐标
# rear_radius: 0.2 # for type "two_circles" 后圆半径
# vertices: [ [0.171, -0.128], [0.171, 0.128], [-0.171, 0.128], [-0.171, -0.128] ] # for type "polygon"多边形边界点
vertices: [ [0.10, -0.05], [0.10, 0.05], [-0.10, 0.05], [-0.10, -0.05] ] # for type "polygon"多边形边界点
is_footprint_dynamic: false # 控制是否考虑机器人的动态足迹变化
# 决定碰撞检测时使用的机器人轮廓模型
# 当禁用（false）时：使用固定的机器人轮廓
#-----------------------------------------------------------------------
#-----------------------------------------------------------------------
#---------------------------目标容差参数-------------------------------
#-----------------------------------------------------------------------
#-----------------------------------------------------------------------
# GoalTolerance 目标点位置误差范围
# 设置太小，可能导致车辆在目标点位置振荡，设置太大，可能导致车辆不在目标点位置停止
xy_goal_tolerance: 0.06 #@ # 机器人到达目标点时，在XY平面上的最大允许位置误差（单位：米）
yaw_goal_tolerance: 0.1 #@ # 到达目标点时允许的朝向角度误差（单位：弧度）
free_goal_vel: false # 移除目标约束，以便机器人能够以最大速度到达目标
#-----------------------------------------------------------------------
#-----------------------------------------------------------------------
#---------------------------障碍物参数----------------------------------
#-----------------------------------------------------------------------
#-----------------------------------------------------------------------
min_obstacle_dist: 0.17 #@ # 这是机器人中心与障碍物之间的最小允许距离（单位：米）。
# TEB算法会确保规划出的路径始终保持机器人与障碍物之间的距离不小于这个值。
# 增大此值会使机器人更加保守，与障碍物保持更远距离，适合安全要求高的场景
# 减小此值允许机器人更靠近障碍物，适合狭窄空间但可能增加碰撞风险
# 使用车辆轮廓配置footprint_model配合膨胀距离即可实现较好效果。
# 请注意，此处设置的机器人轮廓必须与局部耗费地图中的一致
inflation_dist: 0.17 #@ # 这是障碍物的膨胀距离（单位：米）。
# TEB算法会将障碍物向外膨胀这个距离，在规划时把膨胀后的区域也视为障碍物。
include_costmap_obstacles: true # 是否考虑代价地图(costmap)中的障碍物信息。
# 当设置为true时，TEB算法会使用costmap中的障碍物信息进行路径规划；设置为false时则忽略。
costmap_obstacles_behind_robot_dist: 0.3 # 考虑机器人后方多远的costmap障碍物（单位：米）。
# 这个参数定义了TEB算法会考虑机器人后方多大范围内的障碍物。
# 增大此值：考虑更多后方障碍物，适合需要频繁后退的场景，但会增加计算量
# 减小此值：忽略更远的后方障碍物，减少计算负担，适合主要前进的场景
obstacle_poses_affected: 10.0 #@ # 每个障碍物影响的未来位姿的数量。这个参数控制每个障碍物会影响优化时未来多少个机器人位姿。
# 增大此值：障碍物对轨迹的影响更持久，路径更安全但可能过于保守
# 减小此值：障碍物影响范围更短，路径更灵活但安全性降低
dynamic_obstacle_inflation_dist: 0.17 #@ # 动态障碍物的膨胀距离（单位：米）。TEB会在此距离内对动态障碍物施加避障代价，模拟安全余量。
# 值越大，机器人会更早、更远地避开动态障碍物，但可能导致路径绕行。
# 值过小可能导致避障响应延迟，尤其在高速场景下。
include_dynamic_obstacles: true # 决定TEB是否考虑动态障碍物（如移动的行人、车辆等）
# true：算法会将动态障碍物的预测轨迹纳入避障规划。
# false：仅处理静态障碍物，忽略动态障碍物的运动状态。
#costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH"
#costmap_converter_spin_thread: True
#costmap_converter_rate: 5
legacy_obstacle_association: false # 控制障碍物关联（匹配）算法的版本。
# true：使用旧版关联逻辑（可能基于简单距离阈值）。
# false：启用改进后的关联策略（如考虑运动状态或更复杂的匹配逻辑）
obstacle_association_force_inclusion_factor: 1.0
#-----------------------------------------------------------------------
#-----------------------------------------------------------------------
#---------------------------优化参数------------------------------------
#-----------------------------------------------------------------------
#-----------------------------------------------------------------------
# no_inner_iterations 和 no_outer_iterations 是控制优化过程迭代次数的关键参数，直接影响路径优化的收敛性和计算效率。
# 外层迭代和内层迭代是优化路径的核心机制，二者协同工作以解决带约束的非线性优化问题。它们的核心区别在于优化目标和作用范围
# 内层迭代：在固定优化问题结构（如障碍物关联、权重参数）的情况下，
# 对弹性带（Elastic Band）的位姿和时间间隔进行局部梯度下降优化，最小化代价函数（包括路径平滑性、避障、速度约束等）。
# 在障碍物关联和约束固定的情况下，快速调整路径位姿，使其满足当前假设的最优解。
# 类似于“在已知敌人位置的情况下，调整跑步路线以绕开它们”。
# 外层迭代：重新评估和调整优化问题的结构，包括：更新障碍物关联（如动态障碍物的匹配、是否忽略某些障碍物）。
# 调整代价权重（如增加靠近障碍物时的避障权重）。处理非线性约束的松弛（如对暂时无法满足的约束放宽限制）。
# 每次外层迭代后，根据最新优化结果重新判断障碍物的影响。
# 外层迭代和内层迭代协调配合，如：
# 第一次外层迭代：关联最近的障碍物，内层迭代生成初步避障路径。
# 第二次外层迭代：发现路径靠近另一个动态障碍物，重新关联并调整权重。
# 第三次外层迭代：验证路径是否满足所有约束，若收敛则退出。
no_inner_iterations: 5 #@ #内部迭代次数，降低迭代次数（no_inner/outer_iterations） （显著）
no_outer_iterations: 4 #@ #外部迭代次数，降低迭代次数（no_inner/outer_iterations） （显著）
optimization_activate: true # 是否启用优化
optimization_verbose: false # 是否打印优化信息
penalty_epsilon: 0.1 #@ # 惩罚系数（接近减速系数）
# 提高penalty_epsilon。此项为速度等约束提供类似膨胀层的缓冲效果。接近限速将产生一定的惩罚值，让小车提前减速达到缓冲效果
# 保障小车安全
obstacle_cost_exponent: 4 # 障碍物代价函数的指数，决定了代价随距离变化的陡峭程度（用于控制障碍物代价函数的非线性程度）
# 值越大：距离障碍物稍近时代价急剧上升，机器人会更早、更强烈地避开障碍物（“保守”避障）。
# 值越小：代价变化平缓，机器人可能更贴近障碍物（“激进”避障）。
# weight_max_vel_x、weight_max_vel_theta、weight_acc_lim_x 和 weight_acc_lim_theta 是用于软约束优化的权重参数，
# 它们通过惩罚函数（Penalty Function）来鼓励机器人尽量满足运动学限制（如最大速度、加速度），而非严格强制（硬约束）。
# 值越大，对速度要求越严格，机器人速度越不容易超过运动学限制
weight_max_vel_x: 10.0 #@ # x方向最大速度权重 避免机器人运行速度过快。
weight_max_vel_theta: 10.0 #@ # 满足最大允许角速度的优化权重 避免机器人旋转过快，导致不稳定或打滑。
weight_acc_lim_x: 10.0 #@ # 满足最大允许平移加速度的优化权重 避免急加速/急减速，提高运动平滑性。
weight_acc_lim_theta: 10.0 #@ # 满足最大允许角加速度的优化权重 避免旋转加速度过大。
weight_kinematics_nh: 50.0 # 用于控制非完整运动学约束的优化权重，主要影响差速驱动机器人（如两轮差速机器人）的运动轨迹生成。
# 差速机器人（如TurtleBot）的运动受非完整约束限制：只能沿当前朝向移动（不能直接横向移动），转弯需通过左右轮速差实现，轨迹需满足曲率连续性
# 全向机器人无此约束，可直接禁用（设为 0）。
weight_kinematics_forward_drive: 500.0 # 用于强制机器人仅选择前进方向（正平移速度）的优化权重。较小的重量（例如 1.0）仍然允许倒车。1000 左右的值几乎可以阻止向后行驶（但不能保证）。
weight_kinematics_turning_radius: 0 # 最小转弯半径权重，设置越大小车越容易达到最小转弯半径 设置范围：0-1
weight_optimaltime: 3.0 #@ # 在轨迹优化过程中，该参数鼓励算法生成时间最短的轨迹（即让机器人尽可能快地到达目标）。
# 主要目的是让小车快速到达目标点
# weight_optimaltime 越高，机器人可能更“冒险”贴近障碍物以节省时间。
# 大幅降低weight_optimaltime，设置较大的inflation_dist，
# 并稍微增加weight_inflation。时间权重降低至0.3以下时，
# 车辆速度明显平稳，不会向最高速度加速。膨胀距离大时将远离障碍物行驶。
weight_shortest_path: 0.0 # weight_shortest_path 是一个用于优化路径长度的权重参数，
# 它直接影响机器人在导航过程中是否倾向于选择更短的路径（即使轨迹可能更曲折或更接近障碍物）
# 在轨迹优化过程中，该参数鼓励算法生成几何长度最短的轨迹
# 通常为 0.0（默认不优化路径长度，优先时间最优）
weight_obstacle: 80.0 #@ # 障碍物避让的优化权重（越高越倾向于避障）
weight_inflation: 0.2 #@ # 该参数用于设置障碍物周围具有非零惩罚成本的缓冲区的权重。
# Inflation Penalty 的优化权重 （应该很小）
# 当机器人的轨迹进入到障碍物的膨胀区域（由inflation_dist参数定义）时，weight_inflation决定了对轨迹施加的惩罚力度。
# 较大的weight_inflation值会使算法更倾向于让轨迹远离障碍物的膨胀区域，以避免机器人与障碍物发生碰撞。
weight_dynamic_obstacle: 20.0 #@ # not in use yet
weight_dynamic_obstacle_inflation: 1.0 #@ # 优化过程中，和动态障碍物膨胀区的权重
weight_viapoint: 50.0 # 控制TEB路径接近全局路径航点的重要性权重
# 值越大，TEB路径会越严格地经过航点
# 值越小，路径可以更自由地偏离航点以避开障碍物。应与weight_obstacle权重平衡调整
weight_adapt_factor: 2.0 # 控制优化权重在障碍物附近的自适应调整因子。实现动态调整障碍物权重
# 当检测到附近有障碍物时，实际障碍物权重 = weight_obstacle × weight_adapt_factor
# 无障碍物时，保持原始weight_obstacle值
weight_global_plan: 1.5 # 默认0.5 → 强制贴近全局直线
#-----------------------------------------------------------------------
#-----------------------------------------------------------------------
#-----------------------在独特的拓扑结构中进行并行规划-------------------
#-----------------------------------------------------------------------
#-----------------------------------------------------------------------
# TEB算法的独特拓扑(homotopy class)相关参数用于处理环境中可能存在多条不同拓扑路径的情况，这些参数允许算法同时考虑多条不同路径候选并选择最优解。
enable_homotopy_class_planning: true # 是否启用独特拓扑规划
# true：同时探索多条不同拓扑路径(如障碍物左侧和右侧的路径)
# false：只优化单一路径
# 调整建议：在复杂障碍物环境(如锥桶场地)中启用，简单走廊式环境可禁用以提高效率
enable_multithreading: true #@ # 是否启用多线程并行规划
# true：并行优化多条候选路径
# false：串行处理
# 启用时显著增加CPU使用率但能发现更优路径
# 关闭时规划速度更快但可能错过最优路径
# 此功能将严重影响规划器性能。当路径中存在大量不连续分布的障碍物（如锥桶）并产生大量可行路径时，此功能有良好的效果。
# 相反，若在单一路径上运行，则没有必要时用此功能
max_number_classes: 4 #@ # 允许的线程数，限制同时优化的候选路径数量
selection_cost_hysteresis: 1.0 # 指定新候选路径必须有多少轨迹成本 先前选择的轨迹以进行选择（如果new_cost <old_cost * factor则选择）
selection_prefer_initial_plan: 1.0 # 为初始计划的等价类中的轨迹指定在区间(0,1)中的成本降低。
selection_obst_cost_scale: 100.0 # 为了选择“最佳”候选人，障碍成本项的额外缩放(new_obst_cost: obst_cost*factor)
selection_alternative_time_cost: false # 是否使用另一个成本项，用于替换时间成本项（如果没有，则使用时间成本项）
# 减小局部耗费地图的大小 （显著）
roadmap_graph_no_samples: 15 # 表示在局部路径优化过程中，用于生成候选路径的采样点数量。
# TEB算法会在机器人的局部邻域内生成多条候选路径，这些路径由一系列采样点（即“橡皮筋”上的点）构成。
# roadmap_graph_no_samples指定了每条候选路径在优化过程中包含的位姿点数量。
# TEB算法采用滑动窗口（Sliding Window）的方式优化路径，每次仅处理局部的一段路径（称为一个"band"或"segment"）。
# roadmap_graph_no_samples 指定的是每个局部优化窗口内的采样点数量，而非全局路径的总点数。
# 也就是说如果目标点较近，局部优化窗口可能覆盖整条路径（此时总点数接近 roadmap_graph_no_samples）。
roadmap_graph_area_width: 2.0 # 定义局部路径采样区域的宽度（单位：米），即机器人当前位姿周围用于生成候选路径的横向范围。
# 算法会在机器人当前位置的左右两侧各扩展 roadmap_graph_area_width/2 的距离，形成一个带状区域，用于采样避障路径。
# 该参数指定了算法在局部路径优化时，允许生成的候选路径在横向（垂直于当前路径方向）上的最大偏移范围。用于生成候选路径（如向左绕或向右绕的选项）
# 较大的值允许更宽的避障机动，但可能导致路径偏离全局参考轨迹；较小的值会使路径更贴近原计划，但可能无法绕过密集障碍物。
roadmap_graph_area_length_scale: 2.0 # 该参数定义局部路径优化时采样区域的长度尺度，与 roadmap_graph_area_width 共同构成一个二维采样区域（长 × 宽）。
# 控制算法在路径方向上探索替代路径的范围。例如，在遇到障碍物时，TEB会在此长度范围内生成候选路径（如绕左或绕右）。
h_signature_prescaler: 1.0 #仅当您在本地成本图中观察到障碍物过多的问题时，才减小此参数
#请勿将其选择得太低，否则障碍物无法彼此区分（0.2 < 值 <= 1）
h_signature_threshold: 0.1
obstacle_heading_threshold: 1.0 #在障碍物航向和目标航向之间指定标量乘积的值
#以便将它们（障碍物）考虑在内进行探索
switching_blocking_period: 0.0 #指定允许切换到新的等效类之前需要终止的持续时间（以秒为单位）
viapoints_all_candidates: true
delete_detours_backwards: true
max_ratio_detours_duration_best_duration: 3.0
visualize_hc_graph: false
visualize_with_time_as_z_axis_scale: false
# Recovery
oscillation_recovery: true #指定是否启用振荡恢复
oscillation_v_eps: 0.1 #指定振荡恢复的最大速度误差（以m/s为单位）
oscillation_omega_eps: 0.1 #指定振荡恢复的最大角速度误差（以rad/s为单位）
oscillation_recovery_min_duration: 10 #指定振荡恢复的最小持续时间（以秒为单位）
oscillation_filter_duration: 10 #指定振荡恢复的滤波时间（以秒为单位）
魔乐社区（Modelers.cn) 是一个中立、公益的人工智能社区，提供人工智能工具、模型、数据的托管、展示与应用协同服务，为人工智能开发及爱好者搭建开放的学习交流平台。社区通过理事会方式运作，由全产业链共同建设、共同运营、共同享有，推动国产AI生态繁荣发展。
更多推荐
33
0- 0
已为社区贡献4条内容
所有评论(0)