Điều khiển trượt cho robot 2 bậc tự do

BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI Hoàng Thăng Long ĐIỀU KHIỂN ROBOT BẬC TỰ DO Chuyên ngành : Điều khiển Tự động hóa LUẬN VĂN THẠC SĨ KỸ THUẬT ĐIỀU KHIỂN VÀ TỰ ĐỘNG HÓA NGƯỜI HƯỚNG DẪN KHOA HỌC GS.TS PHAN XUÂN MINH HÀ NỘI – 2014 MỤC LỤC LỜI CAM ĐOAN MỞ ĐẦU DANH MỤC CÁC TỪ VIẾT TẮT VÀ KÝ HIỆU CHƯƠNG I: TỔNG QUAN VỀ ROBOT CÔNG NGHIỆP 12 VÀ PHƯƠNG PHÁP ĐIỀU KHIỂN 12 1.1 Sơ lược phát triển Robot công nghiệp 12 1.2 Định nghĩa robot số khái niệm 12 1.2.1 Các định nghĩa Robot công nghiệp 12 1.2.2 Một số khái niệm 13 1.2.3 Một số thông số đặc trưng hệ thống Robot 14 1.3 Mục tiêu ứng dụng Robot công nghiệp 14 1.3.1 Mục tiêu ứng dụng Robot công nghiệp 14 1.3.2 Ứng dụng mơ hình hóa Robot cơng nghiệp 14 1.4 Cấu trúc hệ thống Robot công nghiệp 16 1.4.1 Các thành phần Robot cơng nghiệp 16 1.4.2 Các dạng cấu hình học Robot 17 1.4.3 Mơ hình động lực học 18 1.5 Các phương pháp điều khiển Robot công nghiệp 29 1.5.1 Các phương pháp điều khiển truyền thống 29 1.5.2 Phương pháp điều khiển thông minh 35 1.6 Lý thuyết ổn định Lyapunov 40 CHƯƠNG II: ĐIỀU KHIỂN TRƯỢT 42 2.1 Đặc điểm điều khiển trượt 42 2.2 Điều khiển trượt 43 2.3 Tổng hợp điều khiển trượt 45 2.4 Ví dụ minh họa 46 CHƯƠNG III: THIẾT KẾ BỘ ĐIỀU KHIỂN TRƯỢT 49 CHO ROBOT BẬC TỰ DO 49 3.1 Mơ hình Robot Planar bậc tự 49 3.2 Thiết kế điều khiển trượt cho robot bậc tự mô hình xác định 50 3.3 Mơ kiểm chứng Matlab-Simulink 51 3.3.1 Xây dựng sơ đồ mô 51 3.3.2 Kết mô 52 3.3.3 Nhận xét, đánh giá kết mô 56 KẾT LUẬN VÀ PHƯƠNG HƯỚNG PHÁT TRIỂN 57 PHỤ LỤC 61 LỜI CAM ĐOAN Tôi xin cam đoan luận văn thạc sĩ với đề tài “Điều khiển trượt cho robot hai bậc tự do” tự thực hướng dẫn cô giáo GS.TS Phan Xuân Minh Các số liệu kết hoàn toàn trung thực Ngoài tài liệu tham khảo dẫn cuối luận văn, đảm bảo không chép cơng trình kết người khác Nếu phát có sai phạm với điều cam đoan trên, tơi xin hồn tồn chịu trách nhiệm Học viên Hồng Thăng Long MỞ ĐẦU Theo q trình phát triển xã hội, nhu cầu nâng cao sản xuất chất lượng sản phẩm ngày đòi hỏi ứng dụng rộng rãi phương tiện tự động hóa sản xuất Xu hướng tạo dây chuyền thiết bị tự động có tính linh hoạt cao hình thành phát triển mạnh mẽ… Vì ngày tăng nhanh nhu cầu ứng dụng người máy để tạo hệ sản xuất tự động linh hoạt Robot ứng dụng rộng rãi đóng vai trị quan trọng sản xuất đời sống Robot cấu đa chức có khả lập trình dùng để di chuyển nguyên vật liệu, chi tiết, dụng cụ thông qua truyền động lập trình trước Khoa học robot chủ yếu dựa vào phép toán đại số ma trận * Robot thao tác người hợp tác với cách thông minh * Robot có cánh tay với nhiều bậc tự thực chuyển động tay người điều khiển máy tính điều khiển chương trình nạp sẵn chip bo mạch điều khiển robot Càng ngày ngành robot phát triển, đem lại thay đổi quan trọng chế tạo sản phẩm nâng cao suất chất lượng nhiều ngành công nghiệp Sử dụng Robot cơng nghiệp (RBCN), xí nghiệp cơng nghiệp thu nhiều lợi ích như: tăng thời gian hoạt động máy móc, tăng độ linh hoạt, có khả định trước công việc sản xuất tăng sản lượng nhờ làm ổn định trình Việc tự động hóa nhờ robot khắc phục tình trạng thiếu nhân cơng, đồng thời tăng độ an tồn cho người thiết bị Việc tiết kiệm nhân công hạ giá thành sản phẩm, ưu điểm định khả cạnh tranh Giá thành robot giảm tính gia tăng công nghệ ngày dễ sử dụng RBCN máy CNC kết hợp với thành hệ thống lập trình điều khiển máy tính chúng hoạt động theo cơng nghệ đặt Robot đóng vai trị quan trọng tự động hóa linh hoạt cơng tác vận chuyển bổ trợ cho máy CNC, dây chuyền lắp ráp, sơn hàn tự động, thao tác lặp lặp lại, vùng nguy hiểm Một robot chuyển động từ vị trí sáng vị trí khác để cung cấp chi tiết đồng thời giao tiếp với thiết bị ngoại vi PLC, bàn điều khiển hệ thống mạng truyền thông công nghiệp Ưu điểm quan trọng kỹ thuật robot tạo nên khả linh hoạt hóa sản xuất Việc sử dụng máy tính điện tử - robot máy điều khiển theo chương trình cho phép tìm phương thức mẻ để tạo nên dây chuyền tự động cho sản xuất hàng loạt với nhiều mẫu, loại sản phẩm Kỹ thuật robot cơng nghiệp máy vi tính đóng vai trò quan trọng việc tạo dây chuyền tự động linh hoạt (Hệ sản xuất linh hoạt FMS) Để hệ điều khiển robot (ĐKRB) có độ tin cậy, độ xác cao, giá thành hạ tiết kiệm lượng nhiệm vụ hệ ĐKRB phải đảm bảo giá trị yêu cầu đại lượng điều chỉnh điều khiển Ngoài ra, hệ ĐKRB phải đảm bảo ổn định động tĩnh, chống nhiễu ngồi, đồng thời khơng gây tác hại cho môi trường như: tiếng ồn mức quy định, sóng hài điện áp dịng điện q lớn cho lưới điện Khi thiết kế hệ ĐKRB mà sử dụng hệ điều chỉnh tự động truyền động, cần phải đảm bảo hệ thực tất yêu cầu công nghệ, tiêu chất lượng yêu cầu kinh tế Chất lượng hệ thống thể trạng thái tĩnh trạng thái động Trạng thái tĩnh yêu cầu quan trọng độ xác điều chỉnh Trạng thái động có u cầu độ ổn định tiêu chất lượng động độ điều chỉnh, tốc độ điều chỉnh, thời gian điều chỉnh số lần dao động Đối với hệ ĐKRB, việc lựa chọn sử dụng biến đổi, loại động điện, thiết bị đo lường, cảm biến, điều khiển đặc biệt phương pháp điều khiển có ảnh hưởng lớn đến chất lượng điều khiển bám xác quỹ đạo hệ Đặc điểm hệ thống ĐKRB thực điều khiển bám theo quỹ đạo phức tạp đặt trước không gian, nhiên dịch chuyển trọng tâm chuyển động thành phần mơmen qn tính hệ thay đổi, điều dẫn đến thông số động học hệ thay đổi theo quỹ đạo chuyển động đồng thời xuất lực tác động qua lại, xuyên chéo chuyển động thành phần hệ với Các yếu tố tác động cho hệ ĐKRB mang tính phi tuyến mạnh, gây cản trở lớn cho việc mơ tả nhận dạng xác hệ thống ĐKRB Do vậy, ĐKRB bám theo quỹ đạo đặt trước phải giải vấn đề sau * Khắc phục lực tương tác phụ thuộc vào vận tốc, gia tốc quỹ đạo riêng chuyển động thành phần quỹ đạo chung hệ như: lực quán tính, lực ly tâm, lực ma sát * Khi trọng tâm chuyển động thành phần hệ thay đổi theo quỹ đạo riêng chung kéo theo thay đổi thơng số động học hệ, điều địi hỏi phải có biến thiên tham số đưa vào điều khiển tương ứng để đảm bảo cân bằng, ổn định bền vững đồng thời bám theo quỹ đạo đặt Các phương pháp điều khiển thơng thường sử dụng biện pháp phân tích gián tiếp thơng qua mơ hình làm việc hệ, song lại không cung cấp thông tin cách đầy đủ tồn hệ thống Cịn phương pháp phân tích trực tiếp ngoại trừ tiêu chuẩn Lyapunov cho việc phân tích tính ổn định phương pháp mặt phẳng pha giới hạn hệ phi tuyến có hai biến trạng thái, chưa có phương pháp cụ thể khác Phương pháp điều khiển động lực học ngược cần phải biết xác thông số đối tượng, đối tượng thực tế lại có thơng số thay đổi nhiễu khơng xác định mơi trường làm việc Do đó, việc thực điều khiển theo phương pháp gặp nhiều khó khăn độ xác, hay nói cách khác khó thực thực tế Phương pháp điều khiển phân ly phi tuyến có nhược điểm hệ thống điều khiển có tính phi tuyến cao, độ phức tạp điều khiển lớn, khó có khả thực thực tế Phương pháp điều khiển thích nghi theo mơ hình chuẩn thực đơn giản cho mơ hình tuyến tính với giả thiết bỏ qua liên hệ động lực học chuyển động thành phần hệ Ngồi ra, ổn định hệ thống kín vấn đề khó giải với tính phi tuyến cao mơ hình chuẩn Phương pháp điều khiển thích nghi theo sai lệch có luật điều khiển thích nghi đơn giản hóa cách áp dụng phương pháp điều khiển thích nghi suy giảm phân ly, phương pháp ln tồn sai lệch quỹ đạo trình điều khiển thực không phù hợp với yêu cầu hệ điều khiển xác quỹ đạo Phương pháp điều khiển trượt có ưu điểm lớn đơn giản có tính bền vững cao Trong thực tế nói chung cơng nghiệp nói riêng có nhiều đối tượng ứng dụng phương pháp điều khiển trượt ưu điểm phương pháp này, đặc biệt đối tượng robot Mơ hình động lực Robot mơ hình phi tuyến với nhiều tham số bất định Phương pháp tiếp cận để điều khiển Robot phương pháp điều khiển tuyến tính, phương pháp thiết kế có số hạn chế đặc biệt Robot có đặc tính phi tuyến mạnh có tham số bất định Chính phương pháp điều khiển phải tiếp tục nghiên cứu để đáp ứng yêu cầu chất lượng hoạt động Robot Hệ thống điều khiển Robot đòi hỏi cấu trúc điều khiển thay đổi linh hoạt để đảm bảo tính ổn định chất lượng bền vững có thay đổi tham số mơ hình có nhiễu tác động Và điều khiển trượt (Sliding Mode Contrrol - SMC) hay cịn gọi điều khiển có cấu trúc thay đổi (Variable Structure System - VSS) phương pháp hiệu để giải vấn đề Từ đánh giá nêu trên, tác giả luận văn định lựa chọn đề tài “Điều khiển trượt cho robot hai bậc tự do” Nội dung luận văn chia thành chương với nội dung sau: Chương 1: Tổng quan Robot công nghiệp phương pháp điều khiển Trình bày kiến thức áp dụng để thiết kế luận văn lí thuyết - Sơ lược phát triển Robot công nghiệp - Định nghĩa robot số khái niệm - Mục tiêu ứng dụng Robot công nghiệp - Cấu trúc hệ thống Robot công nghiệp - Các phương pháp điều khiển Robot công nghiệp - Ổn định Lyapunov Chương 2: Thiết kế điều khiển trượt - Đặc điểm điều khiển trượt - Nguyên lý điều khiển trượt - Tổng hợp tín hiệu điều khiển trượt Chương 3: Ứng dụng điều khiển trượt vào điều khiển robot planar hai bậc tự Thiết kế điều khiển trượt mô Matlab-Simulink Cuối tiến hành mô đánh giá kết đạt Trong trình nghiên cứu, tác giả luận văn cố gắng tiếp cận giải vấn đề cách triệt để Tuy vậy, thời gian có hạn trình độ chun mơn cịn nhiều điểm chưa hồn thiện, chắn khơng tránh khỏi sai sót định Kính mong nhận đóng góp bảo thêm thầy cô Xin trân trọng gửi lời cảm ơn tới Viện Đào tạo Sau đại học, Bộ môn Điều khiển Tự động trường Đại học Bách Khoa Hà Nội tạo điều kiện thuận lợi cho tác giả trình thực luận văn Xin bày tỏ lòng biết ơn sâu sắc tới GS.TS Phan Xuân Minh, người định hướng tận tình bảo, giúp đỡ để tác giả hồn thành luận văn tốt nghiệp Cuối cùng, xin gửi lời cảm ơn sâu sắc tới gia đình, bạn bè đồng nghiệp, người ủng hộ nhiệt tình nguồn động viên to lớn tác giả suốt q trình nghiên cứu hồn thành luận văn Hà nội, ngày 26 tháng năm 2014 Học viên Hoàng Thăng Long DANH MỤC CÁC TỪ VIẾT TẮT VÀ KÝ HIỆU SMC Sliding Mode Control Điều khiển trượt VSC Variable Structure Control Điều khiển cấu trúc thay đổi RBCN Robot công nghiệp ĐKRB Điều khiển Robot CHƯƠNG III: THIẾT KẾ BỘ ĐIỀU KHIỂN TRƯỢT CHO ROBOT BẬC TỰ DO 3.1 Mơ hình Robot Planar bậc tự Đối tượng Robot bậc tự hình: Hình 3.1: Mơ hình Robot Planar bậc tự Có động học biểu diễn phương trình q&& = M −1 (q)[τ − V (q, q& ) − F (q, q& )] M (q) Với M(q) =  11  M 12 ( q ) M 12 ( q )  ; V(q, q& ) = M 22 ( q )  V1 ( q, q& )  V ( q, q& )  ; F(q, q& ) =   Trong đó: q = [ θ1 θ ]T vector vị trí, q& = [θ&1 θ&2 ]T vector vận tốc M(q) ma trận [n x n] biểu diễn khối lượng Robot V(q, q& ) vector [n x 1] vector ly tâm lực Coriolis F(q, q& ) ma trận [n x 1] biểu diễn thành phần ma sát τ vector momen xoắn Biểu thức cụ thể đối tượng sau: M11q = m1l c21 + m2(l 12 +lc22 + 2l1lc c2 ) + Izz1 + Izz2 M12(q) = m2l c22 + m2l1lc2c2 + Izz2 M22(q) = m2l c22 + Izz2 49  F1 (q, q& )   F ( q, q& )    V1(q, q& ) = - m2l1lc2c2( θ&1 + θ&2 ) θ&2 - m2l1lc2c2 θ&1θ&2 s2 V2(q, q& ) = m2l1lc2c2( θ&2 )2 F1(q, q& ) = µ1 θ&1 F2(q, q& ) = µ θ&2 Trong s2 = sin( θ ) , c2 = cos( θ ) 3.2 Thiết kế điều khiển trượt cho robot bậc tự mơ hình xác định Xây dựng mặt trượt cho khớp” (4.1) S1 = e1 + λ1e& S2 = e + λ e& λ1 , λ2 số thực dương Ở đây: e1 = qd − q1 e2 = qd − q2 với q1 , q1d quỹ đạo quỹ đạo đặt khâu 1, q2 , q2 d quỹ đạo quỹ đạo đặt khâu ⇒ e&1 = q&d − q&1 ⇒ e&&1 = q&&d − q&&1 ; &e&2 = q&&d − q&&2 e&2 = q&d − q&2 Tín hiệu điều khiển trượt thiết kế dựa tồn hàm Lyapunov V(S) cho hệ kín, với: V = S Từ ta có: V& ( s) = S S& = − SK sgn S Với K > (4.2) * Thiết kế điều khiển trượt cho khớp Lấy đạo hàm (4.1) ta có: 50 dS1 (e1 ) = λ e&1 + && e1 dt = λ (q&1d − q&1 ) + q&&1d − q&&1 = λ q&1d + q&&1d − λ q&1 − ( f1 + u1 ) (4.3) kết hợp điều kiện trượt ta có: [ λ q&1d + q&&1d − λ q&1 − ( f1 + u1 )] K sgn(S1 ) < (4.4) ⇒ λ q&1d + q&&1d − λ q&1 − ( f1 + u1 ) = − K sgn(S1 ) ta có điều khiển trượt cho khớp thứ nhất: (4.5) u1 = K sgn( S1 ) + λ q&1d + q&&1d − λ q&1 − f1 * Tương tự cho khớp thứ hai ta có: (4.6) u2 = K sgn(S2 ) + λq&2d + q&&2d − λq&2 − f2 Sơ đồ cấu trúc hệ thống điều khiển trượt cho robot bậc tự q d , q& d ed , e&d SMC U SMC H(q) τ Robot 3.3 Mô kiểm chứng Matlab-Simulink 3.3.1 Xây dựng sơ đồ mơ • Mơ hình simulink đối tượng robot planar Hình 3.2: Mơ hình Simulink rơbot planar 51 q, q& • Mơ hình simulink điều khiển SMC cho robot planar Hình 3.3:Mơ hình simulink điều khiển trượt cho robot planar 3.3.2 Kết mơ • Các tham số mơ hình đối tượng sử dụng mơ Matlab chọn bảng sau: Thông số Giá trị Mô tả l1 0.3 m Chiều dài khớp l2 0.25 m Chiều dài khớp m1 kg Khối lượng khớp m2 0.3 kg Khối lượng khớp Bảng 3.1: Tham số mơ hình Robot 52 1) Kết mô khảo sát quỹ đạo bám sai lệch quĩ đạo robot planar khơng có tải nhiễu Hình 3.4: Quỹ đạo bám sai lệch quỹ đạo khớp Hình 3.5: Quỹ đạo bám sai lệch quỹ đạo khớp 2) Kết mô khảo sát đáp ứng mô men, quỹ đạo bám sai lệch quĩ đạo có nhiễu tải Hình 3.6: Đáp ứng mơ men có nhiễu tải khớp 53 Hình 3.7: Đáp ứng mơ men có nhiễu tải khớp Hình 3.8: Quỹ đạo bám sai lệch quỹ đạo khớp có nhiễu tải 54 3) Kết mơ khảo sát đáp ứng momen, quỹ đạo bám sai lệch quĩ đạo có nhiễu tải nhiễu ổn trắng Hình 3.9: Đáp ứng mơ men khớp có nhiễu tải nhiễu ổn trắng Hình 3.10: Đáp ứng mơ men khớp có nhiễu tải nhiễu ổn trắng 55 Hình 3.11: Bám quỹ đạo khớp có nhiễu tải nhiễu ổn Hình 3.12: Bám quỹ đạo khớp có nhiễu tải nhiễu ổn 3.3.3 Nhận xét, đánh giá kết mô - Dựa vào kết mô khảo sát cho thấy hệ thống điều khiển thiết kế cho đáp ứng tốt, đáp ứng vị trí tốc độ robot bám sát giá trị đặt sau khoảng thời gian nhỏ - Thông qua kết mơ thu ta thấy hệ thống thiết kế hoàn toàn đáp ứng tiêu điều khiển Điều chứng tỏ xem xét áp dụng hệ thống điều khiển trượt để điều khiển robot công nghiệp cụ thể thực tế 56 KẾT LUẬN VÀ PHƯƠNG HƯỚNG PHÁT TRIỂN Những kết đạt Với đề tài luận văn điều khiển trượt cho hệ phi tuyến ứng dụng điều khiển robot bậc tự tác giả nỗ lực tìm tịi, nghiên cứu lý thuyết điều khiển trượt, lý thuyết Lyapunov Các kết đạt luận văn: - Nghiên cứu tìm hiểu robot cơng nghiệp, mơ hình hóa phương pháp điều khiển - Nghiên cứu lý thuyết ổn định Lyapunov - Nghiên cứu phương pháp điều khiển trượt - Mơ phỏng, kiểm chứng tính đắn khả ứng dụng thuật toán điều khiển trượt cho mơ hình robot cơng nghiệp hai bậc tự phần mềm Matlab & Simulink Những mặt hạn chế Trong thời gian làm luận văn, tác giả cố gắng nghiên cứu, thiết kế để có kết nêu Tuy nhiên, thời gian kiến thức em bị hạn chế, cộng với số điều kiện thiết bị không cho phép nên luận văn cịn có hạn chế sau: - Mới mô cho đối tượng robot có tham số cố định - Chưa áp dụng phương pháp điều khiển trượt để viết chương trình cho vi điều khiển thực điều khiển mơ hình thực tế cho robot bậc tự mà mô Matlab-Simulink SimMechanics hạn chế mặt thiết bị khó khăn việc áp dụng thực tế Hướng phát triển đề tài Dựa kết đạt được, hạn chế tồn gợi ý giáo viên hướng dẫn, đề tài tiếp tục phát triển hướng sau: - Nghiên cứu, tìm cách khắc phục hạn chế nêu 57 - Nghiên cứu phương pháp điều khiển nâng cao khác điều khiển thích nghi, điều khiển mờ hay điều khiển nơron vào đối tượng robot bậc cao 58 TÀI LIỆU THAM KHẢO [1] Nguyễn Doãn Phước, Phân tích điều khiển hệ phi tuyến, Nhà xuất Bách Khoa Hà Nội (II/2010) [2] Nguyễn Doãn Phước, Điều khiên nâng cao, Nhà xuất khoa học kĩ thuật, (2011) [3] Nguyễn Phùng Quang, MATLAB SIMULINK dành cho kĩ sư điều khiên tự động, Nhà xuất khoa học kĩ thuật, (10/2005) [4] Nguyễn Thiện Phúc, Robot công nghiệp - Nhà xuất khoa học kĩ thuật, (2002) [5] Nguyễn Thiện Phúc, Người máy công nghiệp sản xuất tự động linh hoạt NXB KHKT 1991 [6] Nguyễn Phùng Quang: Điều khiển Robot công nghiệp - Những vấn đề cần biết Tạp chí Tự động hố ngày - số tháng 4, 5, - 2006 [7] Nguyễn Thiện Phúc: Robot - Thế giới công nghệ cao bạn NXB KHKT 2005 [8] Nguyễn Doãn Phước, Phan Xuân Minh, Hán Thành Trung , Lý thuyết điều khiển phi tuyến, Nhà xuất khoa học kỹ thuật hà nội, 2003 [9] Wilfrid Prerruquetti, Jean Pierre Barbot (Eds) (2002): Sliding Mode Control in Engineerring Copyright by Marcel Dekker, Inc All Rights Reserved Printed USA [10] Nei E Cotter: The Stone – Weiestras Theorem and Its Application to Neural Networks, IEEE Transaction on Neural Networks Vol.1, No.4.1990, PP.290-295 [11] Hornik K, Stinchcomble M & White H.: Multilayer feedforward networks are univesal appoximator Neural Network, No.2, 1989, PP.359-365 [12] Gomn J B., Yu D L.: Selecting radial basis function networks centers with resursive orthogonal least sepuares training IEEE Transaction on Neural Networks Vol.11, ISSUE 2, 2000, PP.306-314 59 [13] Panchapakesan C., Ralph D & Palanisami M.: Effects of moving the center in an RBF Network Proceeding of the 1998 IEEE Wold Congress on computational Intelligence Neural Networks Vol 2, 1990, PP.1256-1269 [14] Huang S N., Tan K K., Lee T H.: A combined PID/Addaptive controller for a class of nonlinear Systems Automatica 37(2001), PP.611-618 [15] J.J.E Slotine and S.S Satry, Tracking control of nonlinear systems using sliding surface with application to robot manipulator, Int J Control, vol 38, no 2, pp 465-492, 1983 [16] V.A.Utkin, Sliding Modes and Their Application in Variable Structure Systems, Moscow, Mir 1987 60 PHỤ LỤC Phụ lục: Code MATLAB Đối tượng MIMO (Robot Planar bậc tự do) function y = smc1(in) a1 = 0.3; a2 = 0.25; m1 = 1; m2 = 0.3; g = 9.81; dqd1 = in(1); ddqd1 = in(2); x1 = in(3); x2 = in(4); x3 = in(5); x4 = in(6); % Cac phan ma tran M(q) m11 = (m1/3 +m2)*a1^2 + a1*a2*m2*cos(x3) +1/3*m2*a2^2; m12 = 1/3*m2*a2^2 + 1/2*a1*a2*m2*cos(x3); m21 = m12; m22 = 1/3*m2*a2^2; % Cac phan ma tran Psi(q,dq) h1 = 1/2*m2*a1*a2*sin(x3)*(2*x2*x4 + x4^2) - (m1/2+m2)*g*a1*cos(x1) 1/2*m2*a2*g*cos(x1+x3); h2 = -1/2*m2*a1*a2*sin(x3)*x2^2 - 1/2*m2*a2*g*cos(x1+x3); % phep dat k = 1/(m11*m22 - m12*m21); f2 = k*(m22*h1-m12*h2); y = 150*(dqd1-x2)+ddqd1-f2; %************************************************************ function y = smc2(in) a1 = 0.3; a2 = 0.25; m1 = 1; m2 = 0.3; g = 9.81; dqd2 = in(1); ddqd2 = in(2); x1 = in(3); x2 = in(4); x3 = in(5); x4 = in(6); % Cac phan ma tran M(q) m11 = (m1/3 +m2)*a1^2 + a1*a2*m2*cos(x3) +1/3*m2*a2^2; m12 = 1/3*m2*a2^2 + 1/2*a1*a2*m2*cos(x3); m21 = m12; 61 m22 = 1/3*m2*a2^2; % Cac phan ma tran Psi(q,dq) h1 = 1/2*m2*a1*a2*sin(x3)*(2*x2*x4 + x4^2) - (m1/2+m2)*g*a1*cos(x1) 1/2*m2*a2*g*cos(x1+x3); h2 = -1/2*m2*a1*a2*sin(x3)*x2^2 - 1/2*m2*a2*g*cos(x1+x3); % phep dat k = 1/(m11*m22 - m12*m21); f4 = k*(-m21*h1+m11*h2); y = 150*(dqd2-x4)+ddqd2-f4; end %************************************************************ function dx = plant1(u) a1 = 0.3; a2 = 0.25; m1 = 1; m2 = 0.3; g = 9.81; in1 = u(1); x1 = u(2); % x1 = q1(bien khop thu nhat) x2 = u(3); % x2 = dq1 x3 = u(4); % x3 = q2(bien khop thu hai) x4 = u(5); % x4 = dq2 % Cac phan ma tran M(q) m11 = (m1/3 +m2)*a1^2 + a1*a2*m2*cos(x3) +1/3*m2*a2^2; m12 = 1/3*m2*a2^2 + 1/2*a1*a2*m2*cos(x3); m21 = m12; m22 = 1/3*m2*a2^2; % Cac phan ma tran Psi(q,dq) h1 = 1/2*m2*a1*a2*sin(x3)*(2*x2*x4 + x4^2) - (m1/2+m2)*g*a1*cos(x1) 1/2*m2*a2*g*cos(x1+x3); h2 = -1/2*m2*a1*a2*sin(x3)*x2^2 - 1/2*m2*a2*g*cos(x1+x3); % phep dat k = 1/(m11*m22 - m12*m21); dx(1) = x2; dx(2) = k*(m22*h1 - m12*h2) + in1; end %************************************************************ function dy = plant2(u) a1 = 0.3; a2 = 0.25; m1 = 1; m2 = 0.3; g = 9.81; in2 = u(1); 62 x3 = u(2); % x1 = q1(bien khop thu nhat) x4 = u(3); % x2 = dq1 x1 = u(4); % x3 = q2(bien khop thu hai) x2 = u(5); % x4 = dq2 % Cac phan ma tran M(q) m11 = (m1/3 +m2)*a1^2 + a1*a2*m2*cos(x3) +1/3*m2*a2^2; m12 = 1/3*m2*a2^2 + 1/2*a1*a2*m2*cos(x3); m21 = m12; m22 = 1/3*m2*a2^2; % Cac phan ma tran Psi(q,dq) h1 = 1/2*m2*a1*a2*sin(x3)*(2*x2*x4 + x4^2) - (m1/2+m2)*g*a1*cos(x1) 1/2*m2*a2*g*cos(x1+x3); h2 = -1/2*m2*a1*a2*sin(x3)*x2^2 - 1/2*m2*a2*g*cos(x1+x3); % phep dat k = 1/(m11*m22 - m12*m21); dy(1) = x4; dy(2) = k*( -m21*h1 + m11*h2) + in2; end end 63 ... c21 + m2(l 12 +lc 22 + 2l1lc c2 ) + Izz1 + Izz2 M 12( q) = m2l c 22 + m2l1lc2c2 + Izz2 M 22( q) = m2l c 22 + Izz2 49  F1 (q, q& )   F ( q, q& )    V1(q, q& ) = - m2l1lc2c2( θ&1 + θ &2 ) θ &2 - m2l1lc2c2... c 22 = l 12 sin θ1θ& 12 + lc 22 sin θ 12 (θ&1 + θ &2 ) + 2l1lc2 sin θ1 sin θ 12 (θ&1 + θ &2 )  &2 2 2 2 Yc2 = l1 cos θ1θ&1 + lc2 sin θ 12 (θ&1 + θ &2 ) + 2l1lc cosθ1 cosθ 12 (θ&1 + θ &2 ) (1 .23 ) → Vc 22. .. x2 + p y2 = l 12 + l 22 + 2l1l2 cosθ Do đó:  p x2 + p y2 − l 12 − l 22 cosθ = 2l1l2    p x2 + p y2 − l 12 − l 22     sin θ = −   2l1l2    (1.13) Góc khớp θ tính sau: (1.14) θ = ATAN 2( sin