# Formation Control and Distributed Goal Assignment for Multi-Agent Non-Holonomic Systems
## 5 Goal Exchange  
This section presents a new control that introduces the ability to exchange goal between agents.  
The block diagram of the new control is shown in Figure 6. Two new blocks are included: goal switching and permutation block.  
In the method proposed here Equation (2) is replaced as follows:  
\[\bar{x}_{id}=x_{l}+d_{ix}\ \ \ \ \ \bar{y}_{id}=y_{l}+d_{iy}. \tag{29}\]  
The new variables \(\bar{x}_{id}\) and \(\bar{y}_{id}\) are not representing the goal position of robot \(i\), but the goal that can be assigned to any robot in the formation.  
One can introduce the following aggregated goal coordinate vectors: \(\bar{x}_{d}=[\bar{x}_{1d}\ \dots\ \bar{x}_{Nd}]^{T}\) and \(\bar{y}_{d}=[\bar{y}_{1d}\ \dots\ \bar{y}_{Nd}]^{T}\) (numbers in lower index represent the numbers of the goals). The assignment of goals to particular robots is computed using \(N\times N\) permutation matrix \(P(t)\):  
\[x_{d}=P(t)\bar{x}_{d}\ \ \ \ y_{d}=P(t)\bar{y}_{d}. \tag{30}\]  
Resulting vectors contained goal coordinates assigned to particular robots \(x_{d}=[x_{1d}\ \dots\ x_{Nd}]^{T}\) and \(y_{d}=[y_{1d}\ \dots\ y_{Nd}]^{T}\) (number in lower index represents the number of the robot).  
An additional control loop is introduced that acts asynchronously to the main control loop (Figure 6).  
Let's assume that at some instant of time \(t_{1}\) an arbitrary goal \(m\) is assigned to the robot \(k\) and another goal \(n\) is assigned to the robot \(l\). This can be written as:  
\[\begin{array}{l}[x_{kd}\ y_{kd}]^{T}=[\bar{x}_{md}\ \bar{y}_{md}]^{T}\\ [x_{ld}\ y_{ld}]^{T}=[\bar{x}_{nd}\ \bar{y}_{nd}]^{T}.\end{array} \tag{31}\]  
There are ones in permutation matrix \(P(t_{1})\) at element \((m,k)\) and \((n,l)\) and all other elements in rows \(m\), \(n\) and columns \(k\), \(l\) were zero.  
At some discrete instant in time \(t_{s}>=t_{1}\) for the pair of robots \(k\) and \(l\) and their goals \(m\) and \(n\  
where \(P(t_{s}^{-})\) is the permutation matrix before switching.  
The elementary matrix \(S_{mn}\) is a row-switching transformation. It swaps row \(m\) with row \(n\) and it takes the following form:  
\[S_{mn}=\left[\begin{array}{ccccccccc}1&0&\ldots&0&\ldots&0&\ldots&0&0\\ 0&1&\ldots&0&\ldots&0&\ldots&0&0\\ \vdots&\vdots&&\vdots&&\vdots&&\vdots&\vdots\\ 0&0&\ldots&0&\ldots&1&\ldots&0&0\\ \vdots&\vdots&&\vdots&&\vdots&&\vdots&\vdots\\ 0&0&\ldots&1&\ldots&0&\ldots&0&0\\ 0&0&\ldots&1&\ldots&0&\ldots&0&0\\ \vdots&\vdots&&\vdots&&\vdots&&\vdots&\vdots\\ 0&0&\ldots&0&\ldots&0&\ldots&1&0\\ 0&0&\ldots&0&\ldots&0&\ldots&0&1\end{array}\right]\leftarrow\text{$m$-th row}\] \[\leftarrow\text{$n$-th row} \tag{34}\] \[\uparrow\] \[\text{$m$-th}\] \[\text{col.}\]  
Transformation (33) describes a process of the goal exchange between agents \(k\) and \(l\) at time \(t_{s}\). After that goal \(m\), was assigned to robot \(l\) and goal \(n\) was assigned to robot \(k\) that is equivalent to the following equalities:  
\[\begin{array}{l}[x_{kd}\;y_{kd}]^{T}=[\pi_{nd}\;\hat{y}_{nd}]^{T}\\ [x_{ld}\;y_{ld}]^{T}=[\pi_{nd}\;\hat{y}_{nd}]^{T}.\end{array} \tag{35}\]  
Note that the process of goal exchange is asynchronous with the main control loop. It operated at lower frequency because it required communication between remote agents, which is time consuming. The low frequency subsystem is highlighted in grey in Figure 6.