Escolar Documentos
Profissional Documentos
Cultura Documentos
1. Introduction
and the Jacobian matrix J being defined as where J† is defined as the right generalized inverse of
J such that
A
J≡ , A, B ∈ <3×n , (7) J† ≡ JT (JJT )−1 , (12)
B
and h is an arbitrary vector of J allowing to satisfy a
where ω ∈ <3 is the angular velocity vector of the secondary task. The first term in the right-hand side
EE, ṗ ∈ <3 is the translation velocity vector of a (RHS) of eq.(11) is known as the minimum-norm so-
point on the EE, while θ˙i is the velocity of joint i. It lution of eq.(5), i.e., the ∆θ M that minimizes k∆θk
is noteworthy that t is not defined as a vector of <6 , among all the ∆θ that are solutions of eq.(5). The
but rather as a set of two vectors of <3 casted into second part of the RHS of eq.(11) is known as the
a column array, and hence, t ∈ 2 × <3 6= <6 . This homogeneous solution of eq.(5), i.e., the ∆θ H that
distinction is important and will be further used in produce ∆t = 0, i.e., no displacement of the EE.
section 3. For the sake of numerical computation, we This joint displacement ∆θ H is also known as the
self-motion of the manipulator. It is symbolically jector M⊥ projects those vectors onto the orthogonal
computed as the projection of an arbitrary vector h subspace M⊥ . These projectors are given for the four
onto the null space of J with the orthogonal projector possible dimensions i of subspaces of <3 as:
(1 − J† J). Equation (11) is used to solve the kine-
matic inversion of intrinsically-redundant manipula-
1
O i = 3 ⇒ 3-D task
tors by many researchers, e.g., Siciliano (1992) and
P L i = 2 ⇒ 2-D task
Yoshikawa (1996), including Arenson et al. (1998) Mi = , M⊥
i = , (14)
who payed a special attention in avoiding the squar-
L
P i = 1 ⇒ 1-D task
ing of the condition number while solving eq.(11).
O 1 i = 0 ⇒ 0-D task
Sometimes, it exists a time-invariant frame in which
the functional redundancy are rows of J. In this case, where the plane and line projectors, P and L, respec-
the eq.(11) can be used to solve the inverse kinematics tively, are defined as:
with these rows previously eliminated from J. For
example, an arc-welding task may require the 3-D P ≡ 1 − L, L ≡ eeT , (15)
positioning of the end-point of the welding electrode in which e is a unit vector along the line L and normal
as keeping the electrode axis always parallel with one to the plane P. The null-projector O is the 3 × 3 zero
axis of the base frame. In this case, the redundant matrix that projects any vector of <3 onto the null-
task is the orientation around the electrode axis, e.g., subspace O, while the identity-projector 1 is the 3×3
the vertical axis, which is time-invariant in the base identity matrix that projects any vector of <3 onto
frame. Most of the times, it is not possible to find itself.
such a time-invariant frame to express the functional
redundancy of a task. For general arc-welding, the 3.2. Orthogonal-Decomposition of Twists
electrode axis needs to undergo arbitrary orientations
in space, and hence, the electrode axis is time-varying Any twist array ( · ) of 2 × <3 can also be decom-
in base frame. Therefore, eq.(11) can not be used. posed into two orthogonal parts, [ · ]T , the component
Below, a TWA on solving this problem is compared lying on the task subspace, T , and [ · ]T ⊥ , the com-
with the augmented approach. ponent lying on the orthogonal task subspace (also
designated as the redundant subspace), T ⊥ , using the
3. Twist-Decomposition Approach
twist projector T and an orthogonal complement of
T, namely T⊥ , as follows:
Before proposing a functional-redundancy resolu-
tion algorithm, let us briefly review the orthogonal ( · ) = [ · ]T + [ · ]T ⊥ = T( · ) + T⊥ ( · ) (16)
decomposition of vectors and twists.
It is apparent from eq.(16), that T and T⊥ are pro-
3.1. Orthogonal-Decomposition of Vectors jectors of twists that must verify all the properties of
projectors reviewed before. However, twists are not
Decomposing any vector ( · ) of <3 into two orthog- vectors of <6 , and hence, projectors of twists cannot
onal parts, [ · ]M , the component lying in the subspace, be defined as in eqs.(14) and (15), e.g.,
M, and [ · ]M ⊥ , the component lying in the orthogo- T 6= ttT , T⊥ 6= 1 − ttT , (17)
nal subspace, M⊥ , using the projector M and an or-
thogonal complement of M, namely M⊥ , as follows: but must rather be defined as block diagonal matrices
of projectors of <3 , i.e.,
( · ) = [ · ]M + [ · ]M ⊥ = M( · ) + M⊥ ( · ). (13)
It is apparent from eq.(13), that M and M⊥ are related Mω O
T≡
, (18)
by M + M⊥ = 1 and MM⊥ = O, where 1 and O O Mv
are the 3 × 3 identity and zero matrices, respectively.
The orthogonal complement of M thus defined, M⊥ ,
1 − Mω O
is therefore unique. The projector M projects vectors T⊥ ≡ 1 − T = , (19)
of <3 onto the subspace M, while the orthogonal pro- O 1 − Mv
where Mω and Mv are projectors of <3 defined in
eqs.(14) and (15) which allow the projection of the an- Algorithm I: Twist-Decomposition Algorithm
gular and translational velocity vectors, respectively. 1 θ ←− initial joint position;
It is noteworthy that the matrices of eq.(17) do not ver-
ify the properties of projectors, and hence, can not be 2 {pd , Qd } ←− desired EE pose;
used for orthogonal decomposition. Finally, eq.(16) 3 {p, Q} ⇐ DK(θ)
becomes
4 ∆Q ⇐ QT Qd
t = tT + tT ⊥ = Tt + (1 − T)t. (20)
5 ∆p ⇐ pd − p
Qvect(∆Q)
3.3. Functional-Redundancy Resolution 6 ∆t ←
∆p
For functionally-redundant serial manipulators, it
is possible to decompose the twist of the EE into two 7 DK(θ) ⇒ e ⇒ Mω , f ⇒ Mv , J
orthogonal parts, one lying into the task subspace and Mω O
another one lying into the redundant subspace. Sub- 8 T⇐
,
stituting eq.(20) into eq.(8) yields O Mv
9 ∆θ ⇐ J† T∆t + J† (1 − T)Jh
∆θ = (J† T)∆t + J† (1 − T)Jh , (21)
| {z } | {z } 10 if k∆θk < ² then stop;
task displacement redundant displacement
else
where h is an arbitrary vector of J allowing to satisfy
a secondary task, and is often chosen as the gradient 11 θ ⇐ θ + ∆θ, and go to 3.
of a performance criterion function to be minimized.
The first part of the RHS of eq.(21) is the joint 4. Performance Criterions
displacement required by the task, while the second
part is the joint displacement in the redundant sub- 4.1. Joint-limits Avoidance
space (or irrelevant to the task). Clearly, eq.(21) does
not require the projection onto the null-space of J While performing the joint-limits avoidance as the
as most of the redundancy-resolution algorithms do, secondary task, we would like to keep the manipulator
but rather requires an orthogonal projection based on as far as possible from its joint limits. Thus, in the
the instantaneous geometry of the task to be accom- case of avoidance of the joint-limits (Baron, 2000;
plished. As shown in Algorithm I, eq.(21) is used Huo and Baron, 2005), the performance criterion can
within a resolved-motion rate method. At lines 1-3, be written as to maintain the manipulator as close as
the joint position θ and the desired EE pose {pd , Qd } possible to the mid-joint position θ̄, i.e.,
are first initialized, then the actual EE pose {p, Q} is
computed with the direct kinematic model DK(θ). 1 min
At lines 4-6, an EE displacement ∆t is computed zjoint = (θ − θ̄)T WT W(θ − θ̄) → , (22)
2 θ
from the difference between the desired and actual
EE poses. The vect( · ) at line 6 is the function trans- with θ̄ and W being defined as
forming a 3 × 3 rotation matrix into an axial vector as
defined in Angeles (2003) (page 34). At lines 7-8, the 1
θ̄ ≡ (θ max + θ min ), W ≡ Diag(w). (23)
instantaneous orthogonal twist projector T is com- 2
puted from DK(θ). At line 9, the orthogonal decom- The setting of the weighting vector w of eq.(23) is
position method is used to compute the correspond- very important for the success. If w is too small, the
ing joint displacement ∆θ. Finally, the algorithm is redundant displacement may not be sufficient to avoid
stop whenever the norm of ∆θ is smaller than a cer- the joint-limits; if w is too large, the redundant dis-
tain threshold ². placement may produce high joint velocities. There-
fore, w is usually set based on trial and error.
4.2. Kinematic Singularity Avoidance
σ1 u1
task, the twist projectors are defined as unit degree mm mm degree degree degree
Table 1
DH parameters of Fanuc M-710ic/50
(1 − eeT ) 0 eeT 0
Tweld ≡
, T⊥
weld ≡
.(31)
0 1 0 0 path Λ is the intersection curve of the two cylinders,
Now, substituting eq.(31) into eq.(21) yields as shown in Figure 3. The length of Λ is 1829mm.
The EE must perform the welding operation con-
T secutively along the path Λ, with the speed about
ee Ah
∆θ = J† Tweld ∆t + J† , (32) 75mm/min. The welding tool symmetry axis must
0 always be directed toward point P , which locates at
where A is the upper three rows of J as defined in [ 0 0 0 ] in the part frame, the intersection point of the
eq.(6). Equation (32) can be used as line 9 of Algo- axes of the two cylinders. The transformation matrix
rithm I in order to solve the inverse kinematics of from the robot base frame to the part frame is
5.2. Test 1: Joint-limits Avoidance
meter
The secondary task h is chosen to avoid the joint-
limits such that
h = −∇zjoint = Diag(wini )(θ̄ − θ), (34)
where θ̄ is the mid-joint position of the robotic ma-
nipulator, and
· ¸
wini = 0.01 0.01 0.01 0.01 0.01 0.01 . (35)
meter meter
Equation(34) is used as the secondary task to avoid
the joint-limits. Figure 5 shows the joint positions for
Figure 3. The welding path in the part frame. two consecutive turns as computed by the TWA, i.e.,
using eq.(21) at line 9 of Algorithm I. The trajecto-
ries of each joint are within its joint limits. The max-
1 0 0 1150 imum rotation velocity of fourth and sixth joints are
74.5 deg./s and 56.9 deg./s.
0 1 0 200
T= . (33) Although the TWA, with only joint limits avoid-
0 0 1 −200 ance, can reach a solution within the joint limits. The
robot is close singularity at the instants 61 and 194
000 1 second, where the corresponding singularity perfor-
mance indices are
Figure 4 shows the joint positions to perform twice
the trajectory Λ as computed by the resolved-motion ωcond = 163, ωmom = 0.0066, ωps = 157. (36)
rate method without considering the functional re-
Clearly, these indices are very close to a singularity.
dundancy, i.e., using eq.(8) at line 9 of Algorithm I.
Thus, a singularity avoidance strategy becomes nec-
Apparently, without taking advantage of the redun-
essary in addition to the joint-limits avoidance for
dant axis, the sixth joint goes out of its joint limit, the
TWA.
manipulator is unable to perform this solution. Thus,
an optimization is needed to avoid the joint-limits. 5.3. Test 2: Joint-limits and Kinematic Singularity Avoidance
¾ 1st turn -¾ 2nd turn -
5 We can combine the two strategies as
θ4
θ5 min
0
θ2 z = zjoint + zsing → , (37)
θ1 θ
θ3
θ
(rad.) −5 and hence, the solution can be obtained as
h = −(∇zjoint + ∇zsing )
−10
4 θ4 4 θ4
2 θ5 2 θ5
θ2 θ2
θ θ
(rad.) 0 θ1 (rad.) 0 θ1
θ3 θ3 θ
−2 −2 6
θ6
−4 −4
−6 −6
0 50 100 150 200 250 300 0 50 100 150 200 250 300
¾ 1st turn -¾ 2nd turn - of the increasing of the distance from singularities,
6
the maximum rotation velocity of the fourth joint is
4 θ4 lower to 22 deg./s.
by eq.(34). The joint trajectory with TWA considering The resulting joint trajectory with the adapted weight-
both joint-limits and singularity avoidance is shown ing vectors is shown as Figure 7, where the motion
in Figure 6, where the joint motion range is smaller range of sixth joint is only 33% of the one in Test II.
than the result reached in Test I. The closest config- The closest configuration to singularity appears at in-
uration to singularity appears at instants 64 and 197 stant 61 and 194 second, with the singularity indices
second, with the singularity indices of of
ωcond = 56, ωmom = 0.03, ωps = 43.2. (40) ωcond = 31, ωmom = 0.076, ωps = 20.2. (43)
By comparing Test I and Test II, the maximum These singularity indices are enhanced almost 100%
value of the condition number ωcond is decreased as comparing to Test II.
from 163 to 56, i.e., the shape of the manipulabil- Among all the three tests above, the orientation and
ity ellipsoid becomes more spherical. The minimum position errors between the reached and desired path
value of ωmom increases from 0.0066 to 0.03, i.e., in task space are studied in order to verify the solution
the volume of the manipulability ellipsoid increase. accuracy. All these three tests can reach same level
The maximum value of ωps decreases from 156 to of position accuracy, i.e., the maximum position er-
43, i.e., the EE move faster in the direction of the mi- ror was 0.0024mm. Relating to the orientation, Tests
nor axis of the manipulability ellipsoid. As a result I and II can reach the same level accuracy, i.e., the
Test Motion Max. Min. Max. Max. J6 Max. J4 Error Error References
range ωcond ωmom ωps velocity velocity orien. posi.
I −5.3 ∼ 4.47 163 0.0066 157 57 74 0.0013 0.0024 Angeles, J., Habib, M. and Lopez-Cajun, C.S. (1987),
II −4.6 ∼ 4.17 56 0.03 43.2 56 22 0.0013 0.0024
“Efficient Algorithms for the Kinematic Inver-
sion of redundant Robot Manipulators”, Int. J. of
III −1.52 ∼ 4.17 31 0.076 20.2 37 26 0.00077 0.0024
Robotics and Automation, vol. 2, no. 3, pp. 106-
unit radian deg./s deg./s radian mm
Table 2
115.
Tests comparison, where ωcond is condition number of J, ωmom Angeles, J. (2003), Fundamental of Robotic Mechan-
is the manipulability measure. ical Systems: Theory, Methods, and Algorithms,
maximum norm of orientation error is 0.00128rad. second edition, Springer-Verlag, New York.
Test III can reach higher orientation accuracy, its ori- Arenson, N., Angles, J. and Slutski, L. (1998),
entation error is 0.00077rad. “Redundancy-resolution algorithms for isotropic
All these comparison are summarized in Table 2. robots”, Advances in Robot Kinematics: Analysis
Obviously, with applying our criterion of eq.(38) in and Control, pp. 425-434, Salzburg.
Test II, the distance from singularities is increased Baron, L. (2000), “A joint-limits avoidance strategy
while minimizing the the joint motion range; and with for arc-welding robots”, Int. Conf. on Integrated
adapting the weighting vectors in Test III, a much Design and Manufacturing in Mech. Eng., Mon-
better solution can be reached. treal, Canada, May 16-19.
Bedrossian, N. (1990), “Classification of singular
6. Conclusions configurations for redundant manipulators”, IEEE
International Conference on Robotics and Automa-
The inverse kinematic solutions that are able to take tion, pp. 818-823.
advantage of the functional redundancy of the weld- Chaumette, F., Marchand, É. (2001), “A redundancy-
ing operation are proposed and studied in this paper. based iterative approach for avoiding joint limits:
When a welding task is performed with a 6-axis ma- application to visual servoing”, IEEE Trans. on
nipulator, the so-called functional redundancy occurs. robotics and automation, vol. 17, no. 5, pp. 719-
With the help of this redundancy, it is possible to op- 730.
timize joint trajectory relative to various performance Huo, L., and Baron, L. (2005), “Kinematic inversion
criterions. of functionally-redundant serial manipulators: ap-
In order to keep the robot configuration as far as plication to arc-welding”, Trans. of the Canadian
possible from singularities, the condition number Society for Mech. Eng., vol. 29, no. 4, pp. 679-690.
ωcond and manipulability ωmom are not only used, but Klein, C.A. and Huang, C.-H. (1983), “Review of
also combined to generate a new kinetostatic perfor- pseudoinverse control for use with kinematically
mance index ωps which evaluates both the shape and redundant manipulators”, IEEE Trans. on Systems,
volume of the manipulability ellipsoid. Man, and Cyb., vol. SMC-13, no. 3, pp. 245-250.
The welding example compares the optimization Liégeois, A. (1977), “Automatic supervisory con-
results with three tests. The proposed twist decompo- trol of the configuration and behavior of multibody
sition approach, i.e., TWA, with joint-limits and sin- mechanisms”, IEEE Trans. on System, Man, Cy-
gularity avoidance as the secondary task is able to bern., vol. SMC-7, no. 12, pp. 245-250.
reach the best solution with this trajectory. Hence, the Marani, G., et al. (2002), “A real-time approach for
twist decomposition approach has proven to be able singularity avoidance in resolved motion reate con-
to optimize the joint space trajectory with the differ- trol of robotic manipualtors”, IEEE International
ent performance criterions. Conference on Robotics and Automation, pp. 1973-
1978.
Acknowledgments Nakamura, Y., Hanafusa, H. and Yoshikawa, T.
(1987), “Task-priority based redundancy control
The financial support from NSERC Industrial Post- of robot manipulators”, Int. J. Robotics Research,
graduate Scholarship and from Jabez Technologies Vol. 6, No. 2.
Inc. are gracefully acknowledged.
Salisbury, J. K. and Craig, J. J. (1982), “Articulated
hands, force and kinematic issues”, The Int. J.
Robotics Res. 1, no.1, pp. 4-17.
Samson, C., Le Borgne, M. and Espiau, B. (1991),
Robot control: the task function approach, Oxford
[Angleterre]: Clarendon Press.
Sciavicco, L. and Siciliano, B. (2000), Modelling and
control of robot manipulators, Springer, London.
Siciliano, B. (1992), “Solving manipulator redun-
dancy with the augmented task space method using
the constraint Jacobian transpose”, IEEE Int. Conf.
on Robotics and Automation, vol. 5, pp. 1-8.
Whitney, D.E. (1969), “Resolved motion rate con-
trol of manipulators and human prostheses”, IEEE
Trans. Man-Machine Syst., vol. 10, no. 2, pp. 47-53.
Whitney, D.E. (1972), “The mathematics of coor-
dinated control of prosthetic arms and manipula-
tor”, ASME J. Dynamics Syst., Measur. and Cont.,
vol. 94, no. 4, pp. 303-309.
Yoshikawa, T. (1984), “Analyais and control of
robot manipulators with redundancy”, Robotics
Research: The First International Symposium,
pp. 735-747.
Yoshikawa, T. (1990), Foundations of robotics: anal-
ysis and control, The MIT press.
Yoshikawa, T. (1996), “Basic optimization meth-
ods of redundant manipulators”, Lab. Robotics and
Aut., vol. 8, No. 1, pp. 49-60.