Barrier Method for Inequality Constrained Factor Graph Optimization with Application to Model Predictive Control

2025-06-17

AI Summary

Factor graph optimization struggles to handle inequality constraints efficiently, limiting its applicability in model predictive control (MPC). Method: This paper proposes an embedded solver framework based on the barrier interior-point method (BIPM), introducing logarithmic barrier-function-driven inequality factor nodes—enabling unified modeling and joint optimization of both equality and inequality constraints, thereby overcoming the traditional limitation of factor graphs to quadratic objectives and equality-only constraints. The framework is systematically integrated into the g2o optimization library, extending it for the first time to support optimal control problems with general inequality constraints. Results: Evaluated on an autonomous vehicle adaptive cruise control task, the method demonstrates faster convergence and significantly higher computational efficiency compared to existing constraint-handling approaches. It establishes a novel paradigm for real-time MPC optimization within factor graph frameworks.

Technology Category

• Constraint Satisfaction and Optimization: Mixed Discrete/Continuous Optimization
• Search and Optimization: Mixed Discrete/Continuous Search
• Reasoning under Uncertainty: Stochastic Optimization

Application Category

• Graph Algorithms and Modeling for the Web: Efficient manipulation of static and dynamic Web-related graphs
• Systems and Infrastructure for Web, Mobile and WoT: Experiences and lessons learnt from Web-based algorithms and system deployments
• Semantics and Knowledge: Methods to enhance, augment, integrate or synergize semantic models such as knowledge graphs and LLMs

Abstract

Factor graphs have demonstrated remarkable efficiency for robotic perception tasks, particularly in localization 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 both equality and inequality 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)

Problem

• Handling inequality constraints in factor graph optimization
• Extending factor graphs for Model Predictive Control applications
• Improving computational efficiency in constrained robotic control

Innovation

• Integrates Barrier Interior Point Method with factor graphs
• Introduces inequality factor nodes for logarithmic barriers
• Unified optimization backend for equality and inequality constraints