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

TL;DR

STELA addresses the gap between planning-models and real-world kinodynamics by unifying trajectory estimation and local adaptation within a factor-graph framework. It initializes from a kinodynamic SBMP solution and employs an incremental iSAM2-based sliding-window optimization to smooth past trajectories and adapt controls, treating edge execution duration as a tunable variable. The approach generalizes to arbitrary first- or second-order dynamics, uses six factor types including obstacle and prior constraints, and achieves online control updates at or above 10 Hz while maintaining collision-free operation. Empirical results on simulated LTV-SDE and real MuSHR systems show STELA’s robustness to noise and model gaps, outperforming or matching prior FG-based methods and demonstrating the value of SBMP initialization, duration optimization, and a sliding-window strategy.

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.[...]

Figures (14)

• Figure 1: Middle: STELA execution on a real MuSHR robot. The middle image is a composite from 2 top-down cameras used for localization, covering a 7.6mx2.3m workspace. The robot follows a trajectory computed by a planner with knowledge of the obstacles (rocks and boxes) but no knowledge of the ramp, affecting execution. Top and Bottom: i) STELA estimation and plan when the robot is on the unknown ramp; ii) the robot recovers from the ramp and avoids an obstacle; iii) STELA adapts the plan to follow the planned trajectory while avoiding another obstacle; iv) the robot reaches the end of the plan without collisions. Rviz visualization includes obstacles, planned trajectory (green), forward horizon (white), and history (cyan). Stars indicate corresponding states between the visualization and the real robot.
• Figure 2: A typical trajectory estimation FG at time $T$ uses state observations $z^x(0:T)$ and the robot model $\dot{x}_t = \hat{f}_\rho(x_t,u_t)$ to generate state estimates $\bar{X}(0:T)$. The unary factors impose a cost between observations and estimated states. The binary factors correspond to the robot's dynamics.
• Figure 3: An FG for robot planning employs the robot's model $\dot{x}_t = \hat{f}_\rho(\hat{x_t},u_t)$ on a dynamics factor to compute a trajectory of $T$ states, starting in $\hat{x}_0$ and ending in the goal region $\bar{X}_T \in X_G$. Beyond the ternary dynamics factor, there are costs imposed for the optimization by unary factors for obstacle avoidance (${\bf c}(\mathbb{X}_o)$) over the intermediate state variables $(\hat{X}_1:\hat{X}_{T-1})$, a state prior for the initial state (${\bf c}(x_0)$) and a goal region prior for the final state (${\bf c}(X_G)$).
• Figure 4: Asynchronous system architecture: Offline, a system identification process generates a FG-based dynamics model of the robot system. A motion planner receives the dynamics model, the environment, and a motion planning query as input to generate a desired solution plan that addresses the query for the given map and model. Upon completion, the desired plan, the model, and the environment are sent to STELA. Online, raw data --i.e., images from external cameras-- are processed by a perception process to provide robot state observations to the control module. These observations are used by STELA to estimate the executed trajectory and generate controls to be forwarded to the robot at a high frequency. The closed-loop framework enables the system to adapt to noise dynamically, execution errors, and the gap between the planning model and the real system. In the accompanying experiments, the robot system is either a real MuSHR robot or a simulated system, where both an idealized LTV-SDE robot and an analytical dynamics model of a MuSHR robot are considered.
• Figure 5: (Left) The dynamics factor graph corresponding to each edge of the desired trajectory with all associated factors. (Middle) For visualization purposes, the dynamics factor graph is also presented in a compressed form, which is symbolized by a hollow factor. (Right) A collision-free trajectory, consisting of a dynamic factor graph sequence, is shown.