Resilient Timed Elastic Band Planner for Collision-Free Navigation in Unknown Environments

In plain words

A self-driving robot needs a local planner: the part that decides how to steer right now, around nearby obstacles. The Timed Elastic Band (TEB) planner is a common choice. It adjusts a rough path into a trajectory that respects timing and motion limits. In crowded spaces, though, it can end up with a trajectory the robot cannot physically follow, and it then takes a long time to settle on a feasible one.

This paper adds a fallback. When the TEB trajectory is not drivable, a search over car-like motions (a hybrid A* search) builds a new drivable route. That route is smoothed so it keeps away from obstacles, and the smoothed route is used to restart TEB. Near the goal, an extra step lines the robot up with the goal's heading before it arrives.

The authors test the planner in a farm-like setting, in simulation and on a real vehicle, and compare it with TEB and an MPC planner at several obstacle densities. They report shorter trips and lower control effort in the denser setups. The target use is field robots on agricultural and industrial sites.

TL;DR

• RTEB is a recovery layer around TEB, a ROS2 local planner for car-like robots, not a replacement for it.
• When the TEB trajectory is infeasible, a hybrid A* search with a novel heuristic generates a drivable car-like path §3.1.1.
• A soft-constraint smoother with a Voronoi-field obstacle term refines that path §3.1.2, and TEB is reinitialised from it §3.
• Author-reported: lower traverse distance, time and control effort than TEB and NMPC in dense-obstacle tests, with gains concentrated at 30 to 50% obstacle density tab. 4. The abstract's "about 20%" is not a single number in the tables (see Pitfalls).
• No official code is published (see Paper vs code).

Problem

TEB formulates local planning as a sparse optimisation over a variable-length receding horizon, which is fast in open space §2. Its computation grows as the number of points that must pass a kinematic feasibility check grows, and in obstacle-rich scenes it can produce infeasible solutions and take longer to reach a consistent feasible trajectory §2. NMPC has the same feasibility-check cost, growing with the horizon length §2. The paper targets car-like robots, where sampling-based planners are said to fit poorly §2.

The paper assumes the robot follows a global path that is a set of straight segments between intermediate poses, generated without obstacle avoidance so that every local planner gets the same reference §3.2.1, §4.3.

Method

RTEB keeps TEB as the optimiser and adds three recovery steps. The overview is in fig. 1 and fig. 2.

1. Feasibility check and trigger. In each planning iteration the kinematic feasibility of the current trajectory is assessed. If it is infeasible, recovery starts §3. The paper does not define the feasibility test or threshold.

2. Hybrid A state propagation.* Car-like states are x = [x, y, θ] §3.1. Each motion uses a steering angle φ and arc length s, with turning radius k = L / tan(φ) and L the wheelbase §3.1. The discrete transition is (unnumbered display) §3.1.1:

\mathbf{x}_{t+1} = \mathbf{x}_t + \begin{cases} \begin{pmatrix} k\,(\sin(\theta_t + s/k) - \sin\theta_t) \\ -k\,(\cos(\theta_t + s/k) - \cos\theta_t) \\ s/k \end{pmatrix}, & \phi \neq 0 \\[1em] \begin{pmatrix} s\cos\theta_t \\ s\sin\theta_t \\ 0 \end{pmatrix}, & \phi = 0 \end{cases}

The control set uses steering δ with |δ| ≤ δ_max, δ = r_s · δ_max, and arc length a = r_a · s_max. For forward motion (v_t ≥ 0) the paper states s_max ≤ a ≤ 2 s_max; for backward motion, −2 s_max ≤ a ≤ −s_max §3.1.1. Parameters r_s = 0.3, r_a = 0.5 and s_max = 1.0 come from unbiased testing §3.1.1. Note the range issue in Pitfalls.

3. Hybrid A cost and heuristic.* The step cost adds a forward or backward penalty on arc length, a steering-times-arc term and a steering-change term. The priority adds a weighted Euclidean goal distance. Both are unnumbered displays §3.1.1:

g^p = g^c + \begin{cases} \lambda_f |a| & \zeta > 0 \\ \lambda_b |a| & \zeta \le 0 \end{cases} + \lambda_s |\delta|\,|a| + \lambda_{sc} |\delta - \phi_t|, \quad \zeta = \begin{cases} 1 & a > 0 \\ -1 & a \le 0 \end{cases}

f^p = g^p + \lambda_{heu}\, \eta\, \lVert \mathbf{x}_t - \mathbf{x}_g \rVert_2

Here η is a tie-breaking factor §3.1.1, citing horne2005dynamic in the source.

4. Voronoi obstacle field. The obstacle term uses a Voronoi field that is zero at d_obs^max and is shaped by the obstacle distance and the distance to the nearest Voronoi edge §3.1.2, eq. 3. It is described as improving on artificial potential fields in narrow passages §3.1.2.

F_v(x_p, y_p) = A(x_p, y_p) \cdot S(x_p, y_p) \cdot R(x_p, y_p) \quad [\text{eq. 3}]

A = 1 - \left(\frac{d_{obs}}{d_{obs}^{max}}\right)^2, \quad S = \frac{\lambda_v}{\lambda_v + d_{obs}}, \quad R = \frac{d_{vor}}{d_{obs} + d_{vor}} \quad [\text{eq. 3}]

5. Smoothing objective. The refinement minimises a weighted sum of obstacle, curvature and path terms §3.1.2, eq. 1:

J_{total} = \lambda_{obs} J_{obs} + \lambda_{cur} J_{cur} + \lambda_{path} J_{path} \quad [\text{eq. 1}]

The obstacle term sums F_v over the Q trajectory points eq. 2:

J_{obs} = \sum_{p=0}^{Q} F_v(x_p, y_p) \quad [\text{eq. 2}]

The curvature term penalises the squared gap between each signed curvature k_p and a signed bound ±k_max, where k_max = 1/ρ_min eq. 4, §3.1.2. The source names this term J_k in eq. 4 but J_cur in eq. 1:

J_k = \sum_{p=0}^{Q-1} \left( k_p - \sigma_p\, k_{max} \right)^2, \quad k_p = \frac{\operatorname{atan2}(\Delta y_{p+1}, \Delta x_{p+1}) - \operatorname{atan2}(\Delta y_p, \Delta x_p)}{\Delta x_p} \quad [\text{eq. 4}]

Here σ_p is 1, 0 or −1 for k_p > 0, = 0 or < 0, with Δx_p = x_p − x_{p+1} and Δy_p = y_p − y_{p+1} eq. 4. The path term penalises changes between successive displacement vectors §3.1.2, eq. 5:

J_{path} = \sum_{p=0}^{Q-1} \left\lVert (\Delta x_{p+1}, \Delta y_{p+1}) - (\Delta x_p, \Delta y_p) \right\rVert^2 \quad [\text{eq. 5}]

6. Reinitialise TEB. The smoothed path replaces the current TEB poses, and TEB optimisation resumes from there instead of its own slower independent search §3.

7. Goal alignment. Near the goal, an intermediate pose is placed a distance d_i behind the goal along its heading. The robot heads for the goal once it is within d_r of that pose §3.1.3. The unnumbered display is:

x_i = x_g - d_i \cos(\phi_g), \quad y_i = y_g - d_i \sin(\phi_g) \quad [\text{§3.1.3}]

The paper's example values are d_i = 1.0 m ("e.g.") and d_r = 0.1 m §3.1.3.

Pseudocode for the recovery loop. Steps 1, 6 and 7 are as described above; the TEB optimiser internals are not part of this paper §2:

# RTEB planning iteration  [§3]
# Input:  teb_trajectory, robot_pose, goal_pose
# Output: optimised teb_trajectory
def rteb_iteration(teb_trajectory, robot_pose, goal_pose):
    # Recover when the TEB trajectory is not kinematically feasible  [§3]
    if not is_kinematically_feasible(teb_trajectory):
        # Build a car-like path with hybrid A*  [§3.1.1]
        raw_path = hybrid_astar(robot_pose, goal_pose)
        # Push the path away from obstacles with the smoother  [§3.1.2]
        smooth_path = smooth(raw_path)
        # Replace the TEB poses with the smoothed path  [§3]
        teb_trajectory = reinitialise_teb(smooth_path)
    # Align the heading near the goal  [§3.1.3]
    if near_goal(robot_pose, goal_pose):
        teb_trajectory = goal_alignment(teb_trajectory, goal_pose)
    # Run the TEB optimisation on the current trajectory  [§2] [§3]
    return optimise_teb(teb_trajectory)

Pseudocode for the smoothing objective (weights as stated in the paper):

# Smoothing objective over a trajectory of Q + 1 points  [eq. 1] [§3.1.2]
# Input:  points (Q + 1, 2), k_max
# Output: scalar cost
def smoothing_cost(points, k_max):
    # Obstacle term: sum of the Voronoi field over the points  [eq. 2]
    obstacle_cost = sum(voronoi_field(point) for point in points)
    # Curvature term against the signed bound  [eq. 4]
    curvature_cost = curvature_penalty(points, k_max)
    # Path term on successive displacements  [eq. 5]
    path_cost = displacement_penalty(points)
    # Weights 0.5, 0.3 and 0.2  [§3.1.2]
    return 0.5 * obstacle_cost + 0.3 * curvature_cost + 0.2 * path_cost

Pseudocode for one hybrid A* expansion cost:

# Hybrid A* cost of one motion primitive  [§3.1.1]
# Input:  g_cost (accumulated), arc (signed), steer, steer_prev
# Output: g_cost of the child node
def expansion_cost(g_cost, arc, steer, steer_prev):
    # Penalise forward or backward arc length  [§3.1.1]
    if arc > 0:
        motion_cost = lambda_f * abs(arc)
    else:
        motion_cost = lambda_b * abs(arc)
    # Penalise steering scaled by arc length  [§3.1.1]
    steer_cost = lambda_s * abs(steer) * abs(arc)
    # Penalise a change of steering angle  [§3.1.1]
    change_cost = lambda_sc * abs(steer - steer_prev)
    return g_cost + motion_cost + steer_cost + change_cost

Hyperparameters

Values stated in the paper text. The paper gives no table of hyperparameters. Items marked "not stated" are needed for an implementation.

Results

Author-reported. All numbers are from the paper; none were rerun. Test platform: Agilex Hunter 2.0 with Trimble RTK-GPS, Livox LiDAR and an Intel NUC i7-10710U §4.2, §3.3. Environment: a section of a strawberry farm, replicated in Gazebo §4.1. Each experiment's exact setting (simulation or real) is stated only where noted.

Goal alignment against TEB (values μ ± σ, setting not stated, parallel-parking-like displacement sweep, 10 repetitions per scenario) tab. 2:

Paired t-test p-values as reported §4.3: traverse time 0.002, control effort 0.005, traverse distance 0.209 (not significant). Which samples were paired is not stated.

Narrow gaps in five randomised simulated environments (four start-goal pairs, 20 trials each; four planners) tab. 3:

Obstacle density sweep, RTEB vs TEB vs NMPC (5 runs each; NaN = planner did not reach the target; setting per row group) tab. 4:

Speed and planning frequency for the same runs tab. 4. The speed cells show only the ± spread, with no mean shown in the source:

Derived from tab. 4 by the compiler, not stated by the authors: at 30% to 50% density, RTEB's traverse distance is 10% to 33% lower than TEB's, its traverse time 23% to 41% lower, and its control effort 7% to 49% lower. The abstract's "approximately 20%" does not match a single table cell.

The authors also say that RTEB's recovery is inactive in obstacle-free environments, so it adds no cost there §4.3.

Paper vs code

No official code. The paper's only code link is a footnote to github.com/GPrathap/topological_navigation §3.2.1, fn. 1. That repository is a fork of LCAS/topological_navigation (Apache-2.0, default branch humble-dev). It holds the topological map manager, a global planning component, not the RTEB planner. Its file tree (304 entries) contains no TEB, RTEB, hybrid A*, Voronoi or smoothing files. GitHub searches for "RTEB", "resilient timed elastic band", "timed elastic band hybrid astar voronoi" and "kulathunga teb" returned no relevant repository. The paper states the navigation stack is written in C++ as ROS2 plugins §4.2 but releases none of it.

Pitfalls

• Recovery trigger is undefined. The paper says feasibility is "assessed" each iteration §3 but gives no test, threshold or number of points checked. An implementation must choose one, and the choice decides when recovery runs.
• Arc-length range is inconsistent. r_a = 0.5 and s_max = 1.0 give a = 0.5, but the stated forward range is [s_max, 2 s_max] = [1, 2] §3.1.1. Either r_a, the range, or s_max is wrong.
• Notation clashes. The problem statement names the control u = [φ, s] with φ the steering angle §3.1. Hybrid A* uses δ for steering and a for arc length §3.1.1. The step cost uses φ_t for the previous steering, undefined, and k = L/tan(φ) uses φ as steering. Read φ_t as the previous steering angle.
• Heuristic norm is undefined. The goal distance is written as ‖x_t − x_g‖₂ with x = [x, y, θ] §3.1, but which components enter the norm is not stated. Using only (x, y) is a choice.
• Curvature formula divides by a coordinate difference. eq. 4 divides the heading change by Δx_p, the signed x-difference between consecutive points, not by arc length. Where consecutive points share an x coordinate, k_p is undefined or unbounded. The paper does not address this.
• Path term does not measure path length. The text says J_path measures "deviation in path length" §3.1.2, but eq. 5 penalises the squared difference between successive displacement vectors, which is a second-difference (smoothness) penalty.
• Curvature term is named two ways. eq. 4 uses J_k, eq. 1 uses J_cur §3.1.2.
• Index ranges differ. The obstacle sum runs p = 0 to Q (Q + 1 points), the curvature and path sums run to Q − 1 eq. 2, eq. 4, eq. 5. The paper does not say how the endpoints are handled.
• Voronoi field can go negative. A = 1 − (d_obs / d_obs^max)² is negative once d_obs > d_obs^max, and no clipping is stated eq. 3. λ_v and d_obs^max are not given.
• Goal-alignment values are examples. d_i = 1.0 m is written as "e.g." with d_i ∈ C, a set the paper never defines §3.1.3. The approach condition is also worded loosely ("will start moving... as long as...") §3.1.3.
• Reinitialisation mechanics are not spelled out. How the smoothed path maps onto TEB's time-stamped poses and its hypergraph is not described §3, §2. The paper refers to TEB's internals without detail.
• Planning frequency is defined two ways. Table 3 calls it "maximum planning frequency" and gives TEB 13 Hz tab. 3. Section 4.3 defines it as "the average time taken per planning iteration" §4.3, and Table 4 gives TEB 15 to 20 Hz tab. 4. These figures cannot be compared as the same metric.
• Obstacle density definition is ambiguous. The paper defines density as a percentage computed from the free space before and after obstacles are placed §4.3. A literal reading gives the share of free space left, so it is unclear whether 20% means 20% of the area occupied. Clarify it before comparing densities across papers.
• Table 2 scenario 5 looks like a typo. RTEB effort goes 13.00 and 18.79 in scenarios 3 and 4, then drops to 8.96 in scenario 5 tab. 2. Nothing in the paper explains the drop.
• Table 2 scenario 3 contradicts the text. RTEB's distance there is 5.95 m against TEB's 5.41 m tab. 2, while the text says RTEB has "lower mean values... across all metrics" and shorter distances across scenarios §4.3.
• Text and table disagree on values. The text says TEB and RTEB both cover "around 63 m" at 20% simulated density §4.3, but Table 4 gives 65.32 and 65.33 m. The text also lists "216 s and 352 s" as RTEB and TEB, which is the reverse of the table order (TEB 352 s, RTEB 216 s) §4.3, tab. 4.
• Speed table is incomplete. The caption says "Speed (min, max)" tab. 4, but the cells show only ± values, with no means. The speed comparison cannot be checked from the paper.
• Statistical test is under-specified. The paired t-tests §4.3 do not state the pairing, sample size or whether they pool scenarios.
• Setting is unclear for the goal-alignment study. "Controlled setup" §4.3 does not say whether it ran in Gazebo or on the robot.
• Global path is part of the method's behaviour. The experiments use a straight-line global path generated without obstacle avoidance so that all planners share it §3.2.1, §4.3. The paper says other global planners could be used §3.2.1, but reports no such test.
• Real-time claim is loose. The abstract says the planner "meets real-time computational requirements" [abstract], but no real-time bound is stated. The reported frequency figures are the only evidence.
• Car-like focus. The paper argues the approach suits car-like vehicles, where differential drive may not be feasible or desirable §1. The reported experiments use a single platform §4.2.
• Hidden costs. The paper does not report the cost of the hybrid A* search or the smoothing separately from TEB. Planning frequency is reported only for the whole planner tab. 3, tab. 4.
• Obstacle-free behaviour. Recovery is inactive without obstacles §4.3, so benefits show up only in cluttered settings.
• NMPC integration is future work. The authors say combining RTEB with NMPC is challenging because NMPC uses a fixed receding horizon, and they foresee a cascade design §5.

Lineage

• Builds on: Timed Elastic Band planner (Rösmann et al.), cited as rosmann2015timed, rosmann2017integrated and 8206458 in the paper §1, §2, §3. No card in this wiki.
• Compared against: TEB (above), NMPC (rosmann2021online, §2, §4.3), MPPI (kim2022smooth, tab. 3).
• Superseded by: not stated in this wiki.