Overtaking decision and trajectory planning in highway via hierarchical architecture of conditional state machine and chance constrained model predictive control.