Motion Planning for Connected Automated Vehicles at Occluded Intersections With Infrastructure Sensors

Motion planning at urban intersections that accounts for the situation context, handles occlusions, and deals with measurement and prediction uncertainty is a major challenge on the way to urban automated driving. In this work, we address this challenge with a sampling-based optimization approach. For this, we formulate an optimal control problem that optimizes for low risk and high passenger comfort. The risk is calculated on the basis of the perception information and the respective uncertainty using a risk model. The risk model combines set-based methods and probabilistic approaches. Thus, the approach provides safety guarantees in a probabilistic sense, while for a vanishing risk, the formal safety guarantees of the set-based methods are inherited. By exploring all available behavior options, our approach solves decision making and longitudinal trajectory planning in one step. The available behavior options are provided by a formal representation of the situation context, which is also used to reduce calculation efforts. Occlusions are resolved using the external perception of infrastructure-mounted sensors. Yet, instead of merging external and ego perception with track-to-track fusion, the information is used in parallel. The motion planning scheme is validated through real-world experiments.