# Fuzzy Risk Evaluation and Collision Avoidance Control of Unmanned Surface Vessels
## 3 Design of Smart Collision Avoidance Control System for USVs
### Generation of Collision Advoidance Course
#### 3.3.3 Optimal Collision Free Trajectory
\right]^{T} \tag{35}\]  
In \(\mathbf{g}\) and \(\mathbf{h}\), the initial waypoints are defined as \(x_{S}\in\{x^{\prime},x^{\prime\prime}{}_{1}\}\) and \(y_{S}\in\{y^{\prime}{}_{1},y^{\prime\prime}{}_{1}\}\), and the end waypoints can be defined by the velocity and acceleration as \(x_{E}\in\{x^{\prime}{}_{n},x^{\prime\prime}{}_{n}\}\) and \(y_{E}\in\{y^{\prime}{}_{n},y^{\prime\prime}{}_{n}\}\). \[\mathrm{A}(\varpi_{1},...,\varpi_{n})=\left[ \begin{array}{cccccccc}c_{S}&0 _{1\times 4}&0_{1\times 4}&\cdots&0_{1\times 4}\\ p(\varpi_{1})&0_{1\times 4}&0_{1\times 4}&\cdots&0_{1\times 4}\\ p(\varpi_{2})&0_{1\times 4}&0_{1\times 4}&\cdots&0_{1\times 4}\\ 0_{1\times 4}&p(\varpi_{2})&0_{1\times 4}&\cdots&0_{1\times 4}\\ -v(\varpi_{2})&v(\varpi_{2})&0_{1\times 4}&\cdots&0_{1\times 4}\\ -a(\varpi_{2})&a(\varpi_{2})&0_{1\times 4}&\cdots&0_{1\times 4}\\ 0_{1\times 4}&p(\varpi_{3})&0_{1\times 4}&\cdots&0_{1\times 4}\\ 0_{1\times 4}&0_{1\times 4}&p(\varpi_{3})&\cdots&0_{1\times 4}\\ 0_{1\times 4}&-v(\varpi_{3})&v(\varpi_{3})&\cdots&0_{1\times 4}\\ 0_{1\times 4}&-a(\varpi_{3})&a(\varpi_{3})&\cdots&0_{1\times 4}\\ \vdots&\vdots&\vdots&\ddots&\vdots\\ 0_{1\times 4}&0_{1\times 4}&0_{1\times 4}&\cdots&p(\varpi_{n})\\ 0_{1\times 4}&0_{1\times 4}&0_{1\times 4}&\cdots&c_{E}\end{array} \right] \tag{36}\]  
\[p(\varpi_{k})=\left[\varpi_{k}{}^{3},{\varpi_{k}}^{2},\varpi_{k},1\right] \tag{37}\]  
\[v(\varpi_{k})=p^{\prime}(\varpi_{k})=\left[3{\varpi_{k}}^{2},2\varpi_{k},1,0\right] \tag{38}\]  
\[a(\varpi_{k})=p^{\prime\prime}(\varpi_{k})=\left[6\varpi_{k},2,0,0\right] \tag{39}\]  
\[ \begin{array}{c}c_{S}\in\{x^{\prime}{}_{d}(\varpi_{0}),x^{\prime\prime}{}_{ d}(\varpi_{0})\},\\ c_{E}\in\{x^{\prime}{}_{d}(\varpi_{n}),x^{\prime\prime}{}_{d}(\varpi_{n})\} \end{array}  \tag{40}\]  
Figure 13: Schematic diagram of optimal collision free trajectory. Because \(\mathbf{A}(\varpi_{1},\,\ldots,\,\varpi_{n})\) is nonsingular, the matrix inversion can be used to find the optimal solutions of the coefficient vectors of \(\mathbf{c}\) and \(\mathbf{s}\) as (41) and (42). \[\mathbf{c}=\mathbf{A}^{-1}\mathbf{g} \tag{41}\]  
\[\mathbf{s}=\mathbf{A}^{-1}\mathbf{h} \tag{42}\]  
By using (15), (16), (30), and (31), a 2D collision avoidance trajectory can be generated with respect to the optimal collision avoidance waypoint for the controlled USV. To ensure the original trajectory and the new collision free trajectory connect to each other smoothly, the position, velocity, and acceleration of the connected point of the instantaneous waypoint at the original trajectory must be the same as those of the new no collision waypoint. As shown in Figure 13, \(\varpi_{i}\) is the connected point, the red line is the original trajectory, and the blue line is the new collision avoidance trajectory; thus, the condition that must satisfy the common waypoint of these two lines can be expressed as follows:  
\[\left\{ \begin{array}{l}v_{i}^{x}=v_{i}^{x}\\ v_{i}^{y}=v_{1}^{y}\end{array} \right. \tag{43}\]  
Equation (43) can be further presented as:  
\[\left\{ \begin{array}{l}3a_{4i}\varpi^{2}+2a_{3i}\varpi+a_{2i}=3a_{41}^{new} \varpi^{2}+2a_{31}^{new}\varpi+a_{21}^{new}\\ 3d_{4i}\varpi^{2}+2d_{3i}\varpi+d_{2i}=3d_{41}^{new}\varpi^{2}+2d_{31}^{new} \varpi+d_{21}^{new}\end{array} \right.