Trajectory planning of vehicles using route information

A non-transitory computer-readable storage medium comprising at least one program for execution by at least one processor of a first device, the at least one program including instructions which, when executed by the at least one processor, cause the first device to perform a method of:

• receiving, by the at least one processor, information indicating a presence of an object operating in an environment;
• determining, by the at least one processor, a trajectory of the object, the trajectory including at least a position, a speed, and a direction of travel of the object;
• determining, by the at least one processor, an expected route of the object, wherein the expected route is pre-planned and includes an expected future position of the object at a future time;
• comparing the trajectory of the object to the expected route of the object;
• and
• in accordance with the comparison that the trajectory of the object is consistent with the expected route of the object, updating the trajectory of the object based on the expected route of the object.

A vehicle comprising:

• at least one sensor configured to capture information of an object;
• at least one transceiver configured to transmit and receive route information of the object;
• and
• at least one processor communicatively coupled to the at least one sensor and the at least one transceiver and configured to execute computer executable instructions, the execution carrying out a method of:
• receiving, by the at least one processor, information indicating a presence of the object operating in an environment;
• determining, by the at least one processor, a trajectory of the object, the trajectory including at least a position, a speed, and a direction of travel of the object;
• determining, by the at least one processor, an expected route of the object, wherein the expected route is pre-planned and includes an expected future position of the object at a future time;
• comparing the trajectory of the object to the expected route of the object;
• and
• in accordance with the comparison that the trajectory of the object is consistent with the expected route of the object, updating the trajectory of the object based on the expected route of the object.