Active Collaborative Localization in Heterogeneous Robot Teams

摘要

Accurate and robust state estimation is critical for autonomous navigation of robot teams.This task is especially challenging for large groups of size, weight, and power (SWAP) constrained aerial robots operating in perceptually-degraded GPS-denied environments.We can, however, actively increase the amount of perceptual information available to such robots by augmenting them with a small number of more expensive, but less resource-constrained, agents.Specifically, the latter can serve as sources of perceptual information themselves.In this paper, we study the problem of optimally positioning (and potentially navigating) a small number of more capable agents to enhance the perceptual environment for their lightweight, inexpensive, teammates that only need to rely on cameras and IMUs.We propose a numerically robust, computationally efficient approach to solve this problem via nonlinear optimization.Our method outperforms the standard approach based on the greedy algorithm, while matching the accuracy of a heuristic evolutionary scheme for global optimization at a fraction of its running time.Ultimately, we validate our solution in both photorealistic simulations and real-world experiments.In these experiments, we use lidar-based autonomous ground vehicles as the more capable agents, and vision-based aerial robots as their SWAP-constrained teammates.Our method is able to reduce drift in visual-inertial odometry by as much as 90%, and it outperforms random positioning of lidar-equipped agents by a significant margin.Furthermore, our method can be generalized to different types of robot teams with heterogeneous perception capabilities.It has a wide range of applications, such as surveying and mapping challenging dynamic environments, and enabling resilience to large-scale perturbations that can be caused by earthquakes or storms.the accuracy of a heuristic evolutionary scheme for global optimization while having a significantly lower computational demand.3) We validate our method in photorealistic simulations and real-world experiments using a robot team composed of one UAV and multiple UGVs as shown in Fig. 1.In particular, we show that our method can reduce VIO drift by as much as 90%.The rest of the paper is organized as follows.Section II summarizes the related work.Section III gives a detailed problem formulation, followed by Section IV in which we present our approach.The analysis of our algorithm is presented in Section V, and its numerical performance is showcased in Section VI.The simulation and real-world experiments in Section VII ultimately demonstrate the efficacy of our method. II. RELATED WORK A. Vision-based State EstimationVision-based state estimation has been maturing and gaining popularity during the past decade.Powerful monocular-based state estimation algorithms such as the classical structure from motion algorithm and recent state-of-the-art monocular odometry methods [3], [4] can estimate camera poses and 3D structures.However, such algorithms cannot be directly used for robot navigation since the absolute scale of the world is not observable with a single camera.VIO algorithms can estimate poses in metric scale and run up to the IMU rate.Therefore, they are commonly used in robotics applications [5].Stateof-the-art VIO algorithms [6]-[9] have robust performance in high-speed 3D navigation with aggressive motions [10]- [13].However, pure geometric-based methods have some intrinsic limitations: (1) The storage demand is high when maintaining a geometric map over a long trajectory.(2) Geometric-based features are sensitive to changes in viewpoint or lighting conditions.(3) It is difficult to distinguish features extracted from dynamic and static objects, leading to failures in dynamic environments.As a result, they can accumulate significant drift over long trajectories, which leads to unsafe behaviors and errors in mapping.Learning-enabled approaches are used to improve state estimation and mapping.Among them