# Three-Dimensional Path-Following with Articulated 6 DoF Robot and ToF Sensors
## 3 Results  
### Experimental Setup  
In order to prove the reliability of the proposed algorithm, experimental tests were carried out using the industrial, articulated, 6 DoF robot Nachi MZ04D (Nachi Fujikoshi Corp., Tokyo, Japan) with a Nachi CFD controller [37]. The maximum load capacity of this robot is 4 kg, and the maximum reach of the flange is 541 mm. The robot is equipped with a Gimatic MPLF 2550 (Gimatic, Roncadelle, Italy) electric gripper and Schunk FT-AXIA 80(Schunk, Lauffen/Neckar, Germany) force and torque sensor, which was installed on the robot but was not used during this research. The path to follow was set up using a shower hose (20 mm diameter) with a 3 mm copper wire inside, so that the hose retains its position when setting the path. The hose was attached at both ends to the worktable with metal brackets. Additionally, it was supported with some blocks and boxes to obtain the desired path, but these supports were placed outside the path-following zone. The robot was enclosed in a safety cell with the dimensions 1250 \(\times\) 1250 \(\times\) 1250 mm. In the robot controller software, the virtual safety walls were set to \(-\)550 \(\ldots\) +550 mm in the X axis, \(-\)550 \(\ldots\) +550 mm in the Y axis, and +5\(\ldots\) +inf mm in the Z axis. Similar boundaries were defined in the IK solver settings utilised in the path-following algorithm. The robot was placed in one of the didactic laboratories of the Gdansk University of Technology, which ensures stable environmental conditions (room temperature around 21 \({}^{\circ}\)C, humidity around 30%, appropriate lighting). The path detection was provided thanks to the specially designed detection tool (Figure 8). It consists of a box with communication electronics and six ST Microelectronics VL6180X (ST Microelectronics, Plan-les-Ouates, Switzerland) infrared Time-of-Flight sensors [38] placed on two perpendicular arms, three sensors per arm. These sensors can detect objects at a distance from 0 to 100 mm away from the sensor. The measurement accuracy is not explicitly provided in the datasheet; however, it contains the following information: measurement noise up to 2 mm, temperature-dependent drift 9 mm, and voltage-dependent drift 3 mm. All of the sensors were calibrated before use according to the procedure described by the manufacturer. Nevertheless, it must be noted that range (distance) measurement data are not directly used for path detection. Only the presence of the path in the detection zone of a sensor is used, not the exact distance value. Experimental verification proved that this is sufficient if only a small part of the path (hose) lies in the sensor's detection sight, as its detection is stable. It must also be noted that the detection field of each sensor is not a narrow, straight line but a cone with a vertex located in the sensor position and 25\({}^{\circ}\) vertex angle. Because of this, as sensors are separated by 22 mm (which makes 31 mm when considering the grid diagonal), and the hose diameter is 20 mm, it is common that the path is detected by more than one sensor. In such situations, path position is calculated using Equation (8). This means that the overall accuracy of the path centre detection is not worse than \(\pm\)16 mm. Data from sensors were read by an ST Microelectronics STM32G431 microcontroller (MCU) on an STM32 Nucleo board via I2C bus and then sent to a personal computer (PC). Figure 8: Path detection tool used during experimental verification: (**a**) view of the real tool in the robot gripper placed over the path to follow; (**b**) main tool diameters. The communication between the tool and a control algorithm running in Matlab on the PC was provided via a wireless Bluetooth 4.0 BLE connection. The tool is independent from the robot, i.e., it is grasped by the robot's gripper at the beginning of the program and then moved to subsequent positions, but there is no connection (i.e., no wires) and no direct communication between the tool and the robot or the robot controller.