Barrier Method for Inequality Constrained Factor Graph Optimization with Application to Model Predictive Control
Anas Abdelkarim, Daniel Görges, Holger Voos
AI summary
Problem
Factor graphs excel in robotic perception but lack efficient mechanisms for rigorously enforcing inequality constraints required in optimal control tasks like Model Predictive Control.
Approach
The authors introduce specialized non-quadratic inequality factor nodes that encode logarithmic barrier functions, enabling direct integration of the Barrier Interior Point Method into the open-source g2o framework.
Key results
- First g2o-based implementation for efficient inequality constraint handling
- Novel logarithmic barrier factor nodes bypassing quadratic cost limitations
- Faster convergence and improved computational efficiency over Augmented Lagrangian baselines
- Successful validation on multi-objective adaptive cruise control for autonomous vehicles
Why it matters
Provides roboticists and autonomous systems developers with a robust, unified optimization backend that bridges perception and constrained control without hyperparameter fragility.
Abstract
Factor graphs have demonstrated remarkable ef- ficiency for robotic perception tasks, particularly in localiza- tion and mapping applications. However, their application to optimal control problems—especially Model Predictive Control (MPC)—has remained limited due to fundamental challenges in constraint handling. This paper presents a novel integration of the Barrier Interior Point Method (BIPM) with factor graphs, implemented as an open-source extension to the widely adopted g2o framework. Our approach introduces specialized inequality factor nodes that encode logarithmic barrier functions, thereby overcoming the quadratic-form limitations of conventional factor graph formulations. To the best of our knowledge, this is the first g2o-based implementation capable of efficiently handling the constraints within a unified optimization backend. We validate the method through a multi-objective adaptive cruise control application for autonomous vehicles. Benchmark comparisons with state-of-the-art constraint-handling techniques demonstrate faster convergence and improved computational efficiency. (Code repository: https://github.com/snt-arg/bipm g2o)