計測自動制御学会東北支部 第 202 回研究集会 (2002.7.2)
資料番号 202-2

Redundancy Resolution Based on the Singularity-Consistent
Method○ D.N. ネンチェフ，妻木勇一

Dragomir N. Nenchev, Yuichi Tsumaki
弘前大学

Hirosaki University
キーワード : redundant manipulator, kinematic decoupling, arm plane, singularity, SC method
連絡先 : 〒 036-8561 弘前市文京町 3 番地 弘前大学 理工学部 知能機械システム工学科 知能制御工学講座
D.N. Nenchev，Tel.: (0172)39-3678，Fax.: (0172)39-3678， E-mail: nenchev@cc.hirosaki-u.ac.jp

1.

Introduction

rate due to singularities, both kinematic ones andalgorithmic ones, the latter being artiﬁcially intro-

Kinematically redundant manipulators have beenstudied for more than three decades 1) , and numer-

duced by the additional constraint used to resolve
the redundancy. What is still needed is a reliable

ous redundancy resolution methods and respective
approach which avoids this major drawback of thecontrol laws have been proposed so far. Such maexisting algorithms.
nipulators are expected to be incorporated in auThe additional constraint for redundancy resotonomous robots for dextrous manipulation taskslution determines the behavior to a great extent.
and in teleoperators for application in space andFor example, when the constraint is formulated inother risky environments. A few undergoing projects
terms of velocities and is a non-integrable one, theare based on commercial seven-DOF manipulatorssuch as the Robotics Research K-1207 arm 2) , or
the Mitsubishi PA-10 arm 3) , well-known throughbehavior lacks cyclic path tracking ability, and the
motion is unpredictable. This is the case when the
redundancy is solved by means of the pseudoin-research laboratories worldwide.
verse, i.e. the joint velocities are being locally miniMost of the research solves the underdetermined

mized 5) . Similarly, when the joint torque is locallyredundancy problem by making use of a generalminimized, unpredictable behavior in terms of joint
ized inverse of the Jacobian, possibly in combinavelocities is observed. Therefore, it is desirable totion with a so-called self-motion term from the null
avoid such local minimization constraints, and to
space of the Jacobian, or the augmented Jacobian
method 4) . Unfortunately, these methods deterio-

employ instead a constraint in terms of positions,i.e. a holonomic constraint.

–1–

In the case of a seven-DOF anthropomorphic arm

clusions are presented.

such as those mentioned above, a suitable constraint
is the orientation of the arm plane determined by
the centers of the shoulder, elbow and wrist joints6) , 7) .

2.

Background

2.1

The SC approach applied to a
non-redundant arm

A method for representing the constraint

through a so-called “arm angle” is described in 2) .

The SC approach is applied to a non-redundant

The arm-angle equation is derived in terms of joint
angles only and yields a predictable arm behavior.

arm according to the following procedure 8) .

Unfortunately, the equation introduces an algorith-

1) Denote the end-eﬀector path as x(p) where p is

mic singularity.

the path parameter, not necessarily time. The

We have proposed a method for stable control

path parameter augments the joint variables

at and around kinematic singularities of nonredun-q ∈ n : q̄ = (q, p).

dant manipulators, the so-called singularity consis-2) Derive the manipulator Jacobian J(q) ∈ n×n

tent or SC method 8) . The method has certain ad-

and form the column-augmented Jacobian:vantages over the well-known damped least-squares
H(q̄) =or DLS method for singularity treatment 9) , 10) .



J(q)

−S(p)


.

Its most important advantage is that no directional

S(p) ∈ m denotes the unit vector along theerror is introduced. Recently, the SC method was

positive direction of the end-eﬀector’s path tan-applied to the Robotics Research K-1207 arm 11) .

gent, and dx(p) = dpS(p), dp = dx(p), see1 .

This arm has non-zero joint oﬀsets, and therefore

Thus, the system is represented as a closed-

the analytical expressions obtained are quite com-loop system with one degree of redundancy:

plex. This is the reason why no closed-form inverseH(q̄)dq̄ = 0.

kinematics solution has been found so far. The(1)3) Find the solution to the last equation as

SC method has been implemented in terms of differential kinematics only, and hence, some of thedq̄ = bf̄ (q̄)(2)

interesting features of the method have been lost.
where b is any small scalar, and f̄ (q̄) ∈ kerH(q̄) ⊂In this paper we will consider an anthropomor-

n+1 .

phic seven-DOF arm with zero joint oﬀsets andkinematic decoupling, such as the Mitsubishi PA-

The last equation represents an autonomous dy-

10 arm. This allows us to derive a closed-form so-namical system, parameterized by the vector S(p).

lution for the null space vector of the arm, and it

It is important to note that the vector ﬁeld f̄ (q̄)

becomes possible to apply the SC method within anshould not be derived as a function of the pseu-

analytic framework. The paper is organized as fol-

doinverse H + , e.g. as a vector from the null-spacelows. First background about the SC approach andprojector (I − H + H). Instead, we derive each en-a numerical implementation is presented. In Sec-

try as f¯i = (−1)i+1 det H i , i = 1, ..., n + 1, wheretion 3 we derive the main result. Section 4 presents

1 dp can be assumed to be dimensionless end-eﬀector“speed.” In such case, the units of the elements of S (p)match those of dx(p).

a simulation study. Finally, in Section 5 our con-–2–

H i stands for H with its i-th column removed 13) ,
14) .

Z

The advantage of this derivation is that it
Syields an inherently stable behavior around criticalq2

points when any of the determinants approaches

E

q3
X

q4
W

zero. This includes the case of a kinematic singularity, i.e. det H n+1 → 0.

2.2

q1
111
000

The SC approach applied to aseven-DOF arm in terms of differential kinematics

Fig. 1

The four-DOF positioning arm.

Theoretically, when applying the SC method topling property, such that the three-DOF sphericala n-DOF kinematically redundant arm, the follow-

wrist can be treated independently from the four-

ing two steps are envisioned. First, augument the

DOF positioning arm. The latter is displayed inm-dimensional task-space constraint (i.e. the end-

Figure 1. We note that the Mitsubishi PA-10 arm

eﬀector motion constraint) by n−m additional con-has such a structure. We will focus from now on thestraints. This technique is well-known as the “au-positioning arm, since it contains the redundancy.

gumented task-space” approach 4) . Second, pro-

The direct kinematics of the manipulator arm is

ceed with the SC method as if the system is a non-solved based on the Denavit and Hartenberg no-

redundant one. The algorithmic singularities due

tation presented in 15) . Upon diﬀerentation, weto the additional constraints will be treated thereby

obtain the Jacobian as follows:

as kinematic singularities. It turns out, however,that from a practical viewpoint the solution is not
as straigtforward because of the complexity of thesystem after the augumentation. Even with just asingle degeree of redundancy it might be impossible to derive a closed-form solution and to apply
the SC method to its full extent. Therefore, in 11)
a numerical approach has been proposed. The arm
angle was employed as an additional constraint.

j11 (q) = d3 S1 S2 + d5 (C4 S2 + (C2 C3 S1 + C1 S3 )S4 )
j12 (q) = −d3 C1 C2 + d5 (C1 C3 S2 S4 − C1 C2 C4 )j13 (q) = d5 (C3 S1 + C1 C2 S3 )S4
j14 (q) = d5 (C4 (S1 S3 − C1 C2 C3 ) + C1 S2 S4 )j21 (q) =−d3 C1 S2 + d5 ((S1 S3 − C1 C2 C3 )S4 − C1 C4 S2 )j22 (q) = −d3 C2 S1 − d5 (C2 C4 S1 − C3 S1 S2 S4 )j23 (q) = −d5 (C1 C3 − C2 S1 S3 )S4
j24 (q) = −d5 (C4 (C2 C3 S1 + C1 S3 ) − S1 S2 S4 )
j31 (q) = 0

3.

The SC approach applied toa seven-DOF arm with kinematic decoupling property

Let us consider an antropomorphic seven-DOF
arm with zero joint oﬀsets and kinematic decou-

j32 (q) = −d3 S2 − d5 (C4 S2 + C2 C3 S4 )j33 (q) = d5 S2 S3 S4
j34 (q) = −d5 (C3 C4 S2 + C2 S4 )where d3 and d5 denote link lengths, Ci and Si
stand for cos qi and sin qi , respectively.

–3–

There are two types of kinematic singularities:

Next, let us consider the c term. This term van-(1) extended/folded-arm singularity (q4 = 0, ±π),ishes at the shoulder singularity. Since it appearsand (2) shoulder singularity, when the end-point isin the denominator of the ﬁrst two components, weon the Z axis.

can expect problems around the shoulder singular-

Denote by v ∈ 3 the (desired) wrist center point

ity, if self-motion via f̄ sm (q̄) is attempted. On thevelocity, and by v̂ the respective unit vector. This

other hand, note that self-motion at the shoulder

vector will be used to parameterize the dynamical

singularity means arm-plane rotation around thesystem as explained in Subsection 2.1. We adjoint

vertical Z axis, i.e. joint one rotation. Therefore,−v̂ to the Jacobian, to obtain the 3 × 5 column-

we do not use the above vector ﬁeld in the neigh-augmented Jacobian. The solution of the respec-

borhood of the shoulder singularity.
The other vector ﬁeld f̄ ep (q̄) has some lengthy

tive homogeneous equation (cf. Eq. 1) is obtainedasexpressions for three of its components, which wedq̄ = bsm f̄ sm (q̄) + bep f̄ ep (q̄)(3)

will omit here. The exceptions are the third compo-

where bsm and bep are arbitrary scalars and f̄ sm (q̄)

nent which is identically zero, and the ﬁfth compo-

and f̄ ep (q̄) are vector ﬁelds from the kernel of H(q̄).

nent which is S4 and hence, non-zero, except at thebsm and bep are considered as design parameterswhich determine the speed along the two vectorﬁelds. The vector ﬁelds we obtained in analyticalform using Mathematica. f̄ sm (q̄) has the following

extended/folded arm kinematic singularity. The
latter shows that f̄ ep (q̄) contributes to the endpoint (or wrist center-point) motion, and at theextended/folded arm singularity the end-point will
be instantaneously at rest (the arm is in the statecomponents:

of instantaneous self-motion) thus complying withf¯sm1 (q̄) = −d5 C3 S42 /c

the physical motion constraint at this conﬁgura-f¯sm2 (q̄) = d5 S2 S3 S42 /ction. This demonstrates again the appropriatenessf¯sm3 (q̄) = S4of the expressions we derived for the Jacobian nullf¯sm4 (q̄) = 0

space.

f¯sm5 (q̄) = 0

The third component of f̄ ep (q̄) being identicallywhere c = (d3 + d5 C4 )S2 + d5 C2 C3 S4 . f̄ sm (q̄) determines the self-motion of the manipulator sincef¯sm5 (q̄) is identically zero. As expected, the joint
four velocity is also always zero during self-motionof the arm. Further, it is apparent that whenever
the arm is at the extended/folded arm singularity, no self-motion will be performed since S4 andhence the entire self-motion vector ﬁeld vanishes.
This matches the physical condition and shows theappropriateness of the expression.

zero is a somewhat surprising result. Note thatthe q3 angle determines the orientation of the arm
plane, and whenever this joint is immobilized, theorientation will be maintained. Thus, we obtained
a nice decoupling property: with bsm = 0, f̄ ep (q̄)
will drive the wrist center-point in the desired direction of motion without changing the arm-planeangle; with bep = 0, f̄ sm (q̄) will drive the selfmotion of the arm. Hence, eﬀective arm motion
control will be possible by proper blending of bsm
and bep.

–4–

There is one exception, when the arm is at theis apparent that after approx. 1 s the arm moves

shoulder singularity. It is intuitively clear that jointsmoothly through the extended-arm singularity atthree motion has no contribution to self-motion in

the workspace boundary. Motion is reversed andthis case. As already mentioned above, self-motion

continues along the straight-line to cross the shoul-

would imply joint one rotation only, which can be

der singularity (when x = 0). Later on, the arm isachieved via the f̄ ep(q̄) vector ﬁeld in combination

once again fully extended and moves through thewith a proper command direction, i.e. v̂ being out

extended-arm singularity on the opposite side. Mo-of the arm plane. The vector ﬁeld must be modi-

tion continues in a cyclic manner.

ﬁed thereby2 in order to avoid the inﬂuence of the

In the second simulation we set bep = 1 andterm c apperaring as denominator in the ﬁrst two

bsm = 1 which yields end-point motion and self-

components.

motion with equal weight. Also, we change the

We would like to emphasize that no explicit re-initial joint angles as (0, 0, 60, −90) deg in order todundancy resolution criterion is used. Neverthe-

avoid the shoulder singularity. As already noted,

less, we obtained eﬀective arm-plane angle control.

self-motion (via the f̄ sm (q̄) vector ﬁeld) at thisOf course, no additional constraint means also no

singularity is physically impossible, so no attemptalgorithmic singularities. This is a signiﬁcant ad-

should be made to approach it. The result is shown

vantage of the method compared with other redun-in Figure 3. It becomes apparent that the extended-

dancy resolution schemes.

arm singularity is reached and crossed (end-point
motion is reversed) without any problem.

4.

This

clearly shows that the arm-plane motion (self-motion)

A simulation study

can be superimposed even at the extended/folded
We will demonstrate the performance of the methodby means of two simulations. The link lengths arearm singularity without deteriorating the smoothness.

d3 = d5 = 1 m. The desired end-point motion isalong a horizontal straight-line, with v̂ = (1, 0, 0).
The b’s are constant, i.e. natural motion is per-5.

Conclusion

formed and hence the initial and ﬁnal velocity is

We have presented a new and eﬃcient method fornon-zero 8) . Note that natural motion is quite suit-redundancy resolution for a seven-DOF arm withable for analysis since it satisﬁes the speciﬁc end-

zero joint-oﬀsets and kinematic decoupling, envi-

point motion constraint at a singularity.

sioning the Mitsubishi PA-10 arm. The method

In the ﬁrst simulation, the initial joint angles are

is eﬃcient in the sense that (1) no additional sin-(0, 0, 0, −90) deg. This yields initial wrist-center

gularities are introduced and (2) motion does not

location at (1, 0, 1) m. bep = 1, while bsm is set

deteriorate around kinematic singularities. The ef-

to zero. This means that the arm-angle will be

fective redundancy resolution scheme is based on

maintained (no self-motion). The result is shownarm-plane rotation control which is an intuitive andin Figure 2. From the end-point position graph itpractically valuable scheme. In a future work it

2 e.g. by choosing b
ep to be in proportion to c.

would be straightforward to derive a feedback con-

–5–

1.5

x
y
z

end-point position [m]end-point position [m]

2
1.5
1
0.5
0
-0.5
-1
-1.5
-2

x
y
z

1
0.5
0
-0.5
-1
-1.5

0

2

4

6

8

10

0

2

time [s]
200

q_1
q_2
q_3
q_4

100
50
0
-50
-100
-150
2

4

6

8

100

8

10

8

10

50
0
-50

10

0

2

q_1
q_2
q_3
q_4

0

2

4

4

6

time [s]6

joint velocity [rad/s]

joint velocity [rad/s]

2
1.5
1
0.5
0
-0.5
-1
-1.5
-2

8

1.5
1
0.5
0
-0.5
-1
-1.5
-2
-2.5

10

q_1
q_2
q_3
q_4

0

time [s]arm-angle.

10

q_1
q_2
q_3
q_4

150

time [s]

2

8

-100
0

Fig.

6

time [s]joint angles [deg]joint angles [deg]

150

4

2

4

6

time [s]Straight-line motion with constant

Fig.

3

arm-angle.

–6–

Straight-line motion with varyingtroller based on the scheme, following a proceduresimilar to the one we proposed earlier for a nonredundant arm.

pp. 159–182, February 2000.
9) Y. Nakamura and H. Hanafusa, “Inverse kinematic solutions with singularity robustness for
robot manipulator control,”ASME J. Dynam.
Sys. Measurment and Control, Vol. 108, pp.

References

163–171, 1986.
1) D. E. Whitney, “Resolved motion rate controlof manipulators and human prostheses,” IEEE
Trans. on Man–Machine Syst., Vol. 10, pp. 47–
53, 1969.

matic solutions based on vector formulations
and damped least-squares methods,” IEEE
Trans. on Systems, Man, and Cyb., Vol. SMC–

2) K. Kreutz, M. Long and H. Seraji, “Kinematic
Analysis of 7-DOF Manipulators,” The Int. J.
of Rob. Research, Vol. 11, No. 5, pp. 469–481,
1992.

16, No. 1, pp. 93–101, 1986.
11) Y. Tsumaki et al., “A numerical SC approach
for a teleoperated 7-DOF manipulator,” inProc. 2001 IEEE Int. Conf. on Robotics and3) Oonishi, ”The open manipulator system of the
MHI PA-10 robot”, in Proc. 30th ISR, 1999.
4) D. N. Nenchev,

Automation, Seoul, Korea, May 21-26, 2001,
pp. 1039–1044.

“Redundancy resolution

through local optimization: a review,” J. Rob.
Syst., Vol. 6, No. 6, pp. 769–798, 1989.

12) D. N. Nenchev, “Tracking manipulator trajectories with ordinary singularities: A null space
based approach,” Int. J. of Rob. Research, Vol.

5) C. A. Klein and C-H. Huang, “Review of pseudoinverse control for use with kinematicallyredundant manipulators,” IEEE Trans. Syst.,
Man and Cybernetics, Vol. SMC–13, pp. 245–
250, 1983.
6) E. Nakano,10) C.W. Wampler II, “Manipulator inverse kine-

14, No. 4, pp. 399–404, August 1995.
13) N. S. Bedrossian and K. Flueckiger, “Characterizing spatial redundant manipulator singularities,” in Proc. 1991 IEEE Int. Conf. on
Robotics and Automation, Sacramento, Cali-“Mechanism and control ofantropomorphous manipulator,” J. of the Society of Instrument and Control Eng., Vol. 15,
No. 8, pp. 637–644, 1976 (in Japanese).

fornia, May 1991, pp. 714–719.
14) Y. Tsumaki,

D. N. Nenchev,

and M.

Uchiyama, “Jacobian adjoint matrix based approach to teleoperation,” Int. Symp. on Mi-

7) J. M. Hollerbach, “Optimal kinematic design
for a seven degree of freedom manipulator,”
Robotics Research: The Second International
Symposium, ed. by H. Hanafusa and H. Inoue,
pp. 215–222, 1984.

crosystems, Intelligent Materials and Robots,
Sendai, Japan, Sept. 27-29, 1995, pp. 532–535.
15) M. W. Spong and M. Vidyasagar. Robot dynamics and control, John Wiley and Sons,
New York, 1989.

8) D. N. Nenchev, Y. Tsumaki and M. Uchiyama,“Singularity-consistent parameterization ofrobot motion and control,” The International
Journal of Robotics Research, Vol. 19, No. 2,–7–