# Research on Trajectory Planning and Tracking Methods for Coal Mine Mobile Robots
## 3 MPC-Based Trajectory Tracking for Mobile Robots
### MPC-Based Trajectory Tracking Control for Mobile Robots  
#### 3.2.1 Construction of Predictive Models  
\(P_{r}(x_{r},y_{r},\theta_{r})\) is the target pose of the robot, where \((x_{r},y_{r})\) and \(\theta_{r}\) are the target position and target heading angle, respectively. Then, the target trajectory can be written in the following form according to Equation (48):  
\[\dot{X}_{r}=\begin{pmatrix}\dot{x}_{r}\\ \dot{y}_{r}\\ \dot{\theta}_{r}\end{pmatrix}=\begin{pmatrix}\cos\theta_{r}&0\\ \sin\theta_{r}&0\\ 0&1\end{pmatrix}\begin{pmatrix}v_{r}\\ \omega_{r}\end{pmatrix}=f(X_{r},u_{r}) \tag{52}\]  
Equation (52) is a nonlinear equation. It is linearized and approximated for ease of computation. A first-order Taylor expansion and omission of higher-order terms at the target position is shown as:  
\[\dot{X}=f(X_{r},u_{r})+f_{X,r}(X-X_{r})+f_{u,r}(u-u_{r}) \tag{53}\]  
where \(f_{X,r}\) is the Jacobi matrix of \(f\) with respect to \(X\). \(f_{u,r}\) is the Jacobi matrix of \(f\) with respect to \(u\):  
\[f_{X,r}=\begin{pmatrix}0&0&-v_{r}\sin\theta_{r}\\ 0&0&v_{r}\cos\theta_{r}\\ 0&0&0\end{pmatrix},f_{u,r}=\begin{pmatrix}\cos\theta_{r}&0\\ \sin\theta_{r}&0\\ 0&1\end{pmatrix} \tag{54}\]  
Denoting \(\overline{\dot{X}}\), \(A\), \(B\), \(\overline{X}\), and \(\overline{u}\) as \(\dot{X}-\dot{X}_{r}\), \(f_{X,r}\), \(f_{u,r}\), \(X-X_{r}\), \(u-u_{r}\), respectively, is shown as:  
\[\overline{\dot{X}}=A\overline{X}+B\overline{u} \tag{55}\]  
where \(\overline{X}\) denotes the error between the actual robot position and the target position. \(\overline{u}\) denotes the error between the actual control quantity and the target control quantity. The  
Assume that \(\overline{A}(k)\) and \(\overline{B}(k)\) are constant at a fixed value during a control cycle, denoted as \(\overline{A}\), \(\overline{B}\). The relationship between the position matrix and the control volume matrix is:  
\[\widetilde{X}(k)=\widetilde{A}\overline{X}(k)+\widetilde{B}\widetilde{u}(k) \tag{64}\]  
where \(\overline{X}\) is the initial condition of the moment \(k\), i.e., the current state deviation of the robot. \(\widetilde{A}\) and \(\widetilde{B}\) can be expressed as follows:  
\[\widetilde{A}=\begin{pmatrix}\overline{A}\\ \overline{A}^{2}\\ \vdots\\ \overline{A}^{N}\end{pmatrix},\widetilde{B}=\begin{pmatrix}\overline{B}&0& \cdots&0\\ \overline{A}\overline{B}&\overline{B}&\cdots&0\\ \vdots&\vdots&\ddots&\vdots\\ \overline{A}^{N-1}\overline{B}&\overline{A}^{N-2}\overline{B}&\ldots& \overline{B}\end{pmatrix} \tag{65}\]  
Bringing Equation (64) into (62) gives the cost function as:  
\[J(k)=\frac{1}{2}\widetilde{u}(k)^{T}H\widetilde{u}(k)+C^{T}\widetilde{u}(k)+D \tag{66}\]  
where:  
\[\begin{array}{l}H=2(\widetilde{B}^{T}\overline{Q}\widetilde{B}+\overline{R}) \\ C=2(\widetilde{B}^{T}\overline{Q}\widetilde{A}^{T}\overline{X}(k))\\ D=\overline{X}(k)^{T}\widetilde{A}^{T}\overline{Q}\widetilde{A}\overline{X}(k) \end{array} \tag{67}\]  
\(D\) is only related to the current actual state error, which is a known quantity. Equation (66) is the standard quadratic programming form on the \(\widetilde{u}(k)\).  
In the real system, the control quantity will be subject to the actual physical limitations and the existence of upper and lower limits, or the user will want to set the control quantity that must be within a range. So, it is necessary to give some constraints on the control quantity in the optimization solution process, which can be expressed in the following form:  
\[\begin{array}{l}\widetilde{u}_{\min}\leq\widetilde{u}(k)\leq\widetilde{u}_{ \max}\\ u_{\min}\leq u(k)\leq u_{\max}\end{array} \tag{68}\]  
where \(u(k)\) and \(\widetilde{u}(k)\) are the control volume matrix and the control volume error matrix, respectively. The optimal control quantity is then obtained by solving this quadratic programming problem with constraints.