# Multi-Robot Formation Control Based on CVT Algorithm and Health Optimization Management
## 2 Multi-Robot Formation Control
### Cost Function Optimization  
In most application scenarios, the CVT algorithm is used to minimize the cost function of the system. The mathematical formula of the cost function is shown in Equations (7) and (8):  
\[E_{V}(p)=\int_{V_{i}}\bigl{|}q-p_{i}\bigr{|}^{2}\rho(q)dq,i=1,2,\ldots,n \tag{7}\]  
\[E_{V}(p)=\sum\limits_{V_{i}}\sum\limits_{j=1}^{N}\bigl{|}q_{j}-p_{i}\bigr{|}^{ 2}\rho(q_{j}) \tag{8}\]  
The so-called cost function can also be expressed as the variance or energy function. The cost function estimates the error between the generated point and the centroid of the Voronoi cell through the density function of the region. The proof of minimizing the cost function using the CVT algorithm is as follows:  
1. The position change of the generated point \(p_{j}\) is shown in Equation (9): \[E_{V_{j}}(p_{j}+\epsilon)-E_{V_{i}}(p_{j})\ =\int\limits_{V_{j}}\rho(q) \epsilon\bigl{\{}\epsilon+2\bigl{(}p_{j}-q\bigr{)}\bigr{\}}dq\] (9)
2. Simplifying Equation (9) to Equation (10): \[\frac{E_{V_{j}}(p_{j}+\epsilon)-E_{V_{i}}(p_{j})}{\epsilon}\ =\int\limits_{V_{j}}\rho(q) \epsilon\bigl{\{}\epsilon+2\bigl{(}p_{j}-q\bigr{)}\bigr{\}}dq\] (10)
3. Assuming that \(\epsilon\) in Equation (10) tends to 0, we obtain: \[\dot{E}_{V_{j}}(p_{j})=2\int\limits_{V_{j}}\rho(q)\bigl{(}p_{j}-q\bigr{)}dq\] (11)
4. Deforming Equation (11) into Equation (12): \[\dot{E}_{V_{j}}(p_{j})=2p_{j}\int\limits_{V_{j}}\rho(q)dq-2\int\limits_{V_{j} }q\rho(q)dq\] (12)
5. If the derivative part is 0, as shown in Equation (13), the solution at this time is the minimum solution. \[p_{j}{}^{*}=\frac{\int\limits_{V_{j}}q\rho(q)dq}{\int\limits_{V_{j}}\rho(q)dq}\] (13)At this time, the position of the generated point is the centroid position of the Voronoi cell, and hence the central Voronoi tessellation can minimize the cost function of the system. Applying the CVT algorithm to the multi-robot system can minimize the cost function. Taking the formation task as an example, multi-robot systems are required to form a specific formation and maintain a fixed distance between robots. Therefore, the cost function is composed of the error between the actual distance and the expected distance, as shown in Equation (14):  
\[E(p)=\sum_{i=1}^{n}\sum_{j=N_{i}}\frac{1}{2}\left(\left\|p_{i}-p_{j}\right\|-d _{ij}\right)^{2}=\sum_{i=1}^{n}E_{i}\left(\left\|p_{i}-p_{j}\right\|\right) \tag{14}\]  
where \(d_{ij}\) is the set threshold distance between robots \(i\) and \(j\) and the immediately expected distance. The distance between the current robots and the expected formation is measured by the cost function \(E\). When the contemporary price function is 0, the robot forms the expected formation, and the derivative of \(E_{i}(x)\) is calculated as shown in Equation (15):  
\[\frac{\partial E_{i}}{\partial p_{i}}=\sum_{j\in N_{i}}\frac{\left\|p_{i}-p_{ j}\right\|-d_{ij}}{\left\|p_{i}-p_{j}\right\|}\left(p_{i}-p_{j}\right)^{ \text{T}} \tag{15}\]  
Equation (15) shows that if the distance between two robots is less than the set threshold distance, the weight value is negative and the distance between the two robots is controlled to increase. When the distance between robots is greater than the set threshold distance, the weight \(\omega_{ij}\) will control the distance between two machines to shorten. When the set threshold distance \(d_{ij}=0\), the task will be transformed into a group consistency problem. The task performed by the multi-robot system is transformed into the optimization problem of the cost function. The constraint-based formulation derivation can enhance the adaptability of the system, and the distribution of the controller can be guaranteed by selecting different weights or defining some assumptions. The robots only need to execute their inputs to ensure the convergence of the system within a limited time.