## 3 Controller Design
### The Upper-Level Controller  
According to the analysis conducted in Section 2, it was determined that the tracking controller guarantees closed-loop stability as long as there is persistent excitation between the actual positions and the reference positions. In order to address the point stabilization problem while taking into account future movements and various constraints, an NMPC approach has been specifically developed in this section. Based on the robot kinematic model (6), and using \(T\) as the sampling time, we obtain the discretized model as  
\[X_{e}(k+1)=f(X_{e}(k))+gu_{e}(k) \tag{8}\]  
with  
\[f(X_{e}(k))= \begin{bmatrix}1&\omega T&0\\ -\omega T&1&v_{r}\frac{\sin{\pi_{3}}}{x_{3}}T\\ 0&0&1\end{bmatrix}  \tag{9}\]  
\[g=T \begin{bmatrix}1&0\\ 0&0\\ 0&1\end{bmatrix}  \tag{10}\]  
The principle of the model predictive controller is shown in Figure 4; it employs an explicit model to predict the future outputs of the system and, at each sampling time, solves an optimization problem for the control input. This optimization process will take into consideration the constraints, ensuring that the generated control inputs adhere to the limitations imposed by the system's dynamics and input boundaries. The optimal control input is obtained by continuously solving the following optimization problem:  
\[\min_{\Delta u_{e}(k)}J(k)=\sum_{j=1}^{N_{p}}\left\|\,X_{e}(k+j|k)\,\right\|_{Q }^{2}+\sum_{j=0}^{N_{m}-1}\left\|\Delta u_{e}(k)\right\|_{W}^{2} \tag{11}\]  
Subject to the constraints  
\[ \begin{array}{c}X_{\min}\leq X_{e}(k)\leq X_{\max}\\ u_{\min}\leq u_{e}(k)\leq u_{\max}\\ \Delta u_{\min}\leq\Delta u_{e}(k)\leq\Delta u_{\max}\end{array}  \tag{12}\]  
where \(N_{p}\) and \(N_{m}\) are the prediction horizon and the control horizon, respectively, and \(N_{m}\leq N_{p}\); \(Q\) and \(W\) are weighting matrices for two optimization terms, and \(\left\|\,*\,\right\|^{2}\) denotes the Euclidean norms of vectors; the input increment is \(\Delta u_{e}(k)=u_{e}(k)-u_{e}(k-1)\); and \([u_{\min},u_{\max}]\), \([u_{\min},u_{\max}]\), and \([u_{\min},u_{\max}]\) are the boundaries of the control variable, the control increment variable, and the state variable, respectively. According to the nonlinear affine model (8), we have  
Figure 4: The block diagram of the model predictive controller. \[\left\{ \begin{array}{l}\begin{array}{l}\mathrm{X}_{e}(k+1|k)=f[X_{e}(k|k-1)]+g(u _{e}(k-1)+\Delta u_{e}(k))\\ X_{e}(k+2|k)=f[X_{e}(k+1|k-1)]+g(u_{e}(k-1)+\Delta u_{e}(k)+\Delta u_{e}(k+1)) \\ \vdots\\ \mathrm{X}_{e}(k+N_{p}|k)=f[X_{e}(k+N_{p}-1|k-1)]+g(u_{e}(k-1)+\Delta u_{e}(k )+\Delta u_{e}(k+1)+...+\Delta u_{e}(k+N_{m}-1))\end{array}\end{array} \right. \tag{13}\]  
Defining the following vectors:  
\[\overline{X}_{e}=[X_{e}(k+1|k),\ X_{e}(k+2|k)\,\cdots\ X_{e}(k+N_{p}|k)] \prime\in\Re^{N_{p}n_{x}} \tag{14}\]  
\[\Delta U=[\Delta u_{e}(k)\,\Delta u_{e}(k+1),\ \cdots\ \Delta u_{e}(k+N_{m}-1)] \prime\in\Re^{N_{m}n_{u}} \tag{15}\]  
Transforming (14) into matrix form, we obtain  
\[\overline{X}_{e}=F+V+G\Delta U \tag{16}\]  
where  
\[F_{i}=\left[ \begin{array}{c}f[X_{e}(k)]\\ f[X_{e}(k+1|k)]\\ \vdots\\ f[X_{e}(k+p-1|k)]]\end{array} \right]\in\Re^{N_{p}n_{x}} \tag{17}\]  
\[V=\left[ \begin{array}{c}gX_{e}(k-1)\\ gX_{e}(k-1)\\ \vdots\\ gX_{e}(k-1)\end{array} \right]\in\Re^{N_{p}n_{x}} \tag{18}\]  
\[G=\left[ \begin{array}{cccc}g&0&\cdots&0\\ g&g&\cdots&0\\ \vdots&\vdots&\ddots&\vdots\\ g&g&\cdots&g\end{array} \right]\in\Re^{N_{p}n_{x}\times N_{m}n_{u}} \tag{19}\]  
Hence, the original optimization Problem (14) becomes  
\[ \begin{array}{ll}\min\limits_{\Delta U}J(k)&=\left\|\overline{X}_{e}\right\| _{Q}^{2}+\left\|\Delta U\right\|_{W}^{2}\\ &=\left\|F+V+G\Delta U\right\|_{Q}^{2}+\left\|\Delta U\right\|_{W}^{2}\\ s.t.&\\ u_{min}\leq u_{e}(k-1)+\Delta U\leq u_{max}\\ \Delta u_{min}\leq\Delta U\leq\Delta u_{max}\\ X_{min}\leq(F+V+G\Delta U)\leq X_{max}\end{array}