# Collision Avoidance Using Finite Control Set Model Predictive Control for Unmanned Surface Vehicle
## 3 Collision Avoidance Based on the Finite Control Set Model Predictive Control Method
### Evaluation Function  
According to the four requirements (attainability, safety, stability and rapidity) of the CAS, the paper proposes the evaluation function \(f_{ei}(X_{pi},S_{i})\) of CAS, where some sub-functions, i.e., \(f_{1}(X_{pi},S_{i})\), \(f_{2}(X_{pi},S_{i})\), \(f_{3}(X_{pi},S_{i})\) and \(f_{4}(X_{pi},S_{i})\), correspond to the requirements, respectively. Considering that the independent variables in evaluation function \(f_{e}\) are discrete variables, this situation will lead to a discontinuous evaluation function, and some sub-functions in the evaluation function are dominant. Hence, the normalized method is used to eliminate this problem, e.g., the first normalized sub-function is:  
\[\tilde{f_{1}}(X_{pi},S_{i})=\frac{f_{1}(X_{pi},S_{i})}{\sum_{i=1}^{c}f_{1}(X_{pi },S_{i})}. \tag{7}\]  
Moreover, with the importance of the CAS requirements, different weights \((\alpha,\beta,\gamma,\lambda)\) are assigned to each evaluation sub-function. The \(i\)-th evaluation function is:  
\[f_{ei}(X_{pi},S_{i})=\alpha\cdot\tilde{f_{1}}(X_{pi},S_{i})+\beta\cdot\tilde{ f_{2}}(X_{pi},S_{i})+\gamma\cdot\tilde{f_{3}}(X_{pi},S_{i})+\lambda\cdot \tilde{f_{4}}(X_{pi},S_{i}). \tag{8}\]  
CAS chooses the control command \(S_{o}\), which can obtain the maximum value of the evaluation function. The sub-functions are defined as follows:  
* Attainability Sub-function \(f_{1}(X_{pi},S_{i})\)  
The first requirement of CAS is to complete the task, i.e., one or more target points from the task layer. If the course keeps pointing to the target point, which is called the navigation angle, the vessel will eventually arrive at the target point with the shortest distance. Thus, we can use the difference angle \(\theta_{pi}\) between the navigation angle and the predicted course \(\psi_{pi}\) to evaluate the attainability of \(X_{pi}\) which is illustrated in Figure 4.  
However, with the increase of difference angle \(\theta_{pi}\), the attainability will be reduced. This is contrary to the optimal value of the evaluation function, that is the \(\theta_{pi}\) can only describe the attainability indirectly. The attainability sub-function is:  
\[f_{1}(X_{pi},S_{i})=\pi-\theta_{pi}. \tag{9}\]  
\(\bullet\)Safety sub-function \(f_{2}(X_{pi},S_{i})\):  
In order to ensure the safety of the system, the vessel runs away from all obstacles in the surrounding environment as far as possible. The distance between the vessel and the nearest obstacle can describe the security of the USV. The safety sub-function \(f_{2}(X_{pi},S_{i})\) is:  
\[ \begin{array}{c}{do_{i}}^{k}=\left|X_{pi}-X{o_{k}}\right|\\ f_{2}(X_{pi},S_{i})=\min_{k=1}^{z}\left\{{do_{i}}^{k}\right\},\end{array}  \tag{10}\]  
where \({do_{i}}^{k}\) is the distance between predicted USV state \(X_{pi}\) and the \(k\)-th obstruction \(X{o_{k}}\), \(z\) is the number of obstructions within \({do_{i}}^{k}\leq d_{r}\) and \(d_{r}\) is the distance range that takes security into account. If \({do_{i}}^{k}>d_{r}\), \({do_{i}}^{k}\) is considered equal to \(d_{r}\). Moreover, considering the motion characteristics of the vessel, stopping distance \(d_{stop}\) should also be considered, and it is the process of slowing down from the current speed \(V\) to stopping according to the acceleration \(\dot{V}\). The distance is calculated by:  
\[d_{stop}^{i}={V_{pi}}^{2}/(2\cdot\dot{V}_{pi}). \tag{11}\]  
Therefore, when \(d_{stop}^{i}>f_{2}(X_{pi},S_{i})\), the \(i\)-th control behavior \(S_{i}\) can be excluded. \(\bullet\)Stability sub-function \(f_{3}(X_{pi},S_{i})\):  
Because the instability of the vessel is mainly caused by the sharp alteration of course, it is necessary to avoid the occurrence of a large propulsion angle \(\delta\), that is the stability sub-function is:  
\[f_{3}(X_{pi},S_{i})=\delta_{\rm m}-\left|\delta_{i}\right|. \tag{12}\]  
\(\bullet\)Rapidity sub-function \(f_{4}(X_{pi},S_{i})\)  
Figure 4: Description attainability sub-function.