Kinodynamic Trajectory Following with STELA: Simultaneous Trajectory Estimation&Local Adaptation

2025-04-28

AI Summary

This work addresses the challenge of safe trajectory tracking for nonholonomic vehicles under perceptual noise, actuation biases, and model mismatch. Methodologically, it introduces a real-time closed-loop framework integrating state estimation and control optimization: (i) execution duration is explicitly incorporated as an optimization variable within the factor graph—a novel formulation; (ii) arbitrary-order nonlinear vehicle dynamics are supported; and (iii) a sliding time window, coupled with incremental iSAM, enables efficient online joint estimation and planning. The key contribution lies in eliminating reliance on Gaussian priors and linear time-varying models—thereby substantially enhancing robustness in complex, uncertain environments. Experimental validation across multiple nonlinear vehicle platforms demonstrates stable online operation at ≥10 Hz, tracking accuracy comparable to linear baselines, and explicit collision avoidance capability.

Technology Category

• Intelligent Robots: State Estimation
• Planning, Routing, and Scheduling: Model-Based Reasoning
• Search and Optimization: Sampling/Simulation-based Search

Application Category

• Graph Algorithms and Modeling for the Web: Efficient manipulation of static and dynamic Web-related graphs
• Systems and Infrastructure for Web, Mobile and WoT: Applied ML and AI for Web-based mobile applications
• Search and Retrieval-Augmented AI: Web learning to rank, online learning, and counterfactual learning for ranking

Abstract

State estimation and control are often addressed separately, leading to unsafe execution due to sensing noise, execution errors, and discrepancies between the planning model and reality. Simultaneous control and trajectory estimation using probabilistic graphical models has been proposed as a unified solution to these challenges. Previous work, however, relies heavily on appropriate Gaussian priors and is limited to holonomic robots with linear time-varying models. The current research extends graphical optimization methods to vehicles with arbitrary dynamical models via Simultaneous Trajectory Estimation and Local Adaptation (STELA). The overall approach initializes feasible trajectories using a kinodynamic, sampling-based motion planner. Then, it simultaneously: (i) estimates the past trajectory based on noisy observations, and (ii) adapts the controls to be executed to minimize deviations from the planned, feasible trajectory, while avoiding collisions. The proposed factor graph representation of trajectories in STELA can be applied for any dynamical system given access to first or second-order state update equations, and introduces the duration of execution between two states in the trajectory discretization as an optimization variable. These features provide both generalization and flexibility in trajectory following. In addition to targeting computational efficiency, the proposed strategy performs incremental updates of the factor graph using the iSAM algorithm and introduces a time-window mechanism. This mechanism allows the factor graph to be dynamically updated to operate over a limited history and forward horizon of the planned trajectory. This enables online updates of controls at a minimum of 10Hz. Experiments demonstrate that STELA achieves at least comparable performance to previous frameworks on idealized vehicles with linear dynamics.[...]

Problem

• Unifies state estimation and control for safer trajectory execution
• Extends optimization to non-holonomic robots with arbitrary dynamics
• Enables real-time trajectory updates and collision avoidance

Innovation

• Simultaneous trajectory estimation and local adaptation
• Factor graph representation for arbitrary dynamical systems
• Incremental updates with iSAM and time-window mechanism