System and method for autonomous vehicle control

A method, comprising:

• capturing a set of measurements of a scene surrounding a vehicle;
• based on the set of measurements, determining a trajectory for the vehicle;
• based on the trajectory, determining a set of constraints for a pair of controllers, the pair of controllers comprising a lateral controller and a longitudinal controller;
• during a first timestep:
• based on the trajectory and using a first controller of the pair of controllers, determining a first set of vehicle commands and a first set of coupling signals;
• and
• based on the trajectory, the set of constraints, and the first set of coupling signals, and using a second controller of the pair of controllers, determining a second set of vehicle commands and a second set of coupling signals;
• and
• during a second timestep:
• based on the second set of coupling signals, and using the first controller, determining a third set of vehicle commands and a third set of coupling signals;
• and
• based on the third set of coupling signals, using the second controller, determining a fourth set of vehicle commands;
• determining a set of vehicle actuator control instructions based on the third set of vehicle commands and the fourth set of vehicle commands;
• and
• controlling a set of actuators of the vehicle according to the set of vehicle actuator control instructions.