tổng hợp các mạch vòng điều chỉnh cho robot scara – serpent theo phương pháp điều khiển không gian khớp độc lập, và mô phỏng

LỜI NÓI ĐẦU
Trong những năm gần đây, công nghệ Robot đã đạt được những thành tựu
to lớn và ngày càng được ứng dụng rộng rãi trong công nghiệp, hoạt động trong
mọi lĩnh vực sản xuất như công nghiệp lắp ráp, hàn và những công việc trong
môi trường độc hại và nguy hiểm. Một trong những vấn đề quan trọng nhất của
Robot công nghiệp là sự chính xác trong chuyển động. Hầu hết các Robot đều
được truyền động bằng động cơ điện và chủ yếu là động cơ điện một chiều, do
đó việc điều khiển chuyển động của Robot chính là điều khiển các động cơ
truyền động cho Robot.
Robot Serpent là Robot thuộc họ Robot Scara là một trong những loại
Robot ứng dụng rộng rãi nhất trong công nghiệp. Trong đồ án này sẽ nghiên cứu
về hệ điều khiển quỹ đạo Robot Scara – Serpent thông qua điều khiển quỹ đạo
các động cơ truyền động cho nó.
Đồ án gồm 5 chương:
Chương I: Tổng quan về Robot công nghiệp, nội dung chính là động học
vị trí và điều khiển chuyển động của Robot.
Chương II: Tổng quan về Robot Scara – Serpent và thiết kế quỹ đạo.
Chương III: Thiết kế mạch lực và mạch điều khiển cho các động cơ
truyền động cho Robot Scara – Serpent.
Chương IV:Ttổng hợp các mạch vòng điều chỉnh cho Robot Scara –
Serpent theo phương pháp điều khiển không gian khớp độc lập, và mô phỏng.
Chương V: Lắp ráp thực nghiệm hệ điều khiển Robot, kiểm tra, hiệu
chỉnh lại các bộ điều chỉnh đã thiết kế.
Chương I. Tổng quan về Robot công nghiệp
Chương này trình bày khái niệm Robot và một số lý thuyết cơ bản để
thực hiện thiết kế và điều khiển quỹ đạo chuyển động của Robot công nghiệp,
bao gồm bốn phần chính là khái niệm về Robot, động học vị trí, lập quỹ đạo
chuyển động và hệ thống điều khiển chuyển động của Robot công nghiệp.
1.1. Giới thiệu chung về Robot công nghiệp
1.1.1 .Khái niệm Robot
Robot công nghiệp là một cơ cấu máy gồm các thanh cứng nối liên tiếp

với nhau bằng các khớp (quay hoặc tịnh tiến), có thể làm việc một cách tự
động nhờ sử dụng máy tính và vi xử lý để điều khiển. Nhìn chung Robot công
nghiệp gồm các hệ thống sau:
- Hệ thống cơ khí (hệ thống chuyển động)
- Hệ thống kích thích (hệ thống truyền động)
- Hệ thống nhận biết (cảm biển)
- Hệ thống điều khiển
a. Hệ thống cơ khí
Robot công nghiệp cấu tạo từ các thanh cứng nối với nhau bằng các
khớp. Một đầu được gắn cố định gọi là thân Robot (base), một đầu chuyển
động tự do gọi là end effector (tay) và được trang bị một dụng cụ (tool) để
thực hiện một nhiệm vụ cụ thể nào đó. Các khớp chuyển động làm cho các
thanh chuyển động dẫn đến tay Robot chuyển động theo. Vì vậy để điều khiển
Robot tại mỗi khớp được trang bị một cơ cấu kích thích (có thể là động cơ
điện, pít tông thuỷ lực, khí nén) và bộ truyền lực.
b. Hệ thống truyền động
Phần lớn Robot công nghiệp được truyền động bằng động cơ điện và
chủ yếu là động cơ điện một chiều. Ngoài ra Robot công nghiệp còn được
truyền động bằng thuỷ lực, khí nén.
c. Hệ thống nhận biết
Robot công nghiệp được trang bị hệ thống nhận biết để có thể xác định
trạng thái làm việc của chính nó. Hệ thống nhận biết thường cho các thông tin
về vị trí, tốc độ và lực của cơ cấu dẫn động. Vị trí có thể là trong toạ độ đề các
hoặc trong toạ độ khớp (góc quay của khớp).
d. Hệ thống điều khiển
Là những tín hiệu điều khiển Robot theo những chỉ tiêu xác định trước.
Hệ thống điều khiển được chia ra làm hai loại:
- Điều khiển quỹ đạo
- Điều khiển lực
Hệ thống điều khiển được xây dựng dựa trên hệ thống điều khiển số.

Tuy nhiên để có thể đáp ứng được yêu cầu về thời gian thực thì tốc độ xử lý
của thiết bị điều khiển số phải đủ nhanh. Với sự phát triển nhanh chóng của
kỹ thuật vi xử lý và máy tính thì hệ thống điều khiển Robot ngày càng hiện
đại và hoàn thiện.
1.1.2. Phân loại Robot công nghiệp
Robot công nghiệp có thể được phân loại theo chức năng (Robot hàn,
Robot lắp ráp ), cũng có thể phân loại theo không gian làm việc như sau:
- Chuyển động theo 3 trục x, y, z (Cartesian)
- Chuyển động theo khối trụ (Cylindrycal)
- Chuyển động theo khối cầu (Spherical)
- Chuyển động theo khối cầu (Revolute)
1.2. Một số lý thuyết cơ bản về Robot công nghiệp
1.2.1. Động học vị trí Robot
Động học vị trí Robot làm rõ mối quan hệ giữa tay Robot (end effector) và
các bộ phận chuyển động (các khớp). Động học vị trí được chia thành động học
thuận và động học ngược. Động học thuận là từ các biến khớp tính ra vị trí và
hướng của tay Robot. Động học ngược là từ vị trí và hướng của tay Robot tính ra
các biến khớp.
Động học vị trí Robot được nghiên cứu dựa trên cơ sở lý thuyết về phép biến
đổi toạ độ, các hệ toạ độ thanh nối và các bài toán động học vị trí.
1.2.1.1. Phép biến đổi đồng nhất
a. Biểu diễn vectơ
Một vectơ được biểu diễn như sau:
______
kcjbiav ++=
(1-1)
Trong đó
______
,, kji
là các vectơ đơn vị của 3 trục x, y, z trong hệ toạ độ đề

các. Vectơ v có thể được biểu diễn ở dạng sau:
v = [x, y, z, ω]
T
(1-2)
Hình 1.1. Không gian làm việc của Robot.
Đề các (Cartesian)
Trụ (Cylindrycal)
Cầu (Revolute)
Cầu (Spherical)
Trong đó: a = x/ω
b = y/ω
c = z/ω
trường hợp ω = 0 thì v là vectơ đơn vị.
b. Phép biến đổi đồng nhất
Phép biển đổi H là một ma trận 4×4 và có thể thực hiện các phép dịch
chuyển tịnh tiến hoặc quay. Nếu cho một vectơ u (hay một điểm u), sau khi thực
hiện phép biến đổi H thu được vectơ v tính theo công thức sau:
v = Hu
c. Phép biến đổi tịnh tiến
Phép biến đổi tịnh tiến theo một vectơ
______
kcjbia ++
là:
H = Trans(a,b,c) =













a
c
b
a
000
100
010
001
(1-3)
Cho một vectơ u = [x,y,z,ω]
T
thực hiện dịch chuyển tịnh tiến theo vectơ
[a,b,c,1]
T
thu được vectơ v như sau:
v = H.u = Trans(a,b,c).u =













1
/
/
/
ω
ω
ω
z
y
x
(1-4)
Phép tịnh tiến Trans(a,b,c) có nghĩa là thực hiện tịnh tiến theo trục x một
đoạn a đơn vị, theo trục y một đoạn b đơn vị, theo trục z đoạn c đơn vị.
d. Phép biến đổi quay
Quay quanh trục x góc θ:
H = Rot(x,θ) =













−
1000
0cossin0
0sincos0
0001
θθ
θθ
(1-5)
Quay quanh trục y góc θ
H = Rot(y,θ) =












−
1000
0cos0sin
0010
0sin0cos
θθ

θθ
(1-6)
Quay quanh trục z góc θ
H = Rot(z,θ) =












−
1000
0100
00cossin
00sincos
θθ
θθ
(1-7)
e. Phép biến đổi tổng hợp
Cho một vectơ u, thực hiện
- Quay quanh trục z góc θ: Rot(z,θ)
- Quay quanh trục y góc ϕ: Rot(y,ϕ)
- Quay quanh trục x góc γ: Rot(x,γ)
- Tịnh tiến theo vectơ [a,b,c,1]: Trans(a,b,c)

Thực hiện biến đổi so hệ toạ độ gốc:
H = Trans(a,b,c).Rot(x,γ).Rot(y,ϕ).Rot(z,θ) (1-8)
Thực hiện biến đổi so hệ toạ độ trước nó:
H = Rot(z,θ).Rot(y,ϕ).Rot(x,γ).Trans(a,b,c) (1-9)
1.2.1.2. Hệ toạ độ các thanh nối trong Robot
Xét 3 khớp liên tiếp: i - 1, i, i + 1
a. Các tham của thanh nối và khớp
kí hiệu:
a
i-1
là độ dài đường vuông góc chung giữa trục i và i - 1
a
i
là độ dài đường vuông góc chung giữa truc i và i - 1
d
i
là khoảng cách giữa hai đường vuông góc chung a
i-1
và a
i
θ
i
là góc giữa hai đường vuông góc chung a
i
và a
i-1
α
i
là góc giữa trục i và trục i + 1
Đối với khớp i: góc θ

i
là biến khớp quay nếu khớp i là khớp quay; là
biến khớp tịnh tiến nếu khớp i là khớp tịnh tiến
Đối với thanh i: a
i
chính là độ dài của thanh
b. Đặt hệ toạ độ cho các thanh nối
i-1
i
i+1
i-1
i
a
i-1
a
i
d
i
θ
i
α
i
z
i
x
i
z
i-1
x
i-1

Hình 1.2. Hệ toạ độ thanh nối
Sử dụng phương pháp đặt hệ toạ độ thanh nối của Denevit và
Hertenberg, nội dung phương pháp như sau:
- Gốc được đặt trên trục i + 1 tại chân đường vuông góc chung
a
i
với trục i
- Trục z
i
có phương trùng với trục khớp i + 1
- Trục x
i
có phương trùng với phương của a
i
, chiều đi từ trục i
đến trục i + 1
- Trục y
i
được xác định theo phương pháp tam diện thuận
c. Quan hệ giữa hai hệ toạ độ thanh nối liên tiếp
Có thể thực hiện biến đổi từ hệ toạ độ thanh i-1 về hệ toạ độ thanh i
bằng cách thực hiện lần lượt các phép biến đổi sau:
Rot(z
i-1
,θ
i
) Trục x
’
i-1
// x

i

Trans(0,0,d
i
)
Trans(a
i
,0,0) Trục x
’’’
i-1
trùng với trục x
i
Rot(x
i-1
,α
i
) Trục z
’’’’
i-1
trùng với trục z
i
Nếu thực hiện biến đổi so hệ toạ độ trước đó thu được ma trận phép
biến đổi giữa hệ toạ độ thanh i-1 và thanh i như sau:
A
i 1−
i
= H = Rot(z
i-1
,θ
i

).Trans(0,0,d
i
).Trans(a
i
,0,0).Rot(x
i-1
,α
i
)
A
i 1−
i
=












−
1000
cossin0
sinsincoscoscossin
cossinsincossincos

iii
iiiiiii
iiiiiii
d
a
a
αα
θαθαθθ
θαθαθθ
(1-10)
Như vậy ma trận A
i
(ma trận
i-1
A
i
được gọi tắt là A
i
) là ma trận biến đổi
từ hệ toạ độ thanh i-1 thành hệ toạ độ thanh i.
1.2.1.3. Động học thuận Robot
Giả sử Robot có n thanh, A
1
là ma trận phép biến đổi giữa hệ toạ độ
thanh 1 so với hệ toạ độ gốc cố định, A
2
là phép biến đổi giữa hệ toạ độ thanh
2 so với thanh 1. Vị trí và hướng của thanh 2 so với toạ độ gốc được xác định
bằng công thức:
T

2
= A
1
A
2
(1-11)
Tương tự, vị trí và hướng của thanh 3 so với hệ toạ độ gốc là:
T
3
= A
1
A
2
A
3
Vị trí và hướng của tay Robot (end effector) được xác định như sau:
T
n
=
∏
=
n
i
i
A
1
Thông thường một Robot có số bậc tự do là 6 (6 thanh), do đó ma trận
mô tả vị trí và hướng của tay Robot là T
6
và được xác định cụ thể như sau:













=
1000
6
zzzz
yyyy
xxxx
paon
paon
paon
T
= [
__
n

__
o

__

a

__
p
]
(1-12)
Vectơ
__
o
: mô tả phương của đường nối tâm giữa hai thanh kẹp (griper)
gắn ở tay Robot.
Vectơ
__
a
: có phương và chiều chính là phương vươn tới của tay Robot.
Vectơ
__
p
: mô tả vị trí của tay Robot.
______
aon ×=
Với ý nghĩa như vậy, ma trận T
6
mô tả cả hướng và vị trí của tay Robot,
muốn xác định vị trí và hướng của tay Robot chỉ cần xác định T
6
.
Bài toán động học thuận đặt ra là từ các biến khớp q
i
tìm ra vị trí và

hướng của tay Robot. Giải bài toán động học thuận bằng cách giải hệ 12
phương trình sau:







=
=
=
)(
)(
)(
12
2
1
qfp
qfn
qfn
z
y
x

(1-13)
Trong đó q = [q
1,
q
2

, , q
n
]
T

là vectơ biến khớp Robot.
1.2.1.4. Động học ngược Robot
Bài toán động học ngược là bài toán từ vị trí và hướng của tay Robot
(biết ma trận T
6
) tính ra các biến khớp.
Bài toán động học ngược là bài toán phức tạp, không có lời giải chung,
tuỳ từng trường hợp cụ thể mà có những cách giải khác nhau.
1.2.2. Động lực học Robot
Sử dụng phương pháp Lagrange để xây dựng phương trình động lực
học, nội dung phương pháp như sau:
Lập hàm Lagrange : L = K – P
(1-14)
Trong đó: L là hàm Lagrange
K là tổng động năng
P là tổng thế năng
Phương trình động lực học với khớp i:
M
i
=
dt
d
(∂L/d
i
q

•
) - ∂L/dq
i
(1-15)
Đối với khớp quay M
i
là mô men, đối với khớp tịnh tiến M
i
là lực.
Phương trình động lực học dạng tổng quát của Robot n bậc tự do:
••
=
_______
).( qqHM
+
)(),(
______
qgqqh +
•
(1-16)
Trong đó:
___
M
là vectơ mô men khớp
__
q
là vectơ mô men biến khớp
H là ma trận n×n đối xứng dương
h, g là ma trận n×1
1.2.3. Hệ thống điều khiển Robot

1.2.3.1 Tổng quan
Trong điều khiển Robot gồm hai giai đoạn: giai đoạn chuyển động tự
do và giai đoạn lắp ráp (làm một nhiệm vụ cụ thể). Trong giai đoạn chuyển
động tự do, lực sinh ra nhỏ nên vấn đề điều khiển chủ yếu là điều khiển quỹ
đạo (điều khiển thô). Khi lắp ráp, Robot cần sinh lực nhất định do đó khi điều
khiển phải điều khiển cả quỹ đạo lẫn lực và được gọi là điều khiển tinh.
Trong điều khiển quĩ đạo thường sử dụng phương pháp điều khiển phản
hồi (feedback) hoặc tiền định (feed forward) để đảm bảo vị trí tay Robot luôn
bám theo một quỹ đạo cho trước và tối ưu về thời gian. Tuỳ thuộc quỹ đạo
thiết kế cho khớp hay cho tay Robot mà tương ứng có điều khiển không gian
khớp hay không gian tay. Đối với điều khiển quĩ đạo tay Robot cần phải giải
bài toán động học để đưa ra những thông tin về sự chuyển động của các khớp
khi tay Robot chuyển động bám theo quỹ đạo đặt một cách liên tục. Có thể
thực hiện bài toán động học vị trí một cách on-line hay off-line tuỳ thuộc vào
từng yêu cầu và nhiệm vụ cụ thể.
Đối với Robot nhiều khớp là một hệ thống động và phi tuyến mạnh.
Thêm vào đó có nhiều tham số của cơ cấu cơ khí và kích thích thay đổi.
Thường thì tại mỗi khớp Robot được lắp một động cơ một chiều và nhiệm vụ
cụ thể là điều khiển vị trí từng khớp theo tín hiệu đặt một cách chính xác nhất.
1.2.3.2 Phân loại hệ thống điều khiển Robot
a. Phân loại theo tín hiệu điều khiển
- Điều khiển phân tán
- Điều khiển tập trung
Điều khiển phân tán có nghĩa là mỗi khớp được điều khiển độc lập, bỏ
qua mối liên hệ giữa các khớp. Đối với những ứng dụng không yêu cầu cao về
tốc độ và tải không đổi thì thường sử dụng phương pháp điều khiển phân tán
do tính đơn giản của hệ thống điều khiển.
Phương pháp điều khiển tập trung coi Robot là một hệ thống, khi đó
mối liên hệ giữa các khớp được tính đến.
b. Phân loại theo phương pháp điều khiển

- Điều khiển không thích nghi
- Điều khiển thích nghi
Các phương pháp điểu khiển không thích nghi như: điều khiển phản
hồi, điều khiển tiền định, phương pháp tính mô men Trong các phương pháp
này bỏ qua sự phi tuyến và sự thay đổi các tham số của Robot.
Các phương pháp điều khiển thích nghi như: phương pháp mô hình
chuẩn (MRAC – Model Reference Adaptive Control), phương pháp tự chỉnh
(self - turning). Trong điều khiển thích nghi có tính đến các yếu tố phi tuyến
và sự thay đổi của các tham số Robot.
1.2.3.3 Phương pháp điều khiển không thích nghi
Đối với điều khiển không thích nghi Robot có nhiều phương pháp được
sử dụng. Trong mục này chỉ đề cập đến những phương pháp thông dụng nhất:
- Phương pháp điều khiển phản hồi sử dụng luật điều chỉnh
PID
- Phương pháp điều khiển tiền định
- Phương pháp tính mô men
Phương pháp điều khiển phản hồi sử dụng luật điều chỉnh PID được áp
dụng rộng rãi và thành công với Robot công nghiệp. Phương pháp này đơn
giản và dễ dàng đáp ứng được yều cầu về thời gian thực. Phương pháp này là
một trong các phương pháp điều khiển phân tán (decentralised system).
Phương pháp điều khiển tiền định (feed forward) tiến hành tính mô
men đặt từ các thông số cho trước để đặt cho khớp. Đối với các nhiễu đặt nên
hệ thống thì bù bằng mô men phụ.
Phương pháp tính mô men sử dụng các mạch vòng feedback và feed
forward. Phương pháp này có thể làm giảm các nhiễu và tính phi tuyến nhờ sử
dụng hệ thống tiền định. Tuy nhiên khi sử dụng phương pháp này phải chú ý
đến đáp ứng thời gian thực của hệ thống. Phương pháp này là một trong các
phương pháp điểu khiển tập trung (centralised system).
Mô hình Robot sử dụng để điều khiển là phương trình (1-16)
)(),().(

_____________
qgqqhqqHM ++=
•••
a. Phương pháp điều khiển phản hồi sử dụng luật điều chỉnh PID
Sơ đồ khối:
Cấu trúc bộ điều khiển là có thể là bộ PD với luật điều khiển như sau:
Feedback
controller
Robot
q
d
M
q
•
q
Hình 1.3. sơ đồ khối phương pháp điều khiển phản hồi.
•
−=
_______
. qkKM
DP
dk
ε
(1-17)
Luật điều khiển có thể là
•
+=
_______
εε
DP

dk
KKM
(1-18)
Cũng có thể sử dụng cấu trúc bộ điều chỉnh là PID để đảm bảo sai lệch
tĩnh bằng không, luật điều chỉnh như sau:
∫
++=
•
τεεε
dKKKM
IDP
dk
_________
.
(1-19)
sơ đồ cầu trúc hệ điều khiển Robot dùng PID như sau:
K
P
K
D
Robot
q
dk
M
dk
q
•
q
Hình 1.4. Điều khiển PD với phản hồi tốc độ.
K

P
K
D
Robot
q
d
M
dk
q
•
q
Hình 1.5. Điều khiển PD có điều khiển tốc độ.
•
d
q
K
P
K
D
Robot
q
d
M
dk
q
•
q
Hình 1.6. Điều khiển PID đảm bảo sai lệch bằng
không.
•

d
q
K
I
/p
b. Phương pháp điều khiển tiền định (feed forward)
Luật điều khiển:
fdddddk
MqqqMM +=
•••
),,(
_________
(1-20)
M
f
được xác định bằng luật PD hoặc PID
c. Phương pháp tính mô men
Phương trình động lực học viết ở dạng:
),().( qqhuqHM
•
+=
(1-21)
∫
•••
+++=
εττεε
DIPd
KdKKqu )(
(1-22)
với ε = q

d
– q;
•••
−= qq
d
ε
; K
I
, K
P
, K
D
, là các ma trận đường chéo
dương.
R
-1
K
P
K
P
Robot
__
d
q
•
__
d
q
••
__

d
q
__
q
•
__
q
••
__
q
Hình 1.7. sơ đồ cấu trúc hệ điều khiển tiền định
sử dụng luật PD.
+ K
Pε
+ K
I
+ K
D
Tính
H(q).u + h(q,)
Robot
u
••
d
q
d
q
•
d
q

q
•
q
M
Hình 1.8. sơ đồ cấu trúc hệ điều khiển Robot theo phương pháp tính mô men.
1.2.3.4 Phương pháp điều khiển thích nghi
Phương pháp điều khiển thích nghi thường sử dụng nhất là phương
pháp mô hình chuẩn (MRAC) và phương pháp tự chỉnh (self - turning)
a. Phương pháp thích nghi theo mô hình chuẩn
Sơ đồ khối như sau:
Tín hiệu u là tín hiệu đặt (ví dụ: đặt quỹ đạo). Đầu ra của mô hình thực
và mô hình chuẩn được so sánh với nhau và sự sai khác giữa chúng
(Adaptation error) được đưa vào để tính toán luật điều khiển thích nghi cùng
với tín hiệu vào u(t) và ra y(t), từ đó điều chỉnh lại hệ thống sao cho mô hình
thực giống mô hình chuẩn. Có hai cách để điều chỉnh thích nghi theo mô hình
chuẩn là thích nghi theo thông số hoặc thích nghi theo tín hiệu.
- Thích nghi thông số làm thay đổi một số thông số của hệ
thống làm cho hệ thống giống với mô hình mẫu
- Thích nghi tín hiệu sử dụng một tín hiệu thích nghi để bù vào
tín hiệu điều khiển làm cho hệ thống phản ứng giống với mô
hình mẫu
b. Phương pháp tự chỉnh (self – turning)
Mô hình
chuẩn
Robot
Luật thích
nghi
u
sai số thích nghi
Thích nghi

thông số
Thích nghi tín hiệu
u
y
nhiễu
Hình 1.9. mô hình phương pháp thích nghi theo mô hình chuẩn.
Hệ thống thích nghi tự chỉnh được chia làm hai phần: ước lượng tham
số và điều chỉnh tham số. Phương pháp thích nghi tự chỉnh thường được áp
dụng trong điều khiển các hệ thống tuyến tính không biết các thông số hoặc
các thông số biến đổi chậm. Đầu tiên là thực hiện ước lượng tham số để đưa
ra những thay đổi mới nhất của thông số hệ thống, sau đó thông số của bộ
điều khiển sẽ được điều chỉnh sao cho phù hợp với hệ thống mới. Như vậy
phương pháp thích nghi tự chỉnh là tự thay đổi tham số bộ điều khiển để phù
hợp với hệ thống khi tham số hệ thống thay đổi.
1.2.4. Lập quỹ đạo chuyển động
1.2.4.1. Khái niệm
Trong điều khiển quỹ đạo Robot, phải điều khiển tay Robot bám theo
một quỹ đạo xác định trước (quỹ đạo mong muốn). Quỹ đạo mong muốn đó
là một hàm phụ thuộc thời gian. Việc xây dựng quỹ đạo chuyển động chính là
tìm phương trình mô tả quỹ đạo Robot theo thời gian.
- Thiết kế quỹ đạo cho tay Robot: xác định x
d
(t), y
d
(t)
- Thiết kế quỹ đạo cho khớp Robot: xác định q
id
(t)
Việc thiết kế quỹ đạo khớp Robot có ưu điểm: q(t) là lượng đặt trực
tiếp, hệ điều khiển là hệ điều khiển vị trí khớp, có thể xác định được điều kiện

giàng buộc của động cơ và hệ truyền động, giải bài toán động học ngược đơn
giản. Nhược điểm là khó đảm bảo chính xác quỹ đạo tay.
điều chỉnh
tham số
bộ điều
khiển
Robot
ước lượng
tham số
q
di
q
i
Hình 1.10. hệ thống thích nghi tự chỉnh.
Thiết kế quỹ đạo tay Robot có ưu điểm: lượng đặt là vị trí tay Robot
nên đảm bảo chính xác quỹ đạo. Nhược điểm: phải giải bài toán động học
ngược phức tạp với khối lượng tính toán lớn và khó tính được điều kiện giàng
buộc.
1.2.4.2. Quỹ đạo điểm điểm (point to point)
Yêu cầu điều khiển sao cho khớp Robot chuyển động từ vị trí q
0
đến vị
trí q
c
trong thời gian t
c
. Với dữ kiện ban đầu như vậy có các dạng quỹ đạo như
sau:
a. Dạng quỹ đạo bậc 3
q(t) = at

3
+ bt
2

+ ct +d (1-23)
Việc thiết kế quỹ đạo là xác định các hệ số a, b, c, d.
Tại thời điểm t = 0, khớp có vị trí q
0
, tốc độ
•
0
q
, gia tốc
••
0
q
Tại thời điểm t = t
c
, khớp có vị trí q
c
, tốc độ
•
c
q
, gia tốc
••
c
q
Thay vào phương trình (2-23) thu được hệ phương trình:










++=
+++=
=
=
•
•
cbtatq
dctbtatq
cq
dq
ccc
ccc
23
2
23
0
0
(1-24)
Giải hệ được a, b, c, d suy ra quỹ đạo
q(t).
b. Dạng quỹ đạo 2 – 1 – 2
Trong quá trình chuyển động từ q

0
đến
q
c
trải qua ba giai đoạn:
- Giai đoạn tăng tốc:
q
q
c
q
0
t
2
t
1
t
t
t
•
q
••
q
t
c
Hình 1.11. Đồ thị vị trí tốc độ
gia tốc quỹ đạo 2-1-2
consttq =
••
)(
)(tq

•
: Hàm bậc nhất theo t
q(t) : Hàm bậc hai theo t
- Giai đoạn chuyển động đều:
0)( =
••
tq
consttq =
•
)(
q(t) : Hàm bậc nhất theo t
- Giai đoạn giảm tốc:
consttq =
••
)(
)(tq
•
: Hàm bậc nhất theo t
q(t) : Hàm bậc hai theo t
Phương trình quỹ đạo:
Đoạn 1: q(t) = q
0
+
2
1
2
1
tq
••
với 0 < t < t

1
(1-25)
Đoạn 2: q(t) = q
0
+
)
2
1
(
111
tttq −
••
với t
1
< t < t
2
(1-26)
Đoạn 3: q(t) = q
c
-
2
1
)(
2
1
ttq
c
−
••
với t

2
< t < t
c
(1-27)
Trong đó: t
1
=
)(
4
2
1
2
0
1
2
qq
q
t
t
cc
c
−−−
••
(1-28)
Chọn
••
1
q
thoả mãn điều kiện :
2

0
1
4
c
c
t
qq
q
−
≥
••
(1-29)
Như vậy nếu biết q
0
, q
c
, t
c
, chọn được giái trị
••
1
q
thích hợp sẽ tính được
t
1
và viết được các phương trình của các đoạn: tăng tốc, chuyển động đều,
giảm tốc. Phương pháp này thực hiện
đơn giản, nhưng vẫn có xung động ở
điểm đầu và điểm cuối quỹ đạo.
c. Dạng quỹ đạo 4 – 1 – 4

Khớp Robot chuyển động từ q
0
đến q
c
trong thời gian t
c
biết:
q(0) = q
0
; q(t
c
) = q
c
;
••
=
0
)0( qq
;
••
=
cc
qtq )(
;
••••
=
0
)0( qq
;
••••

=
cc
qtq )(
;
Quỹ đạo chuyển động có ba giai đoạn:
- Khởi động: 0 ÷ t
1

- Chuyển động đều: t
1
÷ t
2

- Hãm: t
2
÷ t
c
Trong quỹ đạo 4 –1 – 4, giai đoạn khởi động và hãm là hàm bậc 4 theo
thời gian, giai đoạn chuyển động đều là hàm bậc 1 theo thời gian.
Phương trình đoạn khởi động (ac):
q
ac
(t) = a
4ac
t
4
+ a
3ac
t
3

+ a
2ac
t
2
+ a
1ac
t + a
0ac
với 0 < t < 2t
0
(1-30)
Phương trình đoạn chuyển động đều (cd):
q
cd
(t) = a
1cd
(t – 2t
0
) + a
0cd
với 2t
0
< t < t
2
(1-31)
Phương trình đoạn hãm (df)
q
df
(t) = a
4df

t
4
+ a
3df
t
3
+ a
2df
t
2
+ a
1df
t + a
0df
với t
2
< t < t
c
(1-32)
t
0
t
1
t
2
t
c
t
0
t

q
q
c
q
0
Hình 1.12. Quỹ đạo chuyển động
4 – 1 – 4.
a
c
d
f
b
e
Để xác định được quỹ đạo chuyển động, phải xác định 12 hệ số bằng
cách giải hệ phương trình 12 ẩn. Theo hình vẽ ta có:
q
1
= q
0
+
0
0
0
2
t
tt
qq
c
c
−

−
(1-33)
q
2
= q
c
-
0
0
0
2
t
tt
qq
c
c
−
−
(1-34)
Phương trình đoạn cd:
q
cd
(t) = q
1
+
)2(
2
0
0
0

tt
tt
qq
c
c
−
−
−
So sánh với phương trình (1-31) suy ra:
a
1cd
=
0
0
2tt
qq
c
c
−
−
(1-35)
a
0cd
= q
1
= q
0
+
0
0

0
2
t
tt
qq
c
c
−
−
(1-36)
Phương trình đoạn ac
Tại t = 0 ⇒







==
==
=
••••
••
0)0(
0)0(
0)0(
0
0
qq

qq
q
ac
ac
ac
(1-37)
Tại t = t
c
⇒





=
=
••
)2()2(
)2()2(
00
00
tqtq
tqtq
cdac
cdac
(1-38)
Lấy đạo hàm cấp 1 và 2 của phương trình (1-30) và sử dụng phương
trình (1-37), (1-38) thu được hệ số của phương trình đoạn ac như sau:
















−
−
−=
−
−
=
=
=
=
)2(16
)2(4
0
0
0
3
0
0

4
0
2
0
0
3
2
1
00
ttt
qq
a
ttt
qq
a
a
a
qa
c
c
ac
c
c
ac
ac
ac
ac
(1-39)
Phương trình quỹ đạo đoạn df
Thực hiện tương tự như xác định quỹ đạo đoạn ac, các hệ số phương

trình (1-32) được xác định theo công thức sau:















−−
−=
+
−=
=
−
−
=
−
−
−=
4
0
010

4
2
0
3
041
3
2
0
0
1
0
0
0
0
16
433
12
32
0
2
2
t
taaq
a
t
taa
a
a
tt
qq

a
t
tt
qq
qa
c
df
dfdf
df
df
c
c
df
c
c
cdf
(1-40)
Kết luận:
Trong chương này đã trình bày về lý thuyết cơ bản của Robot công
nghiệp. Dựa trên cơ sở lý thuyết này sẽ áp dụng vào tính toán thiết kế quỹ đạo
(ở chương II) và hệ thống điều khiển (ở chương IV) cho Robot Scara –
Serpent.
Chương II. Robot Scara - Serpent
Nội dung chương trình bày về Robot Scara – Serpent gồm có: giới
thiệu về Robot Scara – Serpent; thực hiện giải bài toán động học vị trí và
thiết kế quỹ đạo chuyển động của Robot Scara – Serpent.
2.1. Giới thiệu chung về Robot Scara – Serpent
Giống như tên gọi của nó, Robot Scara – Serpent là một Robot thuộc
lớp Robot Scara. Mặc dù Robot Scara có chuyển động đơn giản, song nó
cũng đáp ứng được hầu hết các yêu cầu về lắp ráp và vận chuyển do đó nó là

một trong những loại Robot được ứng dụng rộng rãi nhất trong công nghiệp.
Robot Scara – Serpent có bốn khớp, ba khớp quay và một khớp tịnh
tiến. Ba khớp quay được điều khiển bằng ba động cơ một chiều với các mạch
vòng phản hồi. Khớp tịnh tiến sử dụng khí nén để điều khiển và chỉ điều
khiển kiểu on – off. Scara – Serpent được trang bị các cảm biến đo vị trí
(Encoder).
Với thiết kế động cơ truyền động của cổ tay đặt trên trục cơ bản (thân
Robot) và liên hệ với cổ tay bằng đai truyền nên nó đảm bảo góc quay của cổ
tay không thay đổi trong quá trình tay máy chuyển động.
Các thông số hình học của Robot Scara – Serpent:
Hình 2.1. Robot Scara – Serpent.
l l
1 2
1
2
4
3
Hình 2.2. Cấu hình Robot Scara – Serpent.
θ
1
θ
2
θ
4
d
3
Khớp 1: Góc quay 192
o
(-96
o

÷ 96
o
)
Chiều dài thanh 1 là l
1
= 250mm
Khớp 2: Góc quay 230
o
(-115
o
÷ 115
o
)
Chiều dài thanh 2 là l
2
= 150mm
Khớp 3: Tịnh tiến tối đa 100mm
Khớp 4: Góc quay 450
0
2.2. Cấu hình Robot Scara – Serpent
Robot Scara – Serpent có cấu hình như sau:
Robot Scara – Serpent gồm 3 khớp quay và một khớp tịnh tiến: khớp
1, 2, 4 là khớp quay; khớp 3 là khớp tịnh tiến.
Các thông số: l
1
= 250mm; l
2
= 150mm
2.3. Động học vị trí Robot Scara – Serpent
2.3.1. Động học thuận

a. Hệ toạ độ các thanh nối
Áp dụng phương pháp đặt hệ toạ độ thanh nối của Denevit – Hatenberg để
đặt các hệ toạ độ thanh nối cho Robot Scara – Serpent như hình 2.3
Bảng thông số D – H của Robot Scara – Serpent
q
i
θ
i
d
i
a
i
α
i
1
θ
1
θ
1
0 l
1
0
o
2
θ
2
θ
2
0 l
2

0
o
3 d
3
0
o
d
3
0 0
o
4
θ
4
θ
4
0 0 0
o
b. Tính các hệ toạ độ thanh nối
Áp dụng công thức (1-10):
l l
1
2
3
z
z
x
x
x
x
z

z
z
1
2
0
1
0
1
2
2
3
3
4
d
3
θ
1
θ
4
a
1
a
2
θ
2
θ
4
Hình 2.3. Hệ toạ độ thanh nối của Robot Scara – Serpent.