# Research on Obstacle Avoidance Replanning and Trajectory Tracking Control Driverless Ferry Vehicles
## 4 Obstacle Avoidance Algorithm and Transverse Controller Design
### Design of MPC Controller with Planning Module
#### 4.2.5 Objective-Function Design  
In the solution, the goal function, which is as follows, must be used to determine how to optimize the system state quantity and control quantity.  
\[J[\xi(t),u(t-1),\Delta U(t)]=\sum_{i=1}^{N}\|\eta(k+i\mid t)-\eta_{ref}(k+i \mid t)\|_{Q}^{2}+\sum_{i=1}^{N_{C}-1}\|\Delta u(k+i\mid t)\|_{R}^{2}+\rho \epsilon^{2} \tag{38}\]  
where \(\eta(k+i\mid t)\) represents the actual output; \(\eta_{ref}(k+i\mid t)\) represents the reference output; \(N_{p}\) is the predictive time domain; \(N_{c}\) is the control time domain; \(\rho\) is the weight coefficient; and \(\varepsilon\) is the relaxation factor, which is used to avoid the collapse of the control system.  
The derivation process for the future output of the linear error model of the driverless ferry vehicle is as follows:  
Convert Formula (34) into the following form:  
\[\xi(k)=\begin{bmatrix}\chi(k)\\ u(k-1)\end{bmatrix} \tag{39}\]  
The following is the new state-space expression that is obtained:  
\[\begin{array}{c}\xi(k+1)=\widetilde{A}_{k}\xi(k)+\widetilde{B}_{k}\Delta U (k)\\ \eta(k)=\widetilde{C}_{k}\xi(k)\end{array} \tag{40}\]  
where \(\widetilde{A}_{k}=\begin{bmatrix}A_{k}&B_{k}\\ 0_{1^{*}6}&I_{1}\end{bmatrix}\), \(\widetilde{B}_{k}=\begin{bmatrix}B_{k}\\ I_{1}\end{bmatrix}\), \(\widetilde{C}_{k}=\begin{bmatrix}C_{k}&0_{2*1}\end{bmatrix}\), \(C_{k}=\begin{bmatrix}0&0&1&0&0&0\\ 0&0&0&0&1&0\end{bmatrix}\).  
After deducing as above, the system's forecast output expression looks like this:  
\[Y(t)=\psi\chi(t|t)+\Theta\Delta U(t) \tag{41}\]\[\text{In the formula, }\ Y(t)=\left(\begin{array}{c}\eta(t+1|t)\\ \eta(t+2|t)\\ \ldots\\ \eta(t+N_{p}|t)\end{array}\right),\psi=\left(\begin{array}{c}\widetilde{ \mathbb{C}}_{t,t}\widetilde{A}_{t,t}\\ \widetilde{\mathbb{C}}_{t,t}\widetilde{A}_{t,t}^{2}\\ \ldots\\ \widetilde{\mathbb{C}}_{t,t}\widetilde{A}_{t,t}^{N_{p}}\end{array}\right), \Delta U(t)=\left(\begin{array}{c}\Delta u(t|t)\\ \Delta u(t+1|t)\\ \ldots\\ \Delta u(t+N_{c}|t)\end{array}\right),\]  
\[\text{and }\Theta=\left[\begin{array}{cccc}\widetilde{\mathbb{C}}_{t,t} \widetilde{B}_{t,t}&0&0&0\\ \widetilde{\mathbb{C}}_{t,t}\widetilde{A}_{t,t}\widetilde{B}_{t,t}&\widetilde {\mathbb{C}}_{t,t}\widetilde{B}_{t,t}&0&0\\ \widetilde{\mathbb{C}}_{t,t}\widetilde{A}_{t,t}^{2}\widetilde{B}_{t,t}& \widetilde{\mathbb{C}}_{t,t}\widetilde{A}_{t,t}\widetilde{B}_{t,t}& \widetilde{\mathbb{C}}_{t,t}\widetilde{B}_{t,t}&0\\ \widetilde{\mathbb{C}}_{t,t}\widetilde{A}_{t,t}^{2}\widetilde{B}_{t,t}& \ldots&\widetilde{\mathbb{C}}_{t,t}\widetilde{B}_{t,t}\\ \widetilde{\mathbb{C}}_{t,t}\widetilde{A}_{t,t}^{N-2}\widetilde{B}_{t,t}& \ldots&\ldots\\ \widetilde{\mathbb{C}}_{t,t}\widetilde{A}_{t,t}^{N-1}\widetilde{B}_{t,t}& \widetilde{\mathbb{C}}_{t,t}\widetilde{A}_{t,t}^{N-2}\widetilde{B}_{t,t}& \ldots&\widetilde{\mathbb{C}}_{t,t}\widetilde{A}_{t,t}^{N_{p}-N_{c}-1} \widetilde{B}_{t,t}\end{array}\right].\]  
By adjusting the objective function to a quadratic function model, the following can be obtained:  
\[J(\xi(t),u(t-1),\Delta U(t))=\left[\Delta U(t)^{T},\varepsilon\right]^{I}H_{t }[\Delta U(t)^{T},\varepsilon]+G_{t}[\Delta U(t)^{T},\varepsilon]+P_{t} \tag{42}\]  
\[\text{In the formula, }H_{t}=\left[\begin{matrix}\Theta_{t}^{T}Q\Theta&0\\ 0&\rho\end{matrix}\right],G_{t}=\left[2E(t)^{T}Q\Theta_{t}\,0\right]\text{, and }P_{t}=E(t)^{T}QE(t).\]  
The following control input increments are obtained by solving the aforementioned equation for each control cycle:  
\[\Delta U_{t}=\left[\Delta u_{t},\Delta u_{t+1},\cdots,\Delta u_{t+N_{c}-1} \right]^{T} \tag{43}\]  
The control system applies the first control input increment as the real control input increment for the system in the manner described below:  
\[u(t)=u(t+1)+\Delta u_{t} \tag{44}\]  
Before the next time comes, the control system will continue to execute the control quantity, and after entering the next sampling time, repeat the above process until the whole system completes the control process.