# Three-Dimensional Path-Following with Articulated 6 DoF Robot and ToF Sensors
## 2 Materials and Methods
### Detected Path Position Calculation  
Figure 3 presents an exemplary path detection tool with 6 sensor placements and the detection points of the path detection tool marked. The current tool position is always known, i.e., it is calculated based on the gripper global position returned by the robot controller. The centre of the tool's coordinate system (\(O\), tool position) is located in the middle of the detection field. It lies at the intersection of the lines of sight associated with the two central sensors (No. 2 and 5 in Figure 3). The \(Y\) and \(Z\) axes lie in the tool plane, while the \(Z\) axis is perpendicular to this plane. The sensor spacing is \(a\). Each of the intersection points of the sensors' lines of sight in the global system can be determined by knowing the position of the tool and its orientation in the form of Euler angles. Euler angles determine the orientation of the local tool coordinate system relative  
Figure 3: Path detection tool schema showing ToF sensors (1–6) placement, sensor separation \(a\), and locations of detection points (purple circles), e.g., point A is the detection point at the intersection of the lines of sight of sensors 2 and 4. Figure 2: Robot holding path detection tool during the path-following. to the global system (located in the robot base). The first stage of the process of calculating these points is to determine the rotation matrix of the local tool system:  
\[R_{x}(\alpha)= \begin{bmatrix}1&0&0\\ 0&\cos\alpha&-\sin\alpha\\ 0&\sin\alpha&\cos\alpha\end{bmatrix} , \tag{1}\]  
\[R_{y}(\beta)= \begin{bmatrix}\cos\beta&0&\sin\beta\\ 0&1&0\\ -\sin\beta&0&\cos\beta\end{bmatrix} , \tag{2}\]  
\[R_{z}(\gamma)= \begin{bmatrix}\cos\gamma&-\sin\gamma&0\\ \sin\gamma&\cos\gamma&0\\ 0&0&1\end{bmatrix} , \tag{3}\]  
\[R=R_{z}(\gamma)\cdot R_{y}(\beta)\cdot R_{x}(\alpha), \tag{4}\]  
where \(\alpha\)--rotation angle around the \(X\) axis (roll); \(\beta\)--rotation angle around the \(Y\) axis (pitch); \(\gamma\)--the angle of rotation around the \(Z\) axis (yaw); \(R_{x},R_{y},R_{z}\)--rotation matrices around individual axes, respectively, \(X\), \(Y\), \(Z\); and \(R\)--tool rotation matrix. The position of a point in the global coordinate system, knowing its position in the local coordinate system, can be determined using the following formula:  
\[P=P_{0}+R\cdot\overrightarrow{u}, \tag{5}\]  
where \(P\)--the desired point position in the global system; \(P_{0}\)--the centre of the local coordinate system; and \(\overrightarrow{u}\)--the vector of the position in the local coordinate system (the vector between the centre of the local coordinate system and the position of the desired point in this system). For example, the position of the point \(A\) in the global coordinate system (Figure 3) is  
\[A=O+R\cdot \begin{bmatrix}0&\frac{\sqrt{2}}{2}&\frac{\sqrt{2}}{2}\end{bmatrix}  ^{T}\cdot a. \tag{6}\]  
The general formula for calculating detection point positions (intersections of sensors' lines of sight) is  
\[P_{i}=O+R\cdot\vartheta\cdot l. \tag{7}\]  
where \(P_{i}\)--the intersection of sensors' lines of sight in the coordinate system of the robot base; \(\vartheta\)--unit vector; and \(l\)--the distance between the centre of the local coordinate system and the desired point \(i\). The next step is the calculation of the middle point based on the set of points where the path was detected, because it is possible that the path is detected in more than one point simultaneously. This is calculated according to the following formula:  
\[S=\left(\frac{1}{n}{\sum}_{i=1}^{n}x_{i},\frac{1}{n}{\sum}_{i=1}^{n}y_{i}, \frac{1}{n}{\sum}_{i=1}^{n}z_{i}\right), \tag{8}\]  
where \(S\)--the midpoint, i.e., the point where the followed path is assumed to be; \(x_{i},y_{i},z_{i}\)--coordinates of points where the path was detected; and \(n\)--the number of points where the detection occurred. In some cases, it is also possible that the path is not detected at all, for example, when the path is located between the sensors' lines of sight.