# 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  
### Mathematical Model of USV Plane Motion  
The mathematical model of the Lanxin USV plane motion is described by the nonlinear equations of motion for a three-degree of freedom (3-DOF) planar model about a surface vessel, where the motions in heave, roll and pitch are neglected [25]. The kinematics of the vessel can be written as:  
\[\left\{\begin{array}{l}\dot{x}=u\cos\psi-v\sin\psi\\ \dot{y}=u\sin\psi+v\cos\psi\\ \dot{\psi}=r\end{array}\right., \tag{1}\]  
where \((x,y)\) denote the position of the center of mass, \(\psi\) is the course angle of the vessel in the inertial reference frame, \((u,v)\) are the surge and sway velocities, respectively, and \(r\) denotes the yaw rate of the vessel in the body-fixed frame.  
Assume that:  
* The sea environment is calm, so the disturbances caused by environmental factors such as wind, wave and current are neglected;
* The USV is symmetrical, and its barycenter coincides with the geometric center, that is the inertia, added mass and damping matrices are diagonal.  
In the process of collision avoidance, the propulsion angle \(\delta\) sometimes requires a larger value, so the influence of sway thrust \(\tau_{v}\) should be considered [26]. The simplified dynamic model of the surface vessel is used to describe [27; 28]:  
\[\left\{\begin{array}{l}\dot{u}=\frac{m_{22}}{m_{11}}vr-\frac{d_{11}}{m_{11}} u+\frac{1}{m_{11}}\tau_{u}\\ \dot{v}=-\frac{m_{11}}{2m}ur-\frac{d_{22}}{m_{22}}v+\frac{1}{m_{22}}\tau_{v}\\ \dot{r}=\frac{m_{11}-m_{22}}{m_{33}}uv-\frac{d_{33}}{m_{33}}r+\frac{1}{m_{33}} \tau_{r}\end{array}\right., \tag{2}\]  
where \(m_{ii}\) are given by the vessel inertia and the added mass effects, \(d_{ii}\) are given by the hydrodynamic damping and \((\tau_{u},\tau_{v},\tau_{r})\) are the surge thrust, sway thrust and yaw moment.  
Additionally [26; 29]:  
\[\left\{\begin{array}{l}\tau_{u}=T\cos(\delta)\\ \tau_{v}=T\sin(\delta)\\ \tau_{r}=T\sin(\delta)L/2\end{array}\right. \tag{3}\] \[T=\left(1-t_{p}\right)\rho t^{2}D_{p}^{4}K_{T}\]where \(\delta\) is the propulsion angle, \(T\) is the thrust produced by the thruster and \(L\) is the vessel length. In the calculation of thrust, \(t_{p}\), \(\rho\), \(n\), \(D_{p}\) and \(K_{T}\) are the reduction coefficient, sea water density, thruster speed, propeller diameter and thrust coefficient, and other quantities except thruster speed \(n\) are constant. Therefore, the \(\delta\) and \(n\) are the inputs of the model.  
In general, the response model of the steering process is considered as a first-order inertia link [29]. However, the thruster steering of the surface vessel is completed by the stern control system with high sensitivity and quick action. After analyzing the actual data collected from the experimental process, the steering gear response model can be regarded as a second-order system [30].  
\[\ddot{\delta}+2\zeta\omega_{n}\dot{\delta}+\omega_{n}^{2}\delta=K\omega_{n}^{ 2}\delta_{r} \tag{4}\]  
Here, \((\omega_{n},\zeta,K)\) are the natural frequency, damping ratio and proportionality coefficient of the second-order system, \(\delta_{r}\) is the command propulsion angle and \(|\delta|\leq 30^{\circ}\) is the propulsion angle range.