In this paper, we consider the problem of planar graph-based simultaneous localization and mapping (SLAM) that involves both poses of the autonomous agent and positions of observed landmarks. We present CPL-SLAM, an efficient and certifiably correct algorithm to solve planar graph-based SLAM using the complex number representation. We formulate and simplify planar graph-based SLAM as the maximum likelihood estimation (MLE) on the product of unit complex numbers, and relax this nonconvex quadratic complex optimization problem to convex complex semidefinite programming (SDP). Furthermore, we simplify the corresponding complex semidefinite programming to Riemannian staircase optimization (RSO) on the complex oblique manifold that can be solved with the Riemannian trust region (RTR) method. In addition, we prove that the SDP relaxation and RSO simplification are tight as long as the noise magnitude is below a certain threshold. The efficacy of this work is validated through applications of CPL-SLAM and comparisons with existing state-of-the-art methods on planar graph-based SLAM, which indicates that our proposed algorithm is capable of solving planar graph-based SLAM certifiably, and is more efficient in numerical computation and more robust to measurement noise than existing state-ofthe-art methods.

Abstract-In this paper, we propose an approach to designing online feedback controllers for input-saturated robotic systems evolving on Lie groups by extending the recently developed Sequential Action Control (SAC). In contrast to existing feedback controllers, our approach poses the nonconvex constrained nonlinear optimization problem as the tracking of a desired negative mode insertion gradient on the configuration space of a Lie group. This results in a closed-form feedback control law even with input saturation and thus is well suited for online application. In extending SAC to Lie groups, the associated mode insertion gradient is derived and the switching time optimization on Lie groups is studied. We demonstrate the efficacy and scalability of our approach in the 2D kinematic car on SE(2) and the 3D quadrotor on SE(3). We also implement iLQG on a quadrator model and compare to SAC, demonstrating that SAC is both faster to compute and has a larger basin of attraction.

No abstract

We consider the problem of distributed pose graph optimization (PGO) that has important applications in multirobot simultaneous localization and mapping (SLAM). We propose the majorization minimization (MM) method for distributed PGO (MM−PGO) that applies to a broad class of robust loss kernels. The MM−PGO method is guaranteed to converge to first-order critical points under mild conditions. Furthermore, noting that the MM−PGO method is reminiscent of proximal methods, we leverage Nesterov's method and adopt adaptive restarts to accelerate convergence. The resulting accelerated MM methods for distributed PGO-both with a master node in the network (AMM−PGO * ) and without (AMM−PGO # )-have faster convergence in contrast to the MM−PGO method without sacrificing theoretical guarantees. In particular, the AMM−PGO # method, which needs no master node and is fully decentralized, features a novel adaptive restart scheme and has a rate of convergence comparable to that of the AMM−PGO * method using a master node to aggregate information from all the nodes. The efficacy of this work is validated through extensive applications to 2D and 3D SLAM benchmark datasets and comprehensive comparisons against existing state-of-the-art methods, indicating that our MM methods converge faster and result in better solutions to distributed PGO. I. INTRODUCTIONPose graph optimization (PGO) is a nonlinear and nonconvex optimization problem estimating unknown poses from noisy relative pose measurements. PGO associates each pose with a vertex and each relative pose measurement with an edge, from which the optimization problem is well represented through a graph. PGO has important applications in a number of areas, including but not limited to robotics [1]-[3], autonomous driving [4], and computational biology [5], [6]. Recent advances [7]-[16] suggest that PGO can be well solved using iterative optimization. However, the aforementioned techniques [7]-[16] rely on a centralized optimizer to solve PGO and are difficult to distribute across a network. Due to communication and computational limitations, most, if not all, of these techniques [7]-[16] are only applicable to small-and medium-sized problems. Moreover, their centralized pipelines are equivalent to using a master node to aggregate information from the entire network, making it impossible to meet potential privacy requirements one may wish to impose [17], [18].In multi-robot simultaneous localization and mapping (SLAM) [19]-[28], each robot estimates not only its own poses but those of the others as well to build an environment map. Even though such a problem can be solved by PGO, communication between robots is restricted and multi-robot SLAM has more unknown poses than single-robot SLAM.

No abstract