# A Method for Supervisory Control of Manipulator of Underwater Vehicle
## 4 Building of Motion Trajectories for a MM Working Tool in Case of Rough Bottom Topography  
If the bottom surface has a rough topography, then it is necessary to use 3D multibeam sonars [35,36], which provide a high accuracy for formation of point clouds on the bottom surface in the area of the UUV operation. These clouds and other known methods are used [31,37] to build a triangulation model (see Figure 4), which is a multitude of triangular shapes stitched together. The trajectory of the MM working tool, passing through the target points \(A_{i}\) set by the operator on the rough seabed surface, is formed on the basis of this model. Each \(k\)th triangle in the triangulation model is set in the CS \(xyz\) as coordinates of three vertices \(V_{k0}\), \(V_{k1}\), \(V_{k2}\in R^{3}\), where \(k=\overline{1,g}\) and \(g\) is the number of triangles. To find the coordinates of each target point \(A_{i}\) at the intersection of the camera optical axis and the obtained triangulation surface in this CS, first, the triangle currently intersected by the axis (Equation (2)) should be found among all \(g\) triangles. For this, the  
Figure 4: A triangulation model of a seabed surface. Moller-Trumbore algorithm [38] is used. Based on the coordinates of the point \(P_{i}\), the vector \(\overset{\rightarrow}{d}_{i}\), and the vertices \(V_{k0}\), \(V_{k1}\), \(V_{k2}\) of all \(g\) triangles, this algorithm identifies the \(k\)th triangle through which the axis (Equation (2)) passes. In this case, the coordinates of the point \(A_{i}\) in the specified CS relative to the vertices of the \(k\)th triangle in vector form can be determined as follows:  
\[A_{ik}=w_{ik}V_{k0}+u_{ik}V_{k1}+v_{ik}V_{k2} \tag{6}\]  
where \(u_{ik},v_{ik},w_{ik}\) are the barycentric coordinates of the point \(A_{i}\) relative to the vertices of the \(k\)th triangle (\(u_{ik}+v_{ik}+w_{ik}=1\)). By expressing \(w_{ik}=1-(u_{ik}+v_{ik})\) and substituting the equation of the axis (Equation (2)) in Equation (4), we obtain a system of equations with respect to the parameters \(t_{ik},\ u_{ik},\ v_{ik}\):  
\[P_{i}+t_{ik}\overset{\rightarrow}{d}_{i}=(1-(u_{ik}+v_{ik}))V_{k0}+u_{ik}V_{k 1}+v_{ik}V_{k2} \tag{7}\]  
which can be expressed in the matrix form:  
\[\left[ \begin{array}{cc}-\overset{\rightarrow}{d}_{i}&V_{k1}-V_{k0}&V_{k2}- V_{k0}\end{array} \right]\left[ \begin{array}{c}t_{ik}\\ u_{ik}\\ v_{ik}\end{array} \right]=[P_{i}-V_{k0}] \tag{8}\]  
Values of the parameters \(t_{ik},\ u_{ik},\ v_{ik}\) are obtained by solving Equation (5) [38]:  
\[\left[ \begin{array}{c}t_{ik}\\ u_{ik}\\ v_{ik}\end{array} \right]=\frac{1}{(\overset{\rightarrow}{d}_{i}\times(V_{k2}- V_{k0}))\cdot(V_{k1}-V_{k0})}\left[ \begin{array}{c}((P_{i}-V_{k0})\times(V_{k1}-V_{k0})) \cdot(V_{k2}-V_{k0})\\ (\overset{\rightarrow}{d}_{i}\times(V_{k2}-V_{k0}))\cdot(P_{i}-V_{k0})\\ ((P_{i}-V_{k0})\times(V_{k1}-V_{k0}))\cdot\overset{\rightarrow}{d}_{i}\end{array}  \right] \tag{9}\]  
where \((\cdot)\) is the dot product of vectors, and \((\times)\) is the vector product of vectors. The triangle \(k\) with the vertices \(V_{k0},\ v_{k1},\ V_{k2}\), which intersects the axis (Equation (2)), is found with the following conditions [38]:  
\[0\leq u_{ik}\leq 1 \tag{10}\]  
\[0\leq v_{ik}\leq 1 \tag{11}\]  
\[u_{ik}+v_{ik}\leq 1 \tag{12}\]  
and the coordinates of the point \(A_{i}\) of intersection of the camera optical axis with the \(k\)th triangle are determined by substituting the found parameter \(t_{ik}\) from Equation (6) in Equation (2):  
\[A_{i}=P_{i}+\frac{((P_{i}-V_{k0})\times(V_{k1}-V_{k0}))\cdot(V_{k2}-V_{k0})}{( \overset{\rightarrow}{d}_{i}\times(V_{k2}-V_{k0}))\cdot(V_{k1}-V_{k0})} \overset{\rightarrow}{d}_{i} \tag{13}\]  
For the subsequent building of the motion trajectory for the MM working tool passing through all the obtained points \(A_{i}\), this trajectory is represented as a sequence of transects of the triangulation surface, which are the vertical profiles of its parts between neighboring target points.