A Lane-Changing Behavior Algorithm for CAVs to Facilitate Emergency Response