Method and device for multi-agent path planning

1. A method, comprising: (a) receiving an optimization problem, the optimization problem including a plurality of cost functions and a plurality of variables, the cost functions representing possible costs for values of the variables in the optimization problem, each of the cost functions having a predetermined relationship with select ones of the variables, wherein the optimization problem comprises a trajectory planning problem in which a plurality of agents are to move from a first configuration to a second configuration; (b) receiving at least one landmark, each landmark indicating at least one point that a selected one of the agents is to visit while moving from the first configuration to the second configuration, a further possible cost for the values of the variables in the optimization problem being generated when the selected agent ignores the landmark; (c) generating a first message for each of the cost functions for each corresponding variable based upon the respective predetermined relationship, the first message indicating a first belief that the corresponding variable has a first value when the optimization problem is solved, the first message having a respective first weight indicating a certainty of the first message; (d) generating a second message for each of the variables for each corresponding cost function based upon the respective predetermined relationship, the second message indicating a second belief that the corresponding variable has a second value when the optimization problem is solved, the second message having a respective second weight indicating a certainty of the second message; (e) generating a disagreement variable for each corresponding pair of variables and cost functions measuring a disagreement value between the first and second beliefs; (f) repeating steps (c), (d), and (e) until a consensus is formed between the first and second messages, a subsequent first message being modified based upon the second message, its corresponding second weight, and the corresponding disagreement variable, and a subsequent second message being modified based upon the first message, its corresponding first weight, and the corresponding disagreement variable; and (g) determining an optimization solution based upon the consensus, wherein the optimization solution comprises a solution to the trajectory planning problem.

2. The method of claim 1 , wherein the optimization problem includes the agents moving in a space including at least two dimensions.

3. The method of claim 1 , wherein the further possible cost is greater than zero.

4. The method of claim 1 , wherein the landmark is a sub-trajectory.

5. The method of claim 4 , further comprising: generating an additional cost for the values of the variables in the optimization problem being generated when the selected agent deviates from the landmark.

6. The method of claim 1 , wherein the optimization solution jointly incorporates the landmark, an assignment between the landmark and the agents, and collision-free paths for the agents that are to visit the landmark.

7. The method of claim 1 , wherein the first messages for the cost functions are computed in parallel to determine the optimization solution.

8. The method of claim 1 , wherein each of the first and second weights is one of a zero weight, an infinite weight, and a standard weight, the zero weight indicates that the corresponding message is not to be used in determining the optimization solution, the infinite weight indicates that the corresponding message specifies an unchangeable value for a variable in determining the optimization solution, and the standard weight is given a predetermined value between zero and infinity.

9. The method of claim 1 , wherein the optimization problem is one of convex and non-convex.

10. A device, comprising: a processor coupled to a memory, wherein the processor is programmed to determine an optimization solution to an optimization problem by: (a) receiving the optimization problem, the optimization problem including a plurality of cost functions and a plurality of variables, the cost functions representing possible costs for values of the variables in the optimization problem, each of the cost functions having a predetermined relationship with select ones of the variables, wherein the optimization problem comprises a trajectory planning problem in which a plurality of agents are to move from a first configuration to a second configuration; (b) receiving at least one landmark, each landmark indicating at least one point that a selected one of the agents is to visit while moving from the first configuration to the second configuration, a further possible cost for the values of the variables in the optimization problem being generated when the selected agent ignores the landmark; (c) generating a first message for each of the cost functions for each corresponding variable based upon the respective predetermined relationship, the first message indicating a first belief that the corresponding variable has a first value when the optimization problem is solved, the first message having a respective first weight indicating a certainty of the first message; (d) generating a second message for each of the variables for each corresponding cost function based upon the respective predetermined relationship, the second message indicating a second belief that the corresponding variable has a second value when the optimization problem is solved, the second message having a respective second weight indicating a certainty of the second message; (e) generating a disagreement variable for each corresponding pair of variables and cost functions measuring a disagreement value between the first and second beliefs; (f) repeating steps (c), (d), and (e) until a consensus is formed between the first and second messages, a subsequent first message being modified based upon the second message, its corresponding second weight, and the corresponding disagreement variable, and a subsequent second message being modified based upon the first message, its corresponding first weight, and the corresponding disagreement variable; and (g) determining the optimization solution based upon the consensus, wherein the optimization solution comprises a solution to the trajectory planning problem.

11. The device of claim 10 , wherein the optimization problem includes the agents moving in a space including at least two dimensions.

12. The device of claim 10 , wherein the further possible cost is greater than zero.

13. The device of claim 10 , wherein the landmark is a sub-trajectory.

14. The device of claim 13 , wherein the processor further generates an additional cost for the values of the variables in the optimization problem being generated when the selected agent deviates from the landmark.

15. The device of claim 10 , wherein the optimization solution jointly incorporates the landmark, an assignment between the landmark and the agents, and collision-free paths for the agents that are to visit the landmark.

16. The device of claim 10 , wherein the first messages for the cost functions are computed in parallel to determine the optimization solution.

17. The device of claim 10 , wherein each of the first and second weights is one of a zero weight, an infinite weight, and a standard weight, the zero weight indicates that the corresponding message is not to be used in determining the optimization solution, the infinite weight indicates that the corresponding message specifies an unchangeable value for a variable in determining the optimization solution, and the standard weight is given a predetermined value between zero and infinity.

18. A non-transitory computer readable storage medium with an executable program stored thereon, wherein the program instructs a microprocessor to perform operations comprising: (a) receiving an optimization problem, the optimization problem including a plurality of cost functions and a plurality of variables, the cost functions representing possible costs for values of the variables in the optimization problem, each of the cost functions having a predetermined relationship with select ones of the variables, wherein the optimization problem comprises a trajectory planning problem in which a plurality of agents are to move from a first configuration to a second configuration: (b) receiving at least one landmark, each landmark indicating at least one point that a selected one of the agents is to visit while moving from the first configuration to the second configuration, a further possible cost for the values of the variables in the optimization problem being generated when the selected agent ignores the landmark; (c) generating a first message for each of the cost functions for each corresponding variable based upon the respective predetermined relationship, the first message indicating a first belief that the corresponding variable has a first value when the optimization problem is solved, the first message having a respective first weight indicating a certainty of the first message; (d) generating a second message for each of the variables for each corresponding cost function based upon the respective predetermined relationship, the second message indicating a second belief that the corresponding variable has a second value when the optimization problem is solved, the second message having a respective second weight indicating a certainty of the second message; (e) generating a disagreement variable for each corresponding pair of variables and cost functions measuring a disagreement value between the first and second beliefs; (f) repeating steps (c), (d), and (e) until a consensus is formed between the first and second messages, a subsequent first message being modified based upon the second message, its corresponding second weight, and the corresponding disagreement variable, and a subsequent second message being modified based upon the first message, its corresponding first weight, and the corresponding disagreement variable; and (g) determining an optimization solution based upon the consensus, wherein the optimization solution comprises a solution to the trajectory planning problem.