# Three-Dimensional Path-Following with Articulated 6 DoF Robot and ToF Sensors
## 3 Results
### Algorithm Implementation, Constraints, and Supporting Solutions  
The path-following procedure schema is presented in Figure 10. The process starts with the initialisation of the communication between the program running on the PC in Matlab and the robot controller as well as the detection tool. Once all hardware elements are connected, some initial moves of the robot are performed. These are detection tool pick-up and moving to the start position located over the path. Then, in the loop, for each stage the following steps are repeated for a given number of iterations:  
* Read sensor data and interpreting these data: The measurement field of the detection tool is represented as a 3 \(\times\) 3 array (sensor array). Path detection is possible at nine points that are located at the intersections of the sensors' line of sight. Each of the points is represented by one element of the sensor array. The matrix rows represent the sensors located in the left arm of the path  
Figure 9: General software architecture of the presented system. detection tool (sensors 1-3) and the columns represent the sensors in the right arm (sensors 4-6). The path is marked as detected at a given point when its presence is signalled by two sensors whose fields intersect in this point. Additionally, if the path is detected too close or too far from the sensor it is marked as a detection error to prevent unambiguous data input to the path-following algorithm. A detection error is not treated as a wrong readout. In such a case, the algorithm continues as if the path was not detected in this step and calculates the next step based on the last successful readout (the \(O^{\prime}\) point of the last step when the path was detected is used). * Read the current position of the gripper from the robot. * Calculate the effector's (detection tool) current position and orientation based on the position of the gripper using Equations (23)-(25). * Perform path-following algorithm step: Calculate the effector's new position based on its current and previous positions and detected path position. First, the detected path position is calculated using Equations (1)-(8). Then, the next tool position and rotation is calculated using Equations (9)-(14) and (15)-(22). It must be noted here that according to the detection condition, a \"long\" or \"short\" step is performed (according to the value of the variable \(d\) in Equations (13) and (14)). A \"short\" step is performed when the path is detected only in the central point of the detection field or when it is not detected by any sensor. In all other cases, a \"long\" movement is performed. This is to prevent an insufficient correction of the detection tool position when the path trajectory bends or losing the path when moving straight ahead. Moreover, in some cases, due to the algorithm's response to detecting a path in a position other than the centre of the detection field, the calculated tool rotation may have a high angle value (see Section 2.4). This has a negative impact on the smoothness of the motion along the path and may also cause the loss of path detection. Rotation reduction is accomplished by reducing the length of the \(\overrightarrow{m}\) vector. It is multiplied by a constant parameter (named \(b\) in Equation (17)), which should have a value greater than 0 and less than 1. This reduction may, however, cause the situation where the tool rotates by a smaller angle than is needed. Fortunately, in such a case the path-following algorithm will recognise in the next iteration that the path is still positioned on the edge of the detection field and, in order to follow the path, it must be rotated again. Eventually, the algorithm corrects the tool orientation in a few subsequent movement steps. * Calculate inverse kinematics (IK): The IK function calculates the articulated variables of the robot based on the detection tool position and orientation determined by the path-following algorithm.