- Email: [email protected]
Efficient Computation of the Inverse Kinematic Model
Copnight © IF .\ C Theory of Rohots . \ 'ie lllla . Austria I~I H6
MODELLING
EFFICIENT COMPUTATION OF THE INVERSE KINEMATIC MODEL B. Caron Ecole Celllrale de LYOII. D,;pol'lfIIlflll de M!/Ihhll!/Iique.\. illfurlll!/Iiqul'. Syslhlle.\, Ec ul/.\' , Frall ce
Abstract. Sorre roOOt =ntrols need the inverse kinenatic m:x1el, thus it Imlst be CClTpUted in real tirre. This paper presents a canPJ,tation metJuj which reduces its nlm1ber of I1'atrerratical operations. ForIl'al procerlures are used to canpute and simplify this nodel, then the carplexity of the inverse kinenatic m:x1els CClTpUterl by different methods are oonparerl. The metJujs have been trierl on a particular roOOt to sh::>w the advantage of the presenterl metJuj. Keyv.ords. RoJ:ot; carputer aiderl design; canputational metJuj inverse kinenatic model ; on line operation.
I o~ ~1 ~ I
INI'IDDUCI'ION Z
There are automatic abilities to obtain the Inverse Kinenatic r-biel (LK.M.) by means of fOrI1'al procedures in opposition with the inverse geometric nodel. Since the LK.M. has to be computerl in real time for sare =ntrols, the numl:er of I1'athenatical operations Imlst obviously be as small as possible. Of =nsider in this paper is a metJuj which allows to obtain the LK.M. first by arranging the equations and second by a rerluction of the rerlundant expressions.
0
Z~
the third =lumn of the I1'atrix
J
lIY
where , , e~ , a~ , a ~ are the four Denavit-Hartenberg parameters (Denavit, 1955) which define the position and orientation of the link ~ or the frame ~+ 1 in frame L
r~
1981 ; whitney, 1972) have developped and corrparerl different ways to obtain the LK.M. which generally allow to rerluce the number of operations. Arrong them, one of the most attracting is the following :
E i
BjjRj
o
E
0
0l llY
Rj
CXMPUI'ATION ME:TIDD (1)
Forredundancies reduction, the LK.M. can be expressed by (first method) (Caron, 1985),
where, J (j) is the nxn kinenatic torque in frarre i written i inj E the 3x3 unit I1'atrix
(UB+V )
m
.y:+1 ~
n
where J . (j) ~
~
~
-
Zj +
0
-
~
-
i
~ Rj Z T ~
-1 = /Uw V I X
This first metJuj allows principally to obtain the inverse ja=bian I1'atrix J"1, but tQ is only needed in function of /Si . Thus, another rretJuj is proposed which gives tQ iI1 function of /Si with:lUt the canputation of the-inverse ja=bian natrix.
The ~th =lumn of J. (j) is,
(4)
-
J
J ~ ~ rotation I1'atrix fran frame ~ fixerl on jOint ~ _ 1 to frame m the tensor of the position canponents of fr:me n+1 in frame i the numl:er of jOints, in our case it's alro the dimension of lIY and n is 6
R~
I
R~J lIY = J -1 lIY
(WB+X) R:
R~ y:+1 R~
B
J
the variation of j o ints value , the ~th element of lIg, callerl lIQ~ is o~ r ~ + a~ r~
Sorre previous papers (Orin, 1983 ; Renaud, 1980,
=J (j)-1/
R~
the position variation (three first canponents) of the end effector along the three axes of the fixed frame. The last three ccrnponents are the variation o rientation of the end effector around the thre e axes of the fixed frame
THE lNVERSE KINEMATIC M)DEL
lIg
(3)
0
(2)
~
The second metJuj is the following,
o~ Zj
where, is 1 when joint or 0 when joint and =1- 0
a ~
~ ~
is priI1'atic is rotoid
t:::l =
(5)
~
(6)
47
48
B . Caron
I: :I~
6Q =
A cos 2 (B) + A sin 2 (B)
(7)
Dcos (A) cos (B+A) +Dsin (A) sin (B+A) = 0 cos (B)
The tv.o methods need,
R~)
r:+ 1 R~ ~
~
Then, rratrix Ji (j) is inverted using the c:x::np1tion of its detenninant and its adjoint rratrix. Finally rratrix Ji (j) -1 is s:iIrplified to s\Wress the ratios of the same quantities. Indices i and j must be ch:::>sen to obtain the rrost siIrple rratrix J. (j) -1 , in our case i = 5 and j = 4 (CDrba, 1984) ~ Then the result is,
=B
J . (j)-l ~
1
R. )
these develo~ts are the same for the tv.o methods and depend of the robot. These c:x::np1tations will be discussed fU.rther, thus _ can r'CW cx::rrpare the other c:x::np1tations by their number of operatiorE in the general case, see Table 1 and Table 2. TABLE 1 Number of operations for the first method multiplications
additions/subtractions
U R:
27
18
W R\
27
18
U B
18
9
)
)
WB
18
9
(nB+V)R~
27
27
27
27
36 180
30 138
)
(WB+X)R\
-1 J
6'!..
)
= A = 0
A - A
J. (j)-l ~
-l/Q3 0 0 -1 0 0 0 0 1 S4C5/Q355 0 0 -C4/Q3 0 0 -54/ Q355 0 0
0 0 0 0 0 0 -C4C5/55 -54C5/ 55 -54 C4 C4/55 54/ 55
Where 5X, ex and OX mean sin (Qx), cos CCMPUrATION OF
(~)
0 0 0 1 0 0
and ~
oQ
First Eq. (5) is carputed and siIrplified to suwress the redundant expressions, i . e. all the products or divisions which appear rrore than once, are replaced by a new variable using the algorithm given in Fig. 1
Search in J i (j) -1 the rrost
TABLE 2 Number of operations for the second method multiplications
frequent variable called VARl
additions/ subtractions
Eq.
(5)
18
12
Eq.
(6)
6
6
Eq.
(7)
36 60
30 48
Search in the expressions including VAAI _
!lOst
frequent variable called w.R2
The second method is approxirratly three times better than the first, but for a particular robot the number of rrat.henatical operations is minor because rratrix J (j) -1 have rrany zeros. i Replace VAAI (op.) \Il\R2 by a new
CCMPUrATICN OF J. (j) -1 ~
variable in all J (j) -I i
The rortputation is tried on a robot defined in Table 3 by its Denavit-Hartenberg parameters.
TABLE 3 Robot definition joint
()'
8
a
r
a
1
0
81
0
0
0
2
1
0
-11/2
Q2
0
3
1
0
0
Q3
0 0
4
0
8.
- 11/2
0
5
0
85
11/2
0
0
6
0
86
11/2
Rs
0
Autanatic carputations are rrade by formal and string libraries written in FORI'RAN 77 for VAX 780. The rratrix J. (j) is c::atputed using Eq. (2) and after, &:Ire f
Fig. 1 Redundancies reduction
Then Eq. (6) and (Eq. (7) are carputed using the same technic given in Fig. 1. Table 4 gives the intermediate variables created, where Yi are elarents of vector 6'!...
The I ll\'Crsc Kinematic !\Iuclcl TABLE 4 Intermediate variables MOl = SlYl -CIY2 M02 = CIYl + SlY2 K04 = Y6 K05 = SlY4 -CIYS K06 = CIY4 + SlY5 DOl = S5R6 D02 = -C5R6 M03 = C4 DOl M04 = 54 DOl KOl = -D02 K05 + M04 K06 + Y3 K02 = D02 K04 -M03 K06 + MOl K03 = - M04 K04 + M03 K05 + M02 Finally
62
62 =
JOl J02 J03 J04 J05 J06 J07 J08 J09 JIO J11
-K02/ Q3 -JOl + K04 J02 C4 = K05 54 = J03 + J04 = J05/S5 = JOG C5 = J07 + KOG = J02 54 = -C4 K05 = J09 + JIO
is given by,
IJOl, KOl, K03, J08, Jll, J06 l
t
(8)
CCMPARISON OF THE MEI'fDDS
Three meth:xl.s are now corrpare:l on the robot define:l in Table 2 by their mnnber of rrathenatical operations. The first I.K.M. is compute:l by hand using the inverse jacobian rratrix (Gxla, 1984). The second is automatically obtaine:l with the first meth:xl.. Eq. (4) and the third with the second algorithm Eq. (5) to Eq. (7).
Megahed, S.M. (1984). Contribution a la modelisation gearretrique et dynamique des robots IlI3niPJlateurs a structure de chaine cinerratique simple ou camplexe. These de doctorat d'etat, Juillet, Toulouse, pp. 38=44.
Orin , D.E., and w.w. Schrader (1983). Efficient jacobian detennination for robot IlI3nipulators. 1st ISRR Bretton Y.OOds, aug-sept 1983. Renaud, M. (1980). Calcul de la rratrice jacobienne necessaire a la cormande ooordonnee d 'un IlI3nipulateur. Mechanism and Machine Theory, vol. 15, FP. 81-91. Renaud, M. (1981). Georretric and kinenatic models of a robot IlI3nipulator : calculation of the jacobian rratrix and its inverse. 11th ISIR, Tokyo, 7-9 aug 1981. Whitney, D.E. (1972). The rratherratics of ooordinated control of prosthetic arms and IlI3nipulators. J. of Dyn. Syst. Meas. and Cont., December, pp. 303 309.
Table 5 gives the matherratical operations for the three meth:xl.s and one inverse geometric model (Megahed, 1984).
TABLE 5 Canparison of the meth:xl.s first LK.M. second LK.M. third LK.M. Inverse geometric model
additions multiplications divisions sine/cosine ATAN Square roots 8 0 31 60 5 o 34 69 G 8 0 o 14 23 2 8 0 o 9 15 2 o 4 2
The third LK.M. uses less rratrematical operations than the tv.o first, but it uses a little rrore operations than the inverse geometric model.
CXlNCLUSION The meth:xl. presented can CO!1pJte the LK.M. with a great re:luction of the re:lundancies, therefore it can be easily used in real time. Scene improverrents are actually done, the rrost iroportant consists in the use of a formal language such as MUMATH written in LISP for IEf1 Fe. This will increase the efficiency of the redundancies reduction but also the computation time of this reduction.
REFERENCES
Caron, B. (1985). Etude et realisation d'un logiciel d'aide a la modelisat~ de systemes mecaniques articules. These de 3 cycle, Lyon (France), ch 2 pp. 79-85. Denavit, J., and R.S. Hartenberg (1955). A kinenatic notation for lower pair mechanisms based on matrices. J. of Applied Mechanics, June, pp. 215221.
49
B. Caroll
50
Search in J i ( j )
-1
the rrost
frequent variable called VARl
Search in the expressions including VARl the IIDst frequent variable called VAR2
YES
.__ ._ _ _ . _ _..L..._ Replace VARl
_ _ __
(op.) VAF2 by a new
variable in all J
i
(j )-1
~
MODELLING
EFFICIENT COMPUTATION OF THE INVERSE KINEMATIC MODEL B. Caron Ecole Celllrale de LYOII. D,;pol'lfIIlflll de M!/Ihhll!/Iique.\. illfurlll!/Iiqul'. Syslhlle.\, Ec ul/.\' , Frall ce
Abstract. Sorre roOOt =ntrols need the inverse kinenatic m:x1el, thus it Imlst be CClTpUted in real tirre. This paper presents a canPJ,tation metJuj which reduces its nlm1ber of I1'atrerratical operations. ForIl'al procerlures are used to canpute and simplify this nodel, then the carplexity of the inverse kinenatic m:x1els CClTpUterl by different methods are oonparerl. The metJujs have been trierl on a particular roOOt to sh::>w the advantage of the presenterl metJuj. Keyv.ords. RoJ:ot; carputer aiderl design; canputational metJuj inverse kinenatic model ; on line operation.
I o~ ~1 ~ I
INI'IDDUCI'ION Z
There are automatic abilities to obtain the Inverse Kinenatic r-biel (LK.M.) by means of fOrI1'al procedures in opposition with the inverse geometric nodel. Since the LK.M. has to be computerl in real time for sare =ntrols, the numl:er of I1'athenatical operations Imlst obviously be as small as possible. Of =nsider in this paper is a metJuj which allows to obtain the LK.M. first by arranging the equations and second by a rerluction of the rerlundant expressions.
0
Z~
the third =lumn of the I1'atrix
J
lIY
where , , e~ , a~ , a ~ are the four Denavit-Hartenberg parameters (Denavit, 1955) which define the position and orientation of the link ~ or the frame ~+ 1 in frame L
r~
1981 ; whitney, 1972) have developped and corrparerl different ways to obtain the LK.M. which generally allow to rerluce the number of operations. Arrong them, one of the most attracting is the following :
E i
BjjRj
o
E
0
0l llY
Rj
CXMPUI'ATION ME:TIDD (1)
Forredundancies reduction, the LK.M. can be expressed by (first method) (Caron, 1985),
where, J (j) is the nxn kinenatic torque in frarre i written i inj E the 3x3 unit I1'atrix
(UB+V )
m
.y:+1 ~
n
where J . (j) ~
~
~
-
Zj +
0
-
~
-
i
~ Rj Z T ~
-1 = /Uw V I X
This first metJuj allows principally to obtain the inverse ja=bian I1'atrix J"1, but tQ is only needed in function of /Si . Thus, another rretJuj is proposed which gives tQ iI1 function of /Si with:lUt the canputation of the-inverse ja=bian natrix.
The ~th =lumn of J. (j) is,
(4)
-
J
J ~ ~ rotation I1'atrix fran frame ~ fixerl on jOint ~ _ 1 to frame m the tensor of the position canponents of fr:me n+1 in frame i the numl:er of jOints, in our case it's alro the dimension of lIY and n is 6
R~
I
R~J lIY = J -1 lIY
(WB+X) R:
R~ y:+1 R~
B
J
the variation of j o ints value , the ~th element of lIg, callerl lIQ~ is o~ r ~ + a~ r~
Sorre previous papers (Orin, 1983 ; Renaud, 1980,
=J (j)-1/
R~
the position variation (three first canponents) of the end effector along the three axes of the fixed frame. The last three ccrnponents are the variation o rientation of the end effector around the thre e axes of the fixed frame
THE lNVERSE KINEMATIC M)DEL
lIg
(3)
0
(2)
~
The second metJuj is the following,
o~ Zj
where, is 1 when joint or 0 when joint and =1- 0
a ~
~ ~
is priI1'atic is rotoid
t:::l =
(5)
~
(6)
47
48
B . Caron
I: :I~
6Q =
A cos 2 (B) + A sin 2 (B)
(7)
Dcos (A) cos (B+A) +Dsin (A) sin (B+A) = 0 cos (B)
The tv.o methods need,
R~)
r:+ 1 R~ ~
~
Then, rratrix Ji (j) is inverted using the c:x::np1tion of its detenninant and its adjoint rratrix. Finally rratrix Ji (j) -1 is s:iIrplified to s\Wress the ratios of the same quantities. Indices i and j must be ch:::>sen to obtain the rrost siIrple rratrix J. (j) -1 , in our case i = 5 and j = 4 (CDrba, 1984) ~ Then the result is,
=B
J . (j)-l ~
1
R. )
these develo~ts are the same for the tv.o methods and depend of the robot. These c:x::np1tations will be discussed fU.rther, thus _ can r'CW cx::rrpare the other c:x::np1tations by their number of operatiorE in the general case, see Table 1 and Table 2. TABLE 1 Number of operations for the first method multiplications
additions/subtractions
U R:
27
18
W R\
27
18
U B
18
9
)
)
WB
18
9
(nB+V)R~
27
27
27
27
36 180
30 138
)
(WB+X)R\
-1 J
6'!..
)
= A = 0
A - A
J. (j)-l ~
-l/Q3 0 0 -1 0 0 0 0 1 S4C5/Q355 0 0 -C4/Q3 0 0 -54/ Q355 0 0
0 0 0 0 0 0 -C4C5/55 -54C5/ 55 -54 C4 C4/55 54/ 55
Where 5X, ex and OX mean sin (Qx), cos CCMPUrATION OF
(~)
0 0 0 1 0 0
and ~
oQ
First Eq. (5) is carputed and siIrplified to suwress the redundant expressions, i . e. all the products or divisions which appear rrore than once, are replaced by a new variable using the algorithm given in Fig. 1
Search in J i (j) -1 the rrost
TABLE 2 Number of operations for the second method multiplications
frequent variable called VARl
additions/ subtractions
Eq.
(5)
18
12
Eq.
(6)
6
6
Eq.
(7)
36 60
30 48
Search in the expressions including VAAI _
!lOst
frequent variable called w.R2
The second method is approxirratly three times better than the first, but for a particular robot the number of rrat.henatical operations is minor because rratrix J (j) -1 have rrany zeros. i Replace VAAI (op.) \Il\R2 by a new
CCMPUrATICN OF J. (j) -1 ~
variable in all J (j) -I i
The rortputation is tried on a robot defined in Table 3 by its Denavit-Hartenberg parameters.
TABLE 3 Robot definition joint
()'
8
a
r
a
1
0
81
0
0
0
2
1
0
-11/2
Q2
0
3
1
0
0
Q3
0 0
4
0
8.
- 11/2
0
5
0
85
11/2
0
0
6
0
86
11/2
Rs
0
Autanatic carputations are rrade by formal and string libraries written in FORI'RAN 77 for VAX 780. The rratrix J. (j) is c::atputed using Eq. (2) and after, &:Ire f
Fig. 1 Redundancies reduction
Then Eq. (6) and (Eq. (7) are carputed using the same technic given in Fig. 1. Table 4 gives the intermediate variables created, where Yi are elarents of vector 6'!...
The I ll\'Crsc Kinematic !\Iuclcl TABLE 4 Intermediate variables MOl = SlYl -CIY2 M02 = CIYl + SlY2 K04 = Y6 K05 = SlY4 -CIYS K06 = CIY4 + SlY5 DOl = S5R6 D02 = -C5R6 M03 = C4 DOl M04 = 54 DOl KOl = -D02 K05 + M04 K06 + Y3 K02 = D02 K04 -M03 K06 + MOl K03 = - M04 K04 + M03 K05 + M02 Finally
62
62 =
JOl J02 J03 J04 J05 J06 J07 J08 J09 JIO J11
-K02/ Q3 -JOl + K04 J02 C4 = K05 54 = J03 + J04 = J05/S5 = JOG C5 = J07 + KOG = J02 54 = -C4 K05 = J09 + JIO
is given by,
IJOl, KOl, K03, J08, Jll, J06 l
t
(8)
CCMPARISON OF THE MEI'fDDS
Three meth:xl.s are now corrpare:l on the robot define:l in Table 2 by their mnnber of rrathenatical operations. The first I.K.M. is compute:l by hand using the inverse jacobian rratrix (Gxla, 1984). The second is automatically obtaine:l with the first meth:xl.. Eq. (4) and the third with the second algorithm Eq. (5) to Eq. (7).
Megahed, S.M. (1984). Contribution a la modelisation gearretrique et dynamique des robots IlI3niPJlateurs a structure de chaine cinerratique simple ou camplexe. These de doctorat d'etat, Juillet, Toulouse, pp. 38=44.
Orin , D.E., and w.w. Schrader (1983). Efficient jacobian detennination for robot IlI3nipulators. 1st ISRR Bretton Y.OOds, aug-sept 1983. Renaud, M. (1980). Calcul de la rratrice jacobienne necessaire a la cormande ooordonnee d 'un IlI3nipulateur. Mechanism and Machine Theory, vol. 15, FP. 81-91. Renaud, M. (1981). Georretric and kinenatic models of a robot IlI3nipulator : calculation of the jacobian rratrix and its inverse. 11th ISIR, Tokyo, 7-9 aug 1981. Whitney, D.E. (1972). The rratherratics of ooordinated control of prosthetic arms and IlI3nipulators. J. of Dyn. Syst. Meas. and Cont., December, pp. 303 309.
Table 5 gives the matherratical operations for the three meth:xl.s and one inverse geometric model (Megahed, 1984).
TABLE 5 Canparison of the meth:xl.s first LK.M. second LK.M. third LK.M. Inverse geometric model
additions multiplications divisions sine/cosine ATAN Square roots 8 0 31 60 5 o 34 69 G 8 0 o 14 23 2 8 0 o 9 15 2 o 4 2
The third LK.M. uses less rratrematical operations than the tv.o first, but it uses a little rrore operations than the inverse geometric model.
CXlNCLUSION The meth:xl. presented can CO!1pJte the LK.M. with a great re:luction of the re:lundancies, therefore it can be easily used in real time. Scene improverrents are actually done, the rrost iroportant consists in the use of a formal language such as MUMATH written in LISP for IEf1 Fe. This will increase the efficiency of the redundancies reduction but also the computation time of this reduction.
REFERENCES
Caron, B. (1985). Etude et realisation d'un logiciel d'aide a la modelisat~ de systemes mecaniques articules. These de 3 cycle, Lyon (France), ch 2 pp. 79-85. Denavit, J., and R.S. Hartenberg (1955). A kinenatic notation for lower pair mechanisms based on matrices. J. of Applied Mechanics, June, pp. 215221.
49
B. Caroll
50
Search in J i ( j )
-1
the rrost
frequent variable called VARl
Search in the expressions including VARl the IIDst frequent variable called VAR2
YES
.__ ._ _ _ . _ _..L..._ Replace VARl
_ _ __
(op.) VAF2 by a new
variable in all J
i
(j )-1
~