Efficient behavior and trajectory planning is one of the major challenges for automated driving. Especially intersection scenarios are very demanding due to their complexity arising from the variety of maneuver possibilities and other traffic participants. A key challenge is to generate behaviors which optimize the comfort and progress of the ego vehicle but at the same time are not too aggressive towards other traffic participants. In order to maintain real time capability for courteous behavior and trajectory planning, an efficient formulation of the optimal control problem and corresponding solving algorithms are required. Consequently, a novel planning framework is presented which considers comfort and progress as well as the courtesy of actions in a graph-based behavior planning module. Utilizing the low level trajectory generation, the behavior result can be further optimized for driving comfort while satisfying constraints over the whole planning horizon. According experiments show the practicability and real time capability of the framework.
Typically, autonomous cars optimize for a combination of safety, efficiency, and driving quality. But as we get better at this optimization, we start seeing behavior go from too conservative to too aggressive. The car's behavior exposes the incentives we provide in its cost function. In this work, we argue for cars that are not optimizing a purely selfish cost, but also try to be courteous to other interactive drivers. We formalize courtesy as a term in the objective that measures the increase in another driver's cost induced by the autonomous car's behavior. Such a courtesy term enables the robot car to be aware of possible irrationality of the human behavior, and plan accordingly. We analyze the effect of courtesy in a variety of scenarios. We find, for example, that courteous robot cars leave more space when merging in front of a human driver. Moreover, we find that such a courtesy term can help explain real human driver behavior on the NGSIM dataset.