Distributed Optimization of Multi-Robot Motion with Time-Energy Criterion

1.1 Global problem formulation

We can present the discrete time global optimization (numerical optimal control) problem for L robots as follows:

with tbeing the time-independent variable, kbeing the time index in discrete domain, tsibeing the sampling period, and Nbeing the number of time discrete instants across the time horizon, i.e., k=0,1,…,N. The sampling period corresponds to the length of time the system input uitis kept constant (zero-order hold): we assume the system input to be

System behavior is governed by the nonlinear dynamic system in fDxikuiktsikwith xikas the robot istates an initial condition of xi0=x0i. The above optimal control problem has a final state objective of HxiNwith the Lagrangian Lxikuiktsikinformation being embedded into a dummy state variable zik.

The above optimization problem can be viewed as having two sets of control variables; the first set resembles the discretized system inputs, uik∀k, and the other set consists of the variable sampling period, tsik∀k. Let us have the Lagrangian for the problem be

with βbeing the scalar weight on time. The performance is restricted by a collection of inequality constraints of robot-specific constraints g.≤0and robot-interaction constraints of Ωi.≤0. The objective function is just the summation of individual objectives. In this paper, as it will be explained later, we consider only collision avoidance as robot-interaction requirement; however, the above formulation can also meet other considerations. It can be shown that the objective function in (1) corresponds to objective function of the form

2.1 Subgradient method

Before going further, we discuss a method used in solving distributed optimization problem which will help us in solving the problem of this paper. This method is called subgradient methods [30]. These methods are similar to the popular optimization algorithms using gradient descent. However, they are extended to escape function differentiation. The works [31, 32, 33] also explore the method in the perspective of multi-agent systems.

Consider the typical problem:

This typical problem can be solved using any gradient descent method. At iteration mof an algorithm, a solver, or an optimizer, can be constructed as

with αmas a predefined step size. For a standard gradient method, the vector dmcontains the gradient information of the problem. The simplest definition is to have

However, for the subgradient method [33], we will have a definition of

with pm, called subgradient of Ju, being any vector that satisfies the following:

The subgradient method is a simple first-order algorithm to minimize a possibly nondifferentiable function. The above definition escapes the requirement of a differentiated objective function. It is defined as finding any vector that makes the optimization algorithm go to better value in a first-order optimality sense. Of course, when a gradient ∇Jumexists, we can compute the subgradient as the gradient. As it is a first-order method, it could have a lower performance than other second-order approaches. However, the advantage here is that it does not require differentiation. Also, and perhaps more importantly, it gives us flexibility to solve problems in a distributed manner as will be seen later.

Now observe the following constrained optimization problem:

Let gube a vector of Mconstraints. Then, we can define the dual problem. Let us define the dual function of λand uas

The vector λof size Mcorresponds to the multipliers associated with each constraint. The dual problem relaxes the constraints of the original primal problem in (10) and solves for λto maximize the dual function:

The dual optimization problem is the pair of two optimization problems, namely, a maximization in λas in (12) and a minimization in u. The pair resembles a maximization-minimization problem. You can visualize the solution of the problem as attacking the effect of constraint violation while solving for the original minimization problem concurrently.

Now, an algorithm for solving the dual problem utilizing subgradient method is discussed. Let us define

The above definition is to clarify the minimum attained at any value of λ. So, with the above definition, at iteration m, with also denoting um=uλm, we can safely have

Now, it is obvious from (14) that at iteration m, a subgradient of the dual function in (14) as function of λcan be computed as pm=gum. At iteration mof the algorithm, an update for the multipliers is constructed as

The projection operator Pλ≥0.is to ensure that the value of the update λm+αmpmis positive or enforced to zero. Also, observe the ascent update with the “+” sign rather than a descent update as it is a maximization. We can assume an initial λ0=0or any other positive value. An optimal solution to the original problem in (10) will be attained as m→∞, with the optimal solution value of um.

2.2 The distributed algorithm

Recall the problem of combination of Lsubproblems in (2). Now, let us have the following global problem:

As mentioned, the constraints giu1u2…uL≤0are the complicating (or coupling) constraints. We can formulate the dual pair problems to be

Put in mind that λi=λ1λ2⋯λL. If we define the notations of

then we can apply the primal-dual update from (13) and (15) at an iteration mas

We can see that the above pair of updates can easily be distributed; after the relaxing of the constraints, the primal problem can be separated. The facility of subgradients lets us propose that any iteration mfor subproblem ihas

The function φmin.is any algorithm minimizer for the primal problem constrained by ui∈Ωi,∀i. Observe that the primal update for each subproblem needs only the latest values of the other subproblem updates. During the computations of an iteration, computation of um+1iand λm+1iis done independent of each other; the updates above can be computed in parallel for each subproblem. The only information shared after each iteration is um1,um2,…,umLamong all.

3.1 Problem formulation

Now, in this section we can apply the previous discussion into the problem of optimizing the motion of multiple robots. Recall the global optimization problem of motion of Lmobile robots from (1)

You can see that the problem above is just the combination of Lsubproblems with superscript icorresponding to each robot. The objective function is just the summation of individual objectives. The coupling constraint of Ωixik∀iuik∀itsik∀i≤0,∀kis the only difference to the single robot problem. To simplify the notations, let us define

The above definition is just to reduce the notation of robot input sequence. If we have, for example, two robot inputs ui=u1iu2iT(e.g., wheel torques), then we have a total of 3×L×Ncontrol variables of the optimization problem. We can condense notation of the global problem of multi-robot system without loss of generality to be

with objectives

which are subject to the set of individual robot iconstraints of

3.2 Distributed algorithm

Returning back to the primal-dual problem pair in Section 2, we can establish the algorithm updates according to the defined updates in (19). At each iteration m, we update each robot inputs and multipliers according to

The minimizer update φmin.is responsible to solve the single robot optimization (primal) problem according to the objective defined in (22) and subject to constraints in (23). In this paper, the minimizer update φmin.is selected to be any state-of-the-art nonlinear programming (NLP) algorithm. Let us have the step size αfor the dual update to be constant. This is sufficient for converging to a solution of the original problem [33]. You can read the algorithm updates in (24) at iteration mas each robot independently optimizes its whole motion throughout the whole time horizon k=0,1,…,Nwhile at the same time puts in mind the extra cost of cooperation/interaction with others introduced by the term λmiTΩmiand so on.

3.3 Algorithm convergence

In this brief section, elaboration is put forth about how to practically use the algorithm. The ultimate goal is to optimize primal problem with no collision violation, i.e., reaching optimal dual (maximum) solution. At each global iteration, we only need to improve the primal problem values for the updated extra cost of the interaction constraint, λiTΩi. In this paper, a perfect solution is to optimize while maintaining Ωi∀k≤0. A logical property is to monitor the M-element vector of constraints for positive values, i.e., violations. So, a stopping criterion for the algorithm can be chosen to be some minimum change TolJin the primal problem value:

We can also distribute the stopping decision to individual robots by observing the change in individual objective values.

With condition (25) on its own, we cannot always be satisfying the collision requirement. So, this condition can be accompanied by a condition on the collision constraint violation. For all robots, elements of the complete constraint vector Ωi∀kshould be less than a relatively small positive value TolΩ. So, for each robot, an extra stopping criterion along with criteria in (25) is to have

Specific values of TolΩand TolJdepend on the nonlinear programming algorithm and/or the global desired requirements. Note that the behavior of the two tolerance parameters could be competitive with each other.

4.1 System description

Figure 1 shows the individual robot system considered here. Robot state includes xi=xyϕθRθLvRvLTwith both position xyand orientation ϕand θRθLvRvLas the right and left wheel angular positions and velocities, respectively. Robot input includes the respective wheel torques ui=τRτLT. You can have the details of the applied nonlinear dynamic model fDxikuiktsikbased on the system model developed in [34, 35]. Details of discretization and choice of parameters of the robot model can be found in [21]. As mentioned before, for choices of Q,Rin Section 1, the Lagrangian for the problem is chosen to include the cost for energy spent by the torques of the wheels, the cost for kinetic energy spent by robot body, and the weight on time. Individual robot constraints include final desired configuration tolerance, torque limits, and the ensurance of zero final velocities (see more details in [36]).

4.2 Collision avoidance

Here, we will discuss the formulation and the structure of the coupling constraints. The robots can be designed to perform any cooperative strategy in their motion. Here, we only consider the global goal of optimizing the motion of each robot in time and energy while avoiding colliding with each other during the motion. Let us define the coupling constraint vector across the discrete time indexes as

For the ith robot, it tries to avoid colliding with the rest of L−1robots at each of its time indexes k. Let us label elements of the constraint vector as Ωijk. Each element is corresponding to a definition of constraint at time index kfor all other robots, j≠i. So, for each robot, the constraint vector Ωiis of size M=L−1×N; of course, the multiplier vector λiin (24) is of the same size.

We define the collision avoidance by constraining motion of other robots to be outside a safety circle region around each irobot at the position xiyiin the 2D plane:

So, we can define each element of the constraint vector as

The radius of the safety region is chosen as p. Because of the definition of the sampling period variable, at each of discrete time step k,the actual time variable does not necessarily imply tik=tjkfor all the other L−1robots. That is why you see that the x- and y- coordinates in (23) of the jthrobot are noted as x̂jŷjwhich are chosen to be calculated as linear interpolation of positions according to available actual times of tik and tjk.

4.3 Simulation examples

You can follow the whole distributed algorithm for the time-energy optimization of multi-robot system with collision avoidance in the flowchart in Figure 2. View the flowchart as the process for each individual robot (subproblem). We implement the algorithm in Figure 2. For the primal minimizer update in (24), the nonlinear programming (NLP) function offminconin MATLAB is used. We solved the problem for motion of L=3mobile robots. Utilizing the parallel capability in MATLAB, the distributed steps are solved independently utilizing three parallel processors. We choose number of instants N=40; so, we are going to optimize 120 control variables for each robot. More details can be found in [36].

Example 1. In this example, exploration of the behavior of the algorithm is shown. The problem has the following desired values of initial and final positions and orientations for the three robots:

Here, robot 1 has equal objective weights of 5 on both time and energy, robot 2 has weights of 10 on energy and 1 on time, and robot 3 has 10 on time and 1 on energy. The maximum number of internal NLP iterations (primal update) is set to only 10. The step size is set to α=0.1. Maximum global iterations are allowed for 100 iterations. The safety circle radius is chosen to be p=2.

Figure 3 contains the objective (time-energy) value evolution throughout iterations. You can see the stable convergence as the algorithm progresses. Figure 4 shows each of the robots’ safety clearances during the optimized motion. In Figure 5, snapshots of motion of the three robots at different time instants are depicted. This illustrates the collision avoidance attained throughout the optimized motion. Observe also how different are the speeds of each robot because of objective weights; note from Figure 4 that each robot has a different final time for their motion. The algorithm has shown good performance at eliminating collision constraint violations. Figures 4 and 5 show an instant (around t = 12) where robots 1 and 3 violate collision distance with very small value, but no collision occurs. This is because the maximum number of iterations of the algorithm is exhausted. This indicates the possibility for the motion to be optimized even more if collision constraints were relaxed or if more algorithm iterations were allowed. Initialization of the algorithm also plays a role in algorithm evolution.

Example 2. This example illustrates more the satisfaction of the objectives. In this example the safety circle radius is put as p=3. Here we choose the following initial and final positions and orientations for the three robots:

After applying our approach, you can see the resulting optimized motions in Figure 6. In Figure 6, errors in x- and y-coordinates and orientation of each robot are shown with respect to time. It is clear that errors of zero are achieved. In Figure 7 for each robot, constraint evaluations, i.e., safety clearance, are displayed for the other two robots throughout time. You can see that robots come close to each other sometimes but without violating the safety distance. This result is attained maybe because of special structure of initial and final positions and orientations. That could have given flexibility for the algorithm.