Reference Tracking Optimization With Obstacle Avoidance via Task Prioritization for Automated Driving

Obstacle avoidance is a fundamental operation for automated driving and its formulation traditionally originates from robotics and decision making control fields. Given the high complexity required to compute an obstacle-free trajectory, this operation is usually demanded to a lower frequency planning layer that provides then a trajectory reference to be followed by a higher frequency control layer. As a result, whenever replanning is needed (for example, due to a new detected obstacle), the control layer must wait for a new planned trajectory to be generated. In this paper, we propose a novel methodology to approach obstacle avoidance already in the control layer, which allows a prompter response. In particular, we show how obstacle avoidance and reference tracking can be integrated, thus with no need to switch among different controllers, based on a null-space based behavioral control approach, implemented in a (possibly nonlinear) model predictive control scheme. We demonstrate practical implementation of the proposed methodology employing two different vehicle dynamic models and in four different (urban and highway) scenarios. Furthermore, we provide a sensitivity analysis to understand how parameters choice affects the automated vehicle behavior.