## 4 Simulation Results
### Numerical Simulation Results
#### 4.1.2 Formation Tracking without Disturbance  
A triangular formation simulation was conducted in three-dimensional space. The weight matrix in Equation (35) is \(R_{1}=R_{2}=R_{3}=0.01\times I_{4}\), \(Q_{1}=\text{diag}(1,0.5,1,1)\), \(Q_{2}=\text{diag}(1,2,1,1),Q_{3}=I_{4},Q_{12}=Q_{23}=Q_{13}=0.15\times I_{4}\). The initial states of the three robots and the formation shape are as per the following:  
\[d_{12}=-d_{21}=[0,4,0]^{T},d_{13}=-d_{31}=[0,2,-2]^{T},d_{23}=-d_{32}=[0,-2,-2]^ {T} \tag{41}\] \[\mathbf{q_{1}}=[0,1.5,0,0,0,0,0]^{T}\] \[\mathbf{q_{2}}=[-0.8,-1,1,0,0,0,0]^{T}\] \[\mathbf{q_{3}}=[0,1,1,0,0,0,0]^{T}\]  
where \(d_{ij}\) represents the three-dimension position vector difference between the \(i\)-th and the \(j\)-th positions. \(\mathbf{q_{i}}\) is the initial state vector including \(x,y,z,\phi,u,w,r\). The simulation results are illustrated in Figure 10. It can be seen that the initial positions of the three robots exhibited a random triangular distribution, which gradually converged into the expected triangular formation. In addition, when the robots formed a triangular formation, Robot 1 and Robot 2 did not converge to their corresponding reference trajectories. However, while maintaining the formation, the three robots gradually converged to the reference trajectories and achieved a triangular formation in three-dimensional space. Figure 11 shows the error between the state variables of each robot and the reference state variables during the formation process, which is defined as the trajectory tracking error for convenience of description. As shown in the figure, the trajectory tracking error gradually converged to 0. The state difference between the two adjacent robots is presented in Figure 10. The position difference between the adjacent robots gradually converged to the expected formation setting, and the other states difference converged to 0. In order to analyze the result more clearly, Table 3 lists the root mean square error of the trajectory tracking and the root mean square error of the formation. In the table, 1-2 represents the error between the actual distance and the expected distance between Robot 1 and Robot 2, which is referred to as the formation error for convenience of description. As can be seen, the trajectory tracking errors of the three robots in the x direction was relatively large, but the formation errors in the x direction was very small. The reason for this was that the formation controller had constraints on the formation shape, and there was a weight adjustment between the trajectory tracking and formation constraints. When the weight of the formation constraint term in the cost function is large, more emphasis will be placed on the formation constraint. The maximum error in the table was 0.1354 m, which was less than the diameter of the small spherical robot (0.3 m), which verified that three robots were able to finish the triangle formation tracking task based on the VS-ESO-MPC. The control inputs of the three robots during the formation process are shown in Figure 12. It can be seen that all control input quantities were within the constraint range. Figure 9: Reference trajectory obtained based on the cubic spine method. (**a**) Reference trajectory. (**b**) The yaw angle of the sampling point. Figure 11: Error analysis during formation. (**a**) Trajectory tracking error during formation. (**b**) State error between any two robots. \begin{table}
\begin{tabular}{c c c c c} \hline
**Robot Label** & \(\Delta x\) **(m)** & \(\Delta y\) **(m)** & \(\Delta z\) **(m)** & \(\Delta\phi\) **(rad)** \\ \hline
1 & 0.135 & 0.087 & 0.020 & 0.043 \\
2 & 0.120 & 0.024 & 0.010 & 0.009 \\
3 & 0.130 & 0.081 & 0.010 & 0.041 \\
1–2 & 0.057 & 0.100 & 0.005 & 0.041 \\
1–3 & 0.029 & 0.098 & 0.004 & 0.023 \\
2–3 & 0.036 & 0.059 & 0.001 & 0.003 \\ \hline \end{tabular}
\end{table} 
Table 3: Root mean square error of the trajectory tracking and formation constraints. Figure 10: Triangle formation trajectory based on the VS-MPC-ESO.