Nghiên cứu giải bài toán động học ngược bằng phương pháp tách nhóm 3 khâu và phương pháp số

ĐẠI HỌC QUỐC GIA TP.HỒ CHÍ MINH
TRƯỜNG ĐẠI HỌC BÁCH KHOA

HHÕÕÕII

TRẦN THỊ LIÊN

NGHIÊN CỨU GIẢI BÀI TOÁN ĐỘNG HỌC
NGƯC BẰNG PHƯƠNG PHÁP TÁCH NHÓM 3
KHÂU VÀ PHƯƠNG PHÁP SỐ

Chuyên ngành : CƠ KHÍ CHẾ TẠO MÁY
Mã số ngành

: 2.01.00

LUẬN VĂN THẠC SĨ

TP. HỒ CHÍ MINH - THÁNG 09 NĂM 2006


ĐẠI HỌC QUỐC GIA TP.HỒ CHÍ MINH
TRƯỜNG ĐẠI HỌC BÁCH KHOA

HHÕÕÕII

TRẦN THỊ LIÊN

NGHIÊN CỨU GIẢI BÀI TOÁN ĐỘNG HỌC
NGƯC BẰNG PHƯƠNG PHÁP TÁCH NHÓM 3
KHÂU VÀ PHƯƠNG PHÁP SỐ

Chuyên ngành : CƠ KHÍ CHẾ TẠO MÁY
Mã số ngành

: 2.01.00

LUẬN VĂN THẠC SĨ

TP. HỒ CHÍ MINH - THÁNG 09 NĂM 2006
1


CÔNG TRÌNH ĐƯC HOÀN THÀNH TẠI
TRƯỜNG ĐẠI HỌC BÁCH KHOA
ĐẠI HỌC QUỐC GIA TP. HỒ CHÍ MINH
---[\---

Cán bộ hướng dẫn khoa học:
PGS. TS. LÊ HOÀI QUỐC

Cán bộ chấm nhận xét 1:
TS. BÙI TRỌNG HIẾU

Cán bộ chấm nhận xét 2:
TS. TRẦN THIÊN PHÚC

Luận văn thạc só được bảo vệ tại HỘI ĐỒNG CHẤM BẢO VỆ LUẬN VĂN THẠC SĨ
TRƯỜNG ĐẠI HỌC BÁCH KHOA

Ngày 19 tháng 9 năm 2006

2


TRƯỜNG ĐẠI HỌC BÁCH KHOA
PHÒNG ĐÀO TẠO SAU ĐH

CỘÄNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM

ĐỘC LẬP – TỰ DO – HẠNH PHÚC

NHIỆM VỤ LUẬN VĂN THẠC SĨ
Họ và tên học viên : Trần Thị Liên
Ngày tháng năm sinh : 18 tháng 3 năm 1965
Chuyên ngành : Chế tạo máy

Phái : nữ
Nơi sinh : Hà nội
Mã số học viên : : 2.01.00

I- TÊN ĐỀ TÀI : Nghiên cứu giải bài toán động học ngược bằng phương
pháp tách nhóm 3 khâu và phương pháp số
II- NHIÊM VỤ VÀ NỘI DUNG :

Xây dựng giải thuật giải bài toán động học ngược tay máy – Bài toán vị trítheo phương pháp tách nhóm ba khâu và phương pháp số
III- NGÀY GIAO NHIỆM VỤ : 7-2005
IV- NGÀY HOÀN THÀNH NHIỆM VỤ : 19-9-2006
V- CÁN BỘ HƯỚNG DẪN : PGS.TS. LÊ HOÀI QUỐC
CÁN BỘ HƯỚNG DẪN

CN. BỘ MÔN
QL CHUYÊN NGÀNH

Nội dung và đề cương Luận văn thạc só đã được Hội đồng chuyên ngành thông
qua
Ngày
TRƯỞNG PHÒNG ĐT- SĐH

tháng

năm

TRƯỞNG KHOA QL NGÀNH


LỜI CẢM ƠN
Trong khi thực hiện tập luận án này, tôi đã nhận được sự giúp đỡ tận tình
của Quý thầy cô và các bạn đồng nghiệp. Hôm nay, luận án đã hoàn thành, tôi
xin chân thành cám ơn :
- Thầy hướng dẫn khoa học :
PGS. TS. Lê Hoài Quốc
- Thầy chấm nhận xét 1 :
TS. Bùi Trọng Hiếu
- Thầy chấm nhận xét 2 :
TS. Trần Thiên Phúc
đã dành thời gian quý báu để duyệt và cho những ý kiến giúp hoàn thiện luận
án.
- Quý thầy cô giảng dạy các môn học trong khóa Cao học K14 đã nhiệt
tình chỉ bảo cho chúng tôi những kiến thức hữu dụng không những cho việc xây
dựng luận án mà còn cho công tác của chúng tôi sau này.

- Đồng nghiệp KS. Lê Đình Phong và Trần Đức Trọng đã hết lòng hỗ
trợ để tôi có thể hoàn thành nhiệm vụ
- Quý Thầy Cô là Chủ tịch, Phản biện và y viên Hội đồng đã dành
nhiều thời gian quý báu để nhận xét và tham gia Hội đồng chấm Luận án.

4


TÓM TẮT LUẬN VĂN

Các nội dung nghiên cứu chính trong luận văn bao gồm:
• Xây dựng giải thuật giải bài toán động học ngược vị trí của các loại tay
máy 6 bậc tự do bằng phương pháp tách nhóm ba khâu. Đó lá các loại tay
máy với ba khâu định vị có thể là: tọa độ Đề các (x, y, z), tọa độ trụ (α,
d2, d3), tọa độ cầu (β, γ, d3), tay máy dạng SCARA (ω1, ω2, d3), tay máy
toàn khớp bản lề (θ1, θ3, θ3). Ba khâu định hướng là RPY. Sử dụng phần
mềm Matlab để giải bài toán động học ngược vị trí trên tay máy tọa độ
Cartesian và tay máy tọa độ Cylindrical 6 bậc tự do.
• Xây dựng giải thuật giải bài toán động học ngược vị trí của các loại tay
máy 6 bậc tự do bằng phương pháp bình phương bé nhất.

5


MỤC LỤC
Tóm tắt luận văn …………………………………………………………………………………………………………………………5
Mục lục …………………………………………………………………………………………………………………………………………….6
Danh mục các hình ảnh …………………………………………………………………………………………………………..8
Chương 1: TỔNG QUAN VỀ NGƯỜI MÁY, TAY MÁY………………………………………11
1.1 Giới thiệu về người máy - tay máy……………………………………………………………………….12

1.1.1 Sơ lược về quá trình hình thành và phát triển robot ………………………….12
1.1.2 Robot công nghiệp (Industrial Robot – IR) …………………………………………..14
1.1.3 Tình hình tiếp cận và ứng dụng Robot công nhgiệp ở Việt Nam……15
1.1.4 Kết cấu cơ bản của robot công nghiệp ……………………………………………………..18
1.2 Giải thuật giải bài toán động học thuận tay máy…………………………………………….21
1.2.1 Mô tả vật rắn trong không gian ………………………………………………………………..22
1.2.2. Phân tích động học tay máy bằng phương pháp lượng giác…………….23
1.2.3. Phân tích động học tay máy bằng phương pháp ma trận …………………24
1.2.4. Mô tả chuyển động tay máy bằng phương pháp tọa độ thuần
nhất ……………………………………………………………………………………………………………………….28
1.2.4.1 Giới thiệu phương pháp tọa độ thuần nhất (homogeneous
coordinate) ………………………………………………………………………………………………….28
1.2.4.2 Mô tả quy tắc Denavit – Hartenberg …………………………………………..33
1.2.4.3 p dụng phương pháp tọa độ thuần nhất để giải bài toán
động học thuận ………………………………………………………………………………………35
Chương 2: BÀI TOÁN ĐỘNG HỌC NGƯC TAY MÁY (bài toán vị trí) và
MỤC TIÊU CỦA LUẬN VĂN………………………………………………………………………36
2.1 Phương pháp lượng giác ……………………………………………………………………………………………38
2.2 Phương pháp biến đổi ngược các ma trận thuần nhất 4x4 ……………………………41
2.3 Mục tiêu của luận văn…………………………………………………………………………………………………47
Chương 3: PHƯƠNG PHÁP TÁCH NHÓM BA KHÂU………………………………………...48

6


3.1 Giải thuật giải bài toán động học ngược tay máy bằng phương pháp tách
nhóm 3 khâu…………………………………………………………………………………………………………………….49
3.1.1 Trường hợp của tay máy Cartesian (3T3R)…………………………………………….57
3.1.2 Trường hợp của tay máy Tọa độ trụ (Cylindrical – R2T3R)…………….58
3.1.3 Trường hợp của tay máy Tọa độ cầu (Spherical – T2R)…………………….59

3.1.4 Trường hợp của tay máy SCARA – 2RT)…………………………………………………60
3.1.5 Trường hợp của tay máy toàn khớp bản lề (6R)…………………………………….61
3.2 Ví dụ minh họa
3.2.1 Xét tay máy Cartesian ……………………………………………...65
3.2.2Xét tay máy Cylindrical ……………………………………….....…73
Chương 4: PHƯƠNG PHÁP SỐ …………………………………………………………………………………………81
4.1 Giới thiệu về phương pháp số …………………………………………………………………………………82
4.1.1 Phương pháp chia đôi…………………………………………………………………………………….82
4.1.2 Phương pháp lặp……………………………………………………………………………………………….84
4.13 Phương pháp Niutơn (tiếp tuyến)……………………………………………………………….87
4.1.4 Phương pháp Gaoxơ……………………………………………………………………………………….90
4.1.5 Phương pháp bình phương bé nhất……………………………………………………………95
4.2 p dụng phương pháp bình phương bé nhất giải bài toán động học ngược
vị trí tay máy…………………………………………………………………………………………………………………95
Chương 5: KẾT LUẬN …………………………………………………………………………………………………………98
Phụ lục A …………………………………………………………………………………………………………………………………..101
Phụ lục B ……………………………………………………………………………………………………………………………………108
Tài liệu tham khảo ………………………………………………………………………………………………………………..115

7


DANH MỤC CÁC HÌNH ẢNH
Hình 1.1

Xe tự hành thám hiểm mặt trăng Lukonohod 1

Hình 1.2

Robot giải trí (chú chó AIBO của hãng Sony)

Hình 1.3

Một số mẫu robot sơn - hàn của Kuka

Hình 1.4

Một số mẫu robot công ngiệp

Hình 1.5

Các bộ phận cấu thành của robot

Hình 1.6

Vị trí và hướng của vật rắn trong không gian

Hình 1.7

Tay máy hai khâu phẳng

Hình 1.8

Chuyển đổi hệ tọa độ tịnh tiến

Hình 1.9

Chuyển đổi hệ tọa độ quay

Hình 1.10

Quay quanh trục z

Hình 1.11

Quay quanh trục y

Hình 1.12

Quay quanh trục x

Hình 1.13

Sự hình thành góc Euler ZYZ

Hình 1.14

Sự hình thành góc RPY

Hình 1.15

Biểu diễn các thông số động học theo quy tắc Denavit - Hartenberg

Hình 2.1

The Planar 3-R manipulator

Hình 2.2

Hai vị trí đáp ứng của tay máy

Hình 2.3

Các vị trí đáp ứng của khớp B

Hình 2.4

Tay máy 6 bậc tự do toàn khớp bản lề

Hình 3.1

Hai vị trí đáp ứng của Tay máy 3 khâu

Hình 3.2

Minh họa vị trí và hướng của đối tượng công tác so với hệ quy chiếu
cố định gắn với giá của tay máy

Hình 3.3

Sơ đồ thể hiện hướng của đối tượng công tác so với vị trí ban đầu
của Tay máy

Hình 3.4a

Tay máy Cartesian

Hình3.4b

Minh họa các hướng có thể tiếp cận đối tượng công tác của tay

máy Cartesian
8


Hình 3.5a

Tay máy Cylindrical

Hình 3.5b

Minh họa các hướng có thể tiếp cận đối tượng công tác của tay
máy Cylindrical

Hình 3.6a

Tay máy Spherical

Hình 3.6b

Minh họa các hướng có thể tiếp cận đối tượng công tác của tay
máy Spherical

Hình 3.7a

Tay máy SCARA

Hình 3.7b

Minh họa các hướng có thể tiếp cận đối tượng công tác của tay
máy SCARA

Hình 3.8a

Tay máy Articulated

Hình 3.8b

Minh họa các hướng có thể tiếp cận đối tượng công tác của tay
Máy Articulated

Hình 3.9

Không gian hoạt động của tay máy Cartesian

Hình 3.10

Không gian hoạt động của tay máy Cylindrical

Hình 3.11

Không gian hoạt động của tay máy Spherical

Hình 3.12

Không gian hoạt động của tay máy SCARA

Hình 3.13

Tọa độ của tay máy SCARA

Hình 3.14

Không gian hoạt động của tay máy Articulated

Hình 3.15

Lược đồ cơ cấu của Tay máy PUMA 6 bậc tự do liên kết toàn khớp
bản lề

Hình 3.16

Minh họa các hướng có thể tiếp cận đối tượng công tác của Tay
Máy

Hình 3.17

Minh họa một hướng tiếp cận của Tay máy đến đối tượng công tác

Hình 3.18

Lược đồ cơ cấu của Tay máy CARTESIAN 6 bậc tự do

Hình 3.19

Các hệ tọa độ của 3 khâu định vị

Hình 3.20

Các hệ tọa độ của 3 khâu định hướng

Hình 3.21

Lược đồ cơ cấu của Tay máy CYLINDRICAL 6 bậc tự do

Hình 3.22

Các hệ tọa độ của 3 khâu định vị
9


Hình 4.1

Sơ đồ khối của phương pháp chia đôi

Hình 4.2

Sơ đồ khối của phương pháp lặp

Hình 4.3

Sơ đồ khối của phương pháp Niutơn

Hình 4.4

Sơ đồ khối của phương pháp bình phương bé nhất

10


Chương 3: Phương pháp tách nhóm 3 khâu

Chương 3

PHƯƠNG PHÁP
TÁCH NHÓM 3 KHÂU

48


Chương 3: Phương pháp tách nhóm 3 khâu
3.1 GIẢI THUẬT GIẢI BÀI TOÁN ĐỘNG HỌC NGƯC TAY MÁY BẰNG
PHƯƠNG PHÁP TÁCH NHÓM 3 KHÂU
Để thuận tiện và dễ dàng trong việc thiết kế và điều khiển, các tay máy
thường được cấu tạo bởi các khâu động có một khả năng chuyển động – tức có một
bậc tự do. Vì vậy, với tay máy có số khâu động lớn hơn 3 – như đã phân tích ở
chương II – thì việc giải bài toán động học ngược thường cho kết quả là đa nghiệm.
Còn đối với tay máy 3 bậc tự do thì kết quả của bài toán động học ngược có số
nghiệm hữu hạn, và thường là 2. Chẳng hạn như 2 kết cấu tay máy dưới đây:

Hình 3.1 – Hai vị trí đáp ứng của Tay máy 3 khâu
Như vậy đối với tay máy chỉ có 3 khâu thì việc giải bài toán động học ngược
sẽ đơn giản hơn nhiều so với tay máy có nhiều hơn 3 bậc tự do. Do đó, trên cơ sở
này, phương pháp tách nhóm 3 khâu được đặt ra nhằm hạn chế tính đa nghiệm của
bài toán ngược.
Trong phạm vi giới hạn đề tài này là nghiên cứu các loại tay máy 6 bậc tự
do là loại dùng phổ biến trong công nghiệp, nên ta có thể phân cơ cấu tay máy
thành hai nhóm 3 khâu bao gồm: nhóm định vị và nhóm định hướng, rồi giải bài
toán động học ngược đối với n=3. Vấn đề đặt ra ở đây là chỗ nối giữa 2 nhóm
định vị và định hướng phải nằm trong miền với của nhóm tách trước đó. Tuy nhiên,
49

Chương 3: Phương pháp tách nhóm 3 khâu
trong thực tế công nghệ, chẳng hạn như hàn một đường hay gắp một vật thì bao
giờ ta cũng xác định được miền không gian hoạt động của tay máy, và như vậy vị
trí của điểm kết nối là có thể xác định được. Vì vậy phương pháp tách nhóm 3
khâu thích hợp cho việc giải quyết bài toán ngược trong những ứng dụng cụ thể.
Nội dung của bài toán ngược được đặt ra như sau:
-

Cho trước cơ cấu Tay máy 6 bậc tự do và hệ trục tọa độ cố định đặt tại giá 0
của Tay máy. Ba khâu định vị có thể là: tọa độ Đề các (x, y, z), tọa độ trụ
(α, d2, d3), tọa độ cầu (β, γ, d3), tay máy dạng SCARA (ω1, ω2, d3), tay máy
toàn khớp bản lề (θ1, θ3, θ3). Ba khâu định hướng là RPY

-

Cho trước tọa độ của đối tượng công tác (vật thể rắn) nằm trong vùng không
gian hoạt động của Tay máy được thể hiện bởi tọa độ của một điểm thuộc
đối tượng và các vectơ chỉ phương của đối tượng.

Lưu ý: tùy theo từng cơ cấu Tay máy cụ thể cho trước, các ràng buộc về kết cấu
động học và vị trí tương quan giữa các khâu của Tay máy sẽ được xác định trước
như là các giả thiết ban đầu trong quá trình giải bài toán ngược.
ym
x

z0
M

z

k
i0

O

j0
y0

x0

Hình 3.2: Minh họa vị trí và hướng của đối tượng công tác so với hệ quy chiếu cố định
gắn với giá của tay maùy.

50


Chương 3: Phương pháp tách nhóm 3 khâu

Fobject

⎡ nx
⎢n
=⎢ y
⎢ nz
⎢
⎣0

ox

oy

ax
ay

oz

az

0

0

Với n (Normal)

Px ⎤ ⎡ C (i0iM ) C (i0 j M ) C (i0 k M ) Px ⎤
Py ⎥⎥ ⎢⎢C ( j0iM ) C ( j0 j M ) C ( j0 k M ) Py ⎥⎥
=
Pz ⎥ ⎢C (k 0iM ) C (k 0 jM ) C (k 0 k M ) Pz ⎥
⎥
⎥ ⎢
1⎦ ⎣ 0
0
0
1⎦

: vectơ pháp tuyến

o (Orientation): vectơ có phương nắm bắt
a (Approach) : Vectơ có hướng tiếp cận với đối tượng

-

Yêu cầu: Xác định các biến khớp để Tay máy đạt được vị trí và hướng của
đối tượng công tác đã cho trong không gian hoạt động.
Để giải bài toán động học ngược theo phương pháp tách nhóm 3 khâu, ta

tiến hành theo các bước sau:
Bước 1: Xác định quan hệ về vị trí và hướng của đối tượng công tác so
với hệ trục tọa độ tuyệt đối gắn liền với giá cố định của tay máy. Ở đây vị trí và
hướng của đối tượng công tác đã được cho bởi ma trận Fobject.
Bước 2: Xác định biểu thức quan hệ giữa các góc định hướng của cổ tay
của Tay máy (Roll-Pitch-Yaw) với các góc định hướng của đối tượng công tác.
Giải bài toán ngược theo một hướng tiếp cận lựa chọn để xác định các góc quay
của cổ tay (RPY – Roll/xoay cổ tay, Pitch/gập cổ tay, Yaw/lắc cổ tay) và vị trí của
các khâu 6, 5, 4 của Tay maùy.

51


Chương 3: Phương pháp tách nhóm 3 khâu

Hình 3.3 – Sơ đồ thể hiện hướng của đối tượng công tác so với vị trí ban đầu
của Tay máy
Ta nhận thấy để đưa bộ phận làm việc của khâu tác động cuối (chẳng hạn
là tay gắp) đến điểm M của đối tượng công tác (thỏa mãn cả về vị trí và hướng của
đối tượng) thì tương ứng ta phải phối hợp chuyển động quay đồng thời cả ba khâu
4, 5, 6 theo những góc xác định để đưa điểm G của tay gắp đến vị trí mục tiêu M
sao cho các vectơ chỉ hướng của tay gắp trùng với các vectơ chỉ hướng của đối
tượng công tác. Khi đó ta có:

⎡ nx
⎢n
RPY (ϕ ,θ ,ψ ) = ⎢ y
⎢ nz
⎢
⎣0

ox
oy
oz

ax
ay
az

0

0

ox
oy
oz

ax
ay
az

0

0

0⎤
0⎥⎥
0⎥
⎥
1⎦

(3.1)

Hoaëc:

⎡ nx
⎢n
RPY (q4 , q5 , q6 ) = ⎢ y
⎢ nz
⎢
⎣0
52

0⎤
0⎥⎥
0⎥
⎥
1⎦

(3.2)


Chương 3: Phương pháp tách nhóm 3 khâu
Do đó ta coù:

⎡Cϕ Cθ
⎢S C
⎢ ϕ θ
⎢ − Sθ
⎢
⎣ 0

Cϕ Sθ Sψ − Sϕ Cψ
Sϕ Sθ Sψ + Cϕ Cψ
Cθ Sψ

Cϕ Sθ Cψ + Sϕ Sψ
Sϕ Sθ Cψ − Cϕ Sψ
Cθ Cψ

0

0

Trong ñoù:

0⎤ ⎡ nx
0⎥⎥ ⎢⎢n y
=
0⎥ ⎢ nz
⎥ ⎢
1⎦ ⎣ 0

ox

oy
oz

ax
ay
az

0

0

0⎤
0⎥⎥
0⎥
⎥
1⎦

+ ϕ = ϕ65 : chuyển động xoay cổ tay (Roll) – chuyển động quay quanh trục z
+ θ = ϕ54 : chuyển động gập cổ tay (Pitch) – chuyển động quay quanh trục y
+ ψ = ϕ43: chuyển động lắc cổ tay (Yaw) – chuyển động quay quanh trục x
Từ việc đồng nhất hai vế của phương trình trên, ta thu được 9 phương trình mà chỉ
có 3 ẩn số là ϕ,θ,ψ, nó thể hiện sự vô định nghiệm của bài toán.
Cϕ C θ = n x

Cθ Sψ = o z

Sϕ C θ = n y

CϕSθCψ + SϕSψ = ax

- Sθ = n z

SϕSθSψ - CϕSψ = ay

CϕSθSψ - SϕCψ = ox

CθCψ = az

(3.3)

SϕSθSψ + CϕCψ = oy
Trong trường hợp tổng quát thì khâu tác động cuối của Tay máy (tạm gọi là
tay gắp) có thể tiếp cận đến đối tượng công tác theo một phương bất kỳ. Tuy
nhiên, trong từng ứng dụng cụ thể, ta sẽ có phương tiếp cận hoàn toàn xác định. Do
đó, ta có thể giải bài toán ngược cho 3 khâu định hướng dựa theo một phương tiếp
cận trùng với một trong ba vectơ chỉ hướng của đối tượng công tác mà không ảnh
hưởng đến tính tổng quát của bài toán thực tế. Do đó để bài toán có nghiệm duy
nhất, ta chọn một hướng tiếp cận xác định cho tay máy trong ba hướng tiếp cận xm,
ym hoặc zm,ta có thể bỏ được 6 phương trình và xác định được lời giải duy nhất theo
từng hướng lựa chọn:

53


Chương 3: Phương pháp tách nhóm 3 khâu
• Giả sử ta chọn hướng tiếp cận theo phương trục xM (phương n - Normal),
tức là ta chỉ đồng nhất hướng của tay máy theo phương n (Normal) là đủ.
Do đó ta coù:
⎧CϕCθ = n x
⎪

⎨ Sϕ C θ = n y
⎪ − Sθ = n
z
⎩

(3.4)

• Hoặc chọn hướng tiếp cận theo phương trục yM (Phương o - Orientation),
tức là ta chỉ đồng nhất hướng của tay máy theo phương o (Orientation) là đủ.
⎧Cϕ .Sθ .Sψ − Sϕ .Cψ = o x
⎪
⎨Sϕ .Sθ .Sψ + Cϕ .Cψ = o y
⎪
Cθ .Sψ = o z
⎩

(3.5)

• Hoặc ta chọn hướng tiếp cận theo phươngtrục zM (Phương a - Approach),
tức là ta chỉ đồng nhất hướng của tay máy theo phương a (Approach) là đủ.
⎧ Cϕ .Sθ .Cψ + Sϕ .Sψ = a x
⎪
⎨Sϕ .Sθ .Cψ − Cϕ .Cψ = a y
⎪
Cθ .Cψ = a z
⎩

(3.6)

Giải hệ các hệ phương trình (3.4), (3.5), (3.6) ta sẽ có nghiệm duy nhất

ϕ,θ,ψ theo từng hướng tiếp cận. Kết hợp với kích thước động cho sẵn của tay máy,
ta suy ra được vị trí chính xác của điểm D (điểm kết nối giữa 3 khâu định vị và 3
khâu định hướng).
Cụ thể như sau:
 Từ G ≡ M, dọc theo hướng và ngược với chiều của zG ≡ zM ta xác định
đoạn GF = l6, thể hiện vị trí của khớp F theo hướng tiếp cận.
 Từ F, với góc θ = ϕ54 (chuyển động gập cổ tay - Pitch) và FE = l5 đã biết,
ta xác định được vị trí và hướng của khớp E.
 Từ E, với góc ϕ = ϕ43 (chuyển động lắc cổ tay – Yaw) và ED = l4 đã biết,
ta xác định được vị trí và hướng của khớp D.

54


Chương 3: Phương pháp tách nhóm 3 khâu

Hình 3.4a)- Tay máy Cartesian

b)- Minh họa các hướng có thể tiếp cận đối
tượng công tác của tay máy Cartesian

Hình 3.5 a)- Tay máy Cylindrical

b)- Minh họa các hướng có thể tiếp cận đối
tượng công tác của tay máy Cylindrical

55


Chương 3: Phương pháp tách nhóm 3 khâu

Hình 3.6 a)- Tay máy Spherical

b)- Minh họa các hướng có thể tiếp cận đối
tượng công tác của tay máy Spherical

Hình 3.7 a) - Tay máy SCARA

b)- Minh họa các hướng có thể tiếp cận đối
tượng công tác của tay máy SCARA

Hình 3.8 a) - Tay máy Articulated

b)- Minh họa các hướng có thể tiếp cận
đối tượng công tác của tay máy Articulated
56


Chương 3: Phương pháp tách nhóm 3 khâu
Ta nhận thấy, để bảo đảm sự linh hoạt của 3 khâu cuối (nhóm định hướng)
sao cho khâu tác động cuối có thể tương tác với đối tượng theo hướng bất kỳ thì
điểm D phải nằm trên hình cầu bán kính R1, với:
R1 = (l4+l5+l6 - ρ)
Hình cầu này là quỹ tích của các điểm D thuộc hai khâu 3 và 4, mà từ đó, ta
phải chọn ra điểm D* để khâu tác động cuối (hay tay gắp) có thể tiếp cận mục tiêu
cả về vị trí và hướng.
Tùy theo yêu cầu của từng bài toán mà ta lựa chọn khoảng dự trữ ρ và
điểm tiếp cận D* sao cho có thể đưa khâu tác động cuối đến điểm công tác M mà
không phải duỗi thẳng 3 khâu cuối.
Bước 3: Dựa trên các ràng buộc về kết cấu động học và vị trí tương quan

của Tay máy đã cho trước để xác định vị trí của các khâu 3, 2 và 1.
3.1.1 Trường hợp của tay máy Cartesian (3T3R)

Hình 3.9 – Không gian hoạt động của tay máy Cartesian
Ma trận xác định vị trí 3 khâu định vị của tay máy Cartesian
⎡0 − 1
⎢1 0
R
TP = TCarte = A1.A2.A3 = ⎢
⎢0 0
⎢
⎣0 0

57

0 d3 ⎤
0 d 2 ⎥⎥
1 d1 ⎥
⎥
0 1⎦

(3.7)


Chương 3: Phương pháp tách nhóm 3 khâu
Đây là tay máy Cartesian, không gian làm việc của nhóm định vị là phần
không gian giới hạn bởi hình hộp chữ nhật nên vị trí của điểm D được xác định do
chuyển động tịnh tiến dọc theo các trục của 3 khâu định vị, với một vị trí của điểm
D(xD,yD,zD) được xác định ở bước 2 thì chỉ có một nghiệm duy nhất của các biến
suy rộng là thỏa mãn, tức là: q1 = d1 = xD, q2 = d2 = yD, q3 = d3 = zD.

Như vậy bài toán động học ngược của tay máy Cartesian đã được giải một cách dễ
dàng theo phương pháp tách nhóm 3 khâu.
3.1.2 Trường hợp của tay máy Tọa độ trụ (Cylindrical – R2T3R)

Hình 3.10 - Không gian hoạt động của tay máy Cylindrical
Ma trận xác định vị trí 3 khâu định vị của tay maùy Cylindrical
R

TP = TARTI = A1.A2.A3
⎡ − Sα
⎢ Cα
R
TP = TCylind = A1.A2.A3 = ⎢
⎢ 0
⎢
⎣ 0

0 Cα
0 Sα
1 0
0 0

− Sα * d 2 ⎤
Cα * d 2 ⎥⎥
− d 3 + L1 ⎥
⎥
1
⎦

(3.8)

Đây là tay máy Cylindrical, không gian làm việc của nhóm định vị là phần
không gian giới hạn bởi mặt trụ. Trong đó, ba cột đầu mô tả hướng, cột cuối xác
định vị trí của hệ tọa độ. Rõ ràng, sự thay đổi hướng của tọa độ trụ là do sự quay
một góc α quanh trục z. Tuy nhiên, ở bài toán này ta chỉ quan tâm đến vị trí của

58


Chương 3: Phương pháp tách nhóm 3 khâu
gốc tọa độ gắn với điểm D – tức cột thứ 4, hơn nữa vì khâu 3 là khâu tịnh tiến nên
điểm D của 3 khâu cuối có thể dễ dàng kết nối với điểm D của 3 khâu đầu. Từ vị
trí của điểm D(xD,yD,zD) được xác định ở bước 2 ta có hệ phương trình sau:
⎧ − Sα .d 2 = x D
⎪
⎨ Cα .d 2 = y D
⎪− d + L = z
1
D
⎩ 3

(3.9)

Giải hệ phương trình (3.9) ta sẽ xác định được các biến khớp α, d2, d3 của nhóm
định vị. Kết hợp với điều kiện biên của tay máy ta có thể xác định được nghiệm
duy nhất cho các biến khớp.
3.1.3 Trường hợp của tay máy Tọa độ cầu (Spherical – T2R)

Hình 3.11 - Không gian hoạt động của tay máy Spherical
Ma trận xác định vị trí 3 khâu định vị của tay máy Spherical

⎡Cβ .Cγ
⎢ Sβ .Cγ
R
Tp = Tsph = ⎢
⎢ − Sγ
⎢
⎣ 0

− Sβ
Cβ
0
0

Cβ .Sγ
Sβ .Sγ
Cγ
0

Cβ .Sγ .d 3 − Sβ .L 2⎤
Sβ .Sγ .d 3 + Cβ .L 2⎥⎥
⎥
Cγ .d 3
⎥
1
⎦

(3.10)

Đây là tay máy Spherical, không gian làm việc của nhóm định vị là phần
không gian giới hạn bởi hình cầu. Trong đó, ba cột đầu mô tả hướng, cột cuối xác

định vị trí của hệ tọa độ. Rõ ràng, sự thay đổi hướng của tọa độ cầu là do sự quay

59


Chương 3: Phương pháp tách nhóm 3 khâu
một góc β quanh trục z và quay một góc γ quanh trục y. Tuy nhiên, ở bài toán này
ta chỉ quan tâm đến vị trí của gốc tọa độ gắn với điểm D – tức cột thứ 4. Tương tự
như tay máy tọa độ trụ ta có thể xác định được điểm kết nối D giữa 2 nhóm 3 khâu.
Từ vị trí của điểm D(xD,yD,zD) được xác định ở bước 2 ta có hệ phương trình sau:
⎧Cβ .Sγ .d 3 − Sβ .L 2 = x D
⎪
⎨Sβ .Sγ .d 3 + Cβ .L 2 = y D
⎪
Cβ .d 3 = z D
⎩

(3.11)

Giaûi hệ phương trình (3.11) ta sẽ xác định được các biến khớp β, γ, d3 của
nhóm định vị. Kết hợp với điều kiện biên của tay máy ta có thể xác định được
nghiệm duy nhất cho các biến khớp.
3.1.4 Trường hợp của tay máy SCARA – 2RT)

Hình 3.12 - Không gian hoạt động của tay máy SCARA

Hình 3.13 – Tọa độ của tay máy SCARA
Ma trận xác định vị trí 3 khâu định vị của tay máy SCARA:

60

Chương 3: Phương pháp tách nhóm 3 khâu
R

TP = TSCARA =

⎡Cω 1 .Cω 2 − Sω 1 .Sω 2
⎢Cω .Sω + Sω .Cω
2
1
2
⎢ 1
⎢
0
⎢
0
⎣

(3.12)
Cω 1 .Sω 2 + Sω 1 .Cω 2
Sω 1 .Sω 2 − Cω 1 .Cω 2
0
0

0 L 2(Cω 1 .Cω 2 − Sω 1 .Sω 2 ) + L1.Cω 1 ⎤
0 L 2( Sω 1 .Cω 2 + Cω 1 .Sω 2 ) + L1.Sω 1 ⎥⎥
⎥
−1
− d3

⎥
0
1
⎦

Đây là tay máy SCARA, không gian làm việc của nhóm định vị là phần
không gian giới hạn bởi hình trụ. Trong đó, ba cột đầu mô tả hướng, cột cuối xác
định vị trí của hệ tọa độ. Rõ ràng, sự thay đổi hướng của tọa độ SCARA là do sự
quay một góc ω1 và ω2 quanh trục z . Tuy nhiên, ở bài toán này ta chỉ quan tâm
đến vị trí của gốc tọa độ gắn với điểm D. – tức cột thứ 4. Tương tự như các tay
máy tọa độ trụ và tọa độ cầu ta có thể xác định được điểm kết nối D giữa 2 nhóm
3 khâu. Từ vị trí của điểm D(xD,yD,zD) được xác định ở bước 2 ta có hệ phương
trình sau:
⎧ L 2(Cω 1 .Cω 2 − Sω 1 .Sω 2 ) + L1.Cω 1 = x D
⎪
⎨ L 2( Sω 1 .Cω 2 + Cω 1 .Sω 2 ) + L1.Sω 1 = y D
⎪
− d3 = zD
⎩

Giải hệ phương trình (3.13) ta sẽ xác định được các biến khớp ω1, ω2, d3.
3.1.5 Trường hợp của tay máy toàn khớp bản lề (6R)

Hình 3.14 - Không gian hoạt động của tay maùy Articulated

61

(3.13)