Thiết kế mô hình robot hai bậc tự do

Thiết kế mô hình robot hai bậc tự do
Mục Lục
Giới thiệu: 2
Mô tả toán học 3
H ph ng trình ng l c h c Lagrangeệ ươ độ ự ọ 3
Tính toán Motor: 6
Tính toán b i u khi n tr tộ đ ề ể ượ 6
Thiết kế trên Simulink: 7
Kết luận: 10
SVTH: Nguyễn Mạnh Hoàng Trang 1
Thiết kế mô hình robot hai bậc tự do
THIẾT KẾ MÔ HÌNH ROBOT HAI BẬC TỰ DO
SỬ DỤNG PHƯƠNG PHÁP ĐIỀU KHIỂN
TRƯỢT BẰNG SIMULINK
Giới thiệu:
Với những mục đích thiết kế và điều khiển, cần thiết phải có một mô hình
toán học mô tả động lực học của hệ thống. Vì thế, ở chương này ta sẽ xác lập
phương trình chuyển động của tay máy dưới dạng phương trình vi phân. Phương
pháp áp dụng ở đây là xây dựng phương trình chuyển động của cơ hệ dựa trên
quan hệ năng lượng, xuất phát từ nguyên lý bảo toàn và chuyển hóa năng lượng
trên cơ sở xác lập quan hệ giữa động năng và thế năng của cơ hệ tay máy, sau đó
sử dụng phương trình vi phân của chuyển động trên cơ hệ với các đại lượng tham
gia vào phương trình gồm lực, quán tính và năng lượng. Việc nghiên cứu động
lực học Robot thường giải quyết hai nhiệm vụ sau :
1. Xác định momen và lực động trong quá trình chuyển động. Khi đó qui
luật biến đổi của biến khớp qi(t) xem như đã biết. Việc tính toán lực cũng như
momen trong cơ cấu tay máy là nhiệm vụ tất yếu trong việc lựa chọn công suất
động cơ, tính toán kiểm tra độ bền, độ cứngnnvững, đảm bảo độ tin cậy cho
Robot.
2. Xác định các sai số động, tức là sai số xuất hiện so với qui luật chuyển
động trong chương trình.

Có nhiều phương pháp nghiên cứu động lực học Robot, nhưng nhiều hơn cả là
phương pháp cơ học Lagrange, cụ thể là phương trình Lagrange-Euler.
SVTH: Nguyễn Mạnh Hoàng Trang 2
Thiết kế mô hình robot hai bậc tự do
Mô tả toán học
Hệ phương trình động lực học Lagrange
Sơ đồ cấu trúc động học và các tham số của tay máy hai bậc tự do được vẽ trên
hình 1:
Hình 1. Cấu trúc động học của tay máy hai khâu hai bậc tự do
Trong đó:
θ
1
, θ
2
Góc quay của từng khâu
l
1
, l
2
Chiều dài của hai khâu
l
g1
, l
g2
Chiều dài từ trục quay đến trọng tâm của từng khâu
m
1
, m
2
Khối lượng toàn bộ của mỗi khâu

Tọa độ:
Lấy đạo hàm:

Tính động năng:
Tính Thế Năng:
SVTH: Nguyễn Mạnh Hoàng Trang 3
Y
l
1
l
g1
X
l
g2
l
2
θ
1
m
1
, J
1
m
2
, J
2
θ
2
m
t

, J
t
Thiết kế mô hình robot hai bậc tự do
Hàm Lagrange : L = K
1
+ K
2
– P
1
– P
2
L= [ m
1
] + [ )cos + –[mg
)]
• =
•
• =
• =
• =
• ( =
Tính Momen:
Đặt:
SVTH: Nguyễn Mạnh Hoàng Trang 4
Thiết kế mô hình robot hai bậc tự do
Tìm: M(q) và N(q, ).
Ta có:

Hệ phương trình động lực học Lagrange của tay máy hai khâu có cấu trúc như trên hình
1 được viết dưới dạng ma trận sau [2]:







+














+













=






•
•
2
1
2
1
2221
1211
2
1
2221
1211
2
1
g
g
nn
nn
mm

mm
F
F
θ
θ
θ
θ


(1)
trong đó F
1
và F
2
lần lượt là các mômen điều khiển tác động lên khâu 1 và khâu 2.
Các thành phần của ma trận được tính theo các biểu thức dưới đây:
2212
2
22
2
12111
2.)( Cllmlmlmmm
ggg
+++=
2
22221212
lmCllmm
g
+=
2

22221221
lmCllmm +=
2
2222 g
lmm =
2.2.2
21211
CllmN
gg
θ
−=
212
.2
θ
SN −=
112221
.2.2
θθ
SmmSN −−=
2
121222
SllmN
θ
=
(2)
)()(
122211211
CglmCglmmg
g
++=

12222
Cglmg
g
=
C
i
và S
i
lần lượt là các ký hiệu của cosθ
i
và sinθ
i
(i = 1, 2) và C
12
là ký hiệu của cos(θ
1
+
θ
2
).
SVTH: Nguyễn Mạnh Hoàng Trang 5
Thiết kế mô hình robot hai bậc tự do
Tính toán Motor:
Dùng động cơ DC cho các khớp :
Phương trình :
Tính toán bộ điều khiển trượt
Qúa trình tính toán cho bộ điều khiển trượt là
Nguyên tắc mặt trượt s(x)=0
Bậc của s<bậc của đối tượng
Sai lệch giữa quỹ đạo nhận được và quỹ đạo đặt là

q
e
=q
d
-q
Chọn mặt trượt
S=-q
Chọn mặt trượt
S=q
’
e
+
λ
q
’
e
Q=

















qn
q
q
.
.
2
1
;q
d
=
















qdn

qd
qd
.
.
2
1
với hàm qd chúng ta đã biết
Q
e
=
















qen
qe
qe
.

.
2
1
;












=
nnn
n
n
λλ
λλλ
λλλ
λ
1

2 2221
1 1211
;S=

















sn
s
s
.
.
2
1
Để đơn giản ta lấy



=
>
hangsoij
ij

λ
λ
0
Ta có s
’
=q
’’
e
+
λ
q
’
e
Ta có q
e
=q
d
-q=>q=q
d
-q
e
M(q’’
e
-q’’
e
)+N(q’
d
-q’
e
)+G=F

=> F=Mq’’
d
-Nq’
d
-(Mq’’
e
+Nq’
e
)+G đặt Q
d
= Mq’’
d
-Nq’
d
=> q’’
e
=-M
-1
[F+Nq’
e
-G-Q
d
]
=> s’=-M
-1
[F+Nq’
e
-G-Q
d
]+

λ
q’
e
=> s’=-M
-1
[F+(N+
λ
)q’
e
-G-Q
d
]
Điều khiển trượt s’
T
.s<0
[M
-1
[F+(N+
λ
)q’
e
-G-Q
d
]]
T
.s>0 (*)
Vấn đề tìm luật điều khiển F để (*) thỏa mãn
[M
-1
[F+(N+

λ
)q’
e
-G-Q
d
]]
T
.s=n(**) với n tùy ý lớn hơn 0
SVTH: Nguyễn Mạnh Hoàng Trang 6
Thiết kế mô hình robot hai bậc tự do



























=














=
)(

)2(
)1(
0
0
0 20
0 1

)(

)2(2
)1(1
snsign
ssign
ssign
sn
s
s
snsignsn
ssigns
ssigns
s
Từ (**) ta có được
M
-1
[F+(N+
λ
)]q’
e
-G-Q
d
=ks với k là ma trận vuông n hàng,n cột
Từ đây ta tính được giá trị của F là
F=M.ks+G-(N+M.
λ
)q’
e
+Q

d
Với M.k.s=M.k.
)(ssigns
 F= M.k.
)(ssigns
+ G-(N+M.
λ
)q’
e
+Q
d
Từ biểu thức tính được ta thay vào bộ điều khiển trượt để tính toán giá trị đầu ra






=
2
1
G
G
G
;Q
d
=M.q’’
d
+N.q
d

=






2
1
Qd
Qd






=
2221
1211
mm
mm
M
;







=
2221
1211
nn
nn
N
Tương tự ta tính được giá trị của biểu thức
(N+
λ
)q’
e
với ma trận N,G,M đã có ở trên và






=
2221
1211
λλ
λλ
λ
Gía trị của biểu thức
M.s(q) cũng được xác định tương tự với s(q)=q’
e
+
λ
q

e
Thiết kế trên Simulink:
Mô hình tổng quát:
Mô hình tổng thể điều khiển trượt
SVTH: Nguyễn Mạnh Hoàng Trang 7
Thiết kế mô hình robot hai bậc tự do
Mô hình tổng thể của khối motor chúng ta mô phỏng dựa trên phương trình rút ra được từ
phương trình của động cơ điện 1 chiều
SVTH: Nguyễn Mạnh Hoàng Trang 8
Thiết kế mô hình robot hai bậc tự do
SVTH: Nguyễn Mạnh Hoàng Trang 9
Thiết kế mô hình robot hai bậc tự do
Khối tính toán của robot
Kết luận:
- Bằng việc mô hình hóa hệ thống bằng simulink ta đã mô phỏng được một hệ thống
điểu khiển robot hai bậc tự do sử dụng phương pháp trượt. Mặc dù đã cố gắng
nhưng vẫn còn nhiều khiếm khuyết và chương trình vẫn chưa thế nào chạy được.
SVTH: Nguyễn Mạnh Hoàng Trang 10