Convex feasible set algorithm for constrained trajectory smoothing

Trajectory smoothing is an important step in robot motion planning, where optimization methods are usually employed. However, the optimization problem for trajectory smoothing in a clustered environment is highly non-convex, and is hard to solve in real time using conventional non-convex optimization solvers. This paper discusses a fast online optimization algorithm for trajectory smoothing, which transforms the original non-convex problem to a convex problem so that it can be solved efficiently online. The performance of the algorithm is illustrated in various cases, and is compared to that of conventional sequential quadratic programming (SQP). It is shown that the computation time is greatly reduced using the proposed algorithm.