## 3 Control Approach
### A Method for Generating the Reference Trajectory Based on Cubic Spline and Virtual Structure
#### 3.2.2 Modeling of the Virtual Structure in Three Dimensions  
The global reference information could be obtained according to the above section. In order to achieve formation tracking control, a reference state information of each robot from the formation system needs to be acquired. As such, this section will describe how to obtain this information based on the virtual structure method. The virtual structure method treats the formation system as a rigid body. If a rigid body moves in space and each point on it has a trajectory corresponding to the motion of the rigid body, then each node of the virtual structure generates a motion trajectory. In this paper, each small spherical robot is seen as a node of the virtual structure, and the center of the virtual structure is seen as a virtual node. It is set that the virtual node moves along the formation path provided by the planning layer. A schematic diagram of the reference trajectory generation based on a virtual structure is depicted in Figure 8. The green plane in the figure is a triangular virtual structural plane. For the convenience of description, a virtual structure coordinate was established and denoted as \(O_{S}x_{S}y_{S}\{S\}\). \(\{S\}\) locates the origin as the center of the triangle virtual structure. The virtual structure coordinate moves along with the virtual node. The virtual structure coordinate system takes the tangent of the desired formation trajectory as the \(y_{S}\). The pink plane (\(plane_{S}\)) in the figure is perpendicular to the \(y_{S}\) axis. The projection of the \(z_{E}\) axis of the global coordinate system on the \(plane_{S}\) is set as \(z_{S}\). \(x_{S}\) could be determined according to the right hand rule, where \(\psi^{r},\theta^{r}\) are the pitch angle and heading angle of the virtual coordinate system relative to the inertial coordinate system, respectively.  
The position coordinates of the virtual node (or formation reference point) in the inertial coordinate system is marked as \(\mathbf{p}\in\mathbf{R}^{3}\). The position coordinate of the \(i\)-th node is labeled as \(\mathbf{p}_{i}^{5}\in\mathbf{R}^{3}\) in the virtual structure coordinate, and it is labeled as \(\mathbf{p}_{i}\in\mathbf{R}^{3}\) in the inertial frame. \(\mathbf{p}_{i}\in\mathbf{R}^{3}\) could be presented as Equation (14):  
\[\mathbf{p}_{i}=\mathbf{p}+R(\psi^{r},\theta^{r})L\mathbf{p}_{i}^{s}, \tag{14}\]  
where \(L\) is a formation-scaling coefficient matrix and is represented as the following:  
\[L=\left[\begin{array}{ccc}l&0&0\\ 0&l&0\\ 0&0&l\end{array}\right], \tag{15}\]  
where \(R(\psi^{r},\theta^{r})\) is the transformation matrix from the virtual structure coordinate to the inertial coordinate, and it can be represented as Equation (16).  
\[R(\psi^{r},\theta^{r})=\left[\begin{array}{ccc}\cos\theta^{r}\cos\psi^{r}& \cos\theta^{r}\sin\psi^{r}&\sin\theta^{r}\sin\psi^{r}\\ -\cos\theta^{r}\sin\psi^{r}&\cos\theta^{r}\cos\psi^{r}&-\sin\theta^{r}\cos\psi ^{r}\\ -\sin\theta^{r}&\sin\theta^{r}&\cos\theta^{r}\end{array}\right], \tag{16}\]  
Figure 8: Schematic diagram of the reference trajectory generation based on a virtual structure.  
where \(\mathbf{p}_{i}^{s},i=(1,2,\cdots,N)\) could be obtained based on the formation set in the virtual structure coordinate. \(\mathbf{p}_{i}\) represents the desired position of the \(i\) robot, which could be obtained based on Equation (14). The other state information of state vector \(\mathbf{q}_{i}^{r}\) is consistent with the state vector \(\mathbf{q}^{r}\).