Method and device for multi-agent path planning

Abstract

A method and device determines an optimization solution for an optimization problem. The method includes receiving the optimization problem having cost functions and variables where the cost functions have a relationship with the variables and receiving a landmark indicating a point that an agent is to visit while moving, a cost being associated with ignoring the landmark. The method includes generating a first message for the cost functions for the corresponding variable based upon the relationship and a second message for each of the variables for the corresponding cost function based upon the relationship. The method includes generating a disagreement variable for each corresponding pair of variables and cost functions measuring a disagreement value between the first and second beliefs. The method includes repeating steps (c), (d), and (e) until a consensus is formed between the first and second messages until the optimization solution is determined based upon the consensus.

Description

BACKGROUND INFORMATION

An optimization algorithm may be used to determine a solution for a problem in which the solution considers all variables and constraints related to the problem and provides a lowest cost configuration of the values for all the variables. For example, in a televised event, a plurality of cameras may be used to capture the event. The cameras may move along planned trajectories to fully capture the event. Accordingly, the problem associated with such a scenario may be to determine the planned trajectories for the cameras that incorporate the variables and constraints associated therewith. Such variables and constraints may include no overlap of space as the cameras cannot physically be co-located at a common time, a trajectory path that provides a sufficient video capture of the event, a tracking algorithm for the occurrences during the event, etc. Through incorporation of all these considerations, the optimization algorithm may determine the trajectories of the cameras to provide the lowest cost solution (e.g., least distance to be covered by the cameras, best coverage of the event, least energy requirements, etc.). However, most optimization algorithms require significantly high processing requirements for all these considerations to be assessed as well as provide the lowest cost solution to the problem.

A conventional algorithm used for convex optimization is the Alternating Direction Method of Multipliers (ADMM). Conventionally, the ADMM algorithm is a variant of an augmented Lagrangian scheme that uses partial updates for dual variables. This algorithm has also been utilized and shown to be well-suited for distributed implementations. Further developments in using the ADMM algorithm have included being used in applications such as Linear Programming (LP) decoding of error-correcting codes and compressed sensing. However, the ADMM algorithm has in practice been used exclusively for convex optimization problems whereas determining optimal trajectories is not convex. Another conventional approach for optimization uses graphical models in which a graph denotes conditional dependencies between random variables. The graphical models often are combined with a Belief Propagation (BP) message-passing algorithm in which inferences are performed on the graphical models. The BP message-passing algorithm calculates the marginal distribution for each unobserved node, conditional on any observed nodes within the graphical model.

Even with altering the above conventional approaches to fit the optimization problem of determining trajectories, a further consideration in determining optimal trajectories involves the number of dimensions in which the objects may move. For example, the cameras may be limited to a surface upon which the camera is placed upon such as mounted on a land vehicle, placed on rails or tracks, etc. That is, this may represent a two-dimensional path restriction as they are limited to move along this plane. In another example, the cameras may be free to move in any direction which may represent a three-dimensional path availability such as aerial movements (e.g., mounted on an unmanned aerial vehicle) or underwater movements (e.g., mounted on an autonomous underwater vehicle). Therefore, even if the ADMM and/or BP algorithms are altered for use in the less complex two-dimensional analysis of determining optimal trajectories, the applicability of these altered algorithms will have limited applicability to the three-dimensional analysis.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 shows a device for generating an optimization solution according to an exemplary embodiment.

FIG. 2 shows a graphical model according to an exemplary embodiment.

FIG. 3A shows a graphical model as applied to a multi-agent trajectory planning problem according to an exemplary embodiment.

FIG. 3B shows input and output variables for each operator of the graphical model of FIG. 3A according to an exemplary embodiment.

FIG. 4 shows a method of generating an optimization solution according to an exemplary embodiment.

FIG. 5A shows a convergence graph from using the method of FIG. 4.

FIG. 5B shows a distribution graph from using the method of FIG. 4.

DETAILED DESCRIPTION

The exemplary embodiments relate to a device and method for planning trajectories for multiple agents. The exemplary embodiments may be further understood with reference to the following description and the appended drawings, wherein like elements are referred to with the same reference numerals. The exemplary embodiments describe a method and device for generating an optimization solution. Specifically, the method and device utilize a message-passing version of the ADMM algorithm that is applicable to determining trajectories that may travel within a three-dimensional space. The message-passing algorithm includes a weighting system in which a message passed between nodes of a graphical model may include weights associated with confidence values of the messages. The optimization solution, the generation thereof, the message-passing algorithm, the ADMM algorithm, the confidence weighting system, and a related method will be described in further detail below.

The exemplary embodiments provide a mechanism for planning collision-free paths for multiple agents using optimization methods. Specifically, the exemplary embodiments utilize proximal algorithms in the mechanism. As will become evident below, the proximal algorithms used with the exemplary embodiments provide parallelization and decentralization such that the speed with which the algorithms may generate good quality solutions is improved and its ability to incorporate different proximal operators is more efficient. The proximal operators may ensure that paths satisfy a desired property that may be initial constraints that are applied to the problem. Through updates and modifications to the proximal operators through the exemplary embodiments, they may be applied to paths beyond two-dimensions and allow for intermediate waypoints to be independent of agents instead of being preassigned to specific agents. The new proximal operators according to the exemplary embodiments may deal with agents moving in arbitrary dimensions that more efficiently compute and further features such as landmarks may be introduced which are space-time positions that are automatically assigned to the set of agents under different optimality criteria.

Initially, the formulation of path planning may be described as an optimization problem. As an optimization problem, the incorporation of the proximal algorithms according to the exemplary embodiments is hereby introduced. For example, a system may have p spherical agents in R^d of radius {r_i}_i=1^p. Accordingly, the objective is to find collision free paths {x_i(t)}_{i∈[p],i∈[0,T]} for all agents between a respective specified initial position (x_i^init)_i=1^p at time 0 and specified final position {x_i^final}_i=1^p at time T. In a simplest case, the total time from 0 to T may be divided into intervals of equal length and the path {x_i(t)}_t=0^T of agent i∈[p] is parametrized by a set of break points {x_i(s)}_s=0^η such that x_i(t)=x_i(s) for t=sT/η and all s. It is noted that between break points, agents may be assumed to have zero-acceleration.

In this manner, a global path planning may be expressed as an optimization problem with an objective function that is a large sum of simple cost functions. Each function may account for a different aspect of the problem. Using the above notion, the following may be solved for the problem:

$\begin{array}{cc}\mathrm{minimize}\sum _i{f}^{\mathrm{pos}}\left(x_i\left(0\right),x_i^{\mathrm{init}}\right)+\sum _i{f}^{\mathrm{pos}}\left(x_i\left(\eta \right),x_i^{\mathrm{final}}\right)+\sum _{i>j,s}{f}_{i,j}^{\mathrm{coll}}\left(x_i\left(s\right),x_i\left(s+1\right),x_j\left(s\right),x_j\left(s+1\right)\right)+\sum _{i,s}{f}_i^{\mathrm{vel}}\left(x_i\left(s\right),x_i\left(s+1\right)\right)+\sum _{i,s}{f}_i^{\mathrm{dir}}\left(x_i\left(s\right),x_i\left(s+1\right),x_i\left(s+2\right)\right)+\sum _{W,i,s}{f}_W^{\mathrm{wall}}\left(x_i\left(s\right),x_i\left(s+1\right)\right)& \left(\mathrm{Equation}1\right)\end{array}$ The function ƒ^coil prevents agent-agent collisions. That is, the function ƒ^coil is zero if ∥αx_i(s)+(1−α)x_i(s+1)−(αx_j(s)+(1−α)x_j(s+1)∥r_i+r_j for all α∈[0,1] and infinity otherwise. The function ƒ^wall prevents agents from colliding with obstacles (i.e., non-agent objects). That is, the function ƒ^wall is zero if ∥αx_i(s)+(1−α)x_i(s+1)−y∥≥r_i for all α∈[0,1],y∈W and is infinity otherwise. W is a set of points defining an obstacle. W may be a line between two points x_L and x_R in a plane and a summation Σ_W is across a set of obstacles. The functions ƒ^vel and ƒ^dir may impose restrictions on the velocities and direction changes of the paths, respectively. The function ƒ^pos may impose boundary conditions. That is, the function ƒ^pos is zero if x_i(0=x_i^init) (or if x_i(η)=x_i^final) and infinity otherwise.

As discussed above, the multi-agent path planning uses proximal algorithms for solving. Furthermore, the multi-agent path planning is formulated as a large non-convex optimization problem. The exemplary embodiments further utilize the Alternating Direction Method of Multipliers (ADMM) and a variant that introduces a Three Weight Algorithm (TWA). Specifically, the exemplary embodiments provide how an ADMM algorithm for a constrained optimization problem may be derived as a specific case of a message-passing version of the ADMM algorithm for convex optimization, particularly when the agents are allowed to move along paths in a three-dimensional space. The message-passing algorithm is improved upon through introduction of the TWA for the messages between nodes of a graphical model with “certain” and “no opinion” weights as well as the standard weight used in the ADMM. The “certain” messages allow the optimization algorithm to implement constraint propagation while the “no opinion” messages speed convergence for some problems by making the optimization algorithm focus only on active constraints. The three-weight version of message-passing in the ADMM further provides greatly improved performance in non-convex problems while retaining the exact performance of ADMM for convex problems. It is noted that the nodes of the graphical model may be equivalent to the proximal operators described above. Thus, the term “node” as related to the graphical model and the term “proximal operator” may be used interchangeably.

The exemplary embodiments overcome the conventional limits of the ADMM algorithm and are used as a well-defined algorithm for general optimization functions and provide a powerful heuristic algorithm even for NP-hard non-convex problems. When the objective function being optimized by the ADMM algorithm consists entirely of “hard” constraints, the ADMM algorithm reduces to a message-passing version of the “Divide and Concur” (DC) constraint satisfaction algorithm which is effective for a very wide variety of non-convex constraint satisfaction problems. Again, the exemplary embodiments are further adapted for application to agents moving in a three-dimensional space.

As will be described in further detail below, the use of the message-passing algorithm between variables and cost functions in the graphical model enables messages to be sent between these nodes until a consensus is reached; that is, until an optimization solution has been generated. Specifically, messages are transmitted from right side nodes representing a first set of consensus functions to left side nodes representing a second set of functions. That is, the left side functions may be the proximal operators. The left side nodes process those messages and compute new messages that are transmitted back from left side nodes to the right side nodes. The right side nodes then use the messages that have come from the left side nodes to compute new messages that are sent to the left side nodes, and the cycle continues. The weights associated with the messages are determined such that the messages have an appropriate impact in the processing of the messages to compute new messages. The weights for the messages have one of three different weights. A “certain” weight indicates that a cost function or variable (i.e., one of the nodes) is certain (e.g., 100% confidence) about a value of a variable and all other opinions should defer thereto; a “no opinion” weight indicates that the node has no opinion about the value (e.g., may be omitted in the considerations for the consensus); and a “standard” weight indicates that the message is weighted equally with all other standard weight messages.

In view of the above discussion of the manner in which the message passing operates, the ADMM and the TWA are iterative algorithms that do not access the objective function directly but indirectly through multiple algorithmic blocks. As used above, these algorithmic blocks may be the nodes of the graphical model or the proximal operators. The blocks may be used one per function-term in the objective. At each iteration, these proximal operators may be applied independently and so both the TWA and the ADMM are easily parallelized. In general, the ADMM and the TWA are not guaranteed to converge for non-convex problems.

In using the TWA version of the message-passing ADMM, the ADMM iteratively updates five kinds of variables living on the edges of a factor graph as used in a graphical model that is composed of function nodes and variable nodes. The factor graph represents the decomposition of the objective function into separate terms. The ADMM is guaranteed to solve convex problems but empirically, the ADMM and the TWA can also find good feasible solutions for large non-convex problems via the modifications described above.

As discussed above with proximal algorithms, the ADMM does not access a function ƒ in the objective directly but rather indirectly through its proximal operator. For example, the proximal operator may be:

$\begin{array}{cc}x\in \arg \underset{x^{\prime }}{\min }f\left(x^{\prime }\right)+\frac{\rho }{2}{x^{\prime }-n}^2& \left(\mathrm{Equation}2\right)\end{array}$ In this proximal operator, ∈ is used instead of = to indicate that, for a non-convex function ƒ, the operator may be one-to-many, in which case extra tie-breaking rule needs to be implemented. The proximal operators appear in the steps that update the left side nodes. Thus, the implementation of these proximal operators is emphasized as an inner-loop step in this framework that provides significant information in the determination of the solution to this optimization problem. For example, when ƒ=ƒ^vel takes a quadratic (e.g., kinetic energy) form, the operator such as used in Equation 2 has a simple closed-form expression. However, for E(r) or E(r), the operator involves solving a semi-infinite programming (SIP) problem. Those skilled in the art will understand that the SIP problem involves solving a non-trivial problem with an infinite number of constraints and a finite number of variables.

The ADMM according to the exemplary embodiments rely upon two constant parameters of ρ and γ>0. Their performance may be improved by using different values of ρ. The TWA improves performance further by assigning to each edge two values {right arrow over (ρ)} and and dynamically assigning to them values in {0,const.,∞}, where {right arrow over (ρ)} represents a direction from a node on the left side (e.g., of the graphical model on a factor graph representation) to another node on the right side and represents an opposite direction. That is, implementing the TWA requires computing proximal operators and specifying outgoing weights {right arrow over (ρ)} of each proximal operator. In the TWA, there are also incoming weights which correspond to the ρ that appear in the definition of the proximal operators but having a fixed update scheme. For the collision operator and landmark operator as described in further detail below, a computation of outgoing weights may be used in which if an operator maps a set of input variables (n₁, n₂, . . . , n_k) to a set of output variables (x₁, x₂ . . . , x_k), then, if n_i≠x_i, a first setting of {right arrow over (ρ)}_i=1 may be used, otherwise {right arrow over (ρ)}_i=0 may be used when n_i=x_i. That is, if a variable is unchanged by the operators, the outgoing weight associated to it should be zero, otherwise it is 1.

With the above basis of using the variant of the ADMM and utilizing the TWA, the exemplary embodiments further modify this mechanism to provide a set of algorithmic building blocks in the proximal operators to plan paths for a system of multiple independent agents (e.g., robots) that need to move optimally across a set of locations and avoid collisions with obstacles and each other. Such an optimization problem may be crucial in applications involving automated storage, exploration, and surveillance. The paths of the agents also incorporates the volume of the agent itself, thereby the agents may further incorporate a clearance along its path. Even if each agent has a few degrees of freedom since the paths may be within a three-dimensional space, the joint system is complex and this problem has a relatively high degree of difficulty.

There have been limitations in previous attempts of using the variant of the ADMM with the TWA in the optimization problem being addressed by the exemplary embodiments. One limitation is that solving the SIP problem for robots is limited to two-dimensional movements since the problem is interpreted as a mechanics problem which excludes the applications for which the exemplary embodiments specifically relate. Another limitation is that the prior mechanism does not allow robots to automatically select waypoint positions from a set of reference positions. Such a feature may be required when, for example, problems involve robots in formations such as with the unmanned aerial vehicles and autonomous underwater vehicles. Rather, any reference position must be pre-assigned to a specific robot.

The exemplary embodiments address at least these limitations. Thus, in a first feature, the exemplary embodiments may be applied to a SIP problem involved in collision proximal operators that are reduced to solving a single-constraint non-convex problem with a solution that is determined explicitly in arbitrary dimensions, and that also more efficiently determines the solution, even in two dimensions. In a second feature, the exemplary embodiments provide further modified proximal operators that automatically assign agents to a subset of reference positions and penalize non-optimal assignments. These features may allow solutions to be determined for instances of path planning problems involving many agents that may need to move tightly together. Furthermore, these feature may be used for proximal algorithms to solve other SIP problems. That is, it is noted that the application to path planning is only exemplary and the exemplary embodiments may be applied to other optimization problems.

FIG. 1 shows a device 100 for generating an optimization solution according to an exemplary embodiment. The device 100 may be any electronic component that is configured to receive and process data such that the generating is performed. The device 100 may include a processor 105, a memory arrangement 110, a transceiver 115, and an input/output (I/O) device 120. The processor 105 may be configured to execute an optimization application. The memory 110 may store data that is received as well as data that is determined. The transceiver 115 and the I/O device 120 may provide a communicative connection to other electronic devices or users such that data may be received for processing. For example, a user may manually enter constraints involved in a particular optimization scenario or problem.

The processor 105 may further execute a plurality of engines that are configured to perform a functionality associated with generating the optimization solution. As illustrated, the processor 105 may execute a variable engine 125, a cost engine 130, a landmark engine 132, a factor graph engine 135, a constraint engine 140, an optimization engine 145, and a signal generating engine 150. The engines will be described in further detail below.

The optimization functionality executed by the processor 105 utilizes a graphical model in order to generate the optimization solution. When relating to optimization, the graphical model includes a first plurality of cost functions and a second plurality of variables, as well as edges connecting cost functions to variables. The cost functions may represent either “soft” costs or “hard” constraints. An edge exists between a cost function and a variable if the variable is involved in the cost function. In a “soft” cost function, the cost function encodes a preference for the connected variables to have particular values, but that preference may be violated, while in a “hard” constraint version of a cost function, the preferences must not be violated. Each cost function may be connected to one or more of the variables and vice versa.

In view of the manner in which the optimization functionality is utilized, the variable engine 125 may receive the set of variables while the cost engine 130 may receive the set of cost functions. As will be described in further detail below, the landmark engine 132 may receive a set of landmarks which represent a point or group of points in the space that the agent should attempt to visit via its path planning. The factor graph engine 135 may generate the graphical model for the variables and the cost functions. Through the connections between the cost functions and the variables, the constraint engine 140 may provide the lines of the graphical model to the factor graph engine 135. It is again noted that the nodes of the graphical model may be the proximal operators of the exemplary embodiments. All this data may be received by the optimization engine 145 that generates the optimization solution. The signal generating engine 150 may generate a signal based upon the generated optimization solution such that a particular application that requires this solution may receive it. For example, a particular application may be trajectories for a plurality of cameras. The signal generating engine 150 may generate signals for each camera that indicate the trajectory at which the camera is to move.

FIG. 2 shows a graphical model 200 according to an exemplary embodiment. Specifically, the graphical model 200 may be the standard “factor graph” for the above described optimization problem. The graphical model 200 includes a set of hard or soft cost functions E1, E2, E3, and E4 as well as a set of variables r1, r2, r3, r4, r5, and r6. When a line connects a cost function node with a variable node, this indicates that the cost function depends upon the corresponding variable. In order to derive the message-passing algorithm for the ADMM algorithm, the problem may be manipulated into a series of equivalent forms before actually minimizing the objective. The first manipulation is a conversion of the problem over the variables r_j into an equivalent problem that depends on variables x_ij that sit on the edges of a “normalized” Forney-style factor graph. Again, the blocks of E1-E4 may be the proximal operators. The variables in the standard factor graph are replaced with equality constraints and each of the edge variables is a copy of the corresponding variable that was on its right. Thus, the edge variables attached to the same equality constraint must ultimately equal each other but they may temporarily be unequal while they separately attempt to satisfy different cost functions on the left. The below description uses the notation x to represent a vector consisting of the entire collection of x_ij edge variables while the notation z represents a vector consisting of the entire collection of z_ij edge variables. It is noted that x normally has a higher dimensionality than r.

As illustrated in FIG. 2, the cost function E1 may be connected to the variables r1 and r2. Thus, the cost function E1 has variables x11 and x12. The cost function E2 may be connected to variables r1 and r3. Thus, the cost function E2 has variables x21 and x23. The cost function E3 may be connected to variables r2, r3, r4, r5, and r6. Thus, the cost function E3 has variables x32, x33, x34, x35, and x36. The cost function E4 may be connected to variables r5 and r6. Thus, the cost function E4 has variables x45 and x46. In this manner, the variable r1 may have variables z11 and z21; the variable r2 may have variables z12 and z32; the variable r3 may have variables z23 and z33; the variable r4 may have variable z34; the variable r5 may have variables z35 and z45; and the variable r6 may have variables z36 and z46. It is noted that the graphical model 200 is only one exemplary embodiment of a graphical model that may represent the interconnections between the cost functions and the variables as well as the ensuing copied variables. It is also noted that the number of nodes on each side of the graphical model 200 is only exemplary as more or fewer nodes may exist on each side.

Due to the bipartite structure of a Forney factor graph such as the graphical model 200, the cost functions may be split into two groups: the nodes on the left may represent the original soft cost functions and hard constraints and the nodes on the right may represent equality constraints. Each edge variable x_ij sits on a left side of the edge and a copy may be made denoted as z_ij that sits on the right side of the edge. The x_ij and z_ij variables are referred to as “belief” variables because the x_ij variables may intuitively be thought of as “beliefs” that the left-hand side of the factor graph has about the values of the original variables r_j, while the z_ij variables may be thought of as “beliefs” that the right-hand side of the factor graph has about the values of the original variables r_j. The objective function E(x) may then be split into a sum of the left cost functions denoted as ƒ(x) and the right cost functions denoted as g(z), where z is a vector made from all the z_ij belief variables.

FIG. 3A shows a small part of graphical model 300 as applied to a multi-agent trajectory planning problem according to an exemplary embodiment. FIG. 3B shows input and output variables (I/O) 350 for each minimizer block of the graphical model of FIG. 3A according to an exemplary embodiment. As illustrated, the graphical model 300 may be a bipartite graph with one minimizer-node (g1, g2, and g3) on the left for each function making up the overall objective function and one equality-node (E1, E2, and E3) on the right per variable in the problem. It is noted that most embodiments would typically have many more minimizer nodes on the left and equality nodes on the right (e.g. hundreds of nodes on each side); the exemplary embodiment in FIG. 3A only shows three on each side for illustrative purposes. The I/O variables 350 illustrate the input and output variables for each minimizer block g1, g2, and g3. Specifically, based upon the graphical model 300, the minimizer block g1 includes input variables n_1,1,_1,1 and n_1,3,_1,3 with output variables x_1,1,{right arrow over (ρ)}_1,1 and x_1,3,{right arrow over (ρ)}_1,3; the minimizer block g2 includes input variables n_2,1,_2,1, n_2,2,_2,2, and n_2,32,3 with output variables x_2,1,{right arrow over (ρ)}_2,1, x_2,2,{right arrow over (ρ)}_2,2, and x_2,3,{right arrow over (ρ)}_2,3; and the minimizer block g3 includes input variable n_3,3,_3,3 with output variable x_3,3,{right arrow over (ρ)}_3,3.

With the graphical models described above and the blocks representing proximal operators, the exemplary embodiments provide no-collision proximal operators to determine solutions for the optimization problem. Specifically, the proximal operator associated with the function ƒ^coll may ensure there is no collision between two agents of radius r and r′ that move between two consecutive break-points. The variables associated to the two agents and the variables associated to the two break-points may be distinguished. Specifically, a prime (′) may be used to distinguish the variables of the agents while an underscore (_) and an opposite underscore ( ) may be used to distinguish the variables of the two break-points, respectively. Accordingly, from Equation 2, the operator may output the following minimizer:

$\begin{array}{cc}\begin{array}{cc}\underset{\underset{\_}{x},{\underset{\_}{x}}^{\prime },x,x^{\prime }}{m\underset{\_}{i}n\underset{\_}{}}& \begin{array}{c}\frac{\underset{\_}{\rho }}{2}{\underset{\_}{x}-\underset{\_}{n}}^2+\frac{\stackrel{\_}{\rho }}{2}{\stackrel{\_}{x}-\stackrel{\_}{n}}^2+\\ \frac{{\underset{\_}{\rho }}^{\prime }}{2}{{\underset{\_}{x}}^{\prime }-{\underset{\_}{n}}^{\prime }}^2+\frac{\stackrel{\_}{{\rho }^{\prime }}}{2}{\stackrel{\_}{x^{\prime }}-\stackrel{\_}{n^{\prime }}}^2\end{array}\\ s.t.& \begin{array}{c}\alpha \left(\underset{\_}{x}-{\underset{\_}{x}}^{\prime }\right)+\left(1-\alpha \right)\left(\stackrel{\_}{x}-\stackrel{\_}{x^{\prime }}\right)\ge r+r^{\prime },\\ \mathrm{for}\mathrm{all}\alpha \in \left[0,1\right]\end{array}\end{array}& \left(\mathrm{Equation}3\right)\end{array}$

The exemplary embodiments provide an efficient procedure to solve the above Equation 3 SIP problem for agents in arbitrary dimensions by reducing it to a max-min problem. As will be described and explained in further detail below, Equation 3 is essentially equivalent to a “most costly” of the problems of single-constraint problems parameterized by a such as in the following family:

$\begin{array}{cc}\begin{array}{cc}\underset{\underset{\_}{x},{\underset{\_}{x}}^{\prime },x,x^{\prime }}{m\underset{\_}{i}n\underset{\_}{}}& \begin{array}{c}\frac{\underset{\_}{\rho }}{2}{\underset{\_}{x}-\underset{\_}{n}}^2+\frac{\stackrel{\_}{\rho }}{2}{\stackrel{\_}{x}-\stackrel{\_}{n}}^2+\\ \frac{{\underset{\_}{\rho }}^{\prime }}{2}{{\underset{\_}{x}}^{\prime }-{\underset{\_}{n}}^{\prime }}^2+\frac{\stackrel{\_}{{\rho }^{\prime }}}{2}{\stackrel{\_}{x^{\prime }}-\stackrel{\_}{n^{\prime }}}^2\end{array}\\ s.t.& \alpha \left(\underset{\_}{x}-{\underset{\_}{x}}^{\prime }\right)+\left(1-\alpha \right)\left(\stackrel{\_}{x}-\stackrel{\_}{x^{\prime }}\right)\ge r+r^{\prime }\end{array}& \left(\mathrm{Equation}4\right)\end{array}$ It is noted that since Equation 4 has a simple closed-form solution, Question 3 may be solved in an efficient manner for two-dimensional objects, as will be described below. It is also noted that the proximal operator according to the exemplary embodiments may further be utilized for planning in three-dimensions as well as four-dimensions.

With regard to Equation 3 being an equivalent to the “most costly” of the problems, a theorem as described below may be used. Specifically, a theorem may be used where if ∥α(n−n′)+(1−α)(n−n′)∥≠0, then Equation 4 has a unique minimizer, x*(α). Furthermore, if this condition holds for α=α* ∈arg max_{α′∈[0,1]}h(α′) where 2h²(α) is the minimum value of Equation 4, then x*(α*) is also a minimizer of Equation 3. In addition, if ∥α(n−n′)+(1−α)(n−n′)∥≠0, then the following:

$\begin{array}{cc}h\left(\alpha \right)=\max \left\{0,\frac{\left(r+r^{\prime }\right)-\alpha \Delta \underset{\_}{n}+\left(1-\alpha \right)\Delta \stackrel{\_}{n}}{\sqrt{{\alpha }^2/\underset{~}{\rho }+{\left(1-\alpha \right)}^2/}\tilde{\rho }}\right\}& \left(\mathrm{Equation}5\right)\end{array}$ and the unique minimizer of Equation 4 is:

$\begin{array}{cc}{\underset{\_}{x}}^{*}=\underset{\_}{n}-\gamma \underset{\_}{\rho }\left({\alpha }^2\Delta \underset{\_}{n}+\alpha \left(1-\alpha \right)\Delta \stackrel{\_}{n}\right)& \left(\mathrm{Equation}6\right)\\ {{\underset{\_}{x}}^{\prime }}^{*}={\underset{\_}{n}}^{\prime }-\gamma {\underset{\_}{\rho }}^{\prime }\left({\alpha }^2\Delta \underset{\_}{n}+\alpha \left(1-\alpha \right)\Delta \stackrel{\_}{n}\right)& \left(\mathrm{Equation}7\right)\\ {\stackrel{\_}{x}}^{*}=\stackrel{\_}{n}-\gamma \stackrel{\_}{\rho }\left(\left(1-\alpha \right)\mathrm{\alpha \Delta }\underset{\_}{n}+{\left(1-\alpha \right)}^2\Delta \stackrel{\_}{n}\right)& \left(\mathrm{Equation}8\right)\\ {\stackrel{\_}{x^{\prime }}}^{*}=\stackrel{\_}{n^{\prime }}-\gamma \stackrel{\_}{{\rho }^{\prime }}\left(\left(1-\alpha \right)\mathrm{\alpha \Delta }\underset{\_}{n}+{\left(1-\alpha \right)}^2\Delta \stackrel{\_}{n}\right)\mathrm{where}\gamma =\frac{2\lambda }{1+2\lambda \sqrt{{\alpha }^2/\underset{~}{\rho }+{\left(1-\alpha \right)}^2/\tilde{\rho }}},\lambda =-\frac{h\left(\alpha \right)}{2\left(r+r^{\prime }\right)\sqrt{{\alpha }^2/\underset{~}{\rho }+{\left(1-\alpha \right)}^2/\tilde{\rho }}},\Delta \underset{\_}{n}=\underset{\_}{n}-{\underset{\_}{n}}^{\prime },\Delta \stackrel{\_}{n}=\stackrel{\_}{n}-\stackrel{\_}{n^{\prime }},\underset{~}{\rho }={\left({\underset{\_}{\rho }}^{-1}+{\underset{\_}{{\rho }^{\prime }}}^{-1}\right)}^{-1},\mathrm{and}\tilde{\rho }={\left({\stackrel{\_}{\rho }}^{-1}+{\stackrel{\_}{{\rho }^{\prime }}}^{-1}\right)}^{-1}.& \left(\mathrm{Equation}9\right)\end{array}$

It is noted that under a few conditions, a Remark may be provided that shows the above theorem may be used to determine a solution to the Equation 3 by solving a simpler Equation 4 for a special value of α. However, in numerical implementations, the conditions of the above theorem are easy to satisfy and the x*(α*) obtained is the unique minimizer of Equation 3. This principle is described in further detail below.

In other words, to find one solution to Equation 3, α* may be found by maximizing Equation 5 and then minimizing Equation 4 using Equations 6-9 with α=α*. Both steps may be performed efficiently as will be described below. The intuition behind the above theorem is that if the optimization problem as expressed in Equation 3 is solved for the “worst” constraint (the α* that produces the largest minimum value), then the solution also satisfies all other constraints. That is, it also holds for all other α∈[0,1]. This may be made precise using the following Lemma that is used to prove the above theorem. It is noted that ∂_i is used to denote the derivative of a function with respect to the i^th variable. The proof of the Lemma is described below.

The following provides various reasons that illustrate why the above Remark holds true. In a first point, if the various ρ are all positive, the objective function of Equation 3 is strictly convex and the constraint ∥x,x′,x,x′∥≤M may be added for M large enough, without changing its minimum value. The new extended set of constraints amounts to the intersection of closed sets with a compact set and by the continuity of the objective function and the extreme value theorem it follows that the problem has a minimizer.

In a second point, the problem expressed in Equation 3 may be interpreted as resolving the following conflict. Specifically, the conflict may be that a first agent wants to move from n at a time 0 to n at a time 1 and a second agent wants to move from n′ at a time 0 to n′ at a time 1. However, if both move in a straight-line, the first and second agents collide into one another. Accordingly, the resolution of the conflict entails how to minimally perturb the initial and final reference positions of the first and second agents so that the collision is avoided. If the vectors (n,0), (n′,0), ({right arrow over (n)},1), (n′,1) lie in a common three-dimensional plane, there may be ambiguity on how to minimally perturb the initial and final positions of the agents. That is, the reference positions of the first agent may either move up and the second agent down or the reference positions of the first agent may move down and the second agent up (where up and down are relative to the plane defined by the vectors (n,0), (n′,0), (n,1), (n′,1)). However, in numerical implementations, those skilled in the art will understand that the vectors (n,0), (n′,0) (n,1), (n′,1) rarely lie in the same plane and, in fact, this may be avoided by adding a very small amount of random noise to the plurality of n prior to solving Equation 3.

In a third point, it may be shown from the continuity of the objective function of Equation 3, from the continuous differentiability of g(x,α)=∥α(x−x′)+(1−α)(x−x′)∥²−(r+r′)², and from the fact that the level sets of g(x,α) as a function of x never have ‘flat’ sections, that h(α) is a continuous function. Since [0,1] is compact, it follows that there always exists an α*. Also, for the purpose of a numerical implementation, the expression ∥α*(n−n′)+(1−α*)(n−n′)∥≠0 may be considered to hold. In fact, this may be assured by adding a very small amount of random noise to the various n before solving Equation 4. Therefore, for practical purposes, each α* may be considered to have a unique minimizer to exist to Equation 4. Finally, a careful inspection of Equation 5 shows that if there exists α such that h(α)>0, then α* is unique and if h(α)=0 for all α, then Equations 6-9 always give x=n. Accordingly, for the purpose of a numerical implementation, a unique x*(α*) is always found and because, in practice, as argued in the two points above, Equation 3 may be considered to have only one solution, it follows that this unique x*(α*) is the unique minimizer of Equation 3.

The above referenced Lemma utilizes A as a convex set in , the function g:^d×A→,(x,α)g(x,α) which is convex in α and continuously differentiable in (x, α) and the function ƒ:^d→,xƒ(x) which is continuously differentiable and has a unique minimizer. For every α∈A, h(α) may be denoted the minimum value of min_{x:g(x,α)≥0}ƒ(x) and if the minimum is attained by a feasible point, this may be denoted by x*(α). Under these conditions, if α*∈arg max_α∈Ah(α), and if x*(α) exists around a neighborhood of α* in A and is differentiable at α* and if ∂_1g(x*(α*),α*)≠0, then x*(α*) minimizes to min_{x:g(x,α)≥0∀α∈A} ƒ(x).

In showing the proof for the above Lemma, two separate cases may be considered. In a first case, it may be assumed that x*(α*) is such that the expression g(x*(α*),α*)>0 is true. This implies that x*(α*) is a minimizer of min_x ƒ(x) which implies that min_{x:g(x,α*)≥0} ƒ(x)=min_x ƒ which implies that min_{x:g(x,α)≥0}ƒ(x)≥min_x ƒ(x)=min_{x:g(x,α*)≥0}ƒ(x)=max_α′∈Amin_{x:g(x,α*)≥0}ƒ(x)≥min_{x:g(x,α)≥0}ƒ(x). That is, the expression min_{x:g(x,α)≥0} ƒ(x)=ƒ(x*(α*)) is true for all α∈A. Since ƒ(x) has a unique minimizer, x*(α*) must be feasible for the problem min_{x:g(x,α)≥0} ƒ(x) which implies that g(x*(α*),α)≥0 for all α∈A. That is, x*(α*) is a feasible point of min_{x:g(x,α)≥0∀α∈A} ƒ(x) and attains the smallest possible objective value, thereby minimizing it.

In a second case, it may be assumed that g(x*(α*),α*)=0. To finish the proof, it suffices to show that ν∂₂g(x*(α*)α*)≥0 for all ν∈ such that α*+ν∈A. To illustrate this, it is first noted that, if this is the case, then, for all α∈A, convexity implies that g(x*(α*),α)≥g(x*(α*),α*)+(α−α*)∂₂g(x*(α*),α*)=(α−α*)∂₂g(x*(α*),α*)≥0 which implies that x*(α*) is a feasible point for the problem min_{x:g(x,α)≥0,∀α∈A} ƒ(x). Secondly, it is noted that the following expression holds ƒ(x*(α*))≥min_{x:g(x,α)≥0,∀α∈A} ƒ(x)≥max_α∈A min_{x:g(x,α)≥0} ƒ(x)=ƒ(x*(α*)). This implies that x*(α*) minimizes min_{x:g(x,α)≥0,∀α∈A} ƒ(x).

Given the above, ν∂₂g(x*(α*),α*)≥0 may be shown for all ν∈ such that α*+ν∈A. First, it is noted that since ƒ is differentiable and x*(α) exists and is differentiable at x*(α*), then, by the chain rule, h(α) is differentiable at α*. In addition, it is noted that if α* maximizes h, then if ν∈ is such that α*+ν∈A, the directional derivative of h in the direction of ν evaluated at α* must be non-positive. That is, the expression ν∂₁h(α*)=ν∂₁ƒ(x*(α*))^†∂₁x*(α*)≤0 holds true. Second, it is noted that in a small neighborhood around α*, the term x*(α) exists and is continuous (because x*(α*) is differentiable) which, by the continuous-differentiability of g and the fact that ∂₁g(x*(α*),α*≠0), implies that ∂₁g(x*(α),α)≠0 in this neighborhood. Therefore, in a small neighborhood around α*, the problem expressed as min_{x:g(x,α)≥0} ƒ(x) has a single inequality constraint and ∂₁g(x*(α),α)≠0 which implies that x*(α), which is assumed exists in this neighborhood, is a feasible regular point and satisfies the first-order necessary optimality conditions below:∂₁ƒ(x*(α))+λ∂₁g(x*(α),α)=0  (Equation 10)λg(x*(α),α)=0  (Equation 11)g(x*(α),α)≥0  (Equation 12)λ≤0  (Equation 13)

Subsequently, the directional derivative of Equation 12 may be taken with respect to α in the direction ν evaluated at (x*(α*),α*) to obtain:ν∂₁g(x*(α*),α*)^†∂₁x*(α*)+ν∂₂g(x*(α*),α*)≥0  (Equation 14)At the same time, by computing the inner product between Equation 10 and ∂₁x* evaluated at α* and multiplying by ν, the expression ν∂₁ƒ(x*(α*))^†∂₁x*(α*)+λν∂₁g(x*(α*),α*)^†∂₁x*(α*)=0 may be obtained. However, since the expression ν∂₁ƒ(x*(α*))^†∂₁x*(α*)≤0 has already been proven, the expression λν∂₁g(x*(α*),α*)^†∂₁x*(α*)≥0 holds true. As noted above, it is again noted that it is being assumed that the expression g(x*(α*),α*)=0 is true. Therefore, the expression λ<0 is true. It thereby follows that the expression ν∂₁g(x*(α*),α*)^†∂₁x*(α*)≤0 and Equation 14 provides the conclusion that the expression ν∂₂g(x*(α*),α*)≥0 is true.

The theorem following Equations 3 and 4 may be proved as follows. The solutions to Equations 3 and 4 remain the same if the portion ∥α(x−x′)+(1−α)(x−x′)∥≥r+r′ is replaced by ∥α(x−x′)+(1−α)(x−x′)∥²−(r−r′)²≥0. The proof for the above theorem may be shown with this replacement. Initially, the proof for the above theorem may assume that the Equations 5-9 hold when ∥α(n−n′)+(1−α)(n−n′)≠0∥. This aspect will be shown after the proof of the theorem under this assumption.

The proof for the above theorem may be shown by considering two separate cases. In a first case, the term r+r′≤min_α∈[0,1]∥α(n−n′)+(1−α)(n−n′)∥ may be considered. This implies that the minimum of Equation 3 is 0 and, because the multiple ρ are positive, there is a unique minimizer which equals (n,n′,n,n′). In this case, h(α)=0 is also true for all α which implies that the solution of Equation 4 is x*(α)=n for all α. Therefore, for any optimal α* for which ∥α*(n−n′)+(1−α*)(n−n′)∥≠0, the minimizer of Equation 4 is equal to the unique solution of Equation 3, thereby illustrating the proof for the above theorem.

Next, it may be assumed that r+r′>min_α∈[0,1]∥α(n−n′)+(1−α)(n−n′)∥. This implies that there exists an α for which the right hand side of Equation 5 is positive and for which ∥α(n−n′)+(1−α)(n−n′)∥≠0. This further implies that there exists an α for which h(α)>0. Therefore, for any optimal α*, the expression h(α*)>0 must be true. If ∥α*(n−n′)+(1−α*)(n−n)∥≠0, then Equation 5 holds around a neighborhood of α* and in this neighborhood, the expression h(α)>0 is true. Therefore, in this neighborhood, x*(α) exists, is unique, and is differentiable. It is noted that the objective function of Equation 3 is continuously differentiable and that the expression g(x,α)≡∥α(x−x′)+(1−α)(x−x′)∥²−(r+r′)² is continuously differentiable in (x,α) and convex in α. If it is true that ∂₁g(x*(α*),α*)≠0, then the above described Lemma may be applied with A=[0,1] and it can be concluded that x*(α*) is also a solution of Equation 3. Thus, the proof for the above theorem is shown for this case as well.

It may further be shown that in this case, the expression ∂₁g(x*(α*),α*)≠0 is true. First, it is noted that the expression ∂₁g=(∂_xg,∂_xg,∂_x′g,∂_x′g)=2∥α*(x*−x*′)+(1−α*)(x*−x*′)∥(α*,1−α*,−α*,−1+α*) is true. As discussed above, it is again noted that if ∥α*(n−n′)+(1−α*)(n−n′)∥≠0, then h(α*)>0. Therefore, it may be concluded that the expression r+r′=∥α*(x*−x*′+(1−α*)(x*−x*′))∥ is true because otherwise the expression ∥α*(x*−x*′)+(1−α*)(x*−x*′)∥<r+r′ implies that the constraint of the problem expressed in Equation 4 for α=α* is inactive, thereby implying that the solution must be x=n with objective value 0 which contradicts the fact that h(α*)>0. Hence, the expression ∂₁g=(r+r′)(α*,1−α*,−α*,−1+α*) is always non-zero is true and that proves that the above theorem is true in this case as well.

As discussed above, Equations 5-9 are shown to hold when ∥α(n−n′)+(1−α)(n−n′)≠0∥. The following provides the proof for this statement. Initially, the problem expressed in Equation 4 is not solved but an equivalent (more smooth) problem is solved. As used above in the proof of the theorem, an equivalent includes a replacement of a term and moving this term to an opposing side of the inequality.

$\begin{array}{cc}\begin{array}{cc}\underset{\underset{\_}{x},{\underset{\_}{x}}^{\prime },x,x^{\prime }}{m\underset{\_}{i}n\underset{\_}{}}& \begin{array}{c}\frac{\underset{\_}{\rho }}{2}{\underset{\_}{x}-\underset{\_}{n}}^2+\frac{\stackrel{\_}{\rho }}{2}{\stackrel{\_}{x}-\stackrel{\_}{n}}^2+\\ \frac{{\underset{\_}{\rho }}^{\prime }}{2}{{\underset{\_}{x}}^{\prime }-{\underset{\_}{n}}^{\prime }}^2+\frac{\stackrel{\_}{{\rho }^{\prime }}}{2}{\stackrel{\_}{x^{\prime }}-\stackrel{\_}{n^{\prime }}}^2\end{array}\\ s.t.& \alpha \left(\underset{\_}{x}-{\underset{\_}{x}}^{\prime }\right)+\left(1-\alpha \right)\left(\stackrel{\_}{x}-\stackrel{\_}{x^{\prime }}\right)-{\left(r+r^{\prime }\right)}^2\ge 0\end{array}& \left(\mathrm{Equation}15\right)\end{array}$

Initially, it is noted that if the expression ∥α(n−n′)+(1−α)(n−n′)∥>r+r′ is true, then the constraint is inactive and the minimizer of Equation 15 is (n,n′,n,n′) with a minimum value of 0. If the expression ∥α(n−n′)+(1−α)(n−n′)>r+r′ is true, the expression h(α) is also true and the minimizer obtained from Equations 6-9 is also (n,n′,n,n′). Therefore, the proof only requires a showing that Equations 5-9 holds in the case when the constraint is active which corresponds to the case when ∥α(n−n′)+(1−α)(n−n′)∥≤r+r′.

To simplify the expressions, several block variables are introduced and expressed in the above problem of Equation 15 to provide a shorter form. Specifically, a first block variable is defined as x=(x,x′,x,x′)∈^4×d, a second block variable is defined as n=(n,n′,n,n′)∈^4×d, a third block variable is defined as α=(α,−α,(1−α),−(1−α))∈⁴, and a fourth block variable is defined as D=diag(ρ,ρ′,ρ,ρ′)∈^4×4. Accordingly, Equation 15 may be rewritten as the following:

$\begin{array}{cc}\begin{array}{cc}\underset{x}{\min }& \frac{1}{2}\mathrm{tr}\left\{{\left(x-n\right)}^{†}D\left(x-n\right)\right\}\\ s.t.& {{\alpha }^{†}x}^2-{\left(r+r^{\prime }\right)}^2\ge 0\end{array}& \left(\mathrm{Equation}16\right)\end{array}$ It is noted that it is necessary that the solutions to the above problem expressed in Equation 16 are among the points that satisfy Karush-Kuhn-Tucker (KKT) conditions. Specifically, these points satisfy:D(x−n)+2αν=0  (Equation 17)ν=λ(α^†x)  (Equation 18)∥ν∥/|λ=r+r′  (Equation 19)where λ≠0 is the Lagrange multiplier associated to the problem's constraint and is non-zero because it is assumed the constraint is active. As will be described in further detail below, the rest of the proof shows that there are only two points that satisfy the KKT conditions and show that, between them, the one that corresponds to the global optimum satisfies Equations 5-9.

The Equations may be written even more compactly with the following:

$\begin{array}{cc}\left(\begin{array}{cc}\frac{1}{2}D& \alpha \\ {\alpha }^{†}& -1/\lambda \end{array}\right)\left(\begin{array}{c}x\\ v\end{array}\right)=\left(\begin{array}{c}\frac{1}{2}\mathrm{Dn}\\ 0\end{array}\right)& \left(\mathrm{Equation}20\right)\end{array}$ It may be claimed that, if 1+2λα^†D⁻¹α≠0, the inverse of the block matrix

$\begin{array}{cc}\left(\begin{array}{cc}\frac{1}{2}D& \alpha \\ {\alpha }^{†}& -1/\lambda \end{array}\right)\mathrm{is}& \left(\mathrm{Equation}21\right)\\ \left(\begin{array}{cc}2(D^{-1}-\frac{2D^{-1}{\mathrm{\lambda \alpha \alpha }}^{†}{D}^{-1}}{1+2{\mathrm{\lambda \alpha }}^{†}{D}^{-1}\alpha }& \frac{2\lambda D^{-1}\alpha }{1+2{\mathrm{\lambda \alpha }}^{†}{D}^{-1}\alpha }\\ \frac{2{\mathrm{\lambda \alpha }}^{†}{D}^{-1}}{1+2{\mathrm{\lambda \alpha }}^{†}{D}^{-1}\alpha }& \frac{-\lambda }{1+2{\mathrm{\lambda \alpha }}^{†}{D}^{-1}\alpha }\end{array}\right)& \left(\mathrm{Equation}22\right)\end{array}$

Although the formula for the inverse of a block matrix may also be used, a simpler manner of the proof is computing the product of Equations 22 and 21 and show it equals the identity. It is evident that the block diagonal entries of the resulting product are indeed identity matrices. Since both matrices are symmetric, the remaining calculation is to verify that one of the non-diagonal block entries is zero. Indeed, such a determination may be illustrated with the following:

$\left({\alpha }^{†}\right)\left(2\left(D^{-1}-\frac{2D^{-1}{\mathrm{\lambda \alpha \alpha }}^{†}{D}^{-1}}{1+2{\mathrm{\lambda \alpha }}^{†}{D}^{-1}\alpha }\right)\right)+\left(-1/\lambda \right)\left(\frac{2{\mathrm{\lambda \alpha }}^{†}{D}^{-1}}{1+2{\mathrm{\lambda \alpha }}^{†}{D}^{-1}\alpha }\right)=\frac{2{\alpha }^{†}{D}^{-1}\left(1+2{\mathrm{\lambda \alpha }}^{†}{D}^{-1}\alpha \right)-4{\alpha }^{†}{D}^{-1}{\mathrm{\lambda \alpha \alpha }}^{†}{D}^{-1}-2{\alpha }^{†}{D}^{-1}}{1+2{\mathrm{\lambda \alpha }}^{†}{D}^{-1}\alpha }=\frac{4{\mathrm{\lambda \alpha }}^{†}{D}^{-1}{\alpha }^{†}{D}^{-1}\alpha -4{\alpha }^{†}{D}^{-1}{\mathrm{\lambda \alpha \alpha }}^{†}{D}^{-1}}{1+2{\mathrm{\lambda \alpha }}^{†}{D}^{-1}}=\frac{\left({\alpha }^{†}{D}^{-1}\alpha \right)\left(4{\mathrm{\lambda \alpha }}^{†}{D}^{-1}-4{\mathrm{\lambda \alpha }}^{†}{D}^{-1}\right)}{1+2{\mathrm{\lambda \alpha }}^{†}{D}^{-1}\alpha }=0$

The linear system of Equation 20 may be solved by multiplying both sides by the inverse matrix Equation 22 to conclude the following:

$\begin{array}{cc}v=\frac{2{\mathrm{\lambda \alpha }}^{†}{D}^{-1}}{1+2{\mathrm{\lambda \alpha }}^{†}{D}^{-1}\alpha }\left(\frac{1}{2}\mathrm{Dn}\right)=\frac{{\mathrm{\lambda \alpha }}^{†}n}{1+2{\mathrm{\lambda \alpha }}^{†}{D}^{-1}\alpha }& \left(\mathrm{Equation}23\right)\\ x=2\left(D^{-1}-\frac{2D^{-1}{\mathrm{\lambda \alpha \alpha }}^{†}{D}^{-1}}{1+2{\mathrm{\lambda \alpha }}^{†}{D}^{-1}\alpha }\right)\left(\frac{1}{2}\mathrm{Dn}\right)=n-\frac{2D^{-1}{\mathrm{\lambda \alpha \alpha }}^{†}n}{1+2{\mathrm{\lambda \alpha }}^{†}{D}^{-1}\alpha }& \left(\mathrm{Equation}24\right)\end{array}$ Using Equation 24, the objective value of Equation 16 may be expressed with the following:

$\begin{array}{cc}\frac{1}{2}\mathrm{tr}\left\{{\left(x-n\right)}^{†}D\left(x-n\right)\right\}=\frac{\mathrm{tr}\left\{n^{†}{\mathrm{\alpha \alpha }}^{†}\lambda D^{-1}\left(-2\right)D\left(-2\right)D^{-1}{\mathrm{\lambda \alpha \alpha }}^{†}n\right\}}{2{\left(1+2{\mathrm{\lambda \alpha }}^{†}{D}^{-1}\alpha \right)}^2}& \left(\mathrm{Equation}25\right)\\ =\frac{\mathrm{tr}\left\{n^{†}{\mathrm{\alpha \alpha }}^{†}n\right\}4{\lambda }^2\left({\alpha }^{†}{D}^{-1}\alpha \right)}{2{\left(1+2{\mathrm{\lambda \alpha }}^{†}{D}^{-1}\alpha \right)}^2}=\frac{2{\lambda }^2{a^{†}n}^2\left({\alpha }^{†}{D}^{-1}\alpha \right)}{{\left(1+2{\mathrm{\lambda \alpha }}^{†}{D}^{-1}\alpha \right)}^2}=2{v}^2\left({\alpha }^{†}{D}^{-1}\alpha \right)& \left(\mathrm{Equation}26\right)\end{array}$ where in the last equality, Equation 23 is made of use. Referring back to the third equation in the KKT conditions, the expression ∥ν∥/λ=r+r′ is used and the following may be concluded:

$\begin{array}{cc}\lambda =\frac{1}{2\left({\alpha }^{†}{D}^{-1}\alpha \right)}\left(-1\pm \frac{{\alpha }^{†}n}{r+r^{\prime }}\right)& \left(\mathrm{Equation}27\right)\end{array}$ Therefore, the following may further be concluded:

$\begin{array}{cc}\frac{1}{2}\mathrm{tr}\left\{{\left(x-n\right)}^{†}D\left(x-n\right)\right\}=2{\left(r+r^{\prime }\right)}^2\frac{1}{4{\left({\alpha }^{†}{D}^{-1}\alpha \right)}^2}{\left(-1\pm \frac{a^{†}n}{r+r^{\prime }}\right)}^2\left({\alpha }^{†}{D}^{-1}\alpha \right)& \left(\mathrm{Equation}28\right)\\ =\frac{{\left(r+r^{\prime }\pm {\alpha }^{†}n\right)}^2}{2\left({\alpha }^{†}{D}^{-1}\alpha \right)}& \left(\mathrm{Equation}29\right)\end{array}$

Because the global minimum is sought and since there is an assumption that ∥α^†n∥=∥α(n−{right arrow over (n)}′)+(1−α)(n−n′)∥≤(r+r′) and the constraint is active, the following may be concluded:

$\begin{array}{cc}\frac{1}{2}\mathrm{tr}\left\{{\left(x^{*}\left(\alpha \right)-n\right)}^{†}D\left(x^{*}\left(\alpha \right)-n\right)\right\}=\frac{{\left(r+r^{\prime }-{\alpha }^{†}\right)}^2}{2\left({\alpha }^{†}{D}^{-1}\alpha \right)}=\frac{1}{2}{\left(h\left(\alpha \right)\right)}^2& \left(\mathrm{Equation}30\right)\end{array}$ This calculation uses the fact that the expressions α^†n=αΔn+(1−αΔn) and α^†D⁻¹α=α²/ρ+(1−α²)/{circumflex over (ρ)} hold true. Thus, Equation 5 is proven to be valid when ∥α^†n∥=∥α(n−n′)+(1−α)(n−n′)∥≤(r+r′) as long as 1+2λα^†D⁻¹α=∥α^†n∥/(r+r′)≠0. It is noted that Equation 5 has already been proven to hold when ∥α^†n∥=∥α(n−n′)+(1−α)(n−n′)∥>(r+r′).

To finish the proof, the validity of Equations 6-9 are proven when the expression ∥α^†n∥=∥α(n−n′)+(1−α)(n−n′)∥>(r+r′) is true. It is again noted that the validity of Equations 6-9 has already been shown when the expression ∥α^†n∥=∥α(n−n′)+(1−α)(n−n′)∥≤(r+r′) is true. In this instance, when the expression ∥α^†n∥=∥α(n−n′)+(1−α)(n−n′)∥>(r+r′) is true, the following may be written:

$\begin{array}{cc}\lambda =\frac{1}{2\left({\alpha }^{†}{D}^{-1}\alpha \right)}\left(-1+\frac{{\alpha }^{†}n}{r+r^{\prime }}\right)=-\frac{h\left(\alpha \right)}{2\left(r+r;\right)\sqrt{{\alpha }^{†}{D}^{-1}\alpha }}& \left(\mathrm{Equation}31\right)\end{array}$ If a variable

$\gamma =\frac{2\lambda }{1+2{\mathrm{\lambda \alpha }}^{†}{D}^{-1}\alpha }$ is defined in this manner, the following expression for x*(α) may be written as follows:x*(α)=n−γD⁻¹αα^†n  (Equation 32)which holds if the expression ∥α^†n∥=∥α(n−n′)+(1−α)(n−n′)∥≠0 is true. One skilled in the art can thereby conclude that this is exactly the same expression as Equations 6-9 to finish the proof.

With the above expressions and the proofs therefore being illustrated, the mechanism used to solve the trajectory optimization problem with the variation of the ADMM and the TWA may be further defined with other collision operators, speeding up computations, and local path planning. Furthermore, as will be described in further detail below, a landmark proximal operator may also be introduced for the mechanism such that the trajectory optimization problem applied to a three-dimensional path of the agents may be determined by the device 100.

With regard to the other collision operators (i.e., non-agent collisions), the wall-agent collision operator may be efficiently extended to path planning of trajectories of agents with obstacles in three-dimensions. To avoid a collision between a first agent having radius r and a line between points y₁,y₂∈^d, the following constraint may be added in the overall optimization problem: ∥αx₁(s)+(1−α)x₁(s+1)−(βy₁+(1−β)γ₂)∥≥r for all α,β∈[0,1] and all s+1∈[η+1]. This constraint is associated with the proximal operator that receives (n,n′) and finds (x,x) that minimizes ρ/2∥x−n∥²+ρ/2∥x−n∥² subject to ∥αx+(1−α)x−βy₁+(βy₁+(1−β)y₂)∥≥r for all α,β∈[0,1]. Using ideas very similar to those behind the above theorem and the Lemma, the problem may be solved for dimensions strictly greater than two by maximizing over α,β∈[0,1] the minimum value the single-constraint version of the problem. In fact, those skilled in the art will recognize that it is easy to generalize the Lemma to A∪^k and the single-constraint version of this optimization problem may be obtained from Equation 4 by replacing n′ and n′ with βy₁+(1−β)y₂ and letting ρ′,ρ′→∞. Thus, Equations 5-9 may be used under this replacement and limit to generalize the line-agent collision proximal operator to dimensions greater than two. The same operator may also be used to avoid collisions between agents and a line of thickness ν by replacing r with ν+r.

It is noted that a proximal operator may not be implemented to avoid collisions between an agent and a convex envelop of an arbitrary set of points y₁,y₂, . . . , y_q by maximizing over α,β₁, . . . , β_1-q∈[0,1] the minimum of the single-constraint problem obtained from Equation 4 after replacing n′ and n′ with β₁y₁+ . . . +β_q-2y_q-2+(1−β₁− . . . −β_q-1)y_q and letting ρ,ρ′→∞. This may only be done when d>q, otherwise the condition ∂₁g(x*(α*)α*)≠0 of the Lemma does not hold and x*(α*) is not feasible for the original SIP problem. Specifically, the max-min approach discussed above cannot directly apply to re-derive the line-agent collision operator for agents in two dimensions but only for dimensions of three or greater. When d≤q, a similar but more complicated principle may be applied to solve the original SIP problem. This may involve considering different portions of the space A separately, computing extermal points instead of maximizing and minimizing and choosing the best feasible solution among these. The exemplary embodiments may be configured to further incorporate such a feature.

With regard to the speeding up of the computations, the computational bottleneck for the collision operators is maximizing Equation 5. There may be two scenarios when avoiding this expensive step to improve performance. A first scenario may be denoted as trivial whereas a second scenario may be denoted as easy.

First, it may be noted that one skilled in the art may readily check whether x=n is a trivial feasible solution. If it is a trivial feasible solution, it must be optimal because it has 0 cost and the operator may return it as the optimal solution. This is the case if the segment from Δn=n−n′ to Δn=n−n′ does not intersect the sphere of radius r+r′ centered at zero which is equivalent to ∥αΔn+(1−α)Δn∥≥r+r′ with α=max{1, min{0,α′}} and α′=Δn^†(Δn−Δn)/∥Δn−Δn∥².

Second, the easy scenario may be a shortcut to directly determine if the maximizer of max_α∈[0,1]h(α) is either 0 or 1. Initially, h may be noted to have at most one extreme point in [0,1] (e.g., this may be illustrated by plotting h(α) for different values of Δn and Δn). This being the case, if ∂₁h(0)>0 and ∂₁h(1)>0, then α*=1 and if ∂₁h(0)<0 and ∂₁h(0)<0, then α*=0. Those skilled in the art will understand that evaluating two derivatives of h is much easier than maximizing h and may save computation time such as ∂₁h(0)=C(−(r+r′)+∥Δn∥+(Δn^†−Δn)/∥Δn∥)) and ∂₁h(1)=C′((r+r′)−∥Δn∥+(Δn^†(Δn−Δn)/∥Δn∥)) for positive constants C and C′.

It is further noted that if these cases do not hold, the maximizing of Equation 5 may not be avoided which may entail a scenario denoted as expensive.

With regard to local path planning, the optimization problem expressed in Equation 1 finds beginning-to-end collision free paths for all agents simultaneously, known as global path planning. It is also possible to solve path planning greedily by solving a sequence of small optimization problems, known as local path planning. Each of these problems plans the path of all agents for the next τ seconds such that, as a group, they get as close as possible to their final desired positions. These small optimization problems may be solved using a special case of the no-collision operator. Accordingly, the exemplary embodiments extend the local path planning to arbitrary dimensions (particularly to three dimensions) and improve its speed further. As will be described in further detail below, the improvements in speed will be illustrated.

As discussed above, the concept of landmarks may be introduced that, automatically and jointly, (i) produce reference points in space-time that, as a group, agents should try to visit, (ii) produce a good assignment between these reference points and the agents, and (iii) produce collision-free paths for the agents that are trying to visit points assigned to them. Therefore, the exemplary embodiments introduce the landmark proximal operator. The three points are essential, for example, to formation control in multi-robot systems and autonomous surveillance or search and are also related to the problem of assigning tasks to robots if the tasks are seen as groups of points to visit. Prior works focus on only one of these points or treat them in isolation. One application where these points are considered although separately is the problem of using color-changing robots as pixels in a display. The pixel-robots arrangement is planned frame by frame and does not automatically guarantee that the same image part is represented by the same set of robots across frames, crating visual artifacts. In contrast, the exemplary embodiments utilize a landmark formalism that enables a penalty for these situations.

The exemplary embodiments introduce landmarks as extra terms in the objective function expressed in Equation 1. With the above basis, the proximal operators associated with Equation 1 may be determined. For example, a set of landmark trajectories {y_j(s)}_{j∈[m],sinit≤s≤send} may be considered where, for each trajectory j, a cost ĉ_j>0 may be assigned which is the cost of ignoring the entire landmark trajectory. Furthermore, for each landmark y_j(s)∈^d, a penalty c_j(s)>0 may be assigned for deviating from y_j(s) assuming the system assigns an agent to the trajectory j. Landmark trajectories extend the objective function of Equation 1 by adding the following term:

$\begin{array}{cc}\sum _{j:{\sigma }_j{\ne }^{*}}\sum _{s=s_{\mathrm{init}}}^{s_{\mathrm{end}}}{c}_j\left(s\right){x_{{\sigma }_j}\left(s\right)-y_j\left(s\right)}^2+\sum _{j:{\sigma }_j{=}^{*}}{\tilde{c}}_j& \left(\mathrm{Equation}33\right)\end{array}$ where the variable σ_j indicates which agent should follow trajectory j. If σ_j=*, that means trajectory j is unassigned. It may be assumed that each trajectory may be assigned to at most one agent which it must follow throughout its duration. Therefore, the agent is σ_j∈[p]∪{*} and there exists a condition that if σ_j=σ_j′, then either j=j′ or σ_j=*. The overall objective function may be optimized over x and σ. It is noted that it is not equally important to follow every point in the trajectory. For example, by setting some costs equal to zero, it may effectively deal with trajectories of different lengths, different beginnings and ends, and even trajectories with holes. By setting some costs equal to infinity, it may be imposed that if the trajectory is followed, it must be followed exactly. In Equation 33, the Euclidean metric may be used but other distances may be considered (even non-convex ones) as long as the resulting proximal operators are easy to compute. It is also noted that, a priori, the term {y_j(s)} is not needed to describe collision free trajectories. The other terms in the overall objective function attempts to enforce no-collision constraints and additional dynamic constraints. It is further noted that if an unreasonable set of path specifications are attempted to be satisfied, the proximal algorithm used may not converge.

The proximal operator associated to the term expressed in Equation 33 receives as an input {n_i(s)} and outputs x_i*(s) where i∈[p], s_init≤s≤s_end, and {x_i*(s)} minimizes the following:

$\begin{array}{cc}\underset{x,\sigma }{\min }\sum _{j:{\sigma }_j{\ne }^{*}}\sum _{s=s_{\mathrm{init}}}^{s_{\mathrm{end}}}{c}_j\left(s\right){x_{{\sigma }_j}\left(s\right)-y_j\left(s\right)}^2+\sum _{j:{\sigma }_j{=}^{*}}{\tilde{c}}_j+\sum _{i=1}^p\sum _{s=s_{\mathrm{init}}}^{s_{\mathrm{end}}}\frac{{\rho }_i}{2}{x_i\left(s\right)-n_i\left(s\right)}^2& \left(\mathrm{Equation}34\right)\end{array}$ The variables σ are used only internally in the computation of the proximal operator because they are not shared with other terms in the overall objective function. The above proximal operator may be efficiently computed with the following process. First, Equation 11 may be optimized over the various x as a function of σ and then the resulting expression may be optimized over the various σ. If optimized over the various x, the term Σ_jw_j,σj is obtained where, if σ_j=*, w_l,*=c_j, and if σ_j=i≠*, then the expression

$w_{j,i}={\min }_x\sum _{s=s_{\mathrm{init}}}^{s_{\mathrm{end}}}{c}_j\left(s\right){x_i\left(s\right)-y_j}^2+\frac{{\rho }_i}{2}{x_i\left(s\right)-n_i\left(s\right)}^2=\sum _{s=s_{\mathrm{init}}}^{s_{\mathrm{end}}}\frac{{\rho }_i{c}_j\left(s\right)}{2c_j\left(s\right)+{\rho }_i}{n_i\left(s\right)-y_j\left(s\right)}^2$ holds true. The last equality follows from a solving of a simple quadratic problem. An optimization may be performed over the various σ by solving a linear assignment problem with cost matrix w which may be done, for example, using a simple Hungarian method, or more advanced methods thereof. Once an optimal σ* is determined, the output of the operator may be computed with the following. If i is such that {j:σ_j*=i}=ϕ, then x_i*(s)=n_i(s) for all s_init≤s≤s_end and if i is such that i=σ_j for some j∈[m], then x_i*(s)=(ρ_in_i(s)+2c_i(s)y_j(s))/(2c_i(s)+ρ_i) for all s_init≤s≤s_end.

The term expressed in Equation 33 corresponds to a set of trajectories between break-points s=s_init and s=s_end for which the different agents must complete. That is, each agent may follow at most one trajectory. However, an agent may be allowed to be assigned to and cover multiple landmark trajectories. One immediate manner of performing this is by adding more terms of the form expressed in Equation 33 to the overall objective function such that the k^th term has all its m^(k) in trajectories within the interval └s_init^(k),s_end^(k)┘ and different intervals for different k are disjoint. It is noted that doing this alone does not enable an imposition of a constraint such as the following: “the j^th trajectory in the set corresponding to the interval └s_init^(k),s_end^(k)┘ must be covered by the same agent as the (j′)^th trajectory in the set corresponding to the interval └s_init^(k+1),s_end^(k+1)┘.” If such a constraint were to be imposed, an additional constraint is required to be imposed that some of the σ^(k) variables across different terms of the form expressed in Equation 33 are the same such as (in the previous example) σ_j^(s)=σ_j′^(s+1). Since the variables may now be shared across different terms, the proximal operator expressed in Equation 34 needs to be changed. Subsequently, this term receives as input a set of values {n_i(s)}_s,i and {n_j′}_j and outputs a set of values {x_i*(s)}_i,s and {σ_j*}_j that minimize the following:

$\underset{x,\sigma }{\min }\sum _{j:{\sigma }_j{\ne }^{*}}\sum _{s=s_{\mathrm{init}}}^{s_{\mathrm{end}}}{c}_j\left(s\right){x_{{\sigma }_j}\left(s\right)-y_j\left(s\right)}^2+\sum _{j:{\sigma }_j{=}^{*}}{\tilde{c}}_j+\sum _{i,s}\frac{{\rho }_i}{2}{x_i\left(s\right)-n_i\left(s\right)}^2+\sum _{j=1}^m\frac{{\rho }_j^{\prime }}{2}{{\sigma }_j-n_j^{\prime }}^2$

In the above expression, {σ_j}_j and {n′_j}_j are both vectors of length p+1 where the last component encodes for no assignment and σ_j must be binary with only one entry. For example, if p=5 and σ₂=[,0,1,0,0,0], the second trajectory in the set of trajectories is assigned to the third agent of the p agents. In another example, if σ₁=[0,0,0,0,0,1], the first trajectory in the set of trajectories is not assigned to any agent. The term W may have real values and several non-zero components.

The above problem may also be solved by first optimizing over x and then over σ. By optimizing over x, the term Σ_jŵ_j,σi is obtained where

${\tilde{w}}_{j,i}=w_{j,i}+\frac{{\rho }_j^{\prime }}{2}{\left[0,\dots 0,1,0,\dots ,0\right]-n_j^{\prime }}^2=w_{j,i}+\frac{{\rho }_j^{\prime }}{2}{n_j^{\prime }}^2+{1-n_j^{\prime \left(i\right)}}^2-{n_j^{\prime \left(i\right)}}^2=w_{j,i}+\frac{{\rho }_j^{\prime }}{2}{n_j^{\prime }}^2+1-2n_j^{\prime \left(i\right)}$

• • Given the cost matrix ŵ, the optimal α* may be found by solving a linear assignment problem. Given α*, the optimal x* may be computed using exactly the same expressions as for the problem expressed in Equation 34.

Furthermore, to include constraints of the kind σ_j^(k)=σ_j′^(k′), a term may be added to the objective that takes the value infinity whenever the constraint is violated and zero otherwise. This term is associated with a proximal operator that receives as input n′_j=(n′_j⁽¹⁾, . . . , n′_j⁽ⁿ⁾) and n′_j′=(n′_j⁽¹⁾, . . . , n′_j′⁽ⁿ⁾) and outputs

$\left({\sigma }_j^{*},{\sigma }_j^{*},\right)\in {\mathrm{argmin}}_{{\sigma }_j={{\sigma }_j}^{\prime }}\frac{{\rho }_j}{2}{{\sigma }_j-n_j^{\prime }}^2+\frac{{\rho }_{j^{\prime }}^{\prime }}{2}{{\sigma }_{j^{\prime }}-n_{j^{\prime }}^{\prime }}^2.$ Again σ_j and σ_j′ are binary vectors of length p+1 with exactly one non-zero entry. The solution has the form σ_j*=σ_j′*=[0,0, . . . ,0,1,0 . . . 0] where the 1 is in position i*=arg max_i∈[p]ρ_jn′_j⁽ⁱ⁾+ρ′_j′n′_j′⁽ⁱ⁾.

FIG. 4 shows a method 400 of generating an optimization solution according to an exemplary embodiment. In step 405, the device 100 receives the optimization problem. For example, via the I/O device 120, a user may provide the optimization problem. Specifically, the optimization problem may correspond to a trajectory optimization problem in which agents are allowed to have path planning in two or more dimensions, in particular three-dimensions. Within the optimization problem are a plurality of cost functions and a plurality of variables or constraints. Thus, in step 410 and 415, the device 100 (via the variable engine 125 and the cost engine 130) determine the cost functions and the variables of the optimization problem. It should be noted that the user may simply provide the cost functions and the variables. Thus, the device 100 may receive the cost functions and the variables.

It should be noted that the device 100 may utilize a factor graph such as the graphical models 200, 300. Thus, the method 400 may include steps that generate these factor graphs. For example, the factor graph engine 135 may generate the graphical models via the constraint engine 140 providing data as to the relationships between the cost functions and the variables. Specifically, the relationships may be those shown in the graphical models 200, 300 for the nodes on the left and the nodes on the right.

In step 417, the device 100 (via the landmark engine 132) determines the landmarks that are introduced to the optimization problem. As discussed above, the landmark may provide a point or set of points that the agent should attempt to visit along its path from an initial position to a final position. Thus, when a path is determined for the agent that deviates from visiting the landmark, the associated cost may be affected for this path.

In step 420, the device 100 generates messages to be passed from the cost functions to the variables (e.g., the messages to be passed from the left nodes to the right nodes on the factor graph). In step 425, the device 100 also determine a respective weight to be applied to each message. As discussed above, the exemplary optimization algorithm utilizes a TWA. Accordingly, each message that is generated also includes a corresponding weight (e.g., zero, infinite, and standard). In step 430, the message and corresponding weight are transmitted from the cost functions to the variables. These steps may further incorporate the landmarks described above. Specifically, the cost functions may be affected directly by whether the agent visits the landmark or not.

In step 435, based upon the messages and weights that were received from the cost functions to the variables, the device 100 generates further messages to be passed from the variables to the cost functions. In step 440, the device 100 determines a respective weight to be applied to each of these further messages. In step 445, the further message and corresponding weight are transmitted from the variables to the cost functions.

In step 450, the device 100 determines whether there is a consensus between the messages being transmitted between the cost functions and the variables. If a consensus has not been reached, the method 400 returns to step 420 where steps 420-445 are repeated. This may repeat as many times as required for the consensus to be reached. Once the consensus is reached, the method 400 continues to step 455. In step 455, the optimization solution is generated as a function of the messages transmitted that were determined to form the consensus. That is, the optimization solution relates to a minimum being reached for each of the cost functions while still satisfying all the requirements of the variables.

It should be noted that the method 400 may include further steps. For example, the method 400 may include a criterion in which the consensus is only formed when a predetermined number of consecutive messages that are passed between the cost functions and the variables holds true. Thus, after step 450, the method 400 may include a further determination such that the satisfaction of the predetermined number is determined. In another example, the method 400 may include steps after step 425 and step 440 in which a determination is made whether nodes or input/outputs to the nodes may be ignored or removed from further computation. For example, when a zero weight or infinite weight is determined for a particular message, the graphical model may be modified to incorporate this knowledge to further improve efficiency in determining the optimization solution.

The above method 400 of FIG. 4 may be executed using the proximal operators described above to determine the solution for the optimization problem. Accordingly, numerical experiments may be performed to determine an efficiency with which the exemplary embodiments provide the intended functionality. The numerical experiments are described herein with a first iteration of the variant on the ADMM (utilizing different proximal operators or blocks) using the TWA that applied to only two-dimensional scenarios (hereinafter “Old”) versus a second iteration according to the exemplary embodiments (hereinafter “New”).

The numerical experiments have been such that the first run using the Old iteration is performed on a configuration where 8 agents each having a radius r=0.918 equally spaced around a circle of radius R=3 where each agent is required to exchange position with the corresponding antipodal agent. While running the TWA using the Old iteration, the trace of all n variables input into the Old operators. The execution speed of the Old iteration versus the New iteration have been compared on this trace of inputs, after segmenting the n variables into trivial, easy, or expensive. For global planning, the distribution of trivial, easy, and expensive inputs resulted in {0.814, 0.001, 0.185}. Although the expensive inputs are infrequent, the total wall-clock time that the New iteration takes to process them is 76 msec compared to 54 msec to process all trivial and easy inputs. By comparison, the Old iteration takes a total time of 551 msec on the expensive inputs, thereby the New iteration yielding an average speedup of 7.25× on the inputs that are most expensive to process. Similarly, the trace of the n variables input into the collision operator are collected when using the local planning method on this same scenario. It is observed a distribution of the trivial, easy, and expensive inputs equal to {0.597, 0.121, 0.282} where a total time spent in the easy and trivial cases of 340 msec for the New iteration and a total time spent in the expensive cases of 2,802 msec for the New iteration and 24,157 msec for the Old iteration. This results in an average speedup of 8.62× on the expensive inputs. For other scenarios, a similar speedup is observed on the expensive inputs, although scenarios easier than the currently used configuration have fewer expensive inputs. For example, if the initial and final target points are chosen at random instead of with antipodal targets, the distribution results in {0.968, 0, 0.032}.

FIG. 5A shows a convergence graph 500 from using the method 400 of FIG. 4. The convergence graph 500 shows the convergence time for instances of the above described configuration in three-dimensions using the New iteration for a different number of agents using both the variant of the ADMM and the TWA. It is again noted that the Old iteration cannot be applied to agents having paths in three-dimensions. The results indicate that the convergence time appears to grow polynomially with p, the TWA is faster than the ADMM, and the proximal operators lend themselves to parallelism, and thus added processing cores decrease time. FIG. 5B shows a distribution graph 550 from using the method 400 of FIG. 4. The distribution graph 550 shows that the paths found when the TWA solves the configuration in three-dimensions over 1,000 random initializations are not very different and seem to remain good in terms of objective value.

The exemplary embodiments provide a method and device for generating an optimization solution for an optimization problem. Specifically, the optimization solution is a modified version of the ADMM algorithm in which the messages in the message-passing algorithm includes various weights. The weights may indicate a “certainty,” a “no opinion,” or otherwise a “standard.” Through incorporating the weights, the optimization solution may be determined in a more efficient manner. The optimization solution may also be modified through incorporation of knowledge that is determined during the course of the message-passing. Whether a reasoner hierarchy or graph dynamics are used, the knowledge may further improve the efficiency at which the optimization solution is determined. The exemplary embodiments further utilize specialized proximal operators for agent-agent collisions, non-agent collisions, and landmarks. Accordingly, the optimization solution determines straightest possible paths for the agents that move from a first configuration to a second configuration in a tight formation.

Those skilled in the art will understand that the above-described exemplary embodiments may be implemented in any suitable software or hardware configuration or combination thereof. An exemplary hardware platform for implementing the exemplary embodiments may include, for example, an Intel x86 based platform with compatible operating system, a Mac platform and MAC OS, etc. In a further example, the exemplary embodiments of the above described method may be embodied as a program containing lines of code stored on a non-transitory computer readable storage medium that, when compiled, may be executed on a processor or microprocessor.

It will be apparent to those skilled in the art that various modifications may be made in the present invention, without departing from the spirit or scope of the invention. Thus, it is intended that the present invention cover the modifications and variations of this invention provided they come within the scope of the appended claims and their equivalents.

Claims

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.