1 Introduction

In the last years, the growing energy demand, especially in the manufacturing industry, has been accompanied by an increased environment awareness. Therefore, the focus of the design and control of robotic and mechatronic systems has moved towards the investigation of energy-efficient and cost-effective solutions. This trend is also encouraged by the European Union policies, which plan to decrease the energy consumption by 32.5% by 2030 [1]. Several studies have demonstrated that an intelligent use of industrial robots can reduce the energy consumption while increasing production rate [2,3,33] first optimize spring parameters for 2-DOF planar parallel robot performing a nominal pick-and-place task. Then, the variation of energy consumption is evaluated with respect to the change of the predefined task, optimizing spring preload for each task while spring stiffness remains fixed. Both numerical and experimental tests demonstrate that an energy consumption reduction up to 51.3% can be achieved even when the task to perform significantly differs from the nominal one.

Alternative approaches for achieving energy efficiency in parallel robots can be found in [34] and [35], where the authors optimize the parameters of the functions that define the motion profile, respectively B-spline and Lamé curves (the latter are used for rounding the corners of a pick-and-place trajectory), achieving a substantial energy saving. In [36] and [37] the authors address the problem of reducing the energy expenditure of a 4-DOF parallel manipulator (4-RUU) by optimizing only the placement of a linear movement and a pick-and-place operation, respectively, with respect to the robot base reference frame. Furthermore, Liu et al. [

Fig. 1
figure 1

3D model of the robot (a); search for the intersection between upper and lower arms (b)

2.1 Kinematics

In this section, the inverse kinematics of the 3-DOF parallel manipulator is described, which aims at calculating the position of the joint variables \(\varvec{q} = \begin{bmatrix} q_1&q_2&q_3 \end{bmatrix}^T\) starting from the pose of the end-effector \(\varvec{X} = \begin{bmatrix} x&y&z\end{bmatrix}^T\). The single joint variable is obtained by finding the intersection between the arc of the circumference representing the possible positions of the upper arm end not fixed to the base (point \(C_{i,upper\, arm}\)) and the positions reachable by the lower arm end not linked to the mobile platform (point \(C_{i,lower\, arm}\)), as shown in Fig. 1b. This can lead to three situations:

  • zero intersections: no solutions found, the chosen pose is outside the robot workspace;

  • one intersection: the chosen pose is at the limits of the workspace;

  • two intersections: only one of them is acceptable, namely the one that leads to a smaller \( q_i \), as it is illustrated in Figs. 1b and 2b.

The kinematic relationship that corresponds to this consideration can be obtained starting from the closing equation of the mechanism:

$$\begin{aligned} \varvec{b_i}^T \varvec{b_i} = b^2 \end{aligned}$$
(1)

where:

$$\begin{aligned} \varvec{b_i} = \varvec{B_i} - \varvec{C_i} = \begin{bmatrix} b_{ix}&b_{iy}&b_{iz} \end{bmatrix}^T \end{aligned}$$
(2)
Fig. 2
figure 2

Notation used in the model of the manipulator: top view (a), lateral view (b), and particular of lower arms movement (c)

Fig. 3
figure 3

Simplification of the lower arms

The positions of points \(\varvec{C_i}\) are derived by knowing the coordinates of points \(\varvec{A_i}\), which are fixed and given by the robot geometry, as function of joint variables \(q_i\) and length of upper arm a (Fig. 2). The coordinates of points \(\varvec{B_i}\) can be obtained starting from the end-effector pose \(\varvec{X}\) and by taking the geometric constants \(r_b\) and h into account.

Substituting expression of \(\varvec{B_i}\) and \(\varvec{C_i}\) in Eq. 2 and applying Eq. (1) leads to the following system of equation:

$$\begin{aligned} I_i\cos (q_i)+L_i\sin (q_i)+K_i=0\,, \;\;\; \text {i=1, 2, 3} \end{aligned}$$
(3)

where \(I_i\), \(L_i\) e \(K_i\) are constants that are function of the robot geometry and the end-effector pose \(\varvec{X}\). Solving Eq. (3), the solution of the inverse kinematic problem for the i-th joint variable can be written as follows:

$$\begin{aligned} q_i = 2\arctan \frac{-I_i - \sqrt{I_i^2 + L_i^2 - K_i^2}}{K_i -L_i}\,, \;\;\; \text {i=1, 2, 3} \end{aligned}$$
(4)

It can be noticed that these solutions are real only if the radicands are greater or equal to zero; if one of them becomes negative, it means that the robot pose \(\varvec{X}\) is outside the robot workspace. Therefore, by imposing this condition, the robot workspace can be easily derived.

The inverse differential kinematics of the parallel robot can be obtained by differentiating Eq. (1) with respect to time and it is given by:

$$\begin{aligned} \varvec{\dot{q}}=\varvec{J_q}^{-1} \varvec{J_x} \varvec{\dot{X}} = \varvec{J}^{-1} \varvec{\dot{X}} \end{aligned}$$
(5)

where:

$$\begin{aligned} \varvec{J_q}= \begin{bmatrix} \varvec{u_1}(\varvec{a_1}\times \varvec{b_1}) &{} 0 &{} 0 \\ 0 &{} \varvec{u_2}(\varvec{a_2}\times \varvec{b_2}) &{} 0 \\ 0 &{} 0 &{} \varvec{u_3}(\varvec{a_3}\times \varvec{b_3}) \end{bmatrix} \end{aligned}$$
(6)

and

$$\begin{aligned} \varvec{J_x}= \begin{bmatrix} x_{b1} &{} y_{b1} &{} z_{b1} \\ x_{b2} &{} y_{b2} &{} z_{b2} \\ x_{b3} &{} y_{b3} &{} z_{b3} \\ \end{bmatrix} = \begin{bmatrix} \varvec{b_1} &{} \varvec{b_2} &{} \varvec{b_3} \\ \end{bmatrix} ^T \end{aligned}$$
(7)

\(\varvec{J}\) is the Jacobian matrix, the vector \(\varvec{u_i}\) is the generic unit vector parallel to motor axis (Fig. 2a), whereas \(\varvec{a_i} = \varvec{C_i} - \varvec{A_i}\) is the vector of the upper arm.

Finally, the inverse acceleration kinematics can be derived by differentiating Eq. (5) with respect to time, obtaining:

$$\begin{aligned} \varvec{\ddot{q}}=\varvec{J}^{-1} \varvec{\ddot{X}}+\varvec{J_q}^{-1} (\varvec{\dot{J}_x}-\varvec{\dot{J}_q} \varvec{J}^{-1}) \varvec{\dot{X}} \end{aligned}$$
(8)

2.2 Dynamics

The dynamics of the 3-DOF parallel robot can be solved adopting the Lagrangian approach restricted to some simplifying assumptions, necessary to facilitate its implementation and resolution. As main hypothesis of the dynamic model, it is supposed that the inertia of the lower arms is small enough to be neglected, and the distributed mass of each pair of lower arms is approximated by two equal point masses positioned at the extremities of the link, as shown in Fig. 3.

Another simplification is to reduce the mobile platform to a mass concentrated at the point \(\varvec{B_0}\) (Fig. 2b). This is a reasonable approximation, since the mobile platform can only translate, so its motion is the same of its center of mass (approximated to the point \(\varvec{B_0}\)) and therefore it can be considered acceptable to neglect its rotational inertia and assimilate it to a point body. Aerodynamics effects are also neglected.

The purpose of the inverse dynamics is to compute the torque needed for the movement of the parallel robot by knowing the position, velocity and acceleration of the joint variables. The total torque \(\varvec{\tau } = \begin{bmatrix} \tau _1&\tau _2&\tau _3 \end{bmatrix}^T\) needed to steer the robot during its motion can be obtained as sum of the torques required to move the different components of the system, i.e., \(\varvec{\tau _{act}}\) for the actuating system (actuators and upper arms), \(\varvec{\tau _{plat}}\) for the mobile platform, and \(\varvec{\tau _{load}}\) for the payload. The first torque contribution can be written as follows:

$$\begin{aligned} \varvec{\tau _{act}}=\varvec{I_{act}} \varvec{\ddot{q}}+g \varvec{M_{arm}}\cos (\varvec{q})+\varvec{f_s}+\varvec{F_v}\varvec{\dot{q}} \end{aligned}$$
(9)

The first term is the inertia term, proportional to the angular acceleration of the joints \(\varvec{\ddot{q}}\). \(\varvec{I_{act}}=\text {diag}(\begin{bmatrix} I_{eq}&I_{eq}&I_{eq} \end{bmatrix})\) is the inertia matrix, where \(I_{eq}=i_{rid}^2I_{act}+I_a+I_b\) represents the equivalent inertia of a single arm evaluated with respect to the corresponding point \(A_i\), \(i_{rid}\) is the gear ratio, \(I_{act}\) is the inertia of the motor, \(I_a =\frac{1}{3}m_a a^2\) is the inertia of the upper arm and \(I_b=m_b a^2\) is the inertia of the point mass \(m_b\). The second term is the gravitational term, given by the upper arm mass \(m_a\) and by the point mass \(m_b\) located at its extremity. These contribution are comprised in matrix \(\varvec{M_{arm}}=\text {diag}(\begin{bmatrix}m_{eq}&m_{eq}&m_{eq}\end{bmatrix})\), where \(m_{eq}=\frac{1}{2} m_a a+m_b a\), whereas g is the gravitational acceleration. Furthermore, the third and fourth terms of Eq. 9 take into account the contribution of Coulomb and viscous friction, respectively: \(\varvec{f_s}=f_s\tanh (\varvec{\dot{q}})\) is the static friction torque, \(\varvec{F_v}=\text {diag}(\begin{bmatrix}f_v&f_v&f_v \end{bmatrix})\) and \(f_v\) is the viscous friction coefficient. The definition of Coulomb friction permits to avoid discontinuities in the torque profile as the sign of the joint velocity changes [41]. By substituting \(\varvec{\ddot{q}}\) with Eq. 8, Eq. 9 can be rewritten as function of the velocity \(\varvec{\dot{X}}\) and acceleration \(\varvec{\ddot{X}}\) of the end-effector as:

$$\begin{aligned} \varvec{\tau _{act}}=\varvec{I_{act}} \varvec{J}^{-1}\varvec{\ddot{X}}+\varvec{J_q}^{-1} (\varvec{\dot{J}_x}-\varvec{\dot{J}_q} \varvec{J}^{-1}) \varvec{\dot{X}}+g \varvec{M_{arm}} \cos (\varvec{q})+\varvec{f_s}+\varvec{F_v} \varvec{\dot{q}} \end{aligned}$$
(10)

The second torque contribution analyzed is the one given by the mobile platform. From the principle of virtual work, the required torque is given by:

$$\begin{aligned} \varvec{\tau }=\varvec{J}^T \varvec{F} \end{aligned}$$
(11)

where \(\varvec{F}\) is the vector of forces acting on the mobile platform. Since it can only translate, the acting forces are only the inertia force and the force of gravity, which can be written as:

$$\begin{aligned} \varvec{F}=\varvec{M_{plat}} (\varvec{\ddot{X}}+\varvec{G}) \end{aligned}$$
(12)

where \(\varvec{M_{plat}}=\text {diag}(\begin{bmatrix}m_p&m_p&m_p\end{bmatrix})\), \(m_p=m_{plat}+3 m_b\), where \(m_{plat}\) is the mass of the mobile platform and \(\varvec{G}=\begin{bmatrix} 0&0&-g\end{bmatrix}^T\). It follows that the torque required by the motors due to the mobile platform is:

$$\begin{aligned} \varvec{\tau _{plat}}=\varvec{J}^T \varvec{M_{plat}} (\varvec{\ddot{X}}+\varvec{G}) \end{aligned}$$
(13)

The last torque contribution is given by the possible payload connected to the end-effector. Since this is connected to the mobile platform, its movement will be the same as the latter and the required torque will be calculated in the same way. Therefore, the torque required by the motors due to the payload is:

$$\begin{aligned} \varvec{\tau _{load}}=\varvec{J}^T \varvec{M_{load}} (\varvec{\ddot{X}}+\varvec{G}) \end{aligned}$$
(14)

where \(\varvec{M_{load}}=\text {diag}(\begin{bmatrix}m_{load}&m_{load}&m_{load}\end{bmatrix})\) and \(m_{load}\) is the mass of the payload.

Finally, the resulting expression for the torque needed for the movement of the parallel robot can be expressed by:

$$\begin{aligned} \begin{aligned} \varvec{\tau }\!=\![\varvec{I_{act}} \varvec{J}^{-1}\!+\!\varvec{J}^T (\varvec{M_{plat}}\!+\!\varvec{M_{load}})] \varvec{\dot{X}} \!+\!\varvec{J}^T (\varvec{M_{plat}}+\varvec{M_{load}}) \varvec{G}+ \\ \varvec{I_{act}} \varvec{J_q}^{-1} (\varvec{\dot{J}_x} \!-\!\varvec{\dot{J}_q} \varvec{J}^{-1}) \varvec{\dot{X}}\!+\!g \varvec{M_{arm}} \cos (\varvec{q}) \!+\!\varvec{f_s}\!+\!\varvec{F_v} \varvec{\dot{q}} \end{aligned} \end{aligned}$$
(15)

2.3 Electro-mechanic model

The electro-mechanic model of robot actuators is needed to assess energy consumption and it is developed by considering equivalent brushless DC motors, as in [36]. The i-th motor armature current \(i_i(t)\) can be computed as function of the torque provided by the motor \(\tau _{m,i}=\tau _i/i_{rid}\) and the motor torque constant \(K_t\) as follows:

$$\begin{aligned} i_i(t)=\frac{\tau _{m,i}}{K_t} \,, \;\;\; \text {i=1, 2, 3} \end{aligned}$$
(16)

Then, the voltage drop \(v_i(t)\) across the i-th motor can be expressed as follows:

$$\begin{aligned} v_i(t)=R\, i_i(t)+K_e\, \dot{q}_{m,i}(t) \,, \;\;\; \text {i=1, 2, 3} \end{aligned}$$
(17)

where R is the resistance of the motor windings, \(K_e\) is the back-emf constant, whereas \(q_{m,i}=q_i i_{rid}\) is the velocity of the i-th motor shaft. Considering the driver efficiency \(\eta _d\), the voltage-current product gives the instantaneous electric power drawn by the i-th robot actuator:

$$\begin{aligned} P_{e,i}(t)=\frac{v_i(t)\, i_i(t)}{\eta _d} \,, \;\;\; \text {i=1, 2, 3} \end{aligned}$$
(18)
Fig. 4
figure 4

Path of the robot end-effector in the 3D space

When the actuators absorb energy from the drive unit, the electric power \(P_{e,i}\) has positive values. Vice versa, when the energy takes the opposite path going from the actuators to the drive unit, \(P_{e,i}\) takes negative values. However, the drives considered for this system are not regenerative. Consequently, the total energy consumption \(E_c\) is calculated taking into account only the power consumed by the three actuators \(P_{c,i}(t)\) and integrating this over the time period T, obtaining the following equation:

$$\begin{aligned}{} & {} E_c\!=\!\sum ^3_{i=1}\int _{0}^{T}P_{c,i}(t)dt \qquad \text {where}\nonumber \\{} & {} \left. P_{c,i}(t)\!=\! \right. {\left\{ \begin{array}{ll} P_{e,i}(t) &{} \text {if }P_{e,i}(t)\ge 0\\ \;\, 0 &{} \text {if }P_{e,i}(t)<0 \end{array}\right. } \end{aligned}$$
(19)