The execution of flight missions by unmanned aerial vehicles (UAV) primarily relies on navigation. In particular, the navigation pipeline has traditionally been divided into positioning and control, operating in a sequential loop. However, the existing navigation pipeline, where the positioning and control are decoupled, struggles to adapt to ubiquitous uncertainties arising from measurement noise, abrupt disturbances, and nonlinear dynamics. As a result, the navigation reliability of the 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. An additional challenge is introduced to the control algorithm due to the complex wind disturbances in urban canyons. Given the fact that the system positioning and control are highly correlated with each other, this research proposes a **tightly joined positioning and control model (JPCM) based on factor graph optimization (FGO)**. In particular, the proposed JPCM combines sensor measurements from positioning and control constraints into a unified probabilistic factor graph. Specifically, the positioning measurements are formulated as the factors in the factor graph. In addition, the model predictive control (MPC) is also formulated as the additional factors in the factor graph. By solving the factor graph contributed by both the positioning-related factors and the MPC-based factors, the complementariness of positioning and control can be deeply exploited. Finally, we validate the effectiveness and resilience of the proposed method using a simulated quadrotor system which shows significantly improved trajectory following performance.

翻译：无人机执行飞行任务主要依赖于导航技术。传统导航流水线将定位与控制分为串行循环的两个独立模块。然而，现有解耦式导航流水线难以适应由测量噪声、突发扰动和非线性动力学引起的普遍不确定性，这使得无人机在复杂动态环境中的导航可靠性面临重大挑战。例如，城市复杂环境中高层建筑群对全球导航卫星系统（GNSS）信号的反射会导致定位精度显著下降，而为控制算法引入额外挑战的还包括城市峡谷中的复杂风场扰动。鉴于系统定位与控制存在高度相关性，本研究提出一种**基于因子图优化（FGO）的紧耦合定位与控制模型（JPCM）**。该模型将定位约束与控制约束的传感器测量值统一纳入概率因子图框架：具体而言，定位测量值被构建为因子图中的节点因子，而模型预测控制（MPC）约束则被转化为附加因子。通过求解由定位相关因子与MPC因子共同构成的因子图，可深度挖掘定位与控制的互补特性。最后，我们在仿真四旋翼系统上验证了所提方法的有效性与鲁棒性，实验结果表明该方法显著提升了轨迹跟踪性能。