tính toán, thiết kế Robot viết chữ phục vụ việc khắc chữ trên các sản phẩm công nghiệp

Tính toán thiết kế robot GVHD: Th.S Đinh Khắc Toản
Mục lục Trang
1
Tính toán thiết kế robot GVHD: Th.S Đinh Khắc Toản

Nền khoa học kỹ thuật ngày nay đang phát triển rất mạnh mẽ, dẫn tới những
thay đổi lớn lao trong sản xuất. Đó là sự thay đổi lực lượng sản xuất trong mọi
nghành nghề bằng việc thay sức lao động của người bằng máy móc nhằm đảm
bảo tăng năng suất lao động, sản lượng cũng như chất lượng sản phẩm. Do đó
việc sử dụng các tay máy hay còn gọi là Robot công nghiệp vào trong sản xuất
đang rất được ưa chuộng bởi vì chúng đáp ứng được các yêu cầu trên. Như
chúng ta đã biết Robot có rất nhiều ưu điểm đặc biệt là chất lượng và độ chính
xác, ngoài ra còn phải kể đến hiệu quả kinh tế cao, có thể làm việc trong môi
trường độc hại mà con người không thể làm được, các công việc yêu cầu cẩn
thận không được nhầm lẫn,thao tác nhẹ nhàng tinh tế đòi hỏi trình độ của thợ
bậc cao, và quan trọng là Robot không bị căng thẳng như con người nên có thể
làm việc suốt cả ngày.
Ngày nay việc viết chữ và khắc chữ trên các vật liệu của con người là một trong
những nhu cầu rất cần thiết , việc viết đẹp và đều trong thời gian dài quả là điều
khó khăn đối với con người, chính vì thế mà việc nghiên cứu chế tạo ra một thiết
bị như cánh tay robot để làm được việc đó có ý nghĩa rất lớn.
Việc tìm hiểu nghiên cứu Robot trong khuôn khổ môn học tính toán thiết kế
robot sẽ là cơ sở để chúng em tính toán, thiết kế cũng như điều khiển các loại
Robot trong công nghiệp phục vụ sản xuất. Cụ thể, ở đây chúng em chọn đề tài
tính toán, thiết kế Robot viết chữ phục vụ việc khắc chữ trên các sản phẩm công
nghiệp.
2
Tính toán thiết kế robot GVHD: Th.S Đinh Khắc Toản




 !"#$%!"&'"
Đề bài yêu cầu tính toán thiết kế Robot khắc chữ trên mặt phẳng ngang với
hướng viết tùy ý trong không gian làm việc 50×70cm, từ đó ta có thể hình
dung cần 2 bậc tự do để xác định tọa độ một điểm trên một mặt phẳng, một
bậc tự do để xác định chiều cao trong không gian, do đó số bậc tự do tối
thiểu mà Robot cần có là 3 bậc tự do. Dưới đây là một số cơ cấu có thể dung
để xác định các vị trí trong không gian.
(!)*%%""+,-./!,!: Là tay máy có 3 chuyển động cơ bản tịnh tiến theo
phương của các trục hệ tọa độ gốc (cấu hình TTT). Không gian làm việc của bàn
tay có dạng khối chữ nhật.
Hình 1.1 Cơ cấu tọa độ Đecac
(!)*%%""+,-"*0: Không gian làm việc của robot có dạng hình trụ rỗng.
Thường khớp thứ nhất là chuyển động quay.
Hình 1.2 Cơ cấu tọa độ trụ
3
Tính toán thiết kế robot GVHD: Th.S Đinh Khắc Toản
(!)*%%""+,-!: Không gian làm việc của robot có dạng hình cầu.
Hình 1.3: Cơ cấu tọa độ cầu
12!3&4(52"&'"6'78!)"*9!!2!6&:6&;3

Phương án 1: Robot 4DOF TTRR Phương án 2 : Robot 3DOF RRR

Phương án 3: Robot 3DOF RRT Phương án 4: Robot 4DOF TTRR
4
Tính toán thiết kế robot GVHD: Th.S Đinh Khắc Toản

Phương án 5: Robot 4DOF RTRR Phương án 6: Robot 3DOF RTR

Phương án 7 : Robot 3DOF TTR
<&:=!&78"&'"6'!)"*9!4>!!&+

Với kết cấu 4, 5, 6 bậc tự do, Robot sẽ trở nên linh hoạt hơn, sự di chuyển
của Robot sẽ uyển chuyển hơn nhưng đồng thời việc tính toán và thiết kế cũng
phức tập hơn do đó chi phí cho Robot là rất lớn. Để tiết kiệm về mặt kinh tế
nhưng vẫn đảm bảo được các yêu cầu của bài toán đặt ra, ta lựa chon phương
án 7: Robot 3 bậc tự do TTR, Robot có 2 khâu tịnh tiến xác định vị trí một
điểm trên mặt phẳng và khâu cuối chuyển động quay để xác định tọa độ theo
chiều cao và hướng viết trong hệ tọa độ Dề Các. Do đó việc lựa chọn phương
án này hoàn toàn thỏa mãn yêu cầu bài toán khi cần thao tác trên mặt phẳng
với hương viết tùy ý.
Kết cấu 3D sơ bộ của Robot được thiết kế như sau.
5
Tính toán thiết kế robot GVHD: Th.S Đinh Khắc Toản
Hình 1.4 phương án thiết kế cho robot TTR
Khớp 1 là khớp tịnh tiến sẽ đi dọc chiều dài miền làm việc theo yêu cầu , khớp
2 là khớp tịnh tiến và sẽ đi ngang miền làm việc, như vậy với 2 khớp trên ta đủ
để xác định vị trí của một điểm trên mặt phẳng thao tác.
Khớp 3 là khớp quay để điều chỉnh độ cao của bút vẽ so với mặt bàn ngang.
Khâu 1 và khâu 2 sẽ dùng vít me ổ bi vì cùng là tịnh tiến
Khâu 3 dùng ổ bi.
Phần tính chọn vít me , ổ bi và các thông số chi tiết cho kết cấu sẽ được trình
bày kĩ trong phần thiết kế cơ khí và tính chọn vít me ổ bi bên dưới.
Với kết cấu như trên theo nhóm 4 là phù hợp với yêu cầu của đề bài.
Sau khi lựa chọn kết cấu và chọn sơ bộ các khâu, khớp thì sẽ tiến hành giải các
bài toán động học, động lực học,chọn động cơ, và mô phỏng.

6
Tính toán thiết kế robot GVHD: Th.S Đinh Khắc Toản
1
?@AB.C


1D&E%F2"8"%2-5&+!"& G%%"
Với mô hình tính toán bên trên ta đặt các hệ trục tọa độ theo quy tắc Denevit –
Hatenberg và có sơ đồ hệ trục như hình vẽ:

Hình 2.1 sơ đồ hệ trục robot
Sau khi có sơ đồ trên thì ta thiết lập bảng DH:
Bảng 2.1 Bảng các tham số động học của robot
khâu
1 0
1
q
a
1
90
0
2
π

2
q
0 90
0
3
3
q
0 a
3
0
Trong đó
1 2 3

, ,q q q
là các biến khớp , còn a
1
, a
3
là các hằng số
Và X=[x
1
,x
2
,x
3
]
T
là véc tơ biểu diễn vị trí của bàn kẹp
trong hệ cố định.
7
Tính toán thiết kế robot GVHD: Th.S Đinh Khắc Toản

1 2 3
[ , , ]q q q q
=
là các góc xoay và tịnh tiến của các biến
khớp
- Dạng tổng quát của ma trận Denavit-Hartenberg cho các
khâu
i-1
A
i
=

( )
i i i i i i i
i i i i i i i
i i i
cosθ sinθ cosα sinθ sinα a cosθ
sinθ cosθ cosα sinα cosθ a sinθ
2.0
0 sinα cosα d
0 0 0 1
−
 
 
−
 
 
 
 
Ma trận Denavit-Hartenbergcủa khâu 1:

( )
1
0
1
1
1 0 0
0 0 1 0
2.1
0 1 0
0 0 0 1
a

A
q
 
 
−
 
=
 
 
 
Ma trận Denavit-Hartenbergcủa khâu 2:

( )
1
2
2
1 0 0 0
0 0 1 0
2.2
0 1 0
0 0 0 1
A
q
−
 
 
 
=
 
 

 
Ma trận Denavit-Hartenbergcủa khâu 3:

( )
3 3 3 3
2 3 3 3
2
3
cos sin 0 .cos
sin cos 0 .sin
2.3
0 0 1 0
0 0 0 1
q q a q
q q a q
A
−
 
 
 
=
 
 
 
1D1&'"H 3!2!3&4(5"*I&-5&+!*%%"
Từ các ma trận (2.1) và (2.2) ta xác định được ma trận biến đổi
tọa độ thuần nhất của khâu 2 so với trục hệ tọa độ cố định là :
8
Tính toán thiết kế robot GVHD: Th.S Đinh Khắc Toản

1
2
0 0 1
2 1 2
1
1 0 0
0 1 0
. (2.4)
0 0 1
0 0 0 1
a
q
A A A
q
−
 
 
− −
 
= =
 
 
 

3 3 3 3 1
3 3 3 3 2
0 0 1 2
3 1 2 3
1
cos sin 0 .cos

sin cos 0 .sin
. . (2.5)
0 0 1
0 0 0 1
q q a q a
q q a q q
A A A A
q
− − +
 
 
− − − −
 
= =
 
 
 
- Ma trận
0
3
A
cho ta biết hướng và vị trí của khâu thao tác trong
hệ tọa độ cố định hay nói cách khác là vị trí của điểm tác động
cuối và hướng của hệ tọa độ động gắn vào khâu tại điểm tác
động cuối trong hệ tọa độ cố định. Vì thế nó còn được biểu diễn
qua thông số các biến khớp ta tạm gọi là q
i.
Trong bài toán cụ
thể thì nó là các khớp xoay θ
i

, với i=1÷3.
- Khi đó, ma trận (2.5) được kí hiệu thành
0
3
( )A q
Sử dụng các góc Cardan xác định hướng của vật rắn. Ta gọi
[ ]
, , , , ,
E E E
x y z
α β η
là giá trị mô tả trực tiếp vị trí và hướng của
EX
2
Y
2
Z
2
so với hệ tọa độ O
0
Z
0
Y
0
Z
0
. Trong đó:
[ ]
, ,
E E E

x y z
là các tọa
độ điểm E và là các góc quay Cardan của EX
3
Y
3
Z
3
so với hệ tọa
độ O
0
Z
0
Y
0
Z
0
. Do các tọa độ thao tác đều là hàm của thời gian.
Nên ta có thể biểu diễn:

0 0
0
( ) ( )
( ) (2.6)
0 1
n E
n
T
R t r t
A t

 
=
 
 

Với:
0
E
A
là ma trận Cardan mô ta hướng EX
2
Y
2
Z
2
so với hệ
tọa độ O
0
Z
0
Y
0
Z
0
.

0
E
r
là vectơ mô tả vị trí của điểm tác động cuối trong

hệ tọa độ O
0
Z
0
Y
0
Z
0
.
9
Tính toán thiết kế robot GVHD: Th.S Đinh Khắc Toản

[ ]
0
( ) (t), (t), ( ) (2.7)
T
E E E E
r t x y z t=
0
cos cos cos sin sin
sin sin cos cos sin sin sin sin cos cos sin cos (2.8)
cos sin cos sin sin cos sin sin sin cos cos

s

co

CDn
R R
β η β η β

α β η α η α β η α η α β
α β η α η α β η α η α β
−
 
 
= + − + −
 
 
− + +
 
=
- Do ma trận
0
3
( )A q
biểu diễn vị trí và hướng của khâu thao
tác trong hệ tọa độ cố định thông qua biến khớp q
i
(Ma trận
trạng thái khâu thao tác theo cấu trúc động học). Còn ma trận
0
( )
E
A t
cũng mô tả vị trí và hướng của khâu thao tác thông qua
hệ tọa độ khâu thao tác. Ở đây ta chọn cách biểu diễn thông
qua các góc Cardan.
Từ đó ta có PT động học ROBOT có dạng:
0 0
3

( ) ( )
n
A q A t=
. Với
n = 3 vì cơ cấu ROBOT có 3 khâu.
0 0
3 3
( ) ( ) (2.9)A q A t=
- Từ các hệ thức (2.5), (2.6), (2.7),(2.8) ,(2.9) Ta xây dựng
được hệ 6 phương trình độc lập như sau:
0 0
1 3 3 3 3 1
0 0
2 3 3 3 3 2
0 0
3 3 3 1
0 0
4 3 3 3
0 0
5 3 3 3
( )[1,4] ( )[1,4] cos
( )[2,4] ( )[2,4] sin
( )[3,4] ( )[3,4]
( )[1,1] ( )[1,1] cos (cos cos )
( )[2,2] ( )[2,2] cos ( sin si
E
E
E
f A q A t a q a x
f A q A t a q q y

f A q A t q z
f A q A t q
f A q A t q
β η
α
= − = − + −
= − = − − −
= − = −
= − = − −
= − = − − −
0 0
6 3 3
(2.10)
n sin cos cos )
( )[3,3] ( )[3,3] 1 (cos cos ) 0f A q A t
β η α η
α β








+


= − = − =


1D<@8"%2-5&+!"& 
Bài toán động học thuận: Các thông số đầu vào:
1 2 3
, ,q q q

Thông số cần xác định: điểm tác động cuối E = (
E
x
,
E
y
,
E
z
), và
hướng của khâu tác động cuối so với hệ tọa độ cơ sở.
* Ta có tọa độ của điểm tác động cuối:
10
Tính toán thiết kế robot GVHD: Th.S Đinh Khắc Toản
[ ]
[ ]
[ ]
0
3 3 3 1
0
3 3 3 2
0
3 1
1,4 cos
2,4 sin (2.11)

3,4
E
E
E
A a q a
A a q q
A q
x
y
z

= = − +


= = − −


= =


Ta chọn quy luật chuyển động cho robot theo thời gian như
sau :
( )
.
2
1
1
.
2
2 2

.
2
3
3
( ) cos( )
( ) sin( )
18 6
2 3 6
( ) sin( ) ( ) cos( ) 2.12
3 6 4 24 4
( ) sin( )
( ) cos( )
6 2 3
6 3
t
t
q t
q t
t t
q t q t
t
t
q t
q t
π π
π π π
π π π π π
π π π
π π




=
= +




 
= − ⇒ = −
 
 
 
= +
 
=



g
g
g

Và các thông số hình học:
a
1
= 50 cm, a
3
= 30 cm
để thuận tiện cho việc tính toán ta sử dụng phần mềm tính

toán maple
Thay các giá trị
1 2 3
( ) , ( ), ( )q t q t q t
và các thông số hình học a
1
, a
3

vào biểu thức của
, ,xE yE zE
ta có ta có tọa độ của điểm thao
tác là :
Ta có thể vẽ đồ thị của điểm thao tác cuối qua các tọa độ như
sau với t=0 10s:
11
Tính toán thiết kế robot GVHD: Th.S Đinh Khắc Toản

Hình 2.1 Đồ thị xE theo t

Hình 2.2 Đồ thị yE theo t
12
Tính toán thiết kế robot GVHD: Th.S Đinh Khắc Toản


Hình 2.3 Đồ thị zE theo t

Hình 2.4: Đồ thị quỹ đạo chuyển động của điểm tác động cuối
trong không gian
J&7 "!KL"2!-5!MN7 "!5!6&:"&,%"2!

Vận tốc điểm tác động cuối của Robot
Từ phần trên ta đã xây dựng được quy luật chuyển cũng như
tìm được tọa độ của khâu thao tác cuối, các biến khớp và đạo
hàm các cấp theo t đã biết :
13
Tính toán thiết kế robot GVHD: Th.S Đinh Khắc Toản
( )
1 2 3
[ , , ] 2.13
T
i
q q q q=
Suy ra

( )
1 2 3
[ , , ] 2.14
T
q q q q
•
=
g g g
Vận tốc của khâu thao tác chính là đạo hàm vị trí khâu thao
tác theo thời gian:
V
E
=
r

E

=
[ ]
( )
, , 2.15
T
E E E
x y z
& &
&
Từ các phương trình về vị trí của điểm tác động cuối của robot
ta tính được vận tốc của điểm cuối

Đồ thị vận tốc của điểm E trong không gian thao tác:
14
Tính toán thiết kế robot GVHD: Th.S Đinh Khắc Toản
Hình 2.5 Đồ thị vận tốc điểm E
Đạo hàm các hàm của vận tốc ta được các đồ thị của gia tốc
điểm E theo các phương:
15
Tính toán thiết kế robot GVHD: Th.S Đinh Khắc Toản
Hình 2.6 Đồ thị gia tốc điểm E theo các phương
Các ma trận cosin chỉ hướng của các khâu:
Ta có :

( )
2
1 0 0
0 1 0 2.18
0 0 1
R

−
 
 
= −
 
 
 

( )
3 3
3 3 3
cos sin 0
sin cos 0 2.19
0 0 1
q q
R q q
−
 
 
= − −
 
 
 

Các vector vị trí của hệ tọa độ gắn với từng khâu khi đó là :

1
1
1
0

a
VT
q
 
 
=
 
 
 

1
2 2
1
a
VT q
q
 
 
= −
 
 
 

3 3 1
3 3 3 2
1
cos
sin
a q a
VT a q q

q
− +
 
 
= − −
 
 
 
(2.
Đạo hàm các ma trận quay trên ta được :
16
Tính toán thiết kế robot GVHD: Th.S Đinh Khắc Toản
( )
1
0 0 0
0 0 0 2.21
0 0 0
R
 
 
=
 
 
 
&

( )
2
0 0 0
0 0 0 2.22

0 0 0
R
 
 
=
 
 
 
&

( )
3 3
3
3 3 3
sin cos 0
cos sin 0 . 2.19
0 0 0
q q
R q q q
• •
 
 
 
= −
 ÷
 
 
 
 

Từ các ma trận trên ta có các ma trận sóng và vận tốc góc (tuyệt đối ) các
khâu là:

0
. 0 (2.24)
0
iz iy
T
i i i iz ix
iy ix
R R
ω ω
ω ω ω
ω ω
 
−
 
= = −
 
 
−
 
&
%
với i=1:3
Ta có :
Vận tốc góc khâu 1:
( )
1 1 1
0 0 0 0

0 0 0 . 0 2.25
0 0 0 0
q
ω ω
•
   
 
   
= ⇒ =
 ÷
   
 
   
   
%
Vận tốc góc khâu 2:
( )
2 2 2
0 0 0 0
0 0 0 . 0 2.26
0 0 0 0
q
ω ω
•
   
 
   
= ⇒ =
 ÷
   

 
   
   
%
Vận tốc góc khâu 3:
3
3 3 3 3
0 0
. 0 0
0 0 0
T
q
R R q
ω
•
•
 
−
 
 
= =
 
 
 
 
&
%

3
3

0
0 (2.27)
q
ω
•
 
 
 
⇒ =
 
 
 

1DO@8"%2-5&+!54>!
Từ bài toán động học thuận ta có được hệ phương trình :
17
Tính toán thiết kế robot GVHD: Th.S Đinh Khắc Toản
[ ]
[ ]
[ ]
0
3 3 3 1
0
3 3 3 2
0
3 1
1,4 a
2,4 sin (2.31)
3,4
E

E
E
A a cosq
A a q q
A q
x
y
z

= = − +


= = − −


= =


Bài toán đặt ra là : cho biết các tọa độ
[ ]
E E E
x y z
và nhiệm vụ
của ta là phải tìm lại các biến khớp
[ ]
1 2 3
T
q q q q
=

Với các x
E
, y
E
, z
E
,a
1
,a
3
đã biết.
Giả sử ta phải cho quỹ đạo của điểm tác động cuối E là theo
một đường cánh hoa trong mặt phẳng thao tác, tức là mặt
phẳng Oyz

( )
40
40 30.sin(2 ).cos(t) cm 2.32
z 30 40.sin(2 ).sin(t) cm
E
E
E
x cm
y t
t
=


= +



= +


Cánh hoa có dạng 4 cánh như sau:

15 20 25 30 35 40 45 50 55 60 65
-10
0
10
20
30
40
50
60
70
yE
zE
Do thi hinh cánh hoa
Hình 2.7 Đồ thị quỹ đạo cần vẽ
18
Tính toán thiết kế robot GVHD: Th.S Đinh Khắc Toản
Từ phương trình vị trí điểm E (2.31) thì ta có thể tìm được các
biến khớp q
i
như sau :
( )
1
2 2
3 1

1
3 3
3 3
3 3 3
2 2
2 3 3 3 1
( )
cos sin
2.33
tan 2(sin ,cos )
.sin ( )
E
E
E
E E E
q z
a a x
a x
q q
a a
q a q q
q a q y a a x y
=


− −
−

= ⇒ =




⇒ =


= − − = − − − −


Thay các giá trị trên vào ta được:
( )
1
3 3 3
2 2 2
2
2
30 40sin(2 )sin( )
50 40 1 8
cos sin 19.5
30 3 3
2.34
30 (50 40 ) 40 30sin(2 )cos( )
40 30sin(2 )cos(t)
E
o
q z t t
q q q
q t t
q t
= = +



−

= = ⇒ = ⇒ =



= − − − − −


⇒ = − −


Suy ra để vẽ được hình hoa 4 cánh trên thì các tọa độ suy rộng
phải thỏa mãn (2.34) và ta có đồ thị các biến khớp của tọa độ
suy rộng:

0 1 2 3 4 5 6 7 8 9 10
-80
-60
-40
-20
0
20
40
60
80
t [s]
q1,q2 [cm]
Do thi cac bien khop q1,q2

q1
q2
Hình 2.8 Đồ thị biến khớp q
1
q
2

19
Tính toán thiết kế robot GVHD: Th.S Đinh Khắc Toản
0 1 2 3 4 5 6 7 8 9 10
18.5
19
19.5
20
20.5
t [s]
deg
thi bien khop q3


q3
Hình 2.9 Đồ thị biến khớp q
3
<
PGA@A
<D&:=!&H#!
Lực môi trường tác động vào khâu cuối chỉ gồm phản lực tác dụng lên đầu
dụng cụ cắt và có giá trị rất nhỏ coi như là bằng 0, như vậy:

[ ]
[ ]
[ ] [ ]
[ ] [ ]
0
3
0
3
0 0
43 3
0 0
43 3
0 0 0
0 0 0
T
E
T
E
T T
E
T T
E
F Fx Fy Fz
M Mx My Mz
F F Fx Fy Fz
M M Mx My Mz

=



=



= − = − − − =

⇒

= − = − − − =


Tính lực (mô men ) dẫn động tại các khớp đảm bảo Robot cân bằng tĩnh. Hệ
phương trình cân bằng trong tọa độ cơ sở:

0 0 0
i,i-1 i+1,i i
0 0
0 0 0 0
i
i,i-1 i+1,i i,i-1
i
i
c
F F
F
= - P
M = M r - r P




−


% %
20
Tính toán thiết kế robot GVHD: Th.S Đinh Khắc Toản
Với :

0
, 1
T
i i x y z
F F F F
−
 
=
 
là lực do khâu i-1 tác dụng lên khâu i ở khớp thứ I
trong hệ tọa độ cơ bản.

0
M
i,i-1
=
y zx
M M M
 
 
T

là mô men do khâu i-1 tác dụng lên khâu i ở khớp
thứ i trong hệ tọa độ cơ bản.



0
yx z
P P P
T
i
P
 
=
 
là trọng lực của khâu i trong hệ tọa độ cơ bản.
0
r
i
=
0
R
i
i
r
i
là vector có gốc là O
0
nối với O
i
trong hệ tọa độ cơ bản.

0
R
i
=
0
R
1
1
R
2
…
i-1
R
i
là ma trận quay biến đổi từ hệ tọa độ 0 đến hệ tọa độ thứ i

i
r
i
=
i
i i
i i
a
sinα
cosd α
d
 
 

 
 
 
là vector có gốc O
i-1
nối với O
i
trong hệ tọa độ khâu i.
0
r
ci
=
0
A
i

i
r
ci
là vector có gốc O
0
nối với C
i
trong hệ tọa độ cơ bản .

i
r
ci
: là vector có gốc O
i

nối với C
i
trong hệ tọa độ khâu i
<DDJ&H#!78L%L/Q6&:<
Ta có ma trận côsin chỉ hướng :
3 3
3 3 3
0
0
0 0 1
c s
R s c
−
 
 
= −
 
 
 

3
3
0
0
m g
P
−
 
 
=

 
 
 
;
3
3
3
0
0
a
r
−
 
 
=
 
 
 
Ta có
3 3
0 3
3 3 3 3 3
0
a c
r R r a s
 
 
= =
 
 

 

3 3
3 3
3
3 3 3 3
0 0
0
0 0
0
a s
a c
a s a c
r
 
 
=> = −
 
 
−
 
%
3
3
3
2
0
0
C
a

r
−
 
 
 
=
 
 
 
 

3 3
0 3
3 3
3 3 3
2
2
0
C C
a c
a s
r R r
 
 
 
 
= =
 
 
 

 
 

3 3
3 3
3
3 3 3 3
0 0
2
0
0 0
2
0
2 2
C
a s
a c
a s a c
r
 
 
 
 
=> = −
 
 
 
−
 
 

%
21
Tính toán thiết kế robot GVHD: Th.S Đinh Khắc Toản
3
3,2 ,3 3E
m g Fx
F F P Fy
Fz
−
 
 
= − = −
 
 
−
 
3 3
~ ~
0 0
3,2 ,3 3 3,2 3 3 3 3
3 3 3
3 3 3 3 3
( )
2
E C
a s Fz Mx
M M r F r P a c Fz My
a s m g
a s m g Fx a c Fy Mz
 

 
−
 
=> = − − = − −
 
 
− + − −
 
 
<DD1J&H#!78L%L/6&:1
Ta có ma trận côsin chỉ hướng :
2
1 0 0
0 1 0
0 0 1
R
−
 
 
= −
 
 
 

2
2
0
0
m g
P

−
 
 
=
 
 
 
2
2 2
0
0
r q
 
 
= −
 
 
 
Ta có
0 2
2 2 2 2
0
0
r R r q
 
 
= =
 
 
 

2
2
2
0 0
0
0 0 0
0 0
q
q
r
 
 
=> =
 
 
−
 
%

2
2 2
0
1
2
0
C
r a
 
 
 

= −
 
 
 
;
0 2
2 2 2 2
0
1
2
0
C C
r R r a
 
 
 
= =
 
 
 

2
2
2
1
0 0
2
0
0 0 0
1

0 0
2
C
a
a
r
 
 
 
=
 
 
−
 
 
%

2 3
2,1 3,2 2
m g m g Fx
F F P Fy
Fz
+ −
 
 
= − = −
 
 
−
 

~ ~
0 0
2,1 32 2 21 2 2C
M M r F r P=> = − −

3 3
2,1 3 3
3 3 3 2 2
3 3 3 3 3 2 2 3
2
( ) ( )
2 2
z z x
z y
x y x z
a s F q F M
M a c F M
a s m g a m g
a s m g F a c F q m g m g F M
 
 
+ −
 
=> = − −
 
 
− + − + + − − −
 
 
22

Tính toán thiết kế robot GVHD: Th.S Đinh Khắc Toản
<DD<J&H#!78L%L/6&:
Ta có ma trận côsin chỉ hướng :
1
1 0 0
0 0 1
0 1 0
R
 
 
= −
 
 
 
;
1
1
0
0
m g
P
−
 
 
=
 
 
 
1
1

1
1
0
a
r
q
−
 
 
=
 
 
 
Ta có
1
1 1
1
1
0 0
0
0
0 0
q
q a
a
r
 
 
=> = − −
 

 
 
%
1
1
1
1
1
2
0
C
a
r
q
 
−
 
 
=
 
 
−
 
 

1 2 3
1,0 2,1 1
( )m m m g Fx
F F P Fy
Fz

+ + −
 
 
= − = −
 
 
−
 
~ ~
0 0
1,0 21 1 10 1 1C
M M r F r P=> = − −
3 3 1 2
1,0 3 3 1 1 2 3 1 1 1
3 3 3 2 2
3 3 3 3 3 2 2 3 1
( )
( ) ( )
2 2
a s Fz q Fy q Fz Mx
M a c Fz q m g m g m g Fx a Fz q m g My
a s m g a m g
a s m g Fx a c Fy q m g m g Fx a Fy Mz
 
 
+ + −
 
=> = − + + + − + − −
 
 

− + − + + − − − −
 
 
<D1J&"%2H#!78L%L/H;&)"Q"*R5"&2S&
Cho các tham số: m
1
=267.5, m
2
=19.2 , m
3
=40 (kg) a
0
=1.1 (chiều dài khâu đế)
1
0.87a =

2
1a
=
(chiều dài tự nhiên khâu 2)
3
0.6a m
=

Với mô hình xây dựng trên solid works, ta có các tham số:
khâu 3:
3 3
0 0 3 0
3 3 3 3 3 3
0.6 0.21

0.6 , 0.21
0 0.03
C
c c
r R r s r s
   
   
= = =
   
   
   

3,2 3,2
3
400 0
0 , 12 ,
0 156
F M
s
  
  
⇒ = = −
  
 
 
   
23
Tính toán thiết kế robot GVHD: Th.S Đinh Khắc Toản
Mà -1
≤

sin(q3)
≤
1,do đó lực và momen lớn nhất ở khâu 3 là :
3,2 3,2 3,2 ax
4.4 0
0 , 12 , 156.5( )
0 156
m
F M M Nm
   
   
= = − =
   
   
   
khâu 2:
0 0
2 2 2
0 0
, 0.32
0 0
C
r q r
   
   
= =
   
   
   

2,1 2,1
3 2
592 0
0 , 12
0 156 592 61.44
F M
s q
  
  
⇒ = = −
  
 
 
+ −
   
Mà -1
≤
sin(q3)
≤
1, q2
≤
0.8
a2,do đó lực và momen lớn nhất ở khâu 2 là :
2,1 2,1 2,1 ax
592 0
0 , 12 , 688( )
0 687.5
m
F M M Nm
   

   
⇒ = = − ⇒ =
   
   
   
khâu 1:
0 0
1 1
1
0.87 0.36
0 , 0
0
C
r r
q
− −
   
   
= =
   
   
−
   
1,0 1,1 1
3 2
3267 0
0 , 12 3267
0 156 592 61.44
F M q
s q

  
  
⇒ = = − +
  
 
 
+ −
   
Mà -1
≤
sin(q3)
≤
1 ; q2
≤
0,8.a2
; q1
≤
0.8.
0
a
(chiều dài khâu đế )
do đó lực và momen lớn nhất ở khâu 1 là :
1,0 1,0 1,0 ax
3267 0
0 , 2863 2900( . )
0 498
m
F M M N m
   
   

= = ⇒ ≈
   
   
   
24
Tính toán thiết kế robot GVHD: Th.S Đinh Khắc Toản
O
.CTGA@A
Chúng ta đã biết phương trình lagrang loại 2 viết từng tọa độ suy rộng như sau:
np
i i i
d T T
Q
dt q q q
 
∂ ∂ ∂Π
− = − +
 ÷
∂ ∂ ∂
 
&
Để tránh dài dòng ta không đi chứng mình dạng ma trận của phương trình
Lagrang áp dụng luôn ( có thể tham khảo cách chứng minh trong quyển Robot
công nghiệp của GS. TSKH. Nguyễn Văn Khang )
( ) ( , ) ( )
np
M q q C q q q G q Q+ + =
&& &&
Từ (4.1) viết lại dạng điều khiển như sau :
( ) ( , ) ( )

T
E
M q q C q q q G q U J F+ + = +
&& &&
ODU:V$#5!)"*9!-5H#!&+!

H Hình 4.1. Sơ đồ tĩnh học
ODDU2!W&"&'X5!Y,*%%"
Chọn gốc tính thế nắng tại mặt đất ta có :
3
1
i Ci
i
m X g
=
Π =
∑
1 1 1 1
1
0.51 1 0
C
m X g m g g
q
δ
δ
Π
Π = = ⇒ = =
25