Certifiably Optimal State Estimation and Robot Calibration Using Trace-Constrained SDP

2025-09-28

AI Summary

In non-convex state estimation and calibration for robotics, standard semidefinite programming (SDP) relaxations often yield high-rank solutions, hindering exact recovery. To address this, we propose a trace-constrained SDP convex relaxation framework. Our method explicitly models rigid-body transformations via a fixed-trace variable, enforcing geometric consistency between rotation and translation; incorporates a gradient-based refinement strategy to accelerate convergence to rank-one solutions; and integrates a dual optimality verification mechanism to rigorously certify global optimality. This work is the first to systematically apply trace-constrained SDP to robotic pose estimation. Experiments demonstrate significant improvements in accuracy and reliability across PnP, hand-eye calibration, and dual-robot cooperative calibration—achieving both theoretical global optimality guarantees and high robustness and practicality.

Technology Category

• Intelligent Robots: State Estimation
• Search and Optimization: Non-convex Optimization
• Constraint Satisfaction and Optimization: Mixed Discrete/Continuous 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
• Security and Privacy: Large-scale security measurements

Abstract

Many nonconvex problems in robotics can be relaxed into convex formulations via semidefinite programming (SDP), which offers the advantage of global optimality. The practical quality of these solutions, however, critically depends on achieving rank-1 matrices, a condition that typically requires additional tightening. In this work, we focus on trace-constrained SDPs, where the decision variables are positive semidefinite (PSD) matrices with fixed trace values. These additional constraints not only capture important structural properties but also facilitate first-order methods for recovering rank-1 solutions. We introduce customized fixed-trace variables and constraints to represent common robotic quantities such as rotations and translations, which can be exactly recovered when the corresponding variables are rank-1. To further improve practical performance, we develop a gradient-based refinement procedure that projects relaxed SDP solutions toward rank-1, low-cost candidates, which can then be certified for global optimality via the dual problem. We demonstrate that many robotics tasks can be expressed within this trace-constrained SDP framework, and showcase its effectiveness through simulations in perspective-n-point (PnP) estimation, hand-eye calibration, and dual-robot system calibration. To support broader use, we also introduce a modular ``virtual robot'' abstraction that simplifies modeling across different problem settings.

Problem

• Achieving rank-1 solutions in trace-constrained SDP relaxations
• Recovering exact rotations and translations from relaxed variables
• Certifying global optimality in robotics estimation and calibration

Innovation

• Uses trace-constrained SDP for global optimization
• Introduces fixed-trace variables for robot quantities
• Develops gradient refinement for rank-1 solutions