# Design and Control of an Omnidirectional Mobile Wall-Climbing Robot
## 4 Nonlinear Model Predictive Trajectory Tracking Control  
Trajectory tracking accuracy could be vital when a wall-climbing robot performs particular tasks in a real application. Traditional PID controllers are widely used in trajectory tracking control. However, the accuracy of the traditional PID controller is significantly impacted by delays of dynamic characteristics of the robot. Therefore, our robot's trajectory tracking motion control is designed based on nonlinear model predictive control(NMPC). The kinematic equations of the system can be expressed as follows:  
\[\dot{\textbf{X}}= \begin{bmatrix}\dot{x}(t)\\ \dot{y}(t)\\ \dot{\theta}(t)\end{bmatrix}\begin{bmatrix}-\dfrac{2}{3}\sin(\theta(t)+\pi)&- \dfrac{2}{3}\sin(\theta(t)-\dfrac{\pi}{3})&-\dfrac{2}{3}\sin(\theta(t)+\dfrac{ \pi}{3})\\ \dfrac{2}{3}\cos(\theta(t)+\pi)&\dfrac{2}{3}\cos(\theta(t)-\dfrac{\pi}{3})& \dfrac{2}{3}\cos(\theta(t)+\dfrac{\pi}{3})\\ \dfrac{1}{3r}&\dfrac{1}{3r}&\dfrac{1}{3r}\end{bmatrix}\begin{bmatrix}v_{1}(t)\\ v_{2}(t)\\ v_{3}(t)\end{bmatrix}  \tag{16}\]  
where \(\dot{\textbf{X}}\) is the velocity state quantity of the robot, and \(\dot{x}\) and \(\dot{y}\) are the velocity component of the robot's motion on the \(X\)-axis and \(Y\)-axis of the world coordinate system, respectively. \(\dot{\theta}\) is the angular velocity around the center of the robot coordinate system. Velocities \(v_{1}\), \(v_{2}\), and \(v_{3}\) are the relative motion velocities of the three wheels to the wall, respectively. The dynamic equations of the system can be expressed as follows. \[\dot{\textbf{X}}= \begin{bmatrix}\dot{x}(t)\\ \dot{y}(t)\\ \dot{\theta}(t)\end{bmatrix}\begin{bmatrix}-\sin(\theta(t)+\pi)&-\dfrac{\sin( \theta(t)-\dfrac{\pi}{3})}{m}&\dfrac{-\sin(\theta(t)+\dfrac{\pi}{3})}{m}\\ \dfrac{\sin(\theta(t)+\pi)}{m}&\dfrac{\sin(\theta(t)-\dfrac{\pi}{3})}{m}& \dfrac{\sin(\theta(t)+\dfrac{\pi}{3})}{m}\\ \dfrac{r}{I_{2}}&\dfrac{r}{I_{2}}\end{bmatrix}\begin{bmatrix}F_{w1}(t)\\ F_{w2}(t)\\ F_{w3}(t)\end{bmatrix} + \begin{bmatrix}0\\ -g\\ 0\end{bmatrix}  \tag{17}\]  
Figure 6: Maximum angular acceleration—orientation curve. where \(\hat{\textbf{X}}\) is the acceleration state quantity of the robot, and \(\ddot{x}\) and \(\ddot{y}\) are the acceleration component of the robot's motion on the \(X\)-axis and \(Y\)-axis of the world coordinate system, respectively. \(\ddot{\theta}\) is the angular acceleration around the center \(O^{\prime}\) of the robot coordinate system. The final control quantity of the robot is the driving force of each wheel of the robot. Moreover, the relationship between the velocity state and acceleration state can be expressed as follows:  
\[\hat{\textbf{X}}(t)=f(\hat{\textbf{X}}(t),\textbf{u}(t)) \tag{18}\]  
where  
\[\textbf{u}(t)= \begin{bmatrix}u_{1}(t)\\ u_{2}(t)\\ u_{3}(t)\end{bmatrix} = \begin{bmatrix}F_{\textit{v}1}(t)\\ F_{\textit{u}2}(t)\\ F_{\textit{u}3}(t)\end{bmatrix}  \tag{19}\]  
For a given reference trajectory, each point on the trajectory satisfies the kinematic and dynamic equations described by (16) and (17). The reference quantity is denoted by the subscript \(r\). Then, we have  
\[\hat{\textbf{X}}_{r}(t)=f(\hat{\textbf{X}}_{r}(t),\textbf{u}_{r}(t)) \tag{20}\]  
where  
\[ \begin{cases}\dot{\textbf{X}}_{r}=\begin{bmatrix}\dot{x}_{r}(t)\\ \dot{y}_{r}(t)\\ \dot{\theta}_{r}(t)\end{bmatrix}\\ \textbf{u}_{r}(t)=\begin{bmatrix}u_{r1}(t)\\ u_{r2}(t)\\ u_{r3}(t)\end{bmatrix}=\begin{bmatrix}F_{\textit{r}\textit{v}1}(t)\\ F_{\textit{r}\textit{u}2}(t)\\ F_{\textit{r}\textit{u}3}(t)\end{bmatrix}\end{cases}  \tag{21}\]  
In order to obtain a better control effect, a model predictive controller with a dual-loop structure including position and velocity loops is designed in this paper, as shown in Figure 7. The control quantity calculated by the position loop is the velocity of the robot in the world coordinate system. It is then used as the reference quantity of the velocity loop. Therefore, the position loop is calculated to obtain its error prediction model. Figure 7: Schematic diagram of the dual-loop structure.