Решение прямой задачи о положениях для кинематической структуры манипулятора ПР

L1 := 200

R2 := 150

L3 := 300

R3

:=100

L4

:= 350

L5

:= 100

L6

:= 100

L7

:= 200

R7

:= 100

L8

:= 150

L9

:= 100

R9 := 20

s1 := 100

α2 := 90o

s3 :=100

α4 := 90o

s5 := 20

α6 := 270o

s7 := 50

α8 := 180o

α9 := 0o

æ R9

×cos(α9 )ö

æ

20 ö

ç

R9

÷

ç

÷

A := ç

×sin (α9 ) ÷

A9 =

ç

0

÷

9 ç

L

÷

ç100÷

ç

9

÷

ç

÷

ç

1

÷

1

è

ø

è

ø

▼ Преобразования систем координат

Переход от системы координат звена 9 к звену 8

-π ö

M9 _ 8 := Ms (L8 ×cos(α8 ), L8 ×sin(α8 ),0)× Mϕz (α8 -π )

æ

×Mϕy ç

2

÷

è

ø

æ0

0

-1

150ö

ç

0

1

0

0

÷

M

=

ç

÷

9 _ 8

ç

1

0

0

0

÷

ç

0

0

0

1

÷

è

ø

Переход от системы координат звена 8 к звену 7

æ

0

0

-1

-100ö

æ

-

ç

0

1

0

0

÷

π ö

M8 _ 7 = ç

÷

M8 _ 7 := Ms(-R7 ,0, -L7 )×Mϕy ç

2

÷

1

0

0

è

ø

ç

-200÷

ç

0

0

0

1

÷

è

ø

Переход от системы координат звена 7 к звену 6

æ

π

ö

× Mϕx

æ π

ö

M7 _ 6 := Ms (s7 ×cos(α6 ), s7 ×sin (α6 ),0)×Mϕz çα6 -

2

÷

ç

÷

è

ø

è 2

ø

æ -1

0

0

0

ö

ç

0

0

1

-50

÷

M

=

ç

÷

7 _ 6

ç

0

1

0

0

÷

ç

0

0

0

1

÷

è

ø

Переход от системы координат звена 6 к звену 5

æ1

0

0

-100

ö

ç

0

1

0

0

÷

M6 _ 5 := Ms(-L5 ,0,0)

M6 _ 5 = ç

÷

ç

0

0

1

0

÷

ç

0

0

0

1

÷

è

ø

Переход от системы координат звена 5 к звену 4

æ 0

1

0

0

ö

M5 _ 4 := Ms (L4 ×cos(α4 ), L4 .sin (α4 ), s5 )×Mϕz (α4 -π )

ç

-1

0

0

350

÷

M5 _ 4

= ç

÷

ç

0

0

1

20

÷

ç

0

0

0

1

÷

è

ø

Переход от системы координат звена 4 к звену 3

æ 0

0

1

0

ö

æ π

ö

M4 _ 3

=

ç

0

1

0

-100

÷

M4 _ 3 := Ms(0, -R3 , L3 )×Mϕy ç ÷

ç

-1

0

0

300

÷

è 2

ø

ç

÷

ç

0

0

0

1

÷

è

ø

Переход от системы координат звена 3 к звену 2

æ1

0

0

0

ö

æ

α2

-

π ö

M3 _ 2

ç

0

1

0

150

÷

M3 _ 2 := Ms (R2 ×cos(α2 ), R2 ×sin (α2 ), s3 )× Mϕz ç

÷

= ç

0

0

1

100

÷

è

2 ø

ç

÷

ç

0

0

0

1

÷

è

ø

Переход от системы координат звена 2 к звену 1

æ0

0

-1 -200ö

æ

-

ç

0

1

0

0

÷

π ö

M2 _1

=

ç

÷

M2 _1 := Ms(-L1 ,0,0)× Mϕy ç

2

÷

1

0

0

0

è

ø

ç

÷

ç

0

0

0

1

÷

è

ø

Переход от системы координат звена 1 к звену 0 (стойке)

æ 0

0

1

100ö

M1_ 0 := Ms (s1

æ

π ö

M1_ 0

ç

0

1

0

0

÷

,0,0)× Mϕy ç

÷

= ç

0

0

0

÷

è

2 ø

ç-1

÷

ç

0

0

0

1

÷

è

ø

Переход от системы координат звена 4

к неподвижной системе координат

M9 _ 0 := M1_ 0 ×M2 _1 ×M3 _ 2 ×M4 _ 3 ×M5 _ 4 × M6 _ 5 × M7 _ 6 × M8 _ 7 × M9 _ 8

æ 0

1

0

120 ö

M

ç

-1

0

0

400

÷

= ç

÷

9 _ 0

ç

0

0

1

1000÷

ç

0

0

0

1

÷

è

ø

▲ Преобразования систем координат

Координаты точки схвата в неподвижной системе координат

æ

120 ö

ç

380

÷

A := M

× A

A = ç

÷

0

9 _ 0

9

0

ç1100

÷

ç

1

÷

è

ø

64