Fast Trajectory Planning for AGV in the Presence of Moving Obstacles: A Combination of 3-dim A* Search and QCQP

This paper concerns about the automatic guided vehicle (AGV) trajectory planning scheme. Nominally it should be formulated as an optimal control problem (OCP) and solved via numerical methods. The concrete procedures to solve an OCP numerically include discretizing it into a mathematical programming (MP) problem and solving the MP via an appropriate solver. However, most of the predominant MP solvers only derive local optima because global optimization takes too long. As the predominant MP solvers only find local optima, the solution quality relies on the homotopy class of the initial guess, i.e. the starting point of an optimization process. A* search in the abstracted x-y-time state space is adopted to find a suitable initial guess, which directly plans a coarse trajectory rather than a path. With the initial guess, an MP in the form of a quadratically constrained quadratic program (QCQP) is solved easily. Simulation results show that the average CPU time spent on the first-A*-then-QCQP method is only l.4035 seconds in MATLAB. Source codes are provided at https://github.com/libai1943/AGV_Motion_Planning_with_Moving_Obstacles.