Trajectory Tracking with Reachability-Guided Quadratic Programming and Freeze-Resume

2025-09-16

AI Summary

To address the challenge of safe pausing and precise trajectory recovery for robots operating in dynamic environments—where obstacles necessitate immediate suspension without compromising tracking accuracy—this paper proposes a replanning-free freeze-and-recover control framework. Methodologically, it employs feedback linearization to construct a double-integrator output-space model; integrates offline reachability verification with online quadratic programming; performs a one-step online reachability test to quantify disturbance rejection capability; and introduces a KKT-inspired heuristic weighting scheme to dynamically correct tracking errors. The key contribution is the first integration of reachability analysis with KKT conditions to achieve zero-error trajectory recovery under disturbances. Simulation results demonstrate significant improvements over pure-pursuit methods in safety-critical stopping, responsiveness to sudden deviations, and recovery precision (achieving zero steady-state error), while rigorously satisfying state and input constraints.

Technology Category

• Planning, Routing, and Scheduling: Replanning and Plan Repair
• Intelligent Robots: State Estimation
• Humans and AI: Human-Aware Planning and Behavior Prediction

Application Category

• Responsible Web: Machine-in-the-loop, human agency and autonomy
• Search and Retrieval-Augmented AI: Web learning to rank, online learning, and counterfactual learning for ranking
• Security and Privacy: Tracking, profiling, and countermeasures against them

Abstract

Many robotic systems must follow planned paths yet pause safely and resume when people or objects intervene. We present an output-space method for systems whose tracked output can be feedback-linearized to a double integrator (e.g., manipulators). The approach has two parts. Offline, we perform a pre-run reachability check to verify that the motion plan respects speed and acceleration magnitude limits. Online, we apply a quadratic program to track the motion plan under the same limits. We use a one-step reachability test to bound the maximum disturbance the system is capable of rejecting. When the state coincides with the reference path we recover perfect tracking in the deterministic case, and we correct errors using a KKT-inspired weight. We demonstrate that safety stops and unplanned deviations are handled efficiently, and the system returns to the motion plan without replanning. We demonstrate our system's improved performance over pure pursuit in simulation.

Problem

• Tracking trajectories while pausing safely for obstacles
• Handling unplanned deviations without needing replanning
• Improving performance over pure pursuit methods

Innovation

• Reachability-guided quadratic programming for trajectory tracking
• Freeze-resume capability for safe human-robot interaction
• One-step reachability test for disturbance rejection bounds