Equality Constrained Linear Optimal Control With Factor Graphs

This paper presents a novel factor graph-based approach to solve the discrete-time finite-horizon Linear Quadratic Regulator problem subject to auxiliary linear equality constraints within and across time steps. We represent such optimal control problems using constrained factor graphs and optimize the factor graphs to obtain the optimal trajectory and the feedback control policies using the variable elimination algorithm with a modified Gram-Schmidt process. We prove that our approach has the same order of computational complexity as the state-of-the-art dynamic programming approach. Furthermore, current dynamic programming approaches can only handle equality constraints between variables at the same time step, but ours can handle equality constraints among any combination of variables at any time step while maintaining linear complexity with respect to trajectory length. Our approach can be used to efficiently generate trajectories and feedback control policies to achieve periodic motion or repetitive manipulation.

Authors

• Shuo Yang
• Gerry Chen
• Yetong Zhang
• Howie Choset
• Frank Dellaert

Keywords

• Regulators
• Heuristic algorithms
• Conferences
• Optimal control
• Process control
• Trajectory
• Feedback control
• Factor Graph
• Linear Optimal Control
• Time Step
• Optimization Problem
• Computational Complexity
• Control Problem
• Dynamic Programming
• Equality Constraints
• Linear Constraints
• Optimal Control Problem
• Trajectory Optimization
• Trajectory Length
• Linear Complexity
• Linear Quadratic Regulator
• Elimination Of Variables
• Dynamic Programming Approach
• Control Input
• Baseline Methods
• Local Constraints
• Optimal Control Policy
• Probabilistic Graphical Models
• Least Squares Problem
• Objective Factors
• QR Decomposition
• Constraint Factor
• State Constraints
• General Constraints
• Optimal Policy