Hi ngh ton quc v iu khin v T ng ho - VCCA-2011

VCCA-2011
Giai bai toan ng hoc ngc robot d dn ng bng phng phap chiu
toa va chiu vn tc
On solving inverse kinematics of redundant robotic manipulators by using
coordinate and velocity projection methods

Nguyn uang oang hai hng hao
rng BK a Ni
e-Mail: hoangnq-dam@mail.hut.edu.vn; thaotp-sme@mail.hut.edu.vn



m tt
Mt t Robot
d d tr y tr y. Tr
t t ty y d d
tr y v t v
t. N t y
r tr t tr
J tr tr
. tr t
t t u
u. N u y
v v t u
tr t t v v tr v v t.
Qu x t
t . Mt s s tr y
tut t.

Abstract
A method for solving inverse kinematics of redundant
robot is introduced in this paper. Firstly, inverse
kinematics of redundant robotic manipulators is
represented in two levels: velocity level and
acceleration level. Solutions of these problems are
given by pseudo inverse of the Jacobian matrix under
consideration of null space in order to avoid joint
limitations. Secondly, the values of joint variables are
found by the integration which is revised by
coordinate and velocity projection methods. By using
this method the joint variables and joint velocities are
adjusted such that they are forced onto manifolds
defined by position constraints and velocity
constraints. Therefore, the accuracy of solutions is
significant improved. Some results of numerical
simulations are illustrated for this method.
1. hn m u
Rt d d u u s v Robot
u v t u u uy
tr vt tr d tr
[1, 9, 10, 11]. Rt d d
s t s t d
u y u uyt
t . P d tr tr
J tr t y s d
t d t t .
y t uyt
tr s tuy t s u s
tr. t su
t v t t t v
u u t t. Tuy t
u t t
t u u d s
s t y tr u tr t t. Mt s
t y
u. 10
u s s vt t
suy r 5,6,7].
Tr y t s d
u t suy r v u v t suy r
u t c d tr
tr J. t u
t u t suy r v v t suy r
su t tr t x
tr t tr v
.
tr y su v tt v
t tr y tr
v . M tr y u t v
u v t t x
t. s
Mt v t ty y t d
tr y tr . u t s t u
r.
2. t bai toan va phng phap giai
2.1. Nu bai toan
t ty y n t d,
n
q vt
t . rt v tr
khng gi t t,
m
x vt v tr
v
m
( m 6 ). t
tu rt uyt
u t Dvt-Harter r
[2, 3, 4, 9, 10]. Kt u t tu
t su
( , ) 0 f x q ,
m
, ,
n
x f q . (1)
Ty y d d m n s t d
ty y s t d .
220
Hi ngh ton quc v iu khin v T ng ho - VCCA-2011
VCCA-2011
tr 1) t t t
tr v t

x q
0 J x J q , (2)
v tr J su
( , ) / , ( , ) /
x q
J x q f x J x q f q .
T t o tr t
tr t

x q x q
0 J x J q J x J q (3)
tr rt m x y 3, [ , , ] f x
u ,2,3 t vt d t
su
( ) x f q
( ) x J q q (2)
( ) ( ) x J q q J q q (3)














H. 1 T cu trc c

N r t t tr v uy
ty y d u d [2, 4, 9, 10]
( ) ( , ) ( ) u M q q C q q q g q (4a)
tr u vt d
( ) M q tr ( , ) C q q q vt
rs v y t v ( ) g q vt
suy r d tr tr.

B t t r y t uy
t t t t t ( ), ( ), ( ) x x x t t
uy t t ( ) q .
2.2. hng an giai quyt
s r tr J
q
J m n y
( )
q
rank m J . Nu t x v q t tr
(2 ) t t s v t
q t q .
Nu s d tr t q , t
t v t q v q .
Nu s d tr t q t
t t q v q .
Ty tr t v tr v
uy t d
d.
3. i u chun ca vct vn tc va gia
tc suy rng
3.1. i u chun ca vct vn tc suy rng
tr t u u
vt v t suy r t. T t q
t x v u

1
2
min, 0
T
f q Wq W (4)
Kt u
( )
W
q J q x (5)
v

1 1 1
( ) ( )[ ( ) ( )]
T T
W
J q W J q J q W J q
tr t tr s
tr Jacobi ( ) J q [8].
Nu tr tr s tr v,
W I t t t s u
t.
Nu tr tr s tr
ty y ( ) W M q t tr t
v tu u t u ng tu.
Nu tr J t
s

0
( ) ( )
W W
q J q x I J J z , (6)
v
0
n
z vt ty . t y s t r
uy u
uy . T t vt y s
t t u ty y
d d tr vt tr d
tr v v . T t
t y t
0
z t t

0
( ) q
z
q
f
a (7)
v ( ) q f tu tu v yu u
t r. tr d t
y t t
( ) det[ ( ) ( )]
T
q J q J q f (8)
H y trt tu t d. D v
y s robot tr d
tr u tr t .
tr v v t v
ho t

2
1
1
( )
2
n
i i
i
i
iM im
q q
c
q q
|
=
| |
=
|

\ .

q (9)

iM
q (
im
q ) u t
t v
i
q tr v
;
i
c tr s. D
y t d d s t
End-Effector
z
x
y O
221
Hi ngh ton quc v iu khin v T ng ho - VCCA-2011
VCCA-2011
tr v
robot tr s v v .
tr v v vt t s d h
t vt
( ) min ( ) | = q p q o (10)
o vt v tr t t tr
vt v d t tr tr
vt u v ( ) p q l vt v tr suy r
u tr robot. D y s
gip robot tr vt tr u tr t
. Tr t t robot v
vt x tr y
t. Tr y v tr v v
u t t. Ty v v
vt t

0
( )
( )
q
z K q q
q
f
. (11)
v
2
/( )
ii i iM im
K c q q = , 1, 2,..., i n = .
3.2. i u chun ca gia tc suy rng
K t t
tr s d t
y. t t
( ) ( ) J q q x J q q (12)
T t v tr tr v s
. y t s t v u
u t. T 12 v u


1
2
min
T
f q Wq (13)
Kt u t u t
( )[ ( ) ]
W
q J q x J q q (14)
Nu tr J t
s
( )[ ( ) ] [ ( ) ( )]
W W o
q J q x J q q I J q J q z (15)
Tr y v vt
0
z xut
sau

0 1 2 1 2
( ), , 0 z Kq K q q K K , (16)
v y v tr v v .
T vy ty v t
tr v v ( ) t q su

1
2
( )[ ( ) ] ( )
( ) ( )
o
q J q x J q q I J J Kq
I J J K q q

hay
1
2
[( ) ]
( ) ( ) ( )
o
q I J J K J J q
I J J K q q J q x
(17)
y v r ( ) t q s tu x
tu v vy s tr s t
t v d s tr v
v .

4. iu chnh bng phng phap chiu
D s s v s s tr tr
u tr t ( ) t q t tr
s ( ), ( ) t t q q t
t t tr t
v . Tr y tr y
u u tr ( ), ( ) t t q q t s
t tr t v
.
4.1. iu chnh toa suy rng
Kt u t t q t t suy r
*
q
tr y t t tr
d s s t s s tr

*
( , ) 0 f x q
y t s u t q t
tr . T u t s t
q tr t x 1 v
*
q v t. K t tr
t: t q t 1 s V su y
t tu

* * 1
2
( ) ( ) min, 0
T
V q q P q q P (18)
S d t y
t Lr t t xt t u
sau

1
2
( , ) ( , )
T
L V f x q f x q A , (19)
v A tr x d u
t u t t t ty g
t s t t ty ftr.
t q t

*
( , )
( ) ( ) ( , ) 0
T
q
L
t h q
q
P q q J q f x q A
(20)
y m tr s tuy v q
v t Nwt-
Rs tr Tyr t ( , ) h q
o
q
t
( , ) ( ) ( ) ...
o o o
t h q q h q H q q (21)
v

*
( ) [ ( ) ( , )]
[ ( , )]
[ ]
o
o
o
T
o q
T
q
T
q q
q
q q
q
H q P q q J f x q
q
P J f x q
P J J
A
A
A
(22)
Lu t tr ( )
o
H q t t s
t [ ( , )]
T
q q
J f x q A . t t
t t t v M tuy s
y t t y t u .
P s d t q
sau:
1) 0 k t s N
k ( ) *
q q
222
Hi ngh ton quc v iu khin v T ng ho - VCCA-2011
VCCA-2011
2) tnh
k ( )
( ) h q
3) tr u d u
k ( )
( ) e h q
k N t d tr t t
4) t tr
k ( )
( ) H q
k k ( ) ( )
( ) ( ) 0 h q H q q tm q
5) t tr
k k ( 1) ( )
q q q ,
6) t k , 1 k k ; uy .
4.2. iu chnh vn tc suy rng
Su u t suy r t t u
v t suy r. u v t suy
r s d t t
t t s d tr
. s t u t t q t v
t suy r
*
q tr y t t
tr 2) d s s t
s s tr t

*
0
x q
J x J q (23)
y t s u t q t
tr (2).
T u t s t q tr
t x 2 v
*
q v
t. N t t tr t t q
t 2 s V t tu

* * 1
2
( ) ( ) min, 0
T
V q q Qq q Q (24)
t y t
Lr t t u su

1 1 1 * *
*
( ) ( )
( )
T T
q q q x q
x q Q Q
q Q J J Q J J x J q q
J J x I J J q
(25)
N vy, tr u tr u u t t
tr t t u t
.
5. phng s












H. 2 T phng c t o

Tr y t u s
Mt r. t st
t ty y t d v uy
uy tr t . u

i
m u d
i
l v tr t x
i
a v u t v tr u
t u
Ci
J . M v t s ty
y r tr 2 v .

Bang . t s ty y
khu i 1

2

3

4

5
l [m] 0.55 0.50 0.45 0.40 0.20
a [m] 0.20 0.20 0.20 0.20 0.10
m [kg] 2.00 1.75 1.50 1.00 0.50
J
Ci
[kg m
2
] 0.0504 0.0364 0.0253 0.0133 0.0016
Tr t uy
t u tr v t ( , )
C C
x y . . v
R . . t d u . s
1.5708 f
rd. Kt u t tu t
tr su
( ) x f q v
5
1 1
5
1 1
5
1
sin( )
( ) cos( )
k
k i
k i
k
k i
k i
i
i
L q
L q
q
f q
y
T
x y [ , , ] f x vt v tr x y ( , ) v
f ;
T
q q q q q
1 2 3 4 5
[ ] q
vt .
Kt u t t
. Kt u r t t t
t ( )
i
q t s s u H.
3, 4).
0 5 10 15 20
-1
-0.5
0
q
1
0 5 10 15 20
0.5
1
1.5
q
2
0 5 10 15 20
1
1.5
2
q
3
0 5 10 15 20
0
0.5
1
q
4
0 5 10 15 20
-2
-1
0
q
5
t [s]

H. 3 th cc in khp tho thi gin qt r

g
x
y
B
q
2

q
3

q
4

q
1

O
A
C
C
1

C
2

C
3

C
4

|
E
q
5

223
Hi ngh ton quc v iu khin v T ng ho - VCCA-2011
VCCA-2011
0 5 10 15 20
-1
0
1
x 10
-3
e
1
0 5 10 15 20
-1
0
1
x 10
-3
e
2
0 5 10 15 20
-1
0
1
x 10
-3
e
3
t [s]

H. 4 th si s qu o t), [m, m, rad]
S s u v
-3
.
Kt u t d tr tr v
t. Tr y r t u
tr tr tr s tr v. M
tr K tr u t
(10,10,10,10,10) diag K
Kt u r t t t
t ( )
i
q t s s u t
d ( )
i
u t (H. 5, 6, 7).

H. 5 th cc in khp tho thi gin qt r

H. 6 th si s qu o t r


H. 7 th n n ng ut

H. t u t d
tr tr gia t v t s s d
tr u t su

1
2
(10,10,10,10,10)
(20, 20, 20, 20, 20)
diag
diag
K
K


H. 8 th cc in khp tho thi gin qt r
224
Hi ngh ton quc v iu khin v T ng ho - VCCA-2011
VCCA-2011

H. 9 th si s qu o t r

H. 10 th n n ng ut, [Nm]

T t u tr t ty r s s u
rt
15
10 . t ty
t t t v s uy
.
6. Kt un
t tru uyt t
rt d d d tr tr
v v t v t. P u u
v t x
t. N r tr J
t
tr v d tr
v v . T v t
y t u
s v ty y t d.
S s t u u v
ty r
u x . P t
tr t t t uyt t
v tu u t u t u
d v t u sut
d.


ai iu tham khao

Nakamura Y.: Advanced Robotics/Redundancy and
Optimization. Addison-Wesley Publishing Company,
Reading 1991.
Nuyn T P: Rt . N xut
K v K tut H N 2004.
Nuyn Vn Khang: u vt. N xut
K v K tut H N 2007.
Nuyn Vn Khang, Chu An M: s rt
. N xut d H N 2011.
Nuy K L t Tr H N. t
tut t t robot d u.
Tuy t H t u t III T
H N .
Nguyen Van Khang, Nguyen Quang Hoang, Tran Hoang
Nam: On an efficient method for improving the accuracy of
the inverse kinematics of robotic manipulators. Int.
Conference on Engineering Mechanics and Automation
(ICEMA 2010), Hanoi, July 1-2, 2010, pp 186-194.
Nguyen Quang Hoang, Nguyen Van Khang: On kinematic
inverse and control of redundant manipulators under
consideration of jammed joint. Proceed. Iftomm 1.
International Symposium on Robotics and Mechatronics,
2009, Hanoi, Vietnam, pp.201-207.
Rao, C.R.: Generalized Inverse of Matrices and its
Applications. New York, Wiley, 1971.
Spong M. W.; Hutchinson S. and Vidyasagar M.: Robot
Modeling and Control. John Wiley & Sons, New York,
2006.
Sciavicco L., Siciliano B.: Modelling and Control of Robot
Manipulators, 2nd Edition, Springer-Verlag, London, UK,
2000.
Zhang Y. and Wang J.: Obstacle Avoidance For
Kinematically Redundant Manipulators Using A Dual
Neural Network. IEEE Transactions on systems, man, and
cyberneticspart b: cybernetics, vol. 34, no. 1, february
2004.

S. Nguyn uang oang sinh nm t H
N tt H
t s
v tut
. T
t
u s t
tut Hur-Harburg,
HL
TS. v t
M v u tu RO. T
y t dy t
H . v u t
t v u
u vt m
s .

225
Hi ngh ton quc v iu khin v T ng ho - VCCA-2011
VCCA-2011
K s Thi Phng Thao sinh
nm 1987 t H N, tt nghi
h Bch Khoa H N nm
2010, chuyn ngnh C i T.
T thng 9 n n nay,
tc gi gi dy t b mn C
h d, Vi C kh trng
h Bch Khoa H N. Cc
l v quan tm c tc gi bao
g m hnh ho v iu khi cc
h l l hc h nhiu vt, m ph s cc h
l.










226