Bài tập lớn robotics đề 2

ROBOTICS Tiểu Luận
Lời nói đầu
Cùng với sự phát triển của khoa học và kĩ thuật, ngành điều khiển học và tự động hóa
đã có những bước tiến quan trọng. Các robot công nghiệp, robot song song, robot dáng
người,mobile robot, cánh tay….có một vị trí quan trọng trong tự động hóa các quá trình
sản xuất cũng như trong dịch vụ và giải trí. Góp phầnkhông nhỏ vào quá trình tăng năng
suất lao động,giảm giá thành, tăng chất lượng, đồng thời tạo điều kiện cải thiện môi
trường làm việc, đặc biệt trong các môi trường làm việc nguy hiểm và có tinh độc hại
cao.
Vì vậy nhu cầu chế tạo và sử dụng robot trong công nghiệp ngày càng tăng, các loại
robot ngày càng đa dạng, chính xác cao, linh hoạt hơn, giá cả phù hợp hơn, năng suất và
tuổi thọ ngày càng cao.
Nhằm giúp sinh viên tìm hiểu và làm quen với nó, trường ĐH Bách Khoa đã đưa vào
giảng dạy môn học : Robotics cho sinh viên ngành cơ khí.
Được sự giảng dạy của PGS.Phan Bùi Khôi ,chúng em được học và tiếp thu thêm
nhiều kiến thức về robot Công Nghiệp.Trong quá trình thực hiện tiểu luận,em có sử dụng
phần mềm Maple , matlap hỗ trợ. Trong quá trình làm tiểu luận, do kiến thức còn hạn chế
nên không tránh khỏi những thiếu sót, em mong nhận được sự đóng góp, chỉ bảo của thầy
cô và các bạn để bài làm của em được hoàn thiện hơn.
Một lần nữa em xin chân thành cảm ơn thầy giáo đã giảng dạy, hướng dẫn em hoàn thành
bài tiểu luận này.
Đỗ Dabh Phượng 1991-20109151
Lớp: CN-CĐT2-K55 Trang 1
ROBOTICS Tiểu Luận
Chương I Xây dựng cấu trúc, thiết lập hệ phương trình động học của
robot.
Tính toán động học thuận, động học ngược Robot RRR
1. Động học thuận
Ta thiết lập phương trình động học theo phương pháp ma trận Denavit Hartenberg
a. Thiết lập hệ tọa độ theo quy tắc Denavit Hartenberg
Từ việc chọn hệ tọa độ ta có bảng DH sau:

Thiết lập phương trình động học robot theo phương pháp Danavit- hartenbeg:
a).Với các hệ trục tọa độ đã chọn, các tham số động học D-H được xác định như trong
bảng sau:
Đỗ Dabh Phượng 1991-20109151
Lớp: CN-CĐT2-K55 Trang 2
ROBOTICS Tiểu Luận
Khâu

i
d

i
θ

i
a
i
α
1

1
a
1
q
0
/ 2
π
2 0
2
q

2
a
0
3 0
3
q

3
a
0
0
1
A
=
1 1
1 1
1
os( ) 0 sin( ) 0
sin( ) 0 os( ) 0
0 1 0
0 0 0 1
c q q
q c q
a
 
 ÷
−
 ÷
 ÷

 ÷
 
,
1
2
A
=
2 2 2 2
2 2 2 2
cos( ) sin( ) 0 os( )
sin( ) os( ) 0 sin( )
0 0 1 0
0 0 0 1
q q a c q
q c q a q
−
 
 
 
 
 
 

2
3
A
3 3 3 3
3 3 3 3
os( ) sin( ) 0 os( )
sin( ) os( ) 0 sin( )

0 1 1 0
0 0 0 1
c q q a c q
q c q a q
 
 ÷
 ÷
=
 ÷
 ÷
 
0 0 1 2
3 1 2. 3
.A A A A
=

1 2 3 1 2 3 1 1 3 2 3 2 2
1 2 3 1 2 3 1 1 3 2 3 2 2
0
3
2 3 2 3 3 2 3 2 2 1
cos(q )cos( ) cos( )sin( ) sin( ) cos ( cos( ) cos( ) )
sin( )cos( ) sin( )sin( ) sin( ) sin ( cos( ) cos( ))
sin( ) cos( ) 0 sin( ) sin( )
0 0 0 1
q q q q q q q a q q a q
q q q q q q q q a q q a q
A
q q q q a q q a q a
+ − + + +

+ − + − + +
=
+ + + + +
 
 ÷
 ÷
 ÷
 ÷
 
b.Xác định vị trí điểm tác động cuối P
Từ ma trận trạng thái của hê X
3
Y
3
Z
3
đối với hệ cố định XoYoZo
0
3
A
ta xác
định được điểm tác động cuối P của khâu 2 so với hệ cố định, và ma trận cosin chỉ
hướng khâu 3 so với XoYoZo
0
P
r
=
Đỗ Dabh Phượng 1991-20109151
Lớp: CN-CĐT2-K55 Trang 3
ROBOTICS Tiểu Luận

[ ]
1 3 2 3 2 2 1 3 2 3 2 2 3 2 3 2 2 1
cos ( cos( ) cos( ) ) sin ( cos( ) cos( )) sin( ) sin( )
T
q a q q a q q a q q a q a q q a q a
+ + + + + + +
1 2 3 1 2 3 1
0
3 1 2 3 1 2 3 1
2 3 2 3
cos(q )cos( ) cos( )sin( ) sin( )
sin( )cos( ) sin( )sin( ) cos( )
sin( ) cos( ) 0
q q q q q q
R q q q q q q q
q q q q
+ − +
 
 ÷
= + − + −
 ÷
 ÷
+ +
 

1 3 2 3 2 2
1 3 2 3 2 2
3 2 3 2 2 1
cos ( cos( ) cos( ) )
sin ( cos( ) cos( ))

sin( ) sin( )
p
p
p
x q a q q a q
y q a q q a q
z a q q a q a

= + +

= + +


= + + +

2.1. Cho quy luật chuyển động của các khớp
q1=20.t
q2=3.cos.t
q3=4.t2
a1=300mm ;
a2=100mm
a3=100mm
b. Vị trí của điểm P
1 3 2 3 2 2
1 3 2 3 2 2
3 2 3 2 2 1
cos ( cos( ) cos( ) )
sin ( cos( ) cos( ))
sin( ) sin( )
p

p
p
x q a q q a q
y q a q q a q
z a q q a q a

= + +

= + +


= + + +


V1=

V2=
Đỗ Dabh Phượng 1991-20109151
Lớp: CN-CĐT2-K55 Trang 4
ROBOTICS Tiểu Luận
V3=
Đỗ Dabh Phượng 1991-20109151
Lớp: CN-CĐT2-K55 Trang 5
ROBOTICS Tiểu Luận
Chú ý: Ta có robot phẳng ba bậc tự do khi tính toán hay điều khiển ta quan
tâm đến điểm tác động cuối trong phẳng không quan tâm đến hướng khấu cuối vì
khi ta tìm được các biến khớp từ điểm tác động cuối và từ đó tìm được ma trận
cosin chỉ hướng
0
2

R
từ đó xác định được các góc Cardan hay Eule
Quỹ đạo chuyển động của diểm P
2. Động học ngược
Khi giải bài toán động học thuận người ta xác định được quan hệ dước dạng
ma trận như sau: x=f(q) . Từ quan hệ đó ta suy ra một cách hình thức q=f
-1
(x)
trong đó x = [x
1
,x
2
,…,x
m
]
T
,q = [q
1
,q
2
,…,q
n
]
T
.
Như vậy ta có hai chiều hướng để giải đó là phương pháp số và phương pháp
giải tích với bài toán RobotRRR đơn giản ta dùng phương pháp giải tích là đơn
giản và dễ giải nhất nhưng với những bài toán robot nhiều bậc phương pháp số là
Đỗ Dabh Phượng 1991-20109151
Lớp: CN-CĐT2-K55 Trang 6

ROBOTICS Tiểu Luận
phương pháp hữu hiệu
từ hệ
1 3 2 3 2 2
1 3 2 3 2 2
3 2 3 2 2 1
cos ( cos( ) cos( ) )
sin ( cos( ) cos( ))
sin( ) sin( )
p
p
p
x q a q q a q
y q a q q a q
z a q q a q a

= + +

= + +


= + + +


(2.1)
Kí hiệu C
1
= cos(q
1
), S

1
= sin(q
1
), C
2
= cos(q
2
), S
2
= sin(q
2
),C
12
= cos(q
1
+q
2
),
S
12
= sin(q
1
+q
2
),
23 2 3
sin( )S q q
= +
Từ hệ (2.1) =>
1 1

tan( ) arctan
p p
p p
y y
q q
x x
= => =

2 2
2 2
1 1
2 2 2 2
sin ( ) ;cos ( )
( ) ( )
p p
p p p p
y x
q q
x y x y
=> = =
+ +
=> vậy
1
arctan
p
p
y
q
x
=

.
Từ hệ (2.1) ta viết lại như sau
Đỗ Dabh Phượng 1991-20109151
Lớp: CN-CĐT2-K55 Trang 7
ROBOTICS Tiểu Luận
( )
( )
2
2 2 2 2
3 23 2 2 3 23 3 2 23 2 2 2
2
1 1
2 2 2 2 2
3 23 2 2 1
3 23 3 2 23 2 2 2 1
2 2 2 2 2
1 2 3
3
2 3
2
2 3
3
2
2 2 2 2 2
1 2 3
2
2 ( )
( ) ( )
2
(2 )

tan 1
( ) ( )
p p
p
p
p p p
p p p
y y
a C a C a C a a C C a C
S S
a S a S z a
a S a a S S a S z a
x y z a a a
C
a a
a a
q
x y z a a a


+ = + + =


=>
 
 
+ = −
+ + = −



+ + − − +
=> =
 
 ÷
=> = −
 ÷
+ + − − +
 
=>
( )
2
2 3
3
2
2 2 2 2 2
1 2 3
(2 )
arctan 1
( ) ( )
p p p
a a
q
x y z a a a
 
 ÷
= −
 ÷
+ + − − +
 

Đỗ Dabh Phượng 1991-20109151
Lớp: CN-CĐT2-K55 Trang 8
ROBOTICS Tiểu Luận
( )
( )
( )
( )
( )
3 2 3 3 2 3 2 2
3 23 2 2
1
1
3 23 2 2 1
3 2 3 3 2 3 2 2 1
3 3 2 2 3 3 2
1
3 3 2 3 3 2 2 1
3 3 2 3 3 1 1
2
2 2
3 2 3 2 3 1
1
2
( ) ( )
( 2 )
(
p
p
p
p

p
p
p p
p
y
y
a C C a S S a C
a C a C
S
S
a S a S z a
a S C a C S a S z a
y
a C a C a S S
S
a S C a C a S z a
a C a y a S S z a
C
a a a a C S
S z
S


− + =
+ =


<=>
 
 

+ = −
+ + = −



+ − =

<=>


+ + = −

+ + −
=
+ +
<=>
=
1 1 3 3 2 3 3
2
1 3 3 2 3 3
3 3 2 3 3 1 1
2 2
3 2 3 2 3 1
1 1 3 3 2 3 3
2
3 3 2 3 3 1 1
( )( )
tan
)( )
( ) ( )

( 2 )
( )( )
arctan
( ) ( )
p
p p
p
p p
S z a a C a a S
q
a a C a a S
a C a y a S S z a
a a a a C S
S z a a C a a S
q
a C a y a S S z a


− + −

=> =

− + −
+ + −


+ +

− + −
 

 ÷
=> =
 ÷
+ + −
 ÷
 
Chương 2. Qũy đạo trong không gian khớp
. Quỹ đạo trong không gian khớp
Ta chọn 2 điểm A
( )
0 0 0
, ,x y z
,B
( )
, ,
i i i
x y z
bất kì trong không gian làm việc, Từ
phương trình động học ngược ta tính ở phần trên ta xác định được góc khớp tại hai
điểm A,B là A(
( )
, ,
ia ia ia
q q q
), B
( )
, ,
ib ib ib
q q q
.

Từ hai cặp góc khớp đã tìm được đó ta đi thiết kế quỹ đạp q
i
(i=1:2) biến thiên
từ q
ia
đến q
ib
theo quỹ đạo bậc 3 ( đáp ứng về mặt tốc độ góc khớp ) trong vòng
t
c
(s). Như vậy:
2 3
1 2 3
( )
i oi i i i
q t a a t a t a t
= + + +

(3.1)
Đỗ Dabh Phượng 1991-20109151
Lớp: CN-CĐT2-K55 Trang 9
ROBOTICS Tiểu Luận
Với các điều kiện như sau:
(0)
(0) 0
( )
( ) 0
i ia
i ia
i c ib

i c ib
q q
q q
q t q
q t q
=


= =


=


= =

& &
& &
(thông thường ta yêu cầu vận
tốc góc khớp tại điểm đầu và điểm cuối bằng 0 ). Từ các điều kiện trên ta thay vào
(3.1) giải ta được các hệ số
0
1
2
2
3
3
0
3( )
2( )

i ia
i
ib ia
i
c
ib ia
i
c
a q
a
q q
a
t
q q
a
t
=


=


−
=



− −
=



Ví dụ : Robot di chuyển A(0.51,0.295,1.07) đến B(0.0125,0.0216,1.236)trong
5(s) từ phương trình động học ngược =>
0
1
30
a
q
=
,
0
1
30
b
q
=

0
1
30
c
q
=
và
0
2
60
a
q
=

,
0
2
60
b
q
=
0
2
60
c
q
=

0
1
2
3
30
0
3.6
0.48
i
i
i
i
a
a
a
a

=


=


=


=−

và

2 3
2
30 3.6 0.48
7.2 1.44
7.2 2.88
i
i
i
q t t
q t t
q t
= + −
= −
= −
g
gg
Đỗ Dabh Phượng 1991-20109151

Lớp: CN-CĐT2-K55 Trang 10
ROBOTICS Tiểu Luận
i=1,2,3
vẽ đồ thị ham qi
2. Quỹ đạo trong không gian làm việc
Để phục vụ cho tín hiệu vào cho bộ điều khiển trong không gian làm việc nên
chỉ xét quỹ đạo di chuyển của robot RR giữa hai điểm A(x
0
,y
0
), B(x
c
,y
c
) là đường
thẳng và đường tròn ( ta thiết kế ở đây đó là đường tròn nhận AB làm đường kính)
a.Quỹ đạo của điểm tác động cuối theo đường thẳng từ A đến B trong t
c
(s)
Ta có phương trình đường thẳng trong không gian làm là mặt phẳng giữa hai
điểm A(x
0
,y
0,
), B(x
c
,y
c
)
0 0 0

0 0 0c c c
x x y y z z
x x y y x x
− − −
= =
− − −
với (x,y) là tọa độ điểm tác
động cuối =>;
0 0 0
0 0
0 0 0
0 0
( ) ( ) ;
( )
c c c
c c
c c c
c c
y y y x y x
y t x t
x x x x
z z z x z x
z x t
x x x x
− −
= +
− −
− −
= +
− −

2 3
0.5 0.075 0.025x t t
= + −

Cũng như cách thiết lập quỹ đạo góc khớp ta để thỏa mãn điều kiện về vận tốc
đầu và cuối ta thiết lập quan hệ x=x(t) là đa thức bậc 3.
Đỗ Dabh Phượng 1991-20109151
Lớp: CN-CĐT2-K55 Trang 11
ROBOTICS Tiểu Luận
2 3
1 2 3
( )
o
x t a a t a t a t
= + + +

(3.2)
cũng các điều kiện
0 0
0
(0) ( , )
(0) 0
( ) ( , )
( ) 0
c c c
c c
s A x y
v v
s t B x y

v t v
=


= =


=


= =

mà quan hệ y=kx+b; z=ux+v => x=x(t) cũng
có điều kiện như quỹ đạo.
Vậy
0 0
0
(0) ( , )
(0) 0
( ) ( , )
( ) 0
c c c
c c
s A x y
v v
s t B x y
v t v
=



= =


=


= =

Thay vào (3.2) vx(t) ta rút ra các hệ số :
0 0
1
0
2
2
0
3
3
0
3( )
2( )
c
c
c
c
a x
a
x x
a
t
x x

a
t
=


=


−
=



− −
=


khi thiết kế x=x(t) =>
0 0 0
0 0
0 0 0
0 0
( ) ( ) ;
( )
c c c
c c
c c c
c c
y y y x y x
y t x t

x x x x
z z z x z x
z x t
x x x x
− −
= +
− −
− −
= +
− −
Ví dụ : Cần thiết kế di chuyển theo đường thẳng từ điểm A(0.5,0.1,0.5) đến
B(0.6,0.2,0.8) trong 2(s) từ các công thức ở trên ta tính được :
2 3
0.5 0.075 0.025x t t
= + −
y =0.1
2 3
0.075 0.025t t
+ −
z =0.5
2 3
0.225 0.075t t
+ −
và quan hệ y=x-0.4 ; z=3x-1
phần do thi
Đỗ Dabh Phượng 1991-20109151
Lớp: CN-CĐT2-K55 Trang 12
ROBOTICS Tiểu Luận
Đỗ Dabh Phượng 1991-20109151
Lớp: CN-CĐT2-K55 Trang 13

ROBOTICS Tiểu Luận
Chương 3: Khảo sát tĩnh học
Cho lực tác dụng vào khâu thao tác tại điểm E gồm vecto lực:
[F ,F ,F ]
T
x y z
F
=
và momen
[M ,M ,M ]
T
x y z
M
=
.
Tính lực momen dẫn động tại các khớp đảm bảo robot cân bằng tĩnh (bỏ qua ma sát)
Tính toán ma trận r[i] trong hệ tọa độ cơ sở:



3.1.Tính toán các ma trận sóng trong hệ tọa độ cơ sở:
Ma trận sóng của vecto r[ i] trong hệ tọa độ cơ sở
Đỗ Dabh Phượng 1991-20109151
Lớp: CN-CĐT2-K55 Trang 14
ROBOTICS Tiểu Luận

-Tính momen ở các khớp RB
Đỗ Dabh Phượng 1991-20109151
Lớp: CN-CĐT2-K55 Trang 15
ROBOTICS Tiểu Luận

Đỗ Dabh Phượng 1991-20109151
Lớp: CN-CĐT2-K55 Trang 16
ROBOTICS Tiểu Luận
Đỗ Dabh Phượng 1991-20109151
Lớp: CN-CĐT2-K55 Trang 17
ROBOTICS Tiểu Luận
Chương 4 khảo sát động lực học
Động năng của robot có dạng:
2.1. Tính toán động năng:
Đỗ Dabh Phượng 1991-20109151
Lớp: CN-CĐT2-K55 Trang 18
ROBOTICS Tiểu Luận
2.1.1.Tính toán
i
vc

Đỗ Dabh Phượng 1991-20109151
Lớp: CN-CĐT2-K55 Trang 19
ROBOTICS Tiểu Luận
a.Tính toán vận tốc góc trong hệ tọa độ gắn với khâu [i]:
Vận tốc góc các khâu:

Đỗ Dabh Phượng 1991-20109151
Lớp: CN-CĐT2-K55 Trang 20
ROBOTICS Tiểu Luận
a.Tính toán vận tốc góc trong hệ tọa độ gắn với khâu [i]:
Đỗ Dabh Phượng 1991-20109151
Lớp: CN-CĐT2-K55 Trang 21
ROBOTICS Tiểu Luận

b.Biểu thức động năng của robot:
Thế năng của robot
1 1 2 1 2 2 3 1 2 2 3 23
( ) ( )
c c c
m gl m g d l S m g d l S l S
π
= + + + + +
Đỗ Dabh Phượng 1991-20109151
Lớp: CN-CĐT2-K55 Trang 22
ROBOTICS Tiểu Luận
Từ đó suy ra :
Tính toán chương trình con thiết lập đạo hàm của biểu thức theo biến
Thiết lập hệ phương trình động lực học:
Đỗ Dabh Phượng 1991-20109151
Lớp: CN-CĐT2-K55 Trang 23
ROBOTICS Tiểu Luận
Đỗ Dabh Phượng 1991-20109151
Lớp: CN-CĐT2-K55 Trang 24
ROBOTICS Tiểu Luận
Đỗ Dabh Phượng 1991-20109151
Lớp: CN-CĐT2-K55 Trang 25