4.3 Wampler’s Method
As it has been said before, SPM does not allow for a closed-form solution of the FDA, due to the peculiar multi-loop architecture, and because of the non linear trigonometric equations, the resolution of the algorithm has a high complexity. For these reasons, Wampler’s approach has been used with the objective of extending the kinematics of a four-bar linkages to the 3-RRR Coaxial SPM. The advantages are mainly two:
managing the two loop equations with compact constant coefficients; solving the equations in a semi-graphically way.
The consideration as for the geometry of the robot in different configurations (eg. general, coaxial) are exactly the same; for this reason, the dissertation should be done on the general architecture of SPM and in literary shape.
(𝟐)
(𝟏)
Figure 4.4: Spherical Four-Bar Linkages Aapplies on 3-RRR SPM [3].
The characteristic parameters of the robot are the same as those already used in the Gosselin’s method.
In fact, knowing the orientation of the ui an wi axes of the revolute joints, one of the
target of the FDA is to identify the orientation of the vi axes, expressing them with
respect to fixed RF:
using the same three unit vectors of the mobile platform as unknowns:
vi· vj = cos α3 i, j = 1, 2, 3, i 6= j ||vi|| = 1 (4.34)
where cos α3 is the angle between two distal joints of two different legs of the robot.
using the Euler angles of the mobile platform as unknowns:
[vi]R0 = Q[vi]REE Q = Q(ϕ) ϕ = [ϕ1, ϕ2, ϕ3]
4. FORWARD KINEMATICS ANALYSIS 4.3. Wampler’s Method
using the actuated joint angles as unknowns:
[vi]R0 = Ri[vi]REE Ri = Ri(ϑi, φi, ψi) i = 1, 2, 3 (4.36) in this case, the actuator angle ϑi are the input and the unknowns are the passive ones
of the remained joints.
Before entering in the core of the algorithm, it is possible to do an explanation, considering a schematic spherical four-bar loop, in order to identify the loop equation. Then, this theory will be extended to the SPM, involving two spherical four-bar linkages.
Figure 4.5: Spherical Four-Bar Linkages [3].
As it possible to see in figure 4.5, by moving from A-point to D-point, two kinds of rotations are considered in this loop:
joint rotation: it describes the relative orientation (it is constant) of each neighbouring pair of joint axes of a link;
side rotation: that is variable.
The loop equations can be expressed as follows:
I = Z4S4Z1S1Z2S2Z3S3 (4.37)
Z1 = Rz(φ) Z2 =Rz(π − ψ) Z3 = Rz(ϑ3) Z4 = Rz(ϑ4)
Si= Rx(αi) i = 1, 2, 3, 4
4.3. Wampler’s Method 4. FORWARD KINEMATICS ANALYSIS
zTS3Tz = zTS4Z1S1Z2S2z z = [0, 0, 1]T (4.38)
Expliciting the relation below, it has been achieved the final expression, based on constant terms (depending on the characteristic angles) and the unknowns φ and ψ angles.
k1+ k2cos ψ + k3cos ψ cos φ − k4cos φ + k5sin ψ sin φ = 0 (4.39)
k1 = cα1cα2cα4− cα3 k2 = sα1sα2cα4 k3 = cα1sα2sα4 k4 = sα1cα2sα4 k5 = sα2sα4
Therefore, the resolute algorithm can be shown, considering two spherical four-bar linkages for the 3-RRR SPM ( A1C1C2A2, A1C1C3A3), as you can see in figure 4.4.
definition of the unit vectors of the joints: ui = − sin ηisin β1 cos ηisin β1 − cos β1 wi = −sηisβ1cα1 + (cηisϑi− sηicβ1cϑi)sα1 −cηisβ1cα1 + (sηisϑi− cηicβ1cϑi)sα1 −cβ1cα1+ sβ1cϑisα1 (4.40) [v1]RFEE = 0 0 1 [v2]RFEE = sin γ2 0 cos γ2 [v3]RFEE = cos ψ2sinγ22 sin ψ2 cos ψ2cosγ22 (4.41)
identification and calculation of the angles on the sphere:
α4= cos−1(w1· w2) α4 ∈ (0, π] (4.42) α3= cos−1([v1]REE· [v3]REE) α3 ∈ (0, π] (4.43) ¯ α3= cos−1([v1]REE· [v3]REE) α¯3 ∈ (0, π] (4.44) (4.45) ( cos α5 = w1· w3 sin α5 = ||w1× w3|| (4.46) ( cos α45= w2· w3 sin α45= ||w2× w3|| (4.47)
σ = 2π − cos−1[(cos α45− cos α4cos α5)/(sin α4sin α5) (4.48)
µ = cos−1[(csc2α3)(cos α3− cos2α3)] µ ∈ (0, π]; (4.49)
¯
φ = 2π − φ − σ (4.50)
¯
4. FORWARD KINEMATICS ANALYSIS 4.3. Wampler’s Method
selection of the two loop equations: – Loop Equation (1):
k1+ k2cos ψ+k3cos ψ cos φ − k4cos φ + k5sin ψ sin φ = 0 (4.52)
A1(φ)cψ + B1(φ)sψ + C1(φ)= 0 (4.53)
where the constant terms are function of characteristics angles: ki = F (α2, α3, α4) i = 1, 2, .., 5
– Loop Equation (2):
j1+ j2cos ¯ψ+j3cos ¯ψ cos ¯φ − j4cos ¯φ + j5sin ¯ψ sin ¯φ = 0 (4.54)
A2( ¯φ)c ¯ψ + B2( ¯φ)s ¯ψ + C2( ¯φ) = 0 (4.55) it is possible to express the second equation in function of φ and ψ, in order to solve the linear system in sin ψ and cos ψ. Moreover, the constants ji are function
of:
ji = F (α2, ¯α3, α5, µ, σ) j = 1, 2, .., 5
solving the linear system, in function of φ and ψ angles: sin ψ = −B2C1+B1C2 −A2B1+A1B2 cos ψ = −A2C1+A1C2 −A2B1+A1B2 (4.56)
4.3. Wampler’s Method 4. FORWARD KINEMATICS ANALYSIS
semi-graphical equation solving: assuming the denominator −A2B1+ A1J2 6= 0
and evaluating the trigonometric identity, it is possible to obtain:
sin2ψ1+ cos2ψ1= 1 (4.57)
C22A21+ C22B12+ 2A2B2A1B1− B22A21+ A22C12− 2A2C2A1C1 (4.58)
− 2B2C2B1C1− A22B12+ B22C12 = 0 (4.59)
Rearraging the new equation in function of cos φ and sin φ, it has been achieved the eight solutions, coming from the intersection with these two curves, as shown in figure 4.6: ( x = cos φ y = sin φ ⇒ ( f (x, y) = 0 x2+ y2= 1 (4.60)
solving the octic polynomial, using the tan-half identities:
8
X
i=0
Niti = 0 i = 1, 2, .., 8 (4.61)
substituting sφ and cφwith an only parameter t (tan-half)
cos φ = 1 − t 2 1 + t2 sin φ = 2t 1 + t2 t = tan φ 2 (4.62)