Handling Constrained Optimization in Factor Graphs for Autonomous Navigation

TL;DR

This letter model constraints over variables within factor graphs by introducing a factor graph version of the Augmented Lagrangian (AL) method, and can model both optimal control for local planning and localization with factor graphs, and solve the two problems using the standard ILS methodology.

Abstract

Factor graphs are graphical models used to represent a wide variety of problems across robotics, such as Structure from Motion (SfM), Simultaneous Localization and Mapping (SLAM) and calibration. Typically, at their core, they have an optimization problem whose terms only depend on a small subset of variables. Factor graph solvers exploit the locality of problems to drastically reduce the computational time of the Iterative Least-Squares (ILS) methodology. Although extremely powerful, their application is usually limited to unconstrained problems. In this paper, we model constraints over variables within factor graphs by introducing a factor graph version of the method of Lagrange Multipliers. We show the potential of our method by presenting a full navigation stack based on factor graphs. Differently from standard navigation stacks, we can model both optimal control for local planning and localization with factor graphs, and solve the two problems using the standard ILS methodology. We validate our approach in real-world autonomous navigation scenarios, comparing it with the de facto standard navigation stack implemented in ROS. Comparative experiments show that for the application at hand our system outperforms the standard nonlinear programming solver Interior-Point Optimizer (IPOPT) in runtime, while achieving similar solutions.

Figures (8)

• Figure 1: Left: a trajectory executed by our real robot avoiding an unforeseen obstacle, a schematic factor graph is superposed to it where some robot states are represented (pink circles). Both the localization problem (orange square) and the optimal control are depicted. States are linked by the motion model (yellow squares) and subject to obstacle avoidance and trajectory tracking (green squares). Right: our custom made unicycle.
• Figure 2: Factor graph modeling the localization problem. Each measurement contributes with a factor. The green square represents the odometry prior, while purple squares represent robot measurements $\mathbf{z}=\mathbf{z}_{0:K}$, e.g. laser endpoints.
• Figure 3: Factor graph modeling an optimal control problem: green squares represent prior factors imposing that the estimate is close to the desired values $\mathbf{x}^{ref}$ and $\mathbf{u}^{ref}$, yellow squares factors modeling the motion model, blue squares upper and lower constraint factors, orange squares obstacle avoidance factors.
• Figure 4: Map of our Department.
• Figure 5: Map of factory-like environment.