- Email: [email protected]
A Solution to the Singular Inverse Kinematic Problem for a Planar Manipulation Robot Mounted on a Track
Copyright @ IFAC Space Robotics, St-Hubert, Quebec, Canada, 1998
A SOLUTION TO THE SINGULAR INVERSE KINEMATIC PROBLEM FOR A PLANAR MANIPULATION ROBOT MOUNTED ON A TRACK Robert Muszynski' Krzysztof Tchon •
* Institute of Engineering Cybernetics, Wroclaw University of Technology, ul. Janiszewskiego 11/17, 50-372 Wroclaw, Poland
Abstract: Using the normal form approach we have solved the singular inverse kinematic problem for the AS EA IRb-6 manipulator mounted on a track. It is established that in a neighbourhood of singular configurations the kinematics of this manipulator assume the hyperbolic normal form. Equivalence transformations of the kinematics into the normal form are found. A normal-form based inversion algorithm of singular kinematics is presented. This algorithm is illustrated with computer simulations that reveal excellent quality of the normal form solution. Although the approach has been illustrated on an exemplary robot it applies to a general manipulation robot mounted on a track. Copyright © 1998 IFAC Keywords: Planar manipulator, kinematics, singularities, singular inverse kinematic problem, normal form approach.
1. INTRODUCTION
kinematic algorithms including the generalized Newton algorithm (Tchon and Dul~ba, 1993) and the adjoint Jacobian approach (Tsumaki et al., 1997; Nenchev and Uchiyama, 1997), as well as path tracking algorithms able to track a singular path in the taskspace (Kieffer, 1992; Kieffer, 1994; Nenchev, 1995; Shankar and Saraf, 1995). A general disadvantage of these algorithms is that they are unable to drive the manipulator through singular configurations with prescribed velocity. The fourth approach that can be referred to as the normal form approach (Tchon and Muszynski, 1998; Muszynski, 1996; Muszynski and Tchon, 1996; Tchon and Muszynski, 1997), is based on establishing an equivalence between the original singular inverse kinematic problem and a similar problem addressed in a simple mathematical model of the singular kinematics called a normal form of the kinematics.
In recent years considerable research effort has been undertaken toward a better understanding of the singular inverse kinematic problem for robotic manipulators. Several approaches to this problem can be detected in the robotic literature; most of them take as a point of departure the resolved motion rate control introduced in (Whitney, 1972), and attempt at adjusting this method to the case of the manipulator kinematics being singular. Let us briefly mention here four distinctive approaches. The first approach appeals to the paradigm of singularity avoidance available in redundant kinematics (Shamir, 1990; Tchon and Matuszok, 1995; Seng et al., 1997). The second approach provides an approximate solution of the singular inverse kinematic problem based on the damped least squares method . The SingularityRobust Inverse is a typical tool elaborated within this approach (Nakamura, 1991) . The third approach invokes existing mathematical methods of solving a set of singular non-linear equations. It has resulted in a number of singular inverse
In this paper we shall apply the normal form approach to a singular inverse kinematic problem encountered in the ASEA IRb-6 manipulator mounted on a track. It should be noticed that in
139
-v
most cases of planar manipulators mounted on a track the similar problem arises. It will be argued that an inversion algorithm designed within the normal form approach is able to ensure excellent performance of the manipulator when driven through and around singular configurations. The inversion algorithm consists of the following steps (Tchon and Muszynski, 1998; Muszynski, 1996):
a,
\
ok
= ( oq (qd)
----
Fig. 1. The IRb-6 manipulator mounted on a track.
)-1
Yd(t)
Table 1. Denavit-Hartenberg parameters of the IRb-6 manipulator mounted on a track.
initialized at qd(O) such that Yd(O) = k(qd(O)) . (2) If at a certain t = to the above system becomes ill-defined, deduce that the jointspace trajectory has entered a neighbourhood 1U of a singular configuration qo . (3) Determine a normal form ko of the kinematics k and find equivalence transformations
d.
-
(1) Given a coordinate expression for the kinematics k : ]Rn --+ ]Rn, Y = k(q), and a desirable taskspace trajectory Yd(t) , t E IT, begin with applying the resolved motion rate control, i.e. with solving the system (id
\\~ , q.
joint
(} i
di
ai
Cl i
0 0
90° 90° 0 0 90° -90°
1
0
ql
2 3
q2
dl
q3
4
q4 - q3
5
qs - q4
0 0 0
6
q6
d6
a2 a3
0 0
IRb-6 manipulator'S kinematics mounted on a track. It is proved that this normal form is hyperbolic (Tchon and Muszynski, 1997; Tchon et al., 1997). Both the normal form as well as the equivalence transformations are further employed in the inversion algorithm of singular ASEA IRb6 kinematics. Results of computer experiments are reported in section 3. Section 4 concludes the paper.
1/1 0 k 0
(4) Convert the desirable taskspace trajectory Yd(t) E V to new coordinates by defining 1)d(t) = 1/1(Yd(t)) .
(5) Solve the inverse kinematic problem for ko, 1)d(t) , i.e. find ~d(t) E
2. THE NORMAL FORM The ASEA IRb-6 manipulator mounted on a track is a 6 d.o.f., 1T5R-type manipulator whose kinematic chain is shown in Figure 1. The DenavitHartenberg parameters of this manipulator are collect.ed in Table 1, bearing the following meaning: d i , ai denote offsets along z and x-axis, respectively, ()i is the ith joint revolut.ion angle, ai stands for the z-axis misalignment between coordinate systems number i-I and i. Numerical values of the offset parameters amount to d 1 = 0.7[m], a2 = 0.45[m], a3 = 0.67[m] and d 6 = 0.095[m]. The motion of the joints is limited to an open subset Q C ]R6 defined as follows (q1 is measured in [m], q2 -7 q6 in [0]):
(6) Transform the solution ~d(t) back to the original jointspace coordinates by setting qd(t) =
(7) By gluing together regular (step 1) and singular (step 6) pieces of the jointspace trajectory, construct a desirable trajectory qd(t), t E IT . Undoubtedly, crucial steps of this algorithm consist in determining a normal form of the singular kinematics and in computing the equivalence transformations. Both these steps will be accomplished in the sequel, and the above algorithm will be implemented to produce a singular jointspace trajectory corresponding to an exemplary prescribed trajectory in the taskspace of the ASEA IRb-6 manipulator mounted on a track. The quality of the solution will be tested in computer sim ulations.
Q = {(q1,'" ,q6) E ]R61 0 < q1 < 1.5, -170 < q2 < 170, 50 < q3 < 130, -40 < q4 < 25, -90 < q5 < 90, (1) - 180 < q6 < 180, 65 < q3 - q4 < 130} .
This paper is composed as follows . Section 2 presents an explicit derivation of the normal form and of equivalence transformations for the ASEA
The forward kinematics of the ASEA IRb-6 manipulator can be represented by a 4 x 4 matrix 140
-r
how much the rank of the Jacobian matrix of the kinematics at this configuration differs from its maximum value. It is easy to prove (Tchon et al., 1997) that at any qo E § corank(qo) = 1, i.e. rank ~~ (qo) = 5. After these preparations we are ready to set about establishing a normal form of the kinematics (3) according to a strategy recommended in (Tchon and Muszynski, 1998). First of all, we need to reorder jointspace coordinates and shift taskspace coordinates to render the Jacobian matrix a suitable structure. This will be accomplished by substitutions
z
Fig. 2. The IRb-6 manipulator at qo E §.
x = (Xl , X2,' " , X6) = (q3, q2, ql,q4, qs, q6),
k(q) =
Y = (YI, Y2,·· . , Y6) = (YI, Y2, Y3, Y4 + 90 , Ys, Y6), (7)
C2CSC6 + 8286 -C28S 82C6 - C2CS86 -C68S -cs 8586 82CSC6 C286 -828S C2C6 - 82 C586 [ 000 C2(a2 C3 + a3 C4 + d6 8S) -dl + d 6cs - a283 - a3 84 ql +a2 82C3 +~382C4 +d68285
therefore the kinematics (3) are now expressed as
Y=
1 E
(2)
-d 1
SE(3),
X3
+ a3C4 + d68S)' + d6Cs - a2 8 1 - a3 84, + a28Zcl + a382C4 + d6828S,
(8) 90 - Xz, 180 - Xs, 180 - X6) .
It is easily checked that in new coordinates the Jacobian matrix of the kinematics (8) 8k (x) 8x
After introducing into the special Euclidean group SE(3) a coordinate system comprising Cartesian coordinates and YZY Euler angles, we obtain the following convenient coordinate expression for the kinematics (2)
=
-a2 81 c2 -82(a2cl
+ a3C4 + d68S)
-a2cI -a28182 -c2(a2 c I
+ a3 C4 + d6 8 S)
o
o o
o
= (c2(a2c3 + a3C4 + d6 8S) , + d 6cs
(9)
0 -a3c284 d 6C2CS 0 -a3 C4 -d68S 1 -a38284 d682CS 0 0 0 -1 0 0 0 0 0
- a283 - a3 84, (3) -q2, 180 - qs, 180 - q6)'
Now, in order to determine the locus of singular configurations of the kinematics (3), we compute the determinant of the Jacobian matrix of (3) 8k
det 8q (q) = -a2 a 3C283-4,
QI
q2 = -90, 90}.
0 0 0 0 0 -1
contains a lower-right 5 x 5 submatrix that remains full rank at all singular configurations. Subject to the coordinate reordering (6) the feasible set Q changes to
(4)
where 83-4 = sin(q3 - q4)' Due to the limitations imposed on feasible configurations of the ASEA IRb-6 manipulator the singularity locus § = {q E
0 -1
o
-d l
=
k(x) = (k l (x),k 2 (x), . .. ,k6(X))
(c2(a2 c l
where, for the sake of conciseness, the standard abbreviations 8i = sin qi and Ci = cos qi have been employed.
Y = k(q)
(6)
< 130, -170 < X2 < 170, 0 < X3 < 1.5, -40 < X4 < 25, -90 < Xs < 90, (10) - 180 < X6 < 180, 65 < Xl - X4 < 130} ,
X = { (Xl, . . . , X6) E IR61 50
(5)
An exemplary singular configuration of the manipulator is displayed in Figure 2. It is easily observed that at this configuration the motions of revolute joints 3, 4, 5 take place in yz plane and have a component collinear with the translation of the joint 1.
<
Xl
whereas the singularity locus is equal to §
= {x E XI
X2
= -90,
90}.
(11)
The structure of the Jacobian matrix (9) suggests that k 2 , . . . , k6 components of the kinematics (8) are independent and can be included into a new coordinate change in the jointspace, that will convert the kinematics to the so-called prenormal
The degree of degeneracy of a singular configuration can be measured by its corank that indicates 141
form (Tchon and Muszynski, 1998). Indeed, the following map is a well defined change of coordinates ~ =
in particular in §I, g~~ f:. O. This being so, the prenormal form (14)-(15) can be simplified considerably by employing the following coordinate change in the jointspace
X3 + a2ClS2 + a3C4S2 + d6s2S5, 90 - X2, 180 - X5, 180 - X6), (12)
(19)
where, to fix attention, we have confined to Xl = {X E XI - 90 < X2 < 170}, so that singular configurations correspond to X2 = 90. The remaining case of X2 = -90 can be treated analogously and will be omitted. It is immediately proved that the map (12) is globally invertible on
where
)2 ? 2 U3 = Si~2 ~2 + (d l + d 6 cos (5 + (4 + a 2 - a 3· It may be checked that (20) is well defined. Expressed in new coordinates, the prenormal form becomes
x1=6, x2=90-~4, x5=180-6, X6 = 180 - ~6, a3 sinx4 = -d l - d 6 COS~5a2 sin 6 -6, X3 =6 -cos~4(a2cos6+ (13) a3 cos X4 + d 6 sin 6)·
y = m(() = l ((1 (2
0
(r(O, 6, ... ,~6), (14)
where r(~)
= kl
((a~
0
a2cos6
the kinematics (3) are transformed to the hyperbolic normal form TJ
4
~4=O
=
~
au (0 =
(17)
E 21 the func-
sin~4 ~4
(dl + d 6 cos 6 + a2 sin6 + 6)a2 COS~\ ( (a~ - (d 1 + d~ cos ~5 + a2 sin 6 + 6) 2) 2"
. c) a2 s111 <'1
=
sin~4 a2 sin(x4 --. ~4
cos X4
xd
= O.
The normal form (24) has been discovered in (Tchon et al., 1997) using general methods of singularity theory. An advantage of a more specific derivation presented above is that all the coordinate transformations involved have been constructed explicitly and defined globally what allows to solve the inverse kinematic problem by applying only steps 3-6 of the inversion algorithm included in section 1. In the next section we shall use these coordinate transformations in order to solve the singular inverse kinematic problem for the ASEA IRb-6 manipulator mounted on a track, for an exemplary singular trajectory specified in the taskspace.
tion u(O is well defined. Next, we shall compute the partial derivative
a6
((1(2, (2, (3, (4, (5, (6),
that becomes singular at (2
~4U(~) + w(~4,6),
= 1. Clearly, for
= h(() = 'ljJ 0 m(() =
(24)
Having determined the prenormal form (14)-(15) of the kinematics, we want to simplify the function r(O as far as possible. To this aim observe that r(~) can be rewritten as
r(O = ~4 sin~4 ~4 1 ((a~ - (d 1 + d6 COS~5 + a2 sin6 + 6)2) 2" +
Ys, Y6), (23)
(16)
where Si~ $41
=
+ W((2, (5), (4, (3, (2, (5, (6~21)
TJ = 'ljJ(f}) = (f}l - w(f}4, Y5), Y4, Y3, Y2,
+d6sin~5) P5)
In new coordinates the feasible set of joint configurations 21 =
+ d6 sin~4 sin6
a-:- 1 (()
Eventually, by performing a coordinate change in the taskspace, specified as
= sin~4
- (d 1 + d6 cos 6 + a2 sin 6 + 6)2)! +
a2 cos 6 )
0
along with w defined by (17). New feasible set of joint positions 2,1 = a-:(2d; the singularity locus is equal to
Subordinated to the coordinate change (12) the kinematics (8) assume the prenormal form
y = l(O = k
= a2(d1 + d6 cos (5 + (4), U2 = a2s~~(t2'
Ul
(2(2
3. SIMULATIONS (18)
To solve the singular inverse kinematic problem with the use of the normal form approach one has to transform a desirable taskspace trajectory Yd(t) to the normal form external coordinates TJ (step 4 of the algorithm), solve the inverse problem for the normal form (step 5 of the algorithm) and
,
the last equality resulting from (13). Now it is easily deduced from (10) that everywhere in 2 1 , 142
finally transform the obtained solution back to the original jointspace coordinates q (step 6 of the algorithm) . In our case in order to transform a desirable taskspace trajectory Yd(t) to the normal form external coordinates we shall apply formulas (7) and (23) and find the desirable trajectory 'TJd(t) for the hyperbolic normal form (24). Then we can solve the singular inverse problem for the normal form (24) by computing
, (d6(t)) = t ) 'TJdd ) ( 'TJd2(t),'TJd2(t),oo. ,'TJd6(t) .
«(d1(t), (d2(t),
a) q2 (01
q1 [ml
160 140 1 20 100 80 60 40
0.4
t (5]
q3 ( 01
q4 (0]
76
00'
-19.5
74
(25)
72 70
It is clear that, if for a time instant to the second desirable trajectory component 'TJd2 (to) becomes equal to zero, the manipulator enters a singular configuration from the set § 1. It follows that for desirable taskspace trajectories that can be produced by bounded jointspace trajectories the condition 'TJd1 (to) = has necessarily to be fulfilled. Translated to the original tasks pace coordinates, this condition is expressed as
b) q2' (0/5 )
q l ' fm/s I
°
0. 4
- 15
0.2
-20
6 t 151 -0.2
-25 - 30
-0 .4
(26)
+--""""----'---=-6 t
q4' (o/s)
q3' [ 0151
hence easily checkable. Having found the solution to the inverse problem for the normal form , we are in a position to transform this solution to the original coordinate system q. To this aim one needs to apply transformations (20), (13) and make variables substitutions according to (6). After this step a jointspace trajectory qd(t) is produced which corresponds to the desirable tasks pace trajectory Yd(t) .
(s]
0.5 6 t (5] -2
-0.5
-4
-6
-1
c) q1"
(m/s'21
q2"
0.25
Now, let us apply the above procedure to solving an exemplary singular inverse kinematic problem. Assume that the manipulator 's end effector has to follow a trajectory in the taskspace described by the function
( 015 '21
0. 4 0.2 5
6 t (5 I
- 0.2 2
3
4
5
6 t(s]
-0. 4
q4' , (0/s"2)
Yd(t) = (0.7sin(0.5t-1.5),0.8 1.5 - 0.4 cos(0.5t - 1.5), -165 + 25t , 180, 180) (27)
+--:--'20---::3 --:4:---;:---/; t
+--'-~==--"-=5 --:-6 t
defined for t E IT = [O,6][8J. This trajectory forms a segment of ellipse in Y1Y3 plane in the manipulator'S taskspace, crossing transversally the track. It is easily observed that at time to = 3[8J the manipulator enters the singular configuration q2 = 90 . Furthermore for to the equality (26) is fulfilled, so we can expect that there exists a bounded jointspace solution to the singular inverse kinematic problem. After applying transformations (7), (23), (25), (20), (13) and making the substitution (6), we obtain two jointspace trajectories (due to a plus/minus sign in the expression (20), but only one of them, depicted in Figure 3, lies within the joint motion limits (1). The joint variables q5 and q6 which are not shown in Figure 3 are constantly equal to zero during the
(5]
-
(5
0.5
Fig. 3. Desirable a) position, b) velocity and c) acceleration trajectories in the jointspace obtained with the normal form approach. manipulator motion. Figure 4 shows the taskspace path corresponding to the desirable trajectory, together with recorded sequence of the manipulator's postures.
4. CONCLUSIONS This paper has demonstrated an applicability of the normal form approach to solving a specific singular inverse kinematic problem arising in the 143
I
Kieffer, J. (1994) . Differential analysis of bifurcations and isolated singularities for robots and mechanisms. IEEE 1'rans. Robotics Automat. 10(1), 1- 10. Muszynski, R. (1996). Mathematical Models and Control Algorithms for Robotic Manipulators with Kinematic Singularities. PhD thesis. Institute of Engineering Cybernetics, Technical University of Wrodaw. (in Polish) . Muszynski, R. and K Tchon (1996). Normal forms of non- redundant singular robot kinematics: Three DOF worked examples. J. Robotic Syst. 13(12), 765- 79l. Nakamura, Y. (1991). Advanced Robotics: Redundancy and Optimization. Addison-Wesley. Nenchev, D. N. (1995). Tracking manipulator trajectories with ordinary singularities: A null space-based approach. Int. J. Robotics Research 14(4) , 399-404. Nenchev, D. N. and M. Uchiyama (1997) . Singularity-consistent path planning and motion control through instantaneous selfmotion singularities of parallel-link manipulators. J. Robotics Syst. 14(13), 27-36. Seng, J., K A. O'Neil and Y. C. Chen (1997). On the existence and the manipulability recovery rate of self-motion at manipulator singularities. Int. J. Robotics Research 16(2), 171-184. Shamir, T. (1990). The singularities of redundant robot arm . Int. J. Robotics Research 9(1), 113-12l. Shankar, S. and A. Saraf (1995). Singularities in mechanisms 1. M echo Machine Theory 30, 1139- 1148. Tchon, K and A. Matuszok (1995). On avoiding singularities in redundant robot kinematics. Robotica 13, 599- 606. Tchon, K and 1. Dul~ba (1993) . On inverting singular kinematics and geodesic trajectory generation for robot manipulators. J. Intelligent Robotic Syst. 8, 325-359. Tchon, K and R. Muszynski (1997). Singularities of non- redundant robot kinematics . Int. J. Robotics Research 16(1) , 60- 76. Tchon, K and R. Muszynski (1998). Singular inverse kinematic problem for robotic manipulators: A normal form approach. IEEE Trans. Robotics Automat. Tchon, K, R . Muszynski and C. Zielinski (1997). Kinematic singularities of the IRp-6 manipulator mounted on a track. In: Proc. MMAR '97 Symposium. Vol. 3 . Mi~dzyzdroje , Poland. pp. 913-920. Tsumaki, Y. et al. (1997) . Teleoperation based on adjoint Jacobian approach. Contr. Systems 17(1),53-62. Whitney, D. E . (1972). The mathematics of coordinated control of prosthetic arms and manipulators. J. Dyn, Syst. Meas. Contr. 94 G(4), 303-309.
Fig. 4. A sequence of the manipulator's postures along the desired trajectory. control of the ASEA IRb-6 manipulator mounted on a track. The problem has been solved in accordance with the normal form-based inversion algorithm. This approach can be applied to all planar manipulation robots mounted on a track for which the considered singular inverse kinematic problem arises. It has been proved that singular configurations of the IRb-6 manipulator have corank 1 and can be given the hyperbolic normal form . A remarkable feature of the IRb6 kinematics is that equivalence transformations establishing equivalence of the original kinematics to the normal form can be defined explicitly and globally, and so can they be inverted. On exploiting this feature efficient computations of the jointspace trajectory have been performed and applied to an exemplary desirable taskspace trajectory meeting transversally the track of the manipulator. Computer simulations have indicated exquisite quality of the singular inverse problem solution produced by the normal form approach. These indications have been confirmed in a series of experiments conducted on the ASEA IRb-6 manipulator that will be reported in a separate paper. The performed experiments have also shown a difficulty concerned with the taskspace trajectory selection for which there exists a bounded jointspace trajectory in the presence of singular configurations. This problem can be overcome by reducing the tasks pace dimensions by one and treating the manipulator as a redundant one. Details will be deferred to a separate paper. 5. REFERENCES Kieffer, J . (1992) . Manipulator inverse kinematics for untimed end-effector trajectories with ordinary singularities. Int. J. Robotics Research 11(3), 225- 237. 144
A SOLUTION TO THE SINGULAR INVERSE KINEMATIC PROBLEM FOR A PLANAR MANIPULATION ROBOT MOUNTED ON A TRACK Robert Muszynski' Krzysztof Tchon •
* Institute of Engineering Cybernetics, Wroclaw University of Technology, ul. Janiszewskiego 11/17, 50-372 Wroclaw, Poland
Abstract: Using the normal form approach we have solved the singular inverse kinematic problem for the AS EA IRb-6 manipulator mounted on a track. It is established that in a neighbourhood of singular configurations the kinematics of this manipulator assume the hyperbolic normal form. Equivalence transformations of the kinematics into the normal form are found. A normal-form based inversion algorithm of singular kinematics is presented. This algorithm is illustrated with computer simulations that reveal excellent quality of the normal form solution. Although the approach has been illustrated on an exemplary robot it applies to a general manipulation robot mounted on a track. Copyright © 1998 IFAC Keywords: Planar manipulator, kinematics, singularities, singular inverse kinematic problem, normal form approach.
1. INTRODUCTION
kinematic algorithms including the generalized Newton algorithm (Tchon and Dul~ba, 1993) and the adjoint Jacobian approach (Tsumaki et al., 1997; Nenchev and Uchiyama, 1997), as well as path tracking algorithms able to track a singular path in the taskspace (Kieffer, 1992; Kieffer, 1994; Nenchev, 1995; Shankar and Saraf, 1995). A general disadvantage of these algorithms is that they are unable to drive the manipulator through singular configurations with prescribed velocity. The fourth approach that can be referred to as the normal form approach (Tchon and Muszynski, 1998; Muszynski, 1996; Muszynski and Tchon, 1996; Tchon and Muszynski, 1997), is based on establishing an equivalence between the original singular inverse kinematic problem and a similar problem addressed in a simple mathematical model of the singular kinematics called a normal form of the kinematics.
In recent years considerable research effort has been undertaken toward a better understanding of the singular inverse kinematic problem for robotic manipulators. Several approaches to this problem can be detected in the robotic literature; most of them take as a point of departure the resolved motion rate control introduced in (Whitney, 1972), and attempt at adjusting this method to the case of the manipulator kinematics being singular. Let us briefly mention here four distinctive approaches. The first approach appeals to the paradigm of singularity avoidance available in redundant kinematics (Shamir, 1990; Tchon and Matuszok, 1995; Seng et al., 1997). The second approach provides an approximate solution of the singular inverse kinematic problem based on the damped least squares method . The SingularityRobust Inverse is a typical tool elaborated within this approach (Nakamura, 1991) . The third approach invokes existing mathematical methods of solving a set of singular non-linear equations. It has resulted in a number of singular inverse
In this paper we shall apply the normal form approach to a singular inverse kinematic problem encountered in the ASEA IRb-6 manipulator mounted on a track. It should be noticed that in
139
-v
most cases of planar manipulators mounted on a track the similar problem arises. It will be argued that an inversion algorithm designed within the normal form approach is able to ensure excellent performance of the manipulator when driven through and around singular configurations. The inversion algorithm consists of the following steps (Tchon and Muszynski, 1998; Muszynski, 1996):
a,
\
ok
= ( oq (qd)
----
Fig. 1. The IRb-6 manipulator mounted on a track.
)-1
Yd(t)
Table 1. Denavit-Hartenberg parameters of the IRb-6 manipulator mounted on a track.
initialized at qd(O) such that Yd(O) = k(qd(O)) . (2) If at a certain t = to the above system becomes ill-defined, deduce that the jointspace trajectory has entered a neighbourhood 1U of a singular configuration qo . (3) Determine a normal form ko of the kinematics k and find equivalence transformations
d.
-
(1) Given a coordinate expression for the kinematics k : ]Rn --+ ]Rn, Y = k(q), and a desirable taskspace trajectory Yd(t) , t E IT, begin with applying the resolved motion rate control, i.e. with solving the system (id
\\~ , q.
joint
(} i
di
ai
Cl i
0 0
90° 90° 0 0 90° -90°
1
0
ql
2 3
q2
dl
q3
4
q4 - q3
5
qs - q4
0 0 0
6
q6
d6
a2 a3
0 0
IRb-6 manipulator'S kinematics mounted on a track. It is proved that this normal form is hyperbolic (Tchon and Muszynski, 1997; Tchon et al., 1997). Both the normal form as well as the equivalence transformations are further employed in the inversion algorithm of singular ASEA IRb6 kinematics. Results of computer experiments are reported in section 3. Section 4 concludes the paper.
1/1 0 k 0
(4) Convert the desirable taskspace trajectory Yd(t) E V to new coordinates by defining 1)d(t) = 1/1(Yd(t)) .
(5) Solve the inverse kinematic problem for ko, 1)d(t) , i.e. find ~d(t) E
2. THE NORMAL FORM The ASEA IRb-6 manipulator mounted on a track is a 6 d.o.f., 1T5R-type manipulator whose kinematic chain is shown in Figure 1. The DenavitHartenberg parameters of this manipulator are collect.ed in Table 1, bearing the following meaning: d i , ai denote offsets along z and x-axis, respectively, ()i is the ith joint revolut.ion angle, ai stands for the z-axis misalignment between coordinate systems number i-I and i. Numerical values of the offset parameters amount to d 1 = 0.7[m], a2 = 0.45[m], a3 = 0.67[m] and d 6 = 0.095[m]. The motion of the joints is limited to an open subset Q C ]R6 defined as follows (q1 is measured in [m], q2 -7 q6 in [0]):
(6) Transform the solution ~d(t) back to the original jointspace coordinates by setting qd(t) =
(7) By gluing together regular (step 1) and singular (step 6) pieces of the jointspace trajectory, construct a desirable trajectory qd(t), t E IT . Undoubtedly, crucial steps of this algorithm consist in determining a normal form of the singular kinematics and in computing the equivalence transformations. Both these steps will be accomplished in the sequel, and the above algorithm will be implemented to produce a singular jointspace trajectory corresponding to an exemplary prescribed trajectory in the taskspace of the ASEA IRb-6 manipulator mounted on a track. The quality of the solution will be tested in computer sim ulations.
Q = {(q1,'" ,q6) E ]R61 0 < q1 < 1.5, -170 < q2 < 170, 50 < q3 < 130, -40 < q4 < 25, -90 < q5 < 90, (1) - 180 < q6 < 180, 65 < q3 - q4 < 130} .
This paper is composed as follows . Section 2 presents an explicit derivation of the normal form and of equivalence transformations for the ASEA
The forward kinematics of the ASEA IRb-6 manipulator can be represented by a 4 x 4 matrix 140
-r
how much the rank of the Jacobian matrix of the kinematics at this configuration differs from its maximum value. It is easy to prove (Tchon et al., 1997) that at any qo E § corank(qo) = 1, i.e. rank ~~ (qo) = 5. After these preparations we are ready to set about establishing a normal form of the kinematics (3) according to a strategy recommended in (Tchon and Muszynski, 1998). First of all, we need to reorder jointspace coordinates and shift taskspace coordinates to render the Jacobian matrix a suitable structure. This will be accomplished by substitutions
z
Fig. 2. The IRb-6 manipulator at qo E §.
x = (Xl , X2,' " , X6) = (q3, q2, ql,q4, qs, q6),
k(q) =
Y = (YI, Y2,·· . , Y6) = (YI, Y2, Y3, Y4 + 90 , Ys, Y6), (7)
C2CSC6 + 8286 -C28S 82C6 - C2CS86 -C68S -cs 8586 82CSC6 C286 -828S C2C6 - 82 C586 [ 000 C2(a2 C3 + a3 C4 + d6 8S) -dl + d 6cs - a283 - a3 84 ql +a2 82C3 +~382C4 +d68285
therefore the kinematics (3) are now expressed as
Y=
1 E
(2)
-d 1
SE(3),
X3
+ a3C4 + d68S)' + d6Cs - a2 8 1 - a3 84, + a28Zcl + a382C4 + d6828S,
(8) 90 - Xz, 180 - Xs, 180 - X6) .
It is easily checked that in new coordinates the Jacobian matrix of the kinematics (8) 8k (x) 8x
After introducing into the special Euclidean group SE(3) a coordinate system comprising Cartesian coordinates and YZY Euler angles, we obtain the following convenient coordinate expression for the kinematics (2)
=
-a2 81 c2 -82(a2cl
+ a3C4 + d68S)
-a2cI -a28182 -c2(a2 c I
+ a3 C4 + d6 8 S)
o
o o
o
= (c2(a2c3 + a3C4 + d6 8S) , + d 6cs
(9)
0 -a3c284 d 6C2CS 0 -a3 C4 -d68S 1 -a38284 d682CS 0 0 0 -1 0 0 0 0 0
- a283 - a3 84, (3) -q2, 180 - qs, 180 - q6)'
Now, in order to determine the locus of singular configurations of the kinematics (3), we compute the determinant of the Jacobian matrix of (3) 8k
det 8q (q) = -a2 a 3C283-4,
QI
q2 = -90, 90}.
0 0 0 0 0 -1
contains a lower-right 5 x 5 submatrix that remains full rank at all singular configurations. Subject to the coordinate reordering (6) the feasible set Q changes to
(4)
where 83-4 = sin(q3 - q4)' Due to the limitations imposed on feasible configurations of the ASEA IRb-6 manipulator the singularity locus § = {q E
0 -1
o
-d l
=
k(x) = (k l (x),k 2 (x), . .. ,k6(X))
(c2(a2 c l
where, for the sake of conciseness, the standard abbreviations 8i = sin qi and Ci = cos qi have been employed.
Y = k(q)
(6)
< 130, -170 < X2 < 170, 0 < X3 < 1.5, -40 < X4 < 25, -90 < Xs < 90, (10) - 180 < X6 < 180, 65 < Xl - X4 < 130} ,
X = { (Xl, . . . , X6) E IR61 50
(5)
An exemplary singular configuration of the manipulator is displayed in Figure 2. It is easily observed that at this configuration the motions of revolute joints 3, 4, 5 take place in yz plane and have a component collinear with the translation of the joint 1.
<
Xl
whereas the singularity locus is equal to §
= {x E XI
X2
= -90,
90}.
(11)
The structure of the Jacobian matrix (9) suggests that k 2 , . . . , k6 components of the kinematics (8) are independent and can be included into a new coordinate change in the jointspace, that will convert the kinematics to the so-called prenormal
The degree of degeneracy of a singular configuration can be measured by its corank that indicates 141
form (Tchon and Muszynski, 1998). Indeed, the following map is a well defined change of coordinates ~ =
in particular in §I, g~~ f:. O. This being so, the prenormal form (14)-(15) can be simplified considerably by employing the following coordinate change in the jointspace
X3 + a2ClS2 + a3C4S2 + d6s2S5, 90 - X2, 180 - X5, 180 - X6), (12)
(19)
where, to fix attention, we have confined to Xl = {X E XI - 90 < X2 < 170}, so that singular configurations correspond to X2 = 90. The remaining case of X2 = -90 can be treated analogously and will be omitted. It is immediately proved that the map (12) is globally invertible on
where
)2 ? 2 U3 = Si~2 ~2 + (d l + d 6 cos (5 + (4 + a 2 - a 3· It may be checked that (20) is well defined. Expressed in new coordinates, the prenormal form becomes
x1=6, x2=90-~4, x5=180-6, X6 = 180 - ~6, a3 sinx4 = -d l - d 6 COS~5a2 sin 6 -6, X3 =6 -cos~4(a2cos6+ (13) a3 cos X4 + d 6 sin 6)·
y = m(() = l ((1 (2
0
(r(O, 6, ... ,~6), (14)
where r(~)
= kl
((a~
0
a2cos6
the kinematics (3) are transformed to the hyperbolic normal form TJ
4
~4=O
=
~
au (0 =
(17)
E 21 the func-
sin~4 ~4
(dl + d 6 cos 6 + a2 sin6 + 6)a2 COS~\ ( (a~ - (d 1 + d~ cos ~5 + a2 sin 6 + 6) 2) 2"
. c) a2 s111 <'1
=
sin~4 a2 sin(x4 --. ~4
cos X4
xd
= O.
The normal form (24) has been discovered in (Tchon et al., 1997) using general methods of singularity theory. An advantage of a more specific derivation presented above is that all the coordinate transformations involved have been constructed explicitly and defined globally what allows to solve the inverse kinematic problem by applying only steps 3-6 of the inversion algorithm included in section 1. In the next section we shall use these coordinate transformations in order to solve the singular inverse kinematic problem for the ASEA IRb-6 manipulator mounted on a track, for an exemplary singular trajectory specified in the taskspace.
tion u(O is well defined. Next, we shall compute the partial derivative
a6
((1(2, (2, (3, (4, (5, (6),
that becomes singular at (2
~4U(~) + w(~4,6),
= 1. Clearly, for
= h(() = 'ljJ 0 m(() =
(24)
Having determined the prenormal form (14)-(15) of the kinematics, we want to simplify the function r(O as far as possible. To this aim observe that r(~) can be rewritten as
r(O = ~4 sin~4 ~4 1 ((a~ - (d 1 + d6 COS~5 + a2 sin6 + 6)2) 2" +
Ys, Y6), (23)
(16)
where Si~ $41
=
+ W((2, (5), (4, (3, (2, (5, (6~21)
TJ = 'ljJ(f}) = (f}l - w(f}4, Y5), Y4, Y3, Y2,
+d6sin~5) P5)
In new coordinates the feasible set of joint configurations 21 =
+ d6 sin~4 sin6
a-:- 1 (()
Eventually, by performing a coordinate change in the taskspace, specified as
= sin~4
- (d 1 + d6 cos 6 + a2 sin 6 + 6)2)! +
a2 cos 6 )
0
along with w defined by (17). New feasible set of joint positions 2,1 = a-:(2d; the singularity locus is equal to
Subordinated to the coordinate change (12) the kinematics (8) assume the prenormal form
y = l(O = k
= a2(d1 + d6 cos (5 + (4), U2 = a2s~~(t2'
Ul
(2(2
3. SIMULATIONS (18)
To solve the singular inverse kinematic problem with the use of the normal form approach one has to transform a desirable taskspace trajectory Yd(t) to the normal form external coordinates TJ (step 4 of the algorithm), solve the inverse problem for the normal form (step 5 of the algorithm) and
,
the last equality resulting from (13). Now it is easily deduced from (10) that everywhere in 2 1 , 142
finally transform the obtained solution back to the original jointspace coordinates q (step 6 of the algorithm) . In our case in order to transform a desirable taskspace trajectory Yd(t) to the normal form external coordinates we shall apply formulas (7) and (23) and find the desirable trajectory 'TJd(t) for the hyperbolic normal form (24). Then we can solve the singular inverse problem for the normal form (24) by computing
, (d6(t)) = t ) 'TJdd ) ( 'TJd2(t),'TJd2(t),oo. ,'TJd6(t) .
«(d1(t), (d2(t),
a) q2 (01
q1 [ml
160 140 1 20 100 80 60 40
0.4
t (5]
q3 ( 01
q4 (0]
76
00'
-19.5
74
(25)
72 70
It is clear that, if for a time instant to the second desirable trajectory component 'TJd2 (to) becomes equal to zero, the manipulator enters a singular configuration from the set § 1. It follows that for desirable taskspace trajectories that can be produced by bounded jointspace trajectories the condition 'TJd1 (to) = has necessarily to be fulfilled. Translated to the original tasks pace coordinates, this condition is expressed as
b) q2' (0/5 )
q l ' fm/s I
°
0. 4
- 15
0.2
-20
6 t 151 -0.2
-25 - 30
-0 .4
(26)
+--""""----'---=-6 t
q4' (o/s)
q3' [ 0151
hence easily checkable. Having found the solution to the inverse problem for the normal form , we are in a position to transform this solution to the original coordinate system q. To this aim one needs to apply transformations (20), (13) and make variables substitutions according to (6). After this step a jointspace trajectory qd(t) is produced which corresponds to the desirable tasks pace trajectory Yd(t) .
(s]
0.5 6 t (5] -2
-0.5
-4
-6
-1
c) q1"
(m/s'21
q2"
0.25
Now, let us apply the above procedure to solving an exemplary singular inverse kinematic problem. Assume that the manipulator 's end effector has to follow a trajectory in the taskspace described by the function
( 015 '21
0. 4 0.2 5
6 t (5 I
- 0.2 2
3
4
5
6 t(s]
-0. 4
q4' , (0/s"2)
Yd(t) = (0.7sin(0.5t-1.5),0.8 1.5 - 0.4 cos(0.5t - 1.5), -165 + 25t , 180, 180) (27)
+--:--'20---::3 --:4:---;:---/; t
+--'-~==--"-=5 --:-6 t
defined for t E IT = [O,6][8J. This trajectory forms a segment of ellipse in Y1Y3 plane in the manipulator'S taskspace, crossing transversally the track. It is easily observed that at time to = 3[8J the manipulator enters the singular configuration q2 = 90 . Furthermore for to the equality (26) is fulfilled, so we can expect that there exists a bounded jointspace solution to the singular inverse kinematic problem. After applying transformations (7), (23), (25), (20), (13) and making the substitution (6), we obtain two jointspace trajectories (due to a plus/minus sign in the expression (20), but only one of them, depicted in Figure 3, lies within the joint motion limits (1). The joint variables q5 and q6 which are not shown in Figure 3 are constantly equal to zero during the
(5]
-
(5
0.5
Fig. 3. Desirable a) position, b) velocity and c) acceleration trajectories in the jointspace obtained with the normal form approach. manipulator motion. Figure 4 shows the taskspace path corresponding to the desirable trajectory, together with recorded sequence of the manipulator's postures.
4. CONCLUSIONS This paper has demonstrated an applicability of the normal form approach to solving a specific singular inverse kinematic problem arising in the 143
I
Kieffer, J. (1994) . Differential analysis of bifurcations and isolated singularities for robots and mechanisms. IEEE 1'rans. Robotics Automat. 10(1), 1- 10. Muszynski, R. (1996). Mathematical Models and Control Algorithms for Robotic Manipulators with Kinematic Singularities. PhD thesis. Institute of Engineering Cybernetics, Technical University of Wrodaw. (in Polish) . Muszynski, R. and K Tchon (1996). Normal forms of non- redundant singular robot kinematics: Three DOF worked examples. J. Robotic Syst. 13(12), 765- 79l. Nakamura, Y. (1991). Advanced Robotics: Redundancy and Optimization. Addison-Wesley. Nenchev, D. N. (1995). Tracking manipulator trajectories with ordinary singularities: A null space-based approach. Int. J. Robotics Research 14(4) , 399-404. Nenchev, D. N. and M. Uchiyama (1997) . Singularity-consistent path planning and motion control through instantaneous selfmotion singularities of parallel-link manipulators. J. Robotics Syst. 14(13), 27-36. Seng, J., K A. O'Neil and Y. C. Chen (1997). On the existence and the manipulability recovery rate of self-motion at manipulator singularities. Int. J. Robotics Research 16(2), 171-184. Shamir, T. (1990). The singularities of redundant robot arm . Int. J. Robotics Research 9(1), 113-12l. Shankar, S. and A. Saraf (1995). Singularities in mechanisms 1. M echo Machine Theory 30, 1139- 1148. Tchon, K and A. Matuszok (1995). On avoiding singularities in redundant robot kinematics. Robotica 13, 599- 606. Tchon, K and 1. Dul~ba (1993) . On inverting singular kinematics and geodesic trajectory generation for robot manipulators. J. Intelligent Robotic Syst. 8, 325-359. Tchon, K and R. Muszynski (1997). Singularities of non- redundant robot kinematics . Int. J. Robotics Research 16(1) , 60- 76. Tchon, K and R. Muszynski (1998). Singular inverse kinematic problem for robotic manipulators: A normal form approach. IEEE Trans. Robotics Automat. Tchon, K, R . Muszynski and C. Zielinski (1997). Kinematic singularities of the IRp-6 manipulator mounted on a track. In: Proc. MMAR '97 Symposium. Vol. 3 . Mi~dzyzdroje , Poland. pp. 913-920. Tsumaki, Y. et al. (1997) . Teleoperation based on adjoint Jacobian approach. Contr. Systems 17(1),53-62. Whitney, D. E . (1972). The mathematics of coordinated control of prosthetic arms and manipulators. J. Dyn, Syst. Meas. Contr. 94 G(4), 303-309.
Fig. 4. A sequence of the manipulator's postures along the desired trajectory. control of the ASEA IRb-6 manipulator mounted on a track. The problem has been solved in accordance with the normal form-based inversion algorithm. This approach can be applied to all planar manipulation robots mounted on a track for which the considered singular inverse kinematic problem arises. It has been proved that singular configurations of the IRb-6 manipulator have corank 1 and can be given the hyperbolic normal form . A remarkable feature of the IRb6 kinematics is that equivalence transformations establishing equivalence of the original kinematics to the normal form can be defined explicitly and globally, and so can they be inverted. On exploiting this feature efficient computations of the jointspace trajectory have been performed and applied to an exemplary desirable taskspace trajectory meeting transversally the track of the manipulator. Computer simulations have indicated exquisite quality of the singular inverse problem solution produced by the normal form approach. These indications have been confirmed in a series of experiments conducted on the ASEA IRb-6 manipulator that will be reported in a separate paper. The performed experiments have also shown a difficulty concerned with the taskspace trajectory selection for which there exists a bounded jointspace trajectory in the presence of singular configurations. This problem can be overcome by reducing the tasks pace dimensions by one and treating the manipulator as a redundant one. Details will be deferred to a separate paper. 5. REFERENCES Kieffer, J . (1992) . Manipulator inverse kinematics for untimed end-effector trajectories with ordinary singularities. Int. J. Robotics Research 11(3), 225- 237. 144