. . . .

:

.

Y
Y2
- 2

Yc

(E)

m1
X2

I3

Xc

m3

Y3
I2
I1
m2

3
X3

X1

Y1
(W)

22/3/2013


. . . .

" "

.
,
,
( end effector )

. ,
, ,

.


.


.



. ,
.
,

( ) .
, ,
,
. ,
,


,
.

. ,

, ,


.

.

.
22/3/2013


. . . .

, ,

.

.
-
. ,
. ,

. ,

:
, ,
.
,
.
,
.
.
()
.
( )
.
.
...



.

.



. ,

,
.

22/3/2013


. . . .

( )
.

( )

Y = f(q)

Y Rm, q Rn

(1)

q: n
Q
Y: m

( -)
Z = g (q)

Z Rk, q Rk

(2)

q: n
Q
: m

-
. , -

.

(1) (2) ,
" "

22/3/2013


. . . .

X=

f (q)
Y
= g(q) = h(q)
Z

X Rn, Z Rk, Y Rm, q Rn

(3)

()
.

22/3/2013


. . . .

(2)
( )
( )
.

.
.
:
? ?

aij
ei

Link i
cij

dc ij
Pij
j

2 : i-th j-th
t.

1. (c)ij i
j
2. (dc)ij (c)ij j
3.
(dc(q))ij (ro) ij c ij
:

(dc(q(t)))ij (ro) ij = c ij g ij (q(t)) 0

(4)

j
22/3/2013


. . . .

(4) , t. g ij < 0
j
(2). ,
Zl = gij(q)
l = 1, 2, k

i
j

j , k


:




.

22/3/2013


. . . .

(3)
, q,

f (q)
Y
= g(q) = h(q)
Z

X=

X Rn, Z Rk, Y Rm, q Rn

(3)

,
" ".
X Rn

q = h-1(X),

, q Rn

(5)

(5)
:
(3)

X=

dh
q = J(q) q
dq

: J =

dh
dq

(6)

Rnxm

(3).
(3),

J = 0e J
c

Je =

df
dq

Je =

df
dq

Rnxm

Rnxm

( (1))

Jc =

dg
dq

kxn

(constrain)
( (2)).
, (6)

J (q(t))

X(t) = 0e

22/3/2013

0
q(t) = J(q)q(t)
Jc(q(t))

(7)


. . . .

, J(q) (6)
, (6)

qd(t) = J-1 (q)Xd(t)

(8)

d .
, ,
qd(t)
Xd(t) .

22/3/2013


. . . .

(6)
(6) J .
J
:
1. Je ,
2. Jc ,
3. Je Jc
(7) J .
:

( ).
.

(2)
.
.

(3).
, ( ).
:

.
(6).
.
(6) "" ,
,
(6)

qd(t) = J-1 (q)Xd(t)

(8)

: ( )
(n)
(n) .

22/3/2013

10


. . . .

(6)
()
(damped least-squares)

q
(residual error)

E = Xd - X

Xd - Jq

(9)

(5)
.
q q
(6)
J(q)

qd = J-1 Xd

E = 0,

L= E

2
w

+ q

2
w

(10)

(matrix)

L= E

2
w

+ q

2
w

= EeTWeTEeT + EcTWcTEcT + q-1 Wvq

(11)

Ee = Yd - Jeq Ec = Zd - Jcq
We, Wc, Wv
mxm , kxk nxn .
:
- We, Wc Wv,
,
22/3/2013

11


. . . .

(11).
(11)

) (
) We , Wc
) - (
) We , Wc
) (
q
) We Wc

:
IIxII = norm of a vector x ( x)
, .

22/3/2013

12


. . . .

q
(10)

q
(10) q

dL
=0
dq

qJ = [ JeTWeJe + JcTWcJc + Wv ]-1 [ JeTWeYd + JcTWcZJ ]

(12)

, (11)
:
qd (N) = [ JT(qn)WJ(qn) + W v ]-1 JT(qn)W [ Xd (N+1) - X(N) ]

(13)

(sampling step)

W = 0 eW
c
:

[ JT(qn)WJ(qn) + W v ]
( W v),
- .
(13)
.
(13)

.
(13)
Xd (N+1) ()
.

22/3/2013

13


. . . .

We, Wc Wv
We, Wc
Wv

.


W e, Wc Wv.
We, Wc Wv,

We
( ).

Wc -

-.

Wv
[ J W J + W ] . Wv
.

22/3/2013

14


. . . .

1. .

,

.
.
.
.

L

L

2. .
:
Pentium 156 z 32 R

3. .
MATLAB
.
) robotics.zip (
) bin Matlab
) current folder robotics Matlab.

MATLAB \matlab\bin\robotics. \matlab\bin
start.m
( 1).
L, prompt
lang

22/3/2013

15


. . . .

( 2).

.
:

3
, .
22/3/2013

16


. . . .

-

.

.
. .

5

.

-----

-----

1)
2)
3)
4)
5)
6)
7)
8)
9)
:
5

9 .

, .
22/3/2013

17


. . . .

. 1
:

.
1 8.
1 .

.

. , :

7
22/3/2013

18


. . . .

" - ".
<enter>
.
2
.

max(,). (,) = 30,40
,
40. ,
max(,)


, 2
.
3
( ) .
.

22/3/2013

19


. . . .

-180 180.
,
, .
4
, .
,

.

.

10

: R2 R1
2 rad/sec
2.9 rad/sec
0.9 2.9 2+2.9 = 4.9
.
5
.

11
22/3/2013

20


. . . .

6 ,

.

12

7 8
.

.
,
: (.)=(30.30), : L1 = 10, L2=10. L3=10 1=-40,
2=60, 3=110, x,y=0,5 R1=2, R2=0.4,
Wv=0.5, We=15 Wc=6800.
9, :

13

22/3/2013

21


. . . .

.
:

14

15

22/3/2013

22


. . . .

. ,

.

2 .
2.


:

22/3/2013

23


. . . .

16

1,2,3,5,6,8
. 4

.

18

22/3/2013

24


. . . .

7
.
9 .
3 4

.
6
prompt MATLAB.
cd
L
. cd
MATLAB.


, .

1
1
.
: =30 =30.
: 10 10
10.
: - 40 60
110
: =0 =5 R1=2 R2=0.4
: Wv=0.8, We=15 Wc=6800
: 50
. = 12 (

).
,

= -13 = 15

2
2
.
: =40 =40.
: 10 20
22/3/2013

25


. . . .

10
: 60,
-130 120 .
1 : = - 10, = 10, = 5, =10, R1= 1
R2= 0.4.
2 : = 15, = - 10 = 0, = - 10,
R1= 1, R2=0.4
: Wv=0.8, We=15 Wc=6800.
: 60
( 7)
.
3
3
.
: =40 =40.
: 10 10
10 10.
: -60 60,
60 60.
: = 3, = 5, R1= 2, R2= 0.4.
: Wv= 1, We= 1 Wc= 9000.
: 50
. = 12 (

).
,
= -15 = 15
4
4

.
: =40 =40.
: 10 10
10 10.
: -60 60,
60 60.
1 : = - 10, = 10, =5, =10, R1= 1
R2= 0.4.
2 : = 15, = - 10 = 0, = - 10,
R1= 1, R2=0.4
: Wv= 1, We= 1 Wc= 9000.
: 60
( 7)
.

22/3/2013

26