35
CHƯƠNG 3 PHƯƠNG TRÌNH ĐỘNG LỰC HỌC ROBOT 3.1. Các khái niệm cơ bản Bài toán động lực học Robot chính là phân tích, tổng hợp quá trình chuyển động của các khớp và mô tả mối quan hệ giữa lực, mômen, năng lượng,… của các khớp với vị trí, tốc độ, gia tốc được biểu diễn trong phương trình chuyển động. Trong phương trình động lực học, lực và mômen là các tín hiệu vào. Dựa vào phương trình động lực học, sẽ tính được lực, mômen cần thiết để khớp Robot có thể chuyển động được với tốc độ và gia tốc mong muốn. Nghiên cứu động lực học Robot phục vụ cho các mục đích: Mô phỏng hoạt động các khớp của Robot để khảo sát, thử nghiệm quá trình làm việc đa dạng mà không cần sử dụng Robot thật, phân tích, tính toán kết cấu các khớp của Robot và thiết kế hệ thống điều khiển tay máy. Một số khái niệm cơ bản sau: - Các biến khớp được mô tả bởi vector trong không gian khớp và được mô tả bởi vector trong không gian làm việc. - Phương trình chuyển động của TM trong không gian khớp là hàm của các biến khớp có dạng như sau: - Có rất nhiều phương pháp để thành lập phương trình động lực học nhưng thường gặp hơn cả là phương pháp Newton – Euler dựa trên sự cân bằng lực, mômen trên từng thanh nối và phương pháp Euler – Lagrange dựa trên sự cân bằng về năng lượng. Cả hai phương pháp đều cùng cho một kết quả như nhau, trong luận án trình bày phương pháp Euler – Lagrange theo tài liệu để áp dụng vào đối tượng điều khiển nghiên cứu. 34

PHẦN I: NHẬP MÔN ROBOT CÔNG NGHIỆP€¦  · Web viewCả 2 phương pháp thiết kế quỹ đạo trong không gian khớp và không gian tay đều được ứng dụng

  • Upload
    others

  • View
    4

  • Download
    0

Embed Size (px)

Citation preview

Page 1: PHẦN I: NHẬP MÔN ROBOT CÔNG NGHIỆP€¦  · Web viewCả 2 phương pháp thiết kế quỹ đạo trong không gian khớp và không gian tay đều được ứng dụng

CHƯƠNG 3PHƯƠNG TRÌNH ĐỘNG LỰC HỌC ROBOT

3.1. Các khái niệm cơ bảnBài toán động lực học Robot chính là phân tích, tổng hợp quá trình chuyển động của

các khớp và mô tả mối quan hệ giữa lực, mômen, năng lượng,… của các khớp với vị trí, tốc độ, gia tốc được biểu diễn trong phương trình chuyển động. Trong phương trình động lực học, lực và mômen là các tín hiệu vào. Dựa vào phương trình động lực học, sẽ tính được lực, mômen cần thiết để khớp Robot có thể chuyển động được với tốc độ và gia tốc mong muốn. Nghiên cứu động lực học Robot phục vụ cho các mục đích: Mô phỏng hoạt động các khớp của Robot để khảo sát, thử nghiệm quá trình làm việc đa dạng mà không cần sử dụng Robot thật, phân tích, tính toán kết cấu các khớp của Robot và thiết kế hệ thống điều khiển tay máy.

Một số khái niệm cơ bản sau:

- Các biến khớp được mô tả bởi vector trong không gian khớp và được mô

tả bởi vector trong không gian làm việc.

- Phương trình chuyển động của TM trong không gian khớp là hàm của các biến khớp có dạng như sau:- Có rất nhiều phương pháp để thành lập phương trình động lực học nhưng thường gặp hơn cả là phương pháp Newton – Euler dựa trên sự cân bằng lực, mômen trên từng thanh nối và phương pháp Euler – Lagrange dựa trên sự cân bằng về năng lượng. Cả hai phương pháp đều cùng cho một kết quả như nhau, trong luận án trình bày phương pháp Euler – Lagrange theo tài liệu để áp dụng vào đối tượng điều khiển nghiên cứu. Phương trình chuyển động Lagrange của Robot nhiều bậc tự do đủ cơ cấu chấp hành ở hệ toạ độ khớp là phương trình vi phân cấp hai phi tuyến biểu diễn quan hệ giữa mômen và góc quay q, tốc độ , gia tốc , .

- Đối với một TM có n khâu, ta có: Động năng của thanh nối thứ i, được xác định:

Thế năng của thanh nối thứ i, được xác định:

- Ki và Pi đều là hàm số phụ thuộc biến khớp i: và

3.2. Mô hình động lực học Mô hình động lực học là phương trình chuyển động của Robot trong không gian. Nó

thường bao gồm các thành phần như: thành phần quán tính, thành phần tương hỗ, thành 34

Page 2: PHẦN I: NHẬP MÔN ROBOT CÔNG NGHIỆP€¦  · Web viewCả 2 phương pháp thiết kế quỹ đạo trong không gian khớp và không gian tay đều được ứng dụng

phần ly tâm và thành phần trọng trường. Ngoài ra khi Robot chuyển động luôn chịu tác động bởi lực ma sát và có cả nhiễu tác động. Do đó mà phương trình động lực học của Robot được viết dưới dạng tổng quát như sau:

(3.1)

Trong đó: : vectơ nx1 mômen sinh ra bởi cơ cấu chấp hành đặt lên khớp

(t) = [1(t), 2(t),..., n(t) ]T q: vectơ nx1 các biến khớp,

q(t) = [q1(t), q2(t),..., qn(t) ]T

: vectơ nx1 tốc độ thay đổi các biến khớp (vận tốc góc, vận tốc thẳng),

,

: vectơ nx1 gia tốc biến khớp,

,

M(q): ma trận quán tính (nxn) trong hệ toạ độ khớp có các thành phần (i,j), : vectơ tương hỗ và ly tâm (nx1),

G(q): vectơ trọng trường (nx1).G = [G1,G2, .... Gn]T

Fd: vectơ lực ma sát động,Fs : vectơ nx1 hằng số ma sát nhớt,d : vectơ nx1 mô tả nhiễu ngoại chưa biết.Phương trình (3.9) viết lại, được mô tả bởi:

(3.2)trong đó:

(3.3)

(3.4)

Phương trình động lực học được viết lại theo đặc điểm của phương pháp thích nghi Craig (1985), được xác định:

(3.5)

35

Page 3: PHẦN I: NHẬP MÔN ROBOT CÔNG NGHIỆP€¦  · Web viewCả 2 phương pháp thiết kế quỹ đạo trong không gian khớp và không gian tay đều được ứng dụng

- ma trận (nxr) biến đổi theo t đã biết, có thể xác định được từ các đại lượng đo được,

- vector rx1 gồm các tham số hằng số hoặc thay đổi cần xác định. Đặc điểm của phương pháp thích nghi Craig (1985), theo tài liệu [29] là công thức được mô tả trong bảng phân tích thông số chưa biết và hàm truyền thời gian. Lý do của việc phân tích động lực học Robot có thể phân tích trong dạng này là động lực học robot tuyến tính trong bảng thông số mô tả bởi vectơ . Trong phần các thông số chưa biết và các hàm truyền thời gian sẽ được sử dụng trong công thức của luật cập nhật thích nghi cũng để phân tích sự ổn định sai số bám của hệ thống.3.3. Thuộc tính của phương trình động lực học

- Ma trận quán tính M(q): là ma trận đối xứng và xác định dương, có các đặc tính sau:

Động năng của Robot được xác định từ ma trận quán tính:

(3.6)

Ma trận quán tính là hàm của biến khớp q, nếu là khớp quay thì biến khớp q xuất hiện trong ma trận quán tính sẽ là cos(q) và sin(q) mà và , do đó M(q) cũng bị chặn bởi:

(3.7)

trong đó:

- thành phần của ma trận bị chặn có thể tính được và được dùng để xác định

giá trị riêng lớn nhất và nhỏ nhất của M(q).

(3.8)

Mặt khác nếu là khớp tịnh tiến thì do kết cấu cơ khí và do yêu cầu công nghệ thì khoảng tịnh tiến d cũng bị chặn bởi:

(3.9)

trong đó: m1, m2 được xác định trong từng trường hợp cụ thể phụ thuộc vào kết cấu cơ khí và yêu cầu công nghệ.

- Thành phần tương hỗ và thành phần ly tâm H(q, ): H(q, ) biểu thị các quan hệ giữa lực động với tốc độ thay đổi giá trị biến khớp. Thành

phần tương hỗ và ly tâm có thể được viết dưới dạng sau:

36

(3.10)

Page 4: PHẦN I: NHẬP MÔN ROBOT CÔNG NGHIỆP€¦  · Web viewCả 2 phương pháp thiết kế quỹ đạo trong không gian khớp và không gian tay đều được ứng dụng

trong đó:

S(q, ): ma trận đối xứng lệch, xác định bởi:

H(q, ) là hàm bậc hai của đạo hàm các biến khớp. Nếu khớp của Robot là khớp quay thì trong H(q, ) có hàm sin(q) và cos(q) do đó H(q, ) cũng bị chặn. Nếu khớp của Robot là khớp tịnh tiến thì cũng do kết cấu cơ khí và yêu cầu công nghệ nên H(q, ) cũng bị chặn được xác định bởi:

(3.11)

trong đó: vb(q) – một hàm vô hướng của q

hoặc:

H(q) được xác định như sau:

(3.12)

Để xác định được với giả thiết là hàm thỏa mãn với điều kiện:

- Thành phần trọng trường G(q):Robot luôn chịu tác dụng của lực trọng trường và phụ thuộc vào biến khớp q. Và

G(q) cũng bị chặn được xác định bởi:

(3.13)

Trong đó: gb là hàm vô hướng được xác định cụ thể cho từng robot.- Thành phần lực ma sát F( ):

Thành phần ma sát xuất hiện khi Robot chuyển động và gồm hai thành phần là lực ma sát nhớt và lực ma sát động. Và các hệ số ma sát này xác định rất khó khăn nên người ta thường đưa ra dưới dạng gần đúng như sau:

(3.14)

Ma sát nhớt thường được giả định:

: - hệ số không đổi,

: - ma trận đường chéo

37

Page 5: PHẦN I: NHẬP MÔN ROBOT CÔNG NGHIỆP€¦  · Web viewCả 2 phương pháp thiết kế quỹ đạo trong không gian khớp và không gian tay đều được ứng dụng

Ma sát động thường được giả định:

ki: - hệ số không đổi,Hàm dấu sgn được xác định:

Hệ số ma sát động xác định:

Hàm dấu xác định:

Vậy các điều kiện ràng buộc của các ma sát:

(3.15)

Thành phần ma sát tĩnh có thể có trong được xác định:

(3.16)

- Thành phần nhiễu là véctơ nx1 mô tả nhiễu ngoại chưa biết được xác định: ,

d: hàm vô hướng không đổi, được tính trong trường hợp Robot cụ thể.- Các tham số động lực học: Các tham số hệ thống như khối lượng mỗi khớp m i, và mômen quán tính In xuất hiện trong phương trình động lực học của Robot.

(3.17)

Nhận xét : Từ phương trình động lực học tổng quát và thuộc tính của phương trình động

lực học của Robot cho thấy : mô hình động lực học của Robot công nghiệp là mô hình

phi tuyến có các tham số hằng bất định, có tác động thành phần tác độngxen kênh giữa các

khớp, tất cả các yếu tố ảnh hưởng đó làm cho chuyển động của TMCN mất ổn định và tồn

tại sai lệch lớn giữa vị trí khớp đặt và vị trí khớp thực.

3.4. Cách thành lập phương trình động lực họcMômen đặt lên trục khớp i được xác định theo phương trình Largange bậc hai có dạng:

Khớp i là khớp tịnh tiến ri , lực đặt theo chuyển động của khớp Fi

(3.18)

Khớp i là khớp quay θi , momen đặt xung quanh khớp Mi

38

Page 6: PHẦN I: NHẬP MÔN ROBOT CÔNG NGHIỆP€¦  · Web viewCả 2 phương pháp thiết kế quỹ đạo trong không gian khớp và không gian tay đều được ứng dụng

(3.19)

Trong đó: L = K – P: Phương trình Largange (3.20)

K: Tổng động năng của thanh nối, P: Tổng thế năng của thanh nối, i : Moment đặt lên khớp i khi thực hiện chuyển động quay, (nx1) qi, : Biến khớp,

: Vận tốc quay của biến khớp.

Thiết lập phương trình động lực học thực hiện các bước sau:

Bước 1: Tính động năng của các thanh nối được xác định bởi:

(3.21)

Trong đó: ci :Trọng tâm thanh nối li : Chiều dài thanh nối mi : Khối lượng thanh nối si : Khoảng cách từ trục quay đến ci

vci :Tốc độ dài của ci

wi : Tốc độ góc thanh nối Ii : Ma trận momen quán tính, được xác định:

(3.22)

39

Hình 3.1. Biểu diễn động năng của một thanh nối

Page 7: PHẦN I: NHẬP MÔN ROBOT CÔNG NGHIỆP€¦  · Web viewCả 2 phương pháp thiết kế quỹ đạo trong không gian khớp và không gian tay đều được ứng dụng

Bước 2: Tính thế năng của các thanh nối , được xác định bởi:

(3.23)

Bước 3: Thiết lập hàm Lagrange (3.20)

Bước 4: Thiết lập phương trình Lagrange (3.18) hoặc (3.19).3.5. Ví dụ minh họa

Xây dựng phương trình động học thuận và động lực học cho Robot Almega 16.

- Thiết lập bài toán động học thuận cho Robot Almega 16

Các bước thực hiện bài toán động học thuận cho robot Almega 16:

Xác định số khớp và số thanh nối3 khớp đầu cảu robot Almega 16 là 3 khớp quay với 4 thanh nối và đế

Gắn lên các thanh nối các hệ trục tọa độ như hình vẽ trên.

40

a2 a3

Hình 3.2. Robot Almega16 có gắn các hệ trục tọa độ

O0

O1 O2 O3

X0

X1

Z2

X2

d1

1

2 3

Z1

Z0

X3

Z3C0

C1

C2

lC1

lC2 lC3

Y0

Y1 Y2 Y3

Page 8: PHẦN I: NHẬP MÔN ROBOT CÔNG NGHIỆP€¦  · Web viewCả 2 phương pháp thiết kế quỹ đạo trong không gian khớp và không gian tay đều được ứng dụng

Các thông số động học được xác định theo phương pháp Danevit-Hartenberg theo bảng sau:

Từ bảng thông số D – H trên ta tính được các Ai như sau:

(3.24)

(3.25)

(3.26)

Nhân các ma trận ta được ma trận biểu diễn vị trí và hướng của khâu tác

động cuối của Robot như sau:

(3.27)

Trong đó: cosθ23 = cos(θ2 + θ3)

- Thiết lập mô hình động lực học cho Robot ALMEGA 16

Bước 1: Tính động năng của các thanh nối

Tính động năng thanh nối thứ nhất

41

θi di ai αi

1 θ*1 d1 0 900

2 θ*2 0 a2 0

3 θ*3 0 a3 0

Page 9: PHẦN I: NHẬP MÔN ROBOT CÔNG NGHIỆP€¦  · Web viewCả 2 phương pháp thiết kế quỹ đạo trong không gian khớp và không gian tay đều được ứng dụng

(3.28)

Tính động năng thanh nối thứ hai

(3.29)

Tính động năng thanh nối thứ ba

(3.30)

Bước 2: Tính thế năng của các

Tính thế năng thanh nối thứ nhất:

(3.31)

Tính thế năng thanh nối thứ hai:

42

Page 10: PHẦN I: NHẬP MÔN ROBOT CÔNG NGHIỆP€¦  · Web viewCả 2 phương pháp thiết kế quỹ đạo trong không gian khớp và không gian tay đều được ứng dụng

(3.32)

Tính thế năng thanh nối thứ ba

(3.33)

Bước 3: Thiết lập hàm Lagrange

Phương trình Lagrange như sau:

(3.34)

(3.35)

Bước 4: Thiết lập phương trình Lagrange

Ta có phương trình:

Tính toán từng thành phần trong phương trình

43

Page 11: PHẦN I: NHẬP MÔN ROBOT CÔNG NGHIỆP€¦  · Web viewCả 2 phương pháp thiết kế quỹ đạo trong không gian khớp và không gian tay đều được ứng dụng

(

Như vậy xác định được:

(3.36)

(3.37)

44

Page 12: PHẦN I: NHẬP MÔN ROBOT CÔNG NGHIỆP€¦  · Web viewCả 2 phương pháp thiết kế quỹ đạo trong không gian khớp và không gian tay đều được ứng dụng

(3.38)

Phương trình động lực học dưới dạng ma trận trạng thái (3.39)

(3.40)

Trong đó:

(3.41)

(3.42)

(3.43)

45

Page 13: PHẦN I: NHẬP MÔN ROBOT CÔNG NGHIỆP€¦  · Web viewCả 2 phương pháp thiết kế quỹ đạo trong không gian khớp và không gian tay đều được ứng dụng

BÀI TẬP

Bài 1. Cho robot như sau, hãy thiết lập phương trình động lực học của Robot

Bài 2. Cho một Robot có 5 khâu phẳng, cấu hình RRRRR. Hãy thiết lập phương trình động lực học của Robot

Bài 3: Cho một Robot như hình vẽ. Hãy thiết lập phương trình động lực học của Robot

Bài 4: Cho một Robot như hình vẽ. Hãy thiết lập phương trình động lực học của Robot

46

r1

4

Page 14: PHẦN I: NHẬP MÔN ROBOT CÔNG NGHIỆP€¦  · Web viewCả 2 phương pháp thiết kế quỹ đạo trong không gian khớp và không gian tay đều được ứng dụng

Bài 5: Cho một Robot có 2 khâu phẳng, cấu hình TT. Hãy thiết lập phương trình động học thuận của Robot

Bài 6: Cho một Robot có 2 khâu phẳng, cấu hình RT. Hãy thiết lập phương trình động lực học của Robot

Bài 7: Cho một Robot có 3 khâu phẳng, cấu hình RTR. Hãy thiết lập phương trình động lực học của Robot:

47

Page 15: PHẦN I: NHẬP MÔN ROBOT CÔNG NGHIỆP€¦  · Web viewCả 2 phương pháp thiết kế quỹ đạo trong không gian khớp và không gian tay đều được ứng dụng

CHƯƠNG 4THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG ROBOT CÔNG NGHIỆP

4.1. Khái niệm chung

Đường di chuyển của TM biểu thị trình tự di chuyển của cơ cấu Robot từ điểm này đến điểm khác trong không gian. Đường di chuyển đơn thuần là trình tự thay đổi cấu hình Robot mà không bao hàm yếu tố thời gian. Còn quỹ đạo chuyển động biểu thị sự di chuyển của cơ cấu Robot theo thời gian trong đoạn đường di chuyển. Như vậy quỹ đạo chuyển động sẽ phụ thuộc vào tốc độ và gia tốc của cơ cấu TM.

Có 2 phương pháp mô tả chuyển động của TM là mô tả trong không gian khớp và trong không gian tay (không gian làm việc hay còn gọi là không gian Đecac).

Phương pháp mô tả trong không gian khớp, chuyển động của cơ cấu TM được xác định thông qua các giá trị biến khớp. Đặc điểm của phương pháp mô tả trong không gian khớp là có thể đảm bảo được tay TM đạt đến vị trí đặt trước một cách chính xác, nhưng đường di chuyển giữa hai điểm không thể dự báo trước được.

Phương pháp mô tả trong không gian tay sẽ biểu diễn chuyển động của Robot giữa các điểm bằng các đoạn di chuyển xác định trong không gian tay và được biến đổi về không gian khớp tương ứng thông qua giải bài toán động học ngược. Giả sử để tay TM di chuyển từ điểm A đến điểm B theo một đường thẳng, ta cần chia đoạn AB thành nhiều đoạn nhỏ và điều khiển TM di chuyển bám theo các đoạn nhỏ đó. Phương pháp thực hiện là tính toán ra tập hợp vị trí khớp tương ứng với các đoạn nhỏ và điều khiển khớp bám theo các vị trí đặt tương ứng. Khi tất cả các đoạn di chuyển nhỏ được xác định, Robot sẽ di chuyển từ A đến B theo đúng quỹ đạo thiết kế.

Cả 2 phương pháp thiết kế quỹ đạo trong không gian khớp và không gian tay đều được ứng dụng điều khiển TM trong công nghiệp. Mỗi phương pháp đều có ưu điểm và nhược điểm riêng. Phương pháp mô tả chuyển động trong không gian tay dễ dàng quan sát được và đảm bảo tay Robot di chuyển đúng quỹ đạo, tuy nhiên nhược điểm của nó là khối lượng tính toán lớn và yêu cầu thời gian xử lý tính toán nhanh.

4.2. Thiết kế quỹ đạo trong không gian khớpThông thường quỹ đạo ở dạng đa thức bậc cao sẽ đáp ứng được các yêu cầu về vị trí,

tốc độ và gia tốc ở mỗi điểm giữa hai đoạn di chuyển. Phần lý thuyết sau đây sẽ trình bày một số phương pháp thiết kế quỹ đạo trong không gian khớp, xác định đường biểu diễn vị trí khớp theo thời gian.

48

Page 16: PHẦN I: NHẬP MÔN ROBOT CÔNG NGHIỆP€¦  · Web viewCả 2 phương pháp thiết kế quỹ đạo trong không gian khớp và không gian tay đều được ứng dụng

4.2.1. Quỹ đạo dạng đa thức bậc 3Bài toán thiết kế quỹ đạo cho khớp ở đây là xácđịnh đường biểu diễn vị trí khớp (góc

quay hoặc độ dịch chuyển tịnh tiến) theo thời gian khi di chuyển từ vị trí ban đầu q0 đến vị trí cuối cùng qc trong thời gian tc, với q là biến khớp tổng quát. Quỹ đạo di chuyển sẽ thoả mãn 4 điều kiện:

- Vị trí ban đầu.- Vị trí cuối cùng.- Tốc độ ban đầu.- Tốc độ cuối cùng. Sử dụng đa thức bậc 3 cho quỹ đạo của khớp:

q(t) = a0 + a1*t + a2*t2 + a3*t3 (4.1)Các điều kiện ban đầu và cuối cùng với giả thiết tốc độ ban đầu và cuối bằng 0: q(t0) = q0

q(tc) = qc

(4.2) Đạo hàm phương trình (4.1) ta nhận được phương trình tốc độ khớp theo thời gian:

= a1 + 2*a2*t + 3*a3*t2 (4.3)Sử dụng các điều kiện đầu và cuối (4.2) kết hợp với (4.3) ta nhận được hệ phương

trình sau: q(t0) = a0 = q0

q(tc) = a0 + a1*tc + a2*tc2 + a3*tc

3 = qc (4.4)

= a1 = 0 = a1 + 2*a2*tc + 3*a3*tc

2 = 0

Giải hệ phương trình (4.4) thu được 4 hệ số a0, a1, a2, a3

Ví dụ: Thiết kế quỹ đạo chuyển động cho một khớp Robot quay từ góc ban đầu 300

đến vị trí cuối cùng 600 trong thời gian 5s. Sử dụng hệ phương trình (4.4) ta có:q(0) = a0 = q0 = 30q(5) = a0 + a1*5 + a2*52 + a3*53

= 60 = a1 = 0= a1 + 2*a2*5 + 3*a3*52

= 0Giải hệ trê thu được: a0 = 30; a1 = 0; a2 = 3.6; a3 = -0.48Phương trình biểu diễn góc khớp, tốc độ góc và gia tốc góc lần lượt là:

49

Page 17: PHẦN I: NHẬP MÔN ROBOT CÔNG NGHIỆP€¦  · Web viewCả 2 phương pháp thiết kế quỹ đạo trong không gian khớp và không gian tay đều được ứng dụng

q(t) = 30 + 3.6*t2 - 0.48*t3 = 7.2*t - 0.48*t2

= 7.2 - 2.88*tĐoạn chương trình sau cho ta kết quả dưới dạng đồ thị trong Matlab:>> t=0:0.01:5;>> q=30+3.6*t.^2-0.48*t.^3;>> dq=7.2*t-1.44*t.^2;>> ddq=7.2-2.88*t;>> figure(1);subplot(2,1,1);grid on; hold on;>> xlabel('times-seconds');ylabel('rotation angle');>> hold on;>> plot(t,q);>> hold on;>> subplot(2,1,2);grid on; hold on;>> xlabel('times-seconds');>> hold on;>> plot(t,dq,t,ddq,'r');

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 530

40

50

60

times-seconds

rota

tion

angl

e

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5-10

-5

0

5

10

times-seconds

dqddq

Hình 4.1. Đồ thị góc quay, vận tốc và gia tốc góc khớp theo thời gian

4.2.2. Thiết kế quỹ đạo theo đa thức bậc 5Phương pháp thiết kế quỹ đạo sử dụng đa thức bậc 3 thoả mãn 4 điều kiện đầu và cuối về vị trí và tốc độ. Để xét đến điều kiện đầu và cuối về gia tốc ta cần sử dụng đến quỹ đạo đa thức bậc 5:

q(t) = a0 + a1*t + a2*t2 + a3*t3 + a4*t4 + a5*t5

50

Page 18: PHẦN I: NHẬP MÔN ROBOT CÔNG NGHIỆP€¦  · Web viewCả 2 phương pháp thiết kế quỹ đạo trong không gian khớp và không gian tay đều được ứng dụng

= a1 + 2*a2*t + 3*a3*t2 + 4*a4*t3 + 5*a5*t4

= 2*a2 + 6*a3*t + 12*a4*t2 + 20*a5*t3

Hệ phương trình trên cho phép ta xác định được 6 hệ số của quỹ đạo dựa vào 6 điều kiện biên.

4.2.3. Quỹ đạo dạng 2-1-2Đây là dạng quỹ đạo chuyển động mà đồ thị tốc độ khớp có dạng hình thang với giả

thiết tốc độ điểm đầu và cuối bằng 0, gia tốc chuyển động là hằng số ở giai đoạn khởi động và hãm. Để đảm bảo tính đối xứng của quỹ đạo, điểm trung bình được chọn:

qm = và tm =

Quỹ đạo phải thoả mãn các điều kiện ràng buộc: - Vị trí khớp biến đổi từ điểm đầu q0 tới điểm cuối qc trong thời gian tc

- Tốc độ cuối đoạn Parabol bằng tốc độ không đổi của đoạn tuyến tính

(4.5)

Phương trình biểu diễn đoạn khởi động (0-t1) có dạng Parabol:q(t) = q0 + 0.5* *t2

Do đó vị trí của khớp tại thời điểm t1 là:q1(t1) = q0 + 0.5* *t1

2 (4.6)

Kết hợp (4.5) và (4.6) ta nhận được phương trình sau:*t1

2 - *tc*t1 + qc - q0 = 0 (4.7)Giải phương trình (4.7) tính được khoảng thời gian t1 như sau:

t1 = (4.8)

Từ (4.8) gia tốc khớp trong giai đoạn khởi động và hãm thoả mãn điều kiện:

(4.9)

Mặt khác gia tốc chuyển động phải nhỏ hơn gia tốc cho phép từ độ bền cơ khí nên gia tốc được chọn phải thoả mãn bất đẳng thức:

(4.10)

51

Page 19: PHẦN I: NHẬP MÔN ROBOT CÔNG NGHIỆP€¦  · Web viewCả 2 phương pháp thiết kế quỹ đạo trong không gian khớp và không gian tay đều được ứng dụng

Như vậy từ (4.10) ta xác định được gia tốc khớp, từ (4.8) xác định được t1 và từ (4.6) xác định được q1. Với các thông số đó, ta xây dựng được phương trình quỹ đạo chuyển động của khớp Robot như sau:

(4.11)

Đồ thị sau cho ta quỹ đạo dạng 2-1-2 với góc ban đầu là 300, góc cuối cùng là 600, thời gian chuyển động là 5 (s), gia tốc giới hạn là 10 rad/s2. Chương trình được cho ở phụ lục.

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 530

35

40

45

50

55

60

Thoi gian (s)

Goc

qua

y

Hình 4.2. Đồ thị góc quay khớp theo thời gian

52

Page 20: PHẦN I: NHẬP MÔN ROBOT CÔNG NGHIỆP€¦  · Web viewCả 2 phương pháp thiết kế quỹ đạo trong không gian khớp và không gian tay đều được ứng dụng

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 50

1

2

3

4

5

6

7

8

Thoi gian (s)

Van

toc

goc

(rad/

s)

Hình 4.3. Đồ thị vận tốc góc theo thời gian

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5-8

-6

-4

-2

0

2

4

6

8

Thoi gian (s)

Gia

toc

goc

(rad/

s2 )

Hình 4.4. Đồ thị gia tốc theo thời gian

4.2.4. Quỹ đạo dạng 4-1-4

Dạng quỹ đạo 2-1-2 đơn giản và dễ thực hiện, tuy nhiên tốc độ khớp ở thời điểm đầu và cuối của giai đoạn khởi động và hãm thay đổi không được êm. Dạng quỹ đạo 4-1-4 sẽ

53

Page 21: PHẦN I: NHẬP MÔN ROBOT CÔNG NGHIỆP€¦  · Web viewCả 2 phương pháp thiết kế quỹ đạo trong không gian khớp và không gian tay đều được ứng dụng

tạo ra sự thay đổi tốc độ êm hơn, trong đó quỹ đạo ở giai đoạn khởi động và hãm có dạng đa thức bậc 4. Quỹ đạo chuyển động được xây dựng như sau :

Hình 4.5. Xây dựng quỹ đạo 4-1-4

Thỏa mãn các điều kiện đầu và cuối về vị trí, tốc độ và gia tốc của khớp :

Để đơn giản việc tính toán có thể chọn tham số t0 bằng 1/2 khoảng thời gian gia tốc hoặc giảm tốc và tốc độ, gia tốc ban đầu và cuối cùng bằng 0. Ta cũng xác định được các điểm phụ của quỹ đạo chuyển động q01 và q02 tại t = t0 và t = tc - t0 là :

q01 =q0 và q02 = qc

Nối q01 và qc2 bằng 1 đường thẳng và xác định điểm q02, qc1 tại t = 2t0 và t = tc-2t0

Bằng cách chọn điểm phụ như trên, quỹ đạo đoạn cd là một đường thẳng với tốc độ không đổi, quỹ đạo ac và df có thể chọn là đa thức bậc 4 có dạng :

q(t) = a0 + a1t + a2t2 + a3t3 + a4t4

* Xác định giá trị q02 và qc1 :Do be là đường thẳng, giá trị q02 và qc1 có thể được xác định từ giá trị đầu q0 và giá trị

cuối qc dựa trên phương trình bậc nhất của đường thẳng be :

54

Page 22: PHẦN I: NHẬP MÔN ROBOT CÔNG NGHIỆP€¦  · Web viewCả 2 phương pháp thiết kế quỹ đạo trong không gian khớp và không gian tay đều được ứng dụng

Với A =

Hệ số góc của đường thẳng cd là tốc độ chuyển động khớp trong đoạn cd.* Phương trình đoạn cd: Quỹ đạo cd là đường thẳng biểu diện bởi phương trình :

qcd = A(t - 2t0) + q02

với => qcd = a0cd +a1cd(t - 2t0)

Cho các hệ số a0cd, a1cd của phương trình đoạn cd được xác đinh như sau :a0cd = q0 +At0

a1cd = A* Phương trình đoạn ac :Quỹ đạo ac biểu diễn bởi phương trình dạng đa thức bậc 4 :

qac = a0ac + a1act + a2act2 + a3act3 + a4act4

Các hệ số của phương trình đoạn ac là a0ac, a1ac, a2ac, a3ac, a4ac được xác định từ điều kiện đầu và điều kiện cuối :

Tại t = 0:

Tại t = 2t0 :

Các hệ số được xác định như sau :a0ac = q0

a1ac = 0a2ac = 0

55

Page 23: PHẦN I: NHẬP MÔN ROBOT CÔNG NGHIỆP€¦  · Web viewCả 2 phương pháp thiết kế quỹ đạo trong không gian khớp và không gian tay đều được ứng dụng

* Phương trình đoạn df :Quỹ đạo đoạn df biểu diễn bởi phương trình bậc bốn như quỹ đạo đoạn ac :

Với : Các điều kiện đầu và cuối cùng cho tính toán các hệ số là :

Sử dụng điều kiện đầu và cuối, tương tự như phương pháp tính phương trình đoạn ac, các hệ số của phương trình qdf được xác định như sau :

* Đoạn chương trình khảo sát quỹ đạo 4-1-4 bằng Matlab với góc ban đầu là 300, góc cuối cùng là 600, thời gian chuyển động là 5 (s) được cho ở phụ lục. Kết quả cho duới dạng đồ thị dưới đây.

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 530

35

40

45

50

55

60

Thoi gian (s)

Goc

qua

y

Hình 4.6. Đồ thị góc quay theo thời gian với quỹ đạo 4-1-4

56

Page 24: PHẦN I: NHẬP MÔN ROBOT CÔNG NGHIỆP€¦  · Web viewCả 2 phương pháp thiết kế quỹ đạo trong không gian khớp và không gian tay đều được ứng dụng

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 50

5

10

Thoi gian (s)

Van

toc

goc

(rad/

s)

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5-10

-5

0

5

10

Thoi gian (s)

Gia

toc

goc

(rad/

s2 )

Hình 4.7. Đồ thị vận tốc và gia tốc góc theo thời gian của quỹ đạo 4-1-4

4.3. Thiết kế quỹ đạo cho tay robot trong hệ tọa độ Decac

Như mục trên đã trình bày, phương pháp xây dựng quỹ đạo chuyển động cho khớp TM không thích hợp biểu diễn TM khi nó thực hiện nhiệm vụ trong môi trường làm việc vì không cho phép tách rời hướng và vị trí tay máy. Trong những hệ thống robot phức tạp, ngôn ngữ lập trình được sử dụng cho điều khiển TM thực hiện nhiệm vụ trong dây chuyền sản xuất. Trong trường hợp như vậy, quá trình làm việc của tay TM là di chuyển giữa các điểm đã được đặt trước trong môi trường làm việc với đường di chuyển giữa các điểm dọc theo quỹ đạo được thiết kế cho điều khiển các khớp TM. Một chu kỳ điều khiển có thể được mô tả như sau:

(1) Tăng thời gian bằng t = t+ (2) tính vị trí và hướng của tay dựa trên dạng quỹ đạo chuyển động của robot.(3) tính vị trí tương ứng của các khớp bằng sử dụng phương trình động học ngược.(4) Gửi thông tin tới các bộ điều khiển vị trí các khớp robot.(5) Quay lại bước đầu tiên

Một cách tổng quát để tay robot thực hiện chuyển động giữa điểm ban đầu tới diểm cuối cùng theo một đường thẳng, cần phải xác định phép biến đổi từ vị trí đầu tới vị trí cuối và chia thành các đoạn nhỏ. Phép biến đổi tổng quát R giữa cấu hình tay ở vị trí ban đầu (T0) đến vị trí cuối cùng (Tc) là : Tc = T0R hoặc R = T0

-1Tc

Ba phương pháp khác nhau được sử dụng để biến đổi phép biến đổi tổng thành các đoạn nhỏ :

57

Page 25: PHẦN I: NHẬP MÔN ROBOT CÔNG NGHIỆP€¦  · Web viewCả 2 phương pháp thiết kế quỹ đạo trong không gian khớp và không gian tay đều được ứng dụng

Chia chuyển động giữa các điểm thành một số lớn đoạn tương ứng với số các chuyển độngvi sai. Sử dụng các phương trình dịch chuyển vi sai, ta xác định được vị trí và hướng tay trong mỗi đoạn nhỏ thông qua ma trận Jacobien :

DT6 = J hoặc =J-1.DT6

dT = Tmới = Tcũ +dT Phép biến đổi R có thể được thực hiện bằng một phép biến đổi tịnh tiến và hai

phép biến đổi quay. phép tịnh tiến thực hiện dịch chuyển khung tọa độ tay từ vị trí ban đầu đến vị trí cuối cùng. Phép quay thứ nhất nhằm đặt khung tọa độ tay ở vị trí mong muốn và phép quay thứ 2 xung quanh trục khung tọa độ để đạt được hướng cuối cùng của tay.

Phép biến đổi R được thực hiện bằng hai phép biến đổi: phép tịnh tiến để đưa khung tọa độ tay đến vị trí cuối cùng và phép biến đổi xoay xung quanh vector nhằm đạt được hướng mong muốn của tay.

BÀI TẬP

1. Thiết kế quỹ đạo chuyển động dạng đa thức bậc 3 cho khớp 1 của Robot 6 bậc tự do từ góc ban đầu 500 đến góc cuối cùng 800 trong thời gian 3s. Xác định góc quay, tốc độ và gia tốc góc của khớp ở thời điểm 1,2,3 s. Biết rằng Robot chuyển động từ trạng thái đứng yên và dừng ở vị trí cuối cùng.

2. Xây dựng quỹ đạo dạng 2-1-2 của khớp 1 robot quay từ góc ban đầu 400 đến góc cuối cùng 1200 trong 4s với tốc độ lớn nhất là 100/s. Vẽ đường biểu diễn vị trí, tốc độ và gia tốc chuyển động của khớp.

3. Thiết kế quỹ đạo chuyển động dạng 2-1-2 cho Robot 3 bậc tự do từ góc ban đầu 500 đến góc cuối cùng 800 trong thời gian 3s. Xác định góc quay, tốc độ và gia tốc góc của khớp ở thời điểm 1,2,3 s. Biết rằng Robot chuyển động từ trạng thái đứng yên và dừng ở vị trí cuối cùng.4. Thiết kế quỹ đạo dạng 2-1-2 cho một khớp quay của robot từ góc ban đầu q 1 = 600

tới góc cuối q3 = 100 qua điểm trung gian q2 = 400

. Biết td1 = td3 = 3s, trị tuyệt đối gia tốc tại các điếm cuối là 600/s.

5. Thiết kế quỹ đạo dạng bậc 3 cho một khớp quay của robot từ góc ban đầu q1 = 100 tới

góc cuối q3 = 600 qua điểm trung gian q2 = 400 , biết td1 = td3 = 3s.

58