Tightly Joined Positioning and Control Model for Unmanned Aerial Vehicles Based on Factor Graph Optimization

The existing motion control pipeline, where the positioning and control are decoupled, struggles to adapt to epistemic and aleatoric uncertainty and nonlinear dynamics. As a result, the motion control reliability of the unmanned aerial vehicle (UAV) is significantly challenged in complex dynamic areas. For example, the ubiquitous global navigation satellite system (GNSS) positioning can be degraded by the signal reflections from surrounding high-rising buildings in complex urban areas, leading to significantly increased positioning uncertainty. Given that positioning and control are highly correlated, this research proposes a tightly joining positioning and control model (JPCM) based on factor graph optimization (FGO). Specifically, the sensor measurements are formulated as the factors in the probabilistic factor graph. In addition, the model predictive control (MPC) is also formulated as the additional factor in the probabilistic factor graph. The factor graph contributed by both the positioning-related factors and the MPC-based factors deeply exploits the complementariness of positioning and control. Finally, we validate the effectiveness and resilience of the proposed method using simulations and real-world experiments that show significantly improved trajectory following performance.