# Three-Dimensional Path-Following with Articulated 6 DoF Robot and ToF Sensors
## 2 Materials and Methods
### Calculation of the Next Tool Position  
To determine the next position where the detection tool should be moved by the robot, it is necessary to calculate the appropriate vector (Figure 4). In the previous section, how to designate the \(S\) point that represents the detected path position was presented. Knowing the position of this point, the next step is to determine the vector between this point and the position of the detection tool (point \(O\)).  
\[\overrightarrow{m}=\overrightarrow{OS}=\left[x_{S}-x_{O}\quad y_{S}-y_{O} \quad z_{S}-z_{O}\right], \tag{9}\]  
where \(x_{S},y_{S},z_{S}\)--point \(S\) coordinates; \(x_{O},y_{O},z_{O}\)--point \(O\) coordinates.  
Additionally, the tool displacement (\(\overrightarrow{t}\) vector) from the position where the path was detected for the last time (point \(O\)) must be calculated:  
\[\overrightarrow{t}=\overrightarrow{O^{\prime}O}. \tag{10}\]  
It must be noted that the \(O^{\prime}\) position is not always the last (previous) tool position. This happens when the path is not detected by the tool. In this case, it is assumed that the path is located between the sensor's lines of sight, and it should be detected again in one of the next moves, so the tool is moved ahead in the previously calculated direction but the \(O^{\prime}\) point used in (10) is still the one obtained when the path was detected the last time. Apart from handling temporary path loss, this also helps to react appropriately to path bends, depending on how sharp they are.  
After summing up both vectors \(\overrightarrow{t}\) and \(\overrightarrow{m}\), the resultant vector \(\overrightarrow{w}\) is obtained (11), which is the vector needed to determine the next tool position. Then, the vector \(\overrightarrow{w}\) must be normalised (12). The resulting unit vector allows the determination of the new position shifted by a given distance \(d\) (Figure 4).  
\[\overrightarrow{w}=\overrightarrow{O^{\prime}S}=\overrightarrow{t}+ \overrightarrow{m}, \tag{11}\]  
\[\hat{w}=\frac{\overrightarrow{w}}{\left\|\overrightarrow{w}\right\|}=\left[ \frac{w_{x}}{\sqrt{w_{x}^{2}+w_{y}^{2}+w_{z}^{2}}}\quad\frac{w_{y}}{\sqrt{w_ {x}^{2}+w_{y}^{2}+w_{z}^{2}}}\quad\frac{w_{z}}{\sqrt{w_{x}^{2}+w_{y}^{2}+w_{z }^{2}}}\right], \tag{12}\]  
\[P_{n}=O+\hat{w}\cdot d, \tag{13}\]  
where \(\overrightarrow{w}\)--vector between \(O^{\prime}\) and \(S\) points; \(\hat{w}\)--unit vector; \(d\)--tool displacement distance; and \(P_{n}\)--next tool position.  
Figure 5 presents the general scheme of the path-following, showing 3 consecutive positions of the path detection tool.  
An exception to the rules described above is the situation when the midpoint \(S\) is placed in the tool centre (point \(O\)). In this case, the next position is calculated based on the vector normal to the tool plane (tangent to the \(X\) axis of the tool).  
\[P_{n}=O+R\cdot\left[1\quad 0\quad 0\right]\cdot d. \tag{14}\]  
This is caused by the fact that tool rotation is calculated separately (which is described in the next section), and even though the path is in the tool centre, the \(\overleftarrow{w}\) vector and normal vector may differ.