# Formation Control and Distributed Goal Assignment for Multi-Agent Non-Holonomic Systems
## 2 Control Algorithm
\tag{13}\]Equations (9) and (12) can be transformed to the following form:  
\[E_{ix} =p_{ix}\cos(\theta_{i})+p_{iy}\sin(\theta_{i})+\sum_{j=1,j\
eq i}^{N} \frac{\partial V_{aij}}{\partial e_{ix}}\] \[E_{iy} =-p_{ix}\sin(\theta_{i})+p_{iy}\cos(\theta_{i})+\sum_{j=1,j\
eq i}^ {N}\frac{\partial V_{aij}}{\partial e_{iy}}\] \[e_{i\theta} =p_{i\theta}, \tag{14}\]  
where each derivative of the APF is transformed from the global coordinate frame to the local coordinate frame fixed to the robot. Finally, the correction variables expressed with respect to the local coordinate frame (Figure 2) are as follows:  
\[E_{ix} =e_{ix}+\sum_{j=1,j\
eq i}^{N}\frac{\partial V_{aij}}{\partial e_{ ix}}\] \[E_{iy} =e_{iy}+\sum_{j=1,j\
eq i}^{N}\frac{\partial V_{aij}}{\partial e_{ iy}}. \tag{15}\]  
Note the similarity of the structure of Equations (8) (updated by Equations (11)) and (15). The trajectory tracking algorithm from [4] was chosen based on the author's experience. It is simple, easy to implement, and, above all, it is effective. Tracking control with persistent excitation [24] and the vector-field-orientation method [25] were also taken into account. The former gives much worse convergence time, the latter gives even better convergence but it is more difficult to implement on a real robot. The control for \(N\) robots extended by the collision avoidance is as follows:  
\[ \begin{array}{l}v_{i}=v_{l}\cos e_{i\theta}+k_{1}E_{ix}\\ \omega_{i}=\omega_{l}+k_{2}\text{sgn}(v_{l})E_{iy}+k_{3}e_{i\theta},\end{array}  \tag{16}\]  
Figure 2: Robot in the environment with an obstacle. where \(k_{1}\), \(k_{2}\) and \(k_{3}\) are positive constant design parameters. **Assumption 3**. : _If the value of the linear control signal is less than considered threshold value \(v_{t}\), i.e., \(|v|<v_{t}\) (\(v_{t}\)-positive constant), it is replaced with a new value \(\vartheta=S(v)v_{t}\), where_  
\[S(v)=\left\{ \begin{array}{ccc}-1&\text{for}&v<0\\ 1&\text{for}&v\geq 0\end{array} \right., \tag{17}\]  
_(indexes omitted for simplicity)._  
Substituting Equation (16) for (7) error dynamics is given by the following equations:  
\[ \begin{array}{c}\dot{e}_{ix}=e_{iy}\omega_{i}-k_{1}E_{ix}\\ \dot{e}_{iy}=-e_{ix}\omega_{i}+v_{t}\sin e_{i\theta}\\ \dot{e}_{i\theta}=-k_{2}\text{sgn}(v_{t})E_{iy}-k_{3}e_{i\theta}\end{array} . \tag{18}\]  
Transforming (18) using (16) and taking into account Assumption 2 (when the robot gets into the collision avoidance region, in the collision avoidance state, velocities \(v_{l}\) and \(\omega_{l}\) are replaced with 0 value) error dynamics can be expressed in the following form:  
\[ \begin{array}{c}\dot{e}_{ix}=k_{3}e_{iy}e_{i\theta}-k_{1}E_{ix}\\ \dot{e}_{iy}=-k_{3}e_{i\theta}e_{ix}\\ \dot{e}_{i\theta}=-k_{3}e_{i\theta}\end{array} . \tag{19}\]  
Orientation error \(e_{i\theta}\) decreases exponentially to zero (refer to the last equation in (19)). In Figure 3 a schematic diagram of the control system is presented. The following signal vectors are marked: \([x\ y]^{T}=[x_{1}\ \dots\ x_{N}\ y_{1}\ \dots\ y_{N}]^{T}\), \(\theta=[\theta_{1}\ \dots\ \theta_{N}]^{T}\), \([x_{d}\ y_{d}]^{T}=[x_{1d}\ \dots\ x_{Nd}\ y_{1d}\ \dots\ y_{Nd}]^{T}\), \([v\ \omega]^{T}=[v_{1}\ \dots\ v_{N}\ \omega_{1}\ \dots\ \omega_{N}]^{T}\), \([p_{x}\ p_{y}]^{T}=[p_{1x}\ \dots\ p_{Nx}\ p_{1y}\ \dots\ p_{Ny}]^{T}\), \([e_{x}\ e_{y}]^{T}=[e_{1x}\ \dots\ e_{Nx}\ e_{1y}\ \dots\ e_{Ny}]^{T}\), \([E_{x}\ E_{y}]^{T}=[E_{1x}\ \dots\ E_{Nx}\ E_{1y}\ \dots\ E_{Ny}]^{T}\).