Chương 3: Một số phép tính về ma trận doc

Trong quá trình tay máy chuyển động trong không gian làm việc, tại mỗi thời điểm M sẽ ở một điểm có tọa độ xác định.. 2.1 Bài toán thuận động học tay máy Cho trước cơ cấu và quy luật chu

Một số phép tính về ma trận1/ Phép nhân vô hướng và có hướng:

r M 0

r M i

X 0

Z 0

Y 0 r E 0 E

2 Quỹ đạo

Xét điểm M bất kỳ cố định trên khâu thứ i

Trong quá trình tay máy chuyển động trong không gian làm việc, tại mỗi thời điểm M sẽ ở một điểm có tọa độ xác định Tập hợp tất cả

vị trí cuả M theo thời gian sẽ tạo ra một đường dẫn liên tục, được gọi là quỹ đạo của

M

Có hai dạng bài toán cơ bản liên quan tới quỹ đạo của khâu tác động cuối:

- Bài toán thuận;

- Bài toán ngược.

2.1 Bài toán thuận động học tay máy

Cho trước cơ cấu và quy luật chuyển động của các yếu tố được thể hiện bởi các toạ độ suy rộng q i, ta phải xác định quy luật chuyển động của điểm trên khâu tác động cuối nói riêng

và của điểm M bất kỳ trên tay máy nói chung trong hệ trục tọa độ tuyệt đối.

Bài toán thuận luôn có một nghiệm duy nhất.

2.2 Bài toán ngược động học tay máy

Cho trước cơ cấu và quy luật chuyển động của một điểm trên khâu tác động cuối hoặc quy luật chuyển động và hướng của khâu tác động cuối trong hệ trục toạ độ tuyệt đối,

phải xác định quy luật chuyển động của các khâu thành viên được thể hiện bằng các toạ độ suy rộng q i

Vị trí cuả điểm O i trong hệ trục tuyệt đối được xác định bởi vector:

0

0 0 0

i x

i i

y i z

2 Ma trận quay hệ trục

2.1 Thiết lập

Vì p i và p 0 là hai vector cùng thể hiện điểm P nên:

p 0 = p xi x i + p yi y i + p zi z i

= [ x i y i z i ] p i

= R 0i p i

R 0i được gọi là matrận quay hệ trục, xác

O 0 x 0 y 0 z 0

i z

i z

i y

i y

i y

i x

i x

i x i

i

i i

z y

x

z y

x

z y

x z

y x

R

0 0

0

0 0

0

0 0

0 0

i zj

i zj

i yj

i yj

i yj

i xj

i xj

i xj j

i i

i

j

i

z y

x

z y

x

z y

x z

y x

R

Tổng quát

3 Phép biến đổi thuần nhất

Sử dụng phép biến đổi thuần nhất sẽ xác định được vị trí tương đối và hướng giữa các hệ trục Do đó sẽ xác định được vị trí và hướng của tất cả các khâu và tay gắp.

3.2 Phép chuyển đổi thuần nhất

Cho điểm P có tọa độ trong hệ địa phương

Nếu biết O i (a,b,c): tọa độ của gốc O i trong hệ O 0

O i

p 0

p i

trong trường hợp tổng quát:

R

0 0 0 1

a b c

Oi(a,b,c) (j)

III Xác định vị trí và hướng

Các thông số động học:

0 0

1 0

0

0 0

) cos(

) sin(

0 0

) sin(

) cos(

1 '

i

i i

i i

i i

0 0

0 )

cos(

) sin(

0

0 )

sin(

) cos(

0

0 0

1

'

i i

i i

0 0

0 )

cos(

) sin(

0

0 )

sin(

) cos(

0

0 0

1

1 0

0 0

1 0

0

0 0

) cos(

) sin(

0 0

) sin(

) cos(

'

1 ' 1

i i

i i

i

i

i i

i i

i i

i i

0 0

) cos(

) sin(

0

) sin(

) sin(

) cos(

) cos(

) cos(

) sin(

) cos(

) sin(

) sin(

) cos(

) sin(

) cos(

1

i i

i

i i

i i

i i

i

i i

i i

i i

i

i

i

d a

3 Phương pháp giải bài toán

thuận-bài toán vị trí

hệ trục toạ độ thứ n và hệ trục toạ độ tuyệt

đối như sau:

0 0

) 0 ,

cos(

) 0 ,

cos(

) 0 ,

cos(

) 0 ,

cos(

) 0 ,

cos(

) 0 ,

cos(

) 0 ,

cos(

) 0 ,

cos(

) 0 ,

cos(

0

n n

n

n

c z

zn y

zn x

zn

b z

yn y

yn x

yn

a z

xn y

xn x

xn A

Từ đó suy ra vị trí và hướng của hệ trục tọa độ gắn liền

với khâu thứ n.

PP2: Dựa vào (3.46) xác định được bộ tham số và

biến DH như sau:

- Tịnh tiến theo trục khoảng đại số

Quay quanh trục góc (góc lượng

giác);

xyz O

Thuật toán giải bài toán thuận

- Bước 1: Xây dựng hệ trục DH

- Bước 2: Xác lập các hệ số và biến DH

- Bước 3: Tính

- Bước 4: Xác định vị trí và hướng cuả

khâu tác động cuối hoặc một khâu bất

kỳ dựa vào hai phương pháp trên

Ví dụ 1

Số khâu?

Số khớp?

0 0

) cos(

) sin(

0

) sin(

) sin(

) cos(

) cos(

) cos(

) sin(

) cos(

) sin(

) sin(

) cos(

) sin(

) cos(

1

i i

i

i i

i i

i i

i

i i

i i

i i

i

i

i

d a

0 0

) 0 , cos(

) 0 , cos(

) 0 , cos(

) 0 , cos(

) 0 , cos(

) 0 , cos(

) 0 , cos(

) 0 , cos(

) 0 , cos(

0

n n

n

n

c z

zn y

zn x

zn

b z

yn y

yn x

yn

a z

xn y

xn x

xn A

Ví dụ 2 Xây dựng hệ phương trình động

học của Robot Stanford

IV VẬN TỐC

Xét tay máy có n biến khớp; khâu tác động cuối

có m bậc tự do

Gọi q 1 , q 2 , …,q n là các toạ độ suy rộng, hay còn goị

là các biến khớp tại các khớp tương ứng.

Chuyển động theo bậc tự do thứ i, i=1…m của

m m

m

n n

q q

q

f q

f q

f

q

f q

f q

f

q

f q

f q

f

x

x x

2 1

2 2

2 1

2

1 2

1 1

x   

T m

x x

m

n n

q

f q

f q

f

q

f q

f q

f

q

f q

f q

2 2

2 1

2

1 2

1 1

1

q J

x   

J được gọi là ma trận Jacopi

khớp quay

khớp tịnh tiến

nx ny nz x y z

V V V

(0) 1 (0) 1

0

i

i n i

i

i i

J

z z J

Cột thứ i trong matrận J, J i, là tác động

của chuyển động khớp thứ i lên trạng thái vận tốc của khâu tác động cuối.

Xác lập matrận Jacobi

J = [ J1,…, Ji,…, Jn]

(0) 1(0) 1

(0) 1 (0) 1

0

i

i n i

i

i i

J

z z J

p 

1

i i

O n-2

2

n n

Trong đó:

( 1) 1( 1)

1

i i i

Ví dụ

Hình 3.8 thể hiện cơ cấu có 2 khớp quay, hai khâu Hệ trục theo quy ước DH Tính vận tốc khâu tác động cuối?

1 1

i

i n

i i

z

p z

J

0 )

(

) (

.

1 0

0

:

1

2 1

2 1

1 2

1 2

1 1

0 0

0

0

0 2

0 1

z y

)](

[

2 1

2 1

1

2 1

2 1

c a

s a s

a

J

) (

.

) (

.

1 0 0

0 ) (

)

(

1 0

0

:

2

2 1

2

2 1

2

2 1

2 2

1 2

0 0

0

1

1 2 1

s a

s a c

a

z y

n

n

2 1

2 2

1 2

1 1

2 1

2 2

1 2

1 1

0 0 0 0 0 0

1 1

0 0

0 0

0 0

) (

)

(

) (

)]

(

a c

a

s a s

a s

• Dùng để khai triển các chế độ điều khiển thích hợp.

Bộ điều khiển hoàn hảo đòi hỏi sử dụng mô hình động lực

để đạt tới chế độ vận hành tối ưu với vận tốc cao dựa trên kết quả tính toán các moment ngẩu lực hoặc lực tập trung tác động lên các khớp quay và tịnh tiến theo thời gian;

Hai vấn đề liên quan tới động lực

học tay máy:

• Động lực học thuận: với các lực và moment tác động vào các khớp đã cho, phải xác định chuyển động cuả cơ cấu theo thời gian.

• Động lực học đảo: tìm moment và lực

tác động vào tất cả các khớp để tạo ra hành trình cuả cơ cấu theo yêu cầu.

Có nhiều phương pháp khảo sát động lực học tay máy:

- Sử dụng các phương trình Newton và Euler;

- Sử dụng nguyên lý d’Alembert và Hamilton;

- Sử dụng các phương trình chuyển động Lagrange

Ở chương này giới thiệu phương pháp thứ ba.

II Phương pháp Lagrang

Phương trình chuyển động Lagrang được xây dựng dựa vào hàm Lagrang.

Phương trình Lagrang viết cho khâu thứ i là:

3 Động năng

Động năng của khâu thứ i được viết:

(4.3)

• I i là ma trận quán tính khâu i tại tâm khối

lượng, viết trong hệ toạ độ tuyệt đối

• là vận tốc tịnh tiến tuyệt đối của khối

tâm và vận tốc quay tuyệt đối của khâu i.

3.1 Tính matrận quán tính

quán tính cuả khâu i trong hệ toạ độ

tuyệt đối:

i i

I

ci ci ci

i x y z C

ci yy

ci xx i

i

I I

I I

0 0

0 0

0 0

0 i ( )0 T

i i i i

I  R I R

2 2

) (

2 2

) (

2 2

) (

0 0

0 )

( 0

0 0

) (

V V

V

i

i

dm y

x

dm z

x

dm z

y I

2 2

) (

2 2

)

(

2 2

) (

0 0

0 )

( 0

0 0

) (

V V

V

i

i

dxdydz y

x

dxdydz z

x

dxdydz z

y I







Ma trận quán tính của khâu phụ thuộc vào:

- hình dáng hình học của khâu;

- cấu tạo của các khớp gắn liền với khâu;

- vị trí tương đối giữa các khớp gắn liền với khâu với đường tâm của khâu

• Ví dụ, xét khâu i là thanh hình hộp chữ

nhật

• Xét một số trường hợp thường gặp liên

quan tới tính toán mômen quán tính như sau:

- Nếu khớp i+1 trên khâu thứ i là khớp bản lề

2 /

2 2

2 /

2 /

2 /

2 /

) (

2 2

) (

) (

dz z

y dy

dx

dxdydz z

y I

2 /

3 2

2 /

2 /

) 12

1 (

y dx



)

( 12

h w

)

( 12

h w

m

( 12

1 )

(

)

( 12

1 )

(

2 2

) (

2 2

2 2

) (

2 2

w l

m dxdydz

y x

I

h l

m dxdydz

z x

I

i V

ci zz

i V

ci yy

2 2

2 2

0 0

0 0

0 0

12

1

w l

h l

w

h m

Tương tự

0 1

0

0 0

0 12

l m

2 2

2 2

00

00

00

12

1

w l

h l

w

h m

I i i i

- Nếu khớp i+1 trên khâu thứ i là khớp

2 2

2 2

0 0

0 0

0 0

12

1

w h

h l

w

l m

Nếu các kích thước h và w rất bé so với l:

0

0 1

0

0 0

1 12

l m

2 2 )

(

2 2 )

(

2 2

) (

0 0

0 )

( 0

0 0

) (

V V

V

i

i

dxdydz y

x

dxdydz z

x

dxdydz z

Ma trận quán tính của khâu i trong

hệ trục tọa độ tuyệt đối Ii

0 i ( ) 0 T

I  R I R

3.2 Vận tốc khối tâm

Trong phương trình động năng (4.3):

các giá trị vận tốc tịnh tiến và quay tuyệt đối cuả khối tâm được tính theo thương pháp Jacobi, đã được trình bày ở chương 3.

Vận tốc khối tâm khâu i, C i, được tính như sau:

ci ci

ci ci

i i i ci ci ci

ci

q

q

q J

J J

J q

J V

V V

x

z y x z y x

( )

( )

2 ( )

1 (

] [

0 0 0 0 0 0







• Chuyển động của các khâu j nằm sau khâu i sẽ

không ảnh hưởng tới chuyển động khối tâm của khâu này.

• Các cột của matrận Jacobi từ cột thứ j sẽ là:

T j

ci j i

 xci J ci q J ci(1) J ci(2) J ci(i) 0 0. q

Tổng quát, tâm khối lượng C i có

Cột thứ j trong ma trận Jacobi:

) 1 3

(

) 1 3

( )

1 6

(

) (

)

( )

j

ci v j

ci

J

J J



) 0 ( 1 )

0 ( 1 )

(

j j

ci

j ci

j j

ci v

z J

p z

J



)13

(

)13

()

16

(

) (

)

( )

j

ci v j

ci

J

J J



nếu khớp j là khớp trượt loại 5:

)13

(

)13

()

16

(

) (

)

( )

j

ci v j

ci

J

J J

) 0 ( 1 )

(

j ci

j j

ci v

J

z J



0 1

) 0

1 (

1

0 1

) 0 (

ci

j

j j j

j

Lưu ý: Khi tính 0 1 1( 1) (0)

) 0 (

ci

j

j j j

j

ci R r p

) 1 ( 1

0 1

) 0

1

0 1

z y

x i

i

ci

ci ci

ci R

i i

y

i i

x i

i

ci

d k

a k

a

k R

1



i ci

r

3.3 Động năng cuả toàn bộ hệ

Đặt

(4.11)

M (n x n) là matrận quán tính cơ cấu chấp

ci

p

5.2 Di chuyển ảo

khả dĩ cuả hệ, là tập hợp bất kỳ những di chuyển vô cùng bé của các điểm thuộc hệ

mà tất cả các liên kết cho phép tại thời điểm khảo sát

5.3 Công ảo

Công ảo còn được gọi là công khả dĩ, là công phân tố (công VCB) mà lực hoặc mô men tác dụng lên hệ có thể sản ra trên di chuyển ảo của điểm đó

5.4 Lực suy rộng

- Lực suy rộng bao gồm tất cả các lực và

mô men tác dụng lên cánh tay máy phù hợp với các ràng buộc cơ học.

- Vector lực suy rộng được

xác định từ nguyên lý công ảo:

Xét trường hợp các khâu chịu tác dụng bởi hoặc lực hoặc moment tại các khớp; ngoại lực và mô men tác động vào khâu tác động cuối

Gọi:

là vector các lực suy rộng n chiều do

các thiết bị dẫn động hoặc các động cơ

tạo ra tại n khớp của tay máy;

Công ảo do hợp lực và moment này tạo ra là:

chiều của khâu tác động cuối.

x J q

Q khi tính tới lực ma sát:

ướt Trong trường hợp này lực ma sát tỷ lệ thuận với vận tốc tương đối giữa các bề mặt chuyển động tiếp xúc

Do đó Vector lực suy rộng trong trường hợp tính tới lực ma sát tại các khớp sẽ là:

6 Phương trình động lực học

Phương trình Lagrang L=K-U Thay K và U

từ (4.12) và (4.13) vào phương trình L ta được:

1 2

Đạo hàm L

1 2

q

M q

j

j ij i

q dt

dM q

M q

L dt

d

1 1

1 2

Thay (4.20)(4.21’)(4.23) vào phương trình chuyển động Lagrang (4.2):

1 2 1



(4.24)

n

ij j i i i j

T n

T n

Q Q

Q

G G

G

V V

V

], ,

[

], ,

[

], ,

[

1 1 1







Giải hệ (4.27) :

- Nếu ở Bài tóan ngược ĐLH:

là các lực suy rộng cần tác dụng vào n khớp của tay máy

- Nếu ở Bài tóan thuận ĐLH:

là vector 6 chiều biểu thị hợp lực và mô men tác dụng tại bộ tác dụng cuối

thiên theo thời gian Do đó,

cũng là đại lượng biến thiên theo thời gian

(4.27)

Mq V   G Q

Fc

]

[ 1  2  n

