Đồ án cơ điện tử

ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS.
NGUYỄN THỊ VÂN HƯƠNG
MỤC LỤC
LỜI NÓI ĐẦU .................................................................................................................... 3

CHƯƠNG I: MỞ ĐẦU ..................................................................................................... 4
1 Tổng quan về hệ cơ điện tử..................................................................................... 4
1.1 Định nghĩa ......................................................................................................... 4
1.2 Các thành phần cơ bản của hệ Cơ điện tử ..................................................... 4
2 Tổng quan về robot công nghiệp ............................................................................ 5
2.1 Lịch sử phát triển ............................................................................................. 5
2.2 Tình hình phát triển Robot công nghiệp ........................................................ 5
2.3 Phân loại Robot công nghiệp ........................................................................... 6
2.4 Ứng dụng của Robot công nghiệp trong sản xuất ......................................... 6
3 Đặt vấn đề................................................................................................................. 7
CHƯƠNG II: TÍNH TOÁN ĐỘNG HỌC ROBOT ....................................................... 9
1 Đặt hệ trục tọa độ theo Denavit- Hartenberg ....................................................... 9
2 Thiết lập bảng D-H ................................................................................................ 10
3 Ma trận Denavit- Hartenberg .............................................................................. 11
4 Bài toán động học thuận Robot ............................................................................ 12
4.1 Tính các ma trận D-H .................................................................................... 12
4.2 Xác định vị trí điểm tác động cuối ................................................................ 12
4.3 Xác định hướng của khâu thao tác ............................................................... 13
4.4 Tính vận tốc, gia tốc điểm tác động cuối ...................................................... 14
4.5 Tính vận tốc góc, gia tốc góc các khâu.......................................................... 15
5 Bài toán động học ngược robot ............................................................................ 17
CHƯƠNG 3: THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG CỦA ROBOT .................... 18
1 Khái niệm, nhiệm vụ của thiết kế quỹ đạo .......................................................... 18
2 Không gian thao tác của Robot ............................................................................ 18
3 Thiết kế quỹ đạo trong không gian khớp ............................................................ 19
3.1 Thiết kế quỹ đạo trong không gian khớp cho Robot phẳng ....................... 19
3.2 Thiết kế quỹ đạo trong không gian thao tác ................................................ 23
1
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. NGUYỄN THỊ VÂN HƯƠNG
CHƯƠNG 4: TÍNH TOÁN ĐỘNG LỰC HỌC ROBOT ............................................ 26

1 Phương trình Lagrange II dạng trận .................................................................. 26
2 Động năng của Robot ............................................................................................ 26
3 Thế năng của Robot .............................................................................................. 28
4 Thiết lập phương trình vi phân chuyển động của Robot .................................. 28
5 Thiết lập phương trình động lực học cho Robot ................................................ 28
CHƯƠNG 5: THIẾT KẾ ĐIỀU KHIỂN ROBOT ....................................................... 34
1 Thiết kế bộ điều khiển ........................................................................................... 34
1.1 Cơ sở lí thuyết ................................................................................................. 34
1.2 Mô phỏng ......................................................................................................... 36
KẾT LUẬN ...................................................................................................................... 47
TÀI LIỆU THAM KHẢO............................................................................................... 48
PHỤ LỤC ......................................................................................................................... 49
2
LỜI NÓI ĐẦU
Cơ điện tử là một trong 6 ngành công nghệ mũi nhọn của thế kỉ XXI. Ở Việt Nam,
trong vòng 10 năm trở lại đây, và đặc biệt là trong những năm tới, xuất hiện nhu cầu lớn
về đào tạo nhân lực Cơ điện tử. Cơ điện tử đóng vai trò quan trọng trong tự động hóa sản
xuất, nâng cao năng suất, chất lượng sản phẩm,… Các sản phẩm Cơ điện tử rất đa dạng,
góp mặt trong hầu hết các lĩnh vực kinh tế xã hội như giao thông, Robot, hệ thống sản xuất,
năng lượng mới, thiết bị y tế, hàng không vũ trụ,… Chính vì thế, việc nghiên cứu hệ thống
Cơ điện tử đóng một vai trò quan trọng. Trong các sản phẩm Cơ điện tử thì Robot công
nghiệp là lĩnh vực tiêu biểu.
Môn học “ Đồ án thiết kế hệ thống Cơ điện tử” giúp em củng cố, ôn tập những kiến
thức đã học trong 3 năm qua, đồng thời bước đầu làm quen , tìm hiểu, tính toán, mô phỏng
một hệ Cơ điện tử điển hình, đó là Robot công nghiệp.
Em xin chân thành cảm ơn sự hướng dẫn tận tình của TS. Nguyễn Thị Vân Hương,
đã trực tiếp hướng dẫn em hoàn thành đồ án này. Sự chỉ bảo của cô giúp em tiếp cận, giải
quyết và trình bày vấn đề một cách khoa học, rõ ràng. Đó thực sự là những kinh nghiệm
quý báu cho em sau này.
Dù đã cố gắng nhưng do khối lượng tính toán khá lớn đồng thời kiến thức của em
còn hạn chế nên không thể tránh khỏi những thiếu sót, em rất mong nhận được sự đóng
góp ý kiến từ các thầy cô.
Em xin chân thành cảm ơn.
Hà Nội, ngày 10 tháng 11 năm 2018
Sinh viên thực hiện

Nguyễn Văn Hoàng
3
CHƯƠNG I: MỞ ĐẦU
1 Tổng quan về hệ cơ điện tử

1.1 Định nghĩa
Từ Cơ điện tử, tiếng Anh “ Mechatronics” là từ ghép của “Mechanics” và
“Electronics”. Được người Nhật sử dụng đầu tiên vào năm 1975 trong việc điều khiển động
cơ điện bằng máy tính. Thuật ngữ này sau đó trở nên phổ thông ở Nhật và nhanh chóng
được nhiều nước trên khắp thế giới sư dụng khi các linh kiện điện tử và tiếp theo là máy
tính được sử dụng ngày càng nhiều trong điều khiển thiết bị, đặc biệt là trong các hệ thống
sản xuất.
Đã hơn 30 năm kể từ khi thuật ngữ Cơ điện tử ra đời, nhưng thuật ngữ này vẫn là một
khái niệm tiến triển không ngừng . Sau đây là một số định nghĩa về Cơ điện tử của một số
cơ quan, tổ chức:
- Cơ điện tử là sự kết hợp của kĩ thuật cơ khí, điều khiển điện tử và kĩ thuật hệ thống
trong thiết kế sản phẩm và quá trình (theo Nanyang Politechnic Singapore).
- Cơ điện tử là sự kết hợp đồng vận của kĩ thuật cơ khí, điều khiển điện tử và tư duy
hệ thống trong thiết kế sản phẩm và các quá trình sản xuất (theo Ủy ban Tư ván Phát triển
và Nghiên cứu Công nghiệp châu Âu, viết tắt là IRDAC).
- Cơ điện tử là sự kết hợp của 3 công nghệ then chốt: cơ khí, điện và điều khiển (theo
Louisian State University U.S.A).
Hiện nay, người ta vẫn đng cố gắng để định nghĩa Cơ điện tử, để phân loại các sản
phẩm Cơ điện tử và phát triển một chương trình giảng dạy Cơ điện tử chuẩn,. Tuy nhiên
rất khó để miêu tả hoàn thiện “Cơ điện tử là gì”. Điều đó cho thấy lĩnh vực này đang tồn
tại, phát triển không ngừng và vẫn rất mới mẻ.
1.2 Các thành phần cơ bản của hệ Cơ điện tử
Lược đồ sau đây mô tả tương đối dẽ hiểu và đầy đủ các thành phần chính của hệ Cơ
điện tử. Năm thành phần cơ bản trong sơ đồ này là đối tượng nghiên cứu của tất cả các giáo
trình Cơ điện tử.
4
Hình 1.1 Mô hình hệ cơ điện tử
2 Tổng quan về robot công nghiệp

2.1 Lịch sử phát triển
Robot là một thuật ngữ chủ người lao động trong hệ ngôn ngữ Slaves. Từ khoảng
năm 1921, nhà viết kịch người Sec Karl Capek đã viết vở kịch mang tên Rossum’s
Universal Robots, trong đó từ Robot là tên một loại máy tự động đã tiêu diệt ông chủ và
chiếm lĩnh thế giới. Năm 1926, thuật ngữ Robot lần đầu tiên xuất hiện trên phim ảnh của
Đức, bộ phim mang tên Metropolis. Năm 1939, Robot đi bộ Elutoo và chú chó Sporko
được triển lãm tại một hội chợ tại New York, Mỹ, thu hút nhiều sự quan tâm của công
chúng. Năm 1948, trước nhu cầu tự động hoá ngày một lớn của các dây chuyền sản xuất
lắp ráp công nghiệp, một số loại tay máy đã được nghiên cứu và chế tạo thử tại các phòng
thí nghiệm ở Mỹ, châu Âu và một số nước khác. Các kỹ thuật điều khiển tự động cơ điện
và điều khiển từ xa đã được đưa vào lĩnh vực Robot trong khoảng thời gian này. Cùng với
sự phát triển nhanh chóng của cơ khí tự động hóa và kĩ thuật điện tử,năm 1952, chiếc máy
CNC đầu tiên trên thế giới đã ra đời tại Viện Công Nghệ Massachusett, Mỹ. Thành tựu này
là cơ sở quan trọng cho sự phát triển các bộ điều khiển Robot hiện đại như ngày nay.
2.2 Tình hình phát triển Robot công nghiệp
Hiện nay, nhu cầu sử dụng Robot công nghiệp ngày càng tăng, các loại Robot được
chế tạo ngày càng đa dạng, độ chính xác cao hơn, linh hoạt hơn, giá cả phù hợp hơn, năng
suất và tuổi thọ cao hơn. Mặc dù dải ứng dụng của Robot ngày càng mở rộng, nhưng theo
thống kê vào năm 2000 tại Mỹ, lượng Robot hàn và Robot xử lí phôi chiếm khoảng 78%
5
tổng số lượng Robot công nghiệp đang sử dụng tại thời điểm đó. Số lượng Robot lắp ráp
chiếm khoảng 10% cũng trong thống kê này. Phần còn lại là các Robot công nghiệp khác.
Nhằm mục tiêu thiết kế, chế tạo các thế hệ Robot công nghiệp ngày càng thông minh,
linh hoạt, nhỏ gọn, rẻ, độ tin cậy cao,… một số bài toán đang là vấn đề thời sự, được các
nhà khoa học quan tâm giải quyết như: Tối ưu cấu trúc cơ khí, Các bài toán cơ học, Các cơ
cấu dẫn động và cảm nhận tín hiệu, Điều khiển thông minh.
Cho đến nay, hầu hết các trường đại học kĩ thuật ở Việt Nam đã đưa môn kĩ thuật
Robot vào chương trình chính khóa. Môn học này dành chủ yếu cho các chuyên ngành cơ
khí, song các khía cạnh riêng biệt của nó như kĩ thuật điều khiển, lập trình mô phorngcuxng
được các ngành khác quan tâm.
2.3 Phân loại Robot công nghiệp
Robot hiện nay rất phong phú và đa dạng, xuất hiện trong hầu hết các lĩnh vực nên
việc phân loại chúng chỉ mang tính tương đối, tùy thuộc vào quan điểm và mục đích phân
loại. Tuy nhiên, có 3 cách phân loại cơ bản: theo kết cấu, theo điều khiển và theo phạm vi
ứng dụng.
a. Phân loại theo kết cấu:
Theo kết cấu hình học của phạm vi làm việc, người ta phân Robot thành các loại:
kiểu Đề-các, cầu, SCARA, kiểu tay người,…
b. Phân loại theo điều khiển:
Theo kiểu điều khiển, người ta phân Robot làm 2 loại: Điều khiển hở, điều khiển
kín.
c. Phân loại theo phạm vi ứng dụng:
Theo phạm vi ứng dụng, người ta phân Robot thành các loại: Robot ứng dụng
công nghiệp, Robot quân sự, Robot dùng trong kĩ thuật thám hiểm vũ trụ, Robot
trong nghiên cứu khoa học, y tế, giáo dục,…
2.4 Ứng dụng của Robot công nghiệp trong sản xuất
Từ khi mới ra đời, Robot công nghiệp đã được áp dụng trong nhiều lĩnh vực khác
nhau để thay thế sức người. Nhờ vậy các dây chuyền sản xuất được tổ chức lại, năng suất
và hiệu quả tăng lên rõ rệt. Mục tiêu ứng dụng của Robot công nghiệp nhằm góp phần nâng
cao năng suất dây chuyền công nghệ, giảm giá thành, nâng cao chất lượng và khả năng
cạnh tranh của sản phẩm, đồng thời cải thiện điều kiện lao động. Đạt được những mục tiêu
trên là nhờ vào những khả năng to lớn của Robot như: làm việc không biết mệt mỏi, dễ
dàng chuyển nghề một cách thành thạo, chịu được phóng xạ và các môi trường làm việc
độc hại, nhiệt độ cao, cảm thấy được cả từ trường và nghe được cả siêu âm,… Robot được
6
dùng thay thế con người trong các trường hợp trên hoặc thực hiện các công việc tuy không
nặng nhọc nhưng đơn điệu, dễ gây mệt mỏi, nhầm lẫn.
Trong ngành cơ khí, Robot được sử dụng nhiều trong công nghệ đúc, hàn, cắt kim
loại, sơn, phun phủ kim loại, tháo lắp vận chuyển phôi, lắp ráp sản phẩm,…
Ngày nay đã xuất hiện nhiều dây chuyền sản xuất tự động gồm các máy CNC với
Robot công nghiệp, các dây chuyền đó đạt mức tự động hóa cao, mức độ linh hoạt cao,…
ở đây các máy và Robot đợc điều khiểu bằng cùng một hệ thống chương trình.
Ngoài các phân xưởng, nhà máy, kĩ thuật Robot cũng được sử dụng trong khai thác
thềm lục địa và đại dương, trong y học, sử dụng trong quốc phòng, trinh phục vũ trụ, công
nghiệp nguyên tử, lĩnh vực xã hội,…
Rõ ràng khả năng làm việc của Robot trong một số điều kiện vượt hơn khả năng của
con người, do đó nó là phương tiện hữu hiệu để tự động hóa, nâng cao năng suất lao động,
giảm nhẹ cho con người những công việc nặng nhọc và độc hại. Nhược điểm lớn nhất của
Robot là chưa linh hoạt như con người, nếu một Robot bị hỏng trong dây chuyền tự đọng
ηcó thể làm ngừng hoạt động của cả dây chuyền, cho nên Robot vẫn luôn hoạt động dưới
sự giám sát của con người.
3 Đặt vấn đề
Nhiệm vụ: Thiết kế điều khiển cho Robot hàn điểm, hoạt động trong một mặt phẳng,

hướng của khâu thao tác không đổi   với η là góc quay Cardan.
3
Lựa chọn mô hình: vì Robot có nhiệm vụ hàn điểm và chỉ hoạt động trong mặt phẳng
nên ta chọn mô hình Robot RRR 3 bậc tự do, gồm 3 khớp quay quanh trục z với các thông
số sau:
- Chiều dài các khâu: a1 = 0,6 (m); a2 = 0,4 (m)
- Giới hạn các góc khớp:

q1 [min = 0; max = ] (rad)
2
 
q2 [min = - ; max =  ] (rad)
2 18
7 
q3 [min =  ; max = ] (rad)
9 6
- Khối lượng các khâu: m1 = 26,25 (kg); m2 = 18,5 (kg); m3 = 19,5 (kg)
7
Hình 1.2 Mô hình Robot phẳng RRR
Hình 1.3 Mô hình 3D Robot phẳng RRR

8
CHƯƠNG II: TÍNH TOÁN ĐỘNG HỌC ROBOT
1 Đặt hệ trục tọa độ theo Denavit- Hartenberg

Dựa vào quy tắc thiết lập D-H, ta chọn hệ trục tọa độ có gốc tại khớp thứ i như sau:
- Trục zi được chọn dọc theo khớp động thứ (i+1).
- Trục xi được chọn dọc theo đường vuông góc chung giữa zi-1 và zi, hướng từ zi-1
sang zi.
- Trục yi được xác định theo quy tắc hệ tọa độ thuận (bàn tay phải).
- Nếu trục zi-1 và zi song song, xi có thể chọn bất kì đường vông góc chung của 2
trục.
- Trong trường hợp 2 trục zi-1 và zi cắt nhau, gốc hệ tọa độ được chọn tại giao điểm
các trục này và hướng trục xi thường được xác định qua tích hữu hướng zi-1x zi,
hoặc có hướng ngược lại.
Từ quy tắc trên, ta xây dựng được các hệ tọa độ như hình dưới đây:
Hình 2.1 Hệ trục tọa độ Robot phẳng RRR
9
Các trục O0z0, O1z1, O2z2, O3z3, lần lượt đặt tại các điểm O0, O1, O2, O3 nằm tại tâm
của các khớp động, phương vuông góc với mặt phẳng tờ giấy, chiều hướng từ trong ra
ngoài.
2 Thiết lập bảng D-H

Ta xác định các thông số của khâu và các biến khớp θi, di, ai, αi theo quy tắc sau:
θi: góc quay quanh trục zi-1 để trục xi-1 trùng với trục x’i (x’i // xi).
di: dịch chuyển tịnh tiến dọc trục zi-1 để gốc tọa độ Oi-1 chuyển đến O’i.
ai: dịch chuyển dọc trục xi đến điểm Oi.
αi: góc quay quanh trục xi về trục zi.
Hình 2.2 Tham số kích thước Robot phẳng RRR

Từ những phân tích và hình vẽ, ta có thể lập bảng thông số D-H như sau:
10
Khâu θi di ai αi
1 q1 0 a1 0
2 q2 0 a2 0
3 q3 0 a3 0
Bảng 1 Bảng thông số D-H, Robot phẳng RRR
Trong đó:
q1, q2, q3 là các biến khớp.
a1, a2, a3 là các hằng số.
3 Ma trận Denavit- Hartenberg
Ta có thể chuyển hệ tọa độ khớp (Oxyz)i-1 sang hệ tọa độ (Oxyz)i bằng việc thực hiện
phép biến đổi cơ bản sau:
 Quay quanh trục zi-1 một góc θi.
 Dịch chuyển tịnh tiến dọc zi-1 một đoạn di.
 Dịch chuyển tịnh tiến dọc trục xi một đoạn ai.
 Quay quanh trục xi một góc αi.
Vậy ma trận của phép biến đổi hệ tọa độ khớp (Oxyz)i-1 sang hệ tọa độ (Oxyz)i, kí
hiệu là i-1Ai là tíc của 4 ma trận biến đổi tọa độ thuần nhất có dạng như sau:
cos i  sin i 0 0  1 0 0 0  1 0 0 ai  1 0 0 0
 sin  cos i 0 0  0 1 0 0  0 1 0  
0  0 cos  i  sin  i 0 
i 1 
Ai  i
 0 0 1 0  0 0 1 di  0 0 1 0  0 sin  i cos  i 0
    
 0 0 0 1   0 0 0 1  0 0 0 1  0 0 0 1
cos i  cos  i sin i sin  i sin i ai cos i 

 sin  cos  i cos i  sin  i cos i ai sin i 
 i
(2.1)
 0 sin  i cos  i di 
 
 0 0 0 1 
Ma trận được xác định bởi công thức (2.1) gọi là ma trận Denavit- Hartenberg địa
phương. Nó cho ta thông tin về vị trí khâu thứ i của Robot đối với hệ quy chiếu (Oxyz)i-1
n 1  0 Rn rE 
0
A n  A1 A 2
0 1
An   T  (2.2)
0 1
Trong đó:
rE   xE , y E , z E  là vector mô tả vị trí điểm tác động cuối trong hệ tọa độ (Oxyz)0.
T
0
R n là ma trận cosin chỉ hướng của khâu thao tác đối với hệ tọa độ (Oxyz)0.
11
4 Bài toán động học thuận Robot

Vị trí mỗi khâu trong không gian được xác định bởi vị trí một điểm định vị và hướng
của khâu đó đối với hệ quy chiếu đã chọn. Điểm định vị là một điểm xác định nào đó của
khâu, thường chọn là khối tâm của khâu. Hướng của khâu được xác định bằng ma trận
cosin chỉ hướng hoặc bằng các tọa độ suy rộng xác định vị trí của vật rắn quay quanh một
điểm.
Nhiệm vụ của bài toán động học thuận là xác định vị trí của khâu thao tác, hay nói
cách khác là vị trí điểm tác đọng cuối và hướng của khâu thao tác đối với hệ tọa độ cố định
với các điều kiện biến khớp đã biết. Ở đây ta sẽ xác định từ ma trận Denavit- Hartenberg
của khâu thao tác. Ngoài ra, ta có thể xác định được vận tốc góc và gia tốc góc các khâu
của Robot.
4.1 Tính các ma trận D-H
 cos q1  C1
 sin q  S
 1 1 cos(q1  q2 )  C12
 cos q2  C1 sin(q  q )  S

Đặt  và  1 2 12
sin q2  S1 cos(q1  q2  q 3 )  C123

cos q3  C3 sin(q1  q2  q 3 )  S123

sin q3  S3
Thay các giá trị trong bảng D-H vào công thức (2.1), ta được các ma trận biến đổi D-
H giữa các hệ trục tọa độ như sau:
C1  S1 0 a1C1   C2  S2 0 a2C2  C3  S3 0 a3C3 
S 
C1 0 a1S1  1 S C2 
0 a2 S 2  2 S C3 0 a3 S3 
0
A1   1 ; A2   2 ; A3   3 (2.3)
0 0 1 0  0 0 1 0  0 0 1 0 
     
0 0 0 1  0 0 0 1  0 0 0 1 
C12  S12 0 a1C1  a2C12 

S C12 0 a1S1  a2 S12 
0
A 2  A1 A 2  12
0 1
(2.4)
 0 0 1 0 
 
 0 0 0 1 
C123  S123 0 a1C1  a2C12  a3C123 

S C123 0 a1S1  a2 S12  a3 S123 
0
A3  A 2 A3  123
0 2
(2.5)
 0 0 1 0 
 
 0 0 0 1 
4.2 Xác định vị trí điểm tác động cuối
Từ (2.2) và (2.5), ta suy ra:
12
rE   a1C1  a2C12  a3C123 ; a1S1  a2 S12  a3 S123 ;0

T
(2.6)
Từ đó, ta có vị trí điểm tác động cuối là:

 xE  a1C1  a2C12  a3C123

 yE  a1S1  a2 S12  a3 S123 (2.7)
 zE  0

4.3 Xác định hướng của khâu thao tác
Từ (2.2) và (2.5), ta suy ra:
C123  S123 0
0
R 3   S123 C123 0  (2.8)
 0 0 1 
Ma trận quay Cardan biểu diễn hướng của hệ trục tọa độ (Oxyz)i+3 so với hệ tọa độ
cố định (Oxyz)1 là tích của 3 ma trận quay thành phần cơ bản, được tính theo công thức
sau:
1 0 0   cos  0 sin   cos  sin  0
R Cardan 
 0 cos   sin    0 1 0   sin  cos 0
0 sin  cos     sin  0 cos    0 0 1 
(2.9)
 cos  cos  cos  cos  sin  

  sin  sin  cos  cos  sin   sin  sin  sin   cos  cos  sin  cos  
  cos  sin  cos  sin  sin  cos  sin  sin   sin  cos cos  cos  
Hình 2.3 Các góc quay Cardan
13
Như vậy bằng cách so sánh 2 ma trận quay RCardan và 0R3, ta có thể tìm được các góc
Cardan α, β, η biểu diễn hướng của khâu thao tác đối với hệ tọa độ cố định (Oxyz)0.
0
R3 = RCardan, hay:
C123  S123 0  cos  cos  cos  cos  sin  
S C123  
0   sin  sin  cos  cos  sin   sin  sin  sin  cos  cos  sin  cos  
 123
 0 0 1    cos  sin  cos  sin  sin  cos  sin  sin  sin  cos cos  cos  
So sánh các phần tử của 2 ma trận, ta có:

 sin   0
 cos   1

 sin   0

 cos   1
 sin   S123

cos  C123
Từ đó, ta suy ra góc Cardan:

  0

  0
  q  q  q
 1 2 3
Vậy đối với cơ cấu Robot RRR phẳng này, để biến đổi hệ tọa độ (Oxyz)0 cố định sang
hệ tọa độ khâu thao tác, ta chỉ cần quay (Oxyz)0 quanh trục z0 một góc η=q1+q2+q3
4.4 Tính vận tốc, gia tốc điểm tác động cuối
Sau khi đã biết các biến khớp q1, q2 và các vận tốc khớp, gia tốc khớp, ta sẽ đi tính
vận tốc góc, gia tốc góc các khâu cũng như vận tốc và gia tốc điểm tác động cuối.
- Vận tốc điểm tác động cuối:
v E  rE   xE , yE , zE 
T
(2.10)
- Gia tốc điểm tác đọng cuối:
a E  v E   xE , yE , zE 
T
(2.11)
- Vận tốc góc và gia tốc góc các khâu:
+ Toán tử sóng của vecto vận tốc góc khâu i trong hệ tọa độ động (Oxyz)i được tính
theo công thức sau:
11 12 13 
i
ωi  R i R i  21 22 23 
o T 0
(2.12)
31 32 33 
14
+ Khi đó, vận tốc góc khâu thứ i tính trong hệ tọa độ (Oxyz)i là:
i
i  32 13 21  (2.13)
4.5 Tính vận tốc góc, gia tốc góc các khâu
 Xét khâu 1, từ (2.2) và (2.3), ta có:
C1  S1 0 
0
R1   S1 C1 0 
 0 0 1 
Suy ra:
 C1 S1 0  q1S1 q1C1 0  0 q1 0
1
ω1  Ro T 0
1 R1    S1 C1 0  q1C1 q1S1 0   q1 0 0
 0 0 1   0 0 0   0 0 0 
Như vậy, vận tốc góc và gia tốc góc của khâu 1 tính trong hệ tọa độ động gắn chặt
với khâu là:
ω1  0 0 q1 
1 T
1  11  0 0 q1 
1 T
 Xét khâu 2, ta có:

C12  S12 0
0
R 2   S12 C12 0 
 0 0 1 
Suy ra:
 C12 S12 0   (q1  q2 ) S12 (q1  q2 )C12 0
2
ω2  R
o T 0
2 R 2    S12 C12 0   (q1  q2 )C12 (q1  q2 ) S12 0 
 0 0 1   0 0 0 
 0 (q1  q2 ) 0 

 (q1  q2 ) 0 0 
 0 0 0 
Như vậy vận tốc góc và gia tốc góc của khâu 2 tính trong hệ tọa độ động gắn chặt với
khâu là:
ω2  0 0 q1  q2 
2 T
ε 2  2 ω2  0 0 q1  q2 
2 T
15
 Xét khâu 3, ta có:

C123  S123 0
0
R 3   S123 C123 0 
 0 0 1 
Suy ra:
 C123 S123 0   (q1  q2  q3 ) S123 (q1  q2  q3 )C123 0
3
ω3  R
o T 0
3 R 3    S123 C123 0   (q1  q2  q3 )C123 (q1  q2  q3 ) S123 0 
 0 0 1   0 0 0 
 0 (q1  q2  q3 ) 0 

 (q1  q2  q3 ) 0 0 
 0 0 0 
Như vậy vận tốc góc và gia tốc góc của khâu 3 tính trong hệ tọa độ động gắn chặt với
khâu là:
ω3  0 0 q1  q2  q3 
3 T
ε3  3 ω3  0 0 q1  q2  q3 
3 T
 Vận tốc và gia tốc điểm tác động cuối:

Từ (2.6), ta có vecto vị trí điểm tác động cuối là:
 xE   a1C1  a2C12  a3C123 
rE   yE    a1S1  a2 S12  a3 S123 
 zE   0 
Lần lượt đạo hàm rE theo thời gian, ta thu được vận tốc và gia tốc điểm tác động cuối
là:
 xE   a1S1q1  a2 S12 (q1  q2 )  a3 S123 (q1  q2  q3 ) 
v E  rE   yE    a1C1q1  a2C12 (q1  q2 )  a3C123 (q1  q2  q3 ) 
 zE   0 
aE  v E
 a1C1q12  a1S1q1  a2C12 (q1  q2 ) 2  a2 S12 (q1  q2 )  a3C123 (q1  q2  q3 ) 2  a3S123 (q1  q2  q3 ) 
 
  a1S1q12  a1C1q1  a2 S12 (q1  q2 ) 2  a2C12 (q1  q2 )  a3 S123 (q1  q2  q3 ) 2  a3C123 (q1  q2  q3 ) 
 0 
 
16
5 Bài toán động học ngược robot

Nhiệm vụ của bài toán động học ngược là tìm các biến khớp với điều kiện tọa độ và
hướng khâu thao tác đã biết. Cụ thể trong bài này, bài toán động học ngược có nhiệm vụ
tìm q1, q2, q3 với điều kiện xE, yE, η đã biết.
Từ (2.9), ta có:
C123=cosη, S123=sinη
Thay vào (2.7), ta có:
 xE  a3 cos  a2C12  a1C1 a C  a C  x  a cos
 ↔  2 12 1 1 E 3
 yE  a3 sin   a2 S12  a1S1  a2 S12  a1S1  yE  a3 sin 
Đặt X= xE  a3 cos và Y= yE  a3 sin , thay vào (2.12), ta có:
a2C12  a1C1  X
 (2.14)
 a2 S12  a1S1  Y
Bình phương 2 vế của 2 phương trình và cộng 2 vế của phương trình lại, ta được:
 2 
  X 2  Y 2  a12  a22  X 2  Y 2  a12  a22 
q2  arctan 2  1    ,
  2a1a2  2a1a2 
 
Sử dụng khai triển lượng giác:
C12  C1C2  S1S2 a (C C  S S )  a1C1  X C (a  a C )  a2 S1S2  X
 ↔ 2 1 2 1 2 ↔ 1 1 2 2
 S12  S1C2  S2C1  a2 (S1 C2  S2C1 )  a1S1  Y  S1 (a1  a2C2 )  a2C1S2  Y
Đặt a1  a2C2 =A; a2 S2 =B. Hệ phương trình (2.14) chuyển thành:
 AX  BY
 C1  2
 AC1  BS1  X  A  B2 →   BX  AY AX  BY 
 ↔  q1  arctan 2  , 2 
 AS1  BC1  Y  S   BX  AY  A B A  B2 
2 2
 1 A2  B 2
Ta có:
  q1  q2  q3 → q3    q1  q2
Vậy ta đã tìm được các biến khớp q1, q2, q3.
17
CHƯƠNG 3: THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG CỦA

ROBOT
1 Khái niệm, nhiệm vụ của thiết kế quỹ đạo

Thiết kế quỹ đạo chuyển động của khâu thao tác của Robot để đảm bảo các yêu cầu
công nghệ được hiểu là thiết kế quỹ đạo. Tạo ra các bộ tham chiếu đầu vào cho hệ thống
điều khiển chuyển động của Robot để thực hiện đúng quỹ đạo đã cho trước.
Có 2 kĩ thuật thiết kế quỹ đạo chính:
 Thiết kế quỹ đạo trong không gian khớp: Thiết lập quy luật chuyển động cho
các biến khớp giúp điều khiển động cơ trực tiếp cho từng khớp. Đồng thời có
thể điều khiển vận tốc và gia tốc như mong muốn. Tuy nhiên, việc điều khiển
quỹ đạo thường không chính xác.
 Thiết kế quỹ đạo trong không gian thao tác: Thiết lập quy luật chuyển động cho
điểm tác động cuối giúp điều khiển quỹ đạo chính xác. Tuy nhiên việc kiểm soát
lực, momen, vận tốc, … khó khăn.
Sau đây, ta sẽ tiến hành thiết lập quỹ đạo chuyển động cho Robot theo 2 phương pháp
trên
2 Không gian thao tác của Robot

Từ các thông số của Robot:
 Chiều dài khâu: a1=0,5 (m); a2=0,4 (m); a3=0,3 (m);

 Đảm bảo hướng khâu thao tác luôn là 
3
 Giới hạn góc các khớp:
      7  
q1  0;  (rad ) ; q2    ;   (rad ) ; q3    ;  (rad )
 2  2 18   9 6
Ta vẽ được không gian làm việc của Robot như hình 3.1:
18
Hình 3.1 Không gian làm việc của Robot RRR phẳng
3 Thiết kế quỹ đạo trong không gian khớp
3.1 Thiết kế quỹ đạo trong không gian khớp cho Robot phẳng
Trong trường hợp chuyển động từ điểm tới điểm, ta có thể không quan tâm đến việc
khâu thao tác đi qua một số điểm trung gian. Khi đó, ta sử dụng kĩ thuật thiết kế quỹ đạo
trong không gian khớp. Thiết kế quỹ đạo trong không gian khớp đòi hỏi phải xây dựng
được một hàm phụ thuộc thời gian của các biến khớp và các đạo hàm bậc nhất, bậc hai của
nó.
 Thiết kế quỹ đạo chuyển động:
Giả sử: Khi các điểm tác động cuối ở điểm A(xA, yA, zA, ηA) tại thời điểm t=t0 tới
điểm B(xB, yB, zB, ηB) tại thời điểm t=tc
Chọn hàm phụ thuộc thời gian của biến khớp qi là đa thức bậc 3 có dạng:
qi (t )  i a0  i a1t  i a2t 2  i a3t 3 (3.1)
Trong đó, i a0 , i a1 , i a2 , i a3 là các hằng số cần xác định đối với khâu thứ i.
Giả thiết vận tốc tại điểm đầu và điểm cuối Robot đứng yên, hay:
i
q0  0 ; i qc  0 (3.2)
19
Đạo hàm 2 vế phương trình (3.2) và thay thời gian t=t0, t=tc vào, ta được hệ phương
trình:
 qi (t0 )  i a0  i q0

 qi (t0 )  i a1  0
 (3.3)
qi (tc )  a0  a1tc  a2tc  a3tc
i i i 2 i 3
 qi (tc )  i a1  i a2tc  i a3tc 2
Từ hệ phương trình (3.3), ta tính được các hệ số i a0 , i a1 , i a2 , i a3 như sau:
 i
a0  i q0


i
a1  0
 i
q0  i qc
 2
i
a   3
 tc 2
 i
q  iq
 i a2  2 0 2 c
 tc
 Áp dụng:
Thiết kế quỹ đạo trong không gian khớp cho Robot phẳng RRR sao cho điểm tác
3     7 10  3 
động cuối đi từ A  0,8; ;0  hướng khâu thao tác  A  đến điểm B  ; ;0 
 10  3  20 20 

hướng khâu thao tác  B  trong thời gian tc=1s
3
 Giải:
Sử dụng công thức ở chương 2, ta tính được các biến khớp tại vị trí A và B lần lượt
là:
 3  
Tại điểm A  0,8; ;0  ;  A  , có:
 10  3
  3 
1
q0  ; 2 q0 
; q0 
3 3 3
 7 10  3  
Tại điểm B  ; ;0  ;  B  , có:
 20 20  3
  3 2
1
qc  ; 2 qc  ; qc 
2 6 3
Thay t0=0 và tc=1s vào công thức (3.3), ta tính được các hệ số:
20
 1   
 a0  3 a1  0 a2  a3 
1 1 1
2 3

2   
 a0  a1  0 a2  a3 
2 2 2
 3 2 3
3  2
 a0  3
3
a1  0 3
a2   3
a3 
 3
Suy ra:
   2  3
 q1 (t )  3  2 t  3 t q1 (t )   t   t 2 q1 (t )    2 t

   2  3
 q2 (t )   t  t q2 (t )   t   t 2 q2 (t )    2 t
 3 2 3
  2 3
q3 (t )  3   t  3 t q3 (t )  2 t  2 t 2 q3 (t )  2  4 t
2

Sử dụng phần mềm matlab, ta vẽ được các đồ thị tương ứng sau:
Hình 3.2 Quỹ đạo chuyển động của điểm tác động cuối
21
Hình 3.3 Quỹ đạo, vận tốc, gia tốc khớp 1
22

3.2 Thiết kế quỹ đạo trong không gian thao tác
Trong trường hợp chuyển động liên tục trên đường di chuyển, kĩ thuật thiết kế quỹ
đạo trong không gian thao tác được sử dụng. Đối với Robot phẳng RRR, ta chọn bài toán
thiết kế quỹ đạo trong không gian thao tác sao cho quỹ đạo của điểm tác động cuối là đường
thẳng từ A tới B trong thời gian tc, sao cho thỏa mãn điều kiện của vị trí đầu và vị trí cuối,
vận tốc tại vị trí đầu và vị trí cuối.
 Lời giải tổng quát
Giả sử thiết kế quỹ đạo trong không gian thao tác đi từ điểm A(xA, yA, zA, ηA) tới
điểm B(xB, yB, zB, ηB) ta có phương trình đường thẳng là:
x  x0 y  y0 z  z0
 
xc  x0 yc  y0 zc  z0
 yc  y0 y0 xc  yc x0
y  x  x x  x  x (3.5)


c 0 c 0
 y  zc  z0 x  z0 xc  zc x0
 xc  x0 xc  x0
Chọn x=x(t) là đa thức bậc 3 có dạng:
23
x  x  t   a0  a1t  a1t 2  a1t 3 (3.6)
Giả sử vận tốc điểm đầu và điểm cuối tay bằng 0, hay:
v  t0   0, v  tc   0
Tương tự các bước như thiết kế quỹ đạo trong không gian khớp, ta tìm được các hệ
số a0, a1, a2, a3 theo công thức sau:
 a0  x  t0 

 a1  0

 3  x  t0   x  tc  
a2  (3.7)
 t02
 a3    0   c  
 2 x t x t
 t03
 Bài toán:
Thiết kế quỹ đạo trong không gian khớp cho Robot phẳng RRR sao cho điểm tác
 3    7 10  3 
động cuối đi từ A  0,8; ;0  hướng khâu thao tác  A  đến điểm B  ; ;0 
 10  3  20 20 

hướng khâu thao tác  B  trong thời gian tc=1s
3
 Giải:
Thay tọa độ điểm A và B vào công thức (3.7) ta thu được quy luật chuyển động của
điểm tác động cuối như sau:
27 2 9 3
x(t )  0,8  t  t
20 10
3 30  3 3 2 3  10 3
y(t )   t  t
10 20 10
Sử dụng phần mềm Matlab, ta vẽ được các đồ thị tương ứng sau đây:
24
Hình 3.6 Đồ thị xE , yE
Hình 3.7 Quỹ đạo điểm tác động cuối
25
CHƯƠNG 4: TÍNH TOÁN ĐỘNG LỰC HỌC ROBOT
1 Phương trình Lagrange II dạng trận

Để tính toán động học Robot, ta sẽ đi thiết lập phương trình vi phân chuyển động của
Robot. Phương trình vi phân chuyển động của Robot được xây dựng theo phương trình
Lagrange loại II dạng ma trận có dạng tổng quát như sau:
d  T  T 
    Qi* i  1, 2,3,..., n (4.1)
dt  qi  qi qi
Với: T- động năng của Robot

 - thế năng của Robot
Qi* - lực suy rộng của các lực không thế
n – số bậc tự do của Robot
2 Động năng của Robot

Động năng của Robot được tính như sau:
n
T  T1  T2  ...  Tn   Ti
1
Với Ti là khâu động thứ i, tính theo công thức:

1 T 1
Ti  vCi mi vCi  i ωTi Ii i ωi (4.2)
2 2
Trong đó:
mi : khối lượng khâu i
v Ci : vận tốc dài của khối tâm khâu i trong hệ tọa độ cố định
i
ωi : vận tốc góc khâu i trong hệ tọa độ động
Ii: ma trận tenxo quán tính của khâu i đối với khối tâm của nó trong tọa độ động
Đặt rCi là vector xác định khối tâm của khâu i trong tọa độ cố định, rCi tính theo công
thức:
rCi  0 rOi  0 Ri i rCi (4.3)
Trong đó:
0
rOi : vị trí điểm gốc hệ tọa độ Oi đối với hệ tọa độ cố định
0
R i : ma trận cosin chỉ hướng của hệ tọa độ (Oxyz)i đối với hệ tọa độ cố định
26
i
rCi : vector xác định vị trí khối tâm của khâu thứ i trong hệ tọa độ (Oxyz)i
Khi đó , vận tốc dài của khối tâm i và vận tốc góc khâu i có thể tính như sau:
 xCi xCi xCi 
 ... 
q q2 qn   q1 
 xCi   1  
 y yCi yCi   q2  rCi
v Ci  rCi   yCi    Ci ...   q
q q2 qn    q
 zCi   1  
 zCi zCi zCi   qn 
 ... 
 q1 q2 qn 
  x  x  x 
 q ...
q2 qn   q1 
 x   
1
  y  y  y   q2   i ωi
i
ωi   y    ...   q
q q2 qn    q
 z   1  
  z  z  z   qn 
 ... 
 q1 q2 qn 
 rCi
 J Ti  q  v  JTi q

Đặt  khi đó, ta có:  i Ci
J   ωi  ωi  J Ri q
i
 Ri
q
Với : JTi: ma trận Jacobian tịnh tiến của khâu i

JRi: ma trận Jacobian quay của khâu i
Thay vào (5.2), ta được:
1

Ti  qT JTiT mi JTi  JTRi I i J Ri q
2

Vậy, động năng của Robot là:
1 n 
 
n
1
T   Ti  qT  JTTi mi JTi  JTRi I i J Ri  q  qT M  q  q (4.4)
i 1 2  i 1  2
Trong đó:
27
 m11  q  m12  q  m1n  q  

 
m  q  m22  q  m2 n  q  
 
n
M  q    J TTi mi J Ti  J TRi I i J Ri   21 (4.5)
i 1
 
 
 mn1  q  mn 2  q  mnn  q   nn
Ta gọi M(q) là ma trận khối lượng suy rộng.
3 Thế năng của Robot

n
   mi gT0 rCi (i  1..n) (4.6)
i 1
Trong đó:
mi: khối lượng khâu i
gT0  0  g 0
g: gia tốc trọng trường, g=9,81 m/s2

Đặt g  q    g1  q  g2  q  gi  q  với gi  q  
T
(4.7)
qi
4 Thiết lập phương trình vi phân chuyển động của Robot
Hệ phương trình vi phân chuyển động của Robot được viết dưới dạng ma trận như
sau:
M  q  q  C  q, q  q  g  q   τ  t  (4.8)
Trong đó:
M  q    mij  : ma trận khối lượng suy rộng

nn
C  q, q   cij  : ma trận li tâm Coriolis

nn
g(q): vector lực do trọng lực gây ra

τ  t  : lực dẫn động từ các động cơ
5 Thiết lập phương trình động lực học cho Robot

Ta có hình vẽ thể hiện các tham số động lực học của Robot RRR phẳng như hình sau:
28
Hình 5.1 Các tham số động lực học Robot phẳng RRR
Gọi l1, l2, l3 lần lượt là khoảng cách từ gốc tọa độ O1, O2, O3 đến khối tâm của từng
khâu 1, 2, 3. Ta lập được bảng tham số động lực học của Robot như sau:
Khâu Vị trí trọng tâm Khối Momen quán tính
(so với hệ tọa độ gắn lượng
với từng khâu)
xCi yCi zCi Ixx Iyy Izz Ixy Iyz Ixz
1 -l1 0 0 m1 I1x I1y I1z 0 0 0
2 -l2 0 0 m2 I2x I2y I2z 0 0 0
3 -l3 0 0 m3 I3x I3y I3z 0 0 0
Bảng 5.1 Tham số động lực học
Từ đó, ta có các ma trận ten xơ quán tính khối và các vector thể hiện vị trí khối tâm
của khâu 1, 2, 3 trong hệ tọa độ động gắn chặt với khâu là:
29
 I1x 0 0   I2x 0 0   I3x 0 0

I1   0 I1 y 0 ,I   0
 2  I2 y 0 ,I   0
 3  I3 y 0 
 0 0 I1z   0 0 I 2 z   0 0 I 3 z 
rC1   l1 0 0 , 2 rC 2   l2 0 0 , 3 rC 3   l3 0 0

1 T T T
Đồng thời, ta có vector xác định vị trí điểm O1, O2, O3 trong hệ tọa độ cố định là:
rO1   a1C1 a1S1 0

0 T
rO 2   a1C1  a2C12 0
T
0
a1S1  a2 S12
rO 3   a1C1  a2C12  a3C123 a1S1  a2 S12  a3 S123 0
0 T
Từ đó, ta biểu diễn vị trí khối tâm khâu 1, 2, 3trong hệ tọa độ cố định như sau:
 a1C1  C1  S1 0  l1   a1  l1  C1 

 
rC1  0 rO1  0 R1 1 rC1   a1S1    S1 C1 0  0     a1  l1  S1 
 0   0 0 1   0   0 
 a1C1  a2C12  C12  S12 0  l2   a2  l2  C12  a1C1 

 
rC 2  rO 2  R 2 rC 2
0 0 2
  a1S1  a2 S12    S12 C12 0  0     a2  l2  S12  a1S1  (4.9)
 0   0 0 1   0   0 
 a1C1  a2C12  a3C123  C123  S123 0   l3 

rC 3  rO 3  R 3 rC 3   a1S1  a2 S12  a3 S123    S123
0 0 3
C123 0   0 
 0   0 0 1   0 
 a3  l3  C123  a1C1  a2C12 
 
   a3  l3  S123  a1S1  a2 S12 
 0 
Lần lượt đạo hàm rC1, rC2, rC3 theo thời gian, ta thu được vận tốc khối tâm các khâu
trong hệ tọa độ cố định như sau:
   a1  l1  S1q1 
 
v C1  rC1    a1  l1  C1q1 
 0 
   a2  l2  S12  q1  q2   a1S1q1 
 
vC 2  rC 2    a2  l2  C12  q1  q2   a1C1q1 
 0 
30
   a3  l3  S123  q1  q2  q3   a1S1q1  a2 S12  q1  q2  

 
vC 3  rC 3    a3  l3  C123  q1  q2  q3   a1C1q1  a2C12  q1  q2  
 0 
Từ đó, ta tính được ma trận Jacobian tịnh tiến và ma trận Jacobian quay của khâu i
như sau:
   a1  l1  S1 0 0 
 
JT 1    a1  l1  C1 0 0 
 0 0 0 
   a2  l2  S12  a1S1   a2  l2  S12 0

 
JT 2    a2  l2  C12  a1C1  a2  l2  C12 0
 0 0 0 
   a3  l3  S123  a1S1  a2 S12   a3  l3  S123  a2 S12   a3  l3  S123 


JT 3    a3  l3  C123  a1C1  a2C12  a3  l3  C123  a2C12  a3  l3  C123 
 0 0 0 
0 0 0  0 0 0  0 0 0 
J R1  0 0 0  , J R 2  0 0 0  , J R 3  0 0 0 
   
1 0 0  1 1 0  1 1 1 
Tính ma trận khối lượng:
 m11 m12 m13 

    m21 m22 m23 
n
M  q    J mi JTi  J I J Ri
T
Ti
T
Ri i
i 1
 m31 m32 m33 
Trong đó:
m11  m1  a1  l1   m2  a2  l2   2a1a2C2  2a1l2C2  a12 

2 2
 
 m3  2a1a2C2  a1  a2  a3  l3  2a3l3  2  a3  l3  a1C23  a2C3   I1z  I 2 z  I 3 z
2 2 2 2
m12  m2  a2  l2   a1C2  a2  l2  
2
 
 
 m3 a22  a32  l32   a2  l2  a1C23  2a2C3   a1a2C2  2a3l3  I 2 z  I 3 z
m13  m3 a32  l32   a3  l3  a1C23  a2C3   2a3l3   I3 z
31
m21  m12
m22  m2  a2  l2   m3 a22  2a2C3  a3  l3    a3  l3    I 2 z  I3 z

2 2
 
m23  m3 a2C3  a3  l3    a3  l3   I 3z
m31  m13
m32  m23
m33  m3  a3  l3   I 3z
2
Thế năng của Robot:

3
   mi gT0 rCi
i 1
Thay gT0  0  g 0 và (5.9) vào biểu thức, ta được:
  m1 gS1  a1  l1   m2 g  S12  a2  l2   S1a1   m3 g  S123  a3  l3   S12a2  S1a1 
Dựa vào công thức (5.7), ta tính được:
g  q    g1  q  g2  q  g3  q 
T
Với:
g1  q   m2 gC12  a2  l2   C12 a2 m3  m3C123  a3  l3 
 m1C1  a1  l1   a1C12  m3  m2 
g2  q    a2  l2  m2 ga2C12  g a2C12  a3C123  a3  l3  m3
g3  q    a3  l3  m3 ga3C123
Áp dụng công thức:
 mik , j  m ji,k  mkj ,i qk

n
1
cij  q, q    (4.10)
k 1 2
Ta tính được ma trận Coriolis C  q, q  như sau:
32
 c11 c12 c13 

C  q, q   c21 c22 c23 
 c31 c32 c33 
Trong đó:
c11   2a1m2 S2  a2  l2     2a1  a1  l1  S23  2a1a2 S2 m3   q2
  2a1S23  a3  l3    2a2 S3  a3  l3   m3q3

c12  a1S2  a2  l2  m2   a1S23  a3  l3   a1a2 S2  m3 q2 
  a1S23  a3  l3   2a2 S23  a3  l3  m3q3
c13  m3a1S23  a3  l3  q2  a2 S3  a3  l3  a1S23  a2 S3  q3

c21  a1m2 S2  a2  l2    a1S23  a2  l2   a1a2 S2  m3 q1
 0,5a m S
1 2  a2  l2   0,5a1S23  a3  l3   0,5a1a2 S2  m3  q2
2
  0,5a1S23  a3  l3   2a2 S3  a3  l3   m3q3
 
c22  0,5a1m2 S2  a2  l2   0,5a1S23  a3  l3   0,5a1a2 S2  m3 q1  2a2 S3  a3  l3  m3q3
c23  0,5a1m3 S23  a3  l3  q1  a2 m3 S3  a3  l3  q3
c31   a1S23  a2 S3  a3  l3  m3q1   0,5a1S23  a2 S3  a3  l3  m3q2

  0,5a1S23  0,5a2 S3  a3  l3  m3q3
c32   0,5a1S23  a2 S3  a3  l3  m3q1  a2 S3  a3  l3  m3q2  0,5a2 S3  a3  l3  m3q3
c33  0,5  a1S23  a2 S3  a3  l3  m3q1  0,5a2 S3  a3  l3  m3q2
Với M(q), C  q, q  và g(q) đã tính ở trên, ta viết được hệ phương trình vi phân chuyển
động của Robot có dạng như sau:
 m11q1  m12 q2  m13q3  c11q1  c12 q2  c13q3  g1  q   1


m21q1  m22 q2  m23q3  c21q1  c22 q2  c23q3  g 2  q    2 (4.11)
 m q  m q  m q  c q  c q  c q  g q   
 31 1 32 2 33 3 31 1 32 2 33 3 3 3
33
CHƯƠNG 5: THIẾT KẾ ĐIỀU KHIỂN ROBOT
1 Thiết kế bộ điều khiển

1.1 Cơ sở lí thuyết
Nhiệm vụ của bài toán điều khiển là thiết kế bộ điều khiển để Robot chạy đúng theo
quy luật q(t) cho trước, nhằm thực hiện một số nhiệm vụ nào đó. Trên cơ sở chuyển động
mong muốn q(t) được định nghĩa trước và chuyển động hiện tại của Robot được đo bởi các
cảm biến đặt tại khớp, bộ điều khiển có nhiệm vụ điều khiển Robot đi theo một quỹ đạo
cho trước. Đồng thời làm cho Robot thực hiện chuyển động mong muốn một cách ổn định,
chính xác. Ta có phương trình vi phân Lagrange loại II mô tả mô hình vi phân của Robot:
M  q  q  C  q, q  q  g q   u (5.1)
Để điều khiển chuyển động của Robot chuyển động trong không gian làm việc, cần
xác định thành phần lực của vector lực u để cho chuyển động của q(t) gần hay trùng với
chuyển động mong muốn, ta có thể xem Robot công nghiệp như một hệ thống mà có đầu
vào là điện áp tạo các động cơ và đầu ra là chuyển động của các khớp mà ta có thể đo được
nhờ các sensor đặt tại các khớp tương ứng.
Hình 5.1 Sơ đồ điều khiển một hệ điều hành Robot

Phương pháp điều khiển dựa trên mô hình động lực học Robot vẫn là phương pháp
kinh điển và mang lại kết quả tương đối hiệu quả cho đến ngày nay.
Mô hình toán học của hệ thống đã được xây dựng từ các phần trước là hệ phương
trình vi phân động học:
M  q  q  C  q, q  q  g q   τ (5.2)
34
Trong đó, vế trái của phương trình là mô hình với các tham số của Robot, vế phải là
momen mà bộ điều khiển tác động lên Robot. Momen này được sinh ra từ bộ điều khiển
với giá trị thỏa mãn yêu cầu đã nêu ở trên.
Chúng ta phải xét xem cấu trúc của bộ điều khiển như thế nào thì có thể đáp ứng mục
tiêu thiết kế. Trước hết, chúng ta chia bộ điều khiển thành hai phần: một phần dựa trên mô
hình và một phần dựa trên phản hồi.
τ   τ '  (5.3)
Trong đó:  ,  là các hàm được chọn lựa sao cho nếu τ với tư cách là đầu vào mới
của hệ, thì hệ trở thành hệ khối lượng đơn vị.
Với cấu trúc này của luật điều khiển, phương trình của hệ trở thành:
M  q  q  C  q, q  q  g  q    τ '  (5.4)
Với mục tiêu biến hệ thành hệ khối lượng đơn vị với τ là thành phần đầu vào thì  , 
có thể được chọn:
   M(q)
 (5.5)
   C  q, q  q  g  q 
Sau khi chọn được  ,  thì phương trình còn lại là:
q  τ'
Chúng ta còn lại tham số cuối cùng cần chọn để thu được hệ khối lượng đơn vị:
τ '  qd  K v  qd  q   K p  qd  q  (5.6)
Trong đó, các giá trị đầu vào là quy luật vị trí, quy luật vận tốc, quy luật gia tốc mong
muốn đã được tính toán trong phần thiết kế quỹ đạo:
q d (t )

q d (t ) (5.7)
q (t )
 d
Và các hệ số K v , K p là các ma trận đường chéo vuông cấp n( n là tham số động của
mô hình Robot), phần sau chúng ta sẽ xem xét chi tiết việc chọn các hệ số K v , K p này theo
các diều kiện ràng buộc và mục tiêu bài toán.
Ta tìm được biểu thức đặc trưng cho hệ điều khiển vòng kín Robot:
E  K vE  K pE  0 (5.8)
35
Với:
E  q d  q

E  q d  q (5.9)
E  q  q
 d
Hệ phương trình trên bao gồm các phương trình độc lập, các ma trận K v , K p là các
ma trận đường chéo, do vậy phương trình này có thể viết riêng cho từng khớp:
ei  kvi ei  k pi ei  0 (5.10)
Khi nhìn vào phương trình trên, ta có thể nhậm xét đây là phương trình dao động tự
do có cản, ứng xử của hệ thống phụ thuộc vào các hệ số kv , k p . Trong đó, mục tiêu của
i i
chúng ta là đưa đáp ứng của hệ càng sát với giá trị mong muốn càng tốt, khoảng thời gian
đạt được đáp ứng càng nhanh càng tốt. Do đó, giá trị nghiệm hướng tới là trường hợp giá
trị nghiệm kép, giá trị thực và trường hợp này gọi là trường hợp tới hạn khi:
Kd  2 K p (5.11)
Ta lựa chọn K d , K p thỏa mãn biểu thức K d  2 K p để đáp ứng tới hạn, Robot bám
sát quỹ đạo nhanh nhất. Ta chọn càng lớn thì tốc độ đáp ứng càng nhanh.
1.2 Mô phỏng
Các số liệu cho vào bộ điều khiển:
a1  0.5 a2  0.4 a3  0.3
m1  26.25 m2  18.5 m3  19.5
I1  0.371 I 2  0.265 I1  0.18
I1z  1.14 I 2 z  0.36 I 3 z  0.33
g  9.81
36
Hình 5.2 Sơ đồ khối Simulink
37
Hình 5.3 Khối Control PD
Hình 5.4 Khối Subsystem
38
Hình 5.5 Khối Robot RRR
Hình 5.6 Khối Integrator1 và Integrator2
39
Hình 5.7 Đồ thị q1 đặt và q1 thực tế
Hình 5.8 Sai lệch góc khớp 1
40
41
42
Hình 5.13 Đồ thị vận tốc q1 đặt và q1 thực tế
Hình 5.14 Sai lệch vận tốc khớp 1
43
44
45
Từ các giá trị vừa thu được, ta có thể nhận thấy bộ điều khiển vừa thiết kế đảm bảo
được tính ổn định toàn cục của hệ thống. Đồng thời, sau mỗi khoảng thời gian vô cùng
nhỏ, các giá trị đầu ra thực tế bằng với các giá trị đặt vào cho Robot.
Như vậy, ta đã thiết kế xong bộ điều khiển cho Robot.
46
KẾT LUẬN
Thông qua học phần “ Đồ án thiết kế hệ thống Cơ điện tử” với sự hướng dẫn của cô
Nguyễn Thị Vân Hương, em đã học được phương pháp vầ cách tính toán Robot. Em đã
biết cách xây dựng mô hình động học, động lực học. Dồng thời áp dụng luật điều khiển tác
dụng lên hệ thống. Nhiều nhiệm vụ đã đạt được nhưng bên cạnh đó cũng tồn tại nhiều hạn
chế. Em sẽ học hỏi thêm và cải thiện trong các công việc tính toán sau này.
 Những nội dung đã hoàn thành:
- Hệ thống quá trình tính toán, thiết kế bộ điều khiển cho Robot công nghiệp.
- Xây dựng mô hình bài toán, tính toán động học thuận, dộng học ngược, xác định miền
làm việc cho Robot.
- Thiết kế quỹ đạo chuyển động của Robot trong không gian khớp với hàm phụ thuộc
vào thời gian của các biến khớp.
- Thiết kế quỹ đạo chuyển động của Robot trong không gian thao tác với quỹ đạo
chuyển động là một đường thẳng.
- Tính toán động lực học cho Robot, viết phương trình Lagrange loại II, phương trình
vi phân chuyển động của Robot.
- Sử dụng phần mềm Maple, Matlab để tính toán, thiết kế bộ điều khiển PD và mô
phỏng số
 Một số vấn đề có thể nghiên cứu và phát triển thêm:
- Tính toán, thiết kế kết cấu cơ khí.
- Tính toán chọn động cơ, bộ chuyền cho Robot.
- Thiết kế, chế tạo bộ điều khiển thật cho Robot.
Mặc dù chưa được hoàn thiện, nhưng những vấn đề cơ bản trong đồ án này đã giúp
em hiểu phần nào về Robot, một trong những ngành mũi nhọn vô cùng quan trọng hiện
nay. Rất mong nhận được sự đóng góp từ thầy, cô để đồ án của em được hoàn thiện hơn.
Em xin chân thành cảm ơn.
47
TÀI LIỆU THAM KHẢO
[1] GS.TSKH Nguyễn Văn Khang: Động lực học hệ nhiều vật, NXB Khoa học và Kỹ thuật,
2006.
[2] GS.TSKH Nguyễn Văn Khang: Cơ học kỹ thuật, NXB Giáo dục, 2012.
[3] PGS.TS Phan Bùi Khôi: Bài giảng Robotics.
[4] PGS.TS Nguyễn Quang Hoàng: Bài giảng Động lực học hệ nhiều vật.
48
PHỤ LỤC
Dưới đây là chương trình để thực hiện bài toán điều khiển trong không gian khớp:
%#quy dao, van toc, gia toc dat khop 1
function [qd1,dq1,ddq1] = fcn(t)
%#codegen
qd1=pi/3+pi/2*t^2-pi/3*t^3;
dq1=pi*t-pi*t^2;
ddq1=pi-2*pi*t;
end

%#codegen
qd2=-pi/3+pi/2*t^2-pi/3*t^3;
dq2=pi*t-pi*t^2;
ddq2=pi-2*pi*t;
end

%#codegen
qd3=-pi/3-pi*t^2+2*pi/3*t^3;
dq3=-2*pi*t+2*pi*t.^2;
ddq3=-2*pi+4*pi*t;
end
% //Bai toan dong luc hoc nguoc

function T = InverseModel(t)
%#codegen
a1=0.5;
a2=0.4;
a3=0.3;
L1=0.371;
L2=0.265;
L3=0.18;
g=9.81;
m1=26.25;
m2=18.5;
m3=19.5;
I1z=1.14;
I2z=0.36;
I3z=0.33;
%Cac quy uoc ham trong Simulink
q=t(1:3);
dq=t(4:6);
u=t(7:9);
q1=q(1);
q2=q(2);
q3=q(3);
dq1=dq(1);
dq2=dq(2);
dq3=dq(3);
49
m11=m1*(L1-a1)^2+I1z+m2*((L2-a2)^2+2*a1*a2*cos(q2)-
2*a1*L2*cos(q2)+a1^2)+I2z+m3*(2*a1*a2*cos(q2)+a1^2+a2^2+a3^2+L3^2-
2*a3*L3+2*(a3-L3)*(a1*cos(q3+q2)+a2*cos(q3)))+I3z;
m12=m2*((a2-L2)^2+a1*cos(q2)*(a2-L2))+(a2^2+a3^2+L3^2+(a3-L3)*(a1*cos(q3+q2)-
2*a2*cos(q3))+a1*a2*cos(q2)-2*a3*L3)*m3+I2z+I3z;
m13=(a3^2+L3^2+(a3-L3)*(a1*cos(q3+q2)+a2*cos(q3))-2*a3*L3)*m3+I3z;
m21=m12;
m22=m2*(a2-L2)^2+m3*(a2^2+2*a2*cos(q3)*(a3-L3)+(a3-L3)^2)+I2z+I3z;
m23=(a2*cos(q3)*(a3-L3)+(a3-L3))*m3+I3z;
m31=m13;
m32=m23;
m33=m3*(L3-a3)^2+I3z;
M=[m11 m12 m13; m21 m22 m23; m31 m32 m33];
c11=((-2*a1*m2*sin(q2)*(a2-L2))+(2*a1*(a1-L1)*sin(q2+q3)-
2*a1*a2*sin(q2)*m3))*dq2+(-2*a1*sin(q2+q3)*(a3-L3)-2*a2*sin(q3)*(a3-
L3))*m3*dq3;
c12=(-a1*sin(q2)*(a2-L2)*m2+(-a1*sin(q2+q3)*(a3-L3)-a1*a2*sin(q2))*m3)*dq2+(-
a1*sin(q2+q3)*(a3-L3)-2*a2*sin(q3)*(a3-L3))*m3*dq3;
c13=-m3*a1*sin(q2+q3)*(a3-L3)*dq2-(a3-
L3)*a2*sin(q3)*(a1*sin(q2+q3)+a2*sin(q3))*dq3;
c21=(a1*m2*sin(q2)*(a2-L2)+(a1*sin(q2+q3)*(a3-L3)+a1*a2*sin(q2))*m3)*dq1+(-
0.5*a1*m2*sin(q2)*(a2-L2)+(0.5*a1*sin(q2+q3)*(a3-L3)-
0.5*a1*a2*sin(q2))*m3)*dq2+(-0.5*a1*sin(q2+q3)*(a3-L3)-2*a2*sin(q3)*(a3-
L3))*m3*dq3;
c22=(0.5*a1*m2*sin(q2)*(a2-L2)-(0.5*a1*sin(q2+q3)*(a3-
L3)+0.5*a1*a2*sin(q2))*m3)*dq1-2*a2*sin(q3)*(a3-L3)*m3*dq3;
c23=0.5*a1*(a3-L3)*sin(q3+q2)*m3*dq1-a2*(a3-L3)*sin(q3)*m3*dq3;
c31=(a1*sin(q2+q3)+a2*sin(q3))*(a3-
L3)*m3*dq1+(0.5*a1*sin(q2+q3)+a2*sin(q3))*(a3-L3)*m3*dq2+(0.5*a1*sin(q2+q3)-
0.5*a2*sin(q3))*(a3-L3)*m3*dq3;
c32=(0.5*a1*sin(q2+q3)+a2*sin(q3))*(a3-L3)*m3*dq1+a2*sin(q3)*(a3-L3)*m3*dq2-
0.5*a2*sin(q3)*(a3-L3)*m3*dq3;
c33=0.5*(a1*sin(q2+q3)+a2*sin(q3))*(a3-L3)*m3*dq1+0.5*a2*sin(q3)*(a3-
L3)*m3*dq2;
C=[c11 c12 c13; c21 c22 c23; c31 c32 c33];
g1=m2*g*cos(q1+q2)*(a2-L2)-cos(q1+q2)*a2*m3+m3*cos(q1+q2+q3)*(a3-
L3)+m1*cos(q1)*(a1-L1)+a1*cos(q1)*(m2+m3);
g2=(a2-L2)*m2*g*a2*cos(q1+q2)+g*(a2*cos(q1+q2)+a3*cos(q1+q2+q3)*(a3-L3))*m3;
g3=(a3-L3)*m3*g*a3*cos(q1+q2+q3);
G=[g1; g2; g3];
D=[120 0 0; 0 120 0; 0 0 120];
T= M*[u(1); u(2); u(3)]+(C+D)*[dq1; dq2; dq3]+G;
end
% //Bai toan dong luc hoc thuan

function ddq = RobotDynamic(t)
%#codegen
a1=0.5;
a2=0.4;
a3=0.3;
L1=0.371;
L2=0.265;
L3=0.18;
g=9.81;
m1=26.25;
m2=18.5;
m3=19.5;
50
I1z=1.14;
I2z=0.36;
I3z=0.33;
%Cac quy uoc ham trong Simulink
q=t(1:3);
dq=t(4:6);
U=t(7:9);
q1=q(1);
q2=q(2);
q3=q(3);
dq1=dq(1);
dq2=dq(2);
dq3=dq(3);
m11=m1*(L1-a1)^2+I1z+m2*((L2-a2)^2+2*a1*a2*cos(q2)-
2*a1*L2*cos(q2)+a1^2)+I2z+m3*(2*a1*a2*cos(q2)+a1^2+a2^2+a3^2+L3^2-
2*a3*L3+2*(a3-L3)*(a1*cos(q3+q2)+a2*cos(q3)))+I3z;
m12=m2*((a2-L2)^2+a1*cos(q2)*(a2-L2))+(a2^2+a3^2+L3^2+(a3-L3)*(a1*cos(q3+q2)-
2*a2*cos(q3))+a1*a2*cos(q2)-2*a3*L3)*m3+I2z+I3z;
m13=(a3^2+L3^2+(a3-L3)*(a1*cos(q3+q2)+a2*cos(q3))-2*a3*L3)*m3+I3z;
m21=m12;
m22=m2*(a2-L2)^2+m3*(a2^2+2*a2*cos(q3)*(a3-L3)+(a3-L3)^2)+I2z+I3z;
m23=(a2*cos(q3)*(a3-L3)+(a3-L3))*m3+I3z;
m31=m13;
m32=m23;
m33=m3*(L3-a3)^2+I3z;
M=[m11 m12 m13; m21 m22 m23; m31 m32 m33];
c11=((-2*a1*m2*sin(q2)*(a2-L2))+(2*a1*(a1-L1)*sin(q2+q3)-
2*a1*a2*sin(q2)*m3))*dq2+(-2*a1*sin(q2+q3)*(a3-L3)-2*a2*sin(q3)*(a3-
L3))*m3*dq3;
c12=(-a1*sin(q2)*(a2-L2)*m2+(-a1*sin(q2+q3)*(a3-L3)-a1*a2*sin(q2))*m3)*dq2+(-
a1*sin(q2+q3)*(a3-L3)-2*a2*sin(q3)*(a3-L3))*m3*dq3;
c13=-m3*a1*sin(q2+q3)*(a3-L3)*dq2-(a3-
L3)*a2*sin(q3)*(a1*sin(q2+q3)+a2*sin(q3))*dq3;
c21=(a1*m2*sin(q2)*(a2-L2)+(a1*sin(q2+q3)*(a3-L3)+a1*a2*sin(q2))*m3)*dq1+(-
0.5*a1*m2*sin(q2)*(a2-L2)+(0.5*a1*sin(q2+q3)*(a3-L3)-
0.5*a1*a2*sin(q2))*m3)*dq2+(-0.5*a1*sin(q2+q3)*(a3-L3)-2*a2*sin(q3)*(a3-
L3))*m3*dq3;
c22=(0.5*a1*m2*sin(q2)*(a2-L2)-(0.5*a1*sin(q2+q3)*(a3-
L3)+0.5*a1*a2*sin(q2))*m3)*dq1-2*a2*sin(q3)*(a3-L3)*m3*dq3;
c23=0.5*a1*(a3-L3)*sin(q3+q2)*m3*dq1-a2*(a3-L3)*sin(q3)*m3*dq3;
c31=(a1*sin(q2+q3)+a2*sin(q3))*(a3-
L3)*m3*dq1+(0.5*a1*sin(q2+q3)+a2*sin(q3))*(a3-L3)*m3*dq2+(0.5*a1*sin(q2+q3)-
0.5*a2*sin(q3))*(a3-L3)*m3*dq3;
c32=(0.5*a1*sin(q2+q3)+a2*sin(q3))*(a3-L3)*m3*dq1+a2*sin(q3)*(a3-L3)*m3*dq2-
0.5*a2*sin(q3)*(a3-L3)*m3*dq3;
c33=0.5*(a1*sin(q2+q3)+a2*sin(q3))*(a3-L3)*m3*dq1+0.5*a2*sin(q3)*(a3-
L3)*m3*dq2;
C=[c11 c12 c13; c21 c22 c23; c31 c32 c33];
g1=m2*g*cos(q1+q2)*(a2-L2)-cos(q1+q2)*a2*m3+m3*cos(q1+q2+q3)*(a3-
L3)+m1*cos(q1)*(a1-L1)+a1*cos(q1)*(m2+m3);
g2=(a2-L2)*m2*g*a2*cos(q1+q2)+g*(a2*cos(q1+q2)+a3*cos(q1+q2+q3)*(a3-L3))*m3;
g3=(a3-L3)*m3*g*a3*cos(q1+q2+q3);
G=[g1; g2; g3];
D=[120 0 0; 0 120 0; 0 0 120];
ddq=M\([U(1); U(2); U(3)]-(C+D)*[dq(1); dq(2); dq(3)]-G);
end
51

Đồ án cơ điện tử

Uploaded by

Document Information

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Đồ án cơ điện tử

Uploaded by

Copyright:

Available Formats

ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS.

NGUYỄN THỊ VÂN HƯƠNG

LỜI NÓI ĐẦU .................................................................................................................... 3

CHƯƠNG 4: TÍNH TOÁN ĐỘNG LỰC HỌC ROBOT ............................................ 26

LỜI NÓI ĐẦU

Sinh viên thực hiện

1 Tổng quan về hệ cơ điện tử

Hình 1.1 Mô hình hệ cơ điện tử

2 Tổng quan về robot công nghiệp

Hình 1.2 Mô hình Robot phẳng RRR

Hình 1.3 Mô hình 3D Robot phẳng RRR

CHƯƠNG II: TÍNH TOÁN ĐỘNG HỌC ROBOT

1 Đặt hệ trục tọa độ theo Denavit- Hartenberg

Hình 2.1 Hệ trục tọa độ Robot phẳng RRR

2 Thiết lập bảng D-H

Hình 2.2 Tham số kích thước Robot phẳng RRR

cos i  cos  i sin i sin  i sin i ai cos i 

4 Bài toán động học thuận Robot

sin q2  S1 cos(q1  q2  q 3 )  C123

C12  S12 0 a1C1  a2C12 

C123  S123 0 a1C1  a2C12  a3C123 

rE   a1C1  a2C12  a3C123 ; a1S1  a2 S12  a3 S123 ;0

Từ đó, ta có vị trí điểm tác động cuối là:

Hình 2.3 Các góc quay Cardan

So sánh các phần tử của 2 ma trận, ta có:

Từ đó, ta suy ra góc Cardan:

 Xét khâu 2, ta có:

 Xét khâu 3, ta có:

 Vận tốc và gia tốc điểm tác động cuối:

5 Bài toán động học ngược robot

Vậy ta đã tìm được các biến khớp q1, q2, q3.

CHƯƠNG 3: THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG CỦA

1 Khái niệm, nhiệm vụ của thiết kế quỹ đạo

2 Không gian thao tác của Robot

 qi (tc )  i a1  i a2tc  i a3tc 2

Từ hệ phương trình (3.3), ta tính được các hệ số i a0 , i a1 , i a2 , i a3 như sau:

Hình 3.3 Quỹ đạo, vận tốc, gia tốc khớp 1

Hình 3.4 Quỹ đạo, vận tốc, gia tốc khớp 2

Hình 3.5 Quỹ đạo, vận tốc, gia tốc khớp 3

Chọn x=x(t) là đa thức bậc 3 có dạng:

x  x  t   a0  a1t  a1t 2  a1t 3 (3.6)

Hình 3.6 Đồ thị xE , yE

Hình 3.7 Quỹ đạo điểm tác động cuối

CHƯƠNG 4: TÍNH TOÁN ĐỘNG LỰC HỌC ROBOT

1 Phương trình Lagrange II dạng trận

Với: T- động năng của Robot

Qi* - lực suy rộng của các lực không thế

n – số bậc tự do của Robot

2 Động năng của Robot

Với Ti là khâu động thứ i, tính theo công thức:

Với : JTi: ma trận Jacobian tịnh tiến của khâu i

 m11  q  m12  q  m1n  q  

Ta gọi M(q) là ma trận khối lượng suy rộng.

3 Thế năng của Robot

M  q    mij  : ma trận khối lượng suy rộng

C  q, q   cij  : ma trận li tâm Coriolis

g(q): vector lực do trọng lực gây ra

5 Thiết lập phương trình động lực học cho Robot

 I1x 0 0   I2x 0 0   I3x 0 0

rC1   l1 0 0 , 2 rC 2   l2 0 0 , 3 rC 3   l3 0 0

rO1   a1C1 a1S1 0

 a1C1  C1  S1 0  l1   a1  l1  C1 

 a1C1  a2C12  C12  S12 0  l2   a2  l2  C12  a1C1 

 a1C1  a2C12  a3C123  C123  S123 0   l3 