Cooperative Motion Planning with 3D Spatio-temporal Map for Connected and Automated Vehicles in Intersection

Connected and automated vehicles (CAV s) are considered to improve road transportation safety and traffic efficiency. For motion planning of CAVs in signalized intersections, we need to consider the right-of-way constraints while satisfying safety and efficiency constraints. However, due to the perception range limitation of a single CAV, it is hard to obtain a satisfying planning result that balances safety, efficiency and legality. In this paper, a hierarchical cooperative motion planning (CMP) framework is proposed using vehicle to infrastructure (V2I) communication. Specifically, The CMP problem is decomposed into two stages. The first stage contains the path planning layer which generates legal path according to the static road network information, then the second stage is further divided into two parts: priority ranking and speed planning. Experimental result demonstrates the capability of our algorithm to devise plans in different traffic flow conditions.