Thiết kế bộ điều khiển PID trượt cánh tay mềm dẻo hai bậc tự do

Bộ điều khiển này sẽ thừa hưởng được các ưu điểm của phương pháp điều khiển PID như triệt tiêu sai số xác lập, đáp ứng nhanh, giảm vọt lố, nó kết hợp giữa bộ điều khiển có cấu trúc thay

TRƯỜNG ĐẠI HỌC BÁCH KHOA

CÔNG TRÌNH ĐƯỢC HOÀN THÀNH TẠI TRƯỜNG ĐẠI HỌC BÁCH KHOA ĐẠI HỌC QUỐC GIA TP HỒ CHÍ MINH

Cán bộ hướng dẫn khoa học:………

Tp HCM, ngày … tháng … năm 2010

NHIỆM VỤ LUẬN VĂN THẠC SĨ

Họ và tên học viên: PHẠM VĂN TRỌNG Phái: Nam

Ngày, tháng, năm sinh: 18 – 3 – 1982 Nơi sinh: Gia Lai

Chuyên ngành: Tự động hóa MSHV: 01506380

I TÊN ĐỀ TÀI: THIẾT KẾ BỘ ĐIỀU KHIỂN PID TRƯỢT CÁNH

TAY MỀM DẺO 2 BẬC TỰ DO

II NHIỆM VỤ VÀ NỘI DUNG:

1 Thiết kế bộ điều khiển PID trượt

2 Lập trình mô phỏng hệ cánh tay máy mềm dẻo bằng Simulink và file_M của

Matlab điều khiển mô phỏng mô hình toán và điều khiển thực trên mô hình

3 Kiểm chứng giải thuật và so sánh điều khiển giữa mô hình thực và mô hình

toán

III NGÀY GIAO NHIỆM VỤ : 25 / 06 / 2009

IV NGÀY HOÀN THÀNH NHIỆM VỤ:…… /………/ 2010

TÓM TẮT

Các robot thông thường được thiết kế với cánh tay máy đủ cứng (độ biến dạng của

nó là không đáng kể khi hoạt động) cho phép điều khiển đơn giản với các khớp nên kích thước và trọng lượng lớn Điều này làm cho các robot hoạt động tiêu tốn năng lượng hơn đồng thời tốc độ hoạt động không cao do có quán tính lớn

Robot với các cánh tay mềm dẻo đang được phát triển để khắc phục các nhược điểm trên, nó cho phép hoạt động với tốc độ cao và hiệu suất hơn bởi kích thước và khối lượng nhỏ Tuy vậy, để điều khiển được các cánh tay mềm dẻo này là rất phức tạp nên chưa sử dụng nhiều trong thực tế

Luận văn đưa ra bộ điều khiển PID có cấu trúc thay đổi đối với mặt trượt PID Bộ điều khiển này sẽ thừa hưởng được các ưu điểm của phương pháp điều khiển PID như triệt tiêu sai số xác lập, đáp ứng nhanh, giảm vọt lố, nó kết hợp giữa bộ điều khiển có cấu trúc thay đổi và mặt trượt PID để điều khiển các góc quay của cánh tay mềm dẻo

MỤC LỤC

Chương 1: Tổng quan 1

Chương 2: Cơ sở lý thuyết .4

2.1 Bộ điều khiển PID .4

2.2 Bộ điều khiển trượt 7

Chương 3: Thiết kế bộ điều khiển .12

3.1 Khảo sát mô hình cánh tay mềm dẻo 2 bậc tự do .12

3.1.1 Phương trình động học 15

3.1.1.1 Phương trình động học thuận .15

3.1.1.2 Phương trình động học ngược .17

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

3.2 Bộ điều khiển PID trượt 35

3.2.1 Điều kiện trượt 36

3.2.2 Ổn định tiệm cận toàn cục của hệ cánh tay mềm dẻo 2 bậc tự do .38

Chương 4: Mô phỏng hệ thống điều khiển dùng Matlab .47

4.1 Mô phỏng cánh tay mềm dẻo 1 bậc tự do 47

4.1.1 Mô phỏng cánh tay mềm dẻo với 1 trục X .48

4.1.1.1 Sơ đồ Simulink .49

4.1.1.2 Kết quả mô phỏng 51

4.1.2 Mô phỏng cánh tay mềm dẻo với 1 trục Y .60

4.1.2.1 Sơ đồ Simulink .60

4.1.2.2 Kết quả mô phỏng 62

4.2 Mô phỏng cánh tay mềm dẻo 2 bậc tự do 72

4.2.1 Sơ đồ Simulink .72

4.2.2 Kết quả mô phỏng 73

Chương 5: Kết luận và hướng phát triển của đề tài 77

Tài liệu tham khảo 78

Phụ lục 81

Hiện nay trên thế giới đã có rất nhiều nghiên cứu về robot và cũng đã có rất nhiều robot được chế tạo và ứng dụng vào quá trình sản xuất như các robot hàn trong nhà máy sản xuất ô tô, các robot lắp ráp linh kiện trong dây chuyền sản xuất board mạch, robot cấp phôi trong các máy gia công chi tiết cơ khí Tuy nhiên, ở Việt Nam thì việc nghiên cứu và chế tạo robot mới ở giai đoạn bắt đầu

và hầu như các robot này chưa có tính thích ứng với môi trường thay đổi mà chủ yếu hoạt động theo một chương trình định trước

Việc nghiên cứu các bộ điều khiển để nâng cao độ chính xác của robot hiện vẫn còn đang được các nhà khoa học trong và ngoài nước quan tâm rất nhiều

Tay máy mềm dẻo là một đối tượng có độ phi tuyến rất cao do đó việc thiết kế bộ điều khiển cho đối tượng này là khá phức tạp Như đã biết, điều khiển với cấu trúc thay đổi (sliding mode control) là phương pháp hữu hiệu để điều

khiển cho các đối tượng phi tuyến bởi luật điều khiển hồi tiếp phi tuyến Trong những năm gần đây, hệ thống điều khiển với cấu trúc thay đổi đã được ứng dụng rộng rãi để ổn định hoá cho chuyển động của robot

Luận văn này giới thiệu một bộ điều khiển PID có cấy trúc thay đổi kết hợp giữa

bộ điều khiển có cấu trúc thay đổi và mặt trượt PID để điều khiển cho góc quay của cánh tay mềm dẻo 2 bậc tự do bám theo góc đặt Điều kiện tồn tại của mặt trượt và tính ổn định tiệm cận toàn cục của hệ thống được thiết lập dưới dạng toàn phương của hàm Lyapunov Tính khả thi của bộ điều khiển được kiểm chứng thông qua kết quả mô phỏng trên phần mềm Matlab

Luận văn được chia làm 5 chương với nội dung như sau:

+ Chương 1: Tổng quan

Nội dung của chương này trình bày tổng quan về hệ thống robot và ứng dụng của

nó trong công nghiệp đồng thời giới thiệu sơ lược về bộ điều khiển được thiết kế trong luận văn

+ Chương 2: cơ sở lý thuyết

Nội dung chương này trình bày lý thuyết cơ sở về bộ điều khiển PID và điều khiển trượt

+ Chương 3: Thiết kế bộ điều khiển PID trượt

Nội dung chương này trình bày về bộ điều khiển PID trượt kết hợp với mặt trượt PID, điều kiện tồn tại mặt trượt và điều kiện ổn định của hệ thống

+ Chương 4: Mô phỏng hệ thống điều khiển trong môi trường Matlab

Nội dung chương này trình bày sơ đồ mô phỏng cánh tay mềm dẻo 2 bậc tự do điều khiển bằng bộ điều khiển PID trượt được thiết kế trong chương 3 dùng simulink và kết quả mô phỏng

Chương 5: Kết luận và hướng phát triển

CHƯƠNG 2

CƠ SỞ LÝ THUYẾT2.1 Bộ điều khiển PID:

Bộ điều khiển PID được sử dụng rất rộng rãi trong thực tế do nó có khả năng làm triệt tiêu sai số xác lập, tăng tốc độ đáp ứng quá độ, giảm vọt lố nếu các thông

số của bộ điều khiển được chọn lựa thích hợp Do tính thông dụng của nó nên nhiều hãng sản xuất thiết bị điều khiển đã cho ra đời các bộ điều khiển PID thương mại rất tiện dụng

Với bộ điều khiển PID, ta có thể tích hợp các luật điều khiển khác nhau như điều khiển tỉ lệ P, điều khiển PI, điều khiển PD

Sự có mặt của khâu PID ở vòng hồi tiếp có thể dẫn đến sự dao động trong hệ thống do đáp ứng quá độ bị vọt lố bởi hàm dirac ( )t

Các bộ hiệu chỉnh PID được ứng dụng nhiều trong công nghiệp dưới dạng thiết bị điều khiển hay thuật toán phần mềm

Hàm truyền của bộ hiệu chỉnh PID có dạng :

 1

e t r t y t sai lệch giữa tín hiệu đặt và ngõ ra

Vấn đề thiết kế là cần xác định giá trị K P,K K I, D sao cho hệ thỏa mãn các yêu cầu về chất lượng

Mối quan hệ giữa các hệ số

Hệ số K P làm tăng tốc độ đáp ứng, làm cho thời gian quá độ nhỏ hơn Hệ số

tỉ lệ lớn sẽ làm giảm giá trị sai lệch ở trạng thái xác lập Nếu K P quá lớn có thể làm

hệ thống mất ổn định tức là làm cho đáp ứng tăng vô tận hay tạo sự dao động liên tục Quá trình hiệu chỉnh K P không làm triệt tiêu giá trị sai lệch ở trạng thái xác lập

Do đó ta thêm vào khâu tích phân để triệt tiêu sai số xác lập (đối với tín hiệu vào là hàm nấc)

Thành phần tích phân tích lũy sai lệch trong quá trình hiệu chỉnh nên làm tăng tác động hiệu chỉnh theo hướng làm giảm sai lệch Việc này có thể làm cho hệ thống mất ổn định

Thành phần vi phân làm giảm vọt lố và làm chậm đáp ứng ban đầu của hệ thống

Bảng 2.1 Tóm tắt ảnh hưởng của các hệ số lên đáp ứng hệ kín

Hệ số Thời tăng Thời gian xác lập Vọt lố Sai số xác lập

Zeigler-Nichols đưa ra hai cách chọn thông số của bộ điều khiển PID tùy theo đặc điểm của đối tượng

Cách 1 : Dựa vào đáp ứng quá độ hệ hở, áp dụng cho các đối tượng có đáp

ứng với tín hiệu vào là hàm nấc có dạng chữ S nhưhình 2.1 Ví dụ như nhiệt độ lò nhiệt, tốc độ động cơ…

Thông số bộ điều khiển PID được cho như bảng sau :

Cách 2 : Dựa vào đáp ứng quá độ của hệ kín, áp dụng cho các đối tượng có

khâu tích phân lý tưởng Ví dụ như mực chất lỏng trong bồn chứa, vị trí hệ truyền động dùng động cơ… Đáp ứng quá độ (hệ hở) của các đối tượng có khâu tích phân

lý tưởng không có dạng chữ S mà tăng đến vô cùng Đối với các đối tượng thuộc loại này ta chọn thông số bộ điều khiển PID dựa vào đáp ứng quá độ của hệ kín như hình 2.2

Ban đầu cho K K I, D bằng không, điều khiển với K P tăng dần đến Kmax khi đáp ứng ra dao động quanh trị đặt với chu kỳ T p Các thông số được chọn như sau

2.2 Bộ điều khiển trượt:

Một phương pháp điều khiển linh hoạt hiện đại đang được sử dụng rộng rãi cho các hệ SISO là điều khiển trượt, phương pháp này điều khiển dễ dàng các hệ thống được mô tả bằng các phương trình vi phân bậc nhất

Xét hệ thống phi tuyến biểu diễn bởi phương trình vi phân

u y y y y g y

y y y

) 2 (

a

Khi đó nếu S=0 thì sai lệch e 0 khi t 

Thay (2.6) và (2.4) vào (2.7), ta được

S=xn+an-2xn-1+…+a1x2+a0x1

- (r(n-1)+an-2r(n-2) +…+a1r+a0r) (2.9) Phương trình S=0 xác định một mặt cong trong không gian n chiều gọi là mặt trượt (sliding surface)

Vấn đề là xác định luật điều khiển u để đưa các quỹ đạo pha của hệ thống về mặt trượt và duy trì nên mặt trượt một cách bền vững đối với các biến động của f(x) và g(x)

Lấy đạo hàm (2.9) và áp dụng (2.4), ta có

) ( ) (

) (

) (

)

2 x r a x r a x r a

u x

( ) (

) (

) (

1 )

1 (

2 x r a x r a x r sign S a

x f

Hình 2.3 biến trạng thái hội tụ về mặt trượt

Tính bền vững của luật điều khiển: trong điều khiển có sai số mô hình, luật điều khiển (2.12) luôn đưa được quỹ đạo pha của hệ thống về mặt trượt S=0 nếu điều kiện sau được thỏa mãn

0 thì S = 0 Do vậy quỹ đạo pha sẽ vượt qua mặt trượt một đoạn và sẽ quay

về mặt trượt sau đó khi u thay đổi giá trị theo (2.12) Quá trình được lặp lại và kết quả là quỹ đạo pha dao động quanh mặt trượt Hiện tượng này được gọi là hiện tượng chattering, gây ra các hiệu ứng không mong muốn như:

- Phát sinh sai số điều khiển

- Làm phát nóng mạch điện tử

- Mài mòn các bộ phận cơ khí

- Kích động các mode tần số cao không mô hình hóa làm giảm chất lượng điều khiển hoặc mất ổn định

Để khắc phục hiện tượng chattering ta có thể :

- Giảm biên độ của u bằng cách giảm hệ số trong (2.12) Tuy nhiên điều này làm giảm tích bền vững của hệ thống điểu khiển đối với sai

số của mô hình

- Thay hàm signum bởi hàm sat

Vấn đề ổn định hóa (regulation) trong điều khiển trượt:

(

) ,

(

2 1 2

Mặt trượt được định nghĩa như sau:

) ( 1

, ( ) , ( ) ,

1 2

1 2

1 2 2 1 1

1

x u x x g x x f x x f

( )

2 f x sign S

x x f

1) Mục đích của điều khiển là tạo ra

Với e(t) là sai lệch giữa tín hiệu đặt r(t) và tín hiệu đầu ra y(t) của hệ thống 2) Để đạt được (2.19), trong điều khiển trượt người ta sử dụng hàm (gọi là hàm trượt)

1 1 2 2 2 1

n n

dt

e d dt

e d a dt

de a e a

e

Trong đó n là bậc của mô hình đối tượng điều khiển và các hệ số a0, a1,….,an-2

phải được chọn sao cho phương trình vi phân

có đa thức đặc tính

1 2 2 1

0

)

(p a a p a n p n  p n

A (2.22)

Là đa thức Hurwitz Điều này sẽ đảm bảo rằng không phụ thuộc vào giá trị đầu e(0),





n n

dt

e d

nghiệm e(t) của (2.21) luôn thỏa mãn được điều kiện (2.19) Phương trình (2.21) mô tả mặt phẳng s(e) = 0 có tên gọi là mặt trượt (sliding surface) trong không gian n chiều có các trục tọa độ e,

dt

e d

3) Với hàm trượt s(e), nhiệm vụ của bộ điều khiển này được cụ thể rõ ràng hơn là phải tạo ra được s(e)  0 để có e( t) 0 và điều này tương đương với việc phải tạo ra được

Điều kiện (2.23) vẫn còn thường được gọi là điều kiện trượt (sliding surface)

Hình 2.4: Minh họa nguyên lý chung của điều khiển trượt phản hồi tín hiệu ra

đối tượng SISO

u

-

Bộ điều khiển trượt

Đối tượng điều khiển

CHƯƠNG 3

THIẾT KẾ BỘ ĐIỀU KHIỂN

Trong chương này, một bộ điều khiển PID trượt cho cánh tay mềm dẻo được thiết kế để điều khiển cánh tay bám theo một quỹ đạo cho trước, tính ổn định của hệ thống được kiểm chứng theo tiêu chuẩn Lyapunov

3.1 Khảo sát mô hình cánh tay mềm dẻo 2 bậc tự do:

Cánh tay mềm dẻo mà chúng ta khảo sát là cánh tay mềm dẻo 2 bậc tự do hình 3.1

Hình 3.1: Cánh tay mềm dẻo 2 bậc tự do Với các tham số cho trong bảng 3.1 sau:

Bảng 3.1: Các thông số của cánh tay mềm dẻo 2 bậc tự do

Động cơ

Thanh mềm dẻo (Flexible links)

Tính chất cứng xếp theo thứ tự Kstiff 18 ~ 22 Nm/rad

Thanh cứng (rigid links)

Bộ kết nối (hình 3.2) và các mô đun điều khiển (hình 3.3) như sau:

Hình 3.2: Bộ kết nối

Hình 3.3: Các mô đun điều khiển

3.1.1 Phương trình động học:

3.1.1.1 Phương trình động học thuận:

Các vị trí góc được cho trong hình 3.4

Hình 3.4: Vị trí các góc do động cơ X và động cơ Y điều khiển

la: là chiều dài mỗi thanh

Ta có các ma trận chuyển đổi sau ( theo tài liệu sản xuất ):

0

) cos(

) cos(

) sin(

) sin(

) sin(

) cos(

x la x x

0

) sin(

) cos(

) sin(

) cos(

) sin(

) cos(

x la

x x

0

) sin(

) cos(

) sin(

) cos(

) sin(

) cos(

y la y y

0

) cos(

) cos(

) sin(

) sin(

) sin(

) cos(

y la y y

13 T T

T 

54 65

64 T T

Tọa độ điểm P(x,y) do động cơ X điều khiển:

) sin(

) sin(

) cos(

) cos(

) sin(

) 3 , 1 (

13

) cos(

) sin(

) cos(

) sin(

) cos(

) 3 , 2 (

) sin(

) cos(

) sin(

) cos(

) 3 , 1 (

64

) sin(

) cos(

) cos(

) sin(

) sin(

) 3 , 2 (

3.1.1.2 Phương trình động học ngược:

Ta viết lại Px_X, Py_X, Px_Y, Py_Y

) cos(

) sin(

_ la x la x x

) sin(

) cos(

) sin(

) cos(

) cos(

) sin(

_ la y la y y

Từ (3.3) và (3.4) ta có:

) sin(

) cos(

_ la y la y y

) cos(

) cos(

Ta giải được  ynhư sau (theo tài liệu nhà sản xuất cung cấp)

)arctan(

4 4

2 ( [ {

2 1

4 _ 2 2 2 _ 4

_ 2 2 _ 4

_

2 _ 2 _ 2

_ _ 2

_ 2 _ 1

P la

P P

la P P

P P P P P P Ay

Y y Y

x Y y Y

y Y

x

Y y Y x Y y Y x Y y Y x

Ay y Y x2Y y_Y

_ 2

_

2  (  )

} )]

4 4

2 ( [ {

2 1

2 2 2 _ 4

_ 2 2 _ 4

_

2 _ 2 _ 2

_ 3

_ 2

_ _ 1

la P P

la P P

P P P P P P By

Y x Y y Y

y Y

x

Y y Y x Y y Y x Y y Y x

By2  ( y2_Y  x2_Y)

Từ (3.1) và (3.2) ta có:

)cos(

)sin(

) sin(

) cos(

_ la x la x x

_ 2 _ lasin( x) P lacos( x) la

Ta giải được

x x

A

A

A 

2 1

x

x x

B

B

B 

})]

44

2([

4 _ 2 2 2 _ 4

_ 2 2 _ 4

_

2 _ 2 _ 2

_ _ 2

_ 2 _ 1

P la

P P

la P P

P P P P

P P Ax

X x X

y X x X

x X

y

X y X x X x X y X y X x

Ax x X y2 X x_X

_ 2

_

2  (  )

})]

44

2([

{

2 2 2 _ 4

_ 2 2 _ 4

_

2 _ 2 _ 2

_ 3

_ 2

_ _ 1

la P P

la P P

P P P P

P P Bx

X y X x X

x X

y

X y X x X x X y X x X y

Bx ( x X y2 X)

_ 2

_

2  

Động học ngược cánh tay mềm dẻo được tính tron file “rob2d_i.c”

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

Ta phân tích cánh tay mềm dẻo như hình 3.5

Các góc t1 , t2 trong hình 3.5 được tính như sau :

t1= m1 + m2 + dx +dy

t2= m1+ m2+ dx +dy

Trong đó: m1,dx là góc do động cơ X tạo ra và độ lệch tương ứng

m2, dy là góc do động cơ Y tạo ra và độ lệch tương ứng

Hình 3.5: Vị trí các góc và các ma trận chuyển đổi Phép biến đổi cho động cơ X (theo tài liệu sản xuất):

Các ma trận chuyển đổi trục (xem hình 3.5):

0

)cos(

2)cos(

)sin(

)sin(

2)sin(

)cos(

1 1

1

1 1

1

1

m dx

l m dx m

dx cg

0

) cos(

) cos(

) sin(

) sin(

) sin(

) cos(

1 1

1

1 1

1

1

m dx l m dx m

dx j

0

)sin(

2)cos(

)sin(

)cos(

2)sin(

)cos(

1 1

1

1 1

cg

T

) sin(

) cos(

) sin(

) cos(

) sin(

) cos(

1 1

1

1 1

1

2

t l t t

0

)sin(

2

3)cos(

)sin(

)cos(

2

3)sin(

)cos(

1 1

1

1 1

0

) sin(

2 ) cos(

) sin(

) cos(

2 ) sin(

) cos(

1 1

1

1 1

1

2

t l t t

0

)cos(

2)cos(

)sin(

)sin(

2)sin(

)cos(

1 1

1

1 1 1

m dx

l m dx m

dx cg

T link

f

Tọa độ (x,y) điểm cg1:

) sin(

2 ) 3 , 1 ( _ 1 _cg f1 link1 l dx m1

) cos(

2 ) 3 , 2 ( _ 1 _cg f1 link1 l dx m1

Vận tốc điểm cg1 theo phương x và y :

) cos(

2 ) cos(

2 1 _ 1 _cg P x cg d x l dx m1 l m1 dx m1

) sin(

2 ) sin(

2 1

_ 1 _cg P y cg d x l dx m1 l m1 dx m1

Động năng do thanh link 1 tạo ra:

2 1

2 2

1 _ 2

1 _

8)(

2ke_link  mf V x cg V y cg l mf d x m

Ma trận chuyển đổi từ gốc tọa độ đến điểm j1 (bản lề joint 1):

) cos(

) cos(

) sin(

) sin(

) sin(

) cos(

1 int

1 1

1

1 1

m dx l m dx m

dx j

T jo

0

) cos(

) cos(

) sin(

) sin(

) sin(

) cos(

) 3 , 1 ( 1 int

1 1

1

1

m dx l m dx m

dx jo

f j

px

)cos(

)3,2(1int

)

1 _

1 P d xl dx m l m dx m

)sin(

)

1 _

1 P d xl dx m l m dx m

V yj  y j     

Động năng do bản lề joint 1 tạo ra:

2 1 1

2 2

1 _ 2

1 _ 1

)(

2)(

21int

0

23 _ 22

_ 21

_

13 _ 12

_ 11

_

2 _ 2

_ 2

_

2 2 1 1 2

l l

l

cg j

f f

f T

T f

Trong đó

) sin(

) sin(

) cos(

) cos(

) cos(

) cos(

) sin(

) sin(

2 ) cos(

) cos(

2 ) sin(

) sin(

) sin(

) cos(

) sin(

) cos(

) cos(

) sin(

2 ) sin(

) cos(

2 ) cos(

2 ) cos(

) cos(

2 ) sin(

) 3 , 1

2 _ 2

) cos( ) sin(

2 ) sin(

) cos(

2 ) cos(

) 3 , 2

2 _ 2

Vận tốc của điểm cg2 theo phương x và y :

) sin(

2 ) sin(

) cos(

2

) cos(

) sin(

2 ) sin(

) cos(

2 ) cos(

) cos( ) sin(

2 ) sin(

) cos(

2 ) cos(

1 1 1

1 1

1 1 1

1 1

1

1 1 1

1 1

2 _ 2

_

t m dx

l t m dx

l

t

t m dx

l t m dx

l m dx l

m

t m dx

l t m dx

l m dx l x d P

) sin(

2 ) cos(

) cos(

2 ) sin(

) sin(

) sin(

2 ) cos(

) cos(

2 ) sin(

) sin(

) sin(

2 ) cos(

) cos(

2

1 1 1

1 1

1 1 1

1 1

1

1 1 1

1 1

2 _ 2

_

t m dx

l t m dx

l m dx l

x

d

t m dx

l t m dx

l m dx l

m

t m dx

l t m dx

l t P

sin(

)sin(

)sin(

2

)cos(

2[)]

sin(

2

)sin(

2)cos(

)cos(

{[

))(

2(2

1ke_link

2 1 1

1 2

2

2 2

2 2

2 1

1 1

2 2 _ 2

2 _ 2

m dx m

l

m dx xl d m dy m

l

m dy

l y d m

dy m

l

m dy

l d m dx m

l m dx xl

d l

m

V V

ml

y

cg y cg x

0

23 22

21

13 12

11

int

2 _ 2

_ 2

_ 2

2 1 1 2

j j

j

f f

f T

j T jo

f

Trong đó

) sin(

) sin(

) cos(

) cos(

) cos(

) cos(

) sin(

) sin(

) cos(

) cos(

) sin(

) sin(

) sin(

) cos(

2

) sin(

) sin(

) cos(

) cos(

) sin(

) sin(

) cos(

) cos(

) cos(

) cos(

) sin(

) 3 , 1

2 int _ 2

) cos( ) sin(

) sin(

) cos(

) cos(

) 3 , 2

2 _ 2

)sin(

)sin(

)cos(

)cos(

)cos(

)sin(

)sin(

)cos(

)cos(

1 1 1

1 1

1 1 1

1 1

1

1 1 1

1 1

2 _ 2

_

t m dx l t m dx l

t

t m dx l t m dx l

m dx l

m

t m dx l t m dx l

m dx l

x d P

) sin(

2 ) cos(

) cos(

2 ) sin(

) sin(

) sin(

2 ) cos(

) cos(

2 ) sin(

) sin(

) sin(

2 ) cos(

) cos(

2

1 1 1

1 1

1 1 1

1 1

1

1 1 1

1 1

2 _ 2

_

t m dx

l t m dx

l m dx l

x

d

t m dx

l t m dx

l m dx l

m

t m dx

l t m dx

l t P

2 1

1 1 1

2 2 _ 2

2 _ 1 2

)]

sin(

) sin(

) cos(

) cos(

[

) (

2

1 ke_joint

m dy m

l

m dy l d m dx m

l m dx xl

d j

m

V V

m

y

j y j x j

0

23 22

21

13 12

11

3 _ 3

_ 3

_

3 2 1 1 2

l l

l

f f

f T

j T link

f

Trong đó:

) sin(

) sin(

) cos(

) cos(

) cos(

) cos(

) sin(

) sin(

) cos(

) cos(

2

3 ) sin(

) sin(

) sin(

) cos(

) sin(

) cos(

) cos(

3

) cos(

) sin(

2

3 ) sin(

) cos(

2

3 ) cos(

2

3 ) cos(

) cos(

2

3 ) sin(

) 3 , 1

2 _ 3

) cos( ) sin(

2

3 ) sin(

) cos(

2

3 ) cos(

) 3 , 2

2 _ 3

)sin(

2

3)sin(

)cos(

2

3

)cos(

)sin(

2

3)sin(

)cos(

2

3)cos(

)cos()sin(

2

3)sin(

)cos(

2

3)cos(

1 1 1

1 1

1 1 1

1 1

1

1 1 1

1 1

3 _ 3

_

t m dx

l t m dx

l

t

t m dx

l t m dx

l m dx l

m

t m dx

l t m dx

l m dx l

x d P

)sin(

2

3)cos(

)cos(

2

3)sin(

)sin(

)sin(

2

3)cos(

)cos(

2

3)sin(

)sin(

)sin(

2

3)cos(

)cos(

23

1 1 1

1 1

1 1 1

1 1

1

1 1 1

1 1

3 _ 2

_

t m dx

l t m dx

l m dx l

x

d

t m dx

l t m dx

l m dx l

m

t m dx

l t m dx

l t P

sin(

) sin(

) sin(

2

3

) cos(

2

3 [ )]

sin(

2

3

) sin(

2

3 ) cos(

) cos(

{[

) (

ke_link

2 1 1

1 2

2

2 2

2 2

2 1

1 1

2 3 _ 2

3 _ 3

m dx m

l m dx xl d m dy m

l

m dy

l y d m

dy m

l

m dy

l d m dx m

l m dx xl

d l

m

V V

ml

y

cg y cg x

0

23 22

21

13 12

11

_ _

_

2 1 1

load load

load

f f

f T

j T load

f

Trong đó

) sin(

) sin(

) cos(

) cos(

) sin(

) cos(

) cos(

) sin(

) sin(

) sin(

2 ) cos(

) cos(

2 ) sin(

) cos(

) sin(

) sin(

) cos(

) sin(

) sin(

) cos(

) cos(

) cos(

) sin(

2 ) sin(

) cos(

2 ) cos(

2 ) cos(

) cos(

2 ) sin(

) 3 , 1

_ _

) cos( ) sin(

2 ) sin(

) cos(

2 ) cos(

) 3 , 2

_ _

)sin(

2)sin(

)cos(

2)cos(

)cos()sin(

2)sin(

)cos(

2)cos(

1 1 1

1 1

1 1 1

1 1

1

1 1 1

1 1

_ _ _

_

t m dx t

m dx t

t m dx t

m dx m

dx l

m

t m dx t

m dx m

dx l x d P

)sin(

2)cos(

)cos(

2)sin(

)sin(

)sin(

2)cos(

)cos(

2

1 1 1

1 1

1 1 1

1 1

1

1 1 1

1 1

_ _ _

_

t m dx t

m dx m

dx l

x

d

t m dx t

m dx m

dx l

m

t m dx t

m dx t

sin(

) sin(

) sin(

2

) cos(

2 [ )]

sin(

2

) sin(

2 ) cos(

) cos(

{[

2

) (

2

1 ke_load

2 1 1

1 2

2

2 2

2 2

2 1

1 1

2 _ _ 2

_ _

m dx m

l m dx xl d m dy m

l

m dy l y d m

dy m

l

m dy l d m dx m

l m dx xl

d mload

V V

mload

y

load cg y load cg x

0

)sin(

2)cos(

)sin(

)cos(

2)sin(

)cos(

2 2

2

2 2

2

4

m dy

l m dy m

dy

T cg

) sin(

) cos(

) sin(

) cos(

) sin(

) cos(

2 2

2

2 2

2

3

m dy l

m dy m

0

)cos(

2)cos(

)sin(

)sin(

2)sin(

)cos(

2 2

2

2 2

0

) cos(

) cos(

) sin(

) sin(

) sin(

) cos(

2 2

2

2 2

2

4

t l t t

T j

Ma trận chuyển đổi từ gốc tọa độ đến điểm cg4 trên thanh link 3:

4 2 4

_link T cg

Tọa độ điểm (x,y) điểm cg4:

) 3 , 1 (

4 _ 4

_cg link

) 3 , 2 (

4 _ 4

_cg x cg

4 _ 4

2 4 2

4

)(

8

)(

2

14_

m y d mf

l

V V

mf link

_jo T j

Tọa độ (x,y) của bản lề joint 3:

) 3 , 1 (

3 int _ 3

_j ho

) 3 , 2 (

3 int _ 3

_j x j

3 _ 3

2 2 1

2

2 3 2

3 1

)(

2

)(

2

13int_

m y d mj

l

V V

mj jo

_ho T j .T cg

Tọa độ (x,y) của điểm cg5:

) 3 , 1 (

5 _ 5

_cg link

) 3 , 2 (

5 _ 5

_cg x cg

5 _ 5

sin(

) sin(

) cos(

2

) cos(

2 [ )]

sin(

2

) sin(

2 ) cos(

) cos(

{[

) )(

2 ( 2

1 ke_link

2 2 2

1 1

1 2

1 2

1 2

2 2

2 5 _ 2

5 _ 4

m dy m

l

m dy yl d m dx m

l

m dx

l x d m

dx m

l

m dx

l d m dy m

l m dy yl d l

m

V V

ml

y

y

cg y cg x

_ho T j .T j

Tọa độ (x,y) điểm j4

) 3 , 1 (

4 int _ 3

_j jo

) 3 , 2 (

4 int _ 3

_j x j

4 _ 4

sin(

)sin(

)cos(

)cos(

[)]

sin(

)sin(

)cos(

)cos(

{[

2

)(

2

1ke_joint

2 1

1 2

2

2 2

2 2

2 1

1 1 1

2 4 _ 2

4 _ 1 4

m dx m

l

m dx xl d m dy m

l

m dy yl d m

dy m

l

m dy l d m dx m

l m dx xl d mj

V V

mj

y

j y j x

Động năng do động cơ 1 (động cơ X) tạo ra:

2 1

2

1 1 _motor Jm m

Động năng do động cơ 2 (động cơ Y) tạo ra:

2 2

2

1 2 _motor Jm m

Trong đó các hệ số Kstiff và Jm xem bảng 3.1

Động năng tổng của hệ cánh tay mềm dẻo:

load ke

jo ke jo

ke jo

ke jo

ke motor ke

motor ke

link ke link ke link ke link ke link ke

ke

_

4int_3int_2int_1int_2_

1_

5_4_3_2_1_

2

1 2

1 3 _ 1 _link pe link Kstiftdx Kstiftd y pe

Hàm Lagrange của hệ cánh tay mềm dẻo:

L=ke-pe

 

2 5 2  sin( ) 2 5 2  sin( )

4 7 3 4 ) sin(

2 5

2

) sin(

2 5 2 4

9 3

4

2 4

7 2

3 8 2

2 4

7 2

3

8

2 4

9 2

3 8 2

2 2

4

9 2

3 8 2

2 1 2 2 1 2 1

2 2

1

2

2 2

2 1 2 2 2

2 2

1

2

2 2 1 2 1 2

2 1 2

2

2 2 2 1 2 2 2

2 2 1 2

2

2 2 2 1 2 2

2 2

2 2

2 1 2 2

t m m m l ml l m l t m d m l ml l

m

l

m d m l ml l m l mf l t m d m l ml l

m

l

t d d m l ml l m l m d m l ml l m

l

mf

l

m m l ml l m l mf l Jm d m l ml l m l

mf

l

d m l ml l m l mf l d Kstiff d

Kstiff m

m l ml l m l mf l

Jm

L

load j

y load j

y load j

x load j

y x load j

x load j

load j

y load j

x load j

x y

load j

2 5

2 ) cos(

2 5

2

2 1 2

2 1 2 2

1 2

2 1

2

1 2

2 1 2 2

2 1 2

1

t m m m l ml l m l t m m m l ml l m

l

t m d m l ml l m l t d d m l ml l m l

m

L

load j

load j

y load j

y x load j

42

93

4

2 2

2 1 2 2

2 1

2

2 2 1 2 2

1 2

2 1 2 2

1

t m m l ml l m l t d m l ml l m

l

d m l ml l m l mf l m m l ml l m l mf l Jm

m

L

load j

y load j

x load j

load j

1 2 2

2 2 2

2 1 2

2 1 2

2 1 2 1

2 2

1 2

2 2

2 1 2 2

2 2

1 2

2 2

2 1 2 2

2 1 2

2 2

2 1 2 2

2 1 2

2 2

1 2 2

1

2

93

4)

cos(

25

2

)cos(

25

2)cos(

25

2

)cos(

25

2)cos(

25

2

)cos(

25

2)cos(

25

2

)sin(

25

2)sin(

25

2

4

93

4

m m l ml l m l mf l Jm t

m m l ml l m

l

t m m m l ml l m l t d m m l ml l m

l

t m d m l ml l m l t d m l ml l m

l

t m d m l ml l m l t d d m l ml l m

l

t m m l ml l m l t d m l ml l m

l

d m l ml l m l mf l m

L dt

d

load j

load j

load j

y load j

y load j

y load j

x load j

y x load j

load j

y load j

x load j

25

2)cos(

25

2

)sin(

25

2)sin(

25

2

4

93

42

93

41

2 2

2

1

2

2 2 2

2 1 2 2

2 2

1

2

2 2

2 1 2 2

2

1

2

2 2 1 2 2

1 2

2 1 2 2

1 1

t m d m l ml l

m

l

t m m l ml l m l t d m l ml

l

m

l

t m m l ml l m l t d m l ml

l

m

l

d m l ml l m l mf l m m l ml l m l mf l Jm m

L m

load j

y load j

load j

y load j

x load j

load j

   

) cos(

2 5

2 ) cos(

2 5

2

) cos(

2 5

2 ) cos(

2 5

2

2 1 2

2 1 2

2 2

2 1 2 2

2 2

1 2

1 2

2 1 2 2

2 1 2

t m m m l ml l m

l

t m d m l ml l m l t m d m l ml l m

l

t d m m l ml l m l t d d m l ml l m

l

Kstiffd d

L

load j

x load j

x load j

y load j

y x load j

x x

44

93

4

2 2

2 1 2 2

2 1 2

1 2

2 1 2 2

2 2 1 2 2

t m m l ml l m l t d m l ml l m

l

m m l ml l m l mf l d m l ml l m l mf l d

L

load j

y load j

load j

x load j

2 5

2 ) cos(

2 5

2

) cos(

4 10

4 ) cos(

2 5

2

) cos(

2 5

2 ) cos(

2 5

2

) sin(

2 5

2 ) sin(

2 5

2

4

9 3

4 4

9 3

4

2 2 2

2 1 2

2 1 2

2 1 2 1

2 2

1 2

2 2

2 1 2 2

2 2

1 2

2 2

2 1 2 2

2 1 2

2 2

2 1 2 2

2 1 2

1 2

2 1 2 2

1 2

2 1 2 2

t m m l ml l m

l

t m m m l ml l m l t d m m l ml l m

l

t m d m l ml l m l t d m l ml l m

l

t m d m l ml l m l t d d m l ml l m

l

t m m l ml l m l t d m l ml l m

l

d m l ml l m l mf l m m l ml l m l mf l d

L

dt

d

load j

load j

y load j

y load j

y load j

x load j

y x load j

load j

y load j

load j

load j

2 5 2

) cos(

2 5

2

) sin(

2 5 2

) sin(

2 5

2

4

9 3

4 2

9 3

4 2

2 2

2

1

2

2 2 2

2 1 2 2

2 2

1

2

2 2

2 1 2 2

2

1

2

2 2 1 2 2

1 2

2 1 2 2

t m d m l ml

l

m

l

t m m l ml l m l t d m l ml

l

m

l

t m m l ml l m l t d m l ml

l

m

l

d m l ml l m l mf l m m l ml l m l mf l Kstiffd d

L d

load j

y load j

load j

y load j

x load j

load j

x x

ml m

m d m d

l

m

L

load j

3 4 4

7 3

4

1 2

2 1 2 2

2 1 2

2 2

1 2 2

2 2

2 1 2 2

2

t m m l ml l m l t d m l ml l m

l

d m l ml l m l mf l m m l ml l m l mf l Jm m

L

load j

x load j

y load j

load j

1 2 2

2 1 2

2 1 2

2 1 2

2 1 2 1

2 2

1 2

1 2

2 1 2 2

2 2

1 2

2 2

2 1 2 2

2 1 2

1 2

2 1 2 2

2 1 2

2 2

1 2 2

2

47

34)

cos(

25

2

)cos(

25

2)cos(

25

2

)cos(

25

2)cos(

25

2

)cos(

25

2)cos(

25

2

)sin(

25

2)sin(

25

2

47

34

m m l ml l m l mf l Jm t

m m l ml l m

l

t m m m l ml l m l t d m m l ml l m

l

t m d m l ml l m l t d m l ml l m

l

t m d m l ml l m l t d d m l ml l m

l

t m m l ml l m l t d m l ml l m

l

d m l ml l m l mf l m

L dt

d

load j

load j

load j

x load j

y load j

x load j

x load j

y x load j

load j

x load j

y load j

2 5

2 ) cos(

2 5

2

) sin(

2 5

2 ) sin(

2 5

2

4 7

3 4 4

7 3

4 3

1 2

2 1

2

2 1 2

2 1 2 2

2 2

1

2

1 2

2 1 2 2

2 1

2

2 2

1 2 2

2 2

2 1 2 2

2 2

t m d m l ml l m

l

t m m l ml l m l t d m l ml l m

l

t m m l ml l m l t d m l ml l m

l

d m l ml l m l mf l m m l ml l m l mf l m

L m

L dt

d

F

x load j

load j

x load j

load j

x load j

y load j

load j

2 5

2 ) cos(

2 5

2

2 1 2

2 1 2 2

2 2

1 2

1 2

2 1 2 2

2 1 2

t m m m l ml l m l t m d m l ml l m

l

t d m m l ml l m l t d d m l ml l m

l

Kstiffd d

L

load j

x load j

y load j

y x load j

y y

344

73

4

1 2

2 1 2 2

2 1 2

2 2

2 1 2 2

2 2

1 2 2

t m m l ml l m l t d m l ml l m

l

m m l ml l m l mf l d m l ml l m l mf l d

L

load j

x load j

load j

y load j

25

2)cos(

25

2

)cos(

410

4)cos(

25

2

)cos(

25

2)cos(

25

2

)sin(

25

2)sin(

25

2

47

344

73

4

2 1 2

2 1

2

2 1 2

2 1 2 1

2 2

1

2

1 2

2 1 2 2

2 2

1

2

2 2

2 1 2 2

2 1

2

1 2

2 1 2 2

2 1

2

2 2

1 2 2

2 2

2 1 2 2

t m m l ml l m

l

t m m m l ml l m l t d m m l ml l m

l

t m d m l ml l m l t d m l ml l m

l

t m d m l ml l m l t d d m l ml l m

l

t m m l ml l m l t d m l ml l m

l

d m l ml l m l mf l m m l ml l m l mf l d

L

dt

d

load j

load j

x load j

y load j

x load j

x load j

y x load j

load j

x load j

y load j

load j

   

) cos(

2 5 2

) cos(

2 5

2

) sin(

2 5 2

) sin(

2 5

2

4 7 3

4 4

7 3

4 4

1 2

2

1

2

2 1 2 2 1 2 2

2 2

1

2

1 2 2 1 2 2

2

1

2

2 2 1 2 2

2 2

2 1 2 2

t m d m l ml

l

m

l

t m m l ml l m l t d m l ml

l

m

l

t m m l ml l m l t d m l ml

l

m

l

d m l ml l m l mf l m m l ml l m l mf l Kstiffd d

L d

load j

x load j

load j

x load j

y load j

load j

y y

KmKgF x

2 21_

KmKgF y

2 2

2 _

3

02

_1

F

y Torque x

Torque F

F

x Torque F

)cos(

25

2)cos(

25

2

)sin(

25

2)sin(

25

2

4

93

42

93

41

_

1

2 2

2

1

2

2 2 2

2 1 2 2

2 2

1

2

2 2

2 1 2 2

2

1

2

2 2 1 2 2

1 2

2 1 2 2

m

l

t m m l ml l m l t d m l ml

l

m

l

t m m l ml l m l t d m l ml

l

m

l

d m l ml l m l mf l m m l ml l m l mf l Jm Rm

load j

y load j

load j

y load j

x load j

load j

25

2)cos(

25

2

)sin(

25

2)sin(

25

2

4

93

42

93

4

0

2

2 2

2 1

2

2 2 2

2 1 2 2

2 2

1

2

2 2

2 1 2 2

2 1

2

2 2 1 2 2

1 2

2 1 2 2

l

t m m l ml l m l t d m l ml l m

l

t m m l ml l m l t d m l ml l m

l

d m l ml l m l mf l m m l ml l m l mf l Kstiffd

F

y load j

load j

y load j

load j

y load j

x load j

load j

   

)cos(

25

2)cos(

25

2

)sin(

25

2)sin(

25

2

47

344

73

4

2_1

_

3

1 2

2 1 2

2 1 2

2 1 2 2

2 2

1 2

1 2

2 1 2 2

2 1 2

2 2

1 2 2

2 2

2 1 2 2

1 2 2 2

l

t m m l ml l m l t d m l ml l m

l

t m m l ml l m l t d m l ml l m

l

d m l ml l m l mf l m m l ml l m l mf

l

Rm

KmKgF Rm

m K K Rm

KmKgF Rm

m K

K

y Torque x

Torque

F

x load j

load j

x load j

load j

x load j

y load j

load j

g m g

25

2)cos(

25

2

)sin(

25

2)sin(

25

2

47

344

73

4

0

4

1 2

2 1 2

2 1 2

2 1 2 2

2 2

1 2

1 2

2 1 2 2

2 1 2

2 2

1 2 2

2 2

2 1 2 2

l

t m m l ml l m l t d m l ml l m

l

t m m l ml l m l t d m l ml l m

l

d m l ml l m l mf l m m l ml l m l mf l Kstiffd

F

x load j

load j

x load j

load j

x load j

y load j

load j

( x

R

a 

) 2 8

( x

R

b 

) 1 2

2 2)

j load

m m m m

Jm l

Kstiff m l m l m l Jm m

l a

18 12

4 4

2 2

1 2 2

JmRm

K K

2 2)

a( 7 , 4 ) 

K K

2 2)

2 2)

j load

m m m m

Jm l

Kstiff m l m l m l Jm m

l a

28 12

4 16

2 2

1 2 2

JmRm

K K

2 2)

2 2)

g m

R J

K K

b(5,1)

m m

g m

R J

K K

b(6,1)

m m

g m

R J

K K

b(7,1)

m m

g m

R J

K K

b(7,2)

m m

g m

R J

K K

b(8,1)

m m

g m

R J

K K

b(8,2)

Các phần tử còn lại trong ma trận a và b đều bằng 0

y x

y

d m

2 1

2 1



m d x m d y m d x m d y

2 1

2 1

