A Decision and Control Strategy for Two-Stage Collision Avoidance in Automated Vehicles

This paper presents a two-stage collision avoidance (CA) approach for automated vehicles. Four one-stage CA scenarios are formulated and solved analytically via Pontryagin’s Minimum Principle (PMP), forming the basis for two-stage CA. By embedding the second-stage cost into the first-stage terminal cost, the problem is reformulated as a single optimal control problem (OCP), reducing computational complexity and enabling real-time execution. The algorithm runs at 23 ms per step on an Infineon TC397 controller, supporting 20 Hz operation. A control strategy with a control allocator and stability controller keeps the vehicle sideslip angle below 2.7° at 20 m/s. Hardware-in-the-loop (HIL) tests confirm collision-free trajectories and stable behavior, even on low-adhesion roads. In challenging scenarios where separate two-stage controllers and artificial potential field (APF) methods fail, the proposed approach maintains a minimum clearance of 0.2 m, demonstrating its practical applicability for real-time emergency CA.