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

TL;DR

This paper introduces a novel barrier method integrated with factor graphs to efficiently handle inequality constraints in optimal control problems, demonstrated through autonomous vehicle control with improved convergence.

Contribution

It presents the first g2o-based implementation of inequality constraint handling using barrier functions, extending factor graph optimization capabilities for control applications.

Findings

Faster convergence compared to existing methods

Improved computational efficiency in constraint handling

Successful application to autonomous vehicle control

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)