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ố

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

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Ĩ

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

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

PHÒNG ĐÀO TẠO SAU ĐH ĐỘ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 Phái : nữ

Ngày tháng năm sinh : 18 tháng 3 năm 1965 Nơi sinh : Hà nội

Chuyên ngành : Chế tạo máy 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 tháng năm

TRƯỞNG PHÒNG ĐT- SĐH 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 :

- Thầy chấm nhận xét 1 :

- Thầy chấm nhận xét 2 :

đã 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à Uû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

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

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 Aù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

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 Aù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

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

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

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ị

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

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,

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

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

0 0

) (

) (

) (

) (

) (

) (

) ( ) ( ) (

1 0 0 0

0 0

0

0 0

0

0 0

0

z M M

M

y M

M M

x M

M M

z z z z

y y y y

x x x x

object

P k

k C j

k C i

k C

P k

j C j

j C i

j C

P k

i C j

i C i

i C P

a o n

P a o n

P a o n F

Với n (Normal) : 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:

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

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 máy

0 0

0 )

, , (

z z z

y y y

x x x

a o n

a o n

a o n

0 0

0 )

, ,

z z z

y y y

x x x

a o n

a o n

a o n q

q q RPY

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ó:

−

1 0 0 0

0 0 0

1 0

0 0

0 0 0

z z z

y y y

x x x

a o n

a o n

a o n C

C S

C S

S C C

S S C C S

S S C

S

S S C S C C

S S S C C

C

ψ θ ψ

θ θ

ψ ϕ ψ

θ ϕ ψ ϕ ψ

θ ϕ θ

ϕ

ψ ϕ ψ θ ϕ ψ

ϕ ψ θ ϕ θ

ϕ

Do đó ta có:

+ ϕ = ϕ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

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:

• 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à đủ

n S

n C S

n C C

θ

θϕ

θϕ

=

−

z y x

o S C

o C C S S S

o C S S S C

ψθ

ψϕψθϕ

ψϕψθϕ

.

.

.

.

z y x

a C C

a C C C S S

a S S C S C

ψθ

ψϕψθϕ

ψϕψθϕ

.

.

.

.

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

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

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

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

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

100

001

010

1 2 3

d d d

(3.7)

Đâ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 máy Cylindrical

00

010

*0

*0

1 3 2 2

L d

d C S C

d S C

S

ααα

αα

α

(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

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:

z L d

y d C

x d S

1 3 2

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

00

.0

2

2

3 3 3

d C C

S

L C d S S S S C C S

L S d S C S C S C

C

γγ

γ

βγ

βγββγβ

βγ

βγββγ

β

(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

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:

=

−

D D D

z d C

y L C d S S

x L S d S C

3 3 3

.

2

.

2

.

β

βγ

β

βγ

β

(3.11)

Giả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:

+

−+

−

10

00

10

0

.1)

(20

.1)

(20

3

1 2

1 2 1 2

1 2 1 2 1 2

1

1 2

1 2 1 2

1 2 1 2 1 2

1

d

S L S

C C

S L C

C S S C S S

C

C L S

S C C L C

S S C S S C

C

ωω

ωω

ωω

ωωωωωω

ω

ωω

ωωωω

ωωωω

ωω

ω

Đâ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:

+

= +

−

D

D D

z d

y S L S

C C

S L

x C L S

S C C L

3

1 2

1 2 1

1 2

1 2 1

1 )

( 2

1 )

( 2

ωω

ωω

ω

ωω

ωωω

(3.13)

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 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

φ ψ

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

A

E C

B

Nhóm định vị gồm 3 khâu đầu của Tay máy, với liên kết gồm các khớp bản

lề ở A, B và C, không gian làm việc của nhóm là phần không gian giới hạn bởi

mặt cầu Do 3 khớp A, B và C là khớp bản lề, do đó điểm D của 3 khâu cuối rất khó tiếp cận với điểm D của 3 khâu đầu Nên trong trường hợp này ta có thể chọn tọa độ điểm D theo từng hướng tiếp cận nằm trong vùng giao của mặt cầu với không gian làm việc của 3 khâu RPY là một mặt cầu Từ tọa độ điểm D này, ta dùng phương pháp tách nhóm 3 khâu để xác định các biến khớp cho 3 khâu đầu và

3 khâu cuối

Cụ thể như sau:

• Xác định các biến khớp của 3 khâu định vị:

3

6

0

0

3 2 3 2 3

2 3 2

2 3

2 1 3 2 1 3

2 1 3 2

1

1 3

2 1 3 2 1 3

2 1 3 2

1

θθθθθ

θθθ

θθ

θθθθθθ

θθθθ

θ

θθ

θθθθθθ

θθθθ

θ

C C S S S

C C S

C C

S S S C S S

S S C C

S

S C

S C S C C S

S C C C

.(

)

(

)

(

1 2 3 2 3 2 3

2 1 2 3 2 1 3 2 1 3

2 1 2 3 2 1 3 2 1 3

θθ

θθθ

θθθ

θθθθθ

θθθ

θθθθθ

S a S

C C S a

C S a S

S S C C S a

C C a S

S C C C C a

(3.14)

Đây là tay máy Articulated, 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 khuyết 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 độ

Articulated là do sự quay một góc θ1, θ2, θ3 quanh trục x 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ừ vị trí của điểm D(xD,yD,zD) được chọn ở bước 2 ta có hệ phương trình sau:

+

=+

−

=+

−

D D D

z S a S

C C S a

y C S a S

S S C C S a

x C C a S

S C C C C a

2 2 3 2 3 2 3

2 1 2 3 2 1 3 2 1 3

2 1 2 3 2 1 3 2 1 3

.)

.(

)

(

)

(

θθ

θθθ

θθθ

θθθθθ

θθθ

θθθθθ

(3.15)

Giải hệ phương trình (3.15) ta sẽ xác định được các biến khớp θ1, θ2, θ3 của nhóm định vị

• Xác định các biến khớp của 3 khâu định hướng:

+ Tiếp cận theo phương xM

31[

]21[]

21[

]11[]

11[1 1 1

D M S

D M C S

D M C C

θ

θϕ

θϕ

32[

]22[]

22[

]12[]

12[

1 1 1

D M S

C

D M C C S S S

D M C

S S S C

ψθ

ψϕψθϕ

ψϕψθϕ

(3.17)

+ Tiếp cận theo phương zM

−

−

−

]33[]

33[

]23[]

23[

]13[]

13[

1 1 1

D M C C

D M C C C S S

D M S S C S C

ψθ

ψϕψθϕ

ψϕψθϕ

3.2 VÍ DỤ MINH HỌA

3.2.1 XÉT TAY MÁY CARTESIAN

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

3.2.1.1 Giải bài toán động học thuận

a/- Xét 3 khâu định vị

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

Bảng 3.1 – Các thông số DH của 3 khâu định vị

010

0100

0001

010

0001

0100

010

0001

0100

100

001

010

1 2 3

d d d

b/- Xét 3 khâu định hướng:

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

Bảng 3.2 – Các thông số DH của 3 khâu định hướng

00

00

10

.0

.0

4 4 4 4

4 4 4 4

θθ

θ

θθ

θ

S L C

S

C L S C

010

00

00

5

5 5

5 5

L

S C

C S

θθ

θθ

100

00

00

6

6 6

6 6

L

C S

S C

θθ

θθ

Từ các ma trận trên ta tính được ma trận tuyệt đối của 3 khâu định hướng:

3T6 = A4.A5.A6

Ký hiệu 3T6 như sau:

3T6 =

[ ] [ ] [ ] [ ] [ ] [ ] [ ] [ ] [ ] [ ] [ ] [ ] [ ] [ ] [ ] [ ]⎥⎥

4241

3433

3231

2423

2221

1413

1211

T T

T T

T T

T T

T T

T T

T T

T T

Với:

T[11] =- cos(θ4).sin(θ5).cos(θ6)-sin(θ4).sin(θ6)

T[12] = cos(θ4).sin(θ5).sin(θ6)-sin(θ6).cos(θ4)

T[13] = cos(θ4).cos(θ5)

T[14] = cos(θ4).cos(θ5).L6-sin(θ4).L5+L4.cos(θ4)

T[21] = sin(θ4).sin(θ5).cos(θ6)+cos(θ4).sin(θ6)

T[22] = sin(θ4).sin(θ5).sin(θ6)+cos(θ4).cos(θ6)

4241

3433

3231

2423

2221

1413

1211

M M

M M

M M

M M

M M

M M

M M

M M

(3.19)

Với:

M[11] = sin(θ4).sin(θ5).cos(θ6)-cos(θ4).sin(θ6)

M[12] = -sin(θ4).sin(θ5).sin(θ6)-cos(θ4).cos(θ6)

M[13] =-sin(θ4).cos(θ5)

M[14] = -sin(θ4) cos(θ5).L6 -L5.cos(θ4) - L4.sin(θ4) + d3

M[21] = -cos(θ4).sin(θ5).cos(θ6)-sin(θ4).sin(θ6)

M[22] = cos(θ4).sin(θ5).sin(θ6)-sin(θ4).cos(θ6)

M[23] = cos(θ4).cos(θ5)

M[24] = cos(θ4).cos(θ5).L6-sin(θ4).L5+ L4.cos(θ4))+d2

M[31] = -cos(θ5).cos(θ6)

7382.03090.03640.08787.0

1252.19187.01249.03748.0

5058.02462.09230.02958.0

3.2.1.2 Giải bài toán động học ngược

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 M_end

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 máy

1)- Xác định các góc định hướng RPY:

3T6 =

[ ] [ ] [ ] [ ] [ ] [ ]

y y y

x x x

a o n

a o n

a o n T

T T

T T

T

T T

T

33 32

31

23 22

21

13 12

3748 0

.

2958 0

.

6 5

6 4 6 5 4

6 4 6 5 4

θθ

θθθθθ

θθθθθ

C C

S C C S S

S S C S C

−

=

−

3640 0

1249 0

.

9230 0

.

6 5

6 4 6 5 4

4 6 6 5 4

θθ

θθθθθ

θθθθθ

S C

C C S S S

C S S S C

9187 0

2462 0

5

5 4

5 4

θ

θθ

θθ

S

C S

C C

(3.22)

Theo hướng tiếp cận này ta thấy khi giải hệ phương trình (3.22) ta chỉ xác định được 2 góc RPY, đồng thời ta thu được 2 nghiệm cho 2 góc RPY Do đó ta không chọn hướng tiếp cận này

θ4 = 1.8326 và θ4 = -1.3090

θ5 = 0.3142 và θ5 = 2.8274

2)- Xác định vị trí các khâu 6, 5, 4:

* Xác định khâu 6 – điểm F:

Ta có: F = M_end 1

6

5A− (3.23)

a)- Tiếp cận theo phương x

Thay các giá trị chiều dài khâu L6 và góc θ6 theo hướng tiếp cận vào ma trận (3.23) ta được ma trận xác định hướng và vị trí của điểm F:

00

8000.03090.00997.09458.0

9414.09187.02887.02697.0

5551.02462.09522.01808.0

F

b)- Tiếp cận theo phương y

Thay các giá trị chiều dài khâu L6 và góc θ6 theo hướng tiếp cận vào ma trận (3.23) ta được ma trận xác định hướng và vị trí của điểm F:

00

8000.03090.08687.03872.0

9414.09187.01672.03579.0

5551.02462.04663.08497.0

a)- Tiếp cận theo phương x

Thay các giá trị chiều dài khâu L5 và góc θ5 theo hướng tiếp cận vào ma trận (3.24) ta được ma trận xác định hướng và vị trí của điểm E:

00

7801.00997.09458.03090.0

8837.02887.02697.09187.0

3646.09522.01808.02462.0

E

b)- Tiếp cận theo phương y

Thay các giá trị chiều dài khâu L5 và góc θ5 theo hướng tiếp cận vào ma trận (3.24) ta được ma trận xác định hướng và vị trí của điểm E:

00

6263.08687.04671.01651.0

9080.01672.05903.07897.0

6483.04663.06584.05909.0

E

* Xác định điểm D, điểm kết nối giữa 3 khâu định hướng và 3 khâu định vị

a)- Tiếp cận theo phương x

Ta có: D =M_end 1

6

3T− (3.25) Thay các giá trị chiều dài khâu L4, L5, L6 và 3 góc RPY theo hướng tiếp cận vào ma trận (3.25) ta được ma trận xác định hướng và vị trí của điểm D:

00

7183.09458.01131.03044.0

0674.12697.07957.05423.0

3154.01808.05950.07831.0

D

b)- Tiếp cận theo phương y:

Tương tự như trên, ta xác định được ma trận của điểm D theo hướng tiếp cận

00

6953.04671.07440.04778.0

0659.15903.06647.04580.0

7665.06584.00681.07496.0

D

O Bước 3 - Xác định các biến khớp của 3 khâu định vị

a)- Tiếp cận theo phương x

d1 = 0.3154 d2 = 1.0674 d3 = 0.7183

b)- Tiếp cận theo phương y

a)- Tiếp cận theo phương x

So sánh với điều kiện biên, ta nhận thấy theo phương tiếp cận này 3 biến đều thỏa

b)- Tiếp cận theo phương y

So sánh với điều kiện biên, ta nhận thấy theo phương tiếp cận này 3 biến đều thỏa

3.2.2 XÉT TAY MÁY CYLINDRICAL

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

3.2.2.1 Giải bài toán động học thuận

a/- Xét 3 khâu định vị

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

Bảng 3.3 – Các thông số DH của 3 khâu định vị

010

00

00

1

1 1

1 1

L

C S

S C

θθ

θθ

010

0100

0001

010

0001

0100

3

d

Từ các ma trận trên ta tính được ma trận tuyệt đối:

010

.0

.0

1 3

2 1 1

1

2 1 1

1

L d

d C S

C

d S C

S

θθ

θ

θθ

θ

b/- Xét 3 khâu định hướng

Ma trận tuyệt đối của 3 khâu định hướng RPY 3T6 = A4.A5.A6 tương tự như

tay máy Cartesian

Từ đây ta tính được ma trận tuyệt đối xác định điểm cuối G ≡ M:

0TM = 0T3 3T6

Ký hiệu 0T6 như sau:

0TM =

[ ] [ ] [ ] [ ] [ ] [ ] [ ] [ ] [ ] [ ] [ ] [ ] [ ] [ ] [ ] [ ]⎥⎥

4241

3433

3231

2423

2221

1413

1211

M M

M M

M M

M M

M M

M M

M M

M M

(3.26)

Với:

M[11] = -sin(θ1).(-cos(θ4).sin(θ5).cos(θ6)

- sin(θ4).sin(θ6))-cos(θ1).cos(θ5).cos(θ6)

M[12] =-sin(θ1).(cos(θ4).sin(θ5).sin(θ6)

- sin(θ4).cos(θ6))+cos(θ1).cos(θ5).sin(θ6)

M[13] = -sin(θ1).cos(θ4).cos(θ5)-cos(θ1).sin(θ5)

M[14] = -sin(θ1).(cos(θ4).cos(θ5).L6-sin(θ4).L5+L4.cos(θ4))

- cos(θ1).sin(θ5).L6-sin(θ1).d2

M[21] = cos(θ1).(-cos(θ4).sin(θ5).cos(θ6)

- sin(θ4).sin(θ6))-sin(θ1).cos(θ5).cos(θ6)

M[22] = cos(θ1).(cos(θ4).sin(θ5).sin(θ6)

- sin(θ4).cos(θ6))+sin(θ1).cos(θ5).sin(θ6)

M[23] = cos(θ1).cos(θ4).cos(θ5)-sin(θ1).sin(θ5)

M[24] = cos(θ1).(cos(θ4).cos(θ5).L6-sin(θ4).L5

+ L4.cos(θ4))-sin(θ1).sin(θ5).L6+cos(θ1).d2

M[31] = -sin(θ4).sin(θ5).cos(θ6)+cos(θ4).sin(θ6)

3954.03448.09300.01276.0

8852.06030.01153.07894.0

5076.07194.03491.06005.0

3.2.2.2 Giải bài toán động học ngược

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 M_end

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 máy