A method for jointly controlling vehicles, wherein the method uses a processor coupled to a memory storing a traffic configuration of a zone of intersection of a first road and a second road, wherein the intersection zone includes a sequencing zone and a control zone specified in the traffic configuration, wherein the sequencing zone includes a section of the first road and a section of the second road in proximity to the intersection, and wherein the control zone includes a section of the first road and a section of the second road between the intersection and the corresponding sections of the sequencing zone, wherein the processor is coupled with stored instructions implementing the method, wherein the instructions, when executed by the processor carry out steps of the method, comprising:
- grouping vehicles traveling on the first road and the second road within the sequencing zone into a set of groups of the vehicles, wherein at least some groups of the vehicles include vehicles traveling on the first road and vehicles traveling on the second road;
- transmitting motion constraints to the vehicles, wherein the transmitted motion constraints are configured to constrain a maximum number of vehicles allowed in the control zone by preventing the vehicles of different groups to travel simultaneously in the control zone;
- determining optimal motion trajectories for the vehicles of the same group traveling in the control zone on the first and the second roads to pass the intersection by solving an optimization problem with computational complexity constrained by the maximum number of vehicles in the control zone;
- and
- transmitting the optimal motion trajectories, thereby causing the vehicles in the control zone to follow the corresponding optimal motion trajectories.