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

Constrained factor graphs; interior point method; robotics control; robotics perception; Barrier method; Constrained factor graph; Factor graphs; Graph optimization; Interior-point method; Localization and mappings; Mapping applications; Model-predictive control; Robotic controls; Robotic perception; Control and Systems Engineering; Biomedical Engineering; Human-Computer Interaction; Mechanical Engineering; Computer Vision and Pattern Recognition; Computer Science Applications; Control and Optimization; Artificial Intelligence; Optimization; Robots; Costs; Convergence; Probabilistic logic; Simultaneous localization and mapping; Predictive control; Covariance matrices; Autonomous vehicles; Vectors

Abstract :

[en] 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 letter 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.

Precision for document type :

Review article

Disciplines :

Electrical & electronics engineering

Author, co-author :

ABDELKARIM, Anas ; University of Luxembourg > Interdisciplinary Centre for Security, Reliability and Trust (SNT) > Automation ; RPTU Kaiserslautern–Landau, Department of Electrical and Computer Engineering (EIT), Kaiserslautern, Germany

Görges, Daniel ; RPTU Kaiserslautern–Landau, Department of Electrical and Computer Engineering (EIT), Kaiserslautern, Germany

VOOS, Holger ; University of Luxembourg > Interdisciplinary Centre for Security, Reliability and Trust (SNT) > Automation

External co-authors :

yes

Language :

English

Title :

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

Publication date :

06 October 2025

Journal title :

IEEE Robotics and Automation Letters

eISSN :

2377-3766

Publisher :

Institute of Electrical and Electronics Engineers Inc.

Volume :

Issue :

Pages :

12333 - 12340

Peer reviewed :

Peer Reviewed verified by ORBi

Additional URL :

http://xplorestaging.ieee.org/ielx8/7083369/11215960/11192685.pdf?arnumber=11192685

FnR Project :

17041397

Funders :

Fonds National de la Recherche Luxembourg

Funding text :

Received 17 June 2025; accepted 19 September 2025. Date of publication 6 October 2025; date of current version 23 October 2025. This article was recommended for publication by Associate Editor Y. Chen and Editor L. Pal-lottino upon evaluation of the reviewers\u2019 comments. This work was supported by Luxembourg National Research Fund (FNR), MOCCA Project under Grant 17041397. For the purpose of open access, and in fulfilment of the obligations arising from the grant agreement, the author has applied a Creative Commons Attribution 4.0 International (CC BY 4.0) license to any Author Accepted Manuscript version arising from this submission. (Corresponding author: Anas Abdelkarim.) Anas Abdelkarim is with the Interdisciplinary Centre for Security, Reliability and Trust (SnT), University of Luxembourg, L-1855 Esch-sur-Alzette, Luxembourg, and also with the Department of Electrical and Computer Engineering (EIT), RPTU Kaiserslautern\u2013Landau, 67663 Kaiserslautern, Germany (e-mail: abdelkarim@eit.uni-kl.de).

Available on ORBilu :

since 05 January 2026

Statistics

Number of views

26 (1 by Unilu)

Number of downloads

16 (3 by Unilu)

More statistics

Scopus citations^®

Scopus citations^®
without self-citations

OpenCitations

WoS citations^™

Bibliography

F. Dellaert and M. Kaess, “Factor graphs for robot perception,” Foundations Trends Robot., vol. 6, no. 1/2, pp. 1–139, 2017.
H. Bavle, J. L. Sanchez-Lopez, M. Shaheer, J. Civera, and H. Voos, “S-graphs+: Real-time localization and mapping leveraging hierarchical representations,” IEEE Robot. Automat. Lett., vol. 8, no. 8, pp. 4927–4934, Aug. 2023.
A. Bemporad, “Model predictive control design: New trends and tools,” in Proc. 45th IEEE Conf. Decis. Control, 2006, pp. 6678–6683.
A. Abdelkarim, “Development of numerical solvers for online optimization with application to mpc-based energy-optimal adaptive Cruise control,” M.S. thesis, Technische Universität Kaiserslautern, Kaiserslautern, Germany, 2020. Accessed: Aug. 28, 2025. [Online]. Available: http://dx.doi.org/10.13140/RG.2.2.11897.28000
B. Bazzana, H. Andreasson, and G. Grisetti, “How-to augmented Lagrangian on factor graphs,” IEEE Robot. Automat. Lett., vol. 9, no. 3, pp. 2806–2813, Mar. 2024.
J. A. Andersson, J. Gillis, G. Horn, J. B. Rawlings, and M. Diehl, “Casadi: A software framework for nonlinear optimization and optimal control,” Math. Program. Computation, vol. 11, pp. 1–36, 2019.
A. Abdelkarim and P. Zhang, “Optimal scheduling of preventive maintenance for safety instrumented systems based on mixed-integer programming,” in Proc. 7th Int. Symp. Model-Based Saf. Assessment, 2020, pp. 83–96.
A. Abdelkarim, Y. Jia, and D. Gorges, “Optimization of vehicle-to-grid profiles for peak shaving in microgrids considering battery health,” in Proc. 49th Annu. Conf. IEEE Ind. Electron. Soc., 2023, pp. 1–6.
A. Abdelkarim, H. Voos, and D. Görges, “Factor graphs in optimization-based robotic control-a tutorial and review,” IEEE Access, vol. 13, pp. 28315–28334, 2025.
F. Bai, T. Vidal-Calleja, and S. Huang, “Robust incremental SLAM under constrained optimization formulation,” IEEE Robot. Automat. Lett., vol. 3, no. 2, pp. 1207–1214, Apr. 2018.
D. P. Bertsekas, Constrained Optimization and Lagrange Multiplier Methods. Burlington, MA, USA: Academic Press, 2014.
S. P. Boyd and L. Vandenberghe, Convex Optimization. Cambridge, UK: Cambridge Univ. Press, 2004.
A. Forsgren, P. E. Gill, and M. H. Wright, “Interior methods for nonlinear optimization,” SIAM Rev., vol. 44, no. 4, pp. 525–597, 2002.
J. Bleyer, “Advances in the simulation of viscoplastic fluid flows using interior-point methods,” Comput. Methods Appl. Mechan. Eng., vol. 330, pp. 368–394, 2018.
S. Yang, G. Chen, Y. Zhang, H. Choset, and F. Dellaert, “Equality constrained linear optimal control with factor graphs,” in Proc. IEEE Int. Conf. Robot. Automat., 2021, pp. 9717–9723.
D.-N. Ta, M. Kobilarov, and F. Dellaert, “A factor graph approach to estimation and model predictive control on unmanned aerial vehicles,” in Proc. Int. Conf. Unmanned Aircr. Syst., 2014, pp. 181–188.
A. Abdelkarim, H. Voos, and D. Görges, “ECG2O: A seamless extension of g2o for equality-constrained factor graph optimization,” 2025, arXiv:2503.01311.
M. Xie, A. Escontrela, and F. Dellaert, “A factor-graph approach for optimization problems with dynamics constraints,” 2020, arXiv:2011.06194.
M. King-Smith, P. Tsiotras, and F. Dellaert, “Simultaneous control and trajectory estimation for collision avoidance of autonomous robotic spacecraft systems,” in Proc. Int. Conf. Robot. Automat., 2022, pp. 257–264.
P. Yang and W. Wen, “Tightly joining positioning and control for trustworthy unmanned aerial vehicles based on factor graph optimization in urban transportation,” in Proc. IEEE 26th Int. Conf. Intell. Transp. Syst., 2023, pp. 3589–3596.
P. Sodhi, S. Choudhury, J. G. Mangelson, and M. Kaess, “ICS: Incremental constrained smoothing for state estimation,” in Proc. IEEE Int. Conf. Robot. Automat., 2020, pp. 279–285.
M. Qadri, P. Sodhi, J. G. Mangelson, F. Dellaert, and M. Kaess, “InCOpt: Incremental constrained optimization using the Bayes tree,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2022, pp. 6381–6388.
D. Steck, “Lagrange multiplier methods for constrained optimization and variational problems in Banach spaces,” Ph.D. dissertation, Universität Würzburg, Würzburg, Germany, 2018.
A. Abdelkarim, Y. Jia, and D. Görges, “An accelerated interior-point method for convex optimization leveraging backtracking mitigation,” in Proc. 49th Annu. Conf. IEEE Ind. Electron. Soc., 2023, pp. 1–6.
Y. Jia, A. Abdelkarim, X. Klingbeil, R. Savelsberg, and D. Görges, “Performance evaluation of energy-optimal adaptive cruise control in simulation and on a test track,” IFAC-PapersOnLine, vol. 56, no. 2, pp. 4994–5000, 2023.
“Commission regulation (EU) 2016/427 of 10 Mar. 2016 amending regulation (EC) No 692/2008 as regards the determination of CO2 emissions and fuel consumption of vehicles powered by compressed natural gas (CNG) and liquefied natural gas (LNG),” Official J. Eur. Union, vol. 82, pp. 1–98, 2016. [Online]. Available: https://eur-lex.europa.eu/eli/reg/2016/427/oj