hệ thống điều khiển robot scara

Nghiên cứu HTĐK Robot SCARA Chơng 1: Tổng quan các hệ thống điều khiển Robot
Chơng 1:
tổng quan các hệ thống điều khiển robot.
I. Giới thiệu chung:
Từ giữa thập kỷ 18, J.de Vancauson xây dựng các bupbe cơ khí có thể
chơi nhạc, đây đợc coi là sản phẩm đầu tiên của Tự động hoá - Robot.
Các thiết bị điều khiển có thể ghi nhớ tín hiệu điện từ và dùng lại với
sự trợ giúp các hoạt động cơ khí của máy móc đợc cấp chứng nhận vào
những năm 1950 tại Anh, Mỹ, Đức.
Năm 1968 Robot Shakey ra đời, phát triển hơn các loại Robot khác là
nó đợc trang bị các cảm biến va chạm, các camera và nó có thể di chuyển
trên sàn nhà.
Robot dùng ngôn ngữ lập trình và máy tính ra đời năm 1973. Ngôn
ngữ WAVE kết hợp với ngôn ngữ AL thành ngôn ngữ VAL, đợc phát triển
mạnh vào thời gian sau đó bởi Victor Sheinman và Bruce Simano.
Các Robot phục vụ công nghiệp cơ khí đợc phát triển từ năm 1974, sau
khi hãng Kawasaki nhận đợc giấy phép của Unimation dùng Robot trong
việc hàn hồ quang cho ô tô.
Ngày nay, Robot đợc sử dụng rộng rãi và phát triển trong các ngành kỹ
thuật. Dới sự điều khiển của máy tính cho phép Robot có thể có nhiều ứng
dụng hơn nữa.
Robot công nghiệp đợc định nghĩa là một cơ cấu máy có thể lập trình
đợc, có khả năng làm việc một cách tự động không cần sự trợ giúp của con
ngời. và giữa các tay máy có thể hợp tác đợc với nhau.
1. Cấu tạo:
Các Robot công nghiệp ngày nay thờng đợc đặt trên đế và gắn chặt với
sàn. Cơ thể đợc gắn với để, tổ hợp cánh tay đợc nối với cơ thể. Cuối cánh tay
là cổ tay.
1
Nghiên cứu HTĐK Robot SCARA Chơng 1: Tổng quan các hệ thống điều khiển Robot
Cổ tay gồm nhiều phần tử cho phép Robot định vị đa dạng các vị trí.

Quan hệ chuyển động giữa các phần tử khác nhau của cơ thể nh: cổ tay, cánh
tay đợc thực hiện qua một chuỗi các khớp nối. Các chuyển động bao gồm
chuyển động quay và chuyển động tịnh tiến.
Về mặt cơ khí, Robot đợc cấu tạo từ những thanh nối và khớp. Các
thanh nối đợc ghép với nhau bởi các khớp, cho phép Robot có các chuyển
động đa dạng.
2. Các chuyển động của Robot:
Robot đợc thiết kế để thực hiện các nhiệm vụ khác nhau trong sản
xuất. Các công việc đợc thực hiện bởi khả năng chuyển động của cơ thể,
cánh tay, cổ tay của Robot qua một chuỗi các chuyển động và vị trí. Cổ tay
đợc sử dụng cho Robot thực hiện chính xác công việc. Các chuyển động của
Robot đợc chia làm hai chuyển động cơ bản là chuyển động của cổ tay và
chuyển động của toàn bộ cơ thể. Các chuyển động riêng lẻ đợc ghép nối và
gắn chặt với hai dạng chuyển động này và chúng đợc giới hạn bởi số bậc tự
do (deggrees of freedom). Các Robot thông thờng có 4 đến 6 bậc tự do.
Các chuyển động cơ bản đợc thực hiện bởi các ghép nối về năng lợng.
Với Robot có từ 4 đến 6 bậc tự do, thờng có 3 ghép nối với hoạt động của
cánh tay và cơ thể, từ 2 đến 3 khớp nối sử dụng cho hoạt động của cổ tay.
Trong một chuỗi các chuyển động đều có liên hệ với nhau. Chuyển động đầu
ra có liên hệ với chuyển động đầu vào.
Các khớp nối đợc sử dụng trong thiết kế Robot công nghiệp điển hình
là khớp tịnh tiến và khớp quay.
+ Khớp tịnh tiến gồm cả chuyển động trợt và di chuyển.
+ Có 3 loại khớp quay trong tay máy Robot:
- Khớp R (Rotation).
- Khớp T (Twisting).
- Khớp V (Revolving).
Cánh tay Robot đợc thiết kế cho phép Robot có thể chuyển động tự do
trong giới hạn về kích thớc. Giới hạn chuyển động của Robot phụ thuộc:
+ Hình dạng vật lý của Robot.

+ Kích thớc các phần tử: cánh tay, cổ tay.
+ Giới hạn chuyển động của các khớp nối.
2
Nghiên cứu HTĐK Robot SCARA Chơng 1: Tổng quan các hệ thống điều khiển Robot
3. Hệ thống truyền động:
Có 3 dạng hệ thống truyền động chính của Robot là:
+ Truyền động bằng thuỷ lực.
+ Truyền động điện.
+ Truyền động khí nén.
Trong đó truyền động điện và truyền động bằng thuỷ lực đợc sử dụng
phổ biến trong các Robot vì tính kinh tế và đơn giản.
Truyền động khí nén thờng sử dụng trong các Robot có số bậc tự do
nhỏ, yêu cầu độ tác động nhanh cao.
4. Hệ thống nhận dạng:
Cảm biến đợc sử dụng nh thiết bị ngoại vi của Robot, gồm 2 loại đơn
giản nh: công tắc hành trình và hệ thống camera. Cảm biến cũng đợc dùng
nh các phần tử tích hợp của hệ thống phản hồi vị trí.
Các loại cảm biến thông dụng:
+ Cảm biến va chạm: là cảm biến phản ứng lại lực khi va chạm với
một vật khác.
+ Cảm biến phạm vi gần: là thiết bị cảm nhận đợc vật ở gần.
+ Cảm biến hỗn hợp: gồm các cảm biến nhiệt độ, áp suất và các đại l-
ợng vật lý khác.
+ Camera đợc sử dụng thực hiện việc kiểm tra, quan sát.
Cảm biến là phần tử quan trọng trong hệ thống điều khiển và giám sát
an toàn.
II. Phân loại Robot công nghiệp:
1. Phân loại theo dạng hình học:
Robot công nghiệp có 4 dạng hình học thông thờng:
+ Dạng Đecac.

+ Dạng hình trụ.
+ Dạng hình cầu.
+ Dạng khớp nối.
1.1 Dạng Đecac (hình 1. 1):
Dạng Đecac sử dụng 3 đờng trợt vuông góc nhau trong không gian là
các trục toạ độ x, y, z.
3
Nghiên cứu HTĐK Robot SCARA Chơng 1: Tổng quan các hệ thống điều khiển Robot
Robot chuyển động theo 3 trục toạ độ. Sử dụng các khớp tịnh tiến,
phạm vi làm việc của Robot đợc mở rộng theo hình chữ nhật.
Hình 1. 1: Cấu hình Robot dạng Decac
1.2 Dạng hình trụ (hình 1. 2):
Trục cơ bản là một trụ dọc, Robot chuyển động lên và xuống dọc theo
trục. Bằng các chuyển động quay quanh trục, tịnh tiến dọc trục phạm vi làm
việc của Robot đợc mở rộng theo một hình trụ bao quanh trục cơ bản.
Hình 1. 2: Cấu hình Robot dạng hình trụ
1.3 Dạng hình cầu (hình 1. 3):
Sử dụng các khớp lồng vào nhau, giúp cho Robot có khả năng chuyển
động lên hoặc xuống theo chiều ngang của trục quay. Dạng đa khớp nối cho
phép Robot mở rộng khoảng không gian làm việc theo hình cầu.
4
Nghiên cứu HTĐK Robot SCARA Chơng 1: Tổng quan các hệ thống điều khiển Robot
Hình 1. 3: Cấu hình Robot dạng hình cầu
1.4 Dạng khớp nối (hình 1. 4):
Tơng tự nh cánh tay con ngời, nó gồm hai phần tử thẳng tơng ứng với
cánh tay và cẳng tay. Các phần tử này đợc ghép nối với nhau bởi hai khớp t-
ơng ứng với khớp bả vai và khớp khuỷu tay. Cổ tay đợc nối với cẳng tay.
Hình 1. 4: Cấu hình Robot dạng khớp nối
2 Phân loại theo phơng pháp điều khiển:
2.1 Robot giới hạn chuyển động liên tục:

Robot giới hạn chuyển động liên tục không sử dụng hệ điều khiển bám
để di chuyển tới các vị trí lân cận trong các khớp nối. Chúng đợc giới hạn
điều khiển bằng các công tắc hành trình để dừng và thiết kế điểm cuối cho
mỗi chuyển động cho mỗi khớp nối.
2.2 Robot lặp lại với điều khiển từ điểm tới điểm:
Robot lặp lại sử dụng hệ điều khiển với một chuỗi các vị trí hoặc
chuyển động đợc dạy cho Robot, ghi lại trong bộ nhớ và đợc lặp lại dới sự
điều khiển của chính nó. Chia làm hai loại:
5
Nghiên cứu HTĐK Robot SCARA Chơng 1: Tổng quan các hệ thống điều khiển Robot
+ Lặp lại từ điểm tới điểm.
+ Lặp lại liên tục.
2.3 Robot điều khiển theo quỹ đạo liên tục:
Robot chuyển động với quỹ đạo đã đợc xác định trớc. Quỹ đạo chuyển
động là một chuỗi các điểm mô tả trên đờng chuyển động. Tín hiệu đặt là vị
trí mong muốn của cổ tay, Robot chuyển động theo luật điều khiển xác định
trớc để đến đích.
3. Phân loại theo số bậc tự do:
Mỗi bậc tự do tơng ứng với một chuyển động độc lập của Robot. Số
bậc tự do là số vị trí cần thiết để xác định hoàn toàn cấu hình của Robot.
Thông thờng số bậc tự do của Robot là số khớp nối từ thân đến cổ tay.
III. Mô hình toán học hệ thống truyền động:
Các khớp trong Robot đợc truyền động bởi hệ thống truyền động điện,
thuỷ lực hoặc khí nén. Mỗi hệ thống truyền động có u, nhợc điểm khác nhau.
Hệ thống truyền động điện sử dụng các động cơ điện một chiều hoặc
động cơ đồng bộ. Sơ đồ cấu trúc động học hệ thống điều khiển cho khớp nh ở
hình 1. 5.
+ Động cơ điện một chiều nam châm vĩnh cửu.
+ J
Đ

: mô men quán tính động cơ.
+ J
t
: mô men quán tính của khớp.
+ M
t
: mô men khớp.
+ i: tỉ số truyền.
+ : hiệu suất bộ truyền.
Hình 1. 5: Mô hình truyền động
Mô men cản qui đổi về trục động cơ:
6
Nghiên cứu HTĐK Robot SCARA Chơng 1: Tổng quan các hệ thống điều khiển Robot
M
t
M
c
=

i .

Trong đó: M
t
: là mô men khớp.
i: tỷ số truyền.
: hiệu suất bộ truyền.
Mô hình toán học động cơ và hệ truyền động khớp:
Hình 1. 6: Mô hình toán học động cơ
Trong đó: E(p) là sức điện động bộ biến đổi.
B là hệ số nhớt.

IV. Tổng quan về các phơng pháp điều khiển:
Theo nhiệm vụ làm việc của Robot có thể chia làm 2 bài toán điều
khiển: bài toán điều khiển thô và bài toán điều khiển tinh.
Giai đoạn vận chuyển chi tiết trong quá trình lắp ráp, Robot chuyển
động trong không gian theo một quỹ đạo nào đó gọi là chuyển động tự do,
không tiếp xúc với đối tợng. Trong giai đoạn này chỉ điều khiển quỹ đạo, gọi
là điều khiển thô, tay Robot chuyển động theo một quỹ đạo đặt trớc.
Khi tay Robot tiếp xúc với đối tợng, Robot phải sinh ra một lực phù
hợp để đa các chi tiết cần lắp ráp vào đúng vị trí, gọi là điều khiển tinh, liên
quan đến điều khiển lực, đồng thời cũng vừa điều khiển quỹ đạo.
Điều khiển quĩ đạo có thể thông qua điều khiển quĩ đạo khớp hoặc
điều khiển trực tiếp quĩ đạo tay.
7
Điều khiển Robot
Điều khiển thô
Điều khiển tinh
Toạ độ khớp Toạ độ tay Thụ động Tích cực
Nghiên cứu HTĐK Robot SCARA Chơng 1: Tổng quan các hệ thống điều khiển Robot
Hình 1. 7: Phân loại các phơng pháp điều khiển Robot
1. Điều khiển quĩ đạo khớp:
Thiết kế quĩ đạo điều khiển cho từng khớp. Chuyển động của tay phụ
thuộc vào chuyển động của từng khớp riệng biệt.
Ưu điểm:
+ Điều khiển trực tiếp động cơ truyền động khớp về tốc độ và mô men.
Có thể đảm bảo một số điều kiện giới hạn của hệ truyền động điện.
+ Giải bài toán động học ngợc đơn giản.
Nhợc điểm: do không trực tiếp điều khiển vị trí của tay nên khó đảm
bảo đợc đờng di chuyển bám theo quỹ đạo đặt trớc.
2. Điều khiển quĩ đạo tay:
Thiết kế quĩ đạo điều khiển trực tiếp cho tay Robot, chuyển động của

các khớp phụ thuộc vào chuyển động của tay.
Ưu điểm:
+ Đảm bảo tay di chuyển đợc chính xác theo quỹ đạo mong muốn của
Robot.
Nhợc điểm:
+ Bài toán động học ngợc có khối lợng tính toán lớn.
+ Khó đảm bảo một số điều kiện nh: gia tốc, mô men.
3. Điều khiển lực:
ở giai đoạn lắp ráp, Robot phải sinh ra lực phù hợp để đa chi tiết vào
vị trí êm, đồng thời vừa phải thực hiện điều khiển quĩ đạo.
8
Nghiên cứu HTĐK Robot SCARA Chơng 1: Tổng quan các hệ thống điều khiển Robot
Trở kháng là khái niệm đánh giá độ cứng của cơ cấu, chống lại lực tác
dụng lên nó.
+ Điều khiển trở kháng thụ động: cơ cấu đàn hồi có tâm điều khiển từ
xa. Khi chi tiết đa đến vị trí lắp ráp, tay máy không cần sinh lực để đa vào.
Trục chi tiết và trục của lỗ không trùng nhau, do đó các lò xo trên tay máy bị
uốn, đẩy chi tiết sao cho trục của chi tiết và trục của lỗ trùng nhau, đồng thời
sinh ra mô men quay quanh trục của lỗ, đẩy chi tiết vào đúng vị trí cần lắp
đặt.
+ Điều khiển trở kháng tích cực: dùng các phản hồi lực và vị trí để
điều khiển tay máy lắp ráp chi tiết. Mô phỏng Robot dới dạng vật tiếp xúc
với đối tợng, lực sinh ra phụ thuộc khe hở và độ cứng của môi trờng. Sau
khi đo đợc lực, thiết kế mạch vòng phản hồi lực.
f
đ
f f
đk
x
- f

Hình 1. 8: Sơ đồ mạch vòng phản hồi lực
4. Điều khiển vị trí:
Tín hiệu đặt là góc quay hoặc mô men, tín hiệu ra là vị trí của tay máy.
a. Luật điều khiển phản hồi:
Coi các bộ biến đổi và động cơ là khâu quán tính.
Trong đó:
q
đ
là tín hiệu đặt góc quay, thờng là khối đặt quỹ đạo.

=
q
đ
-
q
là sai số giữa tín hiệu vị trí đặt và tín hiệu vị
trí thực của hệ thống.

q
đ


q
M
đk
-
.
q
9
K

f
Robot
K
e
Bộ điều khiển Robot
Nghiên cứu HTĐK Robot SCARA Chơng 1: Tổng quan các hệ thống điều khiển Robot
Hình 1. 9: Sơ đồ điều khiển phản hồi
Trong trờng hợp đơn giản, ta dùng bộ biến đổi PD.
M
đk
= K
P
.

+ K
D
.
.

Khớp thứ i:
M
đki
= K
Pi
.

i
+ K
Di
.

.

i
K
P
, K
D
là hệ số khuếch đại tỉ lệ và hệ số khuếch đại vi phân của bộ
diều chỉnh.

= [
1
,
2
, ,
n
]
T
.

= [
.

1
,
.

2
, ,
.

n
]
T

M
đk
= [M
đk1
, M
đk2
, , M
đkn
]
T

K
P
, K
D
là các ma trận đờng chéo.
q
q
đ
M
đk
.
q
.
q

đ
Hình 1. 10: Sơ đồ điều khiển PD đơn giản




















dkn
M
.
.
.
M
M
dk2

dk1
=




















Pn
P
P
K
K
K
00

0 0
0 0
2
1





















n
n

.
.
.

1

+




















Dn
D
D

K
K
K
00



0 0
0 0
2
1

n
.
2
.
1
.

.
.
.


Ưu điểm: tính toán hệ thống đơn giản, độ tác động nhanh cao.
Nhợc điểm: không khử đợc sự phi tuyến ràng buộc giữa các khớp.
10
K
P
K
D
Nghiên cứu HTĐK Robot SCARA Chơng 1: Tổng quan các hệ thống điều khiển Robot
b. Luật điều khiển mô men tính toán:
Từ phơng trình động lực học, thiết kế luật điều khiển để khử phi tuyến
giữa các khớp.
Phơng trình động lực học:
M = H(
q
).

q
+ h(
q
,
.
q
) + g(
q
) (1. 4)
Chọn tín hiệu ra bộ điều khiển:
M
đk
= H(
q
).
U
+ h(
q
,
.
q
) + g(
q
) (1. 5)
Trong đó
U
là tín hiệu điều khiển phụ.
M
đk
=

M
Khi đó:
H(
q
).
U
+ h(
q
,
.
q
) + g(
q
) = H(
q
).

q
+ h(
q
,
.
q
) + g(
q
) (1. 7)
Với Robot cụ thể ta có: H(
q
) 0, nên

q
=
U
(1. 8)
là phơng trình mô tả động học của hệ thống.
q

U

M
đk

.
q
Hình 1. 11: Sơ đồ điều khiển mô men tính toán
Chọn luật điều khiển
U
đảm bảo một số chỉ tiêu: quá trình quá độ và
sai số tĩnh. Thờng chọn bộ điều chỉnh U là PID.

q
=
U
U
=

q
đ
+ K
P

.

+ K
D
.
.

+ K
I
.


. dt (1. 9)
=
q
đ
-
q
: sai số vị trí.
.

=
.
q
đ
-
.
q
: sai số tốc độ.
K

P
, K
D
, K
I
: là các hệ số của bộ điều chỉnh.
Phơng trình động học của hệ thống:
11
Bộ điều khiển Robot
Nghiên cứu HTĐK Robot SCARA Chơng 1: Tổng quan các hệ thống điều khiển Robot

q
=

q
đ
+ K
P
.

+ K
D
.
.

+ K
I
.

. dt (1. 10)
Với


=

q
đ
-

q
ta có:



+

q
đ
+ K
P
.

+ K
D
.
.

+ K
I

.


. dt = 0 (1. 11)
Khớp thứ i ta có:


i
+

q
đi
+ K
Pi
.

i
+ K
Di
.
.

i
+ K
Ii
.


i
. dt = 0 (1. 11)

Bằng phơng pháp mô hình hoá hoặc phơng pháp đặt nghiệm cực, ta xác định
đợc các hệ số K
P
, K
D
, K
I
sao cho hệ thống hội tụ, ổn định tức là các sai số
i
0.
Ưu điểm: khử đợc tính phi tuyến của hệ thống, biến hệ thống thành
tuyến tính.
Nhợc điểm:
+ Hệ thống điều khiển cồng kềnh, khối lợng tính toán lớn.
+ Chỉ thực hiện đợc khi biết đầy đủ các tham số của Robot.
+ Luật điều khiển không thích nghi khi tham số biến đổi.
V. ứng dụng của Robot:
1. Ưu điểm của Robot:
Robot có thể làm việc liên tục trong thời gian dài, chúng chỉ ngừng
hoạt động khi cần duy tu, bảo dỡng, thay thế.
Robot có khả năng làm việc trong môi trờng độc hại, khu vực nguy
hiểm, hoặc những nơi con ngời không thể đến đợc.
Với chơng trình đợc đặt trớc, Robot có khả năng làm việc với hiệu suất
cao hơn con ngời, tiết kiệm nguyên vật liệu , độ chính xác làm việc cao.
Giá thành và chi phí lắp đặt, chế tạo Robot ngày càng thấp do sự tiến
bộ của khoa học kỹ thuật.
Khi thay đổi công việc, lập trình lại cho Robot nhanh hơn và chi phí
thấp hơn so với việc đào tạo một công nhân.
2. ứng dụng của Robot:
+ Trong ngành công nghiệp cơ khí, Robot thực hiện những công việc

nặng nhọc nh nâng hạ, lắp ráp các chi tiết lớn, cồng kềnh yêu cầu độ chính
xác cao. Sử dụng Robot để hàn điểm, hàn hồ quang, sơn, mạ.
12
Nghiên cứu HTĐK Robot SCARA Chơng 1: Tổng quan các hệ thống điều khiển Robot
+ Sử dụng Robot trong môi trờng độc hại, nguy hiểm nh trong các lò
phản ứng hạt nhân, dới nớc
+ Robot thám hiểm vũ trụ, các hành tinh thuộc hệ mặt trời mà con ng-
ời cha tới đợc.
+ Trong sinh hoạt, Robot trợ giúp ngời già, phục vụ trong nhà bếp
làm giảm sức lao động của con ngời.
Nh vậy cùng với sự phát triển của khoa học kỹ thuật, công nghệ tự
động hoá, Robot ngày càng đợc hoàn thiện và có nhiều ứng dụng trong công
nghiệp cũng nh trong cuộc sống hàng ngày của con ngời.
Chơng 2:
tính toán động học - động lực học robot serpent
I. Khái niệm robot scara -serpent:
Robot SCARA (Selectively Compliant Articulated Robot Arm) có
nghĩa là có thể lựa chọn dễ dàng khớp nối của cánh tay Robot. Chúng có một
lợng nhỏ đàn hồi trong quá trình làm việc.
13
Nghiên cứu HTĐK Robot SCARA Chơng 1: Tổng quan các hệ thống điều khiển Robot
Do chuyển động của Robot SCARA đơn giản, dễ dàng nên nó đợc sử
dụng khá phổ biến trong công nghiệp. Robot Serpent là loại cơ bản trong
nhóm Robot công nghiệp này.
Hình 2. 1: Robot Serpent
Với thiết kế động cơ truyền động của cổ tay đợc đặt trên trục cơ bản 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.
Truyền động cho 2 khớp của tay máy và cổ tay bằng động cơ servo
một chiều có phản hồi vị trí tạo thành một vòng điều khiển kín. Chuyển động

thẳng đứng đợc thực hiện bằng pitton khí nén.
Chiều cao của Robot có thể thay đổi dễ dàng bằng cách thay đổi vị trí
gá thân Robot trên trục cơ bản, giúp tay máy thuận lợi trong việc thay đổi
công việc.
Robot Serpent có thể đợc lập chơng trình từ máy tính bằng cách đặt dữ
liệu cho mỗi trục. Hoặc điều khiển bằng tay sử dụng thiết bị lái điện
(steering) cho tay máy dùng các cuôn dây điện từ trong pendant.
II. khái niệm về động học vị trí robot:
Một tay máy có cấu tạo gồm một chuỗi các thanh nối cứng đợc liên
kết với nhau bởi các khớp. Mặt khác các thanh nối và các khớp đặt ở các vị
14
Nghiên cứu HTĐK Robot SCARA Chơng 1: Tổng quan các hệ thống điều khiển Robot
trí khác nhau trong không gian. Động học vị trí sẽ giải quyết mối quan hệ vị
trí của khớp và tay.
Ta đặt lên mỗi thanh nối một hệ trục toạ độ, sử dụng phép biến đổi
đồng nhất để miêu tả mối quan hệ về vị trí và hớng giữa các hệ toạ độ.
Phép biến đổi đồng nhất mô tả quan hệ tịnh tiến và quay giữa giữa hệ
thống toạ độ một thanh nối và một thanh nối kế tiếp bằng một ma trận vuông
A (4x4).
1. Các phép biến đổi đồng nhất:
Một vectơ
v
= a
i
+ b
j
+ c
k
đợc mô tả bằng một ma trận cột:
v

=












w
z
y
x
Trong đó: a = x/w.
b = y/w.
c = z/w.
w gọi là hệ số tỷ lệ.
Một vectơ điểm nh trên đợc sử dụng để định nghĩa vị trí cuối cùng của
cánh tay trong tay máy Robot.
a. Phép biến đổi tịnh tiến:
Tịnh tiến một điểm lần lợt theo 3 trục x, y, z tơng ứng với khoảng cách
a, b, c:
H = Trans(a, b, c) =

1000
100
010
001
c
b
a
ý nghĩa: H biểu diễn một khung toạ độ đặc biệt có các vectơ đơn vị
i
,
j
,
k
trùng hớng với
x
,
y
,
z
trong hệ toạ độ gốc và gốc toạ độ có vị trí biểu
diễn bằng vectơ:

[ ]
cba
T
.
15
Nghiên cứu HTĐK Robot SCARA Chơng 1: Tổng quan các hệ thống điều khiển Robot
b. Phép biến đổi quay:
Quay một vectơ theo 3 trục với 1 góc đợc thực hiện bởi phép quay:
+ Quay quanh trục x một góc :
Rot (x,) =













1000
00
00
0001


CosSin

SinCos
+ Quay quanh trục y một góc :
Rot (y,) =













1000
00
0010
00


CosSin
SinCos
+ Quay quanh trục z một góc :
Rot (z,) =

1000
0100
00
00


CosSin
SinCos
ý nghĩa của phép biến đổi:
+ Phép biến đổi H biểu diễn bằng một ma trận 4 x 4, biểu diễn một hệ
toạ độ mới so với hệ toạ độ gốc, trong đó ba cột đầu tiên của ma trận H biểu
diễn hớng của hệ toạ độ mới so với hệ toạ độ gốc và cột thứ t biểu diễn vị trí
gốc toạ độ.
c. Phép quay xung quanh một trục bất kỳ:
Phép quay xung quanh vectơ K một góc :
H = Rot (K, )
16
Nghiên cứu HTĐK Robot SCARA Chơng 1: Tổng quan các hệ thống điều khiển Robot
Vectơ K có điểm gốc trùng với gốc tọa độ và có các thành phần trên 3
trục là k
x

, k
y
, k
z
:
K
= k
x
.
i
+ k
y
.
j
+ k
z
.
k
Rot (K, ) =

+
++
++
1000
0
0
0



CVkzkzSkxVkzkySkyVkzkx
SkxVkykzCVkykySkzVkykx
SkyVkxkzSkzVkykxCVkxkx
Trong đó: C biểu dễn cho Cos
S biểu diễn cho Sin
V = biểu diễn cho Ver = 1 - Cos
2. Biểu diễn tay Robot:
Hệ toạ độ tay Robot đợc biểu diễn bằng một ma trận 4 x 4:
T
6
=

1000
zzzz
yyyy
xxxx
paon
paon
paon
n
= n
x
.
i
+ n
y
.
j
+ n
z
.
k
: trục định vị.
o
= o
x
.
i

+ o
y
.
j
+ o
z
.
k
: trục toạ độ định hớng.
a
= a
x
.
i
+ a
y
.
j
+ a
z
.
k
: trục toạ độ tới.
p
= p
x
.
i
+ p
y

.
j
+ p
z
.
k
: vị trí gốc toạ độ.
Ma trận T có thể viết tách thành 2 phần:
Ma trận
[ ]
R
là ma trận biểu diễn hớng và một ma trận biểu diễn vị trí.
T =












R
.

1000
100
010
001
z
y
x
p
p
p
17
Nghiªn cøu HT§K Robot SCARA Ch¬ng 1: Tæng quan c¸c hÖ thèng ®iÒu khiÓn Robot
Cã mét sè phÐp biÓu diÔn híng cña tay Robot nh:
+ PhÐp biÓu diÔn Euler.
+ PhÐp biÕn ®æi Roll - Pitch - Yaw.
2. 1. PhÐp biÓu diÔn Euler:
PhÐp biÓu diÔn Euler lµ tæ hîp cña ba phÐp quay:
+ Quay mét gãc φ quanh trôc z.
+ Quay mét gãc θ quanh trôc y.
+ Quay mét gãc ψ quanh trôc z (míi).
Euler (φ, θ, ψ = Rot (z, φ) . Rot (y, θ) . Rot (z, ψ)

=












−
1000
0100
00
00
φφ
φφ
CS
SC













−
1000
0θ0θ
0010
0θ0θ
CS
SC












−
1000
0100
00ψψ
00ψψ
CS
SC

=












−
−+−+
−−−
1000
0θψ.θψ.θ
0θ.φψ.φψ.θ.φψ.φψ.θ.φ
0θ.θθ.φψ.θ.φψ.φψ.θ.φ
CSSCS
SSCCSCSSCCCS
SCCSSCCSSCCC
2. 2. PhÐp biÕn ®æi Roll, Pitch, Yaw:
PhÐp biÕn ®æi Roll, Pitch, Yaw lµ tæ hîp cña 3 phÐp quay:
+ Quay quanh trôc x mét gãc φ.
+ Quay quanh trôc y mét gãc θ.
+ Quay quanh trôc z mét gãc ψ.
RPY = Rot (z, ψ) . Rot (y, θ) . Rot (x, φ)
=













−
−+
+−
1000
0
0
0
ψθψθθ
ψφψθφψφψθφθφ
ψθψθφψφψθφθφ
CCCCS
SCCSSCCSSSCS
SSCSCCSSSCCC
18
Nghiên cứu HTĐK Robot SCARA Chơng 1: Tổng quan các hệ thống điều khiển Robot
3. Động học vị trí:
Động học vị trí Robot gồm 2 bài toán:
Động học thuận: cho phép tính đợc vị trí và hớng của tay Robot từ vị

trí và hớng của các khớp Robot. Có hai phơng pháp giải bài toán động học
thuận là:
+ Dùng các phép biến đổi.
+ Gán hệ toạ độ cho các thanh nối.
Động học ngợc: giải bài toán khi biết vị trí tay của Robot, tính toán vị
trí các khớp để điều khiển các khớp giúp tay chuyển động theo quỹ đạo mong
muốn.
4. Động học thuận Robot:
Để xây dựng động học Robot, một cách tổng quát gán cho mỗi thanh
nối một hệ trục toạ độ và tìm quan hệ giữa 2 hệ toạ độ 2 thanh nối liền nhau.
Từ đó có thể xác định đợc quan hệ vị trí của tay Robot với các khớp.
Xét 2 khớp liền nhau và 2 thanh nối liền nhau (n và n + 1)nh hình 2. 2
+ Thanh nối n liên kết giữa khớp n và n + 1.
+ Hai khớp có trục chéo nhau trong không gian.
a
n
là độ dài pháp tuyến chung của 2 trục khớp n và n + 1.
d
n
là khoảng cách của 2 chân pháp tuyến chung trên trục n.

n
là góc giữa 2 pháp tuyến chung n và n - 1.

n
là góc giữa 2 trục n và n + 1.
Qui tắc đặt hệ toạ độ cho thanh n:
+ Gốc toạ độ đặt trên trục n + 1 và trùng với chân pháp tuyến chung
giữa trục n và n + 1.
+ Trục z

n
trùng với trục n + 1.
19
Nghiên cứu HTĐK Robot SCARA Chơng 1: Tổng quan các hệ thống điều khiển Robot
+ Trục x
n
trùng với hớng của a
n
, đi ra từ trục n + 1.
Hình 2. 2: Đặt hệ toạ độ cho thanh nối Robot
Hệ toạ độ n + 1 nhận đợc từ hệ toạ độ n bằng 4 phép biến đổi:
+ Quay quanh z
n-1
góc
n
sao cho trục x
n-1
trùng hớng với trục x
n
.
+ Tịnh tiến gốc O
n-1
theo trục z
n-1
đoạn d
n
.
+ Tịnh tiến theo trục x đoạn a
n
.

+ Quay quanh trục x góc
n
sao cho trục z
n-1
trùng hớng với trục n - 1
Gọi phép biến đổi giữa n và n - 1 là A
n
:
A
n
= Rot (z,
n
) . Trans (0, 0, d
n
) . Trans (a
n
, 0, 0) . Rot (x,
n
)
A
n
=

1000
0


dCS
SaSCCCS
CaSSCSC



Trong khớp tịnh tiến: a = 0.
Với Robot có n bậc tự do, toạ độ điểm cuối của tay máy đợc miêu tả:
T
n
= A
1
. A
2
. A
3
. . . A
n
20
joint n joint n+1

Link n
a
n
z
n
x
z
n -1
x
n -1


n
n
d
n
n
n
Nghiên cứu HTĐK Robot SCARA Chơng 1: Tổng quan các hệ thống điều khiển Robot
III. xây dựng động học vị trí của robot serpent:
Robot Serpent gồm 3 khớp chuyển động quay và một khớp chuyển
động tịnh tiến.
+ Khớp 1 quay quanh trục Z
0
góc
1
.
+ Khớp 2 quay quanh trục Z
1
góc

2
.
+ Khớp 3 chuyển động tịnh tiến theo trục Z
2
đoạn d
3
.
+ Khớp 4 quay quanh trục Z
3
góc
4
.
Hình 2. 3: Cấu hình Robot Serpent
Theo cấu hình Robot ta có bảng tham số:
Bảng 2. 1: Tham số động học Robot Serpent
Thanh nối Biến a d
1
2
3
4

1

2
d
3

4
0
-180

0
0
0
a
1
a
2
0
0
0
0
d
3
0
21
l l
1
2
3
4
z
z
x
x
x
x
z
z
z
1

2
0
1
0
1
2
2
3
3
4
d
3



1
2
4
a
1
a
2
Nghiên cứu HTĐK Robot SCARA Chơng 1: Tổng quan các hệ thống điều khiển Robot
Theo Denavit Hartenberg: toạ độ tay của Robot đợc xác định so với hệ
toạ độ gốc:
T
4
= A
1
. A

2
. A
3
. A
4
Trong đó:
A
n
=














1000
0


nnn
nnnnnnn
nnnnnnn

dCosSin
SinaSinCosCosCosSin
CosaSinSinCosSinCos



Thay số liệu trong bảng tham số ta có:
A
1
=













1000
0100
.
.0
1111
1111
SinaCosSin

CosaSinCos
0

A
2
=














1000
0100
.
.0
222
2222
0

2
SinaCosSin

CosaSinCos
A
3
=












1000
100
010
0001
3
d
0
A
4
=

1000
0100
0
00
44
44
0

CosSin
SinCos
Ký hiệu: S
1
Sin
1
C
1
Cos
1
22
Nghiªn cøu HT§K Robot SCARA Ch¬ng 1: Tæng quan c¸c hÖ thèng ®iÒu khiÓn Robot
S

2
⇔ Sinθ
2
C
2
⇔ Cosθ
2
S
4
⇔ Sinθ
4
C
4
⇔ Cosθ
4
Ta cã:
1.
3
T
4
= A
4
3
T
4
= A
4
=













−
1000
0100
0
00
44
44
0CS
SC
2.
2
T
4
= A
3
.
3
T
4
2

T
4
=












1000
100
010
0001
3
d
0














−
1000
0100
0
00
44
44
0CS
SC
=












−
1000
100

0
00
3
44
44
d
CS
SC
0
3.
1
T
4
= A
2
.
2
T
4
1
T
4
=













−
−
1000
0100
.
.0
222
2222
SaCS
CaSC
0
2












−

1000
100
0
00
3
44
44
d
CS
SC
0
=
=












−−
−−−
+−+
1000
100

.0
3
2242424242
2242424242
d
SaCCSSSCCS
CaCSSCSSCC
0
4.
0
T
4
= A
1
.
1
T
4
=
23
Nghiên cứu HTĐK Robot SCARA Chơng 1: Tổng quan các hệ thống điều khiển Robot
=

1000
0100
.
.0
1111
1111
SaCS
CaSC
0

++
1000
100

.0
3
2242424242
2242424242
d
SaCCSSSCCS
CaCSSCSSCC
0
=













++++++
+++
1000
100

) () () () (
0) () () () (
3
1112221242421424214242142421
1121221242421424214242142421
d
SaCSaCSaCCSSCCSSCSSCCSCSSCCS
CaSSaCCaCCSSSCSSCCSCCSSSSCCC
0
Ma trận
0
T
4
biểu diễn tay máy Robot trong hệ toạ độ gốc.
Mặt khác theo ký hiệu tổng quát:
0
T
4
=

1000
zzzz
yyyy
xxxx
paon
paon
paon
Do đó hệ phơng trình động học thuận tay máy Robot:
n
x
= C
1
(C
2
. C
4
+ S
2
. S
4
) - S
1
(S
2
. C
4
- C
2
. S

4
) (2.1)
n
y
= S
1
(C
2
. C
4
+ S
2
. S
4
) + C
1
(S
2
. C
4
- C
2
. S
4
) (2. 2)
n
z
= 0 (2. 3)
o
x

= C
1
(-C
2
. S
4
+ S
2
. C
4
) - S
1
(S
2
. S
4
- C
2
. C
4
) (2. 4)
o
y
= S
1
(-C
2
. S
4
+ S

2
. C
4
) + C
1
(S
2
. S
4
- C
2
. C
4
) (2. 5)
o
z
= 0 (2. 6)
a
x
= 0 (2. 7)
a
y
= 0 (2. 8)
a
z
= - 1 (2. 9)
p
x
= a
1

. Cos
1
+ a
2
. Cos(
1
+
2
) (2. 10)
p
y
= a
1
. Sin
1
+ a
2
. Sin(
1
+
2
) (2. 11)
p
z
= - d
3
(2. 12)
IV. xây dựng phơng trình động học ngợc robot serpent: Động
học ngợc giải quyết bài toán xác định các biến khớp khi biết vị trí tay Robot.
24

Nghiên cứu HTĐK Robot SCARA Chơng 1: Tổng quan các hệ thống điều khiển Robot
Từ 12 phơng trình xác định vị trí tay Robot (2. 1) đến (2. 12) khi biết
các thông số biến khớp trong phần tính toán động học thuận ở trên, ta thấy:
Từ phơng trình (2. 10) và (2. 11) ta có:
p
x
2
= a
1
2
. C
1
2
+ a
2
2
. C
12
2
+ 2. a
1
. a
2
. C
1
. C
12
p
y
2

= a
1
2
. S
1
2
+ a
2
2
. S
12
2
+ 2. a
1
. a
2
. S
1
. S
12
Trong đó: C
12
= Cos(
1
+
2
)
S
12
= Sin(

1
+
2
)
p
x
2
+ p
y
2
= a
1
2
(C
1
2
+ S
1
2
) + a
2
2
(S
12
2
+ C
12
2
) + 2. a
1

. a
2
.(C
1
. C
12
+ S
1
. S
12
) =
= a
1
2
+ a
2
2
+ 2. a
1
. a
2
.(C
1
. C
12
+ S
1
. S
12
)

Rút gọn theo các công thức lợng giác ta có:
p
x
2
+ p
y
2
= a
1
2
+ a
2
2
+ 2. a
1
. a
2
. Cos(
1
- (
1
+
2
))
= a
1
2
+ a
2
2

+ 2. a
1
. a
2
. Cos(-
2
)
Do đó:
Cos
2
=
21
2
2
2
1
22
2 aa
aapp
yx
+
Sin
2
=
2
2
1 Cos
Từ đó tính đợc góc
2
:

2
= atan2(Sin
2
, Cos
2
) (2. 13)
Mặt khác từ phơng trình (2. 10) ta có:
p
x
= a
1
. Cos
1
+ a
2
. Cos(
1
+
2
)
= a
1
. Cos
1
+ a
2
. Cos
1
.

Cos
2

- a
2
. Sin
1
. Sin
2

= (a
1
+ a
2
. Cos
2
). Cos
1
- a
2
. Sin
1
. Sin
2
Cos
1
=
221
212

.

Cosaa
SinSinap
x
+
+
(2. 14)
Phơng trình (2. 11):
p
y
= a
1
. Sin
1
+ a
2
. Sin(
1
+
2
) =
= a
1
. Sin
1
+ a
2
. Sin
1

.

Cos
2

+ a
2
. Cos
1
. Sin
2
= (a
1
+ a
2
. Cos
2
). Sin
1
+ a
2
. Sin
2
. Cos
1
25