Online motion planning for UAVs with multi-constraints using C/GMRES method

This paper presents an online motion planning method based on Continuation/Generalized Minimum Residual (C/GMRES) to solve a receding horizon optimization problem for unmanned aerial vehicles (UAVs). Hard constraints, such as nonlinear state, input saturation constraints and obstacle constraint are included into the problem formulation by introducing the dummy factors. The optimal control problem model of motion planning for UAVs in Bolza form is constructed and the necessary optimality conditions are derived based on the Lagrange Multipliers method. The singularity that method may exist is avoided by adding a small penalty to the Lagrange function. Compared with conventional iterative methods, the proposed method requires much less computational burden, since the linear equation is solved only once at each sampling time and the product of a matrix and a vector is replaced with forward difference approximation, then GMRES is used to rapidly solve a large linear equation. Finally, the proposed method is applied to motion planning problem for a fixed-wing UAV in both obstacle-free and obstacle environment. Simulation results show that the method can generate a feasible trajectory from initial position to destination while satisfying all constraints and the real-time performance.