Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

Towards Fast and Safety-Guaranteed Trajectory Planning and Tracking for Time-Varying Systems

Seth Siriya, Mo Chen, Ye Pu

TL;DR

This work tackles safe trajectory planning for time-varying nonlinear systems with online constraint updates by marrying an offline dynamic-programming-based safety analysis with an online replanning loop. The core idea is to compute a time-varying value function $V(r,t)$ and an optimal tracking controller $u_s^*(r,t)$ in a relative-coordinate framework $r=Ls-Mp$, enabling real-time constraint satisfaction and goal-reaching when following replanned planning trajectories. The paper develops both a finite-horizon method for general time-varying dynamics and a periodic-extension that removes horizon limits for periodic systems, with rigorous guarantees and an autonomous underwater vehicle (AUV) case study demonstrating reduced conservatism and teleportation-enabled adaptability. The results show that modeling time variation explicitly in the tracking dynamics yields faster and safer navigation through unknown environments, while maintaining theoretical guarantees under bounded disturbances and online constraint updates, making the approach practical for real-time motion planning in changing contexts.

Abstract

When deploying autonomous systems in unknown and changing environments, it is critical that their motion planning and control algorithms are computationally efficient and can be reapplied online in real time, whilst providing theoretical safety guarantees in the presence of disturbances. The satisfaction of these objectives becomes more challenging when considering time-varying dynamics and disturbances, which arise in real-world contexts. We develop methods with the potential to address these issues by applying an offline-computed safety guaranteeing controller on a physical system, to track a virtual system that evolves through a trajectory that is replanned online, accounting for constraints updated online. The first method we propose is designed for general time-varying systems over a finite horizon. Our second method overcomes the finite horizon restriction for periodic systems. We simulate our algorithms on a case study of an autonomous underwater vehicle subject to wave disturbances.

Towards Fast and Safety-Guaranteed Trajectory Planning and Tracking for Time-Varying Systems

TL;DR

This work tackles safe trajectory planning for time-varying nonlinear systems with online constraint updates by marrying an offline dynamic-programming-based safety analysis with an online replanning loop. The core idea is to compute a time-varying value function and an optimal tracking controller in a relative-coordinate framework , enabling real-time constraint satisfaction and goal-reaching when following replanned planning trajectories. The paper develops both a finite-horizon method for general time-varying dynamics and a periodic-extension that removes horizon limits for periodic systems, with rigorous guarantees and an autonomous underwater vehicle (AUV) case study demonstrating reduced conservatism and teleportation-enabled adaptability. The results show that modeling time variation explicitly in the tracking dynamics yields faster and safer navigation through unknown environments, while maintaining theoretical guarantees under bounded disturbances and online constraint updates, making the approach practical for real-time motion planning in changing contexts.

Abstract

When deploying autonomous systems in unknown and changing environments, it is critical that their motion planning and control algorithms are computationally efficient and can be reapplied online in real time, whilst providing theoretical safety guarantees in the presence of disturbances. The satisfaction of these objectives becomes more challenging when considering time-varying dynamics and disturbances, which arise in real-world contexts. We develop methods with the potential to address these issues by applying an offline-computed safety guaranteeing controller on a physical system, to track a virtual system that evolves through a trajectory that is replanned online, accounting for constraints updated online. The first method we propose is designed for general time-varying systems over a finite horizon. Our second method overcomes the finite horizon restriction for periodic systems. We simulate our algorithms on a case study of an autonomous underwater vehicle subject to wave disturbances.

Paper Structure

This paper contains 36 sections, 5 theorems, 45 equations, 7 figures, 5 algorithms.

Table of Contents

  1. Introduction
  2. Problem Formulation
  3. Offline Computation
  4. The Planning and Relative System
  5. Planning system
  6. Relative system
  7. The Value Function and Optimal Tracking Controller
  8. Trajectory Planning for Time-Varying Systems
  9. The Planning System Sublevel Set, Set Satisfaction Map, and Set Avoidance Map
  10. Trajectory Planning Method Over a Bounded Interval
  11. Theoretical Guarantees for Tracking Planned Trajectories
  12. Trajectory Replanning and Tracking
  13. The Online Constraint Satisfaction Problem
  14. Method for Replanning and Tracking
  15. Theoretical Guarantees for Replanning and Tracking
  16. ...and 21 more sections

Key Result

Proposition 1

(Sublevel Set Invariance) Let $s$ and $p$ be the states of the tracking system eqn:time-varying-tracking-model and planning system eqn:time-varying-planning-model at time $t \in [0,T_{\textnormal{off}}]$, and $r = Ls-Mp$ be the corresponding relative state. For all $t' \in [t,T_{\textnormal{off}}]$, where and $u_p^*(\cdot ; r,t) \in \mathbb{U}_p(t)$, $d^*(\cdot;r,t) \in \mathbb{D}(t)$, $u_s^*(\cd

Figures (7)

  • Figure 1: Illustrations showing how $\mathcal{T}_p$ is used to construct $\mathcal{F}_B$ and $\mathcal{A}_B$, and their importance for the planning system and tracking system trajectories, when $n_s = n_p$.
  • Figure 2: Block diagram for online phase of Algs. \ref{['alg:finite-interval-method']} and \ref{['alg:periodic-method']}. Objects computed offline are colored purple.
  • Figure 3: Illustration of tubes containing the tracking system trajectory. Case a) represents the method in this paper where the inlet just needs to contain the tracking system state. Case b) is analogous to Majumdar2017FunnelPlanning: where the outlet of the previous tube is a subset of the inlet of the current tube. Case c) represents a modification to handle non-instantaneous trajectory planning.
  • Figure 4: Snapshots of the tracking and planning systems at time instances where replanning occurs up until the goal is reached, for Simulation 1. Dark red blocks correspond to known obstacles, and the corresponding dark red dotted lines correspond to the planning system obstacles. The dark red dashed squares are due to $\Xi^{\mathsf{c}}$, which is included in the known obstacles. Light red blocks correspond to unknown obstacles, and the corresponding light red dotted lines are the unknown future planning system obstacles. The grey box is the goal region. The black 'x' is the tracking system position, and the black line is the tracking system trajectory up to the current time. The green box is the sensor range for the tracking system. The blue circle is the planning system, the blue line is the planning system trajectory up to the current time, and the purple line is the current planned trajectory.
  • Figure 5: Snapshots of the tracking and planning system at times where replanning occurs up until the goal is reached for Simulation 2. The meaning of symbols and lines are consistent with Fig. \ref{['fig:case_study_tv_noswitch']}.
  • ...and 2 more figures

Theorems & Definitions (24)

  • Proposition 1
  • Remark 1
  • Remark 2
  • Remark 3
  • Remark 4
  • Lemma 2
  • Theorem 3
  • Remark 5
  • Remark 6
  • Remark 7
  • ...and 14 more