  This paper presents a novel model predictive control strategy for controlling
autonomous motion systems moving through an environment with obstacles of
general shape. In order to solve such a generic non-convex optimization problem
and find a feasible trajectory that reaches the destination, the approach
employs a quadratic penalty method to enforce the obstacle avoidance
constraints, and several heuristics to bypass local minima behind an obstacle.
The quadratic penalty method itself aids in avoiding such local minima by
gradually finding a path around the obstacle as the penalty factors are
successively increased. The inner optimization problems are solved in real time
using the proximal averaged Newton-type method for optimal control (PANOC), a
first-order method which exhibits low runtime and is suited for embedded
applications. The method is validated by extensive numerical simulations and
shown to outperform state-of-the-art solvers in runtime and robustness.
