Dynamic Filtered Path Tracking Control for a 3RRR Robot Using Optimal Recursive Path Planning and Vision-Based Pose Estimation

This study proposes a dynamic filtered path tracking control schema for a 3RRR planar parallel robot to reduce the positioning errors caused by various sources such as backlash in the mechanical system, system nonlinearities, uncertainties, and unknown disturbances. The optimal recursive algorithm is implemented absolutely in path planning based on the optimization problem. Compared to other methods, this algorithm not only helps the robot avoid singularities but also ensures the shortest movement distance of links thanks to the optimization in path planning. The vision system using the 3D Intel RealSense camera D435i is proposed to provide the visual measurement as feedback for the pose estimation of the 3RRR planar parallel robot. The unscented Kalman filter is designed to reduce the errors of the pose estimation due to the camera’s intrinsic parameters, the vibration of the mechanical system, and unknown noises. The establishment of important equations and parameters of the unscented Kalman filter is detailed in this study. The simulation, experiment, and comparison are conducted to verify the effectiveness and feasibility of the proposed approaches.


I. INTRODUCTION
Robots have become very popular these days, from industrial production to daily activities. Industrial robots with fast speed, high accuracy, durable working ability, and many other outstanding advantages have gradually replaced human labor, contributed to reducing costs and increasing productivity in the manufacturing factories. Serial manipulators with the advantage of the large workspace are the most common industrial robots. However, it has many disadvantages such as low stiffness related to open kinematic structure, accumulated errors during operation, large weight of actuators, and relatively low load performance. Compared to serial manipulators, parallel manipulators have several superior advantages. With high stiffness, fast speed and acceleration, high accuracy, and high carrying capacity, these manipulators have become the solution for applications in the industry [1]- [3].
The associate editor coordinating the review of this manuscript and approving it for publication was Aysegul Ucar . Therefore, many ideas for the application of parallel manipulators to the fields of robotics and machine tools have been developed [4]- [6]. In this study, the 3RRR robot [7], a type of three degree-of-freedom (3-DOF) planar parallel manipulator, is selected as the research object to apply the proposed approaches.
Like many closed-loop structures, 3RRR planar parallel robots have the main drawback of limited workspace and complex singularities [8], [9]. Bonev et al. [10] used the crew theory to analyze the singularity of 3-DOF planar parallel robots. The authors in [11] analyzed the singularity to select an optimal structure of the 3-DOF planar parallel robots. However, the papers did not mention controlling robots to avoid singularities. To control the robot, the inverse kinematics problem must be solved to determine the rotation angles of the active links corresponding to a given position of the end-effector (plate). The inverse kinematics problem has eight solutions, which is called the moving state in this study. As the robot moves through a multipoint path, each point has eight possible moving states, there will be a huge number of combination of moving states to satisfy the desired path. Selecting the optimal moving states to both minimize travel distances and avoid singularities is a problem that needs to be addressed. The optimal design problem was proposed in [12] to determine the best geometry of the 3RRR planar parallel robot to increase the workspace without the singularities for all the eight moving states. Liu et al. [13] and Zhang et al. [14] analyzed the singularity and path planning for a 3RRR planar parallel robot using the working (actuation) mode conversion. The working modes are converted in turn in different schemas. Most of the research analyzed the singularities and proposed a feasible approach for path planning of the 3RRR planar parallel robots. However, most of the works did not solve the optimization for path planning. In this study, the optimal recursive algorithm is proposed for the path planning of a 3RRR planar parallel robot. The algorithm divides the moving points of the desired path into several groups to reduce the number of combinations of moving states. After that, the absolute recursive algorithm is implemented in each group based on the optimization problem, which both minimizes travel distances and avoids singularities. The proposed algorithm ensures optimal and smooth movement of the robot without the need for mechanical structure intervention.
Over the years, many studies have been conducted to improve the ability of the robot to reach the desired pose (position and orientation) accurately. The direct solution of adding the absolute encoders to the revolute joints is highly effective but costly and sometimes difficult to implement thoroughly. Therefore, other solutions including static calibration and dynamic positioning were considered [15]- [17]. Farajtabar et al. [18] modeled the joint clearance as a virtual actuator to compensate for the errors in positioning. The authors in [19] used three sliding mode controllers and a proportional-integral-derivative (PID)-neural network controller for positioning of the 3RRR planar parallel robot in the simulation. It tries to reduce the errors between the links caused by noises and disturbances. In [20], a genetic algorithm was used to optimize a PID controller of the 3RRR planar parallel robot. It is expected to improve the precision of trajectory control. Yao et al. [21] used the vision measurement as feedback to control the position of the 3RRR planar parallel system. It combined an adaptive control algorithm, a fuzzy logic controller, and a PI controller to achieve the accuracy of positioning. [22] used particle swarm optimization (PSO) to optimize the trajectory planning for a robotic work cell.
In general, positioning errors can be caused by various sources such as backlash in the mechanical system, system nonlinearities, uncertainties, and unknown disturbances. Addressing all sources of error will be complex and less effective. Solutions based on visual measurement [23] meet positioning requirements with high accuracy for various trajectories because it addresses directly the pose of the end effector. However, the vision system often has errors due to noises during its operation. Therefore, some studies used the extended Kalman filter (EKF) [24] or adaptive Kalman filter (AKF) [25], [26] to optimize the pose estimation. In this study, the pose estimation of the 3RRR planar parallel robot is performed based on the vision system using the 3D Intel RealSense camera D435i. The unscented Kalman filter (UKF) [27] is proposed to correct the pose estimation, reduce the errors due to the camera's intrinsic parameters, the vibration of the mechanical system, and unknown noises. Compared to EKF and AKF, UKF uses many points, called sigma points, to linearize a nonlinear model, so it can predict the system state more accurately.
Based on the literature review and the above discussion, the primary contributions of this article involve: 1. The optimal recursive path planning is proposed for the 3RRR planar parallel robot. This method divides the moving points of the desired path into several groups to reduce the number of combinations of moving states, thus speed up the performance. After that, the optimal recursive algorithm is implemented absolutely in each group based on the optimization problem. Compared to other methods, this algorithm not only helps the robot avoid singularities but also ensures the shortest movement distance of links thanks to the optimization in path planning. The proposed approach ensures optimal and smooth movement of the robot without the need for mechanical structure intervention. 2. The vision system using the 3D Intel RealSense camera D435i is proposed to provide the visual measurement as feedback for the pose estimation of the 3RRR planar parallel robot. The UKF is designed for the vision-based pose estimation to reduce the errors due to the camera's intrinsic parameters, the vibration of the mechanical system, and unknown noises. The establishment of important equations and parameters of UKF is detailed in this study. 3. The dynamic filtered path tracking control schema is proposed for the 3RRR planar parallel robot to reduce the positioning errors caused by various sources such as backlash in the mechanical system, system nonlinearities, uncertainties, and unknown disturbances. The simulation, experiment, and comparison are conducted to verify the effectiveness and feasibility of the proposed approaches.
The rest of this article includes: Section II is the system description and problem statement. Section III presents the optimal recursive path planning. Section IV presents the vision-based pose estimation using the UKF. Section V describes the dynamic filtered path tracking control. Section VI shows the simulation, experiment, and comparison results. Section VII presents the conclusions. Section VIII discusses future improvement. Fig. 1 is the model of the 3RRR robot used in this study. The robot has three equal kinematics chains, which is described as a serial RRR chain, where R stands for the revolute joint. The fixed base frame {F} is defined at the center of the joint A 1 .  The active frame {E} is defined at the center of the plate with the X axis parallel to C 1 C 2 and the Y axis toward C 3 . The Z axis is directed from the base up. The camera frame {C} is defined at the center of the 3D Intel RealSense camera D435i with the Z axis pointing up. For the i th kinematic chain, i = 1, 2, 3, θ i is the rotation angle of the active link A i B i , ϕ i is the rotation angle of the passive link B i C i . l 1 is the length of the active links. l 2 is the length of the passive links. R is the radius of the circle of the joints A i . r is the radius of the circle of the joints C i . The vector q = θ 1 θ 2 θ 3 T defines the joint space vector as the input variable of the robot. The vector p = x y α T defines the task space vector as the pose of the plate, where x y T is the position of the plate with respect to the frame {F}, α is the rotation angle of the plate. The 3D Intel RealSense camera points to the surface of the plate, captures the image of the colored feature points. It detects and provides the 3D coordinate of the feature points for the pose estimation of the plate. To control motion, the inverse kinematics problem needs to be solved to determine the input variable of the robot based on a given pose of the plate.

A. INVERSE KINEMATICS PROBLEMS
The coordinate of the motor joints with respect to the frame {F} are: The coordinate of the joints C i with respect to the frame {E} are: The coordinate of the joints C i with respect to the frame {F} is expressed as follow: where E F H is the homogeneous transformation matrix from the frame {E} to the frame {F}: From Equations (2), (3), and (4), the coordinate of the joints C i with respect to the frame {F} can be expressed as follow: The constraint of the closed-loop vector in each kinematic chain is: Equation (6) can be rewritten as follow: After solving Equation (7), the input variable of the robot can be obtained as: where:

B. WORKSPACE AND SINGULARITY ANALYSIS
Using the time derivative of Equation (8) yields: where J p is the forward kinematics Jacobian matrix, J q is the inverse kinematics Jacobian matrix.
The inverse singularity happens outside the workspace when the determinant of J q is zero. However, when the pose of the plate is outside the workspace, k ws in Equation (9) will have a negative value, Equation (8) cannot be solved, thereby the determinant of J q is impossible to be calculated. Therefore, in practice, the workspace and inverse singularity is checked by the value of k ws in Equation (9). The forward singularity happens when the determinant of J p is zero. Fig. 2 shows two types of forward singularity. In type (a), the centerlines of the passive links pass through the center of the plate. In type (b), three passive links are parallel. In both types of forward singularity, the motors cannot provide enough torque to move the plate. Therefore, the forward singularities must be detected and avoided when planning the path for the robot.

C. PROBLEM STATEMENT
In operation, the robot is desired to move point to point following a certain task path with high accuracy. In this study, the desired task path of the plate with respect to the frame {F} is defined as follow: where n = 0, . . . , N , N is the total number of points in the path. n = 0 indicates the start point, which is also the current point. The control objective is to control the plate following the desired path and track the pose p x , p y , p α to minimize the error. To achieve the control objective, this study addresses path planning and pose estimation, then proposes a dynamic filtered path tracking control schema to improve the movement accuracy in real time.

III. OPTIMAL RECURSIVE PATH PLANNING
For a given pose p = p x p y p α T of the plate, by solving the inverse kinematics problem, the rotation vector q = θ 1 θ 2 θ 3 T of the active links are obtained. Each active link has two solutions as expressed in Equation (8). Therefore, there are a total of eight moving states of the active links that can correspond to a specific pose of the plate geometrically. Consider Equation (8), the solution corresponding to the positive sign is solution 1, the solution corresponding to the negative sign is solution 2. The eight moving states are defined as in Table 1.
To ensure a stable movement, the path is divided into N equal segments.
[0] indicates the start point of the path. The path planning will drive the plate across points from [1] to [N ]. The points are spaced p = p x , p y , p α , where p x and p y are the distances between two adjacent points in the X and Y axes, respectively, p α is the change in the rotation angle of the plate between two adjacent points. In practice, the distance between two adjacent points p t = p 2 x + p 2 y is defined and used. In this study, the Bresenham algorithm [28], [29] is used and modified to divide the path between the start point and the endpoint into N segments contained in the path with ( p t , p α ) given. By experiment, p t = 10 mm is selected in this study ( p α is determined by equally dividing the desired rotation angle of the plate by N ). Each point in the path , from [1] to [N ], will be solved by the inverse kinematics problem and singularity analysis to obtain the two-dimensional array A. The first dimension of A, denoted as row, is the index of points, which has the value of zero to N . The second dimension, denoted as col, is the index of possible combinations of moving states of the active links for each point, which has a value of 1 to 8. Each element of A, expressed as A [row, col], is a record of q and det J p .
Each point in the path has a total of eight moving states of the active links, so the path has a total of 8 N possible combinations of moving states of the active links. Each combination needs to be evaluated to select the best states for the movement. The evaluation includes avoiding forward singularity and minimizing the distance traveled by active links. If all 8 N combinations are evaluated, the solution is optimal and the evaluation is called the absolute evaluation. However, it is impossible in practice due to the computation time. For example, suppose N = 12, there are more than sixty-eight billion possible combinations. It will take a very long time for evaluation and be impossible to apply in real-time control. Therefore, this study proposes a novel approach, which still considers all possible states but reduces the number of evaluated combinations to speed up the calculation.
In this approach, N points contained in the path are divided into G groups with N points in each group. The absolute evaluation will be implemented in each group. It considers all the moving states to select the best states for the movement. Transitions between groups are negligible because the points are a small distance apart. Finally, the movement of the path is very smooth and optimal. The total of possible combinations of moving states is G × 8 N . Returning to the above example with N = 12, suppose G = 3 and N = 4, the total combinations is 12288, which is greatly reduced. The evaluation will be executed very fast and easily in real time.
The evaluation process can be expressed as the following optimization problem: where N is the number of points in each group. n is the index of the point in the group. k is the index of the kinematic chain. ζ is the limit value to ensure that the robot does not fall into the forward singularity. The optimal recursive path planning is presented in Algorithm 1. The returned parameter is the N − element array BestState in which each element contains the index of the best of the eight moving states of the active links at the corresponding point. N is an optional parameter, which is selected to ensure a balance in computation time and smoothness of the movement. In this study, N = 4 produces the desired results.

IV. VISION-BASED POSE ESTIMATION USING THE UNSCENTED KALMAN FILTER A. VISION-BASED POSE ESTIMATION
In the pose estimation, the position and rotation angle of the plate is determined with respect to the frame {C} by the vision system using the 3D Intel RealSense Camera D435i. After that, the pose with respect to the frame {F} will be calculated by using the homogeneous transformation matrix C F H from {C} to {F}. Because the 3RRR robot is a planar robot, the depth Z respect to the frame {C} is ignored. The camera is placed at the center of the triangle A 1 A 2 A 3 . The homogeneous transformation matrix C F H is as follow: BestCost ← 1000000 8: ORA(j, j, min(j + N − 1, N )) 9: j ← j + N 10: end while 11: procedure ORA(pos, index, des) 12: if (index > des) 13: Cost ← 0 14: ppos ← pos −   the SDK of the Intel RealSense Camera D435i. Fig. 4 presents the feature points detected in the control software. The coordinate of the plate with respect to the frame {C} is calculated as follow: where (F ix , F iy ) is the coordinate of the i th feature point with respect to the frame {C}. Finally, the pose of the plate with respect to the frame {F} is determined as follow:

B. UNSCENTED KALMAN FILTER FOR THE POSE ESTIMATION
The vision-based pose estimation has an error due to the calibration of the camera, the noise in image processing, the vibration of the mechanical system, and unknown disturbances. In this study, the UKF is proposed and configured to reduce the error in the pose estimation before pass it to the dynamic filtered path tracking control. Fig. 5 shows the process of the UKF for the pose estimation.

1) DESIGN THE SYSTEM MODEL
Equation (10) is rewritten as follow: VOLUME 8, 2020 where J p is the forward kinematics Jacobian matrix, J q is the inverse kinematics Jacobian matrix. J p and J q are calculated by Equations (11) to (18). ω i is the angular velocity of the i th active link of the robot.
The vision system tracks the pose of the plate following the path . The state variable is expressed as p = x y α T .
Equation (24) can be rewritten as follow: where ω = ω 1 ω 2 ω 3 T is the angular velocity vector of the active links. Equation (25) in discrete form is described as follow: where k and k+1 are the current and next instant, respectively. t is the discrete interval.

2) DESIGN THE SIGMA POINTS
The dimensionality of the state variable is d = 3, so there are 2d + 1 = 7 sigma points required for the filter. The sigma points are computed as follows: where X is the sigma points matrix of size 3 × 7.p is the mean vector of the state variable. λ is the scaling factor. C p is the covariance matrix of size d × d = 3 × 3. The squared root of the matrix (d + λ) C p can be obtained by using the Cholesky factorization. The subscript col indicates the column of the matrix. For initialization, the covariance matrix C p is set by the conjectured standard deviation of elements of the state variable, the mean vectorp is set equal to the desired pose of the plate. For other time instants,p and C p are the estimation from the end of the previous time instant. The scaling factor λ is defined as follow: where α is the spread of the sigma points around the mean value. κ = 3 − d is the second scaling factor. In this study, α = 10 and κ = 0. The weights are computed as follows: where w m col is the weights for the mean corresponding to the col th sigma point. w c col is the weights for the covariance corresponding to the col th sigma point. The factor β = 2 is the optimal value for Gaussian problems.

3) THE PREDICTION STEP
The predicted mean and covariance are calculated as follows: where col indicates the column of the matrix. N p is the process noise covariance matrix of size d ×d = 3×3. P is the mapping sigma points matrix. g (·) is the nonlinear function, which is calculated based on Equation (26).

4) THE UPDATE STEP
The vision system measures the pose of the plate and results in a pose vector M = x y α T . This measurement has an error due to the calibration of the camera, the noise in image processing, the vibration of the mechanical system, and unknown disturbances. The difference M between the measurement M and the predicted meanp must be calculated as follow: After that, the sigma points need to be transformed into the measurement space based on the difference M by using a function h (·) as follow: where Z is the transformed sigma points matrix of a size 3×7 corresponding to P. Each column of Z can be calculated as follow: The measured mean and covariance are calculated as follows: where col indicates the column of the matrix. N s is the sequences noise covariance matrix of size d × d = 3 × 3.
The cross variance is calculated as follow: The Kalman gain is determined as follow: Finally, the new state and covariance are estimated as follows:p The new state and covariance are used for the next instant repeated from the step of designing the sigma points.

V. DYNAMIC FILTERED PATH TRACKING CONTROL
To obtain the control objective discussed in Section II.C and evaluate the effectiveness of the proposed methods, the schema of dynamic filtered path tracking control is designed as shown in Fig. 6. Part A of Fig. 6 is the optimal recursive path planning. The desired path is analyzed and divided into points. The optimal rotation angles of the motors are calculated using the inverse kinematics problem and the optimal recursive algorithm. The optimal rotation angles of the motors corresponding to the points of the desired path are the reference input of the motion control. The optimal recursive path planning is presented in Section III. The part B of Fig. 6 is the vision-based motion control. In this study, motor control is handled by the PID control embedded in the motor controller board. The vision system using the Intel RealSense Camera D435i measures the pose of the plate as presented in Section IV.A. The measurement is smoothed out by the UKF as presented in Section IV.B. The corrected pose of the plate is used as the feedback to compensate for the pose error. This process is performed in real time when the robot is running.
In Fig. 6, p d is the desired pose of the plate corresponding to each point in the desired path. q d is the desired rotation vector of the active links. p d and q d are presented in Section III. The relation between p d and q d is as follow: where T is the transformation of the inverse kinematics problem presented in Section II.A. q r is the real rotation vector of the active links determined by the motion controller. ω d is the desired angular velocity of the active links. ω r is the real angular velocity determined by the motion controller. p r is the real pose of the plate after the position control. p e is the pose estimation of the vision system after corrected by UKF. The relation between p r and p e is as follow: where F is the apparent function for the vision-based pose estimation and UKF presented in Section IV. p e is then compared with p d to calculate the compensation for the pose error. This is a closed-loop control that is performed repeatedly so that the pose error is controlled within the desired limit.
The verification of the proposed control approach will be presented in the next section.

VI. SIMULATION, EXPERIMENT, AND COMPARISON
A. HARDWARE Fig. 1 shows the 3RRR robot used in this study. The length of links are l 1 = 180mm, l 2 = 180mm. The other dimensions include The vision system is the 3D Intel RealSense Camera D435i. The camera has the resolution of 1280×720, the frame rate of 30fps, the depth accuracy of less VOLUME 8, 2020 than 0.5 percent of the distance from the object. Because the process interval of the control software is t = 50ms, the real frame rate of the camera in the program is 20fps. The distance between the camera and the object is 780mm, so the expected accuracy of the camera is between 1.8mm to 3.9mm.
The simulation is implemented in MATLAB. The control software for the experiment is developed in Visual C++ by our lab. The experiment was conducted on a laptop with medium hardware included Intel Core i5 9 th Gen, 8GB RAM, SDD 120GB, Graphics card Nvidia GeForce GTX 1050 3GB.   Fig. 7 to Fig. 9 are the workspace scanning data for three different rotation angles α of the plate. The red lines are the connections between the motors. The blue area is where the plate can reach. As presented in Section II.B, the workspace can be determined by checking the value of k ws in Equation (9). When k ws < 0 it is out of the workspace. The workspace is depended on the rotation angle of the plate.       angle α = 0 • . The black area is the forward singularity area. As presented in Section II.B, the forward singularity   occurs when det(J p ) = 0. In practice, the control program will detect the forward singularity when the value of det(J p ) VOLUME 8, 2020 close to zero. In the figures, the dark black area is very near to the singularity point. The black fades as it gets further away from the singularity point. It can be seen that, at every position in the workspace, there is always at least one state that does not fall into the forward singularity. This is still true when changing the rotation angle α of the plate. This fact makes sure that the proposed optimal recursive path planning algorithm always can find an optimal path to avoid the forward singularities.

C. OPTIMAL RECURSIVE PATH PLANNING
To verify the proposed optimal recursive path planning, the path is defined from the start point (x = 150mm, y = 20mm, α = 0 • ) to the endpoint (x = 300mm, y = 250mm, α = 0 • ). Based on Fig. 10 to Fig. 17, the path passes through several points where several moving states fall into the forward singularity. The path is divided into 231 points. The inverse kinematics problem is solved for all points, then the optimal recursive path planning is conducted to determine the best working state. Table 2 is the resulted data of points from 166 to 173. Before point 170, the moving state 5 is chosen because it satisfies the least moving distance of angles θ i and the links do not fall into the forward singularity. From point 170, the moving state is changed to 1 because the moving state 5 is too close to the forward singularity. The angle θ 1 changes quite large. However, it remains the state with the least distance traveled. The data of points in the path created by the optimal recursive path planning (The translation unit is millimeter, the rotation unit is degree).

D. DYNAMIC FILTERED PATH TRACKING CONTROL
Section V has presented the dynamic filtered path tracking control schema, which applies the optimal recursive path planning and vision-based pose estimation using UKF. This section will verify the proposed control in both simulation and experiment.

1) INITIALIZATION OF UNSCENTED KALMAN FILTER
In Equations (28) and (29), the factors are set as α = 10, β = 2, κ = 3 − d = 0. In Equation (30), N p = diag σ p σ p , where is the Hadamard product, σ p is the standard deviation of the process, σ p = (0.5, 0.5, 0.00873). In Equation (34), N s = diag (σ s σ s ), where σ s is the standard deviation of the measurement, σ s = (3, 3, 0.0262). In Equation (27), the initial covariance matrix is set temporarily as C p = N p , the initial mean vectorp is set equal to the desired pose of the plate at the start point of the path.

2) SIMULATION
The desired path is defined from the start point (x = 100mm, y = 100mm, α = 0 • ) to the endpoint (x = 150mm, y = 150mm, α = 0 • ). This is a short trajectory to be easily observed data on the figure. The desired angular velocity is ω d = π rad/s. As presented in Fig. 6, the feedback angular velocity ω r , the feedback rotation q r , and the measurement of the vision system are required for the UKF.
In the simulation, the feedback angular velocity is set as ω r = ω d + random_noise_ω. Consider that the motor has a maximum error of 0.5 degrees after a rotation, random_noise_ω is generated randomly in the range [−0.00139ω d , 0.00139ω d ] rad/s, where the unit of ω d is rad/s. The feedback rotation is set as q r = q d + random_noise_q, where random_noise_q is generated randomly in the range [−0.54, 0.54] degrees. The value of 0.54 degrees is corresponding to three pulses of the motor encoder. q r , ω r are used in Equation (26) to calculate the nonlinear function g (·) in Equation (30).
The measurement of the vision system is simulated by adding noise into the desired pose at the current point. Based on the accuracy of the 3D Intel RealSense Camera D435i, the noise is generated randomly in the range [−5, 5] mm for the coordinates X and Y , and in the range [−0.06, 0.06] rad for the rotation angle α.  The simulation is repeated three times. Fig. 18 to Fig. 20 show the path tracking data in the first simulation.  The dot green line is the measurement of the vision system, which is simulated noise very much like the real system. The dashed red line is the real value. The solid blue line is the estimation of the UKF. It can be seen that the UKF can estimate the pose of the plate very close to the real pose. Table 3 shows the summary of mean squared error (MSE) and mean absolute error (MAE) of the path tracking data in three simulations. The data changes in each simulation due to the random value of noise. However, in all three simulations, the pose estimation is corrected and the errors are greatly reduced by using UKF.

3) EXPERIMENT
In the experiment, the robot will move the plate following the spiral trajectory as shown in Fig. 21. The trajectory includes sixty points. The feedback angular velocity ω r and rotation q r are provided by the motor controller board. ω r and q r are used in Equation (26) to calculate the nonlinear function g (·) in Equation (30). The measurement of the vision system is provided based on the real 3D Intel RealSense Camera D435i. When the plate moves from one point to the next point, the UKF vision-based measurement is used as the feedback of the position control to compensate for the pose error. At each point, the program will be temporarily stopped to measure the real pose manually by instrument. The manually measured real pose is considered as the ground truth in the experiment.   Fig. 24 show the path tracking data in the experiment. The dashed red line is the manual measurement. The dash-dot yellow line is the desired value created by the control program. The dot green line is the measurement of the vision system. The solid blue line is the measurement of the vision system after smoothing by the UKF. By applying the UKF, the pose measurement is corrected very close to the real pose. It will help the position control to compensate for the pose error continuously. That is why when the program stops to measure the pose manually, the real value is very close to the desired value as shown in the figures. Table 4 is a summary of the errors in the experiment. This is the comparison to the real values calculated during the movement. The MAE of the vision system is larger than the inherent measurement error of the 3D Intel RealSense Camera D435i and the measurement has a lot of noise when the robot is moving. However, The UKF can smooth and correct the measurement. The MAE after using UKF is small.

E. COMPARISON 1) THE COMPARISON OF THE PATH TRACKING CONTROL
To see the difference when applying the dynamic filtered path tracking control, the spiral trajectory moving experiment is repeated using the PID control without the UKF visionbased feedback. Fig. 25 shows the comparison of the manual measurement (real position) in two cases. Table 5 is the summary of errors in two cases compared to the desired values. If only the PID control is used, the pose error is quite big due to the vibration of the mechanical system and the noise such as the backlash of the motor gearbox. By using the UKF visionbased feedback, the pose error is reduced significantly.

2) THE EXTENDED COMPARISON OF THE OPTIMAL RECURSIVE PATH PLANNING
A popular path planning method of the 3RRR planar parallel robot to improve the kinematics and avoid the singularities is the working (actuation) mode conversion, which is proposed in [13] and [14]. In this method, the selected working mode   out of the eight possible working modes is changed after each step. By experiments, the best schema of the change is determined. It is expected to avoid the singularity, but it did not mention about the optimum travel distance.
Let's review the experiment of the optimal recursive path planning shown in Section VI.C. As presented in Table 2, the moving state is changed at point 170. However, at many points earlier, the moving state was unchanged. Consider the rotation angles of the active links corresponding to the selected moving states, the working mode (as defined in [13]) is also not changed. The proposed optimal recursive path planning does not need to change the working mode each step, but it still looks to avoid all cases of singularity. Besides, it ensures the movement distance of the active links is minimal. Minimize changing the working state after each step also ensures smooth movement of the links. The smoothness and effectiveness of the optimal recursive path planning proposed in this study can be verified by the video in [30].

VII. CONCLUSION
In this study, the dynamic filtered path tracking control is proposed for the 3RRR planar parallel robot. The optimal recursive algorithm is designed in detail for path planning. By grouping the moving points in the path, the algorithm reduces computation but still ensures all possible moving states are considered. Compared to other methods, this algorithm not only helps the robot avoid singularities but also ensures the shortest movement distance of links thanks to the optimization in path planning. The 3D vision system is used to measure the pose of the robot. To reduce the errors due to the camera's intrinsic parameters, the vibration of the mechanical system, and unknown noises, the unscented Kalman filter is designed to correct the pose measurement and smooth out the movement of the robot. Compared with PID control, the proposed method has the remarkable advantage of reducing the positioning error to about 0.193 to 0.21 mm for translation and 0.36 degrees for rotation. This result is not too good compared to systems using high-tech positioning equipment. However, it is an encouraging result for low-cost robot systems. The simulation, experiment, and comparison are performed to verify the effectiveness and feasibility of the proposed methods.

VIII. FUTURE IMPROVEMENT
The path planning should be conducted by using the wellknown algorithms, then compare with the proposed optimal recursive algorithm in this study.
Some other methodologies of the path tracking control can be studied and applied to the 3RRR robot such as a finite-time tracker for nonholonomic systems [31], fast terminal sliding mode tracking control [32], neural networks-based distributed adaptive control [33], or type-2 fuzzy-based distributed supervisory control [34].
For further research, the dynamic filtered path tracking control could be applied to the robots in 3D such as the Hexa parallel robot or the robot arms.