Source-linked AI summary
FaSTrack: a Modular Framework for Fast and Guaranteed Safe Motion Planning
Sylvia L. Herbert, Mo Chen, SooJean Han, Somil Bansal, Jaime F. Fisac, Claire J. Tomlin
TL;DR
Autonomous motion planning must balance real-time computation with safety and dynamic accuracy in unknown cluttered environments. FaSTrack combines a simplified fast planner with a dynamic safety controller and guaranteed tracking-error bound, and demonstrates safe real-time tracking by a wind-affected 10D quadrotor following an RRT path.
Problem
Real-time planning with simplified dynamics can sacrifice safety and dynamic feasibility, while planning directly with sophisticated dynamics is often too slow.
Method
FaSTrack uses offline Hamilton-Jacobi reachability to compute a tracking-error bound and safety-control lookup function for a dynamic tracker following a simplified planner.
Results
A wind-affected 10D quadrotor safely planned and tracked a trajectory in real time through a static environment using an RRT planner and limited-range sensing.
Takeaways & Limitations
FaSTrack provides a modular way to add guaranteed tracking robustness to fast path or trajectory planners, including planners such as MPC, RRT, and neural-network-based planners.
Abstract
from arXiv · showhide
Fast and safe navigation of dynamical systems through a priori unknown cluttered environments is vital to many applications of autonomous systems. However, trajectory planning for autonomous systems is computationally intensive, often requiring simplified dynamics that sacrifice safety and dynamic feasibility in order to plan efficiently. Conversely, safe trajectories can be computed using more sophisticated dynamic models, but this is typically too slow to be used for real-time planning. We propose a new algorithm FaSTrack: Fast and Safe Tracking for High Dimensional systems. A path or trajectory planner using simplified dynamics to plan quickly can be incorporated into the FaSTrack framework, which provides a safety controller for the vehicle along with a guaranteed tracking error bound. This bound captures all possible deviations due to high dimensional dynamics and external disturbances. Note that FaSTrack is modular and can be used with most current path or trajectory planners. We demonstrate this framework using a 10D nonlinear quadrotor model tracking a 3D path obtained from an RRT planner.
I. INTRODUCTION
FaSTrack addresses the tension between fast planning with simplified models and safe tracking of high-dimensional systems in uncertain environments. It combines a simplified planner with dynamic tracking, guaranteed error bounds, and safety control, demonstrated on a windy quadrotor navigating with RRT.
- Motivation: Simplified planning models enable real-time computation but create tracking errors between planned paths and high-dimensional vehicle trajectories.These errors can arise when simplified dynamics or kinematics are used for computational efficiency.
- FaSTrack framework: FaSTrack pairs a fast simplified planning system with a dynamic tracking system that accounts for complex dynamics and bounded disturbances.The framework models navigation as a sophisticated tracker pursuing the simplified planner.
- FaSTrack framework: Offline Hamilton-Jacobi reachability computes a tracking-error bound and safety-control lookup function, while online sensing augments obstacles before simplified planning.The bound acts as a safety bubble around the planning system, and the controller is used when the relative state approaches that bound.
- Demonstration: A 10D quadrotor affected by wind safely plans and tracks trajectories in real time through a static environment with limited-range obstacle sensing using an RRT planner.
- Positioning: FaSTrack is designed to combine modularly with existing path or trajectory planners while handling bounded disturbances and known or unknown static-obstacle environments.The paper distinguishes this modularity and disturbance handling from prior robust planning methods.
III. PROBLEM FORMULATION
The problem formulation separates a potentially nonlinear, high-dimensional tracking model from a planning model used to generate desired paths online. The formulation imposes regularity assumptions on tracking dynamics while allowing the planner to remain simpler and disturbance-free.
- III. PROBLEM FORMULATION: FaSTrack seeks to plan and track paths or trajectories online in real time using separate planning and tracking models.Static obstacles may be known beforehand or observed within limited sensing range.
- III. PROBLEM FORMULATION: The formulation permits static obstacles that are either known a priori or detected within the system’s limited sensing range.
- A. Tracking Model: The tracking model represents the autonomous system and may be nonlinear and high-dimensional, with state variables denoted by s.
- A. Tracking Model: The tracking dynamics are represented by an ordinary differential equation driven by tracking control and disturbance inputs.The displayed dynamics are written as dt = ˙s = f(s, us, d).
- A. Tracking Model: The tracking dynamics are assumed uniformly continuous, bounded, and Lipschitz continuous in s for fixed control us.The control and disturbance functions are drawn from specified sets.
- A. Tracking Model: Under the stated assumptions, a unique trajectory exists for each admissible tracking-control function over the finite time interval.The trajectories satisfy the initial condition and the ordinary differential equation almost everywhere.
B. Planning Model
The planning model generates the desired path online using kinematics or low-dimensional dynamics, while disturbance treatment remains confined to the tracking model. This separation allows FaSTrack to work with planners that do not model disturbances.
- B. Planning Model: The planning model is used by the path or trajectory planner to solve for the desired path online.
- B. Planning Model: Kinematics or low-dimensional dynamics are typically selected for the planning model according to planner requirements.
- B. Planning Model: The planning state p is a subset of the tracking state s, and its dynamics are specified by an ordinary differential equation.The model uses planning control up over the stated time interval.
- B. Planning Model: The planning model has no disturbance input because FaSTrack handles disturbances only in the tracking model.This design is modular with respect to planning methods that do not account for disturbances.
C. Goals of This Paper
The paper aims to provide a modular tool for guaranteed tracking-error bounds and safety control, integrate it with fast planners, and demonstrate it on a high-dimensional system.
- FaSTrack precomputes guaranteed tracking-error bounds and an optimal safety controller for robust motion planning with nonlinear dynamic systems.
- The framework integrates fast path and trajectory planners with a tracking system that follows the planner’s desired state.
- The hybrid tracking controller combines safety and performance controllers to regulate the physical system relative to the planning state.The safety controller is computed offline via HJ reachability and is designed to preserve the tracking-error bound under worst-case disturbances.
- The paper organizes the framework into offline computation, online execution, and a complete example.
V. OFFLINE COMPUTATION
The offline computation formulates tracking as a pursuit-evasion game, derives relative dynamics, and uses HJ reachability to obtain a value function whose sub-level sets provide guaranteed tracking-error bounds.
- The offline computation models the tracking and planning systems as a pursuit-evasion game analyzed with HJ reachability.The tracking system seeks to capture the planning system while the planning system avoids capture, representing a worst-case scenario.
- Relative states are formed by fixing the planning model at the origin and expressing tracking dynamics relative to it.The relative state represents tracking states relative to planning states; Q aligns their common states by augmenting the planning state space.
- The cost function l(r) measures relative distance to the origin, representing tracking error between the two systems.For the illustrative quadrotor example, l(r) is defined as distance in position space to the origin.
- Non-anticipative strategies constrain how the planning system and disturbance choose controls in the differential game.
- HJ reachability computes a value function mapping initial relative states to the maximum possible cost over the time horizon.When the value function converges, its sub-level sets map initial relative states to guaranteed furthest tracking errors over all time.
- In the online framework, V∞(r) serves as the tracking-error bound and ∇V∞(r) provides the safety controller, both stored as grid-based lookup tables.
C. Invariance of Converged Value Function
Proposition 1 establishes that every level set of the converged value function is invariant under worst-case tracking, planning, and disturbance policies. The resulting smallest invariant level set defines the guaranteed tracking error bound used online.
- C. Invariance of Converged Value Function: Every level set of V∞(r) is invariant when the tracking system uses optimal tracking control and planning and disturbance act adversarially.In practice, less adversarial planning or disturbance benefits the tracking system.
- C. Invariance of Converged Value Function: The smallest invariant level set, with V := minr V∞(r), represents the smallest possible tracking error.
- C. Invariance of Converged Value Function: The guaranteed tracking error bound is B = {r : V∞(r) ≤ V}.Numerically, the bound uses B = {r : V∞(r) ≤ V + ϵ} for suitably small ϵ > 0.
- C. Invariance of Converged Value Function: The bound is used online as the region where arbitrary control is allowed, while the safety-optimal controller is required on its border.
VI. ONLINE COMPUTATION
FaSTrack’s online computation augments sensed obstacles by the tracking-error bound, plans with a simplified model, and switches between performance and safety control based on the relative state. The loop then advances both tracking and planning models until the planning goal is reached.
- VI. ONLINE COMPUTATION: The online algorithm takes the tracking error function V∞(r) and safety-control lookup function ∇V∞(r) as inputs.After discretization, both functions are represented as lookup tables.
- VI. ONLINE COMPUTATION: The path planner uses the simplified planning state and augmented obstacles to produce the next planning state pnext.
- VI. ONLINE COMPUTATION: If the updated relative state lies on Bp(0), the hybrid controller uses the safety controller; otherwise, it uses a performance controller.The performance controller can be application-specific, and for the example it is identical to the safety controller.
- VI. ONLINE COMPUTATION: The vehicle advances for a time step ∆t, updates the planning model state from the next tracking state, and repeats until the planning goal is reached.
VII. 10D QUADROTOR RRT EXAMPLE
The framework is demonstrated on a 10D near-hover quadrotor tracking a 3D point-source path generated by an RRT planner through a static-obstacle environment.
- VII. 10D QUADROTOR RRT EXAMPLE: The experiment uses a 10D near-hover quadrotor tracking a 3D point-source path generated by an RRT planner.
- VII. 10D QUADROTOR RRT EXAMPLE: The evaluation first computes the tracking error bound and safety-controller lookup tables offline, then applies the online framework in a 3D environment with static obstacles.
A. Precomputation of 10D-3D system
The example models a 10D nonlinear quadrotor and a 3D holonomic planning vehicle, with wind disturbances affecting the quadrotor. HJ reachability produces the invariant value function and tracking-error lookup data after decomposing the relative system into three positional subsystems.
- A. Precomputation of 10D-3D system: The tracking model is a 10D quadrotor, while the planning model is a 3D holonomic vehicle controlling velocity in each positional dimension.The 3D states are a subset of the 10D state space, and Q matches their position states.
- A. Precomputation of 10D-3D system: Wind disturbances act on the quadrotor’s velocity in each dimension, with disturbance bounds |dx|, |dy|, |dz| ≤ 0.1 m/s.
- A. Precomputation of 10D-3D system: The model uses quadrotor control bounds |ax|, |ay| ≤ 10 degrees and 0 ≤ az ≤ 1.5g m/s2, while planning velocities satisfy |bx|, |by|, |bz| ≤ 0.5 m/s.
- A. Precomputation of 10D-3D system: HJ reachability evaluates the decomposed cost function until convergence to produce the invariant value function for the relative system.New decomposition methods split the 10D nonlinear relative system into three positional subsystems.
- A. Precomputation of 10D-3D system: 0.81 m in each direction is the tracking-error bound when the real system starts at the origin in relative coordinates.The value function and its spatial gradients are saved as lookup tables for the online framework.
B. Online Planning with RRT and Sensing
FaSTrack combines a precomputed tracking error bound and safety controller with fast RRT planning, obstacle sensing, and replanning for real-time navigation. The simulation illustrates bounded tracking relative to the planning model while maintaining obstacle clearance.
- Framework: FaSTrack combines a precomputed value function and its gradients with planners such as RRT in a modular framework.The value function provides the tracking error bound, while its gradients provide the optimal tracking controller.
- RRT planning: The RRT planner uses piecewise linear paths assigned a speed of 0.5 m/s for the planning model.
- Sensing and replanning: Obstacles sensed within a 2 m range are expanded by the tracking bound V before RRT replanning.This augmentation accounts for tracking error when generating collision-free paths.
- Runtime: 25 ms was the average iteration time, with approximately 5 ms spent obtaining the tracking controller in an unoptimized MATLAB implementation.Control was applied once every 100 ms on a Core i7-2600K desktop computer.
- Simulation: The simulation displays tracking and planning trajectories alongside seen and unseen obstacles and the translucent tracking error bound.The top-left panel shows the complete trajectory, while other panels zoom into selected positions.
- Simulation: Replanning after sensing new 3 m^2 portions of obstacles produced brief exploration behavior without using a separate exploration algorithm.
VIII. CONCLUSIONS AND FUTURE WORK
The paper introduces FaSTrack as a tool for adding robustness to path and trajectory planners without sacrificing fast online computation. Its demonstrated scope is limited to static obstacles in unknown environments with limited sensing, with future work targeting broader settings.
- Conclusions: FaSTrack adds robustness to various path and trajectory planners while preserving fast online computation.
- Scope: The demonstrated framework applies to unknown environments with limited sensing ranges and static obstacles.
- Future work: Future work includes moving obstacles, disturbance-adaptive error bounds, and demonstrations with a variety of planners.