Điều khiển có giám sát dùng logic mờ ứng dụng điều khiển mô hình robot scara dùng DSP TMS320C50

BỘ GIÁO DỤC VÀ ĐÀO TẠO ĐẠI HỌC QUỐC GIA TP HỒ CHÍ MINH TRƯỜNG ĐẠI HỌC BÁCH KHOA -oOo - NGUYỄN ĐÌNH PHÚ ĐIỀU KHIỂN CÓ GIÁM SÁT DÙNG LOGIC MỜ ỨNG DỤNG ĐIỀU KHIỂN MÔ HÌNH ROBOT SCARA DÙNG DSP TMS320C50 CHUYÊN NGÀNH: KỸ THUẬT VÔ TUYẾN – ĐIỆN T Ử Mà SỐ NGÀNH: LUẬN ÁN CAO HỌC Tp HCM, tháng năm 2004 LỜI CẢM ƠN Tôi xin gởi lời cảm ơn chân thành đến: - Thầy hướng dẫn Ts Dương Hoài Nghóa phó Khoa Điện – Điện Tử trường Đại học Kỹ Thuật Tp HCM trực tiếp hướng dẫn hoàn thành luận án - Tất thầy cô giảng dạy, truyền đạt kiến thức giúp trưởng thành nghề nghiệp sống - Tất bạn bè đồng nghiệp người thân gia đình động viên giúp đỡ, thăm hỏi thời gian thực luận án tốt nghiệp Tháng 04/2004 Nguyễn Đình Phuù ABSTRACT In the control systems, the parameters of the control systems change acording to load capacity – they are called system which adjust itself The adjustable parameters are performed according to an algorithm with very large calculation which is not able to with the real time The purposes of the thesis: ƒ Studying the supervise control system use the repeating method ƒ Self adjust system uses DSP KIT and the model of the robot is simulinked on PC MỤC LỤC Chương I: Trang GIỚI THIỆU VỀ ROBOT SCARA I Giới thiệu .2 II Mô hình toán học robot scara .3 III Mô hình trạng thái robot scara Chương II: GIẢI THUẬT ĐIỀU KHIỂN ROBOT I Giới thiệu II Vòng điều khiển trong: hồi tiếp tuyến tính hóa III Vòng điều khiển ngoài: điều khiển PD IV Thiết kế hệ thống điều khieån 10 Thông số robot 10 Vòng điều khiển 11 Vòng điều khiển .15 V Moâ hệ thống điều khiển Matlab 17 Hệ thống danh ñònh 18 Ảnh hưởng thay đổi tải trọng .18 Ảnh hưởng thay đổi hệ số ma sát 20 Chương III: ĐIỀU KHIỂN GIÁM SÁT DÙNG LOGIC MỜ 25 I Logic mờ .26 Tập mờ .26 Quan hệ mờ 29 Suy diễn mờ 30 Hệ thống xử lý mờ .33 Nhận dạng hàm thuộc luật hợp thành 36 II Ứng dụng logic mờ giám sát điều khiển 38 Giới thiệu 38 Thông số chỉnh định 38 Phương pháp chỉnh định lặp 39 Hệ thống chỉnh định mờ 40 III Mô hệ thống điều khiển có giám sát Matlab 44 Chương IV: KẾT QUẢ THỰC NGHIỆM TRÊN KIT DSP 46 I Giới thiệu 47 II Mô robot scara (trên máy tính PC) 48 III Giải thuật điều khiển moment tính (trên kit DSP) .51 IV Giải thuật giám sát dùng logic mờ .52 V Đáp ứng với bước nhảy đơn vị 52 VI Chuyeån động theo qũy đạo cho trước 55 VII Chỉnh định thông số điều khiển dùng logic mờ 59 VIII.Ảnh hưởng thay đổi tải trọng hệ số ma sát 62 IX Kết luận 66 i KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN 67 Phuï luïc A .68 Phuï luïc B 71 Phuï luïc C 81 Phuï luïc D 94 TÀI LIỆU THAM KHAÛO 109 ii GIỚI THIỆU Trong điều khiển thường phải thay đổi thông số điều khiển theo thay đổi tải trọng – gọi hệ thống tự chỉnh định Việc chỉnh định thông số thực theo giải thuật đòi hỏi khối lượng tính toán lớn – khó đáp ứng thời gian thực Mục tiêu luận văn tốt nghiệp nghiên cứu hệ thống điều khiển có giám sát dùng phương pháp lập logic mờ Áp dụng hệ thống điều khiển có giám sát để điều khiển robot bậc tự Nghiên cứu thực hệ thống điều khiển thời gian thực với độ dài từ liệu hữu hạn (2byte) Hệ thống điều khiển có giám sát thực kit DSP, mô hình robot bậc tự mô máy tính Đề tài chia làm chương: Chương 1: GIỚI THIỆU VỀ ROBOT SCARA Giới thiệu robot scara, mô hình toán học mô hình trạng thái robot scara Chương 2: GIẢI THUẬT ĐIỀU KHIỂN ROBOT Trình bày giải thuật điều khiển robot dùng hồi tiếp tuyến tính hóa điều khiển PD Mô hệ thống điều khiển Matlab Chương 3: ĐIỀU KHIỂN GIÁM SÁT DÙNG LOGIC MỜ Giới thiệu logic mờ để chỉnh định thông số điều khiển sử dụng phương pháp chỉnh định lặp Mô hệ thống điều khiển có chỉnh định Matlab Chương 4: KẾT QUẢ THỰC NGHIỆM TRÊN KIT DSP Khảo sát đáp ứng hệ thống điều khiển với bước nhảy đơn vị, khảo sát tính hiệu giám sát với thay đổi tải trọng thay đổi hệ số ma sát CHƯƠNG 1: GIỚI THIỆU VỀ ROBOT SCARA I GIỚI THIỆU II MÔ HÌNH TOÁN HỌC CỦA ROBOT SCARA III MÔ HÌNH TRẠNG THÁI CỦA ROBOT SCARA Luận Văn Cao Học I Điều Khiển Robot Scara GIỚI THIỆU Robot hệ thống điều khiển tự động vận hành, đa chức năng, lập trình, thiết kế để di chuyển chi tiết, phận thiết bị đặc biệt thông qua chuyển động khác để thực thi công việc khác Có nhiều loại robot khác nhau, xin giới thiệu loại robot: loại PUMA loại SCARA Hình 1-1 Robot loại PUMA: Hình 1-2 Robot loại SCARA: Robot scara thường sử dụng để nhặt đặt vật thể hay dùng để siết mở vít hàn linh kiện tử Việc nghiên cứu robot ngày phát triển mạnh nước phát triển sử dụng nhiều nhà máy công nghiệp Robot người máy chế tạo ngày hoàn hảo gần gũi người Các nhà nghiên cứu xây dựng ngày nhiều tính cho robot GVHD: TS Dương Hoài Nghóa Luận Văn Cao Học Điều Khiển Robot Scara Để vận hành robot phải có hệ thống điều khiển hệ thống cấu chấp hành Hệ thống điều khiển sử dụng chip vi xử lý lập trình, nhớ hệ thống cảm biến Lónh vực điều khiển robot tiếp tục phát triển tương lai lónh vực nghiên cứu hấp dẫn nhà nghiên cứu lý chọn để thực đề tài tốt nghiệp Robot có nhiều ứng dụng robot phân loại theo nhiều dạng Trong đề tài tốt nghiệp, người nghiên cứu chọn robot scara bậc tự II MÔ HÌNH TOÁN HỌC CỦA ROBOT SCARA: Robot SCARA bậc tự có dạng hình 1-3 – gồm cánh tay với chiều dài tương ứng l1, l2 khớp tác động Khớp thứ motor tác động trực tiếp moment τ1 Khớp nối thứ motor tác động gián tiếp moment τ2, thông qua nhông tăng lực có hệ số truyền động ke Đầu mút cánh tay (end-effector) mang tải trọng m Phương trình mô tả hệ thống cho (1-1), (1-2): Joint2 Joint1 I1 τ2 Link1 ϕ2 τ1 a) I2 ϕ1 l1 Với: M11 = I1+m l12 ; C11 = V1; Link2 Payload m l2 y θ2 (x,y) b) (0,0) θ1 ⎤⎡ • ⎤ C12 sin( k e (ϕ1 + ϕ )) ϕ ⎥ ⎢ϕ1 ⎥ ⎡τ ⎤ +⎢ ⎥ • ⎥ ⎢ϕ ⎥ ⎣τ ⎦ C 22 ⎦⎣ ⎦ M12 = M21 = -ml1l2; C22 = V2; x Hình 1-3.- Robot SCARA •• M 11 M 12 cos(k e (ϕ1 + ϕ ))⎤ ⎡ϕ1 ⎤ ⎡ ⎢ ⎥ ⎢ • • ⎥⎥ = ⎢ M cos(k (ϕ + ϕ )) M 22 e ⎦ ⎣ϕ ⎦ ⎣ 21 ⎡ C11 −⎢ • ⎢C sin( k (ϕ + ϕ )) ϕ e ⎣ 21 I3 • (1-1) M22 = I2+I3+ m l 22 C12 = C21 = ml1l2ke; θ1: góc tạo cánh tay phương nằm ngang θ2: góc tạo cánh tay [τ1, τ2]: moment cánh tay cánh tay [φ1, φ 2]: góc quay khớp nối khớp nối Các thông số robot: I1 moment quán tính qui khớp nối moment quán tính cánh tay qui khớp nối I2 moment quán tính tải qui khớp nối I3 m khối lượng tải chiều dài cánh tay l1 chiều dài cánh tay l2 GVHD: TS Dương Hoài Nghóa Luận Văn Cao Học fi_r20:=0; fi_r21:=0; kd1:= kd2:= kp1:= kp2:= Phuï Luïc D //gan cac gia tri ban dau -20 * M11 + C11; -20 * M22 * C22; -100 * M11; -100 * M22; T:=T1/nn; fi_r12:= 5; fi_r22:= -3; {end kit dsp} end; procedure TForm1.tinh_fi_12_fi_22;{chuong trinh tinh goc fi_12 va fi_22} var a1,a2,b1,b2,e:double; //cac bien trung gian u,v,q,p,z,w,delta,deltafi_12,deltafi_22:double; //cac bien cho dinh thuc LL: integer; begin X10:=X11; X20:=X21; X30:=X31; X40:=X41; TAM:= KE*(X10+X30); AA:= COS(TAM); BB:=SIN(TAM); X11:= X10 + T*X20; X21:= X20 + T*(-F1*AA*BB*X20*X20 - F3*BB*X40*X40 - F4*X20 - F5*AA*X40 + F2*TO1 + F6*AA*TO2)/(F7-F8*AA*AA); X31:= X30 + T*X40; X41:= X40 + T*(-F9*BB*X20*X20 - F1*AA*BB*X40*X40 - F10*AA*X20 - F11*X40 + F6*AA*TO1 +F12*TO2)/(F7-F8*AA*AA); fi_12:=x11; fi_22:=x31; end; procedure TForm1.Button2Click(Sender: TObject); var lk,kl:integer; begin tinhdulieu; kl:=1; repeat lk:= 1; repeat tinhdsp; tinh_fi_12_fi_22; lk:= lk+1; memo10.Lines.Add(inttostr(lk)); until lk = 500; memo8.Lines.Add(floattostr(X11)); memo11.Lines.Add(floattostr(X31)); kl:=kl+1; until kl=10; end; GVHD: TS Dương Hoài Nghóa 108 Luận Văn Cao Học Phụ Lục D end GVHD: TS Dương Hoài Nghóa 109 PHỤ LỤC C CHƯƠNG TRÌNH TRÊN KIT DSP Luận Văn Cao Học #include #include #include #include #include #define NULL int lcd_b; const const const const const const float float float float float float Phuï Luïc C 0x00 T=0.01; I1=1.8, ke=0.5; V1=5.0, m=10; L1=0.4, /*chu ky lay mau tan so 100Hz*/ I2=0.041, I3=0.134; /*kgm2*/ V2=2.0; /*kgm2/s*/ /*kg*/ L2=0.35; /*m*/ float M12,C12,kd1,kp1,M11,C11,M21,C21,kd2,kp2,M22,C22; double fi_r12,fi_r11,fi_r10; double fi_r22,fi_r21,fi_r20; double dh_2_fir1, dh_2_fir2; double dh_1_fir1, dh_1_fir2; double fi_12=0,fi_11=0,fi_10=0,fi_22=0,fi_21=0,fi_20=0; double dh_2_fi1=0, dh_2_fi2=0; double dh_1_fi1=0, dh_1_fi2=0; double to_1,to_2; /*xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx*/ /* chuong trinh khoi tao 8255*/ /*xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx*/ void ktao8255(void) { PPICW=0xb4; /*khoi portA: in,portB:out*/ PPICW=0x03; /*PC1=1*/ PPICW=0x09; } /*xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx*/ /* chuong trinh truyen du lieu ve may tinh 8255*/ /*xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx*/ void truyen(int dli) { int datain,t,tam,a ; t=dli; PPIB=t; /*xuat du lieu cao*/ { datain=PPIC; /*kiem tra trang thai*/ a=datain & 0x04; /*giu lai PC2*/ } while(a==0x04); /*chi thoat a=0*/ { datain=PPIC; /*kiem tra trang thai*/ a=datain & 0x02; /*giu lai PC1 */ } while(a!=0x02); /*chi thoat a=2*/ PICW = 0x00; tam = t; /*truyen bit thap*/ tam