Because of their elastic links and joints, high-speed parallel robots for pick-and-place operations inevitably suffer from residual vibrations that significantly degrade their positioning accuracy. An effective approach based on the input shaping technique is presented in this paper for suppressing the residual vibration in these parallel robots. After addressing the design principle of an input shaper for a parallel robot with flexible actuated joints, a robust optimal input shaper is developed by considering the configuration-dependent flexible modes and minimizing the maximum percentage of residual vibration at the end-effector. The input shaper allows a good overall performance to be achieved throughout the entire workspace. Experimental results on a 4-DOF SCARA-type parallel robot show that the residual vibration of the end-effector is dramatically reduced and the dynamic positioning accuracy of the robot significantly improved.
1 Introduction
Pick-and-place parallel robots are increasingly used in the food, pharmaceutical, packaging, and numerous other light industries. Typical examples include the delta robot and similar robots [1‐4] that have high speed, accuracy, and stability performance. However, these parallel robots generally have lightweight limbs and suffer from residual vibrations during high-speed operations which significantly degrade their positioning accuracy. Over the last decade, much effort has been made to improve the positioning accuracy of high-speed pick-and-place robots during pick-and-place operations by reducing their residual vibration.
It is difficult to effectively reduce the residual vibration of a robot as it performs high-speed and high-acceleration motion by using speed and acceleration feedforward control or notch filters. Therefore, active vibration reduction has become a research hotspot in related fields. Current active vibration reduction approaches can be roughly classified into the two categories of feedback-based and feed-forward-based approaches. Feedback-based vibration reduction approaches [5, 6] are built upon an online control strategy using real-time vibration signals captured from the end-effector, but the need for additional measuring hardware limits their use in practice. Feed-forward-based approaches [7‐9] are based on modifying the joint inputs according to the dynamic behavior of the system. These approaches are usually combined with a PID regulator to achieve the desired vibration reduction. In input shaping [10‐12], which is a commonly used feed-forward-based approach for vibration reduction, the residual vibration is reduced by convolving the input command with a suitable train of impulses. This technique has been implemented in a wide variety of machines that ranges from machine tools to industrial robots [13‐15]. Because positioning accuracy is important for pick-and-place parallel robots, input shaping is particularly suitable for reducing their residual vibration and thereby improving their dynamic positioning accuracy.
Advertisement
To design a suitable input shaper, the natural frequencies and damping ratios of the system are generally required. However, owing to modelling and experimental uncertainties, these modal parameters may not be known exactly; therefore, the robustness of the input shaper must be ensured. Different input shapers, such as the zero vibration and derivative shaper (ZVD) and the extra insensitive shaper (EI), have been proposed based on modifying the constraints of the zero-vibration shaper [16]. Although the robustness of these input shapers can be enhanced by introducing additional constraints, this invariably leads to large time lags or limited vibration reduction. Furthermore, the robustness of these input shapers against uncertainties is uniquely determined by the constraints. Optimal input shapers with a smaller time lag under a fixed bandwidth have been proposed for systems with an exact range of natural frequencies based on optimal control theory [17, 18]. The optimal tradeoff between efficiency and robustness is achieved in the optimal shape shaper by optimizing the time-lag coefficient. In multi-mode-dominated systems, the two approaches of convoluting many single-mode shapers together and the solution of the full set of multi-mode vibration equations have been used to obtain multi-mode input shapers [19‐21]. Although input shaping was developed mainly for single-mode linear time-invariant systems, its use has gradually extended to nonlinear systems. Its effectiveness in residual vibration reduction for parallel robots has been studied in several works [9, 22, 23]. In addition, adaptive input shapers [24, 25] with online adjustable amplitudes and time lags of impulses have been proposed for nonlinear systems with large uncertainties. Although these adaptive inputs shaper have good performance, online identification of the modal parameters requires the use of high-hardware quality, which limits their practical applications.
A high-speed pick-and-place parallel robot is a typical nonlinear system, and its dynamic properties vary greatly throughout the workspace [9, 26]. The pick-and-place positions of these robots usually cover the entire workspace, and the picked load varies in some application cases, which brings great challenges for input shaping. Therefore, the development of a broadly effective approach for designing a highly robust input shaper with good overall performance throughout the entire workspace would be highly beneficial for improving the dynamic positioning accuracy of high-speed pick-and-place parallel robots.
Driven by numerous practical needs and inspired by existing achievements, an input-shaping-based residual vibration reduction approach to improve the dynamic positioning accuracy of high-speed pick-and-place parallel robots throughout the workspace is presented in this paper. The remainder of this paper is organized as follows. The design principle of the input shaper for a robot with flexible actuated joints is described and a robust input shaping approach that minimizes the percentage of residual vibration at the end-effector throughout the workspace proposed in Section 2. The results from both simulations and experiments to demonstrate the performance of the proposed method using a 4-DOF SCARA-type parallel robot [26] are presented in Section 3. The conclusions are presented in Section 4.
2 Input Shaping of Pick-and-Place Parallel Robot with Flexible Joint
2.1 Design of Input Shaper
Previous studies have shown that the dynamic behavior of a high-speed parallel robot is dominated by the elasticity of the actuated joint. Therefore, the robot can be simplified to a system with a finite number of flexible-body degrees of freedom. Without loss of generality, the dynamic model of a parallel robot with f rigid-body and n vibrational degrees of freedom (here \(f = n\), which equals the number of degrees of freedom of the parallel robot) can be formulated as
where \({\varvec{M}}\), \({\varvec{C}}\), and \({\varvec{K}}\) are the inertia, damping, and stiffness matrices, respectively; \({\varvec{d}}\left( {{\varvec{x}}_{r} ,\;\dot{\user2{x}}_{r} } \right)\) is a disturbance term; and \({\varvec{x}} = {\varvec{x}}_{r} + {\varvec{x}}_{f}\), in which \({\varvec{x}}_{r}\) and \({\varvec{x}}_{f}\) denote the rigid-body and elastic generalized displacements, respectively.
Advertisement
The first f rigid-body displacements in \({\varvec{x}}_{r}\) are set as the rigid-body displacements that correspond to the reference input of the actuated joints and denoted as \({\varvec{x}}_{r}^{Input}\). By dropping the disturbance term and taking the Laplace transform of both sides of the linearized dynamic model for a given configuration of the parallel robot, Eq. (1) can be rewritten as
$$\left( {s^{2} {\varvec{M}} + s{\varvec{C}} + {\varvec{K}}} \right){\varvec{X}}\left( s \right) = {\varvec{KX}}_{r} \left( s \right),$$
(2)
where \({\varvec{X}}\left( s \right)\) and \({\varvec{X}}_{r} \left( s \right)\) are the Laplace transform of \({\varvec{x}}\) and \({\varvec{x}}_{r}\), respectively.
Because of the orthogonality of the natural vibration modes, \({\varvec{M}}\), \({\varvec{C}}\), and \({\varvec{K}}\) can be expressed as
where \(\omega_{i}\) and \(\zeta_{i}\) are the natural angular frequency and damping ratio of the ith mode, respectively; \({\varvec{\varPhi}}= \left[ {\begin{array}{*{20}c} {{\varvec{\phi}}_{1} } & {{\varvec{\phi}}_{2} } & \cdots & {{\varvec{\phi}}_{n} } \\ \end{array} } \right]\) is the modal matrix; and \({\varvec{\phi}}_{i}\) is the modal vector of the ith mode.
Substituting Eq. (3) into Eq. (2), the transfer function matrix can be obtained as
$$\begin{aligned} {\varvec{G}}\left( s \right) = & \frac{{{\varvec{X}}\left( s \right)}}{{{\varvec{X}}_{r} \left( s \right)}} ={\varvec{\varPhi}}\left( {{\text{diag}}\left[ {\frac{{\omega_{i}^{2} }}{{s^{2} + 2\zeta_{i} \omega_{i} s + \omega_{i}^{2} }}} \right]} \right){\varvec{\varPsi}}^{{\text{T}}} \\ = & \sum\limits_{i = 1}^{n} {\frac{{\omega_{i}^{2} {\varvec{\phi}}_{i} {\varvec{\psi}}_{i}^{{\text{T}}} }}{{s^{2} + 2\zeta_{i} \omega_{i} s + \omega_{i}^{2} }}} \approx \sum\limits_{i = 1}^{N} {\frac{{\omega_{i}^{2} {\varvec{\phi}}_{i} {\varvec{\psi}}_{i}^{{\text{T}}} }}{{s^{2} + 2\zeta_{i} \omega_{i} s + \omega_{i}^{2} }},} \\ \end{aligned}$$
The residual vibrations of the robot are superpositions of the free damping vibrations of each mode. The element in the jth row and pth column of the transfer matrix is the transfer function between the reference input \(x_{rj}^{Input}\) of the jth actuated joint and pth output displacement. Because the transfer functions have the same poles, the input commands of the parallel robot can be shaped using the same shaper. The shaped command \(x_{rsj}^{Input} \left( t \right)\) can be obtained as the input sequence
where \(A_{q}\) and \(t_{q}\) are the amplitude and time lag of the qth impulse, respectively; and \({\mathbf{1}}\left\langle {t - t_{q} } \right\rangle { = }1\) at \(t > t_{q}\), \({\mathbf{1}}\left\langle {t - t_{q} } \right\rangle { = }0\) at \(t \le t_{q}\).
2.2 Optimization Problem
High-speed pick-and-place robots generally have stringent response-time requirements. To improve the robustness of the shaper without increasing the time lag, it is necessary to balance the tradeoff between the vibration reduction efficiency and the bandwidth of the input shaper. The sensitivity levels of ZV, ZVD, and EI are uniquely determined by the constraints. They are hence not suitable for handling the wide variation ranges in the natural frequencies of parallel robots in their workspace. In this study, an optimal input shaping approach is used to realize the vibration reduction of high-speed pick-and-place parallel robots throughout the entire workspace. The residual vibrations of high-speed pick-and-place parallel robot are dominated by the first-order mode. Therefore, the design of the input shaper is consistent with that of a linear system with a single degree of freedom. Using optimal control theory, an optimal input shaper [27] with three impulses is designed with the following parameters:
where \(M = 1 - 2\exp \left( { - \zeta_{{{\text{des}}}} \omega_{{{\text{des}}}} T} \right)\cos \left( {\omega_{{d,{\text{des}}}} T} \right) + \exp \left( { - 2\zeta_{{{\text{des}}}} \omega_{{{\text{des}}}} T} \right)\); \(T = {{k_{T} T_{d} } \mathord{\left/ {\vphantom {{k_{T} T_{d} } 2}} \right. \kern-\nulldelimiterspace} 2}\) is the time lag of the impulse sequence, \(0 \le k_{T} \le 1\); \(\omega_{{d,{\text{des}}}} = \omega_{{{\text{des}}}} \sqrt {1 - \zeta_{{{\text{des}}}}^{2} }\) in which \(\omega_{{{\text{des}}}} { = }2\uppi f_{{{\text{des}}}}\), and \(f_{{{\text{des}}}}\) and \(\zeta_{{{\text{des}}}}\) are the designed natural frequency and damping ratio of the input shaper, respectively; and \(T_{d} = {{2\uppi } \mathord{\left/ {\vphantom {{2\uppi } {\omega_{d,1} }}} \right. \kern-\nulldelimiterspace} {\omega_{d,1} }}\), in which \(\omega_{d,1} = \omega_{1} \sqrt {1 - \zeta_{1}^{2} }\).
The amplitude of the residual vibration after applying the last impulse, that is, the percentage of residual vibration, can be expressed as Eq. (7) [28].
Therefore, the design of the optimal input shaper for high-speed pick-and-place parallel robots can be transformed into the closely related problem of the determination of a suitable natural frequency \(f_{{{\text{des}}}}\), damping ratio \(\zeta_{{{\text{des}}}}\), and time lag coefficient \(k_{T}\) to minimize the residual vibration \(V\) throughout the workspace. To address the difficulties posed by the wide variation of the natural frequencies in the workspace, a suitable constant natural frequency should be selected; otherwise, a large sensitivity would lead to a very slow input shaper. To ensure the overall performance of the input shaper throughout the workspace \(W_{t}\), two performance objectives were established as follows.
(1)
Minimization of the maximum residual vibration percentage of the end-effector throughout the workspace, that is, \(V_{\max } \to \min\). The optimization problem can be formulated as
where \(f_{\min }\) (\(\zeta_{\min }\)) and \(f_{{{\text{m}} {\text{ax}}}}\) (\(\zeta_{{{\text{m}} {\text{ax}}}}\)) are the minimum and maximum natural frequencies (damping ratios) of the parallel robot in the workspace, respectively.
(2)
Minimization of the average residual vibration percentage of the end-effector throughout the workspace, that is, \(V_{{{\text{mean}}}} \to \min\). The optimization problem can be formulated as:
The resulting optimization problems can be easily solved by, for example, calling the fmincon function in the MATLAB Optimization Toolbox.
In practice, high-speed pick-and-place parallel robots are time-varying systems during pick-and-place operations, and the solution of the vibration amplitude is very time-consuming. Therefore, the residual vibration percentage is not an efficient index for measuring the effectiveness of an input shaper for a parallel robot. In this study, the effectiveness of the input shaper throughout the workspace was evaluated by using the residual energy index introduced in Ref. [9]:
where \(E\left( t \right)\) is the total residual energy of the vibratory modes, \(E_{u}\) and \(E_{s}\) are the residual energies upon completion of the unshaped and shaped commands, respectively, and \(t_{s}\) is the time required to complete the shaped command.
3 Verification
Both simulations and physical experiments were carried out on a 4-DOF SCARA-type parallel robot to verify the effectiveness of the proposed approach. Figure 1 shows a 3D view of the robot used in this study. The robot has two identical closed-loop sub-chains, two sub-platforms, and a base. Each of the sub-chains is connected to the base at one end by an actuated revolute joint and to the sub-platform at the other end by spherical joints. The required rotation about the z-axis is generated by a relative translation between the two sub-platforms. For details on the dimensional and structural parameters of the robot, please refer to Ref. [26].
×
Based on the kinematics and rigid-body dynamics in Ref. [26], a rigid-elastic coupling dynamic model considering the flexible joints was established as
\({\varvec{\theta}}_{r} = \left( {\begin{array}{*{20}c} {\theta_{r1} } & {\theta_{r2} } & {\theta_{r3} } & {\theta_{r4} } \\ \end{array} } \right)^{{\text{T}}}\), \({\varvec{\theta}}_{f} = \left( {\begin{array}{*{20}c} {\theta_{f1} } & {\theta_{f2} } & {\theta_{f3} } & {\theta_{f4} } \\ \end{array} } \right)^{{\text{T}}}\), \({\varvec{\theta}}_{r}\) and \({\varvec{\theta}}_{f}\) denote the rigid-body and elastic angular displacements of the actuated joints, respectively; \({\varvec{J}}\) and \({\varvec{H}}\) are the velocity Jacobi and second-order influence coefficient matrices of the rigid body motion, respectively; \(\hat{\user2{z}}\) is the z-axis unit vector; \({\varvec{c}} = c{\varvec{E}}_{4}\) and \({\varvec{k}} = k{\varvec{E}}_{4}\), where \(c\) and \(k\) denote the viscous damping and stiffness coefficients of the reducer, respectively; mp denotes the summed mass of the two sub-platforms; \(m_{3}\) and \(I_{3}\) denote the mass and inertia of the end-effector, respectively; \(r\) denotes the radius of the end-effector gear; and \(I_{a}\) and \(m_{a} r_{a}\) are the equivalent moment of inertia and mass-radius product of an actuated limb about its rotary axis, respectively.
Equation (11) can be linearized by ignoring the rigid-elastic coupling and taking the static equilibrium position as the reference point. Note that the rotation angles of the actuated joints are the inputs of the motion control system. \({\varvec{\theta}}_{r}\) is therefore selected as the input of the dynamic model, and the linearized dynamic model can be expressed as Eq. (12).
Obviously, because the modal parameters of the parallel robot are configuration-dependent, the robot behaves as a linear time-varying system during motion. Therefore, the input shaper should be designed considering the configuration-dependent modal parameters. It was noted in Ref. [26] that the first two modes of the robot play a leading role in the residual vibration along the horizontal direction and are the dominant modes that affect the positioning accuracy. Because the first-order natural frequency has the most obvious variation and its variation range includes that of the second-order natural frequency, the residual vibrations caused by the first two modes can be effectively reduced through an input shaper designed based on the first-order natural frequency. To investigate the influence of the payload on the dynamic performance, the distributions of the first-order natural frequencies of the robot with different payloads are shown in Figure 2. The figure shows that the first-order natural frequency is inversely proportional to the payload, but the influence is small within the payload range of 0 to 1 kg.
×
Input shapers are generally less sensitive to damping ratio errors than to frequency errors [9]. The influence of the damping ratio \(\zeta_{{{\text{des}}}}\) on the input shaper was investigated using response surface analysis (the specific results will not be shown owing to space limitations). We found that when \(\zeta_{{{\text{des}}}}\) varied within the range of 0.05 to 0.1, the response surfaces of \(V_{\max }\) and \(V_{{{\text{mean}}}}\) with respect to \(f_{{{\text{des}}}}\) and \(k_{T}\) remained almost unchanged, which implies that the value of \(\zeta_{{{\text{des}}}}\) had no influence on the performance of the input shaper. Here, \(\zeta_{{{\text{des}}}}\) was selected to be 0.075.
To evaluate the effectiveness of the input shaper throughout the workspace of the parallel robot, random experiments were designed based on a numerical simulation and probabilistic analysis. The experiments were performed to obtain the statistical distribution of the residual energy ratio before and after input shaping for arbitrary pick-and-place trajectories. The steps in each random experiment are as follows: (1) Pick-and-place positions in the workplace were randomly selected to construct pick-and-place trajectories, and the payload was randomly selected within the range of 0 to 1 kg. (2) Rough interpolation was performed on the trajectory, and the inputs of the actuated joints were calculated using inverse kinematics. (3) Two input shapers were designed to shape the commands of the actuated joints according to Eqs. (8) and (9), and the residual vibration energy ratios before and after input shaping were calculated using Eq. (10). Figure 3 shows the probability densities for the residual energy ratios in random pick-and-place experiments with a sample size of 1000.
×
Figure 3 shows that the probability densities of the residual energy ratios approximately obeyed a normal distribution. The mean residual energy ratio of the input shaper designed based on \(V_{\max } \to \min\) was 4.9%, which is slightly larger than that of the input shaper based on \(V_{{\text{mean}}} \to \min\) (4%), but the maximum value and standard deviation of the latter are larger. Furthermore, the probabilities of \(V_{e} \le 10{\text{\% }}\) in the entire workspace of the input shapers designed based on \(V_{\max } \to \min\) and \(V_{{\text{mean}}} \to \min\) were 96.3% and 89.8%, respectively. Although the average residual energy ratio of the input shaper designed based on \(V_{\max } \to \min\) was slightly larger, its fluctuation range was comparatively narrower, and a balanced vibration reduction performance could be achieved in the workspace. Therefore, the input shaper designed based on \(V_{\max } \to \min\) was used to shape the input commands of the high-speed pick-and-place parallel robot.
To ensure the overall performance of the input shaper, the first-order natural frequency of the parallel robot was obtained through experimentation. The prototype robot is shown in Figure 4. In the experiment, the hammer excitation method [29] and the Simcenter Testlab Impact Testing system were used to determine the first-order natural frequency of the parallel robot. Figure 5 shows the measured and fitted first-order natural frequencies in the upper, middle, and lower layers of Wt. The optimized parameters of the input shaper are listed in Table 1.
Table 1
Parameters of the input shaper
\(k_{T}\)
\(f_{{{\text{des}}}}\)
\(\zeta_{{{\text{des}}}}\)
0.77
13.7
0.075
×
×
Vibration reduction experiments were also carried out on the prototype robot shown in Figure 4, and the input commands of the actuated joints were shaped in an upper-computer program. The joint torques were measured through the servomotor currents, and the residual vibrations along the u and v axes measured using an IEPE MEAS 7131A-0500 3D accelerometer and analyzed using an LMS Test Lab-Signature Testing Processor. The pick-and-place trajectory was a specific EAC path given in Ref. [30]. Figure 6 shows the measured joint torque curves. The torque curve of joint 1(3) was similar in shape to that of joint 2(4) because of the symmetry of the trajectory. After input shaping, the peak values of the joint torque were reduced by up to 30% compared to those before input shaping. The overall torque curves were smoother, and the fluctuations clearly decreased. However, there were time lags in the torque curves, and the motion time increased by approximately 0.06 s. In addition, the residual vibration of the joint torques at the completion of the pick-and-place operation was reduced by approximately 85% after input shaping.
×
Figure 7 shows the measured residual vibrations along the u- and v-axes. The peak value and maximum magnitude of the residual vibration along the u axis were 35% and 91% of their values before input shaping, respectively. The corresponding reductions along the v-axis were 45% and 86%, respectively. Experiments for different trajectories were also carried out to verify the generality of the proposed method. Similar results were observed, although they will not be reported here because of space limitations. In summary, the proposed input shaper is highly effective for reducing the residual vibration of a high-speed parallel robot during pick-and-place operations. This in turn effectively enhances the dynamic positioning accuracy of the end-effector. Compared with trajectory planning, input shaping has a better vibration reduction effect. However, because it will inevitably produce a time delay, it is more suitable for applications that require high pick-and-place accuracy.
×
4 Conclusions
(1)
A robust optimal input shaper design approach was presented to address the challenges in input shaping that result from the large variation in the dynamic properties of a high-speed pick-and-place parallel robot throughout its workspace. Furthermore, a highly efficient index, the residual energy index, was presented for quick measurements of the vibration reduction efficiency of the designed input shaper.
(2)
Verification was carried out on a 4-DOF SCARA-type parallel robot, and an optimal input shaper designed. The experimental results show that the residual vibration of the joint torques was reduced by approximately 85%, and the maximum magnitude of the end-effector residual vibration reduced by up to 86% after input shaping. This indicates that the residual vibrations in the unshaped commands can be dramatically reduced by up to 80% through the use of the proposed input shaper. This results in a significant improvement in the dynamic positioning accuracy of the end-effector.
Acknowledgements
Not applicable.
Competing Interests
The authors declare no competing financial interests.
Open AccessThis article is licensed under a Creative Commons Attribution 4.0 International License, which permits use, sharing, adaptation, distribution and reproduction in any medium or format, as long as you give appropriate credit to the original author(s) and the source, provide a link to the Creative Commons licence, and indicate if changes were made. The images or other third party material in this article are included in the article's Creative Commons licence, unless indicated otherwise in a credit line to the material. If material is not included in the article's Creative Commons licence and your intended use is not permitted by statutory regulation or exceeds the permitted use, you will need to obtain permission directly from the copyright holder. To view a copy of this licence, visit http://creativecommons.org/licenses/by/4.0/.