Decentralized and Fully Onboard: Range-Aided Cooperative Localization and Navigation on Micro Aerial Vehicles

TL;DR

This work presents a block coordinate descent approach to localization that does not require strict coordination between the robots, and presents a novel formulation for formation control as inference on factor graphs that takes into account the state estimation uncertainty and can be solved efficiently.

Abstract

Controlling a team of robots in a coordinated manner is challenging because centralized approaches (where all computation is performed on a central machine) scale poorly, and globally referenced external localization systems may not always be available. In this work, we consider the problem of range-aided decentralized localization and formation control. In such a setting, each robot estimates its relative pose by combining data only from onboard odometry sensors and distance measurements to other robots in the team. Additionally, each robot calculates the control inputs necessary to collaboratively navigate an environment to accomplish a specific task, for example, moving in a desired formation while monitoring an area. We present a block coordinate descent approach to localization that does not require strict coordination between the robots. We present a novel formulation for formation control as inference on factor graphs that takes into account the state estimation uncertainty and can be solved efficiently. Our approach to range-aided localization and formation-based navigation is completely decentralized, does not require specialized trajectories to maintain formation, and achieves decimeter-level positioning and formation control accuracy. We demonstrate our approach through multiple real experiments involving formation flights in diverse indoor and outdoor environments.

Figures (8)

• Figure 1: A snapshot of three micro aerial vehicles (MAVs) flying in a triangular formation under a tree canopy (top) and in a GPS-denied environment inside a metal dome (bottom) using our proposed method for localization and distance-based formation control. All estimation and control is done onboard the MAVs in a decentralized manner. A video of all the experiments can be found at following link: http://tiny.cc/mara_loc_nav.
• Figure 2: Factor graph for range-aided cooperative localization with three agents. The states of the agents at different time steps are shown using colored nodes. The edges between the nodes represent intra-agent and inter-agent measurements. Each agent measures its ego motion between using an odometry sensor represented by factor $\phi_{o_{t+\i}}^a = \phi_o(\mathbf{x}_{a_{t}}, \mathbf{x}_{a_{t+1}})$ (see \ref{['eqn:odometry_factor_potential']}). Additionally, an agent $a$ measures its distance to agent $b$ represented by factor $\phi_{{r_t}}^{ab} = \phi_r(\mathbf{x}_{a_{t}}, \mathbf{x}_{b_{t}})$.
• Figure 3: Factor graph associated with agent $a$'s formation control step. The state at a time $t$, $\boldsymbol{x}_{a_t}$, consists of the agent's position and velocity input. A Gaussian process-based smooth motion prior is added as a factor between two consecutive time stamps: $\psi_{w_{t}}^a = \psi_{w}(\boldsymbol{x}_{a_{t}}, \boldsymbol{x}_{a_{t+1}})$ (see \ref{['eqn:form_ctrl_mm_factor_potential']}). Additionally, the desired distances to neighbor agents, for maintaining a pre-specified formation, are represented by the factors: $\psi_{d_{t,t'}}^{ab} = \psi_d(\boldsymbol{x}_{a_{t}}, \mathbf{x}_{b_{t'}})$ (see \ref{['eqn:form_ctrl_dist_factor_potential']}), where the neighboring agent's state is fixed to its latest estimate (as indicated by the grayed out node).
• Figure 4: Error distribution plots for localization (left) and formation control (right) from 10 simulations. The proposed decentralized cooperative localization (dcl) method has positioning RMSE similar to centralized batch trajectory estimation. The formation RMSE (FRE) (see \ref{['eqn:formation_rmse_error']}) distributions show that the proposed (prop.) approach achieves lower formation error than the baseline gradient control (gcm) approach.
• Figure 5: Estimated and ground-truth trajectories from a leader-follower flight in simulation. The leader (mav1) tracks a straight path, and the other agents (mav2 and mav3) estimate their state and the control inputs to follow the leader in a triangular formation (indicated by the red dotted lines).