GVHD: PGS.TS PHAN BÙI KHÔI Bài Tập Lớn Môn :Robotics
BÀI TẬP LỚN: ROBOTICS
PHẦN I: XÂY DỰNG CẤU TRÚC VÀ THIẾT LẬP HỆ PHƯƠNG TRÌNH
ĐỘNG HỌC CỦA ROBOT
1.xây dựng cấu trúc:
Cấu trúc 3D:
SVTH: NGUYỄN QUANG TRẠCH Trang1
GVHD: PGS.TS PHAN BÙI KHÔI Bài Tập Lớn Môn :Robotics
2. thiết lập hệ phương trình động học của robot:
Bảng tham số DH:
θ
i
di ai αi
1
q
h l
1
0
2
q
0 l
2
π
3 0
q
0 0
4
÷
−
−
SVTH: NGUYỄN QUANG TRẠCH Trang2
GVHD: PGS.TS PHAN BÙI KHÔI Bài Tập Lớn Môn :Robotics
Quy ước viết tắt:
i i i i
C S C S
θ θ θ θ θ θ
= = = + = +
Các ma trận vị trí và hướng của khâu của khâu thứ i so với khâu thứ i-1:
C l C
S l S
S
C
A
h
=
−
A
d
÷
÷
=
÷
÷
C
A
h
÷
÷
÷
÷
÷
−
=
S l C l C
C l S l S
C
S
A A A
h
− +
÷
+
÷
=
÷
+
÷
÷
−
=
− −
C C S S C S S C
S C C S C C S S
A
l C l C
l S l C
h q l
+ − +
÷
Ta có
E
T
=
A
suy ra:
x
y
z
x
y
z
x
y
z
x
=
=
= −
= +
= +
= − −
⇒
x
y
z
x
z
y
x
y
z
p l C l C
p l S l C
p h q l
= +
= +
= − −
• Hướng của khâu thao tác có ma trận cosin chỉ hướng:
SVTH: NGUYỄN QUANG TRẠCH Trang4
GVHD: PGS.TS PHAN BÙI KHÔI Bài Tập Lớn Môn :Robotics
C C S S C S S C
R S C C S C C S S
i A
c
i
i B
c c c
i i i i
c c c
i i i
i
i
i A
c
i
i B
a a t a t a t
a t
q t q a
q t q
q t q a
q t a t a tq
+ +
a
q q
a
t
q q
a
t
=
=
−
⇔
=
− −
=
Giải hệ ta được các hệ số của phương trình quỹ đạo.
Bài toán cụ thể:
cho điểm A(700; 0; 240) và điểm B(175; 153.11; 140). Thời gian chuyển động
o
o
o
và
B
B
B
B
q
q
q
q
=
EE
d
v r
dt
=
Px x
Py y
Pz z
d
v P l q S l q q S
dt
d
v P l q C l q q S
dt
d
v P q
dt
= = − − +
= = − +
S
R C
C
S
−
÷
÷
÷
=
−
C
R S
S
C
÷
R R
ω
=
&
%
T
q
R R q
q
ω ω
÷
÷
÷
÷
÷
÷
−
= = ⇒ =
&
= = ⇒ =
& &
&
%
& &
& &
T
q q
R R q q
q q
ω ω
−
÷
÷
+
÷
÷
÷
÷
+
>
>
>
!"!#$
>
>
>
Đồ thị xp theo thời gian:
>
Đồ thị yp theo thời gian:
SVTH: NGUYỄN QUANG TRẠCH Trang8
GVHD: PGS.TS PHAN BÙI KHÔI Bài Tập Lớn Môn :Robotics
>
Đồ thị zp theo thời gian:
>
SVTH: NGUYỄN QUANG TRẠCH Trang9
GVHD: PGS.TS PHAN BÙI KHÔI Bài Tập Lớn Môn :Robotics
%!& $
SVTH: NGUYỄN QUANG TRẠCH Trang10
GVHD: PGS.TS PHAN BÙI KHÔI Bài Tập Lớn Môn :Robotics
2.bài toán động học ngược
a.xây dựng quy luật chuyển động của khâu cuối:
điểm tác động cuối chuyển động theo đường xoắn ốc:
xp t
yp t
zp t
π
− + +
÷
÷
÷
÷
• tính q3:
ta có:
z
p h q l
= − −
suy ra:
z
q h p l
= − −
• tính q2:
x
y
p l C l C
p l S l C
= +
= +
=
• Tính q1:
Cũng từ hệ:
x
y
p l C l C
p l S l C
= +
= +
ta có:
x
y
p l l C C l S S
p l S C l l C S
= + +
= + +
+ −
=
+
Suy ra :
' q a S C
=
• Tính q4 :
Ta có :
x
n C C S S C
ϕ
= + = =
Suy ra :
q q q
ϕ
= − −
c.ứng dụng phần mềm Matlap, Maple để tính toán và mô phỏng số :
PHẦN III : TÍNH TOÁN TĨNH HỌC
1.tính lực và momen dẫn động tại các khớp
• ()!*+$
,-./!0!1
' '
=
i
i i
i
r R r
=
i
i ci
ci
r R r
=
Nhn xt:
=> ?@%!@-A!+BC%D!./.!"7/%!@-!1$
EFBC BG%!0%
'
T
i
i i i
F
z
τ
− −
=
=> ?@%!@-H*"%D!./I%!@-!1
SVTH: NGUYỄN QUANG TRẠCH Trang12
GVHD: PGS.TS PHAN BÙI KHÔI Bài Tập Lớn Môn :Robotics
'
+
$S"./BP%!&!1Q4R%!&'I
i
O
'
' 'i i i i
M
M
+ +
= −
• D!7T
i
i
r
?G
i
r
$
l
h
r
÷
=
÷
C l
S l
r
÷
=
÷
÷
q
r
÷
=
÷
÷
l
r
÷
=
÷
÷
−
• D!7T
i
ci
r
?G
ci
r
$
c
c
l
c
c
l
r
−
÷
=
÷
÷
c
c
c
l C
l S
r
−
÷
= −
÷
÷
=
÷
÷
c
c
l
r
÷
=
÷
÷
−
c
c
l
−
÷
= −
÷
÷
−
%
S l
C l
S l C l
r
÷
= −
÷
÷
−
%
l
l
r
÷
= −
÷
÷
%
c
c
c
l C
l C
r
÷
=
÷
÷
−
c
c
c
l
lr
−
÷
=
÷
÷
%
c
c
c
l
l
=
÷
÷
−
P
m g
÷
=
÷
÷
−
P
m g
÷
=
÷
÷
−
SVTH: NGUYỄN QUANG TRẠCH Trang14
GVHD: PGS.TS PHAN BÙI KHÔI Bài Tập Lớn Môn :Robotics
• Tính momen phản lực ở các khớp
>
7U$
'
V ' WXY
i i
F i i
+
+
'
V'QWXS
i i
M
+
VWXZ
i
P
V WX 7
i
sr i
%
V WX 7
GVHD: PGS.TS PHAN BÙI KHÔI Bài Tập Lớn Môn :Robotics
n
T T T
i Ti Ti Ri i i i Ri
i
M q m J J J R I R J
=
= +
∑
:ma trận khối lượng suy rộng của robot
'
T
n n
M q M q
C q q I q q I
q q
∂
= × − ×
÷
∂ ∂
& & &
: là ma trận coriolis
π
: là thế năng của robot
x
I
y
I
z
I
0 0 0
3 0 0
C
l−
m
x
I
y
I
z
I
0 0 0
4 0 0
C
l
−
m
x
I
y
+
÷
= +
÷
÷
c
C c
c
l C l C
r l S l S
h q l
+
÷
= +
÷
÷
− +
∂
÷
=
÷
∂
÷
c c
C
T c c
l S l S l S
r
J l C l C l C
q
− − −
∂
÷
= +
÷
∂
÷
c c
C
T c c
l S l S l S
r
J l C l C l C
q
− − −
∂
÷
= +
÷
∂
÷
b.ma trận Jacobi quay của các khâu :
R
J
ω
θ
∂
R
J
q
ω
∂
÷
=
÷
∂
÷
&
R
J
q
ω
∂
÷
=
÷
∂
÷
7U$
C
z z z z C C
z z z C C
z z z C
z z z
m I I I I m l m l l l l C m m l l l l C
m I I I m l l l C m m l l l C
m m m m m m
m I I I m l m m l
m I I I m
= + + + + + + + + + + +
c c c c
c c c c
M q M q
C q q I q q I
c c c c
q q
c c c c
÷
∂ ∂
÷
= − =
÷
÷
∂ ∂
÷
& & &
Trong đó :
c c
m gL m gL m g h q l m g h l q l
Π = + + − + + − − +
Từ đó suy ra :
g
m m g
q
∂Π
= =
÷
+
∂
7.phương trình vi phân chuyển động của các khâu
Thế các biểu thức vào phương trình Lagrange loại hai :
' M q q C q q q g q
τ
+ + =
GVHD: PGS.TS PHAN BÙI KHÔI Bài Tập Lớn Môn :Robotics
V W
z z z C C
z z z C z C
I I I m l l l C m m l l l C q
I I I m l m m l q I q m l m m l l S q
τ
+ + + + + + +
+ + + + + + + + + + =
&&
&& && &
Khâu 3:
m m q m m g
τ
+ + + =
&&
Khâu 4:
z z
I q I q
τ
+ =
&& &&
T T
L p
V K q Hq
ε ε
= +
& &
;G
L
V
4!AgcB8aN!6! 74!G!-!`
T
p
K
ε ε
h
B6cB8a`?G?G!G!-!`
T
q Hq
& &
BGcN74
p
K
K
ε
&
Q
T
q
&&
;
T
q
&
Q
T
q
&
H
g
q
&
Q
T
q
&
;
T
q
&
;
T
q
&
Q
T
q
&
;
q
&&
SVTH: NGUYỄN QUANG TRẠCH Trang21
GVHD: PGS.TS PHAN BÙI KHÔI Bài Tập Lớn Môn :Robotics
!*-!897<!BC!.IgH?G-!897<!'?@
E!+%!0U!G!-!`0"7BC
p
G
'!T8a-!897<!
$
L
V
g
X
X
T
q
&
D
K
ε
&
=
'
T T
P D
q C q q q q H q qK K+
g
& & & & &
X=
T
q
&
D
K
ε
&
Q
T
K
q
&
≤
mkn!1!!k*7]!6! gA!*6
Quỹ đạo chuyển động của các khâu:
Khâu 1:
q t t t= −
Khâu 2:
q t t t= −
Khâu 3:
q t t t= −
SVTH: NGUYỄN QUANG TRẠCH Trang22
GVHD: PGS.TS PHAN BÙI KHÔI Bài Tập Lớn Môn :Robotics
Khâu 4:không chuyển động.
2.Các thao tác thực hiện mô phỏng
Các hệ số khuếch đại tỉ lệ và đạo hàm được lựa chọn như sau :
P
K
hiệu đầu ra là giá trị của hàm
Mô hình bộ điều khiển PD :
3.kết quả mô phỏng
Sau khi mô phỏng ta được kết quả là đồ thị của các đường đặc tính của các
biến:
SVTH: NGUYỄN QUANG TRẠCH Trang24
GVHD: PGS.TS PHAN BÙI KHÔI Bài Tập Lớn Môn :Robotics
Mô men của các khớp theo thời gian
Vị trí các khớp theo thời gian
SVTH: NGUYỄN QUANG TRẠCH Trang25
Copyright: Tài liệu đại học ©