PaperlayerView exact revision on arXiv

Source-linked AI summary

Bridging the Gap Between Safety and Real-Time Performance in Receding-Horizon Trajectory Design for Mobile Robots

Shreyas Kousik, Sean Vaskov, Fan Bu, Matthew Johnson-Roberson, Ram Vasudevan

arXiv:1809.06746v2cs.ROcs.SYeess.SY

TL;DR

Robots planning with limited sensor horizons must generate safe trajectories in real time while ensuring another safe plan is available before the current one ends. RTD uses an offline Forward Reachable Set to constrain online trajectory optimization. In simulations, it reached goals comparably often to RRT and NMPC while recording zero crashes.

  • Problem

    Receding-horizon planners must provide safe, dynamically feasible trajectories in real time, while existing approaches trade model complexity and planning speed against guarantees.

    Source: §1 · p. 1, §2.5 · p. 9, §9.7 · p. 37

  • Method

    RTD computes an offline Forward Reachable Set bounding robot motion while tracking parameterized trajectories, then maps obstacles to unsafe parameters for online optimization.

    Source: §3.1 · p. 11, §11 · p. 40

  • Results

    RTD reached desired goals comparably often to RRT and NMPC for both robots and had 0 crashes in all simulations.

    Source: §9.7 · p. 37

  • Takeaways & Limitations

    RTD provides safe, persistently feasible, real-time trajectory planning through obstacle constraints and trajectory plans that preserve a safe stopping option.

    Source: §11 · p. 40

    RTD sometimes cannot reach the goal, but still brakes safely.

    Source: §9.7 · p. 37

Abstract

from arXiv · show

To operate with limited sensor horizons in unpredictable environments, autonomous robots use a receding-horizon strategy to plan trajectories, wherein they execute a short plan while creating the next plan. However, creating safe, dynamically-feasible trajectories in real time is challenging; and, planners must ensure persistent feasibility, meaning a new trajectory is always available before the previous one has finished executing. Existing approaches make a tradeoff between model complexity and planning speed, which can require sacrificing guarantees of safety and dynamic feasibility. This work presents the Reachability-based Trajectory Design (RTD) method for trajectory planning. RTD begins with an offline Forward Reachable Set (FRS) computation of a robot's motion when tracking parameterized trajectories; the FRS provably bounds tracking error. At runtime, the FRS is used to map obstacles to parameterized trajectories, allowing RTD to select a safe trajectory at every planning iteration. RTD prescribes an obstacle representation to ensure that obstacle constraints can be created and evaluated in real time while maintaining safety. Persistent feasibility is achieved by prescribing a minimum sensor horizon and a minimum duration for the planned trajectories. A system decomposition approach is used to improve the tractability of computing the FRS, allowing RTD to create more complex plans at runtime. RTD is compared in simulation with Rapidly-Exploring Random Trees and Nonlinear Model-Predictive Control. RTD is also demonstrated in randomly-crafted environments on two hardware platforms: a differential-drive Segway, and a car-like Rover. The proposed method is safe and persistently feasible across thousands of simulations and dozens of real-world hardware demos.

1 Introduction

Limited sensor horizons motivate receding-horizon planning, where robots plan while executing and must meet safety, persistent-feasibility, and real-time requirements. The introduction positions RTD as a reachability-based approach to these tradeoffs and outlines its method and contributions.

1 Introduction

  • Receding-horizon planning responds to new sensor information as the robot moves, so each plan must be safe and a subsequent safe plan or stopping maneuver must remain available.Planning time must not exceed the duration of the plan being executed.Source: §1 · p. 1

1.1.1 Sample-Based Methods

  • Sample-based planners trade finer discretization, which can strengthen safety claims, for computational cost and real-time performance.Persistent feasibility adds pressure to simplify dynamics or collision checking, risking weaker safety guarantees or more obstacle buffering.Source: §1.1.1 · p. 3

1.1.2 Nonlinear Model Predictive Control

  • NMPC discretizes time and often linearizes dynamics; faster computation can require simplifications that complicate safety guarantees and persistent feasibility.The literature review concludes that NMPC faces the same safety versus real-time tradeoff as sample-based methods.Source: §1.1.2 · pp. 3–4

1.1.3 Reachability-based Methods

  • Reachability-based planners use precomputed reachable sets for collision avoidance, but may be conservative or struggle to guarantee a feasible plan.Existing approaches can prescribe a finite trajectory set or allow arbitrary trajectories, each with limitations discussed in the paper.Source: §1.1.3 · pp. 4–5

1.2.1 Proposed Method in Context

  • RTD uses reachability analysis to tune hyperparameters without losing safety guarantees, and ensures a fail-safe maneuver in every plan for persistent feasibility.The present work extends prior RTD to real-time planning for higher-dimensional ground robots.Source: §1.2.1 · p. 5

1.2.2 Proposed Method Overview

  • RTD computes an offline FRS that contains the robot’s motion while tracking parameterized trajectories, accounting for bounded tracking error.The method begins with high-fidelity and trajectory-producing models, then uses tracking-error bounds to compute the FRS.Source: §1.2.2 · p. 5
  • At runtime, RTD maps obstacles through the FRS to unsafe trajectory parameters, then optimizes over safe parameters while preserving a fail-safe stopping option.The plans are designed to be long enough for the robot to stop safely if no new trajectory is found.Source: §1.2.2 · p. 5, §1.2.1 · p. 5

1.3 Contributions

  • RTD’s three contributions are system decomposition for forward reachability, a finite discrete obstacle representation for safe real-time planning, and demonstrations on two robots.The demonstrated platforms are a differential-drive Segway and a car-like Rover.Source: §1.3 · p. 5

2 Dynamic Models

This section defines how RTD plans in a low-dimensional trajectory-parameter space while a feedback controller tracks trajectories under a high-fidelity robot model. It describes bounding estimation and tracking errors, accounting for the robot’s footprint, and representing tracking error as a time-varying disturbance to support later FRS-based trajectory screening.

2.1 High-Fidelity and Trajectory-Producing Models

  • RTD uses a simpler trajectory-producing model to plan parameterized trajectories for a high-fidelity model, with a fixed planning-time limit τplan per iteration.Each trajectory lasts T > 0, and the parameters remain fixed over that horizon; planning directly with the high-fidelity model is challenging.Source: §2.1 · p. 6, §2 · p. 6

2.2 Required States

  • For safe obstacle avoidance, the trajectory-producing state must account for the robot’s entire rigid-body footprint, while the high-fidelity model includes speed and bounded yaw rate where applicable.A circular footprint or no yaw motion allows body points to be treated as the center-of-mass position for obstacle avoidance.Source: §2.2 · p. 7

2.3 Low-Level Controllers

  • RTD allows users to choose feedback controllers to track parameterized trajectories; the controller need only track trajectories in the compact parameterized family.The framework is agnostic to feedback type, and the Segway example uses a PD controller.Source: §2.3 · p. 8

2.4 Projection Operators

  • Projection operators relate the high-fidelity and trajectory-producing state spaces and select lower-dimensional subspaces for tracking-error analysis and reachability computations.They project points or sets into subspaces and can back-project a subspace set into the full trajectory-producing state space.Source: §2.4 · p. 8

2.5 Bounding Sources of Error

  • Safety analysis bounds both state-estimation error and trajectory-tracking error, which can be represented by a bounded function g over shared states.Larger conservative bounds enlarge the offline Forward Reachable Set and make runtime trajectory selection more conservative; hardware estimation errors are accounted for by expanding obstacles by ±εx and ±εy.Source: §2.5 · pp. 9–10
  • The tracking model adds a bounded, time-varying disturbance d to trajectory-producing dynamics, and a suitable d can make its shared-state trajectory match the high-fidelity model.The disturbance is bounded componentwise in [−1, 1] almost everywhere over the planning horizon.Source: §2.5 · p. 10
  • For the Segway, example tracking-error functions are time-varying polynomials of degree 4 in x and degree 3 in y.The example describes mismatch between the high-fidelity Segway model and the trajectory-producing model’s Dubins paths.Source: §2.5 · p. 10
  • The tracking model is used to precompute the robot’s Forward Reachable Set (FRS), which identifies unsafe trajectories at runtime.The text assigns FRS precomputation to Section 3 and runtime unsafe-trajectory identification to Section 7.Source: §2.5 · p. 10

3 Forward Reachable Set Computation

RTD precomputes a Forward Reachable Set (FRS) that bounds the robot’s motion while tracking parameterized trajectories, then optimizes online over safe trajectory parameters. Its SOS-based computation gives conservative reachable-set approximations, but memory demands rise sharply with system dimension.

3 Forward Reachable Set Computation

  • RTD precomputes an FRS over a time interval, then selects safe trajectory parameters online using a user-specified cost function.The precomputation captures possible trajectories and associated parameters over [0, T].Source: §3 · p. 10

3.1 Problem Formulation

  • The FRS contains positions reachable by the high-fidelity robot model while tracking trajectories from the simplified model, including tracking error over horizon T.The FRS is formulated through an infinite-dimensional program whose feasible solutions conservatively approximate the reachable set.Source: §3.1 · pp. 11–12

3.2.1 Computing the FRS

  • The SOS relaxation yields outer approximations whose 1-superlevel sets contain the FRS and whose functions converge from above to its indicator in L1.The implementation relaxes the continuous-function program to polynomial functions and transforms its inequalities into SOS constraints.Source: §3.2.1 · pp. 12–13

3.3 Sums-of-Squares Memory Usage

  • SOS program memory grows as O((n + 1)^l) for fixed l and O(l^(n+1)) for fixed n, motivating decomposition for higher-dimensional systems.The polynomial decision variables are represented by monomials, making the program memory-intensive.Source: §3.3 · p. 13
  • The Rover’s full-system D3 required 504 GB of RAM, and D4—with approximately 3.8×10^5 free variables—could not be solved on a 3.5 TB RAM computer.The Segway D5 required approximately 1.4 × 10^5 free variables; Rover D3 required approximately 1.1 × 10^5.Source: §3.3 · p. 13

4 System Decomposition

System decomposition makes FRS computation tractable for higher-dimensional robot models by computing reachable sets for lower-dimensional subsystems and reconstructing an outer approximation. For the Rover, the reconstruction program used less RAM than the full-system computation reported for comparison.

4 System Decomposition

  • System decomposition adapts prior backward-reachability methods to forward reachability, enabling lower-dimensional FRS computations whose reconstructed set is guaranteed to overapproximate the full-system reachable set.The approach applies when the robot dynamics can be split into lower-dimensional subsystems; exact recovery is not always possible.Source: §4 · pp. 13–14

4.1 An Example System

  • The Rover’s seven-dimensional trajectory model produces lane-change, lane-keeping, and lane-return maneuvers, but its dimension makes direct FRS computation intractable.Its parameters determine maneuver timing, initial yaw rate, final heading, and longitudinal speed.Source: §4.1 · p. 14

4.2 Self-Contained Subsystems

  • Self-contained subsystems evolve using their own states and shared states, allowing the Rover model to be split into x-heading and y-heading subsystems.The trajectory parameters may appear in both subsystems, and tracking-error functions are defined independently for each.Source: §4.2 · p. 15

4.3 FRS Reconstruction

  • Reconstruction combines subsystem FRS information by overapproximating the intersection of their back-projections; the resulting 1-superlevel set contains the full-system FRS.The reconstruction program is formulated from feasible subsystem solutions.Source: §4.3 · p. 15, A Reachability Analysis Proofs · p. 42

4.4 Implementation

  • For the Rover, reconstruction program R5 used approximately 5.5×10^4 free variables and 227 GB of RAM, versus 504 GB for full-system D3.Each subsystem’s D4 required approximately 1.5 × 10^5 free variables and 473 GB of RAM.Source: §4.4 · p. 16

5 Conditions for Safety and Persistent Feasibility

RTD’s safety and persistent-feasibility guarantees depend on explicit assumptions about sensing, computation, braking, and the robot’s environment. A safe plan must avoid the FRS representation of obstacles, and the robot must be able to brake within that reachable set if replanning fails.

5.1 Obstacles and Sensors

  • Obstacles are assumed static, compact, connected, and finite in number; within Dsense, the robot knows their size, shape, and location.Occlusions and unexplored areas are treated as static obstacles, and obstacles are sensed upon entering the horizon rather than appearing inside it.Source: §5.1 · p. 16
  • Sensed obstacles are expanded by the state-estimation error before planning, and the planning-time budget must cover sensor processing and trajectory optimization.The required relation is τplan ≥ τprocess + τtrajopt.Source: §5.1 · p. 16
  • The method enforces an online planning time limit, but does not prove that its trajectory-planning time is bounded.Computation is terminated after τplan if the limit is reached.Source: §5.1 · p. 16

5.2 Safety

  • RTD defines a safe plan by requiring the FRS spatial projection for its trajectory parameter not to intersect any obstacle.The computed over-approximation of the FRS can be used for this non-intersection test.Source: §5.2 · p. 17

5.3 Persistent Feasibility

  • If replanning cannot find another safe trajectory, the current plan must allow the robot to stop safely within its collision-free FRS subset.The parameterized trajectories do not necessarily include braking, so the stopping maneuver is checked against the FRS.Source: §5.3 · p. 18, §5.3.1 · p. 18

5.3.1 Braking Within the FRS

  • RTD assumes finite braking time and requires the braking trajectory to stay within the points reachable by the trajectory-tracking model.Rather than adding braking maneuvers to the parameterized trajectories—which would increase offline reachability-analysis complexity—the paper chooses a sufficiently long planning horizon.Source: §5.3.1 · pp. 18–19

5.3.2 Planning Time Horizon

  • The maximum stopping time τstop can lower-bound T: T ≥ τplan + τstop ensures time to brake, although the resulting FRS may be conservative.The paper also describes choosing a shorter T empirically when non-braking trajectories still cover the braking distance; its Segway example uses T = 1 s.Source: §5.3.2 · p. 19

5.3.3 Minimum Sensor Horizon

  • Theorem 39 guarantees a new safe plan every τplan seconds or safe braking when its sensor-horizon and braking conditions hold.The section identifies a minimum sensor horizon and a sufficient planning duration as key conditions for this guarantee.Source: §5.3.3 · pp. 19–20

6 Obstacle Representation

This section develops an FRS-based discretized representation of buffered obstacles that conservatively identifies unsafe trajectory parameters and supports real-time planning for robots with arbitrary convex, compact footprints. It derives bounds for safely sampling obstacle boundaries and describes how buffer choice trades runtime against the free space available to the robot.

6 Obstacle Representation

  • Theorem 68 shows the obstacle representation can inner-approximate safe trajectory parameters, unlike the prior SDP approach, which is too slow for real-time planning.The proposed representation is suitable for real-time planning.Source: §6 · p. 20

6.1 FRS Projections

  • The FRS parameter projection maps obstacle points to trajectory parameters that could reach them, so parameters outside that projection avoid those points.Lemma 41 establishes that trajectories parameterized outside the projection cannot reach the specified point.Source: §6.1 · p. 20

6.2 Robot and Obstacle Geometry

  • A finite set of points discretizes each obstacle so its corresponding unsafe-parameter set is conservatively covered by point constraints evaluable at runtime.The point constraints allow the online trajectory planner to run in real time.Source: §6.2 · pp. 20–21

6.2.1 Generality of Proposed Method

  • The method assumes a compact, convex robot footprint and closed polygonal obstacles, which may be nonconvex and may represent occupancy grids or fitted line segments.The representation applies to arbitrary convex robot footprints, not only circles or rectangles.Source: §6.2.1 · p. 21

6.2.2 Geometric Objects

  • The geometric construction uses line segments, chords, and arcs to describe obstacle boundaries and related shapes.A chord joins two boundary points, while an arc is a connected, closed strict subset of a circle.Source: §6.2.2 · p. 21

6.2.3 Buffer and Point Spacing Motivation

  • Sampling only an obstacle’s boundary can leave gaps a robot may pass through, so the method buffers the obstacle and bounds point spacing to prevent passage between samples.The buffer reduces navigable free space, so the method also finds an upper bound on its distance.Source: §6.2.3 · pp. 21–22

6.2.4 Geometric Representation of the Dynamics

  • The robot’s body is modeled geometrically as its footprint under a continuous family of planar translations and rotations, allowing obstacle discretization to depend on footprint geometry rather than the high-fidelity dynamics.The transformations represent the body’s motion through space.Source: §6.2.4 · p. 23

6.3.1 Bounding the Point Spacing

  • The section computes geometric quantities for arbitrary convex, compact footprints; the maximum point spacing r is bounded by the largest gap the robot cannot pass through.The bound r is used to upper-bound both line-segment and arc point spacings.Source: §6.3 · p. 23, §6.3.1 · p. 23
  • The robot can pass through a line-segment gap if and only if its width W is less than the gap length L [Strang, 1982, Theorem 1].Thus, the robot footprint’s width sets the maximum point spacing r; r must be exact or underapproximated to ensure safety.Source: §6.3.1 · p. 24

6.3.2 Bounding the Buffer

  • For a gap of length r, the robot has a finite maximum penetration distance b, which bounds how much obstacle buffering is needed.The buffer b must be underapproximated when used to determine point spacing.Source: §6.3.2 · pp. 24–25

6.3.3 Finding the Point Spacing

  • Choosing a buffer b below the maximum penetration distance yields a point spacing r ≤ the robot width that limits penetration through line-segment gaps to b.Sampling buffered-obstacle line segments with adjacent points no farther apart than r prevents the robot from reaching the obstacle between those points.Source: §6.3.3 · pp. 25–26

6.3.4 Finding the Arc Point Spacing

  • Line-segment spacing r cannot generally be used on arcs, so a separate arc spacing a is needed to bound penetration into circles by b.The section establishes a ∈ (0, r) for chords of circles with radius b.Source: §6.3.4 · p. 26

6.3.5 Example Geometric Quantities

  • For a rectangular footprint of length L and width W, r = W and b = W/2; for a circular footprint of radius R, r = 2R and b = R.For rectangles, the section gives r = 2b and a = 2b sin(π/4) for selected b. Spacings r and a must be underapproximated if not found exactly.Source: §6.3.5 · p. 26

6.4 Preserving Safety with Discretized Obstacles

  • Avoiding every point in the discretized obstacle Xp guarantees that the robot also avoids the obstacle.The section states this safety result and proves it in Theorem 68.Source: §6.4 · p. 27

6.4.1 Constructing the Discretized Obstacle

  • The discretization procedure buffers polygonal obstacles, extracts line segments and arcs from the buffered boundary, then samples those curves to form Xp.The sampling function includes curve endpoints and places points so each has another point within the specified arclength distance.Source: §6.4.1 · p. 27
  • The construction samples buffered obstacle boundaries, retaining segment and arc endpoints and adding points spaced by r along lines and by arclength a along arcs, yielding a finite set.Source: §6.4.1 · p. 27

6.4.2 Proving Safety

  • Under Theorem 68’s buffer and spacing conditions, unsafe parameters for the obstacles are contained in those represented by the discretized points, so their complement inner-approximates the safe set.The theorem states πK(Xp) ⊇ πK(Xobs), hence πK(Xp)C ⊆ Ksafe.Source: §6.4.2 · p. 27

6.4.3 Conservatism of the Discretized Obstacle in Practice

  • Although the runtime FRS uses a relaxed-problem solution rather than the exact projection map, Remark 69 shows that Theorem 68’s safety guarantee still holds.Source: §6.4.3 · p. 28
  • A small buffer requires denser obstacle samples and may slow optimization, while a large buffer reduces free space; choose it as small as possible yet large enough for fast execution.Any buffer in the stated range can be chosen empirically without risking safety under Theorem 68.Source: §6.4.3 · p. 28

7 Online Trajectory Optimization

RTD selects parameterized trajectories online using obstacle constraints derived from a discretized obstacle representation. Its planning loop is designed for real-time execution and is proved safe under the stated assumptions.

7 Online Trajectory Optimization

  • RTD’s online trajectory optimization selects a new parameterized trajectory at each planning iteration using a user-specified cost and initial robot state.The algorithm’s real-time operation relies on obstacle discretization and enforcing a trajectory-optimization time limit.Source: §7 · p. 28, §7.1 · p. 28

7.1 Online Planning Algorithm Overview

  • Each planning iteration applies the current controller while sensing and buffering obstacles, discretizing them, and computing the next control input.OptK returns a new feedback controller or the previous iteration’s braking controller; braking stops within the corresponding FRS spatial projection under the stated assumptions.Source: §7.1 · p. 28

7.2 Trajectory Optimization Formulation

  • The optimization uses discretized obstacle points to conservatively approximate unsafe trajectory parameters and select a trajectory for the user-specified cost.The formulation has finitely many constraints and a low-dimensional decision variable, allowing OptK to typically terminate within its time limit.Source: §7.2 · pp. 28–29

7.3 Proving Safety

  • RTD either finds a safe plan of duration T or brakes, and its online algorithm is proved safe at every iteration.The safety argument bounds sensing distance, safely represents obstacles with Xp, and accounts for state-prediction error by expanding obstacles.Source: §7.3 · p. 29

8 Application

RTD is applied to a differential-drive Segway and a front-wheel-steering, all-wheel-drive Rover, using the online planner for simulation and hardware demonstrations. The applications specify robot models, FRS computations, and obstacle representations.

8.1 Segway

  • The Segway is a differential-drive robot used to demonstrate collision-free planning in unstructured, random environments.Its circular footprint has a 0.38 m radius; its maximum speed is 1.5 m/s in simulation and 1.25 m/s on hardware.Source: §8.1 · p. 29, §8.1.1 · p. 29

8.1.1 Model Parameters

  • The Segway model uses bounded speed and yaw rate, with tracking gains set separately for simulation and hardware.Hardware state-estimation error is εx = εy = 0.1 m; the tracking-error function is fit to simulated data, and the braking controller is shown to satisfy the stated FRS assumption.Source: §8.1.1 · p. 29

8.1.2 FRS Computation

  • Multiple Segway FRSs are selected by estimated initial speed because tracking error is proportional to initial speed in practice.Simulation uses three initial-speed ranges and hardware uses two; their FRS horizons and planning period are specified separately.Source: §8.1.2 · pp. 29–30

8.1.3 Obstacle Representation

  • Segway obstacle buffers trade conservatism against optimization runtime, and the selected buffer was the smallest that solved consistently within τplan.The simulation buffer is 0.001 m and the hardware buffer is 0.05 m.Source: §8.1.3 · p. 30

8.2 Rover

  • The Rover is a front-wheel-steering, all-wheel-drive platform used to demonstrate RTD’s application to passenger robots.Its rectangular footprint is 0.5 m long and 0.29 m wide; the system decomposition technique is used to compute its FRSs.Source: §8.2 · p. 30, §8.2.1 · p. 30

8.2.1 Dynamic Models and Parameters

  • The Rover model uses steady-state assumptions for lateral dynamics, while fitted constants account for wheel slip.Its tracking controller is proportional, and hardware state-estimation error is εx = εy = 0.1 m; the braking controller is verified empirically.Source: §8.2.1 · p. 30

8.2.2 FRS Computation

  • The Rover’s full-system FRS is reconstructed from subsystem FRSs, with 42 FRSs covering ranges of initial speed, wheel angle, and heading.Runtime selection uses initial velocity, wheel angle, and heading; hardware testing uses one FRS for velocities between 1 and 1.5 m/s because the physical testing area is limited.Source: §8.2.2 · p. 30

8.2.3 Obstacle Representation

  • The Rover obstacle representation uses a 0.01 m buffer, with point spacing r = 0.02 m and arc point spacing a = 0.014 m.Source: §8.2.3 · p. 30

9 Simulation Results

The simulations evaluate RTD against RRT and NMPC, including obstacle-buffer selection and performance under real-time planning with limited sensor horizons. They report crash and goal-reaching outcomes across these conditions, with examples of planner failures and RTD stopping safely without reaching a goal.

9.3.3 Experiment 3 Overview

  • RTD remained safe at the minimum sensor horizon; for the Segway, that horizon was 1.9 m versus 4 m of hardware range, and RRT and NMPC were not tested.Experiment 3 evaluates RTD against the minimum horizon specified by Theorem 39.Source: §9.3.3 · p. 34

9.4.4 Results

  • For the Segway, a 0.45 m buffer balanced RRT and NMPC goals and crashes; for the Rover, the expanded-footprint buffer with 0.0 m added gave best performance without crashes.With the Segway’s largest tested buffer, 0.65 m, both planners stopped without crashing, but reached only 71.8% of goals with RRT and 83.5% with NMPC.Source: §9.4.4 · p. 34, Table 1 · p. 34, Table 2 · p. 34

9.4.5 Discussion

  • NMPC crashed on the Rover with its largest buffer after failing to find a feasible plan and then colliding with the environment boundary while emergency braking.RRT and NMPC crashes can also arise from insufficient buffers for tracking or approximation error, or from being unable to brake safely when no feasible plan is found.Source: §9.4.4 · p. 34, §9.4.5 · p. 34

9.5.4 Results

  • In Segway Experiment 2, with the same environments used across all three experiments, RTD reached the goal in 96.3% of trials versus 78.2% for RRT and 0% for NMPC, with no RTD crashes.Source: §9.5.4 · p. 35
  • For the Rover, RTD reached the goal 95.4% of the time and had no crashes; RRT reached 97.6% (down from 99.8% in Experiment 1) and crashed once (0.01%), while NMPC reached 0% (down from 99.6%).Source: §9.5.4 · p. 35

9.5.5 Discussion

  • Under limited horizons, Rover RRT heuristics exploited the environment to reach more goals than RTD, whereas RTD reached more goals than RRT in the Segway trials.GPOPS-II NMPC struggled to solve quickly near randomly placed obstacles, reaching no goals for either robot under real-time constraints.Source: §9.5.5 · p. 35
  • In Segway examples, RTD safely brakes when high-level routing selects an infeasible route, while RRT crashes and NMPC gets stuck at a gap; another route favors RRT and NMPC.In the gap example, RRT cannot find a feasible plan and crashes while braking along its previous trajectory; NMPC brakes safely but cannot find a plan. In another example, RTD’s sharper initial turn leads to a safe stop, while RRT and NMPC reach the goal.Source: §9.5.5 · pp. 35, 37, Figure 15 · p. 36

9.6.4 Results

  • With the minimum sensor horizon, RTD had 0 crashes; Segway goal reach was 96.2% versus 96.3% in Experiment 2, and Rover goal reach was 95.2% versus 95.4%.Experiment 3 used τmove = τplan = 0.5 s, with Dsense = 1.9 m for the Segway and 4 m for the Rover; Theorem 39 sets these sensor horizons.Source: §9.6.4 · p. 37, §9.6.1 · p. 37, §9.6.2 · p. 37

9.7 Overall Simulation Discussion

  • RTD had 0 crashes across all simulations and reached goals comparably often to RRT and NMPC.The simulation experiments overall confirm that RTD is safe and persistently feasible.Source: §9.7 · pp. 37–38
  • RRT and NMPC can crash when they fail to find a feasible plan near obstacles, and larger buffers reduce crashes but also reduce goal-reaching opportunities.Crashes persist for both planners even without real-time or limited-sensor-horizon requirements, indicating they are not persistently feasible. RRT may brake unsafely after failing to build a dense tree; NMPC faces many non-convex constraints and has no safety guarantee when continuing its last feasible plan.Source: §9.7 · p. 37
  • RTD sometimes stops safely before reaching the goal, and RRT and NMPC perform especially well in the Rover’s sparse, structured, static environments.For the Segway, OptK may struggle to find a moving solution after a path leads too close to an obstacle; for the Rover, the FRS decomposition can be conservative when the footprint rotates, making tight lane changes difficult. RTD’s benefits were greater in the random Segway environments.Source: §9.7 · pp. 37–38

9.5.5 Discussion

  • In Rover examples, RTD succeeds where RRT crashes and NMPC gets stuck, while another example shows all planners braking safely.RRT can travel too close to an obstacle to find a feasible next plan, then crash while trying to brake; NMPC sometimes finds plans until obstacles enter its sensor horizon.Source: §9.5.5 · p. 37, Figure 16 · p. 38

10 Hardware Demonstration

Hardware demonstrations apply RTD to a Segway navigating randomly placed obstacles and a car-like Rover handling obstacles on a mock road. In both demonstrations, RTD plans safe trajectories in real time, with practical limits on sensing and maneuverability.

10 Hardware Demonstration

  • The hardware demonstrations affirm RTD’s safe, real-time planning capability on the Segway and Rover platforms.The paper describes RTD as safe and persistently feasible across its hardware demonstrations.Source: §10 · p. 38

10.1.1 Hardware Setup

  • The Segway demo used onboard computation, lidar accurate to 4.0 m, and a 0.3 s trajectory-optimization limit alongside 0.2 s state estimation.It navigated a 4 × 8 m tile floor with randomly placed 30 cm obstacles and no prior knowledge of their locations.Source: §10.1.1 · p. 38, §10.1.2 · p. 38

10.1.2 Demonstration

  • The Segway operated safely and consistently reached its goal despite randomly placed obstacles, switching to a low-speed FRS to navigate gaps too small for its high-speed FRS.The Segway used a high-speed FRS while moving and swapped to the low-speed FRS after stopping when braking was needed for a tight gap.Source: §10.1.2 · pp. 38–39

10.2.1 Hardware Setup

  • The Rover demo used lidar accurate to at least 3.5 m and enforced a combined planning and processing limit of 0.375 s.It drove a 7 m mock road with one to three obstacles, using real-time lidar detection and no prior obstacle knowledge.Source: §10.2.1 · p. 39, §10.2.2 · p. 39

10.2.2 Demonstration

  • The Rover completed 26 trials without crashes under a 0.375 s planning limit, but its single FRS could not navigate some tight gaps.The 26 trials comprise eight filmed trials and 18 additional trials; the Rover used one FRS at speeds of 1.0–1.5 m/s.Source: §10.2.2 · p. 39

11 Conclusion

RTD uses offline reachability analysis and online trajectory optimization to plan provably safe trajectories in real time. Simulations and hardware demonstrations on a Segway and Rover show that it outperforms RRT and NMPC in goal-reaching without crashes when real-time limits are enforced.

11 Conclusion

  • RTD uses an offline Forward Reachable Set that bounds reachable positions, including tracking error, while following parameterized trajectories.The FRS supports runtime obstacle mapping into nonlinear constraints on trajectory parameters, so satisfying those constraints ensures collision avoidance.Source: §11 · p. 40
  • RTD prescribes sensor-horizon and stopping-distance criteria for persistent feasibility and uses discrete obstacle points for safe, real-time constraint evaluation.A system decomposition technique for FRS computation extends RTD to higher-dimensional systems.Source: §11 · p. 40
  • With real-time planning limits enforced, RTD reached more goals than RRT and NMPC without crashes in simulations of the Segway and Rover.The evaluated systems were a Segway navigating random obstacles and a car-like Rover performing lane changes on a mock two-lane road.Source: §11 · p. 40

A Reachability Analysis Proofs

The appendix proves two claims about the Forward Reachable Set: a trajectory-related function becomes non-positive, and the set is contained in a specified superlevel set. It also introduces conditions intended to support persistent feasibility.

A Reachability Analysis Proofs

  • Lemma 18 establishes that v is non-positive and decreasing along trajectories when the stated constraints are satisfied.For points in XFRS, the lemma further states that v is non-positive at some time in [0, T].Source: A Reachability Analysis Proofs · p. 42
  • Theorem 27 states that the Forward Reachable Set is contained in the 1-superlevel set of any feasible solution to (R).The proof invokes subsystem trajectories and constraints to establish this containment for the decomposable full system.Source: A Reachability Analysis Proofs · pp. 42–43

B Conditions for Persistent Feasibility

  • The persistent-feasibility conditions are designed to make the non-braking planned trajectory spatially longer than the corresponding braking trajectory.This supports the assumption that the robot can stop safely in its direction of travel when a non-braking trajectory is safe over its full distance.Source: B Conditions for Persistent Feasibility · p. 43

B.1 Proof of Theorem 39

The appendix derives conditions for persistent feasibility, relates the planning horizon to braking distance, and examines the runtime cost of an alternative obstacle-intersection procedure. It states a sensor-horizon condition under which the robot can replan safely or brake.

B.1 Proof of Theorem 39

  • Under the stated assumptions, the robot can find a new safe plan every τplan seconds or brake safely if none is found.The theorem’s conditions include a sufficiently large time horizon and a sensor horizon satisfying the specified lower bound.Source: §B.1 · p. 43
  • Theorem 39’s proof requires sensing obstacles within (T + τplan) · vmax + 2ε to account for travel during planning and state-estimation error.The proof combines the robot’s maximum travel during the planning interval with the distance reachable during the trajectory horizon.Source: §B.1 · p. 43

B.2 Choosing the Time Horizon

  • The chosen horizon must make the tracking-model trajectory travel farther than the braking trajectory, so the robot can brake within the FRS.For the Segway’s constant-speed model, setting τv = Cstop satisfies Assumption 72; the Segway and Rover braking distances are reported as linearly bounded by speed.Source: §B.2 · p. 44, Figure 17 · p. 44

C Set Intersection

  • Set intersection approximates Ksafe through an SOS program translated into a runtime SDP, which may run slowly depending on the obstacle representation.The appendix presents this procedure as too slow for real-time trajectory planning.Source: C Set Intersection · p. 44, §C.1 · p. 45

C.2 Inspecting Set Intersection Speed

The set-intersection procedure is substantially slower than obstacle discretization, while the appendix supplies proofs and geometric tools supporting the obstacle representation.

C.2 Inspecting Set Intersection Speed

  • Set intersection takes a mean 17.8 s for box obstacles and 1.05 s for line-segment obstacles.The passage notes that line-segment solve time grows linearly with the number of segments.Source: §C.2 · p. 45
  • The proposed obstacle discretization is three orders of magnitude faster than set intersection.It avoids set intersection by evaluating the FRS polynomial on discretized obstacle points to form nonlinear constraints overapproximating Ksafe.Source: §C.2 · p. 45

D Obstacle Representation

  • Appendix D gives proofs from Section 6 and geometric lemmas for point spacings and penetration distance.Lemma 76 provides a method for constructing penetration distance b for arbitrary convex robot footprints.Source: D Obstacle Representation · p. 45

D.1 Proofs from Section 6.1

  • For safe trajectory parameters k, any point outside the forbidden set X \ X0 is never reached by the robot body while it tracks k.This is the safety result stated in Lemma 41.Source: §D.1 · p. 45

D.2 Proof from Section 6.2

  • Buffering a closed, bounded polygon produces a boundary made of finitely many line segments and circular arcs.The buffered obstacle is described as the Minkowski sum of the obstacle and a closed disk of radius b.Source: §D.2 · p. 45

D.3 Proof from Section 6.3.1

These proofs establish when a convex robot footprint can pass through a line segment and show that its maximum penetration through a segment of its width exists.

D.3 Proof from Section 6.3.1

  • A footprint of width W can pass fully through a line segment of length L if and only if W < L.This is Lemma 55, citing Strang [1982, Theorem 1].Source: §D.3 · p. 45

D.4 Proof from Section 6.3.2

  • For a segment whose length equals the footprint width r, a maximum penetration distance b exists.The proof bounds penetration using the footprint’s finite diameter and the fact that the footprint cannot pass fully through the segment.Source: §D.4 · p. 46

D.5 Proofs from Section 6.3.3

The proofs establish geometric bounds and a construction for choosing point spacing so penetration through a line segment stays within a specified bound.

D.5 Proofs from Section 6.3.3

  • Among three distinct parallel chords of a convex compact set, the middle chord is not the shortest.The stated chord-length comparison also gives a strict inequality when the outer chord lengths differ as specified.Source: §D.5 · p. 47
  • As the footprint penetrates farther through the segment, its intersection chord is longer than every parallel chord lying beyond it.This geometric relation is used to bound the point spacing above and below.Source: §D.5 · p. 47
  • For any chosen b below the maximum penetration depth, some point spacing r ensures penetration through a segment of length r is at most b.The guarantee holds for any transformation family attempting to pass the footprint through that segment.Source: §D.5 · p. 47
  • The spacing r is the shortest intersection segment obtained when the footprint penetrates the line segment by distance b.Program (79) searches across transformation families for this minimum.Source: §D.5 · p. 48

D.6 Proof from Section 6.3.4

The proof establishes a circle-chord spacing that limits footprint penetration, then constructs that chord from a line-segment penetration setup.

D.6 Proof from Section 6.3.4

  • For any b below the maximum penetration distance, a chord length a exists that limits penetration into a radius-b circle to no more than b.The claim applies to any chord of the circle having the specified length a.Source: §D.6 · p. 48
  • The construction searches for the shortest circle chord formed from the footprint-circle intersection across transformation families reaching penetration b.The circle is translated to place its center at a point of the footprint on the penetration boundary.Source: §D.6 · pp. 48–49
  • A feasible line-segment solution at penetration b supplies a boundary point for centering the circle and defining chord κa.The chord lies between the endpoints of the footprint-circle intersection arc.Source: §D.6 · p. 49

D.7 Proof from Section 6.4

The proof establishes that discretizing obstacles with prescribed point spacings yields a conservative set of unsafe trajectory parameters. The appendix also characterizes maximum penetration through a width-r segment using rotation followed by translation.

D.7 Proof from Section 6.4

  • πK(Xp) contains πK(Xobs), so any parameter outside the discretized-obstacle unsafe set is safe for Xobs.Source: §D.7 · p. 50
  • The proof bounds penetration between sampled points on line segments and arcs, relying on prescribed spacings and continuous robot motion.Source: §D.7 · p. 50

D.8 Finding the Maximum Penetration Distance

  • For a footprint of width r crossing a length-r segment, some rotation followed by translation alone reaches maximum penetration b.Source: §D.8 · p. 50
  • For each rotation, translate the footprint as far as possible into the half-plane; at that stopping position, both segment endpoints lie on its boundary.Source: §D.8 · p. 50
Loading 1809.06746v2…