Certifiable Mutual Localization and Trajectory Planning for Bearing-Based Robot Swarm

Bearing measurements,as the most common modality in nature, have recently gained traction in multi-robot systems to enhance mutual localization and swarm collaboration. Despite their advantages, challenges such as sensory noise, obstacle occlusion, and uncoordinated swarm motion persist in real-world scenarios, potentially leading to erroneous state estimation and undermining the system's flexibility, practicality, and robustness.In response to these challenges, in this paper we address theoretical and practical problem related to both mutual localization and swarm planning.Firstly, we propose a certifiable mutual localization algorithm.It features a concise problem formulation coupled with lossless convex relaxation, enabling independence from initial values and globally optimal relative pose recovery.Then, to explore how detection noise and swarm motion influence estimation optimality, we conduct a comprehensive analysis on the interplay between robots' mutual spatial relationship and mutual localization. We develop a differentiable metric correlated with swarm trajectories to explicitly evaluate the noise resistance of optimal estimation.By establishing a finite and pre-computable threshold for this metric and accordingly generating swarm trajectories, the estimation optimality can be strictly guaranteed under arbitrary noise. Based on these findings, an optimization-based swarm planner is proposed to generate safe and smooth trajectories, with consideration of both inter-robot visibility and estimation optimality.Through numerical simulations, we evaluate the optimality and certifiablity of our estimator, and underscore the significance of our planner in enhancing estimation performance.The results exhibit considerable potential of our methods to pave the way for advanced closed-loop intelligence in swarm systems.

Simultaneous Time Synchronization and Mutual Localization for Multi-robot System

Mutual localization stands as a foundational component within various domains of multi-robot systems. Nevertheless, in relative pose estimation, time synchronization is usually underappreciated and rarely addressed, although it significantly influences estimation accuracy. In this paper, we introduce time synchronization into mutual localization to recover the time offset and relative poses between robots simultaneously. Under a constant velocity assumption in a short time, we fuse time offset estimation with our previous bearing-based mutual localization by a novel error representation. Based on the error model, we formulate a joint optimization problem and utilize semi-definite relaxation (SDR) to furnish a lossless relaxation. By solving the relaxed problem, time synchronization and relative pose estimation can be achieved when time drift between robots is limited. To enhance the application range of time offset estimation, we further propose an iterative method to recover the time offset from coarse to fine. Comparisons between the proposed method and the existing ones through extensive simulation tests present prominent benefits of time synchronization on mutual localization. Moreover, real-world experiments are conducted to show the practicality and robustness.

Bearing-based Relative Localization for Robotic Swarm with Partially Mutual Observations

Mutual localization provides a consensus of reference frame as an essential basis for cooperation in multirobot systems. Previous works have developed certifiable and robust solvers for relative transformation estimation between each pair of robots. However, recovering relative poses for robotic swarm with partially mutual observations is still an unexploited problem. In this paper, we present a complete algorithm for it with optimality, scalability and robustness. Firstly, we fuse all odometry and bearing measurements in a unified minimization problem among the Stiefel manifold. Furthermore, we relax the original non-convex problem into a semi-definite programming (SDP) problem with a strict tightness guarantee. Then, to hold the exactness in noised cases, we add a convex (linear) rank cost and apply a convex iteration algorithm. We compare our approach with local optimization methods on extensive simulations with different robot amounts under various noise levels to show our global optimality and scalability advantage. Finally, we conduct real-world experiments to show the practicality and robustness.

Certifiably Optimal Mutual Localization with Anonymous Bearing Measurements

Mutual localization is essential for coordination and cooperation in multi-robot systems. Previous works have tackled this problem by assuming available correspondences between measurements and received odometry estimations, which are difficult to acquire, especially for unified robot teams. Furthermore, most local optimization methods ask for initial guesses and are sensitive to their quality. In this paper, we present a certifiably optimal algorithm that uses only anonymous bearing measurements to formulate a novel mixed-integer quadratically constrained quadratic problem (MIQCQP). Then, we relax the original nonconvex problem into a semidefinite programming (SDP) problem and obtain a certifiably global optimum using with off-the-shelf solvers. As a result, our method can determine bearing-pose correspondences and furthermore recover the initial relative poses between robots under a certain condition. We compare the performance with local optimization methods on extensive simulations under different noise levels to show our advantage in global optimality and robustness. Real-world experiments are conducted to show the practicality and robustness.

Decentralized Spatial-Temporal Trajectory Planning for Multicopter Swarms

Multicopter swarms with decentralized structure possess the nature of flexibility and robustness, while efficient spatial-temporal trajectory planning still remains a challenge. This report introduces decentralized spatial-temporal trajectory planning, which puts a well-formed trajectory representation named MINCO into multi-agent scenarios. Our method ensures high-quality local planning for each agent subject to any constraint from either the coordination of the swarm or safety requirements in cluttered environments. Then, the local trajectory generation is formulated as an unconstrained optimization problem that is efficiently solved in milliseconds. Moreover, a decentralized asynchronous mechanism is designed to trigger the local planning for each agent. A systematic solution is presented with detailed descriptions of careful engineering considerations. Extensive benchmarks and indoor/outdoor experiments validate its wide applicability and high quality. Our software will be released for the reference of the community.