## 3 Control Approach
### Formation Tracking Controller Based on ESO-MPC  
#### 3.3.1 Linear Predictive Model of the Small Spherical Robot  
Because the lower control input is the PWM (pulse width modulation) signal driving the thruster to generate the thrust, in the predictive model, the relationship between the robot states (the position \(x\), \(y\), and the yaw angle \(\phi\)) and the driving force is considered. The dynamics of the robot reflect the relationship between the velocity and the driving force, and the kinematics describes the relationship between the robot states and the velocity. Therefore, the predictive model for the trajectory tracking is established by combining Equations (10) and (11) as follows:  
\[\dot{\mathbf{x}} =\left[ \begin{array}{c}\mathbf{J(\phi)v}\\ \mathbf{M}^{-1}(\mathbf{\tau}-D(\mathbf{v})\mathbf{v})\end{array} \right]. \tag{17}\] \[=\mathbf{f}(\mathbf{x}_{r}\mathbf{\tau})\]  
Considering the limited computation ability of the small spherical robot, the linear MPC was designed. As such, it is essential to linearize Equation (17) in order to develop a state-space-based model predictive control algorithm. When referring to the system represented by Equation (17), \((\mathbf{x}_{0},\tau_{0})\) is a working point. It is assumed that a sustained force \(\mathbf{\tau}_{0}\) is applied to the system in the future, and then the state trajectory that can be presented as Equation (18) can be obtained. At the same time, a series of reference points from the above state trajectory can be obtained and are marked as \((\mathbf{\tau}_{0},\mathbf{x}_{r}(1)),(\mathbf{\tau}_{2},\mathbf{x}_{r}(2))\)\(\cdots\)\((\mathbf{\tau}_{k},\mathbf{x}_{r}(k))\). \[\dot{\mathbf{x}}_{r}(t) =\mathbf{f}(\mathbf{x}_{r}(t),\mathbf{\tau}(t)) \tag{18}\] \[\mathbf{\tau}(t) =\mathbf{\tau}_{0}\] \[\mathbf{x}_{r}(0) =\mathbf{x}_{0}\]  
Due to the current state of the robot being closest to the first reference point, the Taylor series expansion on the closest reference point was applied to Equation (17), and the high-order terms were ignored in order to obtain the following expression:  
\[\dot{\mathbf{x}}(t) =A_{t}(\mathbf{x}(t)-\mathbf{x}_{r}(0))+B_{t}(\mathbf{\tau}(t)-\mathbf{ \tau}_{0})+f(\mathbf{x}_{r}(0),\mathbf{\tau}_{0}) \tag{19}\] \[=A_{t}\mathbf{x}(t)+B_{t}\mathbf{\tau}(t)+f(\mathbf{x}_{r}(0),\mathbf{ \tau}_{0})-A_{t}\mathbf{x}_{r}(0)-B_{t}\mathbf{\tau}_{0},\]  
where \(d_{t}=f(\mathbf{x}_{r}(0),\mathbf{\tau}_{0})-A_{t}\mathbf{x}_{r}(0)-B_{t}\mathbf{\tau} _{0}\) represents the error between the next state, which is calculated from the linearization equation and the next state that is obtained from the nonlinear equation. By using the approximately discrete method, the discrete form of Equation (19) can be expressed as follows:  
\[\mathbf{x}(k+1)=\widetilde{\mathbf{A}}\mathbf{x}(k)+\widetilde{\mathbf{B}}\bm {\tau}(k)+d(k), \tag{20}\]  
where \(\widetilde{\mathbf{A}}=I+TA_{t}\), \(\widetilde{\mathbf{B}}=TB_{t}\), \(d(k)=f(\mathbf{x}_{r}(0),\mathbf{\tau}_{0})-\widetilde{\mathbf{A}}\mathbf{x}_{r} (0)-\widetilde{\mathbf{B}}\mathbf{\tau}_{0}\), \(I\) is an identity matrix, and \(T\) is the sampling time. \(\widetilde{\mathbf{A}},\widetilde{\mathbf{B}},d(k)\) update depending on the updating states of the robot. If Equation (19) is set as the predictive model, the optimization objective is the control input vector \(\mathbf{\tau}\), which is unable to avoid a sudden increment of the control input and will affect the continuity of the control input. Therefore, Equation (19) is transformed as follows:  
\[\mathbf{\xi}(k)=\left[ \begin{array}{c}\widetilde{\mathbf{x}}(k)\\ \widetilde{\mathbf{\tau}}(k-1)\end{array} \right]. \tag{21}\]A new state-space expression is obtained as follows:  
\[\mathbf{\xi}(k+1) =\mathbf{A}\mathbf{\xi}(k)+\mathbf{B}\Delta\mathbf{\tau}(k)+d_{e}(k) \tag{22}\] \[\mathbf{\eta}(k) =\mathbf{C}\mathbf{\xi}(k),\]  
where \(d_{e}(k)=[d(k),0_{3}]^{T}\), \(0_{3}\) is a three-dimensional all-zero column vector. The concrete description of \(\mathbf{\Lambda}\) and \(\mathbf{\widetilde{\mathbf{B}}}\) can be found in reference [20].