Simulation–Based Control and Vision Prototype for a 5-DOF Waste-Sorting Robot

2.1 Modeling of the 5-DOF robot

2.1.1 Robot parameters

The parameters of the robot manipulator used in the simulations are listed in Tables 1 and 2.

Table 1. Robot parameters

Table 2. Joint ranges

The moments of inertia (MoI) and the products of inertia (PoI) are both reported so that the full inertia tensor of each link is specified in Tables 3 and 4.

Table 3. Parameters for moment of inertia (MoI)

Table 4. Products of inertia (PoI)

2.1.2 Forward kinematics

To address the forward kinematics problem, a reference coordinate system is defined at the robot base, followed by local coordinate frames for individual links, as shown in Figure 1. The robot’s kinematic structure is then modeled using the D–H formulation. Through this approach, the spatial position and orientation of the end-effector can be computed with respect to the base reference frame. The associated D–H parameters are summarized in Table 5. 

1.png

Figure 1. Coordinate frames of the 5-DOF robotic arm

Table 5. Denavit - Hartenberg (D-H) parameter table

where,

$\theta_i$ defines the joint rotation that aligns the $x_{i-1}$ axis with the $x_i$ axis through a rotation around the $z_i$ axis.

$d_i$ specifies the linear offset between the $x_{i_{-1}}$ and $x_i$ axes taken along the direction of the axis.

$a_i$ describes the link length, corresponding to the translation from the $z_{i_{-1}}$ axis to the $z_i$ axis along the axis.

$\alpha_i$ indicates the twist angle required to rotate the $z_{i_{-1}}$ axis into alignment with the $z_i$ axis about the $x_i$ axis.

General homogeneous transformation matrix:

${ }^{i-1} T=\left[\begin{array}{cccc}c_i & -s_i c\left(\alpha_i\right) & s_i s\left(\alpha_i\right) & a_i c_i \\ s_i & c\left(\alpha_i\right) c_i & -c_i s\left(\alpha_i\right) & a_i s_i \\ 0 & s\left(\alpha_i\right) & c\left(\alpha_i\right) & d_i \\ 0 & 0 & 0 & 1\end{array}\right]$    (1)

where,

$s_i=\sin \left(\theta_i\right) ; s\left(\alpha_i\right)=\sin \left(\alpha_i\right) ;$

$c_i=\cos \left(\theta_i\right) ; c\left(\alpha_i\right)=\cos \left(\alpha_i\right)$

Transformation matrix from link 1 to link 5:

${ }_1^0 T=\left[\begin{array}{cccc}c_1 & 0 & s_1 & 0 \\ s_1 & 0 & -c_1 & 0 \\ 0 & 1 & 0 & d_1 \\ 0 & 0 & 0 & 1\end{array}\right]$    (2)

${ }_2^1 T=\left[\begin{array}{cccc}c_2 & s_1 & 0 & a_2 c_2 \\ s_2 & c_2 & 0 & a_2 c_2 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1\end{array}\right]$    (3)

${ }_3^2 T=\left[\begin{array}{cccc}c_3 & -s_3 & 0 & a_3 c_3 \\ s_3 & c_3 & 0 & a_3 c_3 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1\end{array}\right]$   (4)

${ }_4^3 T=\left[\begin{array}{cccc}c_4 & 0 & s_4 & 0 \\ s_4 & 0 & -c_4 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1\end{array}\right]$   (5)

${ }_5^4 T=\left[\begin{array}{cccc}c_5 & -s_5 & 0 & 0 \\ s_5 & c_5 & 0 & 0 \\ 0 & 0 & 1 & d_5 \\ 0 & 0 & 0 & 1\end{array}\right]$   (6)

The end-effector transformation matrix is calculated as follows:

${ }_5^0 T=\left[\begin{array}{cccc}n_x & o_x & a_x & p_x \\ n_y & o_y & a_y & p_y \\ n_z & o_z & a_z & p_z \\ 0 & 0 & 0 & 1\end{array}\right]$   (7)

where,

$\begin{aligned} & s_{234}=\sin \left(\theta_2+\theta_3+\theta_4\right) \\ & c_{234}=\cos \left(\theta_2+\theta_3+\theta_4\right) \\ & c_{23}=\cos \left(\theta_2+\theta_3\right) \\ & n_x=s\left(\theta_1\right) s\left(\theta_5\right)+c\left(\theta_{234}\right) c\left(\theta_1\right) c\left(\theta_5\right) \\ & n_y=c\left(\theta_{234}\right) c\left(\theta_5\right) s\left(\theta_1\right)-c\left(\theta_1\right) s\left(\theta_5\right) \\ & n_z=s\left(\theta_{234}\right) c\left(\theta_5\right) \\ & o_x=c\left(\theta_5\right) s\left(\theta_1\right)-c\left(\theta_{234}\right) c\left(\theta_1\right) s\left(\theta_5\right) \\ & o_y=-c\left(\theta_1\right) c\left(\theta_5\right)-c\left(\theta_{234}\right) s\left(\theta_1\right) s\left(\theta_5\right) \\ & o_z=-s\left(\theta_{234}\right) c\left(\theta_5\right) \\ & a_x=s\left(\theta_{234}\right) c\left(\theta_1\right) \\ & a_y=s\left(\theta_{234}\right) s\left(\theta_1\right) \\ & a_z=-c\left(\theta_{234}\right) \\ & P_x=c\left(\theta_1\right)\left(a_3 c\left(\theta_{23}\right)+a_2 c\left(\theta_2\right)+d_5 s\left(\theta_{234}\right)\right) \\ & P_y=s\left(\theta_1\right)\left(a_3 c\left(\theta_{23}\right)+a_2 c\left(\theta_2\right)+d_5 s\left(\theta_{234}\right)\right) \\ & P_z=d_1+a_3 s\left(\theta_{23}\right)+a_2 s\left(\theta_2\right)-d_5 c\left(\theta_{234}\right)\end{aligned}$

2.1.3 Inverse kinematics

The inverse kinematic equations of the 5-degree-of-freedom robot are formulated as follows:

$\begin{aligned} & { }_5^0 A={ }_1^0 A_2^1 A_3^2 A_4^3 A_5^4 A =>\left({ }_1^0 A\right)^{-1}{ }_5^0 A={ }_2^1 A_3^2 A_4^3 A_5^4 A\end{aligned}$   (8)

The left-hand side and right-hand side of Eq. (8) are computed as follows:

LHS $=$$\left[\begin{array}{cccc}n_x c_1+n_y s_1 & o_x c_1+o_y s_1 & a_x c_1+a_y s_1 & P_x c_1+P_y s_1 \\ n_y & o_z & a_z & P_z-d_1 \\ n_x s_1-n_y c_1 & o_x s_1-o_y c_1 & a_x s_1-a_y c_1 & P_x s_1-P_y c_1 \\ 0 & 0 & 0 & 1\end{array}\right]$   (9)

$R H S=$$\left[\begin{array}{cccc}c_{234} c_5 & -c_{234} s_5 & s_{234} & a_3 c_{23}+a_2 c_2+d_5 s_{234} \\ c_{234} c_5 & -c_{234} s_5 & -c_{234} & a_3 s_{23}+a_2 s_2-d_5 c_{234} \\ s_5 & c_5 & 0 & 0 \\ 0 & 0 & 0 & 1\end{array}\right]$   (10)

By equating the corresponding elements on both sides:

• Calculate $\theta_1$:

$P_x s_1-P_y c_1=0 \Leftrightarrow \frac{P_y}{P_x}=\frac{s_1}{c_1} \Rightarrow \tan \theta_1=\frac{P_y}{P_x}$

Therefore, $\theta_1=\operatorname{atan} 2\left(\mathrm{P}_{\mathrm{x}}, \mathrm{P}_{\mathrm{y}}\right)$

• Calculate $\theta_5$:

$\left\{\begin{array}{l}s_5=n_x s_1-n_y c_1 \\ c_5=o_x s_1-o_y c_1\end{array} \Rightarrow \tan \theta_5=\frac{n_x s_1-n_y c_1}{o_x s_1-o_y c_1}\right.$

Thus, $\theta_5=\operatorname{atan} 2\left(n_x \mathrm{~s}_1-n_y \mathrm{c}_1, o_x \mathrm{s}_1-o_y \mathrm{c}_1\right)$

• Calculate $\theta_2+\theta_3+\theta_4$:

$\left\{\begin{array}{l}s_{234}=a_x c_1+a_y s_1 \\ -c_{234}=a_z\end{array} \Rightarrow \tan \left(\theta_2+\theta_3+\theta_4\right)=\frac{a_x c_1+a_y s_1}{-a_z}\right.$

Hence, $\theta_2+\theta_3+\theta_4=\operatorname{atan} 2\left(-a_x c_1-a_y s_1, a_z\right)$.

+ Calculate $\theta_3$:

$\left\{\begin{array}{l}P_x c_1+P_y s_1=a_3 c_{23}+a_2 c_2+d_5 s_{234} \\ P_z-d_1=a_3 s_{23}+a_2 s_2-d_5 c_{234}\end{array}\right.$

Thus, $\left\{\begin{array}{l}P_x c_1+P_y s_1-d_5 s_{234}=a_3 c_{23}+a_2 c_2 \\ P_z-d_1+d_5 c_{234}=a_3 s_{23}+a_2 s_2\end{array}\right.$

Let $\left\{\begin{array}{l}m=P_x c_1+P_y s_1-d_5 s_{234} \\ n=P_z-d_1+d_5 c_{234}\end{array}\right.$

Then: $\left\{\begin{array}{l}m=a_3 c_{23}+a_2 c_2 \\ n=a_3 s_{23}+a_2 s_2\end{array}\right.$

Thus, $\left\{\begin{array}{l}m^2=\left(a_3 c_{23}\right)^2+\left(a_2 c_2\right)^2+2 a_3 c_{23} a_2 c_2 \\ n^2=\left(a_3 s_{23}\right)^2+\left(a_2 s_2\right)^2+2 a_3 s_{23} a_2 s_2\end{array}\right.$

$\begin{aligned} & \Rightarrow m^2+n^2=\left(a_3\right)^2+\left(a_2\right)^2+2 a_2 a_3\left(c_{23} c_2+s_{23} s_2\right) \\ & \Rightarrow m^2+n^2-\left(a_3\right)^2-\left(a_2\right)^2=2 a_2 a_3 c_3\end{aligned}$

Finally, the two values $c_3$ and $s_3$ can be deduced as follows:

$c_3=\frac{m^2+n^2-\left(a_3\right)^2-\left(a_2\right)^2}{2 a_2 a_3}$

$s_3= \pm \sqrt{1-\cos \left(\theta_3\right)^2}= \pm \sqrt{1-\left(\frac{m^2+n^2-\left(a_3\right)^2-\left(a_2\right)^2}{2 a_2 a_3}\right)^2}$

At last, the angle $\theta_3$ is $\theta_3=\operatorname{atan} 2\left(\mathrm{s}_3, \mathrm{c}_3\right)$.

• Calculate $\theta_2$:

The following transformations are implemented to get $\theta_2$.

$\left\{\begin{array}{l}m=a_3 c_{23}+a_2 c_2 \\ n=a_3 s_{23}+a_2 s_2\end{array} \Leftrightarrow\left\{\begin{array}{l}m=c_2\left(a_3 c_3+a_2\right)-a_3 s_2 s_3 \\ n=s_2\left(a_3 c_3+a_2\right)+a_3 c_2 s_3\end{array}\right.\right.$

Thus,

$c_2=\frac{m+a_3 s_2 s_3}{a_3 c_3+a_2}$   (11)

$s_2=\frac{n-a_3 c_2 s_3}{a_3 c_3+a_2}$   (12)

Substituting Eq. (11) into Eq. (12), one can deduce:

$\mathrm{s}_2=\frac{n\left(a_3 c_3+a_2\right)-a_3 s_3 m}{\left(a_3 c_3+a_2\right)^2+\left(a_3 s_3\right)^2}$

Substituting Eq. (12) into Eq. (11), it is straightforward to compute $c_2$ and $\theta_2$ as follows:

$c_2=\frac{m\left(a_3 c_3+a_2\right)+a_3 s_3 n}{\left(a_3 c_3+a_2\right)^2+\left(a_3 s_3\right)^2}$

$\theta_2=\operatorname{atan} 2\left(\mathrm{s}_2, c_2\right)$

• Calculate $\theta_4$:

$\theta_4=\theta_{234}-\theta_2-\theta_3$

2.2 Development of the PD-Fuzzy controller

2.2.1 Structural diagram of the PD-Fuzzy controller

The control scheme for the robotic arm is illustrated in Figure 2. This controller is designed to address system uncertainties.

A fuzzy control framework for robotic applications generally includes three fundamental elements:

(i) Fuzzification: Maps the tracking error e(t) and its rate of variation de(t) into corresponding fuzzy sets through predefined membership functions.

(ii) Rule Base: Implements the Sugeno inference approach to formulate a collection of fuzzy control rules derived from expert experience.

(iii) Defuzzification: Transforms the inferred fuzzy output into a precise control signal u that is applied to the robot.

2.png

2.2.2 Membership functions

With the input variables e(t) (error), de(t) (change in error) and the output value u (control signal) defined in Figures 3-5, the linguistic variable values are constructed as follows: NB (Negative Big), NM (Negative Medium), NS (Negative Small), ZE (Zero), PS (Positive Small), PM (Positive Medium) and PB (Positive Big)

From these sets, the membership functions are constructed based on the pre-calculated PD parameters [9-11].

3.png

Figure 3. Position parameters

4.png

Figure 4. Differential position error parameters

5.png

Figure 5. Differential control signal parameters

2.2.3 Fuzzy control rules

The design of the fuzzy controller relies on expert knowledge to establish the inference rules and the defuzzification mechanism.

During the tuning process, multiple simulations were performed to observe the impact of parameter changes and to select the configuration that yields the smallest error. This iterative process helped to refine the controller, resulting in a stable fuzzy system that is well-adapted to real-world conditions.

Table 6. Fuzzy control rules for the robotic arm

The rule base is constructed in Table 6 based on control experience and the following principles:

• If the error is large and increasing then apply a strong control action in the opposite direction.
• If the error is small then apply a small control action to prevent oscillation.
• If the error is zero then apply a zero or very small control action.

Training: The dataset was divided into training, validation, and test sets with a ratio of 80%, 10%, and 10%, respectively. The dataset split was performed at the physical object level rather than the image level. This ensures that the same physical object does not appear in more than one subset, thereby preventing data leakage and ensuring a fair evaluation process. The dataset construction and splitting strategy ensures transparency, reproducibility, and the avoidance of data leakage, allowing reliable evaluation of the YOLOv11 model for waste detection and classification in real-world conditions.

The training workflow of the customized YOLOv11s model is illustrated in Figure 15. The training set is used to train the model, while the validation set is employed to monitor model performance during training and to support model selection or hyperparameter tuning. It can also be used to apply strategies such as early stopping. The test set is reserved for the final evaluation of the trained model.

In practice, the validation set may be derived from the training data. However, the test set must remain completely independent from both the training and validation sets to ensure that the evaluation accurately reflects the model’s generalization ability on unseen data.

The YOLOv11s model was trained using the Visual Studio Code environment. The training process utilized an NVIDIA GPU with 6 GB of memory to accelerate computation and improve training efficiency.

Table 14 below summarizes the hyperparameter values used during training of the YOLOv11s model to obtain optimal performance.

Table 14. Training hyperparameters of the YOLOv11s model

Simulation results: The detection performance achieved by the YOLOv11s model is presented and discussed in Figure 16.

The results shown in Figure 16 indicate that the training process of the proposed model remains stable, effective, and reliable. The YOLOv11s model shows strong learning capability, as evidenced by the continuous and consistent reduction in loss values throughout the training phase. No signs of overfitting or underfitting are observed, and the model reaches a stable and high level of performance after approximately 30 training epochs.

Figure 16. Training results of the YOLOv11s model

The first row of the training curves corresponds to the training loss components, including train/box_loss, train/cls_loss, and train/dfl_loss, all of which exhibit a smooth and monotonic decreasing trend. This trend reflects a gradual enhancement in the model’s capability to precisely localize objects (box_loss), correctly classify object categories (cls_loss), and refine the confidence distribution of bounding boxes (dfl_loss). The consistent decline observed in these loss metrics indicates that the model successfully captures representative features from the training data and steadily converges toward an optimal solution.

The second row of the training curves presents the validation loss computed on the validation dataset, including val/box_loss, val/cls_loss, and val/dfl_loss. While val/cls_loss shows relatively higher variability during the early training epochs, it progressively declines and reaches a stable pattern as the training continues. This initial fluctuation is likely associated with class imbalance in the dataset. The consistent and comparable decreasing trends observed between the training and validation loss curves suggest that the proposed model does not suffer from significant overfitting and maintains good generalization performance when evaluated on previously unseen data.

The curves associated with metrics/precision (B) and metrics/recall (B) reveal that the proposed approach maintains a robust and stable performance throughout training. Specifically, precision values rise to nearly 0.95, while recall values consistently remain around 0.90. The high precision level reflects the model’s capability to effectively suppress false positive predictions, whereas the sustained recall rate confirms its proficiency in correctly detecting target objects and minimizing missed detections.

Furthermore, the curves corresponding to mAP50 (B) and mAP50–95 (B) exhibit stable convergence at relatively high values, reaching approximately 0.96 and 0.82, respectively. The high mAP50 value reflects robust detection performance at an Intersection over Union (IoU) threshold of 0.5. The performance gap observed between mAP50 and mAP50–95 is expected, as higher IoU thresholds impose more stringent requirements on bounding box localization accuracy. Consequently, even minor deviations in predicted bounding boxes or slight inaccuracies in ground-truth annotations can lead to a noticeable reduction in the mAP50–95 metric.

As shown in Figure 17, YOLOv11 accurately identifies the label of each object.

Figure 17. Experimental results after training

Model performance on the test dataset is analyzed through a confusion matrix, which provides an overall evaluation of its capability to detect and distinguish among different waste classes. In addition, this representation facilitates the identification of class-specific misclassifications and systematic error patterns that may require further refinement.

Figure 18. Confusion matrix results of the model

The confusion matrix illustrated in Figure 18 provides an overview of the classification outcomes based on four key elements: True Positives (TP), False Positives (FP), True Negatives (TN), and False Negatives (FN). The formal definitions and detailed explanations of these elements are presented in Table 15.

As shown in Table 16, the model demonstrates strong performance in the Metal and Plastic classes, indicating effective feature extraction and robust class discrimination for these material categories.

Nevertheless, challenges persist when the model processes images containing complex backgrounds, leading to a relatively higher number of misclassifications across certain classes. In particular, a total of 182 misclassified instances is identified, with a noticeable concentration in the Cardboard and Glass categories. This pattern suggests an increased occurrence of false positive predictions in scenarios where background elements exhibit rich visual details or share similar characteristics with the target objects.

Table 15. Results from the confusion matrix

Table 16. Evaluation of the five classes

Moreover, cross-class confusion is evident among categories with closely related visual characteristics, particularly between Cardboard and Paper, as well as Plastic and Glass. This behavior is anticipated given the similarities in color, texture, and structural appearance shared by these materials. Notably, the Cardboard and Paper classes exhibit a higher tendency toward mutual misclassification, which can be primarily attributed to their comparable material composition and surface features.

To further improve model performance, several enhancements are recommended, including the inclusion of more diverse background samples, improved class balancing within the dataset, and the application of material-specific data augmentation strategies. In addition, further fine-tuning of the model is required to strengthen its capacity to capture subtle discriminative features among visually similar categories.

Overall, the proposed model demonstrates satisfactory performance in the waste classification task, delivering stable recognition across the majority of categories. Nonetheless, further improvements in background separation, robustness to noise in real-world environments, and deeper feature representation are necessary to enhance classification accuracy. In addition, the continuous expansion and diversification of the dataset remain essential to ensure reliable performance under practical deployment conditions.

4.2 Comparative discussion

To justify the selection of YOLOv11 as the optimal model, a comparative evaluation was conducted among three YOLO variants, namely YOLOv5su, YOLOv8s, and YOLOv11. The comparison is specifically presented in Table 17 and Figure 19. They summarize the detection performance of the three YOLO models after 30 training epochs. The results indicate that YOLOv5su and YOLOv8s achieve comparable detection accuracy, whereas YOLOv11s obtains higher precision and recall values. In addition, YOLOv11s achieves the highest mAP@0.5 among the evaluated models, outperforming the two baseline approaches. These findings suggest that YOLOv11s provides improved object detection capability for the waste classification task and is well suited for integration into vision-based robotic system.

Table 18 compares the real-time performance of the evaluated YOLO models. All experiments were conducted on the same hardware platform equipped with an NVIDIA RTX3000 GPU to ensure a consistent evaluation environment. The results show that YOLOv5su achieves the highest processing speed due to its lightweight single-stage detection architecture; however, its larger model size may limit deployment on resource-constrained platforms. YOLOv8s exhibits slightly higher latency, which may be attributed to the decoupled head design that increases computational complexity during inference.

Table 17. Best performance comparison of three models after 30 training epochs

Figure 19. Comparison of detection accuracy (mAP50) between YOLOv5su, YOLOv8su, and YOLOv11s

Table 18. Real-time performance comparison of YOLO-based models

In comparison, YOLOv11s achieves an inference time similar to YOLOv8s while maintaining lower overall latency and a smaller model size than YOLOv5su. These results indicate that YOLOv11s offers a balanced trade-off between detection performance, processing speed, and deployability in real-time robotic waste-sorting applications. Consequently, YOLOv11s was selected as the primary model for the robotic vision system in this study.

To evaluate the computational performance of the proposed model on embedded hardware, inference latency was measured on a Raspberry Pi 4B (ARM Cortex-A72, 4 GB RAM) using a set of 100 static test images. Each image was processed individually through the full model pipeline, and wall-clock time was recorded using Python's time.perf_counter(), capturing the duration from image input to detection output. Ten warm-up iterations were performed prior to measurement to stabilize memory allocation and remove initialization overhead. To mitigate the impact of operating system interrupts, a 5% trimmed mean was applied by excluding the five lowest and five highest latency values before computing descriptive statistics. All three models-YOLOv5su, YOLOv8s, and YOLOv11s-were evaluated under identical conditions to ensure a fair comparison.

Figure 20. Inference latency distribution of YOLOv5su, YOLOv8s, and YOLOv11s on Raspberry Pi 4B

The results, shown in Figure 20, indicate that YOLOv11s achieved an average latency of 20.3 ± 4.9 ms per frame. YOLOv8s and YOLOv5su reached 19.8 ± 7.8 ms and 19.7 ± 5.0 ms, respectively. Although the mean latency differences among the models were less than 0.6 ms-within the expected measurement noise on ARM-based hardware-a clear distinction was observed in latency stability. YOLOv11s exhibited the lowest standard deviation (4.9 ms), whereas YOLOv8s showed higher variability (7.8 ms), indicating more consistent per-frame processing. This stability is critical in practical applications such as automated waste sorting on conveyor systems, where irregular latency spikes can disrupt downstream control logic. These findings demonstrate that YOLOv11s maintains competitive inference speed on resource-constrained hardware while offering improved temporal stability compared with the baseline models.

4.3 Sending coordinates to MATLAB

Prior to object detection and recognition by the camera system, an image preprocessing stage is performed, encompassing noise reduction, brightness enhancement and image sharpening. Subsequently, the detected objects are categorized, and their centroid positions, object counts, and orientation angles are calculated, with the results presented on the display, as illustrated in Figure 21.

Subsequently, the extracted object parameters, including positional and orientation information, are transmitted to MATLAB, where inverse kinematics calculations are carried out to determine the joint configurations required for object grasping and manipulation. The overall processing pipeline is summarized in the algorithmic flowchart shown in Figure 22.

To elucidate the operational workflow of the perception–control pipeline, the timing characteristics were quantitatively evaluated within a simulation environment. System integration was implemented using the User Datagram Protocol (UDP) to enable high-speed data exchange between the vision processing platform (Python/VSCode) and the control computation framework (MATLAB/Simulink).

1. Perception Module Timing Characteristics (T_percept):

The image processing pipeline, based on the YOLO architecture and accelerated by GPU computation, achieves a total processing cycle of 23.3 ms. The contributions of the individual components are as follows:

+  Image Capture Time: 0.3 ms.

+  Inference Time: 22.4 ms.

+  Post-processing Time: 0.6 ms.

Figure 21. Image preprocessing

22.png

Figure 22. Flowchart of the image-processing algorithm and the position-coordinate signal transmission to MATLAB

2. Communication and Throughput Parameters (T_comm):

The transmission of coordinate and classification data between processes is optimized for real-time responsiveness:

+  Communication Period (Comm Period): 25.9 ms.

+  Transmission Command Latency (Comm Latency): 0.1 ms.

+  System Throughput: Achieves 38.6 FPS.

+  Measured Transmission Delay: Empirical data indicates that inter-process communication latency consistently ranges between 1.27 ms and 2.76 ms.

3. Control Execution Timing (T_ctrl):

In the MATLAB/Simulink environment, the control system for the 5-DOF robot is configured with the following parameters (Table 19):

+  Solver Type: ode45.

+  Maximum Step Size: Strictly set to 0.001 s (1 ms).

+  Setting the simulation step size to 1 ms provides a temporal resolution for the controller that is 25 times higher than the vision update rate, thereby contributing to the dynamic stability of the robotic manipulator during simulated operations.

Table 19. System integration and timing analysis parameters table

These coordinates are subsequently used as reference inputs for robot motion control. The adopted control strategy [24, 25] is implemented through two sequential stages, as illustrated in Figure 23.

23.png

Figure 23. Coordinate system transformation

Under the pinhole camera model, the mapping from three-dimensional (3D) world coordinates to the two-dimensional (2D) image plane is described by perspective projection. Based on the geometric relationship of similar triangles, the coordinates of a projected point on the image sensor, denoted as $\left(x_i, y_i\right)$, can be expressed as follows:

$x_i=f \cdot \frac{x_c}{z_c} ; y_i=f \cdot \frac{y_c}{z_c}$    (23)

where,

$x_c, y_c, z_c$: The 3D coordinates of the object in the camera coordinate system.

f: The focal length of the camera lens.

$x_i, y_i$: The 2D coordinates of the object projected onto the image plane, expressed in millimeters (mm).

To convert coordinates from the image plane (in millimeters) to the pixel coordinate plane, scaling factors $\mathrm{m}_{\mathrm{x}}$ and $\mathrm{m}_{\mathrm{y}}$ are introduced. These parameters represent the number of pixels per millimeter along the horizontal and vertical directions of the image sensor, respectively. Accordingly, the relationship between the physical focal length f and the focal lengths in pixel units, $f_x, f_y$, can be expressed as follows:

$f_x=f . m_x ; f_y=f . m_y$   (24)

The pixel coordinates (u, v) of the object are then obtained by combining the perspective projection with a translation of the coordinate origin from the image center to the top-left corner of the image frame.

$\mathrm{u}=\mathrm{f}_{\mathrm{x}} \frac{\mathrm{x}_{\mathrm{c}}}{\mathrm{z}_{\mathrm{r}}}+\mathrm{o}_{\mathrm{x}} ; \mathrm{v}=\mathrm{f}_{\mathrm{y}} \frac{\mathrm{y}_{\mathrm{c}}}{\mathrm{z}_{\mathrm{r}}}+\mathrm{o}_{\mathrm{y}}$   (25)

where, $\left(c_x, \mathrm{c}_{\mathrm{y}}\right)$ denote the principal point, which represents the offset used to shift the coordinate origin from the image center to the top-left corner of the digital image.

To represent all intrinsic parameters within a unified mathematical framework, homogeneous coordinates are employed. Under this formulation, the projection from the camera space to the pixel plane is derived from the scale equivalence between the coordinates on the virtual image plane $[\tilde{u}, \tilde{v}, \widetilde{w}]^{\mathrm{T}}$ and the corresponding pixel coordinates $[\mathrm{u}, \mathrm{v}, 1]^{\mathrm{T}}$ parameterized by the depth $\mathrm{Z}_{\mathrm{c}}$:

$\begin{aligned} {\left[\begin{array}{c}\mathrm{u} \\ \mathrm{v} \\ \mathrm{l}\end{array}\right] } & \equiv\left[\begin{array}{c}\tilde{\mathrm{u}} \\ \tilde{\mathrm{v}} \\ \widetilde{\mathrm{w}}\end{array}\right]=\left[\begin{array}{c}\mathrm{z}_{\mathrm{c}} \mathrm{u} \\ \mathrm{z}_{\mathrm{c}} \mathrm{v} \\ \mathrm{z}_{\mathrm{c}}\end{array}\right]=\left[\begin{array}{c}\mathrm{f}_{\mathrm{x}} \mathrm{x}_{\mathrm{c}}+\mathrm{z}_{\mathrm{c}} \mathrm{o}_{\mathrm{x}} \\ \mathrm{f}_{\mathrm{y}} \mathrm{y}_{\mathrm{c}}+\mathrm{z}_{\mathrm{c}} \mathrm{o}_{\mathrm{y}} \\ \mathrm{z}_{\mathrm{c}}\end{array}\right] =\left[\begin{array}{cccc}\mathrm{f}_{\mathrm{x}} & 0 & \mathrm{o}_{\mathrm{x}} & 0 \\ 0 & \mathrm{f}_{\mathrm{y}} & \mathrm{o}_{\mathrm{y}} & 0 \\ 0 & 0 & 1 & 0\end{array}\right]\left[\begin{array}{c}\mathrm{x}_{\mathrm{c}} \\ \mathrm{y}_{\mathrm{c}} \\ \mathrm{z}_{\mathrm{c}} \\ 1\end{array}\right]\end{aligned}$

Therefore,

$\tilde{\mathrm{u}}=\mathrm{M}_{\mathrm{int}} \tilde{\mathrm{x}}_{\mathrm{C}}$   (26)

where,

$u=\frac{\tilde{u}}{\widetilde{w}} ; v=\frac{\tilde{v}}{\widetilde{w}} ; M_{\text {int }}=\left[\begin{array}{ccc}f_x & 0 & o_x \\ 0 & f_y & o_y \\ 0 & 0 & 1\end{array}\right]$

The intrinsic matrix describes the internal parameters of the camera. In contrast, the extrinsic matrix Mext specifies the pose of the camera, including its translation vector t and rotation matrix R, with respect to the robot base coordinate frame.

The extrinsic matrix can be expressed as a homogeneous transformation matrix, which combines rotation and translation to transform coordinates from the world (or robot) coordinate system to the camera coordinate system:

$\left[\begin{array}{c}\mathrm{x}_{\mathrm{c}} \\ \mathrm{y}_{\mathrm{c}} \\ \mathrm{z}_{\mathrm{c}} \\ 1\end{array}\right]=\left[\begin{array}{cccc}\mathrm{r}_{11} & \mathrm{r}_{12} & \mathrm{r}_{13} & \mathrm{t}_{\mathrm{x}} \\ \mathrm{r}_{21} & \mathrm{r}_{22} & \mathrm{r}_{23} & \mathrm{t}_{\mathrm{y}} \\ \mathrm{r}_{31} & \mathrm{r}_{32} & \mathrm{r}_{33} & \mathrm{t}_{\mathrm{z}} \\ 0 & 0 & 0 & 1\end{array}\right]\left[\begin{array}{c}\mathrm{x}_{\mathrm{w}} \\ \mathrm{y}_{\mathrm{w}} \\ \mathrm{z}_{\mathrm{w}} \\ 1\end{array}\right]$

Thus,

$\tilde{\mathbf{x}}_{\mathrm{c}}=\mathrm{M}_{\mathrm{ext}} \tilde{\mathbf{x}}_{\mathrm{w}}$   (27)

where,

Rotation matrix: This is a 3 x 3 orthogonal matrix consisting of elements r₁₁ through r₃₃. It represents the orientation of the camera relative to the working plane.

Translation vector $\mathrm{t}\left(\mathrm{t}_{\mathrm{x}}, \mathrm{t}_{\mathrm{y}}, \mathrm{t}_{\mathrm{z}}\right)$: Comprising components t_x, t_y, and t_z, this vector defines the position of the camera's optical center in the world space. Specifically, t_z represents the operational height (working distance) from the camera lens to the object surface.

By combining Eq. (26) and Eq. (27), a transformation model from world coordinates to pixel coordinates can be obtained:

$\tilde{\mathrm{u}}=\mathrm{M}_{\text {int }} \mathrm{M}_{\mathrm{ext}} \widetilde{\mathrm{x}}_{\mathrm{W}}=\mathrm{P} \widetilde{\mathrm{x}}_{\mathrm{W}}$   (28)

Since our purpose is to determine the 3D coordinates of the object in the world and not the other way around, we used the inverse of our matrices, and the computation was realized with the following Eq. (29):

$\tilde{\mathrm{x}}_{\mathrm{w}}=\mathrm{M}_{\text {ext }}^{-1} \cdot \mathrm{M}_{\text {int }}^{-1} \cdot \tilde{\mathrm{u}}$   (29)

Camera calibration using the checkerboard method

To accurately determine the parameters of the intrinsic matrix $\left(\mathrm{M}_{\mathrm{int}}\right)$ and the extrinsic matrix $\left(M_{\mathrm{ext}}\right)$, the camera calibration procedure in this study follows Zhang’s method [26] based on a checkerboard pattern. This approach is widely adopted in computer vision systems due to its reliability and high calibration accuracy.

24.png

Figure 24. Experimental image dataset for camera calibration process

Figure 25. Extrinsic parameters visualization

(1) Calibration procedure

Pattern preparation: A standard checkerboard pattern with a known square size (d = 23 mm) was used as the calibration reference object (see Figure 24).

Data acquisition: The camera captured 19 images of the checkerboard pattern from different viewpoints, distances, and orientations within the robot workspace.

Image processing: The MATLAB Camera Calibrator tool was used to automatically detect the corner points of the checkerboard squares in the captured images (see Figure 25).

(2) Parameter estimation

The calibration parameters were obtained by solving an optimization problem that minimizes the reprojection error between the known corner positions on the checkerboard pattern and their corresponding pixel coordinates in the captured images. Through this process, several sets of camera parameters were extracted.

The intrinsic parameters are used to correct image distortion and ensure accurate object detection, particularly near the image boundaries. The intrinsic camera matrix is denoted as Mint.

$\begin{gathered}\mathrm{M}_{\text {int }}=\left[\begin{array}{cccc}\mathrm{f}_{\mathrm{x}} & 0 & \mathrm{o}_{\mathrm{x}} & 0 \\ 0 & \mathrm{f}_{\mathrm{y}} & \mathrm{o}_{\mathrm{y}} & 0 \\ 0 & 0 & 1 & 0\end{array}\right]= {\left[\begin{array}{cccc}2732.5 & 0 & 1978.3 & 0 \\ 0 & 2735.6 & 1506.9 & 0 \\ 0 & 0 & 1 & 0\end{array}\right]}\end{gathered}$   (30)

Radial distortion coefficients: Despite the use of the pinhole camera model, the parameters $\mathrm{k}_1$ and $\mathrm{k}_2$ are introduced to correct geometric distortions occurring toward the peripheral regions of the image, ensuring greater linearity in the projection computation.

$\mathrm{k}_1=0.0825 \pm 0.0029$

$k_2=-0.0786 \pm 0.0033$

From matrix Eq. (30) it is possible to see the intrinsic parameters as given in Table 20.

Table 20. Intrinsic parameters

Extrinsic parameters: The extrinsic parameters define the relative pose between the world coordinate system (X_w, Y_w, Z_w), which is attached to the checkerboard pattern, and the camera coordinate system (X_c, Y_c, Z_c). The extrinsic transformation matrix is denoted as (M_ext).

$\begin{gathered}\mathrm{M}_{\text {ext }}=\left[\begin{array}{ll}\mathrm{R} & \mathrm{t} \\ 0 & 1\end{array}\right]=\mathrm{M}_{\text {ext }}= {\left[\begin{array}{cccc}0.9924 & -0.0249 & 0.1208 & -31.5 \\ 0.0197 & 0.9989 & 0.0435 & -490.5 \\ -0.1218 & -0.0407 & 0.9917 & 1363.4 \\ 0 & 0 & 0 & 1\end{array}\right]}\end{gathered}$   (31)

The reprojection error plot indicates that the mean calibration error is approximately 0.19 pixels. The lowest error of 0.0900054 pixels occurs in Image 5, whereas the highest error of 0.508615 pixels is observed in Figure 26.

Figure 26. Mean reprojection error per image

Using the estimated parameters $+\mathrm{f}_{\mathrm{y}} \approx 2735.6 \pm 7.95 \mathrm{px}$ và $\mathrm{t}_{\mathrm{z}} \approx 1363.4 \mathrm{~mm}$, the corresponding scale factor is calculated as follows:

$\mathrm{s}=\frac{\mathrm{t}_{\mathrm{z}}}{\mathrm{f}_{\mathrm{avg}}}=\frac{1363.4}{\frac{2732.5+2735.6}{2}} \approx 0.498\left(\frac{\mathrm{~mm}}{\mathrm{pixel}}\right)$    (32)

Using the obtained scale factor $\left(s \approx \frac{0.498 \mathrm{~mm}}{\text { pixel }}\right)$, the average 3D localization error in real-world units is computed as follows:

$\mathrm{E}_{\text {real }}=0.19 \times 0.498 \approx 0.094(\mathrm{mm})$