Matrix Structure Driven Interior Point Method for Quadrotor Real-Time Trajectory Planning

Sequential convex programming (SCP) has been recently employed in various trajectory planning problems, including entry flight, planetary landing, and aircraft formation. In SCP, convex programming subproblems are sequentially solved to obtain the optimum of original nonconvex problems. For SCP-based quadrotor trajectory planning, this paper proposes a matrix-structure-driven interior point method (MSD-IPM) to improve the efficiency of solving search directions in convex programming. In MSD-IPM, primal-dual systems for solving search directions are derived from the Karush-Kuhn-Tucher (KKT) conditions of quadrotor trajectory planning subproblems. Then, the successive elimination technique is used to solve the inverse of large-scale coefficient matrices of primal-dual systems by more efficient operations on small-scale matrices. In successive elimination, the positive definiteness of several small-scale matrices is used to enhance the numerical stability of computing search directions, and the specific diagonal structures of small-scale matrices are exploited to efficiently compute the search directions. The complexity analysis shows that the efficiency of the proposed method is about one order of magnitude higher than that of the standard IPM. The comparative studies on simulation experiments demonstrate that the MSD-IPM generally outperforms several well-known optimizers (e.g., MOSEK, SDPT3, and SeDuMi) in terms of efficiency and robustness. Finally, the indoor trajectory tracking experiments indicate that the proposed method can generate smooth trajectories for real-world applications.