A Successive Linearization in Feasible Set Algorithm for Vehicle Motion Planning in Unstructured and Low-Speed Scenarios

Abstract

Motion planning in unstructured and low-speed environments is a fundamental and difficult task for all mobile robotics. If we view motion planning as an optimization problem, the non-convex collision avoidance constraints and the nonlinear vehicle dynamic constraints make motion planning challenging and time-consuming. In this paper, we propose a Successive Linearization in Feasible Set (SLiFS) algorithm to address these two difficulties. SLiFS consists of two steps. The first step is to iteratively construct convex feasible sets around the current trajectory to approximate non-convex collision avoidance constraints. The second step is to successively linearize the nonlinear dynamic constraints along the current trajectory and further penalize them into the objective function to avoid infeasible linearized constraints, so that the current trajectory can be reshaped within the obtained convex feasible sets by iteratively solving the linearized optimization problem. The main innovation of SLiFS algorithm is that we consider the <inline-formula xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"> <tex-math notation="LaTeX">$L_{1}$ </tex-math></inline-formula> norm type penalty function in the second step. We find that the sparsity of the <inline-formula xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"> <tex-math notation="LaTeX">$L_{1}$ </tex-math></inline-formula> norm might help to satisfy the robotic dynamic constraints by numerical experiments. Numerical testing results show that our proposed SLiFS algorithm has a high success rate to find feasible trajectories and costs much less time than the classical interior-point method.