Bài tập lớn : Xây dựng cấu trúc và thiết lập hệ phương trình động học của robot,
Bạn đang xem bản rút gọn của tài liệu. Xem và tải ngay bản đầy đủ của tài liệu tại đây (1.36 MB, 25 trang )
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
q
l
0 0
Ma trận DH của khâu i so với khâu thứ i-1 có dạng:
i i i i i i i
i i i i i i i
i
i
i i i
a
a
A
d
θ θ α θ α θ
θ θ α θ α θ
α α
−
÷
÷
=
÷
÷
−
−
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
÷
÷
÷
÷
÷
−
=
S l C
C l S
C
S
A
÷
−
÷
÷
÷
÷
=
−
A
d
÷
÷
=
÷
÷
C
C
l
S
S
A
÷
÷
÷
÷
÷
−
=
Các ma trận vị trí và hướng của khâu thứ i so với khâu thứ 0:
C l C
S l S
S
C
A
h
÷
÷
÷
÷
÷
−
=
S l C l C
C l S l S
C
S
A A A
h
− +
÷
+
÷
=
÷
÷
÷
=
−
C l C l C
S l S l C
h
S
C
A A A A
q
+
÷
+
÷
=
÷
+
÷
÷
−
=
− −
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
+ − +
÷
− − −
÷
÷
÷
÷
+
+
=
− − −
Hệ phương trình động học robot:
SVTH: NGUYỄN QUANG TRẠCH Trang3
GVHD: PGS.TS PHAN BÙI KHÔI Bài Tập Lớn Môn :Robotics
Điểm tác động cuối được mô tả bởi ma trận:
x x x x
y y y y
E
z z y z
n s a p
n s a p
T
n s a p
÷
÷
=
÷
÷
Ta có
E
T
=
A
suy ra:
x
y
z
x
y
z
x
y
z
x
y
z
n C C S S
n S C C S
n
s C S S C
s C C S S
s
a
a
a
p l C l C
p l S l C
p h q l
= +
= −
=
= − +
= − −
=
=
=
= −
= +
= +
= − −
⇒
x
y
z
x
z
y
f p l C l C
f p l S l C
f p h q l
f n C C S S
f s
f a
= − +
= − +
= − − −
= − +
=
=
Với:h=340(mm); l1=l2=350(mm); l4=100(mm)
PHẦN II:GIẢI BÀI TOÁN ĐỘNG HỌC
1.bài toán động học thuận:
Các thông số đầu vào :q1, q2, q3,q4
Thông số cần xác định: điểm tác động cuối P=( Px, Py,Pz),hướng của khâu
cuối
• Tọa độ điểm tác động cuối:
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
+ − +
÷
− − −
÷
÷
=
−
a.xây dựng quy luật chuyển động của các khâu của robot:
giả sử robot chuyển từ vị trí ban đầu (A) tới vị trí cuối (B) trong thời gian
.quỹ đạo của khớp robot theo đa thức bậc 3:
(t)=
giả thiết vận tốc tại điểm đầu và điểm cuối bằng 0: ;
Ta có hệ phương trình:
i
i
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
+ +
= = =
= =
= =
+
+
=
= = =+
& &
& &
i
i A
i
i B i A
i
i B i A
i
a q
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
Từ bài toán ngược ta xác định được các thong số biến khớp:
A
A
A
A
q
q
q
q
=
=
=
=
o
o
o
và
B
B
B
B
q
q
q
q
=
=
=
=
o
o
o
Quỹ đạo chuyển động của các khâu:
Khâu 1:
SVTH: NGUYỄN QUANG TRẠCH Trang5
GVHD: PGS.TS PHAN BÙI KHÔI Bài Tập Lớn Môn :Robotics
Với các hệ số:0 0 7.2 ta có:
q t t t= −
Khâu 2:
Với:0
q t t t= −
Khâu 3:
Với:0
q t t t= −
Khâu 4:không chuyển động.
b.tính vận tốc điểm tác động cuối và vận tốc góc của khâu thao tác:
• Vận tốc của điểm tác động cuối:
R
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
= = − − +
= = − +
= = −
& & &
& & &
&
• Vận tốc góc của các khâu:
Ma trận cosin chỉ hướng của các khâu so với R0:
C
R S
S
C
÷
÷
÷
−
=
S
R C
C
S
−
÷
÷
÷
=
−
C
R S
S
C
÷
=
÷
÷
−
−
SVTH: NGUYỄN QUANG TRẠCH Trang6
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
+ − +
÷
− − −
÷
÷
=
−
Ta có:
T
i i i
R R
ω
=
&
%
T
q
R R q
q
ω ω
÷
÷
÷
÷
÷
÷
−
= = ⇒ =
&
&
%
&
&
T
q q
R R q q
q q
ω ω
−
÷
÷
+
÷
÷
÷
÷
+
−
= = ⇒ =
& &
&
%
& &
& &
T
q q
R R q q
q q
ω ω
−
÷
÷
+
÷
÷
÷
÷
+
−
= = ⇒ =
& &
&
%
& &
& &
• ứng dụng phần mềm Maple vẽ quỹ đạo của điểm tác động cuối
Khởi động chương trình và khai báo thư viện sử dụng
>
>
>
>
Khai báo ma trận Denavit-Hartenbergtổng quát
>
Tính ma trận Denavit-Hartenbergcác khâu
Thế bảng thông số DH cho mỗi khâu vào ma trận H tổng
quát
>
>
SVTH: NGUYỄN QUANG TRẠCH Trang7
GVHD: PGS.TS PHAN BÙI KHÔI Bài Tập Lớn Môn :Robotics
>
>
>
>
>
>
>
>
>
>
!"!#$
>
>
>
Đồ 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
π
π
=
=
=
b.giải bài toán động học ngược:
chọn góc cardan làm góc định hướng cho khâu thao tác:
ma trận cardan:
px
py
Cd
pz
β η α β β
α β η α η α β η α η α β
α β η α η α β η α η α β
−
+ − + −
=
− + +
÷
÷
÷
÷
• 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
= +
= +
Lấy tổng bình phương của hai vế:
x y
x y
x y
p p l l l l C C S S
p p l l l l C
p p l l
C
l l
+ = + + +
+ = + +
+ − −
=
S C= ± −
' q a S 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
= + +
= + +
SVTH: NGUYỄN QUANG TRẠCH Trang11
GVHD: PGS.TS PHAN BÙI KHÔI Bài Tập Lớn Môn :Robotics
Giải hệ ta được :
x y
x y
y x
x y
l p C p S
C
p p
l l C p l S p
S
p p
+
=
+
+ −
=
+
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 i i
i ci
i i i i i i
F P
M P
F
M r F r
− +
− + −
= −
= + × − ×
% %
2!3)$4.5!67/!"-!89-!-:;!<$
i
i
i
i i
i i
a
r d s
d c
α
α
=
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
'
T
i
i i i
M
z
τ
− −
=
T
i
z
−
$JK99?A!L.!"!M.89N7/%!@-!1
O&*$
'i i
F
+
$(C./BP%!&!1Q4R%!&I
i
O
'
' 'i i i i
F
F
+ +
= −
'i i
M
+
$S"./BP%!&!1Q4R%!&'I
i
O
'
' 'i i i i
M
M
+ +
= −
• D!7T
i
i
r
?G
i
r
$
l
h
r
÷
=
÷
÷
l C
l S
h
r
÷
=
÷
÷
l
r
÷
=
÷
÷
C l
S l
r
÷
=
÷
÷
q
r
÷
=
÷
÷
q
r
÷
=
÷
÷
−
l
r
÷
=
÷
÷
l
r
÷
=
÷
÷
−
• D!7T
i
ci
r
?G
ci
r
$
c
c
l
r
−
÷
=
÷
÷
c
c
l C
r
−
÷
=
÷
÷
c
c
l
r
−
÷
=
÷
÷
c
c
c
l C
l S
r
−
÷
= −
÷
÷
c
c
l
r
÷
=
÷
÷
−
c
c
l
r
÷
=
÷
÷
c
c
l
r
÷
=
÷
÷
−
c
c
l
r
÷
=
÷
÷
• D!7TU
i
r
%
ci
r
%
$
h l S
h l C
l S l C
r
−
÷
= −
÷
÷
−
%
S l
C l
S l C l
r
÷
= −
÷
÷
−
%
SVTH: NGUYỄN QUANG TRẠCH Trang13
GVHD: PGS.TS PHAN BÙI KHÔI Bài Tập Lớn Môn :Robotics
q
q
r
÷
=
÷
÷
−
%
l
l
r
÷
= −
÷
÷
%
c
c
c
l C
l C
r
÷
=
÷
÷
−
%
c
c
c c
c
l S
l C
l S l C
r
−
÷
=
÷
÷
−
%
c
c
c
l
lr
−
÷
=
÷
÷
%
c
c
c
l
l
r
−
÷
=
÷
÷
%
• Trọng lực:
P
m g
÷
=
÷
÷
−
P
m g
÷
=
÷
÷
−
P
m g
÷
=
÷
÷
−
P
m g
÷
=
÷
÷
−
• Ngoại lực tác dụng:
x
y
z
F
F F
F
÷
=
÷
÷
x
y
z
M
M M
M
÷
=
÷
÷
Sử dụng phần mền Maple nhân ra được:
• Tính phản lực ở các khớp
>
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
ci
src i
%
• [!4?K99?A!L.!"!M.89N7/%!@-!1
SVTH: NGUYỄN QUANG TRẠCH Trang15
GVHD: PGS.TS PHAN BÙI KHÔI Bài Tập Lớn Môn :Robotics
>
>
>
>
• D!BC.\R%!@-E4E74&4]^!
>
PHẦN IV: TÍNH TOÁN ĐỘNG LỰC HỌC
1.cơ sở lý thuyết:
Khi đó phương trình vi phân chuyển đông của các khâu :
'
T
M q q C q q q
q
π
τ
∂
+ + =
÷
∂
&& &&
Trong đó:
SVTH: NGUYỄN QUANG TRẠCH Trang16
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
2.Bảng tham số động lực học của robot
khâu
Vị trí trọng tâm
Khối
lượn
g
Ma trận mômen quán tính
1
C
l−
0
0
m
x
I
y
I
z
I
0 0 0
2
C
l
−
0 0
m
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
I
z
I
0 0 0
3.ma trận Jacobi của các khâu
Từ cấu tạo hình học của robot ta xác định được tọa độ khối tâm của các khâu
so với hệ tọa độ cố định :
C
C C
l C
r l S
h
÷
=
÷
÷
c
C c
l C l C
r l S l S
h
+
÷
= +
÷
÷
c
C c
c
l C l C
r l S l S
h q l
+
÷
= +
÷
÷
− +
c
C c
c
l C l C
r l S l S
h l q l
+
÷
= +
÷
÷
− − +
a.các ma trận Jacobi tịnh tiến của các khâu
C
C
T C
l S
r
J l C
q
−
∂
÷
=
÷
∂
÷
c c
C
T c c
l S l S l S
r
J l C l C l C
q
− − −
∂
÷
= +
÷
∂
÷
SVTH: NGUYỄN QUANG TRẠCH Trang17
GVHD: PGS.TS PHAN BÙI KHÔI Bài Tập Lớn Môn :Robotics
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
ω
∂
÷
=
÷
∂
÷
&
R
J
q
ω
∂
÷
=
÷
∂
÷
&
4.ma trận khối lượng suy rộng của robot:
n
T T T
i Ti Ti Ri i i i Ri
i
M q m J J J R I R J
=
= +
∑
_F./-!`M-B"!&78a%+HE$
m m m m
m m m m
M q
m m m m
m m m m
÷
÷
=
÷
÷
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
z
l l l C m m l l l C
m m m
m m m m m I
+ + + +
= +
= = = = =
5.Ma trận ly tâm và coriolis :
SVTH: NGUYỄN QUANG TRẠCH Trang18
GVHD: PGS.TS PHAN BÙI KHÔI Bài Tập Lớn Môn :Robotics
' b b
T
n n
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
C
C
c m l m l m l l S q
c m l m l m l l S q
c c c c c c c c c c c c
c m l m l m l l S q
c m l q q m m q q l l S
= − + +
= + +
= = = = = = = = = = = =
= + +
= − + + −
&
&
&
& & & &
6.Thế năng của robot
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
τ
+ + =
&& &&
Ta nhận được hệ phương trình vi phân chuyển động của robot scara :
Khâu 1 :
V W
V W
z z z z C C C
z z z C C
z C C
I I I I m l m l l l l C m m l l l l C q
I I I m l l l C m m l l l C q
I q m l m l m l l S q q m l m l m l l S q
τ
+ + + + + + + + + +
+ + + + + + + +
+ − + + − + + =
&&
&&
& && &
Khâu 2 :
SVTH: NGUYỄN QUANG TRẠCH Trang19
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
τ
+ =
&& &&
PHẦN V: CHỌN LUẬT ĐIỀU KHIỂN
1.Hệ thống điều khiển phản hồi trong không gian khớp
2!1cN!6! M%!BGI7"`!+7*M
%!@-74E4E%!@-744!"?A7Dd
Sử dụng cấu trúc bộ điều khiển có dạng tỉ lệ - đạo hàm (PD):
7U$
d
q q
ε
= −
$BG ?A7DN%!@-74
d
q q
ε
= −
&
& &
$BG N%!@-74
P
K
X.''e'$7T8#!K!6 %!+!If%!@-7P46
SVTH: NGUYỄN QUANG TRẠCH Trang20
GVHD: PGS.TS PHAN BÙI KHÔI Bài Tập Lớn Môn :Robotics
X.''e'$7T8#!K!6 I!Gf%!@-7P46
Hệ thống điều khiển với cấu trúc điều khiển trên ổn định tuyệt đối toàn cục.thực
vậy .chọn hàm Liapunov có dạng :
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
?G;
BG7T!6 .89P!G
L
V
i?@
j
d
q q
D!I!Gk-!G
L
V
'!T8a$
L
V
g
X
T
ε
&
T
ε
P
K
ε
Q
P
K
ε
&
Q
T
q
&&
;
T
q
&
Q
T
q
&
H
g
q
&
Q
T
q
&
;
q
&&
:D! l1N!G!-!`
T
ε
P
K
ε
'
T
q
&
;
q
&
'8a73.I$
L
V
g
X
T
ε
&
P
K
ε
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
T
ε
&
P
K
ε
Q
T
q
&
;
T
q
&
Q
T
q
&
VS=JH'
q
&
W
_F./!D!N-!897<!BC!?-./BTM%!
b'-!897<!8a4g!G!.I
L
V
g
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
q H C q−
g
& &
7U$
' ' V q q C q q q
=
& &&
:7T
H C−
g
BG7T l18a'
T
q H C q− =
g
?@H
'Pf!T8a$
L
V
g
X=
T
q
&
D
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
÷
=
÷
÷
D
K
÷
=
÷
÷
Ta có mô hình simulink của robot :
Trong đó :
, là tín hiệu quỹ đạo đặt và vận tốc đặt vào các khớp của robot đươc thiết lập
trong phần thiết lập quỹ đạo
q, v là các tín quỹ đọa và vận tốc phản hồi lại từ robot(quỹ đạo thực)
ở đây ta sử dụng các khối:
+khối Gain dùng để khếch đại các tín hiệu đầu vào
+khối mux chập các tín hiệu đơn thành các tín hiệu tổng hợp và tách thành
nhiều tín hiệu
+khối input, ouput đầu vào và ra của tín hiệu
SVTH: NGUYỄN QUANG TRẠCH Trang23
GVHD: PGS.TS PHAN BÙI KHÔI Bài Tập Lớn Môn :Robotics
+khối hàm biểu diễn một hàm toán học khi có tín hiệu đi vào là các biến và tín
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
