Đồ án Nghiên cứu các phương pháp điều khiển robot công nghiệp

c truyền động từ một động cơ secvo. Động cơ secvo được điều khiển bởi hệ thống truyền động điều khiển vị trí với tín hiệu phản hồi là vị trí của piston được đo nhờ cảm biến vị trí. Động cơ chỉ cần sinh một lực nhỏ để di chuyển piston của van secvo, từ đó điều chỉnh được lưu lượng và hướng của đường dầu cung cấp cho xilanh và điều khiển được tốc độ và hướng dịch chuyển của xilanh. Ưu điểm của các chấp hành thủy lực là công suất lớn và cho phép chịu được tải lớn. Tuy nhiên hệ truyền động thủy lực lại có nhiều nhược điểm như: hiện tượng rò rỉ dầu gây ảnh hưởng tới môi trường, có thể gây cháy khi ứng dụng cho hàn hồ quang, cần nhiều cơ cấu phụ trợ, độ ồn lớn, phải kiểm tra chất lượng dầu thường xuyên. b. Truyền động khí nén Nguyên lý làm việc của cơ cấu khí nén tương tự như cơ cấu thủy lực nhưng dầu áp suất cao được thay bằng khí nén. Cơ cấu khí nén cũng chia làm hai loại tuyến tính và quay. Ưu điểm của cơ cấu khí nén: nguồn khí nén sẵn có, giá thành cơ cấu khí nén thấp, không làm ảnh hưởng tới môi trường, chuyển động nhanh. Nhược điểm của cơ cấu khí nén là khó áp dụng luật điều khiển phản hồi. Cơ cấu khí nén chỉ được dùng cho công suất nhỏ và cho các ứng dụng đơn giản như trong các cơ cấu vận chuyển, bàn kẹp. c. Truyền động điện Hệ thống truyền động điện bao gồm bộ biến đổi, nguồn điện và động cơ điện. Các dạng động cơ điện sử dụng trong hệ thống truyền động robot là: động cơ secvo một chiều, động cơ secvo xoay chiều, động cơ secvo một chiều không chổi than và động cơ bước. Hệ thống truyền động điện thường được chia làm hai loại: truyền động trực tiếp và gián tiếp qua bộ truyền động cơ khí. Động cơ điện sẽ cung cấp mômen cần thiết để định vị góc quay chính xác cho các khớp trực tiếp hoặc gián tiếp thông qua bộ truyền động cơ khí như các hệ thống puli và đai truyền, các hộp giảm tốc và các hệ thống truyền động điều hòa. Ưu điểm của động cơ điện là các hệ thống truyền động trực tiếp cho chuyển động nhanh và chính xác, dễ dàng thực hiện luật điều khiển phản hồi, dễ dàng phối hợp với máy tính trong hệ thống điều khiển. Do đó dây là loại cơ cấu chấp hành phổ biến nhất trong các hệ thống robot. 1.5.3. Hệ thống điều khiển robot Bộ điều khiển có thể được thiết kế từ các vi xử lý, các vi điều khiển, bộ điều khiển logic khả trình PLC hoặc máy tính. Liên quan đến đặc điểm làm việc của robot có thể chia bài toán điều khiển robot thành hai loại: điều khiển thô và điều khiển tinh. Điều khiển thô còn gọi là điều khiển chuyển động hay điều khiển quỹ đạo, được áp dụng cho robot chuyển động tự do trong không gian làm việc của robot nghĩa là không tương tác với môi trường làm việc. Khi đó cần phải xác định luật điều khiển thích hợp để tốc độ, vị trí do đó chuyển động của các khớp bám sát quỹ đạo thiết kế trong thời gian quá trình quá độ nhỏ nhất. Điều khiển chuyển động có thể thực hiện ở hệ tọa độ khớp hay tọa độ Đề các tùy thuộc quỹ đạo thiết kế cho tọa độ khớp hay tọa độ Đề các. Đồ án này sẽ tập trung nghiên cứu các phương pháp và các luật điều khiển chuyển động. Điều khiển tinh còn gọi là điều khiển lực, được áp dụng cho robot có tương tác với môi trường làm việc. Khi đó yêu cầu điều khiển cả lực và chuyển động. Hai phương pháp điều khiển lực là: điều khiển trở kháng (điều khiển độ nhún) và điều khiển hỗn hợp. 1.5.4. Hệ thống cảm biến Các cảm biến trong robot có thể chia làm hai loại: Cảm biến ngoại tuyến tăng khả năng nhận thức cho robot về môi trường xung quanh. Cảm biến nội tuyến cung cấp các thông tin về đặc tính của bản thân robot. a. Cảm biến nội tuyến Gắn trực tiếp trên trục khớp hoặc động cơ, thường là các encodor, chiết áp đo vị trí, các cảm biến lực, thiết bị đo lực. b. Cảm biến ngoại tuyến Cung cấp các thông tin về đối tượng hoặc môi trường tương tác. Các cảm biến ngoại tuyến có chức năng như các giác quan chính của con người. + Cảm biến hình ảnh (Camera) Camera có cấu tạo bao gồm thấu kính, tế bào quang học, màng chắn. Các tín hiệu về ánh sáng sẽ được chuyển thành tín hiệu điện + Cảm biến thính giác (Micro phone) Chuyển các âm thanh trong không gian thành tín hiệu điện. Ngoài ra còn có cảm biến về mùi vị, cảm biến nhiệt độ cao bằng tia hồng ngoại, cảm biến khoảng cách bằng phát siêu âm. 1.6. ỨNG DỤNG CỦA ROBOT CÔNG NGHIỆP Robot được sử dụng rộng rãi trong nhiều ngành công nghiệp khác nhau bởi những lợi ích kinh tế mà nó mang lại là rất lớn. Nhìn chung robot có thể nâng các vật nặng, làm việc với các nguyên liệu không an toàn, các hoạt động nguy hiểm hoặc môi trường không thích hợp vơi con người hoặc những công việc nhàm chán lặp đi lặp lại. Có thể phân loại ứng dụng công nghiệp của robot gồm các lĩnh vực chính: vận chuyển, bốc dỡ vật liệu, gia công, lắp ráp thăm dò và các ứng dụng khác. a. Ứng dụng robot công nghiệp trong vận chuyển, bốc dỡ vật liệu Ứng dụng vào vận chuyển, robot có nhiệm vụ di chuyển đối tượng từ vị trí này đến vị trí khác. Nhiệm vụ này của robot thực hiện bởi các thao tác nhặt và đặt vật thể. Robot nhặt chi tiết ở một vị trí và chuyển dời đến một vị trí khác. Robot có thể gắp một chi tiết ở một vị trí cố định hoặc trên một băng tải đang chuyển động và đặt trên một băng tải khác đang chuyển động với định hướng chi tiết. b. Ứng dụng robot trong lĩnh vực gia công vật liệu Trong công nghiệp gia công vật liệu, robot thực hiện nhiệm vụ như một máy gia công. Do đó tay robot sẽ gắn một dụng cụ thay cho một cơ cấu kẹp. Ứng dụng của robot trong công nghiệp gia công vật liệu bao gồm các công nghệ sau: hàn điểm, hàn hồ quang liên tục, sơn phủ, công nghệ gia công kim loại. c. Ứng dụng robot trong lắp ráp và kiểm ta sản phẩm Công nghệ lắp ráp là lắp một chi tiết vào một bộ phận khác. Robot được sử dụng trong dây chuyền lắp ráp thông thường ở bốn dạng sau: lắp chi tiết vào lỗ, lắp lỗ vào chi tiết, lắp chi tiết nhiều chân vào lỗ và lắp ngăn xếp. Trong công nghiệp lắp ráp, robot có thể hoạt động riêng lẻ để lắp hoàn thiện một thiết bị hoặc làm việc trong một dây chuyền, trong đó mỗi robot sẽ có nhiệm vụ lắp một chi tiết trong một thiết bị máy. Robot cũng được sử dụng trong công đoạn thử nghiệm và kiểm tra. Một trong những ứng dụng của robot trong lĩnh vực đo và kiểm tra sản phẩm là các máy đo tọa độ (Coordiante Measureent Machine - CMM). Máy đo tọa độ được sử dụng rộng rãi để kiểm tra kích thước, vị trí và hình dạng của các chi tiết máy hoặc các bộ phận cơ khí. CHƯƠNG 2 CÁC PHƯƠNG PHÁP ĐIỀU KHIỂN ROBOT TRONG CÔNG NGHIỆP 2.1. KHÁI QUÁT Bài toán đặt ra với điều khiển chuyển động là đảm bảo tay Robot chuyển động bám theo quỹ đạo đặt trước trong môi trường làm việc. Chuyển động của tay robot thực hiện nhờ các hệ thống truyền động khớp robot. Trên cơ sở đó, có hai dạng hệ thống điều khiển chuyển động: hệ thống điều khiển ở không gian khớp và hệ thống điều khiển ở không gian làm việc. 2.2. PHÂN LOẠI Ta có thể phân loại các hệ thống điều khiển chuyển động như sau: 2.2.1. Phân loại theo không gian điều khiển, ta có hệ thống điều khiển không gian khớp và hệ thống điều khiển không gian làm việc. * Với hệ thống điều khiển không gian khớp, đại lượng được điều khiển là vị trí của khớp robot (góc quay đối với khớp quay, độ dịch chuyển thẳng đối với khớp tịnh tiến). Bộ điều khiển được thiết kế đảm bảo vị trí khớp luôn bám theo vị trí đặt, tức là sai lệch vị trí khớp hội tụ về không với thời gian nhỏ nhất. Vị trí đặt của khớp được tính toán từ lượng đặt vị trí của tay robot trong không gian làm việc thông qua khâu tính toán động học ngược. Động học ngược Bộ điều khiển BBĐ Động cơ Cơ cấu robot Cảm biến Xd qd X q Hình 2.3. Sơ đồ khối hệ thống điều khiển ở không gian khớp Trong sơ đồ trên thì Xd, X tương ứng là vectơ vị trí đặt và vectơ vị trí thực của tay robot; qd, q tương ứng là vectơ vị trí đặt và vectơ vị trí thực của khớp robot. ** Với hệ thống điều khiển trong không gian làm việc có chức năng duy trì trực tiếp sai lệch vị trí của tay robot trong không gian làm việc bằng không. Lượng đặt của hệ thống điều khiển là vị trí đặt của tay trong không gian làm việc và lượng phản hồi vị trí thực của tay. Khâu tính toán động lực học ngược sẽ phụ thuộc mạch vòng điều khiển phản hồi. Bộ điều khiển BBĐ Động cơ Cơ cấu robot Cảm biến Xd X Hình 2.4. Sơ đồ khối hệ thống điều khiển ở không gian làm việc 2.2.2. Phân loại theo mức độ ràng buộc của robot, ta có hệ thống điều khiển phân tán và hệ thống điều khiển tập trung. Đối với các robot có tỉ số truyền của bộ truyền lớn, có thể coi hệ thống robot n bậc tự do sẽ gồm n hệ thống độc lập 1 đầu vào/ 1 đầu ra (SISO) và sự ràng buộc giữa các khớp được coi là thành phần nhiễu, có thể bỏ qua. Bởi vậy bộ điều khiển của các khớp được thiết kế độc lập với nhau. Đó chính là hệ thống điều khiển phân tán. Hệ thống điều khiển tập trung được xây dựng cho các robot có tỉ số truyền của bộ truyền nhỏ, khi đó robot là 1 hệ thống có tính ràng buộc và phi tuyến cao gồm nhiều đầu vào và nhiều đầu ra (MIMO). Đồ án này sẽ đi sâu nghiên cứu hệ thống điều khiển tập trung và không quan tâm tới hệ thống điều khiển phân tán. 2.2.3. Phân loại theo sự thay đổi tham số, ta có hệ thống điều khiển không thích nghi, hệ thống điều khiển thích nghi và hệ thống điều khiển bền vững. Hệ thống điều khiển không thích nghi là hệ thống có tham số bộ điều khiển cố định, được áp dụng khi tham số robot không thay đổi. Hệ thống điều khiển thích nghi là hệ thống mà khi tham số robot không được xác định chính xác hoặc biến đổi thì bộ điều khiển sẽ tự hiệu chỉnh tham số cho phù hợp. Hệ thống này đòi hỏi phải có khâu nhận dạng thích nghi (tức là phải có luật thích nghi). Hệ thống này có thể điều khiển chính xác nhưng mà chậm. Hệ thống điều khiển bền vững là hệ thống điều khiển cũng được áp dụng khi tham số robot biến đổi tuy nhiên hệ thống này không đòi hỏi khâu nhận dạng. Đặc điểm của hệ thống điều khiển bền vững là tham số của bộ điều khiển sẽ bền vững trong trong một giới hạn của các tham số robot. Tuy nhiên cấu trúc bộ điều khiển sẽ biến đổi cho phù hợp với sự thay đổi tham số dựa theo nguyên lý điều khiển trượt hoặc nguyên lý điều khiển mờ. 2.3. MỘT SỐ HỆ THỐNG ĐIỀU KHIỂN 2.3.1. Hệ thống điều khiển trong không gian khớp a. Hệ thống điều khiển phản hồi Khi thiết kế hệ thống điều khiển, có thể bỏ qua động lực học của cơ cấu chấp hành: quán tính của động cơ và bộ biến đổi. Như vậy chức năng của bộ điều khiển là tạo ra một mômen cần thiết để truyền động khớp robot luôn đảm bảo bám theo vị trí đặt. Sơ đồ khối tổng quát của hệ thống điều khiển phản hồi robot được trình bày trên hình () qd là vecto tín hiệu đặt vị trí của các khớp ( đối với khớp quay và đối với khớp tịnh tiến ) q là vecto vị trí thực của các khớp robot tương ứng là θ va r đối với khớp quay và tịnh tiến, M là vecto momen của các khớp quay và lực đối với khớp tịnh tiến. Hình() Phương trình động lực học tổng quát của robot có dạng: (2-1) Giả thiết thành phần momen trọng lực G(q) được bù hoàn toàn, sơ đồ hệ thống điều khiển phản hồi với cấu trúc điều khiển PD có dạng đơn giản như hình () . Trên cơ sở đó, tín hiệu đặt vị trí được so sánh với vị trí thực của khớp q sai lệch được đặt vào khâu khuếch đại với hệ số . Tín hiệu ra của khâu tỉ lệ được cộng đại số với tín hiệu tỉ lệ với tốc độ của khớp và đặt tới cơ cấu chấp hành của robot: (2-2) Trong đó : -ma trận đường chéo các hệ số khuếch đại của từng khớp - ma trận đường chéo các hệ số đạo hàm của từng khớp Tín hiệu đầu ra của bộ điều khiển (2-2) là momen truyền động cho robot mô tả bởi mô hình động lực học trực tiếp R (2-1), đầu ra của mô hình robot R là vị trí thực của các khớp. Luật điều khiển (2-2) của một khớp không phụ thuộc vào mô hình robot , chỉ phụ thuộc sai lệch vị trí của khớp đó. Do đó bộ dideuefd khiển độc lập với các khớp của robot , nên gọi là bộ điều khiển PD độc lập. Hệ thống với cấu trúc luật điều khiển (2-2) có độ tắt dần lớn sẽ không thích hợp với một số dạng robot. Một dạng hệ thống điều khiển khác trình bày trên hình () với bổ sung thêm tín hiệu đặt tốc độ và sai lệch tốc độ đạt được đặt vào khâu khuếch đại . Khi đó bộ điều khiển có dạng tỉ lệ -đạo hàm (PD) kinh điển: (2-3) Trong đó: = - sai số vị trí của khớp robot =- sai số tốc độ khớp robot -ma trận đường chéo các hệ số khuếch đại của từng khớp - ma trận đường chéo các hệ số đạo hàm của từng khớp Hình ve() b. Hệ thống điều khiển PD bù trọng lực Bộ điều khiển Cơ cấu robot Cảm biến qd q Hình 2.6. Sơ đồ khối tổng quát của hệ thống điều khiển phản hồi Trong sơ đồ trên, qd là vectơ tín hiệu đặt vị trí của các khớp (qd = đối với khớp quay và qd = rd đối với khớp tịnh tiến), q là vectơ vị trí thực của các khớp robot tương ứng là với khớp quay và r với khớp tịnh tiến, là vectơ mômen đối với khớp quay và lực đối với khớp tịnh tiến. Phương trình động lực học tổng quát của robot có dạng: (2-4a) hoặc (2-4b) trong đó q = [q1, q2, …, qn]T , ; là ma trận dương đối xứng; , , là ma trận đối xứng ngược. Từ chương này, ta sẽ dùng q thay cho và qi thay cho để kí hiệu cho các biến khớp nhằm làm tiện lợi hơn trong việc lập trình và mô phỏng với Matlab và Simulink. Luật điều khiển có cấu trúc dạng tỷ lệ - đạo hàm (PD): +) (2-5) +) (2-6) trong đó: Kp = diag(kp1, kp2, …, kpn) là ma trận đường chéo các hệ số khuếch đại của từng khớp riêng biệt. Kd = diag(kd1, kd2, …, kdn) là ma trận đường chéo các hệ số đạo hàm của từng khớp riêng biệt. là sai số vị trí của khớp robot, với qd = [qd1, qd2, …, qdn]T là vectơ vị trí đặt của các khớp robot. là sai số tốc độ khớp robot. Hệ thống điều khiển với cấu trúc điều khiển (2-5), (2-6) đã được chứng minh là ổn định tuyệt đối xung quanh điểm cân bằng, không phụ thuộc vào khối lượng thanh nối và tải dựa vào lý thuyết ổn định Lyapunov. Thật vậy, sau đây ta sẽ trình bày cách chứng minh đối với luật điều khiển (2-5). Đặt biến trạng thái của hệ thống là: . Chọn hàm Lyapunov có dạng: VL = (2-7) Hàm VL biểu thị tổng năng lượng của hệ thống robot: thành phần biểu thị thế năng tích lũy trong hệ thống có hệ số tỷ lệ là Kp, thành phần là động năng robot. Do Kp, M(q) là các ma trận đối xứng dương nên VL > 0 với Tính đạo hàm cấp 1 của VL ta được: (2-8) Do qd là hằng số nên . Vì Kp, M(q) là các ma trận đối xứng dương nên: và Sử dụng các ràng buộc trên, phương trình (2-8) trở thành: (2-9) Cân bằng phương trình luật điều khiển (2-5) và phương trình động lực học robot (2-4b): (2-10) Thay (2-9) vào (2-10) được: (2-11) Vì S(q,) là ma trận đối xứng ngược nên do đó (2-11) trở thành: (2-12) Từ (2-7) và (2-12), theo tiêu chuẩn ổn định Lyapunov, ta có khi t. Như vậy ở trạng thái xác lập, các thành phần tốc độ và gia tốc đều bằng 0 do đó thay vào phương trình hệ thống kín (2-10) sẽ có: c. Hệ thống điều khiển mômen tính toán Phương pháp này còn được gọi là phương pháp điều khiển động lực học ngược hoặc phương pháp điều khiển theo mô hình. Vì robot là một hệ thống ràng buộc và phi tuyến cao nên ta sẽ lựa chọn luật điều khiển sao cho khử được các thành phần phi tuyến của phương trình động lực học robot và phân ly đặc tính động lực học các thanh nối. Do đó ta sẽ nhận được 1 hệ thống tuyến tính, từ đó dễ dàng thiết kế theo các phương pháp kinh điển của hệ thống tuyến tính đảm bảo độ chính xác chuyển động yêu cầu. Sau đây là sơ đồ khối hệ thống: ĐK2 ĐK1 RB U q qd, q, Hình 2.7. Sơ đồ khối hệ thống điều khiển mômen tính toán Trong sơ đồ trên thì q , tương ứng là vectơ vị trí thực và vectơ tốc độ thực của các khớp robot; qd , tương ứng là vectơ vị trí đặt và vectơ tốc độ đặt của các khớp robot; là mô men ở các khớp của robot; U là vectơ tín hiệu điều khiển phụ. Bộ ĐK1 là bộ điều khiển phi tuyến có tác dụng khử tính phi tuyến và ràng buộc của hệ thống robot. Bộ ĐK2 là bộ điều khiển tuyến tính. Phương pháp thiết kế bộ điều khiển mômen tính toán Bộ ĐK1: Dựa vào phương trình động lực học robot (2-1), giả thiết các thông số robot đã biết hoặc được xác định chính xác (tức là M(q), V(q), G(q) đã được xác định chính xác), luật điều khiển của bộ ĐK1 được chọn như sau: (2-13) trong đó U là vectơ tín hiệu điều khiển phụ. Cân bằng hai phương trình (2-1) và (2-13), do M(q) là ma trận thực dương có thể lấy nghịch đảo nên ta thu được phương trình vi phân tuyến tính cấp 2: (2-14) (2-14) là hệ phương trình vi phân tuyến tính cấp 2 độc lập với từng khớp. Do đó có thể thiết kế các bộ điều khiển độc lập có cấu trúc PD cho từng khớp. Bộ ĐK2: Từ lập luận ở trên ta xây dựng luật điều khiển phụ U có cấu trúc PD như sau: U = (2-15) với ; e = qd - q, tương ứng là sai lệch vị trí khớp và sai lệch tốc độ khớp; Kp , Kd là các ma trận đường chéo dương. Các ma trận hệ số Kp, Kd được xác định như sau: Thay (2-14) vào (2-15) ta được: (2-16) Trong đó: là sai số gia tốc khớp. Viết cho khớp thứ i: (2-17) Phương trình đặc tính viết ở dạng toán tử Laplace: (2-18) Các hệ số kpi, kdi được tính toán theo các tiêu chuẩn ổn định và hội tụ. 2.3.2. Hệ thống điều khiển trong không gian làm việc Trong các hệ thống điều khiển không gian khớp được trình bay ở các mục trên, tín hiệu đặt là quỹ đạo chuyển động biểu diễn vị trí khớp theo thời gian, sai lệch điều khiển là sai lệch vị trí khớp. Trong thực tế, chuyển động của robot được đặt trong không gian làm việc( không gian Đecac), do đó thuật toán động lực học ngược cần thiết để biến đổi quỹ đạo đặt trong không gian tay về không gian khớp. Điều đó sẽ làm tăng khối lượng tính toán. Vì vậy, hệ thống điều khiển robot công nghiệp thông thường sẽ tính vị trí của khớp thông qua bài toán động học ngược, và sau đó tính toán tốc độ và gia tốc khớp bằng phương pháp vi phân số. ở hệ thống không gian làm việc, tín hiệu đặt trực tiếp là quỹ đạo chuyển động mong muốn của tay robot trong không gian làm việc, lượng phản hồi sẽ tính từ vị trí khớp thông qua khâu động học thuận. Khâu tính động học ngược được cài đặt trong mạch vòng điều khiển phản hồi sẽ tính đổi các biến về không gian khớp. Hai hệ thống điều khiển điển hình là : Hệ thống điều khiển ma trận chuyển vị và hệ thống điều khiển ma trận nghịch đảo. Hệ thống điều khiển ma trận chuyển vị Lực cần thiết để di chuyển tay theo quỹ đạo đặt trong không gian làm việc được xác định từ sai lệch vị trí và sai lệch tốc độ trong không gian làm việc theo luật điều khiển phản hồi PD kinh điển: (2-19) với: F là lực cần thiết để tay robot di chuyển theo quỹ đạo và tốc độ đặt trước. Xd , X tương ứng là vectơ vị trí đặt và vectơ vị trí thực của tay robot. , tương ứng là vectơ tốc độ đặt và vectơ tốc độ thực của tay robot. Kp = diag(kp1, kp2, …, kpn) là ma trận đường chéo các hệ số khuyếch đại. Kd = diag(kd1, kd2, …, kdn) là ma trận đường chéo các hệ số đạo hàm. Công thực hiện ở tay được tính theo công thức: FT . (với F là lực tác dụng ở tay, là vectơ dịch chuyển nhỏ của tay). Công thực hiện ở khớp được tính theo công thưc: . (với là mômen hoặc lực ở khớp, là vectơ dịch chuyển nhỏ của khớp). Công thực hiện ở tay sẽ cân bằng với công thực hiện ở khớp: FT . = . (2-20) Mặt khác lại có: (với J là ma trận Jacobian) (2-21) Thay (2-21) vào (2-20) thu được: (2-22) Thay (2-19) vào (2-22) xác định được vectơ mômen truyền động khớp như sau: (2-23) Để bù được thành phần trọng lực của robot thì phải dùng luật điều khiển sau: (2-24) Từ (2-24) ta xây dựng được sơ đồ khối hệ thống điều khiển vị trí tay robot sử dụng ma trận Jacobian chuyển vị như sau: KP Kd RB ĐHT J Xd F q X JT + + Kp G(q) RB + Tính G(q) G(q) Hình 2.8. Sơ đồ khối hệ thống điều khiển ma trận Jacobian chuyển vị Hệ thống điều khiển ma trận Jacobi nghịch đảo Sai lệch vị trí và sai lệch tốc độ của tay robot được định nghĩa như sau: , trong đó Xd , X tương ứng là vectơ vị trí đặt và vectơ vị trí thực của tay robot, , tương ứng là vectơ tốc độ đặt và vectơ tốc độ thực của tay robot. Khi coi sai lệch vị trí và sai lệch tốc độ của tay robot là các đại lượng nhỏ thì sai lệch vị trí khớp và sai lệch tốc độ khớp robot được xác định bởi phương trình sau: trong đó tương ứng là sai lệch vị trí khớp và sai lệch tốc độ khớp robot; J là ma trận Jacobian. Tương tự như luật phản hồi PD kinh điển (trong hệ thống điều khiển robot tập trung), vectơ mômen cần thiết truyền động cho khớp robot được xác định như sau: (2-25) trong đó Kp , Kd là các ma trận đường chéo dương. Do đó phương trình (2-25) được viết lại: (2-26) Để bù được thành phần trọng lực của robot thì phải dùng luật điều khiển sau: + G(q) (2-27) KP Kd RB ĐHT J Xd F q X J-1 + + Kp G(q) RB + G(q) Hình 2.9. Sơ đồ khối hệ thống điều khiển ma trận Jacobian nghịch đảo . CHƯƠNG 3 ROBOT BA BẬC TỰ DO 3.1. GIỚI THIỆU VỀ ROBOT BA BẬC TỰ DO Robot ba bậc tự do EC do hãng Feedback của Anh sản xuất phục vụ cho mục đích nghiên cứu, được biểu diễn như hình 2.1. Robot có 5 trục quay và 1 bàn kẹp, tuy nhiên ba khớp khởi động đầu tiên ( tương ứng với chuyển động của các trục “ Hông”, “ Vai” và “ Cánh tay”) được gọi là bộ phận cơ bản vì trước hết, nhờ chúng tay máy có thể thực hiện bước chủ yếu trong thao tác định vị, tức là đưa bàn kẹp đến lân cận điểm làm việc, sau đó nhờ các khớp động còn lại bàn kẹp được định hướng và vi chỉnh đến vị trí gia công chính xác. Hình 3.1. Robot ba bậc tự do Robot được điều khiển bởi bốn vi xử lý cho phép điều khiển đặt vật chính xác. Mỗi trục của Robot được điều khiển bởi một động cơ bước với bộ mã hóa phản hồi. Trong bộ điều khiển, một vi xử lý sẽ giám sát vị trí các trục. Hai cái khác sẽ quản lý các động cơ và cái còn lại sẽ giám sát cả ba cái trên đồng thời làm nhiệm vụ giao tiếp với máy chủ. 3.1.1. Các thông số động học của Robot ba bậc tự do Các thông số động học cơ bản của Robot ba bậc tự do với các thông số cụ thể như sau: Bảng 1. Các thông số động học cơ bản của Robot ba bậc tự do STT Thông số Ghi chú 1 M1= 15 kg Khối lượng thanh nối 1 2 M2 = 12 kg Khối lượng thanh nối 2 3 M3 = 8 kg Khối lượng thanh nối 3 4 D=185 mm Chiều cao từ chân đế đến giá quay của khớp 1 5 L1=135 mm Kích thước động học thanh nối 1 6 L2=225 mm Kích thước động học thanh nối 2 7 L3=225 mm Kích thước động học thanh nối 3 8 R = 30 mm Bán kính trục quay 1 Hình 3.2. Các thông số động học 3.1.2. Vùng làm việc của Robot ba bậc tự do EC Vùng làm việc của Robot ba bậc tự do khi khảo sát với 3 trục khớp động đầu tiên được xác định theo số liệu như sau: Hình 3.3. Vùng làm việc của các chuyển động a. Chuyển động 1, b. Chuyển động 2, c. Chuyển động 3 Chuyển động quay của khớp thứ nhất có hình chiếu bằng trong hệ trục tọa độ và như hình 2.3a với góc quay tổng là 2700 . Chuyển động quay của khớp thứ hai có hình chiếu bằng trong hệ trục tọa độ OX1Y1 và OX2Y2 như hình 2.3b với góc quay tổng là 1650. Chuyển động quay của khớp thứ ba có hình chiếu bằng trong hệ trục tọa độ OX2Y2 và OX3Y3 như hình 2.3c với góc quay tổng là 3000. 3.2. CÁC PHÉP BIẾN ĐỔI TỌA ĐỘ DÙNG MA TRẬN THUẦN NHẤT 3.2.1. Biểu diễn ma trận Ma trận được sử dụng để biểu diễn một điểm, một vectơ, một khung tọa độ, các phép biến đổi tịnh tiến, quay và biểu diễn một đối tượng trong một khung tọa độ. a. Biểu diễn một vectơ trong không gian Tùy thuộc vào hệ qui chiếu được chọn, trong không gian 3 chiều, một điểm V có thể được biểu diễn bằng nhiều vectơ khác nhau: Hình 3.4. Biểu diễn một điểm trong không gian và là hai vectơ khác nhau mặc dù cả hai vectơ cùng biểu diễn điểm V. Nếu , , là các vectơ đơn vị của một hệ trục tọa độ nào đó (chẳng hạn E), ta có: Với a, b, c là tọa độ vị trí của điểm V trong hệ đó. Trong kỹ thuật robot, vectơ được biểu diễn bằng một ma trận cột và bổ sung thêm thành phần thứ tư là hệ số tỷ lệ w như sau: với , , , w có giá trị 1 hoặc 0. Nếu w= 1 thì giá trị các thành phần không thay đổi, khi đó 3 thành phần x, y, z sẽ biểu diễn 1 điểm. Nếu w = 0 thì các thành phần a, b, c là ∞, khi đó 3 thành phần x, y, z sẽ biểu diễn hướng của một vectơ và x, y, z là các thành phần của vectơ đơn vị của các trục. b. Biểu diễn một khung tọa độ Hình 3.5. Biểu diễn khung tọa độ B trong hệ khung tọa độ A Gốc khung tọa độ B biểu diễn tương đối so với khung tọa độ chuẩn bằng vectơ . Khung tọa độ B được biểu diễn ở dạng ma trận 4x4 trong đó 3 cột đầu biểu diễn hướng của khung tọa độ B, cột thứ tư xác định vị trí của gốc khung tọa độ B: (3-1) Ma trận gọi là ma trận biến đổi đồng nhất. c. Biểu diễn một đối tượng trong không gian Gắn một khung tọa độ lên đối tượng ( gọi là “Khung tọa độ đối tượng”) và xác định vị trí của khung đối tượng đó trong không gian. Đối tượng gắn cố định ở “Khung tọa độ đối tượng” nên có thể xác định được vị trí và hướng của đối tượng đó trong ” Khung tọa độ đối tượng”. Do đó khi đã xác định được mối quan hệ giữa “Khung tọa độ đối tượng” trong khung tọa độ chuẩn, sẽ xác định được vị trí và hướng của đối tượng so với khung tọa độ gốc cố định. Hình 3.6. Biểu diễn một vật thể rắn trong không gian Ma trận biểu diễn “ Khung tọa độ đối tượng” trong khung tọa độ chuẩn: (3-2) Một đối tượng có sáu bặc tự do nghĩa là đối tượng có thể di chuyển dọc theo ba trục x, y, z và quay xung quanh 3 trục đó. Do đó cần 6 thông số để mô tả vị trí đối tượng trong khung tọa độ chuẩn và hướng đối tượng so với các khung của tọa độ chuẩn. Nhưng ma trận (3-2) có 12 thành phần: 9 thành phần mô tả hướng và 3 thành phần xác định vị trí bởi vậy cần phải có sáu phương trình ràng buộc để khử 6 thông số và số thông số độc lập là 6. Các điều kiện ràng buộc này là thuộc tính của một khung tọa độ: 3 vectơ đơn vị vuông góc với nhau và môđun của các vectơ đơn vị bằng 1. Hai thuộc tính này có thể biểu diễn bởi 6 phương trình ràng buộc: (3-3) 3.2.2. Các phép biến đổi a. Phép biến đổi tịnh tiến đơn Một khung tọa độ di chuyển trong không gian nhưng không thay đổi hướng của nó sẽ tương ứng với một phép biến đổi tịnh tiến. Khi đó các vectơ đơn vị của khung tọa độ đó không thay đổi hướng, gốc tọa độ của khung tọa độ sẽ di chuyển tương đối so với khung tọa độ gốc. Dùng ma trận 4x4 để biểu diễn 1 phép biến đổi tịnh tiến: F = Trans(a,b,c) = (3-4) b. Phép biến đổi quay đơn Ta vẫn dùng ma trận 4x4 để biểu diễn phép quay một góc θ quanh một trục nào đó: (3-5) (3-6) (3-7) c. Phép biến đổi kết hợp Tổng quát, phép biến đổi có thể gồm một số phép biến đổi tịnh tiến và quay so với khung tọa độ cố định hoặc khung tọa độ đang chuyển động. Xét một phép biến đổi kết hợp ba phép biến đổi đơn theo thứ tự sau: 1. Quay xung quanh trục x một góc . 2. Tịnh tiến dọc theo các trục lần lượt là a, b, c. Ta có 2 trường hợp: Trường hợp 1: Các phép biến đổi trên được thực hiện so với khung tọa độ gốc ( khung tọa độ ban đầu, cố định).Ma trận biểu diễn phép biến đổi kết hợp: Trường hợp 2: Các phép biến đổi trên được thực hiện so với khung tọa độ di chuyển ( khung tọa độ hiện tại).Ma trận biểu diễn phép biến đổi kết hợp: 3.2.3. Phép biến đổi biểu diễn vị trí và hướng của tay robot so với thân robot Một robot bất kỳ nào cũng có thể coi là tập hợp các thanh nối (links) gắn liền với nhau bởi các khớp (joints). Ta hãy đặt trên mỗi thanh nối của robot một hệ tọa độ. Sử dụng các phép biến đổi tịnh tiến và quay có thể mô tả vị trí và hướng tương đối giữa các hệ trục tọa độ này. Dùng một ma trận A để mô tả cho phép biến đổi biễu diễn vị trí và hướng của hệ tọa độ của thanh nối so với hệ tọa độ của thanh nối kề trước nó. A1 mô tả vị trí và hướng của thanh nối đầu tiên, A2 mô tả vị trí và hướng của thanh nối thứ hai so với thanh nối thứ nhất. Như vậy vị trí và hướng của thanh nối thứ hai so với hệ tọa độ gốc được biểu diễn bởi ma trận: T2 = A1.A2 A3 mô tả vị trí và hướng của thanh nối thứ ba so với thanh nối thứ hai thì vị trí của thanh nối thứ ba so với tọa độ gốc là: T3 = A1.A2.A3 Nếu một robot có 6 thanh nối, ta có ma trận biểu diễn vị trí và hướng của thanh nối cuối so với hệ tọa độ gốc là: T6 = A1.A2.A3. A4.A5.A6 (3-8) Một robot 6 thanh nối tức là có 6 bậc tự do vì vậy có thể xác định được vị trí và hướng của tay robot (hand), 3 bậc tự do xác định vị trí và 3 bậc tự do khác xác định hướng. T6 sẽ là ma trận biểu diễn cả hướng và vị trí của tay robot (hand) so với khung tọa độ gốc. Gắn một khung tọa độ lên tay robot (hand) và tìm phép biến đổi biểu diễn khung tọa độ tay trong khung tọa độ gốc. Từ đó sẽ xây dựng được hệ phương trình động học thuận biểu diễn quan hệ giữa vị trí và hướng của tay so với các biến khớp. Hình 3.7. Khung tọa độ tay trong khung tọa độ gốc Hình 3.7 biểu diễn khung tọa độ tay trong khung tọa độ gốc. Gốc của khung tọa độ tay đặt tại điểm giữa của các ngón tay, được biểu diễn bởi vectơ trong khung tọa độ gốc. Ba vectơ đơn vị mô tả hướng của bàn tay được xác định như sau: Vectơ có hướng mà theo đó bàn tay sẽ tiếp cận đến đối tượng gọi là vectơ (approach). Vectơ có hướng mà theo đó các ngón tay của bàn tay nắm vào nhau khi cầm, nắm đối tượng gọi là vectơ (orientation). Vectơ cuối cùng là vectơ pháp tuyến (normal) Như vậy ta có: = x Hệ tọa độ tay của Robot được biểu diễn bằng một ma trận 4 x 4: (3-9) Hay TE có dạng: Trong đó: R (3x3) là ma trận chỉ hướng của bàn tay Robot P(3x1) là ma trận biểu diễn vị trí của tay robot 3.3. Bài toán động lực học thuận Với một robot đã biết cấu hình như: độ dài các thanh nối và góc quay các khớp hoặc độ dịch chuyển các khớp tịnh tiến, bài toán động lực học thuận là tính toán vị trí và hướng của tay robot tương ứng với cấu hình robot xác định. Ngược lại, động học ngược sẽ tính toán các góc quay hoặc độ dịch chuyển của các khớp tịnh tiến tương ứng với vị trí và hướng của tay robot, nghĩa là tính toán các giá trị biến khớp cần thiết để đặt tay robot đúng vị trí mong muốn. Phương pháp chung để thiết lập phương trình động học thuận robot là sử dụng công cụ ma trận để miêu tả: vị trí, hướng và chuyển động sau đó sử dụng phép sử dụng phép biểu diễn Denavit Hartenberg. 3.3.1. Phương pháp thiết kế khung tọa độ - Phép biểu diễn Danevit – Hartenberg a. Tham số của thanh nối và khớp Xét 3 trục khớp trong không gian như hình sau: Hình 3.8. Thiết kế khung tọa độ thanh nối Trong đó: an: Độ dài pháp tuyến chung của trục khớp n và n+1, chính là độ dài thanh nối n. dn: Khoảng cách giữa hai chân pháp tuyến chung của trục n. : Góc giữa hai trục của khớp n và khớp n+1. : Góc giữa hai pháp tuyến chung của trục khớp n. b. Nguyên tắc thiết kế khung tọa độ Theo phương pháp biểu diễn Danevit – Hartenberg (D-H), khung tọa độ thanh nối n được xây dựng theo nguyên tắc sau: + Gốc của khung tọa độ thanh nối n đặt tại giao điểm của pháp tuyến an với trục khớp thứ n+1. Trường hợp hai trục khớp cắt nhau, gốc khung tọa độ sẽ đặt tại chính giao điểm và trục x được đặt dọc theo đường vuông góc với mặt phẳng chứa hai trục z đó. Nếu các trục khớp song song với nhau, sẽ có nhiều pháp tuyến chung. Khi đó sẽ chọn pháp tuyến chung trùng với pháp tuyến chung của khớp trước. Gốc khung tọa độ chọn sao cho dn nhỏ nhất. + Trục zn của khung tọa độ thanh nối n đặt theo phương của trục khớp n+1. + Trục xn thường được đặt dọc theo pháp tuyến chung của trục n và n+1 theo hướng từ trục n đến n+1. + Đối với khớp tịnh tiến thì khoảng cách dn là biến khớp, hướng của trục khớp trùng với hướng di chuyển. Khi đó chiều dài an không có ý nghĩa nên đặt an = 0. Gốc tọa độ đặt trùng với gốc thanh nối tiếp theo. + Đối với khớp quay thì θn là biến khớp và dn= const. Trục xn được chọn sao cho thực hiện được phép quay từ zn-1 đến zn. Các thông số an, , dn và θn được gọi là bộ thông số DH. c. Quan hệ giữa hai khung tọa độ n-1 và n Một cách tổng quát, quan hệ giữa hai khung tọa độ n và n-1 được xác định bằng các phép biến đổi theo thứ tự sau đây. Quay quanh trục zn-1 một góc θn sao cho trục xn-1 trùng với phương của trục xn. Tịnh tiến dọc theo trục zn-1 một khoảng dn để gốc khung tọa độ mới trùng với chân pháp tuyến chung của trục khớp n và trục khớp n+1. Tịnh tiến dọc theo trục xn-1 (phương pháp tuyến chung) một đoạn an. Quay quanh trục xn-1 một góc sao cho trục zn-1 trùng với trục zn. Các phép biến đổi trên được thực hiện so với khung tọa độ hiện tại (khung tọa độ ngay trước đó. Do đó phép biến đổi kết hợp được xác định như sau: (3-10) 3.3.2. Phương trình động học thuận của robot ba bậc tự do Trình tự thiết lập hệ phương trình động học của robot. Để thiết lập hệ phương trình động học của robot, ta tiến hành theo các bước sau: 1. Chọn hệ tọa độ cơ sở, gắn các hệ tọa độ mở rộng lên các thanh nối. 2. Lập bảng thông số DH (Dennavit Hartenberg). 3. Dựa vào thông số DH xác định ma trận An 4. Tính ma trận T và viết các phương trình động học của robot Robot ba bậc tự do có 5 bậc tự do nhưng trong đồ án này ta chỉ xét đến bậc thứ 3 (3 thanh nối). Khi áp dụng phương pháp Denavit – Hartenberg gắn các hệ trục tọa độ vào các khâu, ta thu được sơ đồ động học của robot ba bậc tự do như hình 2.9. Hình 3.9. Khung tọa độ thanh nối Theo thuật toán D-H ta có bảng tham số D-H ứng với sơ đồ động học trên: Bảng 2. Bảng tham số D-H Trục θ a d 1 θ1 90 0 l1 2 θ2 0 l2 0 3 θ3 0 l3 0 Các ma trận chuyển tương ứng: Vị trí và hướng của thanh nối thứ hai so với hệ tọa độ gốc được biểu diễn: Phương trình động học thuận biểu diễn quan hệ giữa vị trí, hướng của tay và vị trí của cá khớp. (3-11) Ma trận biểu diễn hướng của tay Robot: (3-12) (3-13) Để tìm miền làm việc của Robot ta dựa vào phương trình điểm tác động cuố (3-14) 3.3.3. Ma trận Jacobian Ý nghĩa của ma trận Jacobian là: Biểu diễn quan hệ giữa tốc độ của tay và tốc độ của khớp: (3-15) Biểu diễn quan hệ giữa dịch chuyển vi sai của tay và dịch chuyển vi sai của khớp: (3-16) Do đó ma trận Jacobian thu được bằng cách lấy vi phân phương trình động học thuận = Hàng 1 của ma trận Jacobian Tính hàng 2 của ma trận Jacobian = Tính hàng 3 của ma trận Jacobian = Như vậy ta có ma trận Jacobien: Và: (3-17) Nếu chia cả 2 vế cho dt sẽ nhận được phương trình quan hệ tốc độ tay Robot và tốc độ khớp. 3.4. PHƯƠNG TRÌNH ĐỘNG LỰC HỌC CỦA ROBOT BA BẬC TỰ DO 2.4.1. Phương trình Lagrange Phương trình động lực học robot được thiết lập từ phương trình Lagrange: (3-18) Trong đó: với là momen khớp i. (3-19) Với: L là hàm Lagrange là hàm tổng động năng, là động năng của thanh nối i. là hàm tổng thế năng, là thế năng của thanh nối i. Thế (3-19) vào (3-18) ta được: (3-20) (vì ) Dạng tương đương của phương trình (3-20): (3-21) Với i = 1, 2, 3 3.4.2.Động năng và thế năng của các thanh nối a. Động năng và thế năng của thanh nối 1 (3-22) Nên (3-23) (3-24) b. Động năng và thế năng của thanh nối 2 (3-25) (3-26) (3-27) c. Động năng và thế năng của thanh nối 3 (3-28) (3-29) (3-30) d. Tổng động năng và thế năng (3-31) (3-32) (3-33) 3.4.3. Phương trình động lực học viết cho các thanh nối a. Phương trình động lực học viết cho thanh nối 1 Phương trình động học viết cho thanh nối 1: (3-34) Với: b. Phương trình động lực học viết cho thanh nối 2 Phương trình động học viết cho thanh nối 2: (2-35) Với: c. Phương trình động lực học viết cho thanh nối 3 Phương trình động học viết cho thanh nối 3: (3-36) Với d. Phương trình động lực học tổng quát của robot ba bậc tự do Kết hợp 3 phương trình (3-34), (3-35), (3-36) thu được phương trình động lực học tổng quát của robot ba bậc tự do có dạng: (3-37) Trong đó: M() là ma trận dương đối xứng Biểu diễn: thì (3-37) trở thành: (3-38) là ma trận 3x3, ma trận này không tồn tại duy nhất tuy nhiên ta cần tìm ma trận sao cho là ma trận 3x3 đối xứng ngược thỏa mãn: với hay Theo [TL - 4], có 1 cách tính để tìm ra được ma trận như sau: (2-39) với , i, j, k = 1, 2, 3 (2-40) Theo (3-39) và (3-40) sẽ tính được các phần tử của ma trận C(): Kiểm tra lại công thức ta thấy thỏa mãn. S được tính theo theo biểu thức: S= - (3-41) Trước hết tính từ M(): Thay và vào (3-41) tính được S(): S11 = 0, S22 = 0, S33 = 0 Thay C() = vào (3-38) thì phương trình động lực học trở thành: (3-42) 3.5. XÂY DỰNG MÔ HÌNH ĐIỀU KHIỂN ROBOT BA BẬC TỰ DO 3.5.1. Hệ thống điều khiển trong không gian khớp a. Hệ thống điều khiển phản hồi PD bù trọng lực + Hệ phương trình động lực học của robot có dạng tổng quát: (3-43) trong đó ; M(q) , V(,) và G(q) đã được xác định . + Luật điều khiển có dạng (3-43): (3-44) với qd = [qd1, qd2, qd3]T ; Kp , Kd là các ma trận đường chéo dương. + Cân bằng (3-43) và (3-44) ta được: (3-45) Từ (3-45) ta xây dựng sơ đồ mô phỏng trong Simulinhk như hình 3.10. Các file có đuôi “.m” đi kèm theo phục vụ cho việc mô phỏng được trình bày trong phần phụ lục. Các tham số của robot: m1 = 15 kg, m2 = 12 kg, m3 = 8 kg; l1 =0.135 m; l2 = l3 =0.225 m; lg1 = l1/2, lg2 = l2/2, lg3 = l3/2, mômen quán tính các thanh nối: Hình 3.10. Sơ đồ mô phỏng hệ điều khiểnphản hồi PD bù trọng lực trong không gian khớp + Để điều khiển điểm đặt với vectơ vị trí đặt của các khớp qd = [qd1, qd2, qd3]T = [1, 1.5, 1.3]T, thay đổi kp trong khoảng [200 , 300] và thay đổi kd trong khoảng [20 , 50], ta chọn được các ma trận hệ số Kp và Kd cho chất lượng hệ thống tốt nhất: và Sau đây là kết quả mô phỏng các góc quay của các khớp 1, 2 và 3: Hình 3.11. Góc quay khớp 1 Hình 3.12. Góc quay khớp 2 Hình 3.13. Góc quay khớp 3 Nhận xét kết quả mô phỏng: +) Từ các hình 3.11, 3.12, 3.13 ta thấy hệ thống điều khiển phản hồi PD bù trọng lực trong không gian khớp đã thiết kế là ổn định với các chỉ tiêu chất lượng sau: - Không có độ quá điều chỉnh. - Sai lệch tĩnh bằng 0. - Thời gian quá độ tương đối nhỏ (nhỏ hơn 1s). Đó là vì: ta đã bỏ qua các lực ma sát trong hệ thống cơ khí của robot, bỏ qua quán tính của cơ cấu chấp hành, bỏ qua nhiễu, …, coi đáp ứng mô men là tức thời. Bởi vậy trong thực tế, chắc chắn thời gian quá độ sẽ lớn hơn. +) ảnh hưởng của ma trận hệ số Kp, Kd tới chất lượng của hệ thống điều khiển: - Giữ nguyên kp: khi tăng kd trong khoảng [10 , 25] thì độ quá điều chỉnh giảm, thời gian quá độ giảm và khi giảm kd thì ngược lại. - Giữ nguyên kd: khi tăng kp trong khoảng [100 , 200] thì thời gian quá độ giảm và khi giảm kp thì ngược lại. b. Hệ thống điều khiển mômen tính toán Phương trình mô tả bộ điều khiển mô men tính được chọn như sau: Bộ ĐK1: Dựa vào phương trình động lực học robot (2-1), giả thiết các thông số robot đã biết hoặc được xác định chính xác (tức là M(q), V(q), G(q) đã được xác định chính xác), luật điều khiển của bộ ĐK1 được chọn như sau: (3-46) trong đó U là vectơ tín hiệu điều khiển phụ. Cân bằng hai phương trình (2-1) và (3-46), do M(q) là ma trận thực dương có thể lấy nghịch đảo nên ta thu được phương trình vi phân tuyến tính cấp 2: (3-47) (3-47) là hệ phương trình vi phân tuyến tính cấp 2 độc lập với từng khớp. Do đó có thể thiết kế các bộ điều khiển độc lập có cấu trúc PD cho từng khớp. Bộ ĐK2: Từ lập luận ở trên ta xây dựng luật điều khiển phụ U có cấu trúc PD như sau: U = (3-48) với ; e = qd - q, tương ứng là sai lệch vị trí khớp và sai lệch tốc độ khớp; Kp , Kd là các ma trận đường chéo dương. Các ma trận hệ số Kp, Kd được xác định như sau: Thay (3-47) vào (3-48) ta được: (3-49) Trong đó: là sai số gia tốc khớp. Viết cho khớp thứ i: (3-50) Phương trình đặc tính viết ở dạng toán tử Laplace: (3-51) Từ đó ta xây dựng sơ đồ mô phỏng trong Simulinhk như hình 3.14. Các file có đuôi “.m” đi kèm theo phục vụ cho việc mô phỏng được trình bày trong phần phụ lục. Hình 3.14. Sơ đồ Simulink mô ta hệ thống điều khiển mômen tính toán Hình 3.15. Góc quay khớp 1 Hình 3.16. Góc quay khớp 2 Hình 3.17. Góc quay khớp 3 3.5.2. Hệ thống điều khiển không gian làm việc a. Hệ thống điều khiển ma trận Jacobian chuyển vị + Các thông số của robot vẫn giữ nguyên như trong mục 3.5.1 + Luật điều khiển có dạng (2-24): với là vectơ mô men ở các khớp; J là ma trận Jacobian, đã xác định trong chương 2; Kp , Kd là các ma trận đường chéo dương; Xd , X tương ứng là vectơ vị trí đặt và vectơ vị trí thực của tay robot, , tương ứng là vectơ tốc độ đặt và vectơ tốc độ thực của tay robot; q là vị trí thực của các khớp robot; G(q) là thành phần trọng lực robot, đã xác định trong chương 3. Sơ đồ mô phỏng trong Simulink như hình 3.18 (các file có đuôi “.m” đi kèm được trình bày trong phần phụ lục). Hình 3.18. Sơ đồ Simulink mô phỏng hệ thống điều khiển ma trận Jaccobian chuyển vị + Để điều khiển điểm đặt với vectơ trị đặt của tay Xd = = [0.3, 0.16, 0.25]T, thay đổi hệ số kp trong khoảng [100 , 200] và thay đổi hệ số kd trong khoảng [5 , 50], ta chọn được các ma trận hệ số Kp và Kd cho chất lượng hệ thống tốt nhất: và Kết quả mô phỏng tọa độ x, y và z của điểm tác động cuối của tay so với phương ngang như sau: Hình 3.19. Tọa độ x của điểm tác động cuối Hình3.20. Tọa độ y của điểm tác động cuối Hình 3.21. Tọa độ z của điểm tác động cuối Nhận xét kết quả mô phỏng: +) Từ các hình 3.19, 3.20, 3.21 ta thấy hệ thống điều khiển ma trận Jacobian chuyển vị đã thiết kế là ổn định với các chỉ tiêu chất lượng: - Không có độ quá điều chỉnh. - Sai lệch tĩnh bằng 0. - Thời gian quá độ tương đối nhỏ(khoảng 2s). Giải thích như trong mục 3.5.1 +) ảnh hưởng của các ma trận hệ số Kp, Kd tới chất lượng hệ thống: - Giữ nguyên kd: khi tăng kp trong khoảng [100 , 200] thì thời gian quá độ giảm và khi giảm kp thì ngược lại. - Giữ nguyên kp: khi tăng kd trong khoảng [5 , 50] thì thời gian quá độ giảm, độ quá điều chỉnh giảm và khi giảm kd thì ngược lại. b. Hệ thống điều khiển ma trận Jacobian nghịch đảo + Các thông số robot vẫn giữ nguyên như trong mục 3.5.1. + Luật điều khiển có dạng (2-27): + G(q) với là vectơ mô men ở các khớp; J là ma trận Jacobian, đã xác định trong chương 2; Kp , Kd là các ma trận đường chéo dương; Xd , X tương ứng là vectơ vị trí đặt và vectơ vị trí thực của tay robot, , tương ứng là vectơ tốc độ đặt và vectơ tốc độ thực của tay robot; q là vị trí thực của các khớp robot; G(q) là thành phần trọng lực robot, đã xác định trong chương 3. Sơ đồ mô phỏng trong Simulink như hình 3.22 nhưng luật điều khiển thì khác (các file có đuôi “.m” đi kèm theo được trình bày trong phần phụ lục). Hình 3.22. Sơ đồ Simulink mô phỏng hệ thống điều khiển ma trận Jaccobian nghịch đảo + Để điều khiển điểm đặt với vectơ vị trí đặt của tay Xd = = [0.3, 0.16, 0.25]T, thay đổi hệ số kp trong khoảng [50 , 150] và thay đổi hệ số kd trong khoảng [5 , 30], ta chọn được các ma trận hệ số Kp và Kd cho chất lượng hệ thống tốt nhất: và Kết quả mô phỏng tọa độ x, y và z như sau: Hình 3.23. Tọa độ x của điểm tác động cuối Hình 3.24. Tọa độ y của điểm tác động cuối Hình 3.25. Tọa độ z của điểm tác động cuối Nhận xét kết quả mô phỏng: +) Từ các hình 3.23, 3.24, 3.25 ta thấy hệ thống điều khiển ma trận Jacobian nghịch đảo là ổn định với các chỉ tiêu chất lượng: - Không có độ quá điều chỉnh. - Sai lệch tĩnh bằng 0. - Thời gian quá độ tương đối nhỏ (nhỏ hơn 1s). Giải thích như trong mục 3.5.1. +) ảnh hưởng của các ma trận hệ số Kp và Kd tới chất lượng hệ thống: - Giữ nguyên kd: khi tăng kp trong khoảng [50 , 120] thì thời gian quá độ giảm và khi tăng kp trong khoảng [120 , 150] thì xảy ra hiện tượng quá điều chỉnh. - Giữ nguyên kp: khi tăng kd trong khoảng [5 , 15] thì độ quá điều chỉnh giảm, thời gian quá độ giảm và khi tăng kd trong khoảng [15 , 30] thì thời gian quá độ tăng. KẾT LUẬN Sau mười hai tuần làm đồ án tốt nghiệp, được sự hướng dẫn của cô giáo ThS. Phạm Thị Hồng Anh bản đồ án tốt nghiệp của em đã hoàn thành. Đồ án đã thực hiện được những vấn đề sau: Nghiên cứu các phương pháp điều khiển Robot công nghiệp. Xây dựng được phương trình động học vị trí và động lực học của Robot ba bậc tự do. Mô phỏng, khảo sát đáp ứng của Robot công nghiệp với bốn phương pháp điều khiển Robot trong công nghiệp. Tuy nhiên, do thời gian và trình độ có hạn nên bản đồ án của em chưa giải quyết được những vấn đề sau: -Mô hình mô phỏng còn nhiều hạn chế. Dù đã cố gắng nhưng bản đồ án của em không tránh khỏi những thiếu sót. Em rất mong nhận được sự đóng góp ý kiến của các thầy cô giáo để bản đồ án tốt nghiệp của em được hoàn thiện hơn. Em xin chân thành cảm ơn! TÀI LIỆU THAM KHẢO [1]. GS.TSKH. Nguyễn Thiện Phúc, “ Robot công nghiệp”, Nhà xuất bản Khoa học và kỹ thuật . [2]. TS. Phạm Đăng Phước, “Giáo trình Robot công nghiệp”, Đại học Bách khoa Đà Nẵng. [3]. TS. Nguyễn Mạnh Tiến, “Điều khiển Robot công nghiệp”, Nhà xuất bản khoa học và kỹ thuật Hà Nội. [4]. Nguyễn Doãn Phước, Lý thuyết điều khiển tuyến tính , Nhà xuất bản Khoa học và kỹ thuật . [5]. Nguyễn Doãn Phước, Lý thuyết điều khiển phi tính , Nhà xuất bản Khoa học và kỹ thuật . [6]. Phạm Công Ngô (2001), Lý thuyết điều khiển tự động, Nhà xuất bản khoa học và kỹ thuật, Hà nội. [7]. Bộ môn Điện tự động công nghiệp, “Bài giảng Điều khiển robot”, Đại học Hàng hải Việt Nam. [8]. Nguyễn Phùng Quang (2005), “Matlab và Simulink dành cho kỹ sư điều khiển tự động”, Nhà xuất bản khoa học và kỹ thuật. [9] Tài liệu tham khảo trên Internet. PHỤ LỤC PL1. Các file có đuôi “.m” đi kèm với sơ đồ Simulink hình 3.10 để mô phỏng hệ thống điều khiển phản hồi PD bù trọng lực trong không gian khớp. % File “thongso_robot.m” global l1 l2 l3 m1 m2 m3 ; % cac thong so cua robot m1=15 m2=12 m3=8 l1=0.135 l2=0.225 l3=0.225 % File “M_term.m” function out= M_term(q1,q2,q3); % ham tinh ma tran M(q) thongso_robot lg1=l1/2 lg2=l2/2 lg3=l3/2 % khoang cach tu dau thanh noi den tam khoi r1=0.03 J1=m1*r1*r1 J2=m2*l2*l2/3 J3=m3*l3*l3/3 % momen quan tinh cac thanh noi qk11=m2*lg2^2*cos(q2)^2+J1 qk12=m3*(lg3^2*cos(q2+q3)^2+2*lg3*lg2*cos(q2)*cos(q2+q3)+l2^2*cos(q2)^2) out(1,1)=qk11+qk12; % phan tu M11 out(1,2)=0; % phan tu M12 out(1,3)=0; % phan tu M13 out(2,1) =out(1,2); % phan tu M21 out(2,2) =m2*lg2^2+J2+J3+ m3*(lg3^2+l2^2+2*l2*lg3*cos(q3)); % phan tu M22 out(2,3)=m3*lg3^2+J3+m3*l2*lg3*cos(q3); % phan tu M23 out(3,1)=out(1,3); % phan tu M31 out(3,2)=out(2,3); % phan tu M32 out(3,3)=m3*lg3^2+J3; % phan tu M33 % File “V.m” function out = V(u); % ham tinh V(q, ) thongso_robot lg1=l1/2 lg2=l2/2 lg3=l3/2 q1=u(1);q2=u(2);q3=u(3);% vi tri khop dq1=u(4); dq2=u(5); dq3=u(6); % toc do khop lsq31=-2*m2*lg2^2*sin(q2)*cos(q2)*dq2*dq1 lsq32=-2*m3*(lg3*cos(q2+q3) +l2*cos(q2))*(lg3*sin(q2+q3)+l2*sin(q2))*dq1*dq2 lsq33=-2*m3*(lg3*cos(q2+q3)+l2*cos(q2))*lg3*sin(q2+q3)*dq1*dq3 out(1)=lsq31+lsq32+lsq33; % phan tu V1 lsq41=-m2*lg2^2*sin(q2)*cos(q2)*dq1^2; lsq42=-m3*dq1^2*(lg3*cos(q2+q3)+l2*cos(q2))*(lg3*sin(q2+q3)+l2*sin(q2)); lsq43=-m3*lg3*l2*sin(q3)*(2*dq2*dq3+dq3^2); out(2)=lsq41+lsq42+lsq43; % phan tu V2 lsq51= -m3*lg3*sin(q2+q3)*dq1^2*(lg3*cos(q2+q3)+l2*cos(q2)); lsq52=-m3*lg3*l2*sin(q3)*dq2*(dq2+dq3+1); out(3)=lsq51+lsq52; % phan tu V3 % File “con_para.m” Kp=[200 0 0;... 0 200 0;... 0 0 200]; % ma tran he so Kp Kd=[25 0 0;... 0 25 0;... 0 0 25]; % ma tran he so Kd qd=[1;1.5;1.3]; % vecto vi tri dat cua cac khop % File “luat_dkpd” function out = luat_dkpd(u); % ham tinh momen theo luat dieu khien con_para; q= [u(1);u(2);u(3)] delq= qd-q % sai lech vi tri khop dq=[u(4);u(5);u(6)] % sai lech toc do khop T=Kp*delq-Kd*dq % luat dieu khien phan hoi PD trong khong gian khop out=[T;qd]' % File “dyna.m” function out = dyna(u); % tinh gia toc khop q1=u(4) q2=u(5) q3=u(6) M=M_term(q1,q2,q3) TU=[u(1);u(2);u(3)]; out=[M\TU]'; PL2. Các file “thongso_robot.m”,“M_term.m”,“V.m”, “G.m” tương tự như PL1   có đuôi “.m” đi kèm với sơ đồ Simulink hình 3.14 để mô phỏng hệ thống điều khiển Momen tính trong không gian khớp. % File “luat_momen” function out = luat_momen(u); % ham tinh momen theo luat dieu khien Kp=[200 0 0;... 0 200 0;... 0 0 200]; % ma tran he so Kp Kd=[25 0 0;... 0 25 0;... 0 0 25]; % ma tran he so Kd % vecto vi tri dat cua cac khop q= [u(1);u(2);u(3)]; qd=[u(4);u(5);u(6)]; delq= qd-q ;% sai lech vi tri khop deldq=[u(7);u(8);u(9)]; % sai lech toc do khop qdd=[u(10);u(11) ;u(12)] T=Kp*delq-Kd*deldq +; % luat dieu khien Momen tinh toan out=T'; PL3. Các file có đuôi “.m” đi kèm với sơ đồ Simulink hình 3.18 để mô phỏng hệ thống điều khiển ma trận Jaccobian chuyển vị. Các file “thongso_robot.m”, “M_term.m”, “Vg.m”, “G.m”, “dyna.m” giống như trong PL1. % File “con_para.m” Kp=[200 0 0;... 0 200 0;... 0 0 200]; Kd=[50 0 0;... 0 50 0;... 0 0 50]; Xd=[0.3;0.16;0.25]; % vecto dat vi tri cua tay % File “J_term.m” function out= J_term(q1,q2,q3); % ham tinh ma tran Jacobian global l1 l2 l3; out(1,1)= -l2*sin(q1)*cos(q2)-l3*sin(q1)*cos(q2+q3); out(1,2)= -l2*cos(q1)*sin(q2)-l3*cos(q1)*sin(q2+q3); out(1,3)= -l3*cos(q1)*sin(q2+q3); out(2,1)= l2*cos(q1)*cos(q2)+l3*cos(q1)*cos(q2+q3); out(2,2)= l2*sin(q1)*sin(q2)-l3*sin(q1)*sin(q2+q3); out(2,3)= -l3*sin(q1)*sin(q2+q3); out(3,1)= 0; out(3,2)= l2*cos(q2)+l3*cos(q2+q3); out(3,3)= l3* cos(q2+q3); % File “X.m” function out= X(u); % ham tinh X tu q qua phuong trinh dong hoc thuan global l1 l2 l3; q1= u(1); q2= u(2); q3= u(3); out(1)=l2 * cos(q1) * cos(q2) + l3* cos(q1) * cos(q2+q3); out(2)=l2 * sin(q1) * cos(q2) + l3* sin(q1) * cos(q2+q3); out(3)= l1+ l2*sin(q2) + l3* sin(q2+q3); % File “Xdot.m” function out= Xdot(u); % ham tinh toc do cua tay q1= u(1); q2= u(2); q3= u(3); J= J_term(q1,q2,q3); out= J*[u(4);u(5);u(6)]; % File “J_chuyenvi.m” function out = J_chuyenvi(u); % ham tinh momen theo luat dieu khien Jacobian chuyen vi con_para; X= [u(1);u(2);u(3)]; delX= Xd-X; % sai lech vi tri cua tay deldX= [u(4);u(5);u(6)]; % sai lech toc do cua tay q1= u(7); q2= u(8); q3= u(9); J= J_term(q1,q2,q3); T= J'*(Kp*delX-Kd*deldX); % luat dieu khien Jacobian chuyen vi out=[T;Xd]; PL4. Các file có đuôi “.m” đi kèm với sơ đồ Simulink hình 3.22 để mô phỏng hệ thống điều khiển ma trận Jaccobian nghịch đảo. Tất cả các file đều giống như trong mục PL2 trừ 2 file “con_para.m” % File “con_para.m” Kp=[120 0 0;... 0 120 0;... 0 0 120]; Kd=[25 0 0;... 0 25 0;... 0 0 25]; Xd=[0.3;0.16;0.25]; % File “J_nghichdao.m” function out = J_nghichdao(u); % ham tinh momen theo luat dieu khien Jacobian nghich dao con_para; X= [u(1);u(2);u(3)]; delX= Xd-X; deldX= [u(4);u(5);u(6)]; q1= u(7); q2= u(8); q3= u(9); J= J_term(q1,q2,q3); T= inv(J)*(Kp*delX-Kd*deldX); % luat dieu khien Jacobian nghich dao out=[T;Xd];