- Email: [email protected]
Forward kinematic model improvement of a spherical parallel manipulator using an extra sensor
Mechanism and Machine Theory 91 (2015) 102–119
Contents lists available at ScienceDirect
Mechanism and Machine Theory journal homepage: www.elsevier.com/locate/mechmt
Forward kinematic model improvement of a spherical parallel manipulator using an extra sensor Houssem Saafi, Med Amine Laribi, Said Zeghloul ⁎ Dept. GMSC, PPRIME Institute CNRS, University of Poitiers, ENSMA, UPR, 3346, France
a r t i c l e
i n f o
Article history: Received 5 October 2014 Received in revised form 7 April 2015 Accepted 8 April 2015 Available online xxxx Keywords: Spherical parallel manipulator Direct kinematic model Parallel Singularity Redundant sensor
a b s t r a c t In this paper an approach to improve the calculation of the forward kinematic model (FKM) of a spherical parallel manipulator (SPM) is presented and tested experimentally. This approach is based on adding an extra sensor on a passive joint. The measure of the passive joint angle reduces the complexity of the forward kinematic model and makes it possible for real time. The use of the extra sensor eliminates the effect of the parallel singularity on the FKM accuracy. An investigation is carried out to choose the position of the extra sensor that gives the better accuracy of the FKM. Experiments are carried out to prove the benefit of the redundant sensor on the FKM. © 2015 Elsevier Ltd. All rights reserved.
1. Introduction Spherical parallel manipulators (SPMs) are parallel mechanisms, with a fixed center of rotation, which provide three degrees of freedom of pure rotation. Different varieties of these mechanisms have been studied before. Gosselin et al. developed a master [1] and slave [2] system to control the orientation of a camera. In [3] and [4], four degrees of freedom haptic devices for Minimally Invasive Surgery applications are proposed. The geometric parameters of the SPM are studied and optimized for two different applications in [4] and [5]. The kinematics of the SPM has been well presented in those works. The forward kinematic model (FKM) of the SPM determines the orientation of the mobile platform in a function of the actuated joint angle values. Due to the nonlinear trigonometric system of equations generated from the multi-loop architecture of the SPM, the FKM, usually, does not allow for closed-form solutions and needs a high computational time. Moreover, the presence of parallel singularity in the workspace of the SPM makes the calculation of the FKM not possible and inaccurate close to the singular configurations. Gosselin et al. [1,2] proposed a special architecture for the SPM with a large singular-free workspace; however a problem of interference was reported due to the big size of the links. This problem of interference was studied also by [6]. The FKM of the SPM, developed by Gosselin et al. [1], was solved using an iterative method using the Jacobian matrix. In [7], the FKM is solved also by iterative techniques, based on the Newton–Raphson method. Unfortunately, iterative methods require both the SPM to be sufficiently far away from singularity and a good initial guess of the actual orientation of the mobile platform, two conditions which cannot always be satisfied and can seriously affect the robustness of these methods. Extra-sensor approach is used in this paper. A theoretical study of this approach is presented in the work of Vertechy and ParentiCastelli [8]. According to [8], the extra sensors are added for at least one of the following reasons: 1) to render the FKM an explicit problem with a closed form solutions; 2) to render the FKM a linear problem, which makes it possible to find the actual orientation ⁎ Corresponding author. E-mail addresses: houssem.saafi@univ-poitiers.fr (H. Saafi), [email protected] (M.A. Laribi), [email protected] (S. Zeghloul).
http://dx.doi.org/10.1016/j.mechmachtheory.2015.04.006 0094-114X/© 2015 Elsevier Ltd. All rights reserved.
H. Saafi et al. / Mechanism and Machine Theory 91 (2015) 102–119
103
of the mobile platform; 3) to speed-up the computation of the FKM solution; 4) to make the method robust against SPM singular configurations; and 5) to improve the accuracy of the solution by reducing the influence of the joint sensor sensibility. In this work, the extra sensor is added, first, to make the FKM robust against the singular configuration, second, to speed-up the computation time of the FKM and finally, to improve the accuracy of the FKM. The choice of the number as well as the location of additional sensors is an important issue addressed in the literature. Bonev et al. [9] proposed an optimal method to locate three linear extra sensors on a general parallel manipulator in order to reduce the complexity of the FKM. Merlet [10] demonstrated that placing three linear extra sensors on a general six d.o.f parallel robot are sufficient to get a closed form solution. In [11], three extra sensors were installed on the passive joints to simplify the dynamic model of 3-RRR planar parallel robot. The SPM presented in this paper is used as a haptic master device for a medical tele-operation system [4,12], (Fig. 1a). This device controls a surgical robot [13] (Fig. 1b). The medical application requires a system having three degrees of freedom of pure rotation as well as a translation. The master architecture was chosen for this reason. This device reproduces the force feedback to the user's hand. A solution based on actuator redundancy is proposed in [14] to eliminate the effect of the singularity in the haptic control. The paper is organized as follows. In Section 2, the kinematic model of the SPM is presented and detailed. In Section 3, an investigation to choose the best accurate location of the extra-sensor is made. A numerical validation is presented in Section 4. The FKM with the extra-sensor is experimentally tested in Section 5. Finally, Section 6 summarizes this paper. 2. Kinematic model of the SPM The SPM is shown in Fig. 2a. It consists of three identical legs connecting the base to a common moving platform. Each leg has one active joint fixed on the base and two passive joints connecting, respectively, the proximal link to the distal link and the distal link to the moving platform. The angles α, β, and γ represent the dimension of the proximal link, distal link, and the end-effector, respectively, see Fig. 2b. All the axes of revolute joints intersect in a common point noted CoR (Center of Rotation). As in [1] and [2], the considered SPM in this paper is a special type. Indeed, the joint base axes are located on an orthogonal frame. This choice was made to simplify the forward kinematic model and the inverse kinematic model of the SPM. The platform has a 3 degrees of freedom (d.o.f) of rotation defined by the ZXZ Euler angles ψ, θ, and φ. 2.1. Forward kinematic model The FKM of parallel manipulators, in general, is very complicated and admits multiple real solutions, each corresponding to an assembly mode of the manipulator [15,16]. In the case of the SPM, the direct model calculates the orientation of the platform (ψ, θ and φ) for a given configuration of the actuated joints (θ1A, θ1B, θ1C). In the literature, many approaches have been proposed to solve the direct kinematics [17,18,19,20]. The SPM studied by Gosselin et al. [17] is described by an 8-degree polynomial and has 8 possible solutions. An alternative to the traditional algebraic approaches to find the solutions of the FKM of parallel manipulators consists in using interval method presented by Celaya [18]. A typical problem of this method is its sensitivity to the size of the initial intervals. Bai et al. [19] proposed a robust procedure to determine the platform orientation using the input/output equations of spherical four-bar linkages; a similar approach is used in this paper. The two considered four-bar spherical mechanisms of the SPM are described by the following two chains: A2 − A3 − B3 − B2 and A3 − A2 − C2 − C3 and represented in Fig. 3. The corresponding input/output equations can be written as follows: • For the chain A2 − A3 − B3 − B2, we obtain the following expression: L1 ðξÞcosðσ Þ þ M 1 ðξÞsinðσ Þ þ N 1 ðξÞ ¼ 0
Fig. 1. Master device (a) and a surgical robot (b) of a medical tele-operation system.
ð1Þ
104
H. Saafi et al. / Mechanism and Machine Theory 91 (2015) 102–119
Fig. 2. (a) Spherical parallel manipulator. (b) Parameters of leg A.
• and for the chain A2 − A3 − C3 − C2, we obtain: L2 ðξÞcosðσ Þ þ M 2 ðξÞsinðσ Þ þ N 2 ðξÞ ¼ 0
ð2Þ
A2 ; B2 , σ is the angle A2 ;d A3 ; B3 , and Li(ξ), Mi(ξ), and Ni(ξ) (i = 1,2) are the variables that depend on cos(ξ) where ξ is the angle A3 ;d and sin(ξ). From the system composed by Eqs. (1) and (2), cos(σ) and sin(σ) can be expressed as follows: 8 > > > < cosðσ Þ ¼ > > sinðσ Þ ¼ > :
M1 N2 −M2 N1 L1 M2 −L2 M 1 −ðL1 N2 −L2 N1 Þ L1 M2 −L2 M 1
ðaÞ ðbÞ
:
ð3Þ
All the possible solutions of angle, ξ, can be found by solving Eq. (4), obtained by calculating the square sum of cos(σ) and sin(σ), defined bellow. 2 2
2
2
2
2
2 2
1
2
2
2
N 2 L1 þ 2L1 M2 L1 M1 −2L2 N2 L1 N1 þ N2 M1 −L2 M 1 −2M2 N2 M 1 N1 −M 2 L1 þ L2 N1 þ M2 N1 ¼ 0:
ð4Þ
Eq. (4) can be solved using semi-graphical method by substituting cos(ξ) and sin(ξ) by the variables x and y respectively, in order to obtain a quadratic equation.
Fig. 3. The considered four-bar spherical mechanisms for the SPM: A2 − A3 − B3 − B2 and A3 − A2 − C2 − C3.
H. Saafi et al. / Mechanism and Machine Theory 91 (2015) 102–119
105
As an example, for given values of the geometric parameters [α, β, γ] = [39.3°, 34.1°, 18.2°] and active joint angles: [θ1A, θ1B, θ1C] = [−100∘, − 100∘, − 100∘], one gets the following polynomial: 4
3
3
2 2
2
2
3
2
0:0174x −0:0461x y−0:0114x þ 0:0371x y þ 0:011x y−0:0127x −0:0365xy −0:00446xy þ 0:0184xy þ 0:00275x 4
3
2
þ0:0318y −0:0128y −0:0191y þ 0:00196y þ 0:00479 ¼ 0:
ð5Þ
The curve defined by Eq. (5) and the unit circle, x2 + y2 = 1, are plotted in Fig. 4. The two intersections correspond to the two possible solutions for cos(ξ) and sin(ξ). The number of solutions depend on the geometrical parameters α, β, and γ and the actuated angles. This number cannot exceed 8. The semi-graphical method is used to show the number of the possible solutions. However to solve the FKM, Eq. (4) is transfered to an 8th order polynomial using tan-half identities (tan(ξ/2)) like in [20]. And, it can be solved using a numeric method, as an example, the Newton–Raphson method. The obtained solutions for angle, ξ, are then used to calculate the values of angle, σ, using Eq. (3). The platform orientation can be calculated using the direct kinematic of leg A, defined by the transformation matrix expressed as follows: 3
T 0 jA ¼ Rot ðZ; θ1A Þ Rot ðX 1A ; α Þ RotðZ 2A ; ξÞ Rot ðX 2A ; βÞ Rot ðZ 3A ; σ Þ RotðX 3A ; ξÞ: The orientation of the platform can be expressed also by using Euler ZXZ transformation. 0 3 00 T 0 jA ¼ Rot ðZ; ψÞ Rot X ; θ Rot Z ; φ :
ð6Þ
ð7Þ
The Euler angles ψ, θ, and φ can be calculated using Eqs. (6) and (7). The geometrical representation of the two obtained solutions for the numerical example considered in Eq. (5) is shown in Fig. 5. This method gives all possible solutions of the FKM for the parallel robot. However, it is computationally expensive and not suitable for real time application. Most of the works on parallel robots propose the use of iterative numerical methods to overcome this problem, which are not adapted to real time application. This paper proposes an enhancement of the method presented previously. We propose to simplify the FKM resolution by adding an extra sensor that gives one of the unknowns. So the calculation time can be greatly reduced in order to meet the real time constraint. 2.2. Singularity In a previous work, the geometric parameters of the SPM was optimized for the Minimally Invasive Surgical (MIS) application with a useful workspace of an apex angle equal to 26°, see Chaker et al. [7]. The geometrical parameters of the optimized structure are defined by the following design vector: I = [α = 39.3°, β = 34.1°, γ = 18.2°]. The kinematic model of the manipulator can be written as follows:
ð8Þ
Aω¼B Θ
Fig. 4. The curves of all the possible geometric solutions.
106
H. Saafi et al. / Mechanism and Machine Theory 91 (2015) 102–119
(a)
(b)
Fig. 5. The two FKM solutions for (θ1A, θ1B, θ1C) = (100°, 100°, 100°).
where, ω is the vector of angular velocities of the end effector and Θ is the vector of the actuated joint velocities. The matrices, A and B, are called the instantaneous direct kinematic matrix and the instantaneous inverse kinematic matrix, respectively. These matrices are used to express the singularity conditions. The matrices A and B are given by: A ¼ ½Z 3A Z 2A
Z 3B Z 2B
B ¼ diagðZ 1A Z 2A Z 3A ;
Z 3C Z 2C
T
ð9Þ
Z 1B Z 2B Z 3B ;
Z 1C Z 2C Z 3C Þ:
ð10Þ
The Jacobian matrix of the SPM is as follows: ‐1
J ¼ A B:
ð11Þ
Dexterity is a measure reflecting the amplification of error due to the kinematic transformations between the joint space and the Cartesian space. It can also be a measure of how far is the robot from singularity. The dexterity η(J) is given by the inverse of the condition number of the Jacobian matrix as follows: 1 : ηð J Þ ¼ −1 J k Jk
ð12Þ
Fig. 6 shows the distribution of the dexterity for the φ = 50∘. It shows the presence of parallel singularity represented by dotted line. The parallel singularity appears when the determinant of matrix A vanishes. The major limitation of the optimized structure is the presence of parallel singularity inside its workspace. Indeed, for φ = 50∘, the singularity is located in the center of the workspace. The parallel singularity distribution over the workspace for three values of the angle ϕ is presented in Fig. 7.
Fig. 6. The dexterity distribution for φ = 50°.
H. Saafi et al. / Mechanism and Machine Theory 91 (2015) 102–119
107
The parallel singularity occurs when the three planes spanned by Z3k and Z2k (k = A, B, C) intersect in one common line. A particular configuration of the robot with a singularity located in the center of the SPM workspace is plotted in Fig. 8. In this case, the common line defined by the vector OA1 ¼ p1ffiffi3 ½1 1 1T . The singularity leads to unexpected behavior of the parallel mechanism, therefore the control becomes unfeasible. If the robot is in singular configuration, the accuracy of the FKM highly decreases due to the amplification of errors in the kinematic transformation between the joint space and the Cartesian space. The measures provided by the actuated joint sensors are no longer sufficient. In addition, the continuity of the solution of the FKM is not ensured. In fact, the FKM of the SPM presented in this paper has two solutions (Fig. 5). Those solutions intersect in the parallel singular configurations. When the SPM leaves this singular configuration, the classic FKM cannot ensure the selection of the suitable solution. An example of active joint angles (θ1A, θ1B, θ1C), is presented in Fig. 9). Those angles were obtained using the inverse kinematic model for (ψ, θ, φ) = (135∘, 54.7∘, 10 ⋅ sin(pi/10 ⋅ t) + 45∘). Only the self-rotation φ was varied. The solutions of the self-rotation of the FKM are presented in Fig. 10. This figure shows the intersection between the solutions near the singularity. In this work, we eliminate the effects of the parallel singularity on the FKM calculation. In the next section, the proposed solution to cope this problem is developed and validated experimentally. 3. Extra sensor position A fourth sensor will be installed on one of a six passive joints. Given the symmetry of the structure, two cases could be considered. The sensor may be placed in the first or second passive joint of the link K, for K = A, B or C. In this section, we discuss the accuracy of FKM with a 4th sensor placed on the first and the second passive joints of leg A, see Fig. 11. The aim is to compare the two cases and identify the most suitable one. Case 1. 4th sensor placed on the first passive joint of leg A. In this case, the 4th sensor is installed on the first passive joint of leg A. So, the angle θ2A is no longer an unknown. A relation between θ2A and ξ can be written as follows (see Fig. 11): 0
ξ ¼ θ2A −ξ :
ð13Þ
Where ξ′ is defined by: ξ′ = atan2(Z2A ⋅ (TA2B2 × TA2A1), TA2B2 ⋅ TA2A1) and, T A2B2 ¼ ðZ 2A Z 2B Þ=kZ 2A Z 2B k T A2A1 ¼ ðZ 2A Z 1A Þ=kZ 2A Z 1A k: The angle σ has a unique solution and, it is calculated using Eqs. (1) and (2) as: σ ¼ atan2ðy; xÞ
ð14Þ
with, x = (M1N2 − M2N1)/(L1M2 − L2M1) and y = − (L1N2 − L2N1)/(L1M2 − L2M1).
Fig. 7. The determinant distribution of matrix A (singularity locus in red). (For interpretation of the references to color in this figure legend, the reader is referred to the web version of this article.)
108
H. Saafi et al. / Mechanism and Machine Theory 91 (2015) 102–119
Fig. 8. Parallel singularity: central configuration.
The orientation of the effector (ψ, θ, φ) is completely described using the obtained values of angles ξ, σ, and θ1A. Case 2. 4th sensor placed on the second passive joint of leg A. In this case, the 4th sensor is installed on the second passive joint of leg A. So, it gives the angle θ3A which represents angle σ. The only unknown of Eqs. (1) and (2) is angle ξ that we can arrange as follows: 8 < L1 ðσ ÞcosðξÞ þ M 1 ðσ ÞsinðξÞ þ N1 ðσ Þ L ðσ ÞcosðξÞ þ M 2 ðσ ÞsinðξÞ þ N2 ðσ Þ : 2
¼ 0 ¼ 0
ð15Þ
where Li ; M i ; Ni ; ði ¼ 1; 2Þ are the variables that depend on cos(σ) and sin(σ) and allow the determination of orientation of the effector (ψ, θ, φ). The angle ξ has a unique solution, defined by: ξ ¼ atan2ðy; xÞ
ð16Þ
with, x ¼ M 1 N2 −M 2 N1 = L1 M 2 −L2 M1 and y ¼ − L1 N 2 −L2 N1 = L1 M 2 −L2 M1 .
π Fig. 9. Active angles for ψ ¼ 135 ; θ ¼ 54:7 and φ ¼ 10 sin 10 t þ 45 .
H. Saafi et al. / Mechanism and Machine Theory 91 (2015) 102–119
109
Fig. 10. Self-rotation φ Solutions of the FKM.
3.1. Accuracy of the FKM with respect to extra sensor location In this section the aim is to make a comparative study between the two previous cases and choose the best solution. An algorithm allowing the calculation of error between the imposed orientations and the obtained one by the inverse kinematic model (IKM) is presented, see Fig. 12. The algorithm of error calculation for the Cases 1 and 2 of adding the 4th sensor is performed on five steps and presented as follows: 1. 2. 3. 4. 5.
Generation of a random vector V(φi = 1,n) with (n = 1000) value of self-rotation near the central singularity position; Calculation of θij (i = 1, n; j = 1A, 1B, 1C, 2A, 3A) using the IKM for each orientation; Adding a random values, dθij(i = 1, n; j = 1A, 1B, 1C, 2A, 3A), representing the sensors sensitivity; Calculation of φ′i and φ′′ i using the FKM for Cases 1 and 2 respectively; Calculation of the error for Cases 1 and 2 respectively.
The probability law of the sensor tolerance obtained experimentally is normal law with σ (standard deviation) equal to 0.04°. The resulted cumulative distribution errors for Cases 1 and 2 are presented in Fig. 13. The error of all samples of Case 2 is less than 0.1°. On the other side, the error of 30% of samples of Case 1 is bigger than 0.1∘. So we can conclude that placing the sensor in the second passive joint A3 is more accurate than the first passive joint A2. Fig. 14 shows a comparison of error generated by solving the FKM with and without 4th sensor. In this section we have proven that adding a 4th sensor highly increase the accuracy of solving the FKM and also this 4th sensor must be in the second passive joint of leg A to have the maximum of accuracy. As described in [14], the inverse Jacobian matrix of the SPM with a redundant sensor located on the joint with Z3A axis can easily be obtained as follows: ‐1
‐1
Jr ¼ Br Ar
ð17Þ
Fig. 11. Extra sensor position.
110
H. Saafi et al. / Mechanism and Machine Theory 91 (2015) 102–119
Fig. 12. Algorithm of comparison.
Fig. 13. Cumulative distribution (CD) errors of the FKM Cases 1 and 2.
with,
Ar ω ¼ Br Θ
ð18Þ
where, 3 θ1A 7 6 6 θ1B 7 7 6 Θ¼ 6 θ 7: 6 1C 7 4θ 5 3A 2
Fig. 14. Cumulative distribution (CD) errors of the FKM with fourth sensor (improved) and the FKM without fourth sensor.
ð19Þ
H. Saafi et al. / Mechanism and Machine Theory 91 (2015) 102–119
111
Fig. 15. The dexterity distribution for φ = 50°.
The serial matrix Br, for the SPM with fourth sensor, becomes 4 × 4 orthogonal one and the parallel matrix Ar becomes 4 × 3 matrix, defined as follows: Ar ¼ ½Z 3A Z 2A
Z 3B Z 2B
Br ¼ diag ðZ 1A Z 2A Z 3A ;
Z 3C Z 2C
Z 1A Z 2A
Z 1B Z 2B Z 3B ;
T
Z 1C Z 2C Z 3C ;
ð20Þ
Z 3A Z 2A Z 1A Þ:
ð21Þ
Fig. 15 shows the distribution of the dexterity for the self-rotation φ = 50°. One can observe that the dexterity does not vanish over the whole workspace. It means that the extra sensor leads to eliminate the singularity from the workspace. The next section validates these results using a trajectory passing through singular configurations. 4. Numerical validation of the FKM calculation In order to validate the accuracy of the FKM, we propose to compare three models: improved FKM (Four sensors), the FKM of a SimMechanics model and the classical SPM (three sensors). Two scenarios are studied. The first scenario is presented in Fig. 16. First, a reference trajectory of the end-effector is considered. Only the self-rotation is varied around the singular configure for φ = 50∘, ψ = 135∘, and θ = 54∘. Then the active joint angles (θ1A, θ1B, θ1C, θ3A) are calculated using the IKM. Finally, the orientation of the end-effector is calculated using each model. Fig. 17 shows the SimMechanics model. The model is built using library blocks of SimMechanics package. The green blocks correspond to the nine revolute joints of the SPM. The base of the SPM corresponds to the blue block while the red block describes the endeffector. The active joints are defined by the angles (θ1A, θ1B, θ1C). The inputs are the active joint angles. The output is the platform orientation.
Fig. 16. First scenario of comparison.
112
H. Saafi et al. / Mechanism and Machine Theory 91 (2015) 102–119
Fig. 17. SimMechanics model. (For interpretation of the references to color in this figure legend, the reader is referred to the web version of this article.)
Fig. 18. The obtained self-rotation for three models.
H. Saafi et al. / Mechanism and Machine Theory 91 (2015) 102–119
113
Fig. 19. Error of the calculation of the classic FKM.
Fig. 20. Error of the calculation of SimMechanics model.
Fig. 18 shows the self-rotation φ for the three FKMs. From this figure we cannot distinguish easily the error of the FKM calculation. Therefore, we present the error for each model in Figs. 19, 20, and 21. The error for the classic FKM is about 15 × (10−3)°. We can observe the amplification of the error near the singular configurations. The error for the SimMechanics model is about 5 × (10−3)°. The amplification of the error near the singular configurations can be observed. The error of the improved FKM is smaller than the others and it is about (10−14)°. There is no amplification of the error near the singular configurations. In the next part we take into account the error of the sensors. We consider the same sensitivity to the one obtained in Section 3 (normal distribution with σ equal to 0.04°). For the second scenario, the flowchart of Fig. 22 describes this scenario. A random sensor
Fig. 21. Error of the calculation of the improved FKM.
114
H. Saafi et al. / Mechanism and Machine Theory 91 (2015) 102–119
Fig. 22. Second scenario of comparison.
Fig. 23. The obtained self-rotation for three models.
errors are added to the active joint angles calculated using IKM. Then the correspondent orientations are calculated using the three models. Fig. 23 shows the resulted self-rotation for each model. We can distinguish the error of calculation for the SimMechanics model and the classic model. These errors are presented in Figs. 24 and 25. We can see how the error amplifies near the singularity for the two models. The error is more than 1°. In Fig. 26, the error of the improved FKM is not visible. Fig. 26 shows this error and we can observe that is less than 0.015° and it is hazardous in all the time. There is no amplification of error in the singular configurations.
5. Experimental validation of the FKM calculation Experiments were performed in order to validate the accuracy of the FKM. The self-rotation of the SPM calculated using the classic FKM and the improved FKM are compared to the self-rotation calculated using Vicon motion capture system. These sensors were calibrated by using a static mechanical device (marked in red in Fig. 27) which constrained the SPM in a position with known angle values. The sensors are Hall-Effect absolute encoders, its reference is MAB18A-0505.
Fig. 24. Error of the calculation of the classic FKM.
H. Saafi et al. / Mechanism and Machine Theory 91 (2015) 102–119
115
Fig. 25. Error of the calculation of SimMechanics model.
Fig. 26. Error of the calculation of the improved FKM.
Once the sensors were calibrated, markers for motion capture system (Vicon) were placed on the SPM as it is shown in Fig. 28. Two markers are placed on the axis of rotation of the end-effector ZE and two other markers are placed to determine the self-rotation. The robot was placed in the singular configuration located at the center of its workspace, see Fig. 8. Perturbations were applied to the selfrotation around the position φ = 50°. Vicon system was used to capture the motion of markers placed on the SPM and also to measure the angular positions of the sensors. To determine the self-rotation using the markers, the markers C and D are projected in a plane perpendicular to the axis ZE, see Fig. 29. The angle φ is calculated first by using the projection of the marker C and then the marker D and then an average is calculated.
Fig. 27. The experimental setup for the calibration of the sensors. (For interpretation of the references to color in this figure legend, the reader is referred to the web version of this article.)
116
H. Saafi et al. / Mechanism and Machine Theory 91 (2015) 102–119
Fig. 28. The experimental setup of the markers.
The error of the self-rotation of the motion capture system is estimated to less than 1° due to the placement of markers and the sensitivity of Vicon system. The self-rotation obtained using the motion capture system is shown in Fig. 30. Fig. 31 shows the evolutions of the angle values of joints equipped by sensors in a function of time. The passive joint equipped by the extra sensor is sensitive to platform movements around the singular configurations, φ = 50°, while the values of the other three sensors are very sensitive. The evolutions of joint angles are used as an input for the forward model in order to calculate the platform orientation using the two formulations, with and without an extra sensor. In the first case, the classic formulation of FKM is solved using the variables of the three active joints. While the second integrates the extra sensor into the FKM computation. Fig. 32 shows the resulted platform orientation for the two cases. Fig. 32 shows that for ψ and θ the variations are the same but for the self-rotation φ, there is a difference about 3°. The improved FKM has a unique solution therefore there is no discontinuity where the classic FKM has more than one solution (assembly modes) as consequence a discontinuity appears in the vicinity of the singularity. Also from Fig. 33, we can observe the fluctuation of the classic model. Fig. 34 shows a comparison between the three models. We can see that the improved FKM and the self-rotation calculated by the motion capture system are close. One can note that the redundant sensor has improved the FKM computation, as well as the accuracy of the model in the vicinity of the singular position. Without an extra sensor the FKM does not detect the variation of the self-rotation near the singular configuration. This causes a high fluctuations and discontinuity near the singular configurations. Besides the problem of singularity, the classic FKM presented in Section 2.1 is not adapted for real-time applications due to the time needed for the resolution of the polynomial of eight degrees. This model needs more than 100 ms to be resolved using a PC running under 2.2 Ghz processor. The new FKM, with four sensors, presents a fast and efficient direct solution since no heavy computation is required. The calculation time is about 50 μs with 2.2 Ghz processor.
Fig. 29. Self-rotation determination from markers.
H. Saafi et al. / Mechanism and Machine Theory 91 (2015) 102–119
Fig. 30. Self-rotation calculated using motion capture system.
Fig. 31. Angle evolutions of the joints equipped by sensors.
Fig. 32. The obtained platform orientation for the two cases of the FKM.
117
118
H. Saafi et al. / Mechanism and Machine Theory 91 (2015) 102–119
Fig. 33. The obtained self-rotation for the two cases of the FKM.
Fig. 34. Comparison of the self-rotation calculated using the three models.
As mentioned in Section 1, the spherical parallel manipulator presented in this paper is used as a master device for a medical teleoperation system. This device was designed to control surgical robots. The execution time must be less than 1 ms in order to meet real time control constraints. The improved FKM has the advantage to run in less than 50 μs. In addition, it gives a direct solution, which means that no iterative method is needed to solve it. This makes the improved FKM robust. 6. Conclusion In this paper an approach based on adding an extra sensor in a passive joint, to improve the calculation of the forward kinematic model (FKM) of a spherical parallel manipulator (SPM), is proposed and detailed. In this approach, the measure of the passive joint reduces the complexity of the direct kinematic model and makes it possible for real time application in one hand and improves the accuracy of the model on the other hand. An investigation is made to choose the best passive joint that gives the maximum of accuracy on the resolution of FKM. The accuracy of the model is enhanced. The model remains accurate even in the singular configurations thanks to the extra sensor. Experiments were performed showing the efficiency of the proposed approach. Acknowledgment This research is supported by ROBOTEX, the French national network of robotics platforms (No. ANR-10-EQPX-44-01). References [1] L. Birglen, C. Gosselin, N. Pouliot, B. Monsarrat, T. Laliberte, Shade, a new 3-dof haptic device, IEEE Trans. Robot. Autom. 18 (2) (2002) 166–175. [2] C. Gosselin, E.St. Pierre, M. Gagne, On the development of the agile eye, IEEE Robot. Autom. Mag. 3 (4) (1996) 29–37. [3] A. Ma, S. Payandeh, Analysis and experimentation of a 4-dof haptic device, Haptic Interfaces for Virtual Environment and Teleoperator Systems, Haptics 2008. Symposium on, 2008 2008, pp. 351–356. [4] A. Chaker, A. Mlika, M. Laribi, L. Romdhane, S. Zeghloul, Synthesis of spherical parallel manipulator for dexterous medical task, Front. Mech. Eng. 7 (2) (2012) 150–162. [5] S. Bai, Optimum design of spherical parallel manipulators for a prescribed workspace, Mech. Mach. Theory 45 (2) (2010) 200–211. [6] Z. Tao, Q. An, Interference analysis and workspace optimization of 3-RRR spherical parallel mechanism, Mech. Mach. Theory 69 (2013) 62–72. [7] A. Chaker, A. Mlika, M. Laribi, L. Romdhane, S. Zeghloul, On the kinematics of spherical parallel manipulators for real time applications, in: M. Haddar, L. Romdhane, J. Louati, A. Ben Amara (Eds.), Design and Modeling of Mechanical Systems, Lecture Notes in Mechanical Engineering, Springer, Berlin Heidelberg 2013, pp. 53–60. [8] R. Vertechy, V. Parenti-Castelli, Robust, fast and accurate solution of the direct position analysis of parallel manipulators by using extra-sensors, in: Huapeng Wu (Ed.)Parallel Manipulators, Towards New Applications, 2008. [9] I. Bonev, J. Ryu, S.-G. Kim, S.-K. Lee, A closed-form solution to the direct kinematics of nearly general parallel manipulators with optimally located three linear extra sensors, IEEE Trans. Robot. Autom. 17 (2) (2001) 148–156. [10] J.-P. Merlet, Closed-form resolution of the direct kinematics of parallel manipulators using extra sensors data, Robotics and Automation, Proceedings., 1993 IEEE International Conference on, 1993, vol.1 1993, pp. 200–204. [11] A. Zubizarreta, M. Marcos, I. Cabanes, C. Pinto, E. Portillo, Redundant sensor based control of the 3RRR parallel robot, Mech. Mach. Theory 54 (2012) 1–17.
H. Saafi et al. / Mechanism and Machine Theory 91 (2015) 102–119
119
[12] H. Saafi, M.A. Laribi, S. Zeghloul, Improvement of the direct kinematic model of a haptic device for medical application in real time using an extra sensor, Intelligent Robots and Systems (IROS 2014), 2014 IEEE/RSJ International Conference on, IEEE 2014, pp. 1697–1702. [13] M. Laribi, T. Riviere, M. Arsicault, S. Zeghloul, A design of slave surgical robot based on motion capture, Robotics and Biomimetics (ROBIO), 2012 IEEE International Conference on, IEEE 2012, pp. 600–605. [14] H. Saafi, M.A. Laribi, S. Zeghloul, Redundantly actuated 3-RRR spherical parallel manipulator used as a haptic device: improving dexterity and eliminating singularity, Robotica FirstView2014. 1–18. [15] J.P. Merlet, Direct kinematics of parallel manipulators, IEEE Trans. Robot. Autom. 9 (6) (1993) 842–846. [16] C.M. Gosselin, J.-P. Merlet, The direct kinematics of planar parallel manipulators: special architectures and number of solutions, Mech. Mach. Theory 29 (8) (1994) 1083–1097. [17] C.M. Gosselin, J. Sefrioui, M.J. Richard, On the direct kinematics of spherical three-degree-of-freedom parallel manipulators of general architecture, J. Mech. Des. 116 (2) (1994) 594–598. [18] E. Celaya, Interval propagation for solving parallel spherical mechanisms, in: J. Lenarcic, F. Thomas (Eds.), Advances in Robot Kinematics, Springer, Netherlands 2002, pp. 415–422. [19] S. Bai, M.R. Hansen, J. Angeles, A robust forward-displacement analysis of spherical parallel robots, Mech. Mach. Theory 44 (12) (2009) 2204–2216. [20] S. Bai, M.R. Hansen, Forward kinematics of spherical parallel manipulators with revolute joints, Advanced Intelligent Mechatronics, AIM 2008. IEEE/ASME International Conference on, IEEE, 2008 2008, pp. 522–527.
Contents lists available at ScienceDirect
Mechanism and Machine Theory journal homepage: www.elsevier.com/locate/mechmt
Forward kinematic model improvement of a spherical parallel manipulator using an extra sensor Houssem Saafi, Med Amine Laribi, Said Zeghloul ⁎ Dept. GMSC, PPRIME Institute CNRS, University of Poitiers, ENSMA, UPR, 3346, France
a r t i c l e
i n f o
Article history: Received 5 October 2014 Received in revised form 7 April 2015 Accepted 8 April 2015 Available online xxxx Keywords: Spherical parallel manipulator Direct kinematic model Parallel Singularity Redundant sensor
a b s t r a c t In this paper an approach to improve the calculation of the forward kinematic model (FKM) of a spherical parallel manipulator (SPM) is presented and tested experimentally. This approach is based on adding an extra sensor on a passive joint. The measure of the passive joint angle reduces the complexity of the forward kinematic model and makes it possible for real time. The use of the extra sensor eliminates the effect of the parallel singularity on the FKM accuracy. An investigation is carried out to choose the position of the extra sensor that gives the better accuracy of the FKM. Experiments are carried out to prove the benefit of the redundant sensor on the FKM. © 2015 Elsevier Ltd. All rights reserved.
1. Introduction Spherical parallel manipulators (SPMs) are parallel mechanisms, with a fixed center of rotation, which provide three degrees of freedom of pure rotation. Different varieties of these mechanisms have been studied before. Gosselin et al. developed a master [1] and slave [2] system to control the orientation of a camera. In [3] and [4], four degrees of freedom haptic devices for Minimally Invasive Surgery applications are proposed. The geometric parameters of the SPM are studied and optimized for two different applications in [4] and [5]. The kinematics of the SPM has been well presented in those works. The forward kinematic model (FKM) of the SPM determines the orientation of the mobile platform in a function of the actuated joint angle values. Due to the nonlinear trigonometric system of equations generated from the multi-loop architecture of the SPM, the FKM, usually, does not allow for closed-form solutions and needs a high computational time. Moreover, the presence of parallel singularity in the workspace of the SPM makes the calculation of the FKM not possible and inaccurate close to the singular configurations. Gosselin et al. [1,2] proposed a special architecture for the SPM with a large singular-free workspace; however a problem of interference was reported due to the big size of the links. This problem of interference was studied also by [6]. The FKM of the SPM, developed by Gosselin et al. [1], was solved using an iterative method using the Jacobian matrix. In [7], the FKM is solved also by iterative techniques, based on the Newton–Raphson method. Unfortunately, iterative methods require both the SPM to be sufficiently far away from singularity and a good initial guess of the actual orientation of the mobile platform, two conditions which cannot always be satisfied and can seriously affect the robustness of these methods. Extra-sensor approach is used in this paper. A theoretical study of this approach is presented in the work of Vertechy and ParentiCastelli [8]. According to [8], the extra sensors are added for at least one of the following reasons: 1) to render the FKM an explicit problem with a closed form solutions; 2) to render the FKM a linear problem, which makes it possible to find the actual orientation ⁎ Corresponding author. E-mail addresses: houssem.saafi@univ-poitiers.fr (H. Saafi), [email protected] (M.A. Laribi), [email protected] (S. Zeghloul).
http://dx.doi.org/10.1016/j.mechmachtheory.2015.04.006 0094-114X/© 2015 Elsevier Ltd. All rights reserved.
H. Saafi et al. / Mechanism and Machine Theory 91 (2015) 102–119
103
of the mobile platform; 3) to speed-up the computation of the FKM solution; 4) to make the method robust against SPM singular configurations; and 5) to improve the accuracy of the solution by reducing the influence of the joint sensor sensibility. In this work, the extra sensor is added, first, to make the FKM robust against the singular configuration, second, to speed-up the computation time of the FKM and finally, to improve the accuracy of the FKM. The choice of the number as well as the location of additional sensors is an important issue addressed in the literature. Bonev et al. [9] proposed an optimal method to locate three linear extra sensors on a general parallel manipulator in order to reduce the complexity of the FKM. Merlet [10] demonstrated that placing three linear extra sensors on a general six d.o.f parallel robot are sufficient to get a closed form solution. In [11], three extra sensors were installed on the passive joints to simplify the dynamic model of 3-RRR planar parallel robot. The SPM presented in this paper is used as a haptic master device for a medical tele-operation system [4,12], (Fig. 1a). This device controls a surgical robot [13] (Fig. 1b). The medical application requires a system having three degrees of freedom of pure rotation as well as a translation. The master architecture was chosen for this reason. This device reproduces the force feedback to the user's hand. A solution based on actuator redundancy is proposed in [14] to eliminate the effect of the singularity in the haptic control. The paper is organized as follows. In Section 2, the kinematic model of the SPM is presented and detailed. In Section 3, an investigation to choose the best accurate location of the extra-sensor is made. A numerical validation is presented in Section 4. The FKM with the extra-sensor is experimentally tested in Section 5. Finally, Section 6 summarizes this paper. 2. Kinematic model of the SPM The SPM is shown in Fig. 2a. It consists of three identical legs connecting the base to a common moving platform. Each leg has one active joint fixed on the base and two passive joints connecting, respectively, the proximal link to the distal link and the distal link to the moving platform. The angles α, β, and γ represent the dimension of the proximal link, distal link, and the end-effector, respectively, see Fig. 2b. All the axes of revolute joints intersect in a common point noted CoR (Center of Rotation). As in [1] and [2], the considered SPM in this paper is a special type. Indeed, the joint base axes are located on an orthogonal frame. This choice was made to simplify the forward kinematic model and the inverse kinematic model of the SPM. The platform has a 3 degrees of freedom (d.o.f) of rotation defined by the ZXZ Euler angles ψ, θ, and φ. 2.1. Forward kinematic model The FKM of parallel manipulators, in general, is very complicated and admits multiple real solutions, each corresponding to an assembly mode of the manipulator [15,16]. In the case of the SPM, the direct model calculates the orientation of the platform (ψ, θ and φ) for a given configuration of the actuated joints (θ1A, θ1B, θ1C). In the literature, many approaches have been proposed to solve the direct kinematics [17,18,19,20]. The SPM studied by Gosselin et al. [17] is described by an 8-degree polynomial and has 8 possible solutions. An alternative to the traditional algebraic approaches to find the solutions of the FKM of parallel manipulators consists in using interval method presented by Celaya [18]. A typical problem of this method is its sensitivity to the size of the initial intervals. Bai et al. [19] proposed a robust procedure to determine the platform orientation using the input/output equations of spherical four-bar linkages; a similar approach is used in this paper. The two considered four-bar spherical mechanisms of the SPM are described by the following two chains: A2 − A3 − B3 − B2 and A3 − A2 − C2 − C3 and represented in Fig. 3. The corresponding input/output equations can be written as follows: • For the chain A2 − A3 − B3 − B2, we obtain the following expression: L1 ðξÞcosðσ Þ þ M 1 ðξÞsinðσ Þ þ N 1 ðξÞ ¼ 0
Fig. 1. Master device (a) and a surgical robot (b) of a medical tele-operation system.
ð1Þ
104
H. Saafi et al. / Mechanism and Machine Theory 91 (2015) 102–119
Fig. 2. (a) Spherical parallel manipulator. (b) Parameters of leg A.
• and for the chain A2 − A3 − C3 − C2, we obtain: L2 ðξÞcosðσ Þ þ M 2 ðξÞsinðσ Þ þ N 2 ðξÞ ¼ 0
ð2Þ
A2 ; B2 , σ is the angle A2 ;d A3 ; B3 , and Li(ξ), Mi(ξ), and Ni(ξ) (i = 1,2) are the variables that depend on cos(ξ) where ξ is the angle A3 ;d and sin(ξ). From the system composed by Eqs. (1) and (2), cos(σ) and sin(σ) can be expressed as follows: 8 > > > < cosðσ Þ ¼ > > sinðσ Þ ¼ > :
M1 N2 −M2 N1 L1 M2 −L2 M 1 −ðL1 N2 −L2 N1 Þ L1 M2 −L2 M 1
ðaÞ ðbÞ
:
ð3Þ
All the possible solutions of angle, ξ, can be found by solving Eq. (4), obtained by calculating the square sum of cos(σ) and sin(σ), defined bellow. 2 2
2
2
2
2
2 2
1
2
2
2
N 2 L1 þ 2L1 M2 L1 M1 −2L2 N2 L1 N1 þ N2 M1 −L2 M 1 −2M2 N2 M 1 N1 −M 2 L1 þ L2 N1 þ M2 N1 ¼ 0:
ð4Þ
Eq. (4) can be solved using semi-graphical method by substituting cos(ξ) and sin(ξ) by the variables x and y respectively, in order to obtain a quadratic equation.
Fig. 3. The considered four-bar spherical mechanisms for the SPM: A2 − A3 − B3 − B2 and A3 − A2 − C2 − C3.
H. Saafi et al. / Mechanism and Machine Theory 91 (2015) 102–119
105
As an example, for given values of the geometric parameters [α, β, γ] = [39.3°, 34.1°, 18.2°] and active joint angles: [θ1A, θ1B, θ1C] = [−100∘, − 100∘, − 100∘], one gets the following polynomial: 4
3
3
2 2
2
2
3
2
0:0174x −0:0461x y−0:0114x þ 0:0371x y þ 0:011x y−0:0127x −0:0365xy −0:00446xy þ 0:0184xy þ 0:00275x 4
3
2
þ0:0318y −0:0128y −0:0191y þ 0:00196y þ 0:00479 ¼ 0:
ð5Þ
The curve defined by Eq. (5) and the unit circle, x2 + y2 = 1, are plotted in Fig. 4. The two intersections correspond to the two possible solutions for cos(ξ) and sin(ξ). The number of solutions depend on the geometrical parameters α, β, and γ and the actuated angles. This number cannot exceed 8. The semi-graphical method is used to show the number of the possible solutions. However to solve the FKM, Eq. (4) is transfered to an 8th order polynomial using tan-half identities (tan(ξ/2)) like in [20]. And, it can be solved using a numeric method, as an example, the Newton–Raphson method. The obtained solutions for angle, ξ, are then used to calculate the values of angle, σ, using Eq. (3). The platform orientation can be calculated using the direct kinematic of leg A, defined by the transformation matrix expressed as follows: 3
T 0 jA ¼ Rot ðZ; θ1A Þ Rot ðX 1A ; α Þ RotðZ 2A ; ξÞ Rot ðX 2A ; βÞ Rot ðZ 3A ; σ Þ RotðX 3A ; ξÞ: The orientation of the platform can be expressed also by using Euler ZXZ transformation. 0 3 00 T 0 jA ¼ Rot ðZ; ψÞ Rot X ; θ Rot Z ; φ :
ð6Þ
ð7Þ
The Euler angles ψ, θ, and φ can be calculated using Eqs. (6) and (7). The geometrical representation of the two obtained solutions for the numerical example considered in Eq. (5) is shown in Fig. 5. This method gives all possible solutions of the FKM for the parallel robot. However, it is computationally expensive and not suitable for real time application. Most of the works on parallel robots propose the use of iterative numerical methods to overcome this problem, which are not adapted to real time application. This paper proposes an enhancement of the method presented previously. We propose to simplify the FKM resolution by adding an extra sensor that gives one of the unknowns. So the calculation time can be greatly reduced in order to meet the real time constraint. 2.2. Singularity In a previous work, the geometric parameters of the SPM was optimized for the Minimally Invasive Surgical (MIS) application with a useful workspace of an apex angle equal to 26°, see Chaker et al. [7]. The geometrical parameters of the optimized structure are defined by the following design vector: I = [α = 39.3°, β = 34.1°, γ = 18.2°]. The kinematic model of the manipulator can be written as follows:
ð8Þ
Aω¼B Θ
Fig. 4. The curves of all the possible geometric solutions.
106
H. Saafi et al. / Mechanism and Machine Theory 91 (2015) 102–119
(a)
(b)
Fig. 5. The two FKM solutions for (θ1A, θ1B, θ1C) = (100°, 100°, 100°).
where, ω is the vector of angular velocities of the end effector and Θ is the vector of the actuated joint velocities. The matrices, A and B, are called the instantaneous direct kinematic matrix and the instantaneous inverse kinematic matrix, respectively. These matrices are used to express the singularity conditions. The matrices A and B are given by: A ¼ ½Z 3A Z 2A
Z 3B Z 2B
B ¼ diagðZ 1A Z 2A Z 3A ;
Z 3C Z 2C
T
ð9Þ
Z 1B Z 2B Z 3B ;
Z 1C Z 2C Z 3C Þ:
ð10Þ
The Jacobian matrix of the SPM is as follows: ‐1
J ¼ A B:
ð11Þ
Dexterity is a measure reflecting the amplification of error due to the kinematic transformations between the joint space and the Cartesian space. It can also be a measure of how far is the robot from singularity. The dexterity η(J) is given by the inverse of the condition number of the Jacobian matrix as follows: 1 : ηð J Þ ¼ −1 J k Jk
ð12Þ
Fig. 6 shows the distribution of the dexterity for the φ = 50∘. It shows the presence of parallel singularity represented by dotted line. The parallel singularity appears when the determinant of matrix A vanishes. The major limitation of the optimized structure is the presence of parallel singularity inside its workspace. Indeed, for φ = 50∘, the singularity is located in the center of the workspace. The parallel singularity distribution over the workspace for three values of the angle ϕ is presented in Fig. 7.
Fig. 6. The dexterity distribution for φ = 50°.
H. Saafi et al. / Mechanism and Machine Theory 91 (2015) 102–119
107
The parallel singularity occurs when the three planes spanned by Z3k and Z2k (k = A, B, C) intersect in one common line. A particular configuration of the robot with a singularity located in the center of the SPM workspace is plotted in Fig. 8. In this case, the common line defined by the vector OA1 ¼ p1ffiffi3 ½1 1 1T . The singularity leads to unexpected behavior of the parallel mechanism, therefore the control becomes unfeasible. If the robot is in singular configuration, the accuracy of the FKM highly decreases due to the amplification of errors in the kinematic transformation between the joint space and the Cartesian space. The measures provided by the actuated joint sensors are no longer sufficient. In addition, the continuity of the solution of the FKM is not ensured. In fact, the FKM of the SPM presented in this paper has two solutions (Fig. 5). Those solutions intersect in the parallel singular configurations. When the SPM leaves this singular configuration, the classic FKM cannot ensure the selection of the suitable solution. An example of active joint angles (θ1A, θ1B, θ1C), is presented in Fig. 9). Those angles were obtained using the inverse kinematic model for (ψ, θ, φ) = (135∘, 54.7∘, 10 ⋅ sin(pi/10 ⋅ t) + 45∘). Only the self-rotation φ was varied. The solutions of the self-rotation of the FKM are presented in Fig. 10. This figure shows the intersection between the solutions near the singularity. In this work, we eliminate the effects of the parallel singularity on the FKM calculation. In the next section, the proposed solution to cope this problem is developed and validated experimentally. 3. Extra sensor position A fourth sensor will be installed on one of a six passive joints. Given the symmetry of the structure, two cases could be considered. The sensor may be placed in the first or second passive joint of the link K, for K = A, B or C. In this section, we discuss the accuracy of FKM with a 4th sensor placed on the first and the second passive joints of leg A, see Fig. 11. The aim is to compare the two cases and identify the most suitable one. Case 1. 4th sensor placed on the first passive joint of leg A. In this case, the 4th sensor is installed on the first passive joint of leg A. So, the angle θ2A is no longer an unknown. A relation between θ2A and ξ can be written as follows (see Fig. 11): 0
ξ ¼ θ2A −ξ :
ð13Þ
Where ξ′ is defined by: ξ′ = atan2(Z2A ⋅ (TA2B2 × TA2A1), TA2B2 ⋅ TA2A1) and, T A2B2 ¼ ðZ 2A Z 2B Þ=kZ 2A Z 2B k T A2A1 ¼ ðZ 2A Z 1A Þ=kZ 2A Z 1A k: The angle σ has a unique solution and, it is calculated using Eqs. (1) and (2) as: σ ¼ atan2ðy; xÞ
ð14Þ
with, x = (M1N2 − M2N1)/(L1M2 − L2M1) and y = − (L1N2 − L2N1)/(L1M2 − L2M1).
Fig. 7. The determinant distribution of matrix A (singularity locus in red). (For interpretation of the references to color in this figure legend, the reader is referred to the web version of this article.)
108
H. Saafi et al. / Mechanism and Machine Theory 91 (2015) 102–119
Fig. 8. Parallel singularity: central configuration.
The orientation of the effector (ψ, θ, φ) is completely described using the obtained values of angles ξ, σ, and θ1A. Case 2. 4th sensor placed on the second passive joint of leg A. In this case, the 4th sensor is installed on the second passive joint of leg A. So, it gives the angle θ3A which represents angle σ. The only unknown of Eqs. (1) and (2) is angle ξ that we can arrange as follows: 8 < L1 ðσ ÞcosðξÞ þ M 1 ðσ ÞsinðξÞ þ N1 ðσ Þ L ðσ ÞcosðξÞ þ M 2 ðσ ÞsinðξÞ þ N2 ðσ Þ : 2
¼ 0 ¼ 0
ð15Þ
where Li ; M i ; Ni ; ði ¼ 1; 2Þ are the variables that depend on cos(σ) and sin(σ) and allow the determination of orientation of the effector (ψ, θ, φ). The angle ξ has a unique solution, defined by: ξ ¼ atan2ðy; xÞ
ð16Þ
with, x ¼ M 1 N2 −M 2 N1 = L1 M 2 −L2 M1 and y ¼ − L1 N 2 −L2 N1 = L1 M 2 −L2 M1 .
π Fig. 9. Active angles for ψ ¼ 135 ; θ ¼ 54:7 and φ ¼ 10 sin 10 t þ 45 .
H. Saafi et al. / Mechanism and Machine Theory 91 (2015) 102–119
109
Fig. 10. Self-rotation φ Solutions of the FKM.
3.1. Accuracy of the FKM with respect to extra sensor location In this section the aim is to make a comparative study between the two previous cases and choose the best solution. An algorithm allowing the calculation of error between the imposed orientations and the obtained one by the inverse kinematic model (IKM) is presented, see Fig. 12. The algorithm of error calculation for the Cases 1 and 2 of adding the 4th sensor is performed on five steps and presented as follows: 1. 2. 3. 4. 5.
Generation of a random vector V(φi = 1,n) with (n = 1000) value of self-rotation near the central singularity position; Calculation of θij (i = 1, n; j = 1A, 1B, 1C, 2A, 3A) using the IKM for each orientation; Adding a random values, dθij(i = 1, n; j = 1A, 1B, 1C, 2A, 3A), representing the sensors sensitivity; Calculation of φ′i and φ′′ i using the FKM for Cases 1 and 2 respectively; Calculation of the error for Cases 1 and 2 respectively.
The probability law of the sensor tolerance obtained experimentally is normal law with σ (standard deviation) equal to 0.04°. The resulted cumulative distribution errors for Cases 1 and 2 are presented in Fig. 13. The error of all samples of Case 2 is less than 0.1°. On the other side, the error of 30% of samples of Case 1 is bigger than 0.1∘. So we can conclude that placing the sensor in the second passive joint A3 is more accurate than the first passive joint A2. Fig. 14 shows a comparison of error generated by solving the FKM with and without 4th sensor. In this section we have proven that adding a 4th sensor highly increase the accuracy of solving the FKM and also this 4th sensor must be in the second passive joint of leg A to have the maximum of accuracy. As described in [14], the inverse Jacobian matrix of the SPM with a redundant sensor located on the joint with Z3A axis can easily be obtained as follows: ‐1
‐1
Jr ¼ Br Ar
ð17Þ
Fig. 11. Extra sensor position.
110
H. Saafi et al. / Mechanism and Machine Theory 91 (2015) 102–119
Fig. 12. Algorithm of comparison.
Fig. 13. Cumulative distribution (CD) errors of the FKM Cases 1 and 2.
with,
Ar ω ¼ Br Θ
ð18Þ
where, 3 θ1A 7 6 6 θ1B 7 7 6 Θ¼ 6 θ 7: 6 1C 7 4θ 5 3A 2
Fig. 14. Cumulative distribution (CD) errors of the FKM with fourth sensor (improved) and the FKM without fourth sensor.
ð19Þ
H. Saafi et al. / Mechanism and Machine Theory 91 (2015) 102–119
111
Fig. 15. The dexterity distribution for φ = 50°.
The serial matrix Br, for the SPM with fourth sensor, becomes 4 × 4 orthogonal one and the parallel matrix Ar becomes 4 × 3 matrix, defined as follows: Ar ¼ ½Z 3A Z 2A
Z 3B Z 2B
Br ¼ diag ðZ 1A Z 2A Z 3A ;
Z 3C Z 2C
Z 1A Z 2A
Z 1B Z 2B Z 3B ;
T
Z 1C Z 2C Z 3C ;
ð20Þ
Z 3A Z 2A Z 1A Þ:
ð21Þ
Fig. 15 shows the distribution of the dexterity for the self-rotation φ = 50°. One can observe that the dexterity does not vanish over the whole workspace. It means that the extra sensor leads to eliminate the singularity from the workspace. The next section validates these results using a trajectory passing through singular configurations. 4. Numerical validation of the FKM calculation In order to validate the accuracy of the FKM, we propose to compare three models: improved FKM (Four sensors), the FKM of a SimMechanics model and the classical SPM (three sensors). Two scenarios are studied. The first scenario is presented in Fig. 16. First, a reference trajectory of the end-effector is considered. Only the self-rotation is varied around the singular configure for φ = 50∘, ψ = 135∘, and θ = 54∘. Then the active joint angles (θ1A, θ1B, θ1C, θ3A) are calculated using the IKM. Finally, the orientation of the end-effector is calculated using each model. Fig. 17 shows the SimMechanics model. The model is built using library blocks of SimMechanics package. The green blocks correspond to the nine revolute joints of the SPM. The base of the SPM corresponds to the blue block while the red block describes the endeffector. The active joints are defined by the angles (θ1A, θ1B, θ1C). The inputs are the active joint angles. The output is the platform orientation.
Fig. 16. First scenario of comparison.
112
H. Saafi et al. / Mechanism and Machine Theory 91 (2015) 102–119
Fig. 17. SimMechanics model. (For interpretation of the references to color in this figure legend, the reader is referred to the web version of this article.)
Fig. 18. The obtained self-rotation for three models.
H. Saafi et al. / Mechanism and Machine Theory 91 (2015) 102–119
113
Fig. 19. Error of the calculation of the classic FKM.
Fig. 20. Error of the calculation of SimMechanics model.
Fig. 18 shows the self-rotation φ for the three FKMs. From this figure we cannot distinguish easily the error of the FKM calculation. Therefore, we present the error for each model in Figs. 19, 20, and 21. The error for the classic FKM is about 15 × (10−3)°. We can observe the amplification of the error near the singular configurations. The error for the SimMechanics model is about 5 × (10−3)°. The amplification of the error near the singular configurations can be observed. The error of the improved FKM is smaller than the others and it is about (10−14)°. There is no amplification of the error near the singular configurations. In the next part we take into account the error of the sensors. We consider the same sensitivity to the one obtained in Section 3 (normal distribution with σ equal to 0.04°). For the second scenario, the flowchart of Fig. 22 describes this scenario. A random sensor
Fig. 21. Error of the calculation of the improved FKM.
114
H. Saafi et al. / Mechanism and Machine Theory 91 (2015) 102–119
Fig. 22. Second scenario of comparison.
Fig. 23. The obtained self-rotation for three models.
errors are added to the active joint angles calculated using IKM. Then the correspondent orientations are calculated using the three models. Fig. 23 shows the resulted self-rotation for each model. We can distinguish the error of calculation for the SimMechanics model and the classic model. These errors are presented in Figs. 24 and 25. We can see how the error amplifies near the singularity for the two models. The error is more than 1°. In Fig. 26, the error of the improved FKM is not visible. Fig. 26 shows this error and we can observe that is less than 0.015° and it is hazardous in all the time. There is no amplification of error in the singular configurations.
5. Experimental validation of the FKM calculation Experiments were performed in order to validate the accuracy of the FKM. The self-rotation of the SPM calculated using the classic FKM and the improved FKM are compared to the self-rotation calculated using Vicon motion capture system. These sensors were calibrated by using a static mechanical device (marked in red in Fig. 27) which constrained the SPM in a position with known angle values. The sensors are Hall-Effect absolute encoders, its reference is MAB18A-0505.
Fig. 24. Error of the calculation of the classic FKM.
H. Saafi et al. / Mechanism and Machine Theory 91 (2015) 102–119
115
Fig. 25. Error of the calculation of SimMechanics model.
Fig. 26. Error of the calculation of the improved FKM.
Once the sensors were calibrated, markers for motion capture system (Vicon) were placed on the SPM as it is shown in Fig. 28. Two markers are placed on the axis of rotation of the end-effector ZE and two other markers are placed to determine the self-rotation. The robot was placed in the singular configuration located at the center of its workspace, see Fig. 8. Perturbations were applied to the selfrotation around the position φ = 50°. Vicon system was used to capture the motion of markers placed on the SPM and also to measure the angular positions of the sensors. To determine the self-rotation using the markers, the markers C and D are projected in a plane perpendicular to the axis ZE, see Fig. 29. The angle φ is calculated first by using the projection of the marker C and then the marker D and then an average is calculated.
Fig. 27. The experimental setup for the calibration of the sensors. (For interpretation of the references to color in this figure legend, the reader is referred to the web version of this article.)
116
H. Saafi et al. / Mechanism and Machine Theory 91 (2015) 102–119
Fig. 28. The experimental setup of the markers.
The error of the self-rotation of the motion capture system is estimated to less than 1° due to the placement of markers and the sensitivity of Vicon system. The self-rotation obtained using the motion capture system is shown in Fig. 30. Fig. 31 shows the evolutions of the angle values of joints equipped by sensors in a function of time. The passive joint equipped by the extra sensor is sensitive to platform movements around the singular configurations, φ = 50°, while the values of the other three sensors are very sensitive. The evolutions of joint angles are used as an input for the forward model in order to calculate the platform orientation using the two formulations, with and without an extra sensor. In the first case, the classic formulation of FKM is solved using the variables of the three active joints. While the second integrates the extra sensor into the FKM computation. Fig. 32 shows the resulted platform orientation for the two cases. Fig. 32 shows that for ψ and θ the variations are the same but for the self-rotation φ, there is a difference about 3°. The improved FKM has a unique solution therefore there is no discontinuity where the classic FKM has more than one solution (assembly modes) as consequence a discontinuity appears in the vicinity of the singularity. Also from Fig. 33, we can observe the fluctuation of the classic model. Fig. 34 shows a comparison between the three models. We can see that the improved FKM and the self-rotation calculated by the motion capture system are close. One can note that the redundant sensor has improved the FKM computation, as well as the accuracy of the model in the vicinity of the singular position. Without an extra sensor the FKM does not detect the variation of the self-rotation near the singular configuration. This causes a high fluctuations and discontinuity near the singular configurations. Besides the problem of singularity, the classic FKM presented in Section 2.1 is not adapted for real-time applications due to the time needed for the resolution of the polynomial of eight degrees. This model needs more than 100 ms to be resolved using a PC running under 2.2 Ghz processor. The new FKM, with four sensors, presents a fast and efficient direct solution since no heavy computation is required. The calculation time is about 50 μs with 2.2 Ghz processor.
Fig. 29. Self-rotation determination from markers.
H. Saafi et al. / Mechanism and Machine Theory 91 (2015) 102–119
Fig. 30. Self-rotation calculated using motion capture system.
Fig. 31. Angle evolutions of the joints equipped by sensors.
Fig. 32. The obtained platform orientation for the two cases of the FKM.
117
118
H. Saafi et al. / Mechanism and Machine Theory 91 (2015) 102–119
Fig. 33. The obtained self-rotation for the two cases of the FKM.
Fig. 34. Comparison of the self-rotation calculated using the three models.
As mentioned in Section 1, the spherical parallel manipulator presented in this paper is used as a master device for a medical teleoperation system. This device was designed to control surgical robots. The execution time must be less than 1 ms in order to meet real time control constraints. The improved FKM has the advantage to run in less than 50 μs. In addition, it gives a direct solution, which means that no iterative method is needed to solve it. This makes the improved FKM robust. 6. Conclusion In this paper an approach based on adding an extra sensor in a passive joint, to improve the calculation of the forward kinematic model (FKM) of a spherical parallel manipulator (SPM), is proposed and detailed. In this approach, the measure of the passive joint reduces the complexity of the direct kinematic model and makes it possible for real time application in one hand and improves the accuracy of the model on the other hand. An investigation is made to choose the best passive joint that gives the maximum of accuracy on the resolution of FKM. The accuracy of the model is enhanced. The model remains accurate even in the singular configurations thanks to the extra sensor. Experiments were performed showing the efficiency of the proposed approach. Acknowledgment This research is supported by ROBOTEX, the French national network of robotics platforms (No. ANR-10-EQPX-44-01). References [1] L. Birglen, C. Gosselin, N. Pouliot, B. Monsarrat, T. Laliberte, Shade, a new 3-dof haptic device, IEEE Trans. Robot. Autom. 18 (2) (2002) 166–175. [2] C. Gosselin, E.St. Pierre, M. Gagne, On the development of the agile eye, IEEE Robot. Autom. Mag. 3 (4) (1996) 29–37. [3] A. Ma, S. Payandeh, Analysis and experimentation of a 4-dof haptic device, Haptic Interfaces for Virtual Environment and Teleoperator Systems, Haptics 2008. Symposium on, 2008 2008, pp. 351–356. [4] A. Chaker, A. Mlika, M. Laribi, L. Romdhane, S. Zeghloul, Synthesis of spherical parallel manipulator for dexterous medical task, Front. Mech. Eng. 7 (2) (2012) 150–162. [5] S. Bai, Optimum design of spherical parallel manipulators for a prescribed workspace, Mech. Mach. Theory 45 (2) (2010) 200–211. [6] Z. Tao, Q. An, Interference analysis and workspace optimization of 3-RRR spherical parallel mechanism, Mech. Mach. Theory 69 (2013) 62–72. [7] A. Chaker, A. Mlika, M. Laribi, L. Romdhane, S. Zeghloul, On the kinematics of spherical parallel manipulators for real time applications, in: M. Haddar, L. Romdhane, J. Louati, A. Ben Amara (Eds.), Design and Modeling of Mechanical Systems, Lecture Notes in Mechanical Engineering, Springer, Berlin Heidelberg 2013, pp. 53–60. [8] R. Vertechy, V. Parenti-Castelli, Robust, fast and accurate solution of the direct position analysis of parallel manipulators by using extra-sensors, in: Huapeng Wu (Ed.)Parallel Manipulators, Towards New Applications, 2008. [9] I. Bonev, J. Ryu, S.-G. Kim, S.-K. Lee, A closed-form solution to the direct kinematics of nearly general parallel manipulators with optimally located three linear extra sensors, IEEE Trans. Robot. Autom. 17 (2) (2001) 148–156. [10] J.-P. Merlet, Closed-form resolution of the direct kinematics of parallel manipulators using extra sensors data, Robotics and Automation, Proceedings., 1993 IEEE International Conference on, 1993, vol.1 1993, pp. 200–204. [11] A. Zubizarreta, M. Marcos, I. Cabanes, C. Pinto, E. Portillo, Redundant sensor based control of the 3RRR parallel robot, Mech. Mach. Theory 54 (2012) 1–17.
H. Saafi et al. / Mechanism and Machine Theory 91 (2015) 102–119
119
[12] H. Saafi, M.A. Laribi, S. Zeghloul, Improvement of the direct kinematic model of a haptic device for medical application in real time using an extra sensor, Intelligent Robots and Systems (IROS 2014), 2014 IEEE/RSJ International Conference on, IEEE 2014, pp. 1697–1702. [13] M. Laribi, T. Riviere, M. Arsicault, S. Zeghloul, A design of slave surgical robot based on motion capture, Robotics and Biomimetics (ROBIO), 2012 IEEE International Conference on, IEEE 2012, pp. 600–605. [14] H. Saafi, M.A. Laribi, S. Zeghloul, Redundantly actuated 3-RRR spherical parallel manipulator used as a haptic device: improving dexterity and eliminating singularity, Robotica FirstView2014. 1–18. [15] J.P. Merlet, Direct kinematics of parallel manipulators, IEEE Trans. Robot. Autom. 9 (6) (1993) 842–846. [16] C.M. Gosselin, J.-P. Merlet, The direct kinematics of planar parallel manipulators: special architectures and number of solutions, Mech. Mach. Theory 29 (8) (1994) 1083–1097. [17] C.M. Gosselin, J. Sefrioui, M.J. Richard, On the direct kinematics of spherical three-degree-of-freedom parallel manipulators of general architecture, J. Mech. Des. 116 (2) (1994) 594–598. [18] E. Celaya, Interval propagation for solving parallel spherical mechanisms, in: J. Lenarcic, F. Thomas (Eds.), Advances in Robot Kinematics, Springer, Netherlands 2002, pp. 415–422. [19] S. Bai, M.R. Hansen, J. Angeles, A robust forward-displacement analysis of spherical parallel robots, Mech. Mach. Theory 44 (12) (2009) 2204–2216. [20] S. Bai, M.R. Hansen, Forward kinematics of spherical parallel manipulators with revolute joints, Advanced Intelligent Mechatronics, AIM 2008. IEEE/ASME International Conference on, IEEE, 2008 2008, pp. 522–527.