Báo cáo Nghiên cứu thiết kế, chế tạo các robot thông minh phục vụ cho các ứng dụng quan trọng - Nhóm sản phẩm robot SCA
BỘ KHOA HỌC VÀ CÔNG NGHỆ
CHƯƠNG TRÌNH KC.03
YZ YZ YZ YZ YZ YZY YZ YZ YZY YZ YZ YZY
“NGHIÊN CỨU THIẾT KẾ, CHẾ TẠO CÁC ROBOT THÔNG
MINH PHỤC VỤ CHO CÁC ỨNG DỤNG QUAN TRỌNG”
MÃ SỐ: KC.03.08
BÁO CÁO CÁC KẾT QUẢ NGHIÊN CỨU
THEO NHIỆM VỤ 2 - ĐỀ TÀI KC.03.08
NHÓM SẢN PHẨM ROBOT SCA
6246-2
25/12/2006
HÀ NỘI 2006
Môc lôc
4
Më ®Çu
PhÇn 1:
Robot SCATM khÝ nÐn
11
I. Giíi thiÖu chung
11
12
12
12
12
12
13
14
15
19
36
40
II. X©y dùng c¸c m« h×nh ®éng häc vµ ®éng lùc häc Robot SCATM
2.1. ThiÕt lËp ph−¬ng tr×nh ®éng häc Robot SCATM
2.1.1. X¸c ®Þnh c¸c hÖ täa ®é cña Robot SCATM
2.1.2. B¶ng th«ng sè DH cña Robot SCATM
2.1.3. X¸c ®Þnh c¸c ma trËn cña Robot SCATM
2.1.4. TÝnh c¸c ma trËn T cña Robot SCATM
2.1.5. Ph−¬ng tr×nh ®éng häc Robot SCATM
2.2. ThiÕt lËp bµi to¸n ®éng häc ng−îc Robot SCATM
2.3. X©y dùng m« h×nh ®éng lùc häc Robot SCATM
III. ThiÕt kÕ c¶i tiÕn chÕ t¹o Robot SCATM khÝ nÐn
PhÇn 2:
M«®un quay dïng b¸nh r¨ng con l¨n
I. Giíi thiÖu chung
40
41
46
46
48
49
49
52
54
56
II. Nhu cÇu cÇn cã m«®un quay dïng BRCL
III. CÊu t¹o vµ nguyªn t¾c lµm viÖc
3.1. C¸c bé phËn chñ yÕu cña hép gi¶m tèc BRCL
3.2. Nguyªn lý lµm viÖc cña hép gi¶m tèc BRCL
IV. D¹ng r¨ng b¸nh r¨ng con l¨n
4.1. D¹ng r¨ng l−în sãng
4.2. X©y dùng biªn h×nh r¨ng con l¨n
4.3. Ph−¬ng ph¸p chän d¹ng r¨ng hîp lý
V. Ph−¬ng ph¸p chÕ t¹o BRCL
2
VI. LËp tr×nh gia c«ng trªnm¸y c¾t d©y CNC
VII. Sö dông BRCL cho m«dun quay Robot lµ gi¶i ph¸p hîp lý nhÊt
VIII. ChuÈn hãa thiÕt kÕ hép gi¶m tèc BRCL
IX. M«®un quay BRCL
57
58
59
69
72
82
83
X. Mét sè s¶n phÈm ®· chÕ t¹o
XI. KÕt luËn
PhÇn 3:
m«®un d©y chuyÒn s¶n xuÊt tù ®éng
I. Giíi thiÖu chung
83
84
II. HÖ thèng b¨ng chuyÒn
III. HÖ thèng ®iÒu khiÓn
98
3.1. Nguyªn t¾c ho¹t ®éng
98
3.2. C¸c bé phËn chñ yÕu
99
PhÇn 4:
102
C¬ së tÝnh to¸n vµ x©y dùng c¸c ch−¬ng tr×nh m¸y
tÝnh vµ ®iÒu khiÓn
I. Giíi thiÖu chung
102
103
105
107
107
109
131
II. Ch−¬ng tr×nh tù ®éng thiÕt lËp
III. Ch−¬ng tr×nh kiÓm nghiÖm lêi gi¶i
IV. Ch−¬ng tr×nh phÇn mÒm tÝnh to¸n ®éng lùc häc
V. Ch−¬ng tr×nh phÇn mÒm ®iÒu khiÓn SCA
VI. Ch−¬ng tr×nh phÇn mÒm ®iÒu khiÓn d©y chuyÒn s¶n xuÊt
Tµi liÖu tham kh¶o
3
BÁO CÁO CÁC KẾT QUẢ NGHIÊN CỨU THEO NHIỆM VỤ 2
CỦA ĐỀ TÀI KC.03.08
NGHIÊN CỨU THIẾT KẾ CHẾ TẠO
NHÓM SẢN PHẨM ROBOT SCA
MỞ ĐẦU
Robot SCA là loại robot thuộc nhóm robot SCARA (selective Compliance
Assembly Robot Arm) có cấu trúc động học theo kiểu cơ cấu tay máy
phỏng sinh trục đứng. SCARA là loại robot dùng để lắp ráp linh hoạt. Tuy
SCARA mới xuất hiện ở Nhật trong những năm 80, nhưng do có nhiều ưu
điểm nên nhanh chóng được nhiều nước áp dụng và cải tiến.
Có thể xếp vào nhóm robot SCARA các kiểu robot sau: Adept - One,
IBM - 7545, Intelldex 440, Rhino SCARA của Hoa Kỳ; Skilam, SR-2, Nam
Robo, Puha 2 của Nhật bản; IS 600 của Đức; Serpent của Anh v.v.
Tại Trung tâm NCKT tự động hóa, ĐHBK - HN cũng đã thiết kế chế
tạo kiểu robot SCA là một biến thể của SCARA. Kiểu robot SCA này có
cấu hình RRRT, với 4 bậc tự do, toàn dùng động cơ bước.
4
Hình 2.0.1. Chế tạo và lắp ráp Robot SCA
Nhằm nâng cao khả năng thao tác linh họat cho robot SCAđể đáp
ứng được những tình huống xử lý thông minh của bộ điều khiển theo các tín
hiệu nhận được từ các sensors, Đề tài đã nghiên cứu thiết kế cải tiến robot
SCA theo 2 định hướng: dùng khí nén để tác động nhanh và môđun hóa để
vạn năng hóa kết cấu.
1) Robot SCATM khí nén có 3 bậc tự do đầu dùng động cơ bước, còn
bậc cuối thực hiện chuyển động tịnh tiến dùng truyền động khí nén. Trên
hình 2.0.2 là bản vẽ thiết kế robot SCATM khí nén. Kết quả nghiên cứu thiết
kế, chế tạo và điều khiển vận hành robot SCATM khí nén ®−îc tr×nh bµy
trong Phần I của Báo cáo về nhiệm vụ 2 của Đề tài.
2) Robot SCA môđun hóa là một thử nghiệm tạo ra môđun cánh tay
chuẩn hóa, từ đó có thể lắp ghép thành nhiều kiểu robot SCA. Trên hình
2.0.3 là ví dụ một kiểu robot SCA tạo ra từ các môđun cánh tay chuẩn hóa
và hình 2.0.4. là một ví dụ khác: robot SCA có 2 tay.
5
Mỗi môđun cánh tay (hình 2.0.5) là một khâu hoàn chỉnh của cơ cấu
tay máy robot, được thực hiện chuyển động quay rất chậm trực tiếp bởi một
động cơ kèm hộp giảm tốc tỷ số truyền cao và không tồn tại khe hở cạnh
răng.
Như vậy, phần cốt lõi của môđun cánh tay là môđun quay (rotation
module). Vì thế khi triển khai thực hiện nhiệm vụ 2 của Đề tài đã phát sinh
một nội dung nghiên cứu và đã nhận được những kết quả bất ngờ và rất có ý
nghĩa không những đối với các truyền động quay trong robot mà còn có tác
dụng trong hầu hết các thiết bị máy móc hiện đại. Tóm tắt các kết quả chủ
yếu về nội dung nghiên cứu này được trình bày trong phần II của Báo cáo
về Nhiệm vụ 2 của Đề tài.
6
Hình 2.0.5. Bản vẽ thiết kế Robot SCATM khí nén
7
H×nh 2.0.3. Robot SCA mét c¸nh tay m«®un hãa
756
H×nh 2.0.4. Robot SCA 2 c¸nh tay m«®un hãa
8
H×nh 2.0.5. M«®un c¸nh tay
Nội dung phần III của Báo cáo về Nhiệm vụ 2 của Đề tài là các kết
quả nghiên cứu thiết kế chế tạo môđun Dây chuyền sản xuất (DCSX) dùng
robot SCA để phân loại sản phẩm theo màu sắc. Nội dung nghiên cứu này
cốt để minh họa về một cách thức thể hiện khả năng thông minh hóa cho
robot ở đây cụ thể là robot SCA, trong môi trường làm việc cụ thể nào đó.
Ngoài ra ®¬n vÞ chñ tr× Đề tài còn tham gia xây dựng Dự án “Chế tạo
robot bốc dì két lên pallet tại phân xưởng chiết của Công ty liên doanh Bia
Sài Gòn”. Hình 2.0.6. là mô phỏng hoạt hình robot này, đó cũng là một biến
thể cỡ lớn của Robot SCARA. Kinh phí từ ngân sách SNKH được duyệt là
2,6 tỷ đồng, phần tham gia của ®¬n vÞ là 250 triệu đồng. Các thông số kỹ
thuật của robot này giới thiệu trong phần phụ lục.
9
Hình 2.0.6. Mô phỏng Robot bốc dỡ két bia
Nội dung Phần 4 của Báo cáo về Nhiệm vụ 2 của Đề tài là các kết
quả nghiên cứu xây dựng cơ sở lý thuyết, thuật toán và chương trình phần
mềm điều khiển robot SCA trong trường hợp hoạt động riêng rẽ và trong
trường hợp thao tác với dây chuyền sản xuất. Trong đó cã các vấn đề với
phương pháp giải bài toán động học thuận, bài toán động học ngược, trong
lập trình quỹ đạo cho robot, bài toán động lực học và phương pháp thực
hiện việc chọn lựa sản phẩm theo màu sắc.
Nội dung chủ yếu của Phần I đã được viết thành một luận văn thạc sĩ,
bảo vệ năm 2006.
Phần II có một sản phẩm đã trưng bày ở chợ Techmart 2003 tại Hà
Nội và được tặng thưởng Huy chương Vàng và một phần nội dung đã có
trong luận văn Thạc sĩ, bảo vệ năm 2005.
Phần IV có 2 chương trình phần mềm được cấp giấy chứng nhận bản
quyền.
10
PhÇn i
B¸o c¸o tãm t¾t c¸c kÕt qu¶ nghiªn cøu theo
nhiÖm vô 2 cña ®Ò tµi kc. 03.08
robocar scaTm khÝ nÐn
i. giíi thiÖu chung
Nghiên cứu thiết kế chế tạo Robot SCATM khí nén để nâng cao mức
linh hoạt thao tác của nó nhằm đáp ứng được điều khiển không bị trễ khi xử
lý thông minh các tín hiệu nhận được từ hệ thống sensor
Phần I báo cáo này trình bày 2 nội dung:
1. Xây dựng các mô hình động học và động lực học của Robot
SCATM
2. Thiết kế cải tiến và chế tạo Robot SCATM khí nén
11
II. tãm t¾t vÒ kÕt qu¶ nghiªn cøu x©y dùng c¸c m«
h×nh ®éng häc vµ ®éng lùc häc cña Robot scatm
2.1. Thiết lập phương trình động học Robot SCATM
2.1.1. X¸c ®Þnh c¸c hÖ täa ®é cña robot SCATM
a
2
z
z
0
1
1
q
q
2
x
,x ,x
0
1
2
d
3
a
1
x
x
3
d4 cè ®Þnh
q
4
4
z
, z ,z
2
3
4
H×nh 2.1.1. HÖ to¹ ®é cña robot SCATM
2.1.2. B¶ng th«ng sè DH cña robot SCATM
qi
αi
ai
a1
a2
0
di
0
Kh©u
q1*
q2*
0
1
0
180o
0
2
3
4
0
d3*
d4
q4*
0
0
cña robot SCATM
A
2.1.3. X¸c ®Þnh c¸c ma trËn
i
12
C
−S1 0 a C
⎡
⎢
⎢
⎢
⎢
⎣
⎤
⎥
⎥
⎥
⎥
⎦
1
1
1
S1 C1 0 a1S1
A =
1
(2.1)
(2.2)
0
0
0
0
1
0
0
1
C
S2
0
0
a2C2
a2S2
0
⎡
⎢
⎢
⎢
⎢
⎣
⎤
⎥
⎥
⎥
⎥
⎦
2
S2 −C2
A2 =
0
0
0
0
−1
0
1
1 0 0 0
0 1 0 0
0 0 1 d3
0 0 0 1
⎡
⎤
⎥
⎥
⎥
⎥
⎦
⎢
⎢
⎢
⎢
⎣
A =
3
(2.3)
(2.4)
C
−S4
0
0
1
0
⎡
⎤
⎥
⎥
⎥
⎥
⎦
4
⎢
⎢
⎢
⎢
⎣
S4 C4
0
0
A4 =
0
0
0
0
0 d4
2.1.4. TÝnh c¸c ma trËn T cña robot SCATM
i
C
−S4 0 0
⎡
⎢
⎢
⎢
⎢
⎣
⎤
⎥
⎥
⎥
⎥
⎦
4
S4 C4 0 0
3
T4 = A4 =
(2.5)
0
0
0
0
1 0
0 d4
n
sx ax px
⎡
⎢
⎢
⎢
⎢
⎣
⎤
⎥
⎥
⎥
⎥
⎦
x
ny sy ay py
nz sz az pz
T4 =
(2.6)
0
0
0
1
13
C
−S4
0
0
0
0
⎡
⎢
⎢
⎢
⎢
⎣
⎤
⎥
⎥
⎥
⎥
⎦
4
S4 C4
2
T4 = A A4 =
3
(2.7)
0
0
0
0
1 d4 + d3
0
1
cos(q − q ) sin(q2 − q4 )
0
C2a2
S2a2
⎡
⎢
⎢
⎢
⎢
⎣
⎤
⎥
⎥
⎥
⎥
⎦
2
4
sin(q2 − q4 ) −cos(q2 − q4 ) 0
1
T4 = A2 A A4 =
3
(2.8)
(2.9)
0
0
0
0
−1 −d4 − d3
0
1
cos(q + q −q ) sin(q1 + q2 − q4 )
0 a C + a C
⎡
⎢
⎢
⎢
⎢
⎣
⎤
⎥
⎥
⎥
⎥
⎦
1
2
4
2
12
1
1
sin(q1 + q2 −q4 ) −cos(q1 + q2 − q4 ) 0 a2S12 + a1S1
1
T4 = A T4 =
1
0
0
0
0
−1 −d4 −d3
0
1
2.1.5. Ph−¬ng tr×nh ®éng häc robot SCATM
n
sx ax px
⎡
⎢
⎢
⎢
⎢
⎣
⎤
⎥
⎥
⎥
⎥
⎦
x
ny sy ay py
nz sz az pz
= T4
(2.10)
0
0
0
1
Tõ ®ã ta cã hÖ ph−¬ng tr×nh sau:
14
n = cos(q + q − q )
⎧
⎪
⎪
⎪
⎪
⎪
⎪
⎪
x
1
2
4
ny = sin(q1 + q2 − q4 )
nz = 0
s = sin(q + q − q )
x
1
2
4
sy = −cos(q1 + q2 − q4 )
⎪
⎪
s = 0
z
→
⎨
⎪
⎪
⎪
⎪
⎪
⎪
⎪
⎪
ax = 0
ay = 0
(2.11)
a = −1
z
px = a2C12 + a1C1
py = a2S12 + a1S1
p = −d − d
⎪
⎩
z
4
3
2.2. ThiÕt lËp bµi to¸n ®éng häc ng−îc Robot SCATM
Như đã ký hiệu trong bài toán động học thuận, ta có phương trình
sau:
i
Tn = Ai Tn
A−1
Nhân hai vế của phương trình này với
, nhận được
i
i Tn = Ai−1Tn
C n +S n C s +S s C a +S a C p +S p −C a −a
⎡
⎢
⎢
⎢
⎢
⎣
⎤
⎥
⎥
⎥
⎥
⎦
12
x
12
y
12 x
12 y
12
x
12
y
12
x
12
y
2 1
2
S n −C ny S s −C s S ax −C ay
S px −C py −S2a1
12
x
12
12 x
12 y
12
12
12
12
−1 −1 −1
X34 = A A A T =
3
2
1
4
−n
−sz
−az
−pz −d3
z
0
0
0
1
(2.12)
15
C n + S n C sx + S12sy C ax + S12ay C p + S p −C a −a
⎡
⎢
⎢
⎢
⎢
⎣
⎤
⎥
⎥
⎥
⎥
⎦
12
x
12
y
12
12
12
x
12
y
2
1
2
S12nx −C ny S12 sx −C sy S12ax −C ay
S12 px −C py − S2a
12
12
12
12
1
−1 −1
X24 = A A T =
2
1
4
−nz
−sz
−az
−pz
0
0
0
1
(2.13)
C ax + S1ay C p + S p −a
C n + S n
C sx + S1sy
⎡
⎢
⎢
⎢
⎢
⎣
⎤
1
x
1
y
1
1
1
x
1
y
1
⎥
⎥
⎥
⎥
⎦
−S1nx +C ny −S1sx +C sy −S1ax +C ay
−S1 px +C py
1
1
1
1
−1
1
X14 = A T =
4
nz
sz
az
pz
0
0
0
1
(2.14)
q1
* T×m
1T4
X14
XÐt phÇn tö hµng 1 cét 4 cña 2 ma trËn
vµ phÇn tö hµng 2 cét 4 cña 2 ma trËn
vµ
tõ (2.14) vµ (2.8)
tõ (2.14) vµ (2.8)
1T4
X14
vµ
p cos(q ) + p sin(q ) −a = a cos(q )
(2.15)
(2.16)
⎧
⎪
x
1
y
1
1
2
2
⎨
−p sin(q ) + p cos(q ) = a sin(q )
⎪
⎩
x
1
y
1
2
2
2
px cos(q1) + py sin(q1) − a1 = a22 cos2 (q2 )
(2.17)
(2.18)
⎧
⎡
⎤
⎪⎣
⎦
⎨
2
2
2
⎡
⎤
⎪
−px sin(q1) + py cos(q1) = a2 sin (q2 )
⎣
⎦
⎩
(2.17) +(2.18) → px2 + py2 − 2a px cos(q ) − 2a py sin(q ) + a2 = a2
2 (2.19)
1
1
1
1
1
2
2
2
2
⎡
⎤
px + py + a1 − a2 = 2a1 px cos(q1) + py sin(q1)
(2.20)
⎣
⎦
px2 + py2 + a12 − a22
2a1 px2 + py2
py
px2 + py2
px
px2 + py2
=
cos(q1) +
sin(q1)
(2.21)
§Æt
16
px
px2 + py2
= sinϕ
(2.22)
(2.23)
py
px2 + py2
= cosϕ
Tõ (2.19) ta cã:
px2 + py2 + a12 − a22
2a1 px2 + py2
= sin ϕ + q
(
)
1
(2.24)
(2.25)
(2.26)
2
2
2
2
2
⎛
⎜
⎞
⎟
px + py + a1 − a2
2a1 px2 + py2
⇒ cos ϕ + q = 1−
(
)
1
⎜
⎝
⎟
⎠
2
⎛
⎜
⎜
⎞
⎟
⎟
p2 + p2 + a2 − a2
2a1 px2 + py2
px + py + a1 − a2
2a1 px2 + py2
2
2
2
2
⎛
⎜
⎞
⎟
x
y
1
2
ϕ + q1 = arctg2
, 1−
⎜
⎟
⎜
⎝
⎟
⎠
⎝
⎠
Tõ (2.22) vµ (2.23)
⇒ ϕ = arctg2 p , p
(
Tõ (2.26) vµ (2.27) ta cã:
(2.27)
(2.28)
x
y
2
⎛
⎞
p2 + p2 + a2 −a2
2a1 px2 + py2
px + py + a1 −a2
2a1 px2 + py2
2
2
2
2
⎛
⎜
⎞
⎟
⎜
⎜
⎟
⎟
x
y
1
2
q1 = arctg2
, 1−
−arctg p , p
(
)
x
y
⎜
⎟
⎜
⎝
⎟
⎠
⎝
⎠
* T×m q2.
Tõ (2.15)
⇒ p cos q + p sin q − a = a cos q
(2.29)
(2.30)
x
1
y
1
1
2
2
py
px2 + py2
px
a1
px2 + py2
a2
px2 + py2
cos(q1) +
sin(q1) −
=
cos q
2
(
)
px2 + py2
§Æt:
17
px
px2 + py2
= sinϕ
= cosϕ
(2.31)
(2.32)
py
px2 + py2
⎡
⎤
⎥
a1
px2 + py2
a2
px2 + py2
⎢
⇒ sin ϕ + q −
=
cos q
(
)
( )
1
2
(2.33)
⎢
⎥
⎣
⎦
KÕt hîp víi (2.24), ta ®−îc:
px2 + py2
2
2
2
2
⎛
⎜
⎞
⎟
px + py + a1 − a2
2a1 px2 + py2
a1
−
= cos q
( )
(2.34)
(2.35)
(2.36)
2
px2 + py2
⎜
⎝
⎟
⎠
a2
px2 + py2 − a12 − a22
2a1a2
⇒
= cos q
( )
2
2
2
2
2
2
⎛
⎞
px + py − a1 − a2
2a1a2
⇒ 1−
= sin q
( )
⎜
⎜
⎟
⎟
2
⎝
⎠
2
⎡
⎤
2
2
2
2
px2 + py2 − a12 − a22
2a1a2
⎛
⎞
px + py − a1 − a2
2a1a2
⎢
⎥
⎥
⇒ q = arctg2 1−
,
⎜
⎜
⎟
⎟
2
⎢
⎝
⎠
⎢
⎥
⎦
⎣
(2.37)
* T×m d3
1
XÐt phÇn tö hµng 3 cét 4 cña hai ma trËn X14 vµ T4 tõ (2.14) vµ (2.8)
pz = −d4 − d3
(2.38)
(2.39)
⇒ d3 = − pz − d4
* T×m q4
3
XÐt phÇn tö hµng 2 cét 1 cña hai ma trËn X34 vµ T4 tõ (2.12) vµ (2.5)
S12nx −C12ny = S4
(2.40)
18
3
XÐt phÇn tö hµng 1 cét 1 cña hai ma trËn X34 vµ T4 tõ (2.12) vµ (2.5)
C12nx + S12ny = C4
(2.41)
(2.42)
⇒ q = arctg2 S ,C
(
4
4
4
2.3. Xây dựng mô hình động lực học Robot SCATM
Ta thiÕt lËp c¸c ma trËn i-1Ai
Tõ c¸c c«ng thøc (1.22) vµ (1.23) ta cã:
C
−S1 0 a C
⎡
⎢
⎢
⎢
⎢
⎣
⎤
⎥
⎥
⎥
⎥
⎦
1
1
1
S1 C1 0 a1S1
0
A =
(2.43)
(2.44)
1
0
0
0
0
1
0
0
1
C
S2
0
0
a2C2
a2S2
0
⎡
⎢
⎢
⎢
⎢
⎣
⎤
⎥
⎥
⎥
⎥
⎦
2
S2 −C2
1
A2 =
0
0
0
0
−1
0
1
1 0 0 0
0 1 0 0
0 0 1 d3
0 0 0 1
⎡
⎤
⎥
⎥
⎥
⎥
⎦
⎢
⎢
⎢
⎢
⎣
2
A =
(2.45)
(2.46)
3
C
−S4
0
0
1
0
0
0
⎡
⎤
⎥
⎥
⎥
⎥
⎦
4
⎢
⎢
⎢
⎢
⎣
S4 C4
3
A4 =
0
0
0
0
0 d4
Tõ c«ng thøc (1.21) ta cã:
C
S12
0
0
a C + a C
⎡
⎢
⎢
⎢
⎢
⎣
⎤
⎥
⎥
⎥
⎥
⎦
12
2
12
1
1
S12 −C12
a2S12 + a1S1
0
0
1
A2 = A A2 =
(2.47)
1
0
0
0
0
−1
0
0
1
19
C
S12
0 a C + a C
⎡
⎢
⎢
⎢
⎢
⎣
⎤
⎥
⎥
⎥
⎥
⎦
12
2
12
1
1
S12 −C12
0
−1
0
a2S12 + a1S1
0
0
1
2
A = A A2 A =
3
1
3
(2.48)
0
0
0
0
−d3
1
⎡cos q + q − q
sin q + q − q
0
0
a2C12 + a1C1 ⎤
(
)
)
)
1
2
4
1
2
4
⎢
⎢
⎢
⎢
⎣
⎥
⎥
⎥
⎥
⎦
sin q + q − q
−cos q + q − q
a2S12 + a1S1
−d3 − d4
1
(
(
)
0
0
1
2
3
1
2
4
1
2
4
A4 = A A2 A A4 =
(2.49)
1
3
0
0
0
0
−1
0
C
S2
0
a2C2
a2S2
⎡
⎢
⎢
⎢
⎢
⎣
⎤
⎥
⎥
⎥
⎥
⎦
2
S2 −C2
0
1
1
2
A = A2 A =
(2.50)
3
3
0
0
0
0
−1 −d3
0
1
⎡ cos −q + q
−sin −q + q
0
0
a2C2
a2S2
⎤
(
)
2
4
2
4
⎢
⎥
⎥
⎥
⎥
⎦
−sin −q + q
−cos −q + q
(
)
(
1
1
2
3
2
4
2
4
⎢
⎢
⎢
⎣
A4 = A2 A A4 =
(2.51)
3
0
0
0
0
−1 −d3 − d4
0
1
C
−S4
0
0
0
0
⎡
⎢
⎢
⎢
⎢
⎣
⎤
⎥
⎥
4
S4 C4
2
2
3
A4 = A A4 =
3
(2.52)
⎥
⎥
⎦
0
0
0
0
1 d3 + d4
0
1
Tõ c«ng thøc (1.26) vµ (1.27) ta cã:
0 −1 0 0
1 0 0 0
0 0 0 0
0 0 0 0
⎡
⎢
⎢
⎢
⎢
⎣
⎤
⎥
⎥
⎥
⎥
⎦
D =
1
(2.53)
20
0 −1 0 0
1 0 0 0
0 0 0 0
0 0 0 0
⎡
⎢
⎢
⎢
⎢
⎣
⎤
⎥
⎥
⎥
⎥
⎦
D2 =
D3 =
D4 =
(2.54)
0 0 0 0
0 0 0 0
0 0 0 1
0 0 0 0
⎡
⎢
⎢
⎢
⎢
⎣
⎤
⎥
⎥
⎥
⎥
⎦
(2.55)
0 −1 0 0
1 0 0 0
0 0 0 0
0 0 0 0
⎡
⎢
⎢
⎢
⎢
⎣
⎤
⎥
⎥
⎥
⎥
⎦
(2.56)
Ta tÝnh Uij theo c«ng thøc (1.31)
−S −C1 0 −a S
⎡
⎢
⎢
⎢
⎢
⎣
⎤
⎥
⎥
⎥
⎥
⎦
1
1
1
C1 −S1
0
0
0
a1C1
0
0
U11 = D A =
(2.57)
(2.58)
1
1
0
0
0
0
0
−S
C12 0 −a S − a S
⎡
⎤
⎥
⎥
⎥
⎥
⎦
12
2
12
1
1
⎢
⎢
⎢
⎢
⎣
C12 S12 0 a2C12 + a1C1
0
U21 = D A2 =
1
0
0
0
0
0
0
0
0
21
−S
C12 0 −a2S12
⎡
⎢
⎢
⎢
⎢
⎣
⎤
⎥
⎥
⎥
⎥
⎦
12
C12 S12
0
0
0
a2C12
0
1
U22 = A D2 A2 =
1
(2.59)
(2.60)
0
0
0
0
0
0
−S
C12 0 −a S − a S
⎡
⎢
⎢
⎢
⎢
⎣
⎤
⎥
⎥
⎥
⎥
⎦
12
2
12
1
1
C12 S12 0 a2C12 + a2C1
0
U31 = D A =
1
3
0
0
0
0
0
0
0
0
−S
C12 0 −a2S12
⎡
⎢
⎢
⎢
⎢
⎣
⎤
⎥
⎥
⎥
⎥
⎦
12
C12 S12 0 a2C12
0
1
U32 = A D2 A =
1
3
(2.61)
(2.62)
0
0
0
0
0
0
0
0
0 0 0 0
0 0 0 0
⎡
⎤
⎥
⎥
⎢
⎢
⎢
⎢
⎣
0
2
U33 = A2D3 A =
3
⎥
0 0 0 −1
⎥
0 0 0 0
⎦
⎡−sin q + q − q
cos q + q − q
0 −a2S12 − a1S1 ⎤
(
)
)
)
1
2
4
1
2
4
⎢
⎢
⎢
⎢
⎣
⎥
cos q + q − q
sin q + q − q
0
0
0
a2C12 + a1C1
(
)
(
0
1
2
4
1
2
4
⎥
⎥
⎥
⎦
U41 = D A4 =
(2.63)
(2.64)
1
0
0
0
0
0
0
⎡−sin q + q − q
cos q + q − q
0 −a2S12 ⎤
(
)
)
)
1
2
4
1
2
4
⎢
⎢
⎢
⎢
⎣
⎥
cos q + q − q
sin q + q − q
0
0
0
a2C12
(
)
(
0
1
1
2
4
1
2
4
⎥
⎥
⎥
⎦
U42 = A D2 A4 =
1
0
0
0
0
0
0
22
0 0 0 0
0 0 0 0
⎡
⎢
⎢
⎢
⎢
⎣
⎤
⎥
⎥
0
2
U43 = A2D3 A4 =
(2.65)
⎥
0 0 0 −1
⎥
0 0 0 0
⎦
⎡ sin q + q − q
−cos q + q − q
0 0⎤
(
)
)
)
1
2
4
1
2
4
⎢
⎢
⎢
⎢
⎣
⎥
⎥
⎥
⎥
⎦
−cos q + q − q
−sin q + q − q
0 0
0 0
0 0
(
)
(
0
3
1
2
4
1
2
4
U44 = A D4 A4 =
3
(2.66)
0
0
0
0
1
3
1
⎛
⎜
⎜
⎜
⎜
⎜
⎞
m a12 0 0 − m a1
1
1
⎟
⎟
⎟
⎟
⎟
2
0
0 0
0 0
0
0
J1 =
J2 =
J3 =
(2.67)
(2.68)
(2.69)
0
1
⎜
⎟
⎟
⎠
− m a 0 0
m
1
⎜
1
1
⎝ 2
1
3
1
⎛
⎜
⎜
⎜
⎜
⎜
⎞
⎟
⎟
⎟
⎟
⎟
m2a22 0 0 − m2a2
2
0
0
0 0
0 0
0
0
1
⎜
⎟
⎟
⎠
− m a 0 0
m2
⎜
2
2
⎝ 2
1
3
1
⎛
⎜
⎜
⎜
⎜
⎜
⎞
⎟
⎟
⎟
⎟
⎟
m3l32 0 0 − m3l3
2
0
0
0 0
0 0
0
0
1
⎜
⎜
⎟
⎟
⎠
− m l 0 0
m3
3 3
⎝ 2
23
1
3
1
⎛
⎜
⎜
⎜
⎜
⎜
⎞
⎟
⎟
⎟
⎟
⎟
m4l42 0 0 − m4l4
2
0
0
0 0
0 0
0
0
J4 =
(2.70)
1
⎜
⎜
⎟
⎟
⎠
− m l 0 0
m4
4 4
⎝ 2
Tõ c«ng thøc (1.51):
D = Tr U J UT +Tr U J UT +Tr U J UT +Tr U J UT
(
)
(
)
)
)
11
11
1
11
21
2
21
31
3
31
41
4
41
1
=m4a22 + m4a12 + m2a22 + m2a1a2C2 − m4l4a2C4 − m4l4a1 cos(−q2 + q4 ) + 2m4a1a2C2
3
1
1
1
−m3l3a1C2 + 2m3 a1a2C2 − m3l3a2 + m2a12 + m3a22 + m3a12 + m a12 + m3l32 + m4l42
1
3
3
3
(2.71)
D = Tr U J UT + Tr U J UT + Tr U J UT
(
12
22
2
21
32
3
31
42
4
41
1
1
1
1
= m2a22 + m2a2a1C2 − a2m3l3 + m3a2a1C2 − a1m3l3C2 − a2m4l4C4 + m3l32
3
2
2
3
1
1
+ m l2 + a2m − a m l cos −q + q + a m a C + a2m
(
)
(2.72)
4 4
2
3
1
4 4
2
4
2
4
1
2
2
4
3
2
D = Tr U J UT + Tr U J UT = 0
(
)
)
(2.73)
(2.74)
13
33
3
31
43
4
41
D = Tr U J UT
(
)
14
44
4
41
1
1
1
= m l2 + m l a C + m l a cos −q + q
(
)
4 4
4 4
2
4
4 4
1
2
4
3
2
2
D = Tr U J UT + Tr U J UT + Tr U J UT
(
)
)
)
22
22
2
22
32
3
32
42
4
42
1
1
1
= −a2m3l3 + a22m3 + a22m2 + a22m4 − a2m4l4C4 + l32m3 + l42m4
(2.75)
(2.76)
(2.77)
3
3
3
D = Tr U J UT + Tr U J UT = 0
(
23
33
3
32
43
4
42
1
1
D = Tr U J UT = − l2m + a m l C
(
)
24
33
4
42
4
4
2
4 4
4
3
2
24
D = Tr U J UT + Tr U J UT = m + m
(2.78)
(2.79)
(
33
33
3
33
43
4
43
3
4
D = Tr U J UT = 0
(
)
34
44
4
33
1
D = Tr U J UT = m l2
(
)
(2.80)
44
44
4
44
4 4
3
D
D
D
D
⎡
⎢
⎢
⎢
⎢
⎣
⎤
11
12
13
14
⎥
⎥
⎥
⎥
⎦
D
D22
D23
D33
D34
D24
12
D q =
( )
(2.81)
D
D23
D24
D34
D44
12
D
14
−C
S1 0 −a C
⎡
⎢
⎢
⎢
⎢
⎣
⎤
⎥
⎥
⎥
⎥
⎦
1
1
1
−S1 −C1 0 −a1C1
U111 = D2 A =
0
1
1
(2.82)
(2.83)
0
0
0
0
0
0
0
0
−C
−S12 0 −a C − a C
⎡
⎢
⎢
⎢
⎢
⎣
⎤
12
2
12
1
1
⎥
⎥
⎥
⎥
⎦
−S12 C12 0 −a2 S12 − a1S1
0
1
U211 = D2 A A2 =
1
1
0
0
0
0
0
0
0
0
−C
−S12 0 −a C − a C
⎡
⎢
⎢
⎢
⎢
⎣
⎤
⎥
⎥
⎥
⎥
⎦
12
2
12
1
1
−S12 C12 0 −a2 S12 − a1S1
0
1
2
U311 = D2 A A2 A =
1
1
3
(2.84)
0
0
0
0
0
0
0
0
⎡−cos q + q − q
−sin q + q − q
0 −a2C12 − a1C1 ⎤
)
)
)
1
2
4
1
2
4
⎢
⎢
⎢
⎢
⎣
⎥
−sin q + q − q
cos q + q − q
0 −a2 S12 − a1S1
(
(
)
0
1
2
3
1
2
4
1
2
4
U411 = D2 A A2 A A4 =
⎥
1
1
3
⎥
⎥
⎦
0
0
0
0
0
0
0
0
(2.85)
25
−C
−S12 0 −a2C12
⎡
⎢
⎢
⎢
⎢
⎣
⎤
⎥
⎥
⎥
⎥
⎦
12
−S12 C12 0 −a2 S12
0
1
U212 = D A D2 A2 =
1
1
(2.86)
0
0
0
0
0
0
0
0
−C
−S12 0 −a2C12
⎡
⎢
⎢
⎢
⎢
⎣
⎤
⎥
⎥
⎥
⎥
⎦
12
−S12 C12 0 −a2 S12
0
1
2
U312 = D A D2 A2 A =
1
1
3
(2.87)
0
0
0
0
0
0
0
0
⎡−cos q + q − q
−sin q + q − q
0 −a2C12 ⎤
)
)
)
1
2
4
1
2
4
⎢
⎢
⎢
⎢
⎣
⎥
0 −a2 S12
⎥
−sin q + q − q
cos q + q − q
(
(
)
0
1
2
3
1
2
4
1
2
4
U412 = D A D2 A2 A A4 =
1
1
3
⎥
⎥
⎦
0
0
0
0
0
0
0
0
(2.88)
(2.89)
0
0
0
0
0
0
0
0
0
0
⎡
⎢
⎢
⎢
⎢
⎣
⎤
⎥
⎥
⎥
⎥
⎦
0
0
0
0
0
0
0
1
2
U313 = D A A2 D3 A =
1
1
3
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
⎡
⎤
⎥
⎥
⎥
⎥
⎦
⎢
⎢
⎢
⎢
⎣
0
1
2
3
U413 = D A A2 D3 A A4 =
1
1
3
(2.90)
⎡cos q + q − q
sin q + q − q
0 0⎤
)
)
)
1
2
4
1
2
4
⎢
⎢
⎢
⎢
⎣
⎥
0 0
⎥
sin q + q − q
−cos q + q − q
(
(
)
1
2
3
1
2
4
1
2
4
U414 = D 0 A A2 A D4 A4 =
1
1
3
⎥
⎥
⎦
0
0
0
0
0 0
0 0
(2.91)
26
−C
−S12 0 −a2C12
⎡
⎢
⎢
⎢
⎢
⎣
⎤
⎥
⎥
⎥
⎥
⎦
12
−S12 C12 0 −a2 S12
0
2 1
U222 = A D2 A2 =
1
(2.92)
0
0
0
0
0
0
0
0
−C
−S12 0 −a2C12
⎡
⎤
⎥
⎥
⎥
⎥
⎦
12
⎢
⎢
⎢
⎢
⎣
−S12 C12 0 −a2 S12
0
2 1
2
U322 = A D2 A2 A =
1
3
(2.93)
0
0
0
0
0
0
0
0
⎡−cos q + q − q
−sin q + q − q
0 −a2C12 ⎤
)
)
)
1
2
4
1
2
4
⎢
⎢
⎢
⎢
⎣
⎥
⎥
⎥
⎥
⎦
−sin q + q − q
cos q + q − q
0 −a2S12
(
(
)
0
2 1
2
3
1
2
4
1
2
4
U422 = A D2 A A A =
1
2
3
4
0
0
0
0
0
0
0
0
(2.94)
(2.95)
0
0
0
0
0
0
0
0
0
0
0
0
0
⎡
⎢
⎢
⎢
⎢
⎣
⎤
⎥
⎥
⎥
⎥
⎦
0
0
0
0
1
2
U323 = A D2 A2 D3 A =
1
3
0
0
0
0
0
0
0
0
0
0
0
0
0
⎡
⎢
⎢
⎢
⎢
⎣
⎤
⎥
⎥
⎥
⎥
⎦
0
0
0
0
1
2
3
U423 = A D2 A2 D3 A A4 =
1
3
(2.96)
⎡cos q + q − q
sin q + q − q
0 0⎤
)
)
)
1
2
4
1
2
4
⎢
⎢
⎢
⎢
⎣
⎥
sin q + q − q
−cos q + q − q
0 0
0 0
0 0
(
(
)
0
1
2
3
1
2
4
1
2
4
⎥
⎥
⎥
⎦
U424 = A D2 A2 A D4 A4 =
1
3
0
0
0
0
(2.97)
27
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
⎡
⎢
⎢
⎢
⎢
⎣
⎤
⎥
⎥
⎥
⎥
⎦
0
1
2 2
U333 = A A2 D3 A =
1
3
(2.98)
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
⎡
⎢
⎢
⎢
⎢
⎣
⎤
⎥
⎥
⎥
⎥
⎦
0
1
2 2
3
U433 = A A2 D3 A A4 =
1
3
(2.99)
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
⎡
⎢
⎢
⎢
⎢
⎣
⎤
⎥
⎥
⎥
⎥
⎦
0
1
2
3
U434 = A A2 D3 A D4 A4 =
1
3
(2.100)
⎡−cos q + q − q
−sin q + q − q
0 0⎤
)
)
)
1
2
4
1
2
4
⎢
⎢
⎢
⎢
⎣
⎥
⎥
⎥
⎥
⎦
−sin q + q − q
cos q + q − q
0 0
0 0
0 0
(
(
)
0
1
2
2 3
1
2
4
1
2
4
U444 = A A2 A D4 A4 =
1
3
0
0
0
0
(2.101)
Tõ (1.53) ta cã:
h = Tr U J UT +Tr U J UT +Tr U J UT +Tr U J UT = 0
(
(
(
111
111
1
11
211
2
21
311
3
31
411
4
411
(2.102)
h = Tr U J UT + Tr U J UT + Tr U J UT
(
112
212
2
21
312
3
31
412
4
41
(2.103)
1
1
1
= −a m a S + m l a S − m l a sin −q + q − m a a S
(
)
2
3
1
2
3 3
1
2
4 4
1
2
4
2
2
1
2
2
2
2
h = Tr U J UT +Tr U J UT = 0
(
(2.104)
113
313
3
31
413
4
41
28
h = Tr U J UT
(
)
114
414
4
41
(2.105)
1
1
= m l a S + m l a sin −q + q
(
)
4 4
2
4
4 4
1
2
4
2
2
h = Tr U J UT + Tr U J UT + Tr U J UT
(
(
122
222
2
21
322
3
31
422
4
41
1
1
= m l a S − a m a S − a m a S − m l a sin −q + q − m a a S
(
)
3 2
1
2
2
3
1
2
2
4
1
2
4 4
1
2
4
4
2
1
2
2
2
(2.106)
h = Tr U J UT +Tr U J UT = 0
(
)
)
(2.107)
123
323
3
31
423
4
41
1
1
h = Tr U J UT = m l a S + m l a sin −q + q
(
)
(
)
124
424
4
41
4 4
2
4
4 4
1
2
4
2
2
(2.108)
h = Tr U J UT + Tr U J UT = 0
(
(2.109)
(2.110)
133
333
3
31
433
4
41
h = Tr U J UT = 0
(
)
134
434
4
41
h = Tr U J UT
(
144
444
4
41
(2.111)
1
1
= − m l a S − m l a sin −q + q
(
)
4 4
2
4
4 4
1
2
4
2
2
h = h
(2.112)
(2.113)
(2.114)
121
112
h = h
131
113
h = h
141
114
h = h
(2.115)
(2.116)
(2.117)
132
123
h = h
142
124
h = h
143
134
29
h = Tr U J UT + Tr U J UT + Tr U J UT
(
(
211
211
2
22
311
3
32
411
4
42
1
1
1
= − m l a S + m a a S + m l a sin −q + q + m a a S + m a a S
(
)
3 3
1
2
3
2
1
2
4 4
1
2
4
4
2
1
2
2
2
1
2
2
2
2
(2.118)
h = Tr U J UT + Tr U J UT + Tr U J UT = 0
(
)
)
)
(2.119)
(2.120)
212
212
2
22
312
3
32
412
4
42
h = Tr U J UT + Tr U J UT = 0
(
213
313
3
32
413
4
42
1
h = Tr U J UT = m l a S
(
)
(2.121)
214
414
4
42
4 4
2
4
2
h = Tr U J UT + Tr U J UT + Tr U J UT = 0
(2.122)
(
)
)
)
222
222
2
22
322
3
32
422
4
42
h = Tr U J UT + Tr U J UT = 0
(
(2.123)
(2.124)
223
323
3
32
423
4
42
1
h = Tr U J UT = m l a S
(
)
224
424
4
42
4 4
2
4
2
h = Tr U J UT +Tr U J UT = 0
(
)
)
(2.125)
(2.126)
233
333
3
32
433
4
42
h = Tr U J UT = 0
(
234
434
4
42
1
h = Tr U J UT = − m l a S
(
)
(2.127)
244
444
4
42
4 4
2
4
2
h221 = h212
h231 = h213
h241 = h214
h232 = h223
h242 = h224
h243 = h234
(2.128)
(2.129)
(2.130)
(2.131)
(2.132)
(2.133)
30
Tải về để xem bản đầy đủ
247 trang
10/04/2024
150
Bạn đang xem 30 trang mẫu của tài liệu "Báo cáo Nghiên cứu thiết kế, chế tạo các robot thông minh phục vụ cho các ứng dụng quan trọng - Nhóm sản phẩm robot SCA", để tải tài liệu gốc về máy hãy click vào nút Download ở trên.
File đính kèm:
- bao_cao_nghien_cuu_thiet_ke_che_tao_cac_robot_thong_minh_phu.pdf
