Nghiên cứu, xây dựng và mô phỏng hệ thống điều khiển cánh tay robot

Phương pháp điều khiển PD bù trọng lực đã thỏa mãn được yêu cầu về đáp ứng tốc độ nhanh, đảm bảo đạt được tốc độ đặt, hệ thống hoạt động khá ổn định khi biết chính xác các thông số vật l

Lời cam đoan

Em xin cam đoan luận văn này là do em nghiên cứu, thiết kế dưới sự hướng dẫn

của TS Nguyễn Cảnh Quang Các số liệu và kết quả trong đề tài là hoàn toàn trung

thực

Để hoàn thành bản đồ án này, em chỉ sử dụng những tài liệu tham khảo đã được ghi trong bảng các tài liệu tham khảo, không sử dụng tài liệu nào khác mà không được liệt kê ở phần tài liệu tham khảo

Học Viên

Nguyễn Văn Hà

Lời cam đoan 1

Mở đầu 6

Chương 1 8

TỔNG QUAN CÁC HỆ THỐNG ROBOT 8

1.1 Giới thiệu chung 8

1.1.1 Cấu tạo Robot 9

1.1.2 Các chuyển động của Robot 10

1.1.3 Các cấu hình cơ bản của Robot 10

1.1.4 Hệ thống truyền động 13

1.1.5 Hệ thống cảm biến 14

1.1.6 Các phương pháp điều khiển tự động cho Robot 14

1.2 Phân loại Robot công nghiệp 15

1.3 Ứng dụng của Robot 17

1.3.1 Ưu điểm của Robot 17

1.3.2 Các xu thế ứng dụng của Robot 17

1.4 Mục tiêu và phạm vi nghiên cứu của luận văn 18

Chương 2 19

PHƯƠNG TRÌNH ĐỘNG LỰC HỌC ROBOT 19

2.1 Xây dựng phương trình động lực học cho hệ Robot 19

2.1.1 Vận tốc và gia tốc của robot 21

2.1.2 Động năng của vi phân khối lượng dm 24

2.1.3.Thế năng của Robot 26

2.1.4 Phương trình động lực học của hệ robot 27

2.2 Các đặc tính của phương trình động lực học Robot 27

2.2.1 Tính đối xứng lệch và đặc tính thụ động 28

2.2.2 Giới hạn trên của ma trận quán tính 29

2.2.3 Đặc tính tuyến tính của tham số 30

2.3 Phương trình động lực học cho Robot Scara 30

2.3.1 Giới thiệu về Robot Scara 30

2.3.2 Phương trình động lực học cho Robot Scara 38

Chương 3 47

HỆ ĐIỀU KHIỂN ROBOT 47

3.1.1 Giới thiệu về điều khiển hệ tuyến tính và hệ phi tuyến 47

3.1.2 Tính ổn định của hệ thống 48

3.1.3 Ổn định Lyapunov 50

3.2 Các phương pháp điều khiển cho hệ phi tuyến 52

3.2.1 Các phương pháp truyền thống 52

3.2.2 Phương pháp điều khiển hiện đại 56

3.3 Hệ điều khiển Robot Scara 58

3.3.1 Mô hình Robot SCARA 58

3.3.2 Bộ điều khiển PD bù trọng lực[1] 59

3.3.3.Xây dựng bộ điều khiển PD bù trọng lực cho Robot SCARA 61

3.4 Kết luận 63

Chương 4 64

MÔ PHỎNG TRÊN MATLAB SIMULINK 64

4.1 Thông số của hệ thống 64

4.2 Kết quả mô phỏng trên Matlab-Simulink 64

4.2.1 Xây dựng sơ đồ khối cấu trúc simulink 65

4.2.2 Kết quả mô phỏng trên Matlab-Simulink 67

Kết luận và hướng phát triển 79

1 Kết luận 79

2 Hướng phát triển 80

TÀI LIỆU THAM KHẢO 86

Danh mục hình vẽ đồ thị

Hình 2.3: Bảng tham số động học của Robot Scara 33

Hình 3.2: Minh họa khái niệm ổn định Lyapunov 52 Hình 3.3: Mô hình kiểu động cơ kích từ độc lập 54

Hình 3.5: Sơ đồ khối hàm truyền chuyển động một bậc tự do 56

Hình 3.8: Sơ đồ điều khiển PD có bù trọng lực 61 Hình 4.1: Bảng thông số vị trí ban đầu của cánh tay Robot Scara 64 Hình 4.2: Bảng thông số vị trí cần đạt tới của cánh tay Robot Scara 64 Hình 4.3: Sơ đồ khối mô phỏng Robot Scara ( 4 bậc tự do) 66 Hình 4.4: Đáp ứng thời gian của góc q1(1) khi Kp=60; KD=8 67 Hình 4.5: Đáp ứng thời gian của góc q1(1) khi Kp=100; KD=10 67 Hình 4.6: Đáp ứng thời gian của q2(2) khi Kp=8; KD=1.2 68 Hình 4.7: Đáp ứng thời gian góc q2(2) khi Kp=20; KD=1.4 68 Hình 4.8: Đáp ứng thời gian của d3 khi Kp=200; KD=50 69 Hình 4.9: Đáp ứng thời gian của d3 khi Kp=300; KD=60 69 Hình 4.10: Đáp ứng thời gian góc q4 (4) khi Kp=30; KD=2 70

Hình 4.11: Đáp ứng thời gian góc q1, q2, q4 khi mt=1kg 70

Hình 4.14: Đáp ứng thời gian của góc q1 khi mt=1kg 72 Hình 4.15: Đáp ứng thời gian của góc q1 khi mt=1kg 72 Hình 4.16: Đáp ứng của d3 khi không biết chính xác khối lượng với giá trị

Hình 4.19: Đáp ứng của d3 khi có tải (m=2kg)

Hình 4.20: Sơ đồ khối mô phỏng Robot Scara (với luật điều khiển PID) 76 Hình 4.21: Đáp ứng của d3

Mở đầu

Theo quá trình phát triển của xã hội, nhu cầu nâng cao sản xuất và chất lượng sản phẩm ngày càng đòi hỏi ứng dụng rộng rãi các phương tiện tự động hóa sản xuất Xu hướng tạo ra những dây chuyền và thiết bị tự động có tính linh hoạt cao đã hình thành và phát triển mạnh mẽ…Vì thế ngày càng tăng nhanh nhu cầu ứng dụng người máy để tạo ra các hệ sản xuất tự động linh hoạt

Robot ứng dụng rộng rãi và đóng vai trò quan trọng trong sản xuất cũng như trong đời sống Robot là cơ cấu đa chức năng có khả năng lập trình được dùng để di chuyển nguyên vật liệu, các chi tiết, các dụng cụ thông qua các truyền động được lập trình trước Khoa học robot chủ yếu dựa vào các phép toán về đại số ma trận

Việc điều khiển cánh tay Robot đòi hỏi tính chính xác cao, ổn định là rất quan trọng, Giải quyết bài toán này trong thực tế có nhiều công trình nghiên cứu về điều khiển như điều khiển PD bù trọng lực, điều khiển thích nghi, điều khiển trượt, logic mờ dùng để điều khiển cánh tay Robot có tính chính xác và ổn định cao Phương pháp điều khiển PD bù trọng lực đã thỏa mãn được yêu cầu về đáp ứng tốc

độ nhanh, đảm bảo đạt được tốc độ đặt, hệ thống hoạt động khá ổn định khi biết chính xác các thông số vật lý của hệ, đặc biệt là kết cấu bộ điều khiển đơn giản, khối lượng tính toán ít

Đó cũng chính là lý do tác giả lựa chọn đề tài “Nghiên cứu, xây dựng và mô

phỏng hệ thống điều khiển cánh tay Robot”

Luận văn này được chia thành 4 chương với những nội dung cơ bản được tóm tắt như sau:

- Chương 1: Giới thiệu tổng quan về các hệ thống Robot Nội dung

chính của chương này là giới thiệu tổng quan cấu trúc, mô hình và phương pháp điều khiển cũng như ứng dụng của Robot trong cuộc sống của con người

- Chương 2: Xây dựng phương trình động lực học cho Robot SCARA

Nội dung của chương này là tính toán, xây dựng các phương trình động

- Chương 3: Giới thiệu hệ điều khiển thường sử dụng để điều khiển Robot Nội dung của chương trình bày thiết kế bộ điều khiển theo

phương pháp PD bù trọng lực

- Chương 4: Mô phỏng cánh tay Robot Scara theo phương pháp PD bù

trọng lực trong môi trường Matlab-Simulink, qua đó rút ra nhận xét và kết luận

Kết luận và hướng phát triển:

Tóm tắt các kết quả đã đạt được, đồng thời phân tích mặt tồn tại và đề xuất hướng phát triển của luận văn

Do thời gian và trình độ có hạn nên bản luận văn không tránh khỏi những khiếm khuyết cần phải hoàn thiện thêm Tác giả rất mong nhận được sự góp ý, chỉ dẫn của các thầy cô và các bạn đồng nghiệp

Tác giả xin chân thành cảm ơn TS Nguyễn Cảnh Quang, Giảng viên Bộ

môn Đo lường và các hệ thống điều khiển, Khoa Điện, Trường Đại học Bách Khoa

Hà Nội đã giúp đỡ tác giả hoàn thành luận văn này

Chương 1 TỔNG QUAN CÁC HỆ THỐNG ROBOT 1.1 Giới thiệu chung[2]

Nhu cầu nâng cao chất lượng sản phẩm ngày càng đòi hỏi ứng dụng rộng rãi các phương tiện tự động hóa sản xuất Xu hướng tạo ra các dây chuyền về thiết bị tự động có tính linh hoạt cao đang hình thành

Thuật ngữ “Robot” xuất hiện năm 1922 trong tác phẩm “Rossum’s Universal Robot” của Karel Capek Hơn 20 năm sau, ở Hoa Kỳ đã xuất hiện những tay máy chép hình điều khiển từ xa trong các phòng thí nghiệm về vật liệu phóng

xạ Vào giữa những năm 50 thì xuất hiện các loại tay máy chép hình thủy lực và điện từ như tay máy Minotaur I hoặc Handyman của General Electric…

Robot công nghiệp được đưa vào ứng dụng đầu tiên vào năm 1961 tại một nhà máy ô tô của General Motor tại Trenton, New Jersey, Hoa Kỳ

Năm 1967, Nhật Bản mới nhập chiếc robot công nghiệp đầu tiên từ công ty AMF Hoa Kỳ và đến năm 1990 có hơn 40 công ty Nhật Bản đưa ra các loại robot nổi tiếng

Những năm 70, việc nghiên cứu nâng cao tính năng của Robot đã chú ý nhiều đến

sự lắp đặt thêm các loại cảm biến ngoại tín hiệu để nhận biết môi trường làm việc Tại trường đại học tổng hợp Stanford, người ta đã tạo ra loại Robot lắp ráp tự động điều khiển bằng máy vi tính trên cơ sở xử lý thông tin từ cảm biến và thị giác

Từ những năm 80 và 90, do sự áp dụng rộng rãi các tiến bộ khoa học kỹ thuật, nhất

là vi xử lý và công nghệ thông tin, số lượng Robot công nghiệp tăng lên, giá thành giảm và tính năng thì vượt trội Nhờ vậy, Robot công nghiệp có vị trí quan trọng trong các dây chuyền tự động sản xuất hiện đại

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

Ứng dụng của Robot: nhằm nâng cao năng suất dây chuyền công nghệ, giảm giá thành, nâng cao chất lượng và khả năng cạnh tranh của sản phẩm đồng thời cải

thiện điều kiện lao động Thông thường, robot công nghiệp được dùng trong các lĩnh vực: đúc, gia công áp lực, hàn và nhiệt luyện, gia công và lắp rắp…

1.1.1 Cấu tạo Robot

Robot có cấu tạo mô phỏng theo những đặc điểm cấu tạo cơ bản của cánh tay người Cũng có thể hiểu robot là tập hợp các bộ phận và cơ cấu cơ khí được thiết kế

để hình thành các khối có chuyển động tương đối với nhau, được gọi là các khâu động Trong đó phần liên kết giữa các khâu động được gọi là các khớp động hay còn gọi là chuyển động để vận hành tay máy như động cơ điện, xylanh dầu ép, xylanh khí nén, … Phần quan trọng khác trên các tay máy là bộ phận hay khâu tác động cuối để thao tác trên đối tượng làm việc – thường là các tay gắp hoặc các đầu công cụ chuyên dùng

Tay máy (cánh tay cơ khí của Robot công nghiệp) thông thường là một chuỗi động hở được tạo thành từ nhiều khâu được kết nối với nhau nhờ các khớp động Khâu cuối của tay máy thường có dạng một tay gắp hoặc được gắn dụng cụ công tác Mỗi khâu động trên tay máy có nguồn dẫn động riêng, năng lượng và chuyển động truyền đến cho chúng được điều khiển trên cơ sở tín hiệu nhận được từ bộ phận phản hồi là các cảm biến nhằm thông báo trạng thái hoạt động của các khâu chấp hành, trong đó vấn đề được đặc biệt quan tâm là vị trí vận tốc dịch chuyển của khâu cuối – khâu thể hiện kết quả tổng hợp các chuyển động của các khâu thành phần

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

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

1.1.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 (degrees of freedom) Bậc tự do là số khả năng chuyển động của một cơ cấu (chuyển động quay hoặc tịnh tiến) Nói chung Robot là một cơ cấu hở, do đó bậc tự do của nó có thể tính theo công thức :

6

i i ip n

1.1.3 Các cấu hình cơ bản của Robot

- Robot ba khớp tọa độ vuông góc Kiểu này gọi là Robot Đề các Vị trí tâm

cổ tay của loại này có thể xác định theo ba tọa độ vuông góc liên quan tới ba khớp lăng trụ Vùng không gian làm việc là khối hộp chữ nhật

Robot hoạt động trong hệ tọa độ này được minh họa như hình 1.2 gồm ba chuyển động định vị X,Y,Z theo các trục tọa độ vuông góc Ứng dụng chính của robot loại này là các thao tác vận chuyển vật liệu, sản phẩm, đúc, dập,…

Ưu điểm:

- Không gian làm việc lớn, có thể dài đến 20m

- Đối với loại gắn trên trần sẽ dành được diện tích sàn lớn cho các công việc khác

-Hệ thống điều khiển đơn giản

Hạn chế:Việc thêm vào các loại cần trục hay các loại thiết bị vận chuyển vật

liệu khác trong không gian làm việc của robot không được thích hợp lắm Việc duy trì vị trí của các cơ cấu dẫn động và các thiết bị dẫn động điện đối với loại trên đều gặp nhiều trở ngại

- Robot trụ Nếu khớp thứ nhất hoặc thứ hai của robot Đề các được thay

bằng khớp quay Vị trí tâm cổ tay của robot trụ có thể được xác định bằng tập hợp các tọa độ cầu liên hệ với ba biến khớp Các khớp lăng trụ thường được giới hạn cơ học ở cả hai đầu Do đó, không gian làm việc của robot trụ bị giới hạn bằng hai hình trụ đồng tâm có chiều dài hữu hạn

Hình 1.2 Robot tọa độ trụ

Trong ba chuyển động chính, robot được trang bị hai chuyển động tịnh tiến

và một chuyển động quay

Ưu điểm:

- Có khả năng chuyển động ngang và sâu vào trong các máy sản xuất

- Cấu trúc theo chiều dọc của máy để lại nhiều khoảng trống cho sàn

- Kết cấu vững chắc, có khả năng mang tải lớn

- Khả năng lặp lại tốt

Nhược điểm: bị giới hạn tiến về phía trái và phía phải do kết cấu cơ khí và

giới hạn các kích cỡ của cơ cấu tác động theo chiều ngang

- Robot cầu Nếu hai khớp đầu là khớp quay khác nhau và khớp thứ 3 là

khớp lăng trụ Nói chung, khớp lăng trụ không song song với trục của khớp thứ 2

Vị trí tâm cổ tay của robot cầu là tập hợp các tọa độ cầu có liên quan với ba biến khớp nối Do đó, không gian làm việc của robot cầu được giới hạn bằng hai khối cầu đồng tâm

Hình 1.3 Robot tọa độ cầu

Robot loại này được bố trí có ít nhất hai chuyển động quay trong ba chuyển động định vị

Không gian làm việc của robot này phức tạp, thường có tiết diện hình xuyến

- Robot SCARA Là loại Robot đặc biệt, gồm hai khớp quay liên tiếp, khớp

tiếp theo là lăng trụ Ngoài ra, cả ba trục khớp đều song song với nhau và thường hướng theo trục trọng trường nên hai bộ tác động đầu không làm việc đối với trọng lực của các khớp và tải Cổ tay thường chỉ có 1 bậc tự do, do đó toàn bộ robot có 4

bậc tự do Kiểu cơ cấu chấp hành này thường được dùng để lắp đặt các chi tiết phẳng

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

1.1.5 Hệ thống cảm biến

Cảm biến được sử dụng như thiết bị ngoại vi của Robot, gồm nhiều loại khác nhau 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

1.1.6 Các phương pháp điều khiển tự động cho Robot

Để điều khiển cho Robot, người ta thường phân chia theo mục đích sử dụng

và môi trường làm việc để có các phương pháp điều khiển khác nhau Có nhiều cách khác nhau để phân loại các hệ thống điều khiển tự động Thường có các hệ thống điều khiển sau:

- Hệ thống liên tục: khi các tín hiệu tác động trong hệ là hàm liên tục theo thời gian

- Hệ thống rời rạc: trong đó chỉ cần có một tín hiệu là hàm rời rạc theo thời gian

- Hệ thống tối ưu: là hệ thống điều khiển trong đó thiết bị điều khiển có chức năng tổng hợp được tín hiệu điều khiển u(t) tác động lên đối tượng điều khiển để đạt được trạng thái tối ưu theo một chỉ tiêu nào đó

- Hệ thống thích nghi (hệ thống tự chỉnh): là hệ thống có khả năng tự thích ứng với những biến đổi của điều kiện môi trường và đặc tính của đối tượng điều khiển bằng cách thay đổi tham số và cấu trúc của thiết bị điều khiển Trong kỹ thuật người máy thì cũng có thể phân chia hệ điều khiển thành:

- Điều khiển theo chương trình: hệ thống thực hiện theo chương trình định sẵn

- Điều khiển thích nghi: việc điều khiển tùy thuộc vào thông tin nhận biết được trong quá trình làm việc về hiện trạng của môi trường thao tác và bản thân người máy

Tùy theo khả năng thực hiện các chuyển động của tay máy mà chia thành các phương pháp điều khiển:

- Điều khiển theo chu tuyến: chuyển động theo đường liên tục

- Điều khiển theo vị trí: chuyển động được đảm bảo qua một số vị trí nhất định

- Điều khiển theo chu kỳ: chuyển động được xác định bằng các vị trí đầu và cuối hành trình của mỗi bậc tự do

1.2 Phân loại Robot công nghiệp

Có thể phân loại Robot theo nhiều cách khác nhau Theo số bậc tự do, cấu trúc động học, hệ truyền động, dạng hình học…

Phân loại theo dạng hình học không gian làm việc

Không gian làm việc của cơ cấu chấp hành được xác định là thể tích không gian tác động có thể với tới Thiết kế với các trục khớp cắt nhau tại một điểm chung gọi là tâm cổ tay Không gian do nhà sản xuất Robot cung cấp thường được xác định theo vùng không gian làm việc

Phân loại theo phương pháp điều khiển

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

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:

- Lặp lại từ điểm tới điểm

- Lặp lại liên tục

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

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

Lưu ý ở đây là thêm càng nhiều bậc chuyển động một mặt sẽ làm tăng khả năng linh hoạt của tay máy, mặt khác cũng kéo theo hệ quả là làm tăng thêm sai số dịch chuyển, tức là làm tăng sai số tích lũy trong điều khiển vị trí của khâu tác động cuối Điều này đồng nghĩa với sự gia tăng về chi phí và thời gian sản xuất và bảo dưỡng Robot

Phân loại theo hệ thống truyền động

Các dạng truyền động phổ biến là :

Hệ truyền động điện: Thường dùng các động cơ điện 1 chiều hoặc các động

cơ bước Loại truyền động nầy dễ điều khiển, kết cấu gọn

Hệ truyền động thuỷ lực: Có thể đạt được công suất cao, đáp ứng những điều

kiện làm việc nặng Tuy nhiên hệ thống thuỷ lực thường có kết cấu cồng kềnh, tồn tại độ phi tuyến lớn khó xử lý khi điều khiển

Hệ truyền động khí nén: Có kết cấu gọn nhẹ hơn do không cần dẫn ngược

nhưng lại phải gắn liền với trung tâm taọ ra khí nén Hệ này làm việc với công suất trung bình và nhỏ, kém chính xác, thường chỉ thích hợp với các robot hoạt động

theo chương trình định sẳn với các thao tác đơn giản “nhấc lên - đặt xuống” (Pick and Place or PTP : Point To Point)

Ngoài ra có rất nhiều cách phân loại khác tùy theo quan điểm và mục đích sử dụng, nghiên Cứu robot

1.3 Ứng dụng của Robot

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

- 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 chưa 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

1.4 Mục tiêu và phạm vi nghiên cứu của luận văn

Mục tiêu nghiên cứu của luận văn là xây dựng được cấu trúc và thuật toán điều khiển cho cánh tay Robot Scara 4 bậc tự do

Luận văn nghiên cứu về lý thuyết điều khiển cánh tay Robot trên cơ sở mô hình và ứng dụng vào Robot SCARA, kiểm nghiệm tính đúng đắn của các công thức tính động học thuận nghịch, động lực học, tổng hợp hệ truyền động trong quá trình thiết kế

Bộ điều khiển cho cánh tay Robot Scara dựa trên luật PD bù trọng lực Chất lượng của hệ thống của hệ thống được đánh giá và phân tích dựa trên các kết luận của mô phỏng sử dụng Matlab Simulink

Trong quá trình mô phỏng tác giả đã dựa vào các thông số của Robot Scara

ở trên và với điều kiện khi Robot chưa mang, Robot mang tải định mức và trường hợp Robot mang tải không xác định, từ đó đưa ra nhận xét cho từng trường hợp

Chương 2 PHƯƠNG TRÌNH ĐỘNG LỰC HỌC ROBOT 2.1 Xây dựng phương trình động lực học cho hệ Robot

Nghiên cứu động lực học Robot là công việc cần thiết khi phân tích cũng

như tổng hợp quá trình điều khiển chuyển động Việc nghiên cứu động lực học

robot thường giải quyết hai nhiệm vụ sau đây:

+ Xác định momen và lực xuất hiện trong quá trình chuyển động Khi đó

quỹ đạo biến đổi của biến khớp qi(t) coi như đã biết Các cấu hình khác nhau của cơ

cấu tay máy trong từng thời điểm xác định bằng các độ dịch chuyển góc hoặc độ

dịch chuyển dài của các khớp quay hoặc khớp tịnh tiến

+ Các độ dịch chuyển tức thời đó, so với giá trị ban đầu nào đó lấy làm mốc

tính toán, được gọi là các toạ độ suy rộng (generalized joint coordinates) Ở đây ta

gọi chúng là các biến khớp (toạ độ suy rộng) của cơ cấu tay máy và biểu thị bằng :

i

Trong đó: i - Độ dịch chuyển góc của các khớp quay

Si - Độ dịch chuyển tịnh tiến của các khớp tịnh tiến

+ Việc tính toán lực trong cơ cấu tay máy là rất cần thiết để chọn công suất

động cơ, kiểm tra độ bền, độ cứng để đảm bảo độ tin cậy của Robot

+ Xác định các sai số động tức là sai lệch so với quỹ đạo chuyển động mong

muốn chương trình Lúc này cần khảo sát phương trình chuyển động của Robot có

tính đến đặc tính động lực của động cơ và các khâu

+ Có nhiều phương pháp nghiên cứu động lực học Robot, nhưng thường gặp

hơn cả là phương pháp cơ học Lagrange, cụ thể là dùng phương trình Lagrange -

Euler Đối với các khớp của Robot, với các nguồn động lực và kênh điều khiển

riêng biệt, không thể bỏ qua các hiệu ứng trọng trường (gravity effect), quán tính

(initial), tương hỗ (Coriolis), ly tâm (centripetal) mà những khía cạnh này chưa

được xét đầy đủ trong cơ học cổ điển, Cơ học Lagrange nghiên cứu các vấn đề nêu

trên như một hệ thống khép kín nên đây là nguyên lý cơ học thích hợp đối với các

bài toán động lực học robot

Các phương trình động lực học được thiết lập dựa trên cơ sở phương trình

L- hàm Lagrange L = K - P (2.3)

K, P- động năng và thế năng của hệ

FMi - Lực hình thành trong khớp động thứ i khi thực hiện chuyển động

qi – Biến khớp (tọa độ suy rộng )

i

q - đạo hàm bậc nhất của biến khớp theo thời gian

Đồng thời khi mô tả vị trí giữa 2 hệ toạ độ thứ i và i-1 dùng ma trận thuần

nhất Ai hoặc viết đầy đủ hơn là 1

i i

A

 Dùng ma trận này có thể mô tả vị trí trạng thái trong hệ toạ độ thứ i-1 của một điểm bất kì thuộc hệ toạ độ thứ i

Các biến khớp qi là bộ các thông số dịch chuyển của các khớp động của

Robot Vị trí trạng thái của điểm tác động cuối của Robot hoàn toàn được xác định

bởi bộ biến khớp qi này

Xét khâu thứ i của một robot có n khâu Tính lực tổng quát Fi của khâu thứ i

với khối lượng vi phân của nó là dm Lực tổng quát Fi đóng vai trò rất quan trọng

khi xây dựng sơ đồ khối để thiết lập hàm điều khiển cho robot có n bậc tự do

2.1.1 Vận tốc và gia tốc của robot

Để xây dựng mô hình động lực học Robot dùng phương trình Lagrange bậc

2, ta cần biết vận tốc của điểm bất kì trên tay máy

Điểm M nào đó trong hệ toạ độ i, xác định bằng véc tơ mở rộng i i

Vậy quan hệ giữa 0r i và i i

r

1

có thể biểu thị như sau :

0r i = 0A i i r i (2.5) với 0A i = 0 1

0 0

S 0

a S

a S S

i

i i

i i i

i i

i i

i i i

i i

i i

d C

S C

S

S C

C C

C C

i i

A

1 =

0 0

S 0

0 S

0 S S

i

i

i i

i i

i i

i i

i i i

d C

C S

S C

C C

Vi phân biểu thức (2.5) với lưu ý rằng các vectơ i r i là không đổi với hệ toạ

độ thứ i vì giả thiết rằng các khâu của tay máy là vật rắn tuyệt đối, ta có:

i

V

0  Vi = ( 0 ) ( 0 )

i i i

i A r dt

d r dt

i D A dq

0 0 0 0

0 0 0 1

0 0 1 0

và đối với khớp tịnh tiến :

1 0 0 0

0 0 0 0

0 0 0 0

j

i A A A A A A q

j j j

dq

dA A A A q

A

1 1

2 1

i A A A D A A

q

A

1 1

2 1

2.1.2 Động năng của vi phân khối lượng dm

Kí hiệu Ki là động năng của khâu i ( i =1,2, …, n) và dKi là động năng của một chất điểm khối lượng dm thuộc khâu i ta có:

TrA = 



n i ii a

1

T i

i p

z dm y dm x

dm z dm z dm y z dm z x

dm y dm y z dm y dm y x

dm x dm z x dm y x dm x dm r r

i i

i

i i

i i i

i

i i

i i

i i

i i

i i

i i

T i i i i

Với các chỉ số i, j, k lấy lần lượt bằng các giá trị xi , yi, zi, đó là các trục của

hệ toạ độ i, và ij là kí hiệu Cronecke, thì ma trận Ji có thể biểu thị ở dạng sau:

T i i i i

i

z y x

2.1.3 Thế năng của Robot

Thế năng Pi của khâu i:

Pi = - mi.g.0r i = -mi.g.0A i1r i (2.26) i=1,2,…,n

2.1.4 Phương trình động lực học của hệ robot

Để xây dựng mô hình động lực học tay máy dùng phương trình Lagrange bậc

2:

i i

F q

L q

J: thể hiện tác dụng của quán tính, là một ma trận đối xứng (n x n)

C: thể hiện tác dụng của lực ly tâm và Cariolis, là một vectơ (n x 1)

G: thể hiện tác dụng của lực trọng trường, cũng là một vectơ (n x 1)

2.2 Các đặc tính của phương trình động lực học Robot

Các phương trình chuyển động cho một robot n-bậc rất được quan tâm đặc biệt nếu robot có chứa một hoặc nhiều khớp tự do Những phương trình này có chứa một số đặc tính quan trọng có thể được sử dụng để khai thác, đặc biệt ứng dụng cho việc phát triển các thuật toán điều khiển Ở đây chúng ta sẽ bàn một số các đặc tính, quan trọng nhất trong đó là những sai lệch được gọi là đặc tính đối xứng và đặc tính

thụ động, các đặc tính của các thông số tuyến tính Đối với khớp nối của robot, các

ma trận quán tính cũng có hữu ích cho việc thiết kế điều khiển

2.2.1 Tính đối xứng lệch và đặc tính thụ động

Tính đối xứng lệch được đề cập đến một mối quan hệ quan trọng giữa các

ma trận quán tính J(q) và ma trận C(q,q)B) trong phương trình (2.30)

Đề xuất bài toán như sau: Xác định ma trận ( , ) ( ) 2 ( , )

Trong đó: ( , )



q q

N là ma trận đối xứng lệch, có nghĩa là các thành phần njk của N thỏa mãn njk=-nkj

Chứng minh: Do tính quán tính của ma trận J(q), mà thành phần thứ kj của J(q) được cho bởi quy tắc sau:

ki i kj i

kj

q q

j j

j q

j q

kj k

ij

q q

j q

()

q

T

0



Để chứng minh đặc tính thụ động, gọi H là tổng năng lượng của hệ thống, tức là tổng động năng và thế năng

) ( ) ( 2

1

q P q q J q H

T T

1

q

P q q q J q q

g q q C q

T T

1 ) ( ) , (

),(2)2

T H d q

0

) 0 ( ) 0 ( ) ( )

( )

Từ đó ta thấy tổng năng lượng H(T) là không âm, và đặc tính thụ động  H i0 j

2.2.2 Giới hạn trên của ma trận quán tính

Các ma trận quán tính cho n bậc tự do của robot là đối xứng và xác định dương Do vậy đối với mỗi giá trị cố định tổng quát của q có 0 < λ1(q) ≤ ,≤λn(q) biểu thị cho các giá trị riêng n của J(q)

λ1(q)In×n ≤ J(q) ≤ λn(q)In×n (2.39) Khi Inxn chỉ rõ bậc của ma trận là nxn Bất đẳng thức trên chỉ ra rằng nếu hai

ma trận A và B có cùng bậc nxn, có nghĩa là nếu B<A có nghĩa là A-B được xác định dương và nếu BA thì A-B được xác định bán tính dương

Nếu tất cả các khớp nối là tự do thì ma trận quán tính chỉ có chức năng giới hạn liên kết các biến chung…chứa hàm sin, cos Cũng có thể tìm ra được các hằng

số λm và λM được giới hạn bởi ma trận quán tính:

λmIn×n ≤ J(q) ≤ λMIn×n < ∞ (2.40)

2.2.3 Đặc tính tuyến tính của tham số

Các phương trình chuyển động của robot được quy định bởi các tham số nhất định, chẳng hạn như khối lượng liên kết, thời gian quán tính phải được xác định

cụ thể theo thứ tự cho từng loại robot

Các phương trình chuyển động tuyến tính với các thông số quán tính theo nghĩa sau đây: giả sử tồn tại hàm ( , , )





q q q

Y kích thước nxl hoàn toàn được biết đến,

và một vecter  một chiều, như vậy các phương trình Euler-Lagrange có thể được viết:

Y được gọi là Regressor và  là vector tham số

2.3 Phương trình động lực học cho Robot Scara

2.3.1 Giới thiệu về Robot Scara

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ánh tay Robot

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

Với thiết kế động cơ truyền động cho 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 được 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 piton khí nén

Robot Serpent có thể được lập 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

a Động học vị trí của Robot Scara

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

Cơ cấu chấp hành của Robot thường là một cơ cấu hở, gồm một chuỗi các khâu (links) nối với nhau bằng các khớp (joints) Các khớp động này có thể là khớp quay (R) hoặc khớp tịnh tiến (T) Để Robot có thể thao tác linh hoạt, cơ cấu chấp hành của nó phải cấu tạo sao cho điểm mút của khâu cuối cùng đảm bảo dễ dàng di chuyển theo một quỹ đạo nào đó, đồng thời khâu này có định hướng theo yêu cầu Khâu cuối cùng thường là bàn kẹp hoặc dụng cụ làm việc Điểm mút của khâu cuối cùng là điểm quan tâm nhất vì đó là điểm tác động của Robot lên vật cần thực hiện, điểm này gọi là điểm tác động cuối (end-effector) Ở điểm này không những quan tâm tới vị trí nó chiếm trong không gian mà cả hướng tác động của khâu cuối đó

Khi gắn vào điểm tác động cuối này là một hệ tọa độ thứ n và gắn với mỗi

khâu động một hệ tọa độ động khác, gắn liền giá đỡ với hệ tọa độ cố định (đánh số

từ 0 tới n bắt đầu từ giá đỡ cố định) Khi khảo sát chuyển động của Robot cần biết

định vị và định hướng tại điểm tác động cuối trong mọi thời điểm Có lúc cần biết

cả vận tốc và gia tốc chuyển động Robot tại điểm tác động cuối và các điểm khác

trên Robot Đây là nội dung quan trọng của bài toán động học Robot Các lời giải của bài toán này được xác định từ những phương trình động học của Robot, chúng được xây dựng trên cơ sở thiết lập các mối quan hệ giữa các hệ tọa độ động nói trên với hệ tọa độ cố định

Phép biến đổi đồng nhất mô tả quan hệ tịnh tiến và quay 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

0 0

α α

0

θ α θ α

θ θ

θ α θ α

θ θ

i i

i

i i i i i

i i

i i i i i

i i

d Cos

Sin

Sin a Sin Cos Cos

Cos Sin

Cos a Sin Sin Cos

Sin Cos

(2.42)

Ma trận này là tích các ma trận thuần nhất, đó là một mô tả biến đổi thuần nhất bởi phép quay và phép tịnh tiến tương đối giữa hệ toạ độ của hai khâu liền nhau 0A1 mô

tả vị trí và hướng của khâu đầu tiên; 1A

2 mô tả vị trí và hướng của khâu thứ hai so với khâu thứ nhất Như vậy vị trí và hướng của khâu thứ hai so với hệ toạ độ gốc được biểu diễn bởi ma trận:

nếu chỉ số đó bằng 0 Chỉ số dưới thường dùng để chỉ khâu chấp hành cuối

b Xây dựng động học vị trí Robot SCARA[2]

Robot 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 Z0 góc 1

- Khớp 2 quay quanh trục Z1 góc 2

- Khớp 3 chuyển động tịnh tiến theo trục Z2 đoạn d3

- Khớp 4 quay quanh trục Z3 góc 4

Hình 2.2 Cấu hình Robot SCARA

Theo cấu hình Robot ta có bảng tham số động học của Robot:

Thanh nối  (rad)  (0) a (m) d (m)

Hình 2.3 Bảng tham số động học của Robot

Toạ độ tay của Robot được xác định so với hệ toạ độ gốc [6]:

0

T4 = 0A1 1A2 2A3 3A4 (2.45) Trong đó:

0 0

α α

0

θ α θ α

θ θ

θ α θ α

θ θ

i i

i

i i i i i

i i

i i i i i

i i

d Cos

Sin

Sin a Sin Cos Cos

Cos Sin

Cos a Sin Sin Cos

Sin Cos

(2.46)

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

0 1 0 0

0 θ θ

0 θ θ

1 1 1

1

1 1 1

1





Sin a Cos

Sin

Cos a Sin

0 0

0 1 0

0

θ 0 θ θ

θ 0 θ θ

2 2 2

2

2 2 2

2

Sin a Cos

Sin

Cos a Sin

1 0 0

0 0 1 0

0 0 0 1

0 1 0 0

0 0 θ θ

0 0 θ θ

4 4

4 4

Cos Sin

Sin Cos

0 1 0 0

0 0

0 0

4 4

4 4

C S

S C

1 0 0

0 0 1 0

0 0 0 1

0 1 0 0

0 0

0 0

4 4

4 4

C S

S C

1 0 0

0 0

0 0

3

4 4

4 4

d

C S

S C

3 1T4 = 1A2 2T4

0 1 0 0

0

0

2 2 2

2

2 2 2

2

S a C

S

C a S

1 0 0

0 0

0 0

3

4 4

4 4

d

C S

S C

0

1 0

0

0

.

0

.

3

2 2 4

2 4 2 4 2 4 2

2 2 4

2 4 2 4 2 4 2

d

S a C

C S S S C C S

C a C

S S C S S C C

0 1 0 0

0

0

1 1 1

1

1 1 1

1

S a C

S

C a S

0

1 0

0

0

.

0

.

3

2 2 4

2 4 2 4 2 4 2

2 2 4

2 4 2 4 2 4 2

d

S a C

C S S S C C S

C a C

S S C S S C C

0 0

1 0

0

0 ) ( ) ( ) ( )

.

.

(

0 ) ( ) ( ) ( )

.

(

3

1 1 1 2 2 2 1 2 4

2 4 2 1 4 2 4 2 1 4 2 4 2 1 4

2 4 2 1 4 2 4 2 1 4 2 4 2 1 4

C S S C C S S C S S C C S C S

C S S S C S S C C S C C S S S

0 0

1 0 0

0

.

0

3

1 1 2 2 4

1 4

2

1 1 2 2 1

2 4

1

d

S a S a C

S

C a C a S

C

(2.51)

Với: C124 = Cos(124); S124 = Sin(124)

Ma trận 0T4 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:

z z z z

y y y y

x x x x

p a o n

p a o n

p a o n

Do đó hệ phương trình động học thuận tay máy Robot:

nx = C1(C2 C4 + S2 S4) - S1(S2 C4 - C2 S4) (2.52a)

ny = S1(C2 C4 + S2 S4) + C1(S2 C4 - C2 S4) (2 52b)

c Phương trình động học ngược Robot SCARA [2]

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

Kết quả của việc giải hệ phương trình động học đóng vai trò hết sức quan trọng trong việc điều khiển Robot Thông thường, điều ta biết là các vị trí và hướng

mà ta muốn Robot phải dịch chuyển tới và điều ta cần biết là mối quan hệ giữa các

hệ toạ độ trung gian để phối hợp tạo ra chuyển động của Robot, hay nói cách khác

đó chính là giá trị của các biến khớp ứng với mỗi toạ độ và hướng của khâu chấp hành cuối hoặc công cụ gắn lên khâu chấp hành cuối, muốn vậy ta phải giải hệ phương trình động học của Robot

Việc giải bài toán động học ngược của robot cần thoả mãn các điều kiện sau:

i*) sao cho robot có hình thể (hình dạng) cho trước

- Điều kiện duy nhất của tập nghiệm: Trong khi xác định các tập nghiệm cần

phân biệt rõ hai loại nghiệm:

+ Nghiệm toán (Mathematical Solution): Các nghiệm này thoả mãn các phương trình cho trước của T6

+ Nghiệm vật lý (Physical Solution): là các tập con của nghiệm toán, phụ thuộc vào các giới hạn vật lý (giới hạn về góc quay, kích thước ) nhằm xác định tập nghiệm duy nhất

Việc giải hệ phương trình động học có thể được tiến hành theo hai phương pháp cơ bản sau:

+ Phương pháp giải tích (Analytical Method): tìm ra các công thức hay các phương trình toán giải tích biểu thị quan hệ giữa các giá trị của không gian biến trục

và các thông số khác của bộ thông số DH (DH là viết tắt của Denavit-Hartenberg) Trong đó bộ thông số cơ bản gồm có: ai là độ dài đường chung giữa 2 trục khớp động i+1 và i  i là góc chéo giữa 2 trục khớp động i+1 và i  i là góc giữa hai đường vuông góc chung nói trên di là khoảng cách đo dọc trục khớp động i từ đường vuông góc chung giữa trục khớp động i+1 và i tới đường vuông góc chung giữa khớp động i và khớp động i-1)

+ Phương pháp số (Numerical Method): Tìm ra các giá trị của tập nghiệm bằng kết quả của một quá trình lặp

Để tính động lực học ngược, ta dùng lời giải của phép biến đổi Euler Từ 12 phương trình xác định vị trí tay Robot (2 36a) đến (2 36l) 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 có:

Sin1 =

2 2 1 2 2 2 1

2 2 2

2 1

2

)

.(







Sin a a a a

p Sin a p Cos a

2 1 2.

.







Cos a a

Sin Sin a

2 2 2 1 2 2

.

2a a

a a p

Sin2 = 2 2

1 Cos 

3 d3 = - pz (2.55)

4 4 = 1 + 2 - atan2( 2

2.3.2 Phương trình động lực học cho Robot Scara

Robot SCARA có các thông số như ở bảng sau [2]

Hình 2.4 Bảng thông số của Robot SCARA

Với: lg1 = l1/2 lg2 = l2/2 lg3 = l3/2

Hình 2.5 Cấu hình Robot SCARA

Hệ qui chiếu gắn với hệ trục toạ độ (O0, x0, y0, z0 ) trên khớp thứ nhất Khi

đó mặt phẳng (O0, x0, y0 ) là mặt phẳng đẳng thế

 Tính toán động năng và thế năng cho từng khớp

1 1 1

1 1 1

C

g C

g C

z

Sin l y

Cos l x





(2.57)

Trong đó xC1, yC1, zC1 là hình chiếu của tâm thanh nối số 1 lên các trục x, y, z

Tốc độ v 1 của tâm C1 trên trục {0}:

θ θ

1

.

1 1 1 1

.

1 1 1 1

.

C

g C

g C

z

Cos l y

Sin l x

2 1

2 1

.

z y

1 2 2 1

2 1 1 2 2

1 θ ) θ θ ( l g Sin  l g Cos

Động năng và thế năng khớp 1:

2 1

2

) θ θ ( θ

2

2 1 2 1 1 2

2 1 2 1 1 2

C

g C

g C

z

Sin l Sin l y

Cos l Cos l x

(2.60)

Tốc độ v 2 của tâm C2 trên trục {0}:

θ θ ( θ θ

) θ θ ).(

θ θ ( θ θ

2

.

2 1 2 1 2 1 1 1 2

.

2 1 2 1 2 1 1 1 2

C

g C

g C

z

Cos l Cos

l y

Sin l Sin l x

2 2

2 2

.

z y

2 1

2 1

2 1 2 1 1 3

2 1 2 1 1

3

) θ θ ( θ

) θ θ ( θ

l z

Sin l Sin l

y

Cos l Cos l

.

2 1 2 1 2 1 1 1 3

.

2 1 2 1 2 1 1 1 3

.

) θ θ ).(

θ θ ( θ θ

) θ θ ).(

θ θ ( θ θ

l z

Cos l Cos

l

y

Sin l Sin l x

2 3

2 3

.

z y



  Cos + 2

.

l