# Nonlinear Model Predictive Control with Terminal Cost for Autonomous Vehicles Trajectory Follow
## 3 Controller Design and Numerical Simulation Verification  
### Controller Design  
Usually, the NMPC controller converts the path tracking problem into a constrained NLP (Nonlinear Program) problem in the finite time domain. The problem is transformed as follows:  
\[\text{min}J=\sum\limits_{i=0}^{N_{p}}\Big{(}Z_{i}-z_{ref,i}\Big{)}^{T}Q\Big{(} Z_{i}-z_{ref,i}\Big{)}+u_{i}^{T}Ru_{i} \tag{6}\]  
\[s.t.\ Z_{0}=Z_{t}\]  
\[Z_{i+1}=f(Z_{i},u_{i}),i=0,1,2,3\dots\dots.N_{p}\]  
\[u_{min}\leq u_{i}\leq u_{max},i=0,1,2,3\dots\dots.N_{p}\]  
\[\dot{u}_{min}\leq(u_{i+1}-u_{i})/T\leq\dot{u}_{max},i=0,1,2,3\dots\dots.N_{p}\]  
where \(J\) is the cost function; \(u_{min},\ u_{max},\dot{u}_{min},\dot{u}_{max}\) are the speed, angle, acceleration, and front wheel angle rate constraints; \(Z_{i+1}=f(Z_{i},u_{i})\) is the differential equation constraint; and the matrix \(Q\) and the matrix \(R\) are diagonal matrices with weights greater than 0, respectively. In this article, \(Q\) = diag(10,10,10), \(R\) = diag(1,0.1). In order to achieve a high-precision tracking and planning path of the vehicle and make the controller have high control stability, we propose NMPC with terminal cost, so that the vehicle can better converge to the reference path. Its cost function format is as follows:  
\[J=J_{N_{p}}+\sum\limits_{i=0}^{N_{p}-1}\left[\Big{(}Z_{i}-z_{ref,i}\Big{)}^{T }Q\Big{(}Z_{i}-z_{ref,i}\Big{)}+u_{i}^{T}Ru_{i}\right] \tag{7}\]  
\[J_{N_{p}}=\alpha*\ \Big{(}Z_{N_{p}}-z_{ref,N_{p}}\Big{)}^{T}Q\Big{(}Z_{N_{p}}-z_{ ref,N_{p}}\Big{)}+u_{N_{p}}^{T}Ru_{N_{p}} \tag{8}\]  
\[s.t.\ Z_{0}=Z_{t}\]  
\[Z_{i+1}=f(Z_{i},u_{i}),i=0,1,2,3\dots\dots.N_{p}\]  
\[u_{min}\leq u_{i}\leq u_{max},i=0,1,2,3\dots\dots.N_{p}\]  
\[\dot{u}_{min}\leq(u_{i+1}-u_{i})/T\leq\dot{u}_{max},i=0,1,2,3\dots\dots.N_{p}\]  
In the Equation (9), \(\alpha\) is the terminal cost the solution of the above equation in each control cycle, a series of control inputs in the prediction time domain are obtained:  
\[u^{*}=\left[u_{0},u_{1},u_{2},u_{3},\ldots.u_{N_{p}-1}\right]^{T} \tag{9}\]  
If the control horizon of the controller is 1, then \(u_{0}\) will be taken as the input of the controller at the next moment, and the input of the controller will be recalculated at the next moment to update \(u^{*}\). If the control horizon of the controller is 3, then the input the controller will take will be: \([u_{0},u_{1},u_{2}]\), the controller will update \(u^{*}\) after 3 Ts. The control horizon is denoted as \(N_{c}\).  
In the vehicle routing control system, \(u=\left[v,\delta_{f}\right]\), then the above formula can be written as:  
\[u^{*}=\left[\left[v_{0},\delta_{f_{0}}\right],\left[v_{1},\delta_{f_{1}} \right]\ldots\ldots\left[v_{N_{p}-1},\delta_{f_{N_{p}-1}}\right]\right]^{T} \tag{10}\]  
In engineering practice, if the MCU (Microcontroller Unit) has enough computing power, we can choose the larger predictive horizon \(N_{p}\) (such as \(N_{p}=60\)) and the smaller control horizon \(N_{c}\) according to the study [21]. The larger the value of the predictive horizon, the more stable and accurate the value solved by MPC is. What' more, the value range of \(N_{p}\) can usually be between 10-20 to balance the calculation time and calculation accuracy, as noted in these studies [22; 23]. According to the basic principles of model predictive control, the first element \(u_{0}\) in the control sequence is used as the actual control quantity to act on the system, namely  
\[u_{0}=\left[v_{0},\delta_{f_{0}}\right]^{T} \tag{11}\]  
After entering the next control cycle, the above process is repeated, and the trajectory tracking control of the vehicle is realized in this cycle.