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.

I. Introduction

Trajectory or motion planning is one of the key challenges in robotics. Robots need to find motion trajectories to accomplish certain tasks in constrained environments in real time. The scenarios include but are not limited to navigation of unmanned areal or ground vehicles in civil tasks such as search and rescue, surveillance and inspection; or navigation of autonomous or driverless vehicles in future transportation systems.