# Research on Trajectory Planning and Tracking Methods for Coal Mine Mobile Robots
## 3 MPC-Based Trajectory Tracking for Mobile Robots  
### Modeling of Robot Kinematics  
The design of the MPC controller system model is based on the kinematic model of the underground CMRRs. Assuming that no relative sliding between the track and the ground occurs, the robot kinematic model can be built as shown in Figure 13.  
The point \(C\) is the center point of the robot, with coordinates \((x_{m},y_{m})\), and \(\theta_{m}\) is the heading angle of the robot. \(X_{C}\) and \(Y_{C}\) are the coordinate axes of the robot's forward direction and its vertical direction, respectively. The state information of the robot at any moment is represented by \(P_{m}(x_{m},y_{m},\theta_{m})\). Then, the robot's kinematic equations can be expressed as follows:  
\[\left\{\begin{aligned} \dot{x}_{m}=v_{m}\cos\theta_{m}\\ \dot{y}_{m}=v_{m}\sin\theta_{m}\\ \dot{\theta}_{m}=\omega_{m}\end{aligned}\right. \tag{47}\]  
where \(v_{m}\) denotes the linear velocity of the robot in the axis direction \(X_{C}\). \(\omega_{m}\) denotes the angular velocity of the robot. \(x_{m}\) and \(y_{m}\) denote the velocities in the \(X\) axes and \(Y\) axes of the world coordinate system, respectively. If the state vector of the robot is denoted by \(X_{m}(x_{m},y_{m},\theta_{m})^{T}\), the kinematic equations of Equation (47) can be written in the following matrix form:  
\[\dot{X}=\begin{pmatrix}\dot{x}_{m}\\ \dot{y}_{m}\\ \dot{\theta}_{m}\end{pmatrix}=\begin{pmatrix}\cos\theta_{m}&0\\ \sin\theta_{m}&0\\ 0&1\end{pmatrix}\begin{pmatrix}v_{m}\\ \omega_{m}\end{pmatrix}=f(X_{m},u_{m}) \tag{48}\]  
where \(\dot{X}\) is the derivative of the state quantity with respect to time \(t\); \(u=\left(v_{m},\theta_{m}\right)^{T}\) is a function of time \(t\), which is the control quantity of the robot.  
Figure 12: Flow chart of planning method based on MINCO trajectory.  
Figure 13: Robot kinematics model.  
For the tracked CMRR, it is necessary to convert the control quantity \(u\) into the speed of the left and right tracks in order to finally control the robot's motion. The relationship between the robot's linear velocity, angular velocity, and the left and right track velocities during differential motion is as follows:  
\[\begin{pmatrix}v_{m}\\ \omega_{m}\end{pmatrix}=\begin{pmatrix}\frac{1}{2}&\frac{1}{2}\\ -\frac{1}{2L}&\frac{1}{2L}\end{pmatrix}\begin{pmatrix}v_{L}\\ v_{R}\end{pmatrix} \tag{49}\]  
where \(v_{L}\) and \(v_{R}\) are the left and right track speeds, respectively.  
The differential flatness of the robot kinematic model is further verified. The function \(z=\left(x_{m},y_{m}\right)^{T}\) about time \(t\) is chosen as the flat output; then, the state variables of the robot can be expressed as:  
\[\begin{pmatrix}x_{m}\\ y_{m}\\ \theta_{m}\end{pmatrix}=\begin{pmatrix}x_{m}\\ y_{m}\\ \arctan(\frac{\dot{y}_{m}}{\dot{x}_{m}})\end{pmatrix} \tag{50}\]  
The control variables of the robot can be expressed as:  
\[\begin{pmatrix}v_{m}\\ \omega_{m}\end{pmatrix}=\begin{pmatrix}\sqrt{\dot{x}_{m}^{2}+\dot{y}_{m}^{2}} \\ \frac{y_{m}\dot{x}_{m}-\dot{y}_{m}\ddot{x}_{m}}{\left(\dot{x}_{m}+\dot{y}_{m} \right)^{2}}\end{pmatrix} \tag{51}\]  
Thus, both the state and control quantities of the system can be determined by the flat output and its finite dimensional derivatives, proving the differential flatness of the robot kinematic model.