Cinemática inversa de un manipulador

Cinemática inversa de un manipulador

Problema:
determinar la posición de las articulaciones dada la posición del elemento terminal
base
Es decir, dado p = Ttool (q) se pretende encontrar q

Es un problema no lineal

Dada una transformación homogénea se tienen 12 ecuaciones con 6 incógnitas:

• la matriz de rotación sólo da 3 ecuaciones independientes

• el vector de translación da las otras 3
Casos posibles:

• No existe solución.

• Existe una solución.

• Existen múltiples soluciones; el problema es elegir cúal.

Hay varios criterios aplicables, lo tı́pico es elegir la más cercana

(dando pesos a las articulaciones).

Como ejemplo, el robot PUMA puede alcanzar hasta 8 configuaciones diferentes

Cuatro soluciones del robot PUMA 560

Enfoques:

• Soluciones numéricas (lentas!)

• Soluciones cerradas

Los robots se disean para que sean sencillas: ai = 0 y αi = 0 ó αi = π/2

Condición para que exista solución cerrada con 6 g.d.l: 3 ejes seguidos se intersecten
 The Unimation PUMA 560
As an example of the algebraic solution technique applied to a manipu­
lator with six degrees of freedom, we will solve the kincrnatic equations
of the PU MA 560 which were developed in Chapter 3. This solution is
in the style of [5].
We wish to solve

(4.49)

for 0i when g7'

is given as numeric values.
A restatement of (4.49) which puts the dependence on ()1 on the left
hand side of the equation is

(4.50)

Inverting ?T we write (4.50) as

o
o 0] [Tll
0 T21
(4.51)
1 0 T31
010
Where ~7' is given by equation (3.13) developed in Chapter 3. This
simple technique of multiplying each side of a t ransforrn equat ion by an
inverse is often used to advantage in separating out variables in search
of a solvable equation.
Equating the (2~ 4) elernents from both sides of (4.51), we have

(4.52)

To solve an equation of this form, we make the trigonometric substi­

t utions
Px=pcos¢,
(4.53 )
Py = psin 4>.
where
p= Jp; + P~.
(4.54)
4> = Atan2 (p Y ' PI)'
 Substituting (4.53) into (4.52), we obtain

(4.55 )

Using the difference of angles formula:

sin(¢ - B.) = d3 • (4.56)

P

Hence

cos (¢ - 0 I J = ± V[df
1 - rJ, (4.57)

and so

(4.58)

Finally, the solution for (}1 may be written:

0 = Atan2 (Pu,PrJ - Atan2 (d ±.jp; + P; - d~) .

1 3, (4.59)

Note that we have found two possible solutions for ()1 corresponding to
the plus-or-minus -sign in (4.59). Now that () 1 is known, the left hand
side of (4.51) is known. If we equate the (1,4) elements from both sides
of (4.51) and also the (3,4) elements, we obtain

(4.60)

If we square equations (4.60) and (4.52) and add the resulting equations.
we obtain

(4.61 )

where
2 + p2 + p2 _ a2 _ a2 _ d2 _ d2
K = Px y z 2 3 3 4 (4.62)
2a2
Note that dependence on 01 has been removed from (4.61). Equa­
tion (4.61) is of the same form as (4.52) and so may be solved by the
same kind of trigonometric substitution to yield a solution for 03:

(4.63 )
 The plus or minus sign in (4.63) leads to two different solutions for 03.
If we consider (4.49) again, we can now rewrite it so that all the
left-hand side is a function of only knowns and 02:

(4.64)

or

T 13 px]
T23 P y _ 3
- 6T, (4.65 )
T3 3 u,
o 1

where ~T is given by equation (3.11) developed in Chapter 3. Equating

the (1,4) elements from both sides of (4.65), as well as the (2,4) elements,
we get

(4.66)

These equations may be solved simultaneously for 823 and c23' resulting
In
(-a3 - a2 c3)pz + (c1Px -+ slP y)(a2 s3 - d4 )
823 = 2
Pz + (C1pz + SlP y )
2 '

(4.67)
(a2 83 - d 4 )pz - (-a3 - a2 c3)(c 1 Pz + SlP y )
C23 = 2 2
v; + (C1Px + SlP y )
Since the denominators are equal and positive, we solve for the sum of
()2 and ()3 as

823 = Atan2[( -a3 - a2 c3)pz - (c1P:t + slP y )(d4 - a 2-'3),

(4.68)·
(a2 s3 - d 4 )pz + (a3 + a2 c3)(c 1Px + SlP y)]

Equation (4.68) computes four values of 023 according to the four

possible combinations of solutions for Oland 3 , Then, four possible
solutions for ()2 are computed as
°
(4.69)

where the appropriate solution for 03 is used when forming the difference.
Now the entire left side of (4.65) is known. Equating the (1. 3)
elements from both sides of (4.65), as well as the (3,3) elements, we get

(4.70)
 As long as 85 -I 0, we can solve for 04 as

(4.71)

When 05 = 0 the manipulator is in a singular configuration in which

joint axes 4 and 6 line up and cause the same motion of the last link
of the robot. In this case, all that matters (and all that can be solved
for) is the sum or difference of 04 and 06' This situation is detected by
checking whether both arguments of the Atan2 in (4.71) are near zero.
If so, 04 is chosen arbitrarily,* and when 06 is computed later, it will
be computed accordingly.
If we consider (4.49) again, we can now rewrite it so that all the
left-hand side is a function of only knowns and 04 by rewriting it as:

[~T(04)] -1 ~T = ~T(05) ~T(06)' (4.72)

where [~T(04)]-1 is given by

Clc23C4 + 81 84 81 C23 C4 ~ CIS ..

-CI C23 84 + 8 1 C4 -8 1 C 2 3 S4 - CI C4
-C 1 8 2 3 -SI S23
[
o 0

and ~T is given by equation (3.10) developed in Chapter 3. Equating the

(1,3) elements from both sides of (4.72), as well as the (3,3) elements,
.we get

(4.74)

So we can solve for Os as:

Os = Atan2 (85' cs), (4.75 )

where 8 S and Cs are given by (4.74) above.

Applying the same method one more time, we compute (~T) -1 and
write (4.49) in the form

(4.76)

Equating the (3, 1) elements from both sides of (4.72), as well as the
(1, 1) elements as we have done before, we get

(4.77)

* It is usually chosen to be equal to the present value of joint 4.

where

C6 = Til [(CIC23 C4 + SIS4)C S - CIS 23 SS] + T21 [(SI C23 C4 - CIS 4)CS - SI S23 SS]

- T31(S23 C4CS + C23 SS)'

Because of the plus-or-minus signs appearing in (4.59) and (4.63), these

equations compute four solutions. Additionally, there are four more
solutions obtained by "flipping" the wrist of the manipulat.or. For each
of the four solutions computed above, we obtain the flipped solution by:

(4.78)

After all eight solutions have been computed some or all of them may
have to be discarded because of joint limit violations. Of the remaining
valid solutions, usually the one closest to the present manipulator
configuration is chosen.