An official website of the United States government
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
AXLE: Computationally-efficient trajectory smoothing using factor graph chains
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
- Award ID(s):
- 1830615
- PAR ID:
- 10294342
- Date Published:
- Journal Name:
- Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), 2021
- Format(s):
- Medium: X
- Sponsoring Org:
- National Science Foundation
More Like this
-
-
null (Ed.)Power system state estimation (PSSE) aims at finding the voltage magnitudes and angles at all generation and load buses, using meter readings and other available information. PSSE is often formulated as a nonconvex and nonlinear least-squares (NLS) cost function, which is traditionally solved by the Gauss-Newton method. However, Gauss-Newton iterations for minimizing nonconvex problems are sensitive to the initialization, and they can diverge. In this context, we advocate a deep neural network (DNN) based “trainable regularizer” to incorporate prior information for accurate and reliable state estimation. The resulting regularized NLS does not admit a neat closed form solution. To handle this, a novel end-to-end DNN is constructed subsequently by unrolling a Gauss-Newton-type solver which alternates between least-squares loss and the regularization term. Our DNN architecture can further offer a suite of advantages, e.g., accommodating network topology via graph neural networks based prior. Numerical tests using real load data on the IEEE 118-bus benchmark system showcase the improved estimation performance of the proposed scheme compared with state-of-the-art alternatives. Interestingly, our results suggest that a simple feed forward network based prior implicitly exploits the topology information hidden in data.more » « less
-
Abstract Decomposition‐based solution algorithms for optimization problems depend on the underlying latent block structure of the problem. Methods for detecting this structure are currently lacking. In this article, we propose stochastic blockmodeling (SBM) as a systematic framework for learning the underlying block structure in generic optimization problems. SBM is a generative graph model in which nodes belong to some blocks and the interconnections among the nodes are stochastically dependent on their block affiliations. Hence, through parametric statistical inference, the interconnection patterns underlying optimization problems can be estimated. For benchmark optimization problems, we show that SBM can reveal the underlying block structure and that the estimated blocks can be used as the basis for decomposition‐based solution algorithms which can reach an optimum or bound estimates in reduced computational time. Finally, we present a general software platform for automated block structure detection and decomposition‐based solution following distributed and hierarchical optimization approaches.more » « less
-
There has been a growing interest in parallel strategies for solving trajectory optimization problems. One key step in many algorithmic approaches to trajectory optimization is the solution of moderately-large and sparse linear systems. Iterative methods are particularly well-suited for parallel solves of such systems. However, fast and stable convergence of iterative methods is reliant on the application of a high-quality preconditioner that reduces the spread and increase the clustering of the eigenvalues of the target matrix. To improve the performance of these approaches, we present a new parallel-friendly symmetric stair preconditioner. We prove that our preconditioner has advantageous theoretical properties when used in conjunction with iterative methods for trajectory optimization such as a more clustered eigenvalue spectrum. Numerical experiments with typical trajectory optimization problems reveal that as compared to the best alternative parallel preconditioner from the literature, our symmetric stair preconditioner provides up to a 34% reduction in condition number and up to a 25% reduction in the number of resulting linear system solver iterations.more » « less
-
Nonlinear Model Predictive Control (NMPC) is a state-of-the-art approach for locomotion and manipulation which leverages trajectory optimization at each control step. While the performance of this approach is computationally bounded, implementations of direct trajectory optimization that use iterative methods to solve the underlying moderately-large and sparse linear systems, are a natural fit for parallel hardware acceleration. In this work, we introduce MPCGPU, a GPU-accelerated, real-time NMPC solver that leverages an accelerated preconditioned conjugate gradient (PCG) linear system solver at its core. We show that MPCGPU increases the scalability and real-time performance of NMPC, solving larger problems, at faster rates. In particular, for tracking tasks using the Kuka IIWA manipulator, MPCGPU is able to scale to kilohertz control rates with trajectories as long as 512 knot points. This is driven by a custom PCG solver which outperforms state-of-the-art, CPU-based, linear system solvers by at least 10x for a majority of solves and 3.6x on average.more » « less
- Free Publicly Accessible Full Text
-
Accepted Manuscript1.0
- Conference Paper:
- The DOI is not currently available.
