# Motion Planning for Mobile Robot with Modified BIT+
## 3 MPC Method
### Implementation of MPC  
The global planner gets a series of path points. In order to make the trajectory of the robot more smoother and calculate the tracking error more conveniently, we can preprocess these path points. A feasible method is to fit the path points with the cubic polynomial curve. It should be noted that it is not necessary to fit all the path points, but to fit the path points within a certain distance in front of the robot, and then we can transform these reference path points to the robot coordinate system \(X_{r}\)-\(Y_{r}\) which can be determined by the current state \((x_{c},y_{c},\theta_{c})\)--the current position and yaw angle in global coordinate system. For example, a reference path point \((x_{g},y_{g})\) in the global coordinate system can be transformed to \((x,y)\) in the robot coordinate system with following equations:  
\[ \begin{split} x&=(x_{g}-x_{c})*\cos(\theta_{c})+(y_{g}- y_{c})*\sin(\theta_{c})\\ y&=(y_{g}-y_{c})*\cos(\theta_{c})-(x_{g}-x_{c})*\sin( \theta_{c})\end{split}  \tag{7}\]  
Then, we use cubic polynomial curves to fit these path points after the transformation to the robot coordinate system  
\[f(x)=m_{0}+m_{1}x+m_{2}x^{2}+m_{3}x^{3} \tag{8}\]  
\(m_{0}\), \(m_{1}\), \(m_{2}\) and \(m_{3}\) are the coefficients of the polynomial, and they will be calculated online as the algorithm runs. In this way, we can use the analytic form of path \(f(x)\) to easily calculate the tracking error and yaw error. An important task of MPC is to reduce tracking error. In Figure 6, the red dotted line denotes the local reference path \(f(x)\). The tangent direction of \(f(x)\) represents the desired yaw angle in \(X_{r}\)-\(Y_{r}\). \((x_{r}(k)\), \(y_{r}(k))\) is the initial position in \(X_{r}\)-\(Y_{r}\), which is actually (0, 0). \(x_{r}(k+i)\), \(y_{r}(k+i)\) and \(\theta_{r}(k+i)\) represent the predicted position and yaw angle respectively in \(X_{r}\)-\(Y_{r}\) at time step \(k+i\). As is shown in the figure, we can define the tracking error and yaw error at discrete time step \(k+i\):  
\[ \begin{split} e(k+i)=f(x_{r}(k+i))-y_{r}(k+i)\\ e_{\theta}(k+i)=arctan(f^{\prime}(x_{r}(k+i)))-\theta_{r}(k+i) \end{split}  \tag{9}\]The objective function is crucial for the performance of mobile robot. In our implementation, we choose the form of objective function as follows:  
\[ \begin{split} min\quad J=&\sum_{i=1}^{N_{p}}a_{1}e^{2} (k+i)+\sum_{i=1}^{N_{p}}a_{2}e_{\theta}^{2}(k+i)+\sum_{i=1}^{N_{e}}\Delta u^{T} (k+i)A_{3}\Delta u(k+i)\\ &+\sum_{i=1}^{N_{e}}a_{4}[\cos(\theta_{1}-\omega(k+i)\Delta T)/g( d_{1})+\cos(\theta_{2}-\omega(k+i)\Delta T)/g(d_{2})]\\ &+\sum_{i=1}^{N_{p}}a_{5}(v(k+i)-v_{d})^{2}+\sum_{i=1}^{N_{e}}a_{ 6}v(k+i)/g(min(d_{1},d_{2}))\\ \end{split}  \tag{10}\]  
\[ \begin{split} s.t\quad 0&\leq v(k+i)\leq v_{max} \\ &-\omega_{max}\leq\omega(k+i)\leq\omega_{max}\\ &-a_{max}\Delta T\leq\Delta v(k+i)\leq a_{max}\Delta T\\ &-a_{max}\Delta T\leq\Delta\omega(k+i)\leq a_{max}\Delta T\\ \end{split}  \tag{11}\]  
In Equation (10), \(N_{p}\) denotes the prediction horizon, and \(N_{e}\) is the execute horizon. The first and second terms is to penalize the tracking error and yaw error. The robot should follow the reference trajectory generated by the global planner with small error, and the heading angle should fit the tangent direction of the trajectory. \(a_{1}\) and \(a_{2}\) are are two adjustable constant parameters. In the third term of Equation (10), \(\Delta u(k+i)\) represents the change of control input (linear velocity and angular velocity) at two adjacent moments and \(\Delta u(k+i)=u(k+i)-u(k+i-1)\). The purpose of this term is to penalize the drastic variation of control input, so that the robot can move steady. \(A_{3}\) is a \(2\times 2\) constant diagonal matrix which can be set. The fourth term is the repulsive function based on the direction of the obstacles. In our simulation, we do collision checking according to the cost map. Where there is an obstacle in the map, there will be a high \"cost value\".