US Patent 11623645 Vehicle behavior planning for overtaking vehicles

Systems, methods, and other embodiments described herein relate to improving vehicle behavior planning to avoid undertaking maneuvers. In one embodiment, a method includes generating a driving context from sensor data about a surrounding environment of an ego vehicle. The driving context identifying lanes of a roadway and a position of the ego vehicle in the lanes. The method includes, in response to determining that the driving context and a state of a nearby vehicle satisfy a merge threshold, generating a trajectory for the ego vehicle that avoids undertaking the nearby vehicle. The method includes controlling the ego vehicle according to the trajectory.