# Design and Control of an Omnidirectional Mobile Wall-Climbing Robot
## 4 Nonlinear Model Predictive Trajectory Tracking Control
According to the sampling theorem, when the number of prediction steps is \(n\), the discretization of the \(\mathbf{X}\) can be expressed as follows:  
\[\mathbf{X}_{\epsilon}(k+n|k)=\mathbf{X}(k|k)+T\sum_{i=1}^{n}\hat{\mathbf{X}}_{r} (k+i-1|k)-\mathbf{X}_{r}(k+n|k) \tag{22}\]  
where \(\mathbf{X}_{r}(k+n|k)\) is a vector of position state quantities of the reference trajectory after the time \(nT\), input to the controller from higher-level decisions such as trajectory planning in the robot. Vector \(\hat{\mathbf{X}}_{r}(k+i-1|k)\) is the velocity state volume vector of the reference trajectory of the robot after the time \((i-1)T\), which is the control volume of the position loop and calculated by the position loop's optimization controller and input to the velocity loop as the reference trajectory of the velocity loop. In this paper, an objective function is designed, which considers the accuracy of trajectory tracking, smoothness of motion control, and solvability of the optimization algorithm [34] of the robot, and its expression form is as follows:  
\[J(k)=\sum_{i=1}^{N_{p}}||\hat{\mathbf{X}}_{\epsilon}(k+i|k)|\big{|}_{\mathbf{Q }}^{2}+\sum_{i=1}^{N_{c}}||\Delta\mathbf{u}(k+i|k)||_{\mathbf{R}}^{2}+||\varepsilon _{v}||_{\rho}^{2} \tag{23}\]  
where \(J(k)\) is the optimization objective function, \(\mathbf{Q}\) and \(\mathbf{R}\) are the weight matrices, \(\varepsilon\) is the relaxation factor, \(\rho\) is the relaxation factor weight coefficient, \(N_{p}\) is the number of prediction steps, and \(N_{c}\) is the number of control steps, with \(N_{p}\geq N_{c}\). We have  
\[\Delta\mathbf{u}(k+i|k)=\mathbf{u}(k+i|k)-\mathbf{u}(k+i-1|k) \tag{24}\]  
where \(k=1,2,3,\cdots,N_{c}-1\). This is considering the fact that the critical slip state into the constraint can fully utilize the robot's motion performance because no undesired slip occurs. Based on the conclusions related to the critical slip state, the constraints are adjusted according to the specific needs of the NMPC algorithm. In practical applications, if the robot is in a critical slip state for a long time, its motion will be unstable and eventually lead to undesired slip. Similarly, if the robot's drive motor is at full load for a long time, it may lead to high current, motor heating, and a decrease in the drive torque that the motor can provide, which may cause the motor to block. Therefore, a safety factor is designed for the constraints aiming at real-life problems to ensure that the robot will not be in a critical slip state and the motors will not be fully loaded for a long time. With the introduction of the safety factor, the constraints of the velocity loop are  
\[\left\{ \begin{aligned} |\mathbf{u}(t+kT)|&\leq \mathbf{S}_{f}\mathbf{f}(t+kT)\\ |\mathbf{u}(t+kT)|&\leq\mathbf{S}_{w}\mathbf{F}_{ wmax}\end{aligned} \right. \tag{25}\]  
where \(\mathbf{S}_{f}\) and \(\mathbf{S}_{w}\) are the safety coefficient matrices. For the position loop, the constraint mainly lies in the fact that the tangential relative motion speed between each wheel and the wall should not exceed the linear speed that the motor can provide for each wheel. Otherwise, the undesired slip between the wheel and the wall will occur. Thus, for the position loop, we have  
\[\mathbf{S}_{v}\mathbf{v}_{min}\leq\mathbf{A}(t+kT)\hat{\mathbf{X}}(t+kT)\leq \mathbf{S}_{v}\mathbf{v}_{max} \tag{26}\]where \(\textbf{S}_{v}\) is the safety coefficient matrix, and  
\[\textbf{A}(t+kT)= \begin{bmatrix}-\sin(\theta(t+kT)+\pi)&\cos(\theta(t+kT)+\pi)&r \\ -\sin(\theta(t+kT)-\dfrac{\pi}{3})&\cos(\theta(t+kT)-\dfrac{\pi}{3})&r\\ -\sin(\theta(t+kT)+\dfrac{\pi}{3})&\cos(\theta(t+kT)-\dfrac{\pi}{3})&r\end{bmatrix}  \tag{27}\]