# Three-Dimensional Path-Following with Articulated 6 DoF Robot and ToF Sensors
## 2 Materials and Methods
### Calculation of the Tool Rotation  
The detection tool detection field, which lies in the \(Y\) and \(Z\) axes (in the local tool coordinate system), should be oriented perpendicularly to the path. In this orientation, it is easiest to detect the path and to avoid tool collision with the path. Therefore, the orientation of the tool is as important as the position in the Cartesian coordinates of the tool and must be determined on-line. The adopted method of determining Euler angles, analogous to the process of determining the sequential tool positions, is based on the calculation of the vector that defines the new orientation of the \(X\) axis of the tool's coordinate system. Thank to this, two rotation angles can be defined, and the third rotation angle (rotation around the  
Figure 4: The \(\overrightarrow{\mathbf{w}}\) vector and its component vectors and location of \(P_{n}\) (next tool position) point in the global coordinate system; \(\hat{\mathbf{w}}\) is the unit vector parallel to \(\overrightarrow{\mathbf{w}}\). Other symbols: \(\overrightarrow{\mathbf{t}}\)—tool displacement vector; O—detection tool centre in the current position; \(O\)’ —detection tool centre in the previous position; and \(S\)—detected path position. Figure 5: Three consecutive positions of the detection tool during path-following. Symbols: \(O\)—detection tool centre in the current position; \(O\)’—detection tool centre in the previous position; \(S\)—detected path position for the current detection tool position; and \(Pn\)—calculated next tool centre position, \(1\ldots 6\)—ToF sensors. \(X\) axis) is imposed. This tool orientation vector differs from the vector used for position calculation because it is possible to modify its parameters. By introducing this approach, it is possible to tune the algorithm and achieve smoother path-following. To obtain the tool rotation vector, the vector normal to the plane lying in the \(Y\) and \(Z\) axes of the local tool coordinate system must be determined:  
\[\hat{\mathbf{x}}= \begin{bmatrix}1&0&0\end{bmatrix} ^{T}, \tag{15}\]  
\[\overset{\rightarrow}{\mathbf{n}}=R\cdot\hat{\mathbf{x}}, \tag{16}\]  
where \(\hat{\mathbf{x}}\)--unit vector oriented along \(X\) axis of the tool coordinate system; \(\overset{\rightarrow}{\mathbf{n}}\)--vector normal to the tool plane. Using the previously determined \(\overset{\rightarrow}{\mathbf{m}}\) vector, the tool orientation vector may be calculated as follows:  
\[\overset{\rightarrow}{\mathbf{w_{a}}}=\overset{\rightarrow}{\mathbf{n}}\cdot a+ \overset{\rightarrow}{\mathbf{m}}\cdot b, \tag{17}\]  
where \(a,b\)--vector's modification parameters, experimentally adjusted during tests; \(\overset{\rightarrow}{\mathbf{w_{a}}}= \begin{bmatrix}w_{a,x}&w_{a,y}&w_{a,z} \end{bmatrix} \)--orientation vector (Figure 6). If, in Equation (17), parameter \(a\) is set to the scalar value (i.e., length) of \(\overset{\rightarrow}{\mathbf{t}}\), the rotation is indirectly proportional to the path arc radius. When the radius is high, the path bend is small and there can be a few subsequent steps when the path is not detected by the tool. When the radius is low, the path bend is high, and the path is detected frequently. This means that normal vector length is dependent on the distance between the subsequent path detection positions. Parameter \(b\), which is manually set constant with a value below 1, acts as tool rotation reduction coefficient. This prevents too high tool rotations when the path is detected in two subsequent positions in different detection tool points. In such a situation, the \(\overset{\rightarrow}{\mathbf{m}}\) vector may be much longer than vector \(\overset{\rightarrow}{\mathbf{t}}\). As a result, the orientation vector may be oriented almost parallelly to the detection tool plane, i.e., it indicates very high rotation. This may cause the collision of the detection tool with the path and also results in high oscillations of the detection tool around the followed path.