An official website of the United States government
This paper proposes a method to generate feasible trajectories for robotic systems with predefined sequences of switched contacts. The proposed trajectory generation method relies on sampling-based methods, optimal control, and reach-ability analysis. In particular, the proposed method is able to quickly test whether a simplified model-based planner, such as the Time-to-Velocity-Reversal planner, provides a reachable contact location based on reachability analysis of the multi-body robot system. When the contact location is reachable, we generate a feasible trajectory to change the contact mode of the robotic system smoothly. To perform reachability analysis efficiently, we devise a method to compute forward and backward reachable sets based on element-wise optimization over a finite time horizon. Then, we compute robot trajectories by employing optimal control. The main contributions of this study are the following. Firstly, we guarantee whether planned contact locations via simplified models are feasible by the robot system. Secondly, we generate optimal trajectories subject to various constraints given a feasible contact sequence. Lastly, we improve the efficiency of computing reachable sets for a class of constrained nonlinear systems by incorporating bi-directional propagation (forward and backward). To validate our methods we perform numerical simulations applied to a humanoid robot walking.
more »
« less
This content will become publicly available on June 23, 2026
Kinodynamic Trajectory Following with STELA: Simultaneous Trajectory Estimation & Local Adaptation
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. STELA also directly applies to and successfully solves trajectory following problems for more complex dynamical models. Beyond generalization, simulations assess STELA's robustness under varying levels of sensing and execution noise, while ablation studies highlight the importance of different components of STELA. Real-world experiments validate STELA's practical applicability on a low-cost MuSHR robot, which exhibits high noise and non-trivial dynamics.
more »
« less
- Award ID(s):
- 2021628
- PAR ID:
- 10599380
- Publisher / Repository:
- Robotics: Science and Systems
- Date Published:
- Subject(s) / Keyword(s):
- Robotics State Estimation Control Factor Graphs Kinodynamic Planning
- Format(s):
- Medium: X
- Location:
- Los Angeles, CA
- Sponsoring Org:
- National Science Foundation
More Like this
-
-
null (Ed.)Factor graph chains– the special case of a factor graph in which there are no potentials connecting non-adjacent nodes– arise naturally in many robotics problems. Importantly, they are often part of an inner loop in trajectory optimization and estimation problems, and so applications can be very sensitive to the performance of a solver. Of course, it is well-known that factor graph chains have an O(N) solution, but an actual solution is often left as “an exercise to the reader”... with the inevitable consequence that few (if any) efficient solutions are readily available. In this paper, we carefully derive the solution while keeping track of the specific block structure that arises, we work through a number of practical implementation challenges, and we highlight additional optimizations that are not at first apparent. An easy-to-use and self-contained solver is provided in C, which outperforms the AprilSAM general-purpose sparse matrix factorization library by a factor of 7.3x even without specialized block operations. The name AXLE reflects the names of the key matrices involved (the approach here solves the linear problem AX = E by factoring A as LLT ), while also reflecting its key application in kino-dynamic trajectory estimation of vehicles with axles.more » « less
-
The trajectory-aware lowest-cost path selection problem aims to find the lowest-cost path using trajectory data. Trajectory data is valuable since it carries information about travel cost along paths, and also reflects travelers' routing preference. Path-centric travel cost estimation models using trajectory data grows popular recently, which considers the auto-correlation of the energy consumption on different segments of a path. However, path-centric models are more computationally expensive than edge-centric models. The main challenge of this problem is that the travel cost of every candidate path explored during the process of searching for the lowest-cost path need to be estimated, resulting in high computational cost. The current path selection algorithms that use path-centric cost estimation models still follow the pattern of "path + edge" when exploring candidate paths, which may result in redundant computation. We introduce a trajectory-aware graph model in which each node is a maximal trajectory-aware path. Two nodes in the trajectory-aware graph are linked by an edge if their union forms a trajectory-union path. We then propose a path selection algorithm to find a path in the proposed trajectory-aware graph which corresponds to the lowest-cost path in the input spatial network. We prove theoretically the proposed algorithm is correct and complete. Moreover, we prove theoretically that the proposed path selection algorithm cost much less computational time than the algorithm used in the related work, and validate it through experiments using real-world trajectory data.more » « less
-
Robust trajectory execution is an extension of cooperative collision avoidance that takes pre-planned trajectories directly into account. We propose an algorithm for robust trajectory execution that compensates for a variety of dynamic changes, including newly appearing obstacles, robots breaking down, imperfect motion execution, and external disturbances. Robots do not communicate with each other and only sense other robots’ positions and the obstacles around them. At the high-level we use a hybrid planning strategy employing both discrete planning and trajectory optimization with a dynamic receding horizon approach. The discrete planner helps to avoid local minima, adjusts the planning horizon, and provides good initial guesses for the optimization stage. Trajectory optimization uses a quadratic programming formulation, where all safety-critical parts are formulated as hard constraints. At the low-level, we use buffered Voronoi cells as a multi-robot collision avoidance strategy. Compared to ORCA, our approach supports higher-order dynamic limits and avoids deadlocks better. We demonstrate our approach in simulation and on physical robots, showing that it can operate in real time.more » « less
-
Robust trajectory execution is an extension of cooperative collision avoidance that takes pre-planned trajectories directly into account. We propose an algorithm for robust trajectory execution that compensates for a variety of dynamic changes, including newly appearing obstacles, robots breaking down, imperfect motion execution, and external disturbances. Robots do not communicate with each other and only sense other robots’ positions and the obstacles around them. At the high-level we use a hybrid planning strategy employing both discrete planning and trajectory optimization with a dynamic receding horizon approach. The discrete planner helps to avoid local minima, adjusts the planning horizon, and provides good initial guesses for the optimization stage. Trajectory optimization uses a quadratic programming formulation, where all safety-critical parts are formulated as hard constraints. At the low-level, we use buffered Voronoi cells as a multi-robot collision avoidance strategy. Compared to ORCA, our approach supports higher-order dynamic limits and avoids deadlocks better. We demonstrate our approach in simulation and on physical robots, showing that it can operate in real time.more » « less
