Thiết kế bộ điều khiển cho mô hình động học tay máy hàn di động 5 bậc tự do

Đại Học Quốc Gia TP Hồ Chí Minh TRƯỜNG ĐẠI HỌC BÁCH KHOA - NGUYỄN PHƯƠNG BẮC THIẾT KẾ BỘ ĐIỀU KHIỂN CHO MƠ HÌNH ĐỘNG HỌC TAY MÁY HÀN DI ĐỘNG BẬC TỰ DO Chuyên ngành: CÔNG NGHỆ CHẾ TẠO MÁY Mã ngành: 2.01.00 LUẬN VĂN THẠC SĨ TP HỒ CHÍ MINH, tháng 11 năm 2007 CƠNG TRÌNH ĐƯỢC HOÀN THÀNH TẠI TRƯỜNG ĐẠI HỌC BÁCH KHOA ĐẠI HỌC QUỐC GIA TP HỒ CHÍ MINH Cán hướng dẫn khoa học: TS Phan Tấn Tùng Cán chấm phản biện 1: TS Trần Thiện Phúc Cán chấm phản biện 2: TS Bùi Trọng Hiếu Luận văn thạc sĩ bảo vệ HỘI ĐỒNG CHẤM BẢO VỆ LUẬN VĂN THẠC SĨ TRƯỜNG ĐẠI HỌC BÁCH KHOA, ngày 12 tháng năm 2008 ĐẠI HỌC QUỐC GIA TP HCM TRƯỜNG ĐẠI HỌC BÁCH KHOA CỘNG HOÀ XÃ HỘI CHỦ NGHIÃ VIỆT NAM Độc Lập - Tự Do - Hạnh Phúc -oOo Tp HCM, ngày tháng năm NHIỆM VỤ LUẬN VĂN THẠC SĨ Họ tên học viên: NGUYỄN PHƯƠNG BẮC Giới tính : Nam Ngày, tháng, năm sinh : 21/11/1977 Nơi sinh : Tây Ninh Chuyên ngành : Công nghệ Chế Tạo Máy Khoá (Năm trúng tuyển) : 2005 1- TÊN ĐỀ TÀI: THIẾT KẾ BỘ ĐIỀU KHIỂN CHO MƠ HÌNH ĐỘNG HỌC TAY MÁY HÀN DI ĐỘNG BẬC TỰ DO 2- NHIỆM VỤ LUẬN VĂN: 3- NGÀY GIAO NHIỆM VỤ : 05/02/2007 4- NGÀY HOÀN THÀNH NHIỆM VỤ : 05/11/2007 5- HỌ VÀ TÊN CÁN BỘ HƯỚNG DẪN: TS PHAN TẤN TÙNG Nội dung đề cương Luận văn thạc sĩ Hội Đồng Chuyên Ngành thông qua CÁN BỘ HƯỚNG DẪN (Họ tên chữ ký) CHỦ NHIỆM BỘ MÔN QUẢN LÝ CHUYÊN NGÀNH (Họ tên chữ ký) i LỜI CÁM ƠN Trong thời gian thực luận văn, nhận nhiều giúp đỡ động viên chân tình Q Thầy, Cơ bạn đồng nghiệp Hơm nay, luận văn hồn thành, tơi xin kính gửi lời biết ơn sâu sắc đến: Cán hướng dẫn: TS Phan Tấn Tùng nhiệt tình giúp đỡ, bảo hướng dẫn cho tơi suốt thời gian thực luận văn Lãnh đạo Trung Tâm Cơ Khí trường Đại Học Cơng Nghiệp TP.HCM tạo điều kiện mặt thời gian để hồn thành luận văn Thầy Nguyễn Ngọc Điệp, trưởng môn Cơ Điện Tử trường Đại Học Công Nghiệp TPHCM hỗ trợ vật chất lẫn động viên mặt tinh thần giúp vượt qua giai đoạn khó khăn q trình làm đề tài Đặc biệt, xin gửi lời biết ơn sâu sắc đến gia đình, người thân bạn bè động viên, khích lệ hỗ trợ tơi suốt năm học thực luận văn Một lần nữa, xin chúc người nhiều sức khỏe, hạnh phúc thành đạt Ngày tháng 11 năm 2007 Người thực luận văn Nguyễn Phương Bắc ii TĨM TẮT LUẬN VĂN Luận văn trình bày việc mơ hình hóa động học phương pháp thiết kế điều khiển cho tay máy hàn di động bậc tự Bộ điều khiển sử dụng dựa giải thuật điều khiển phi tuyến điều khiển thích nghi nhằm nâng cao đặc tính bám theo quỹ đạo tay máy hàn Giải thuật điều khiển phi tuyến sử dụng trường hợp kích thước tay máy xác định trước Giải thuật điều khiển thích nghi sử dụng trường hợp kích thước tay máy khơng chắn Với việc sử dụng điều khiển này, tay máy hàn thực việc di chuyển bám theo quỹ đạo trước tốc độ hàn cố định Trong luận văn, tác giả trình bày điều khiển dựa ràng buộc động học để điều khiển động học tay máy hàn di động bậc tự Ngoài ra, tác giả kết hợp sử dụng luân phiên điều khiển để khắc phục nhược điểm sử dụng điều khiển đơn lẽ trình hàn Hiệu điều khiển thể thông qua kết mô iii ABSTRACT This thesis presents the methods to model the kinematics and to design the controller of a DOF mobile manipulator The controllers are based the nonlinear and adaptive control algorithms to enhance the tracking properties of the DOF mobile manipulator The nonlinear control algorithm is used when the parameter are known The adaptive control algorithm is used when the parameters are uncertain By using these controllers, a DOF mobile manipulator can track any trajectory with the constant welding velocity In this thesis, I would like to present controllers based on the kinematic constraints to control the kinematic model of DOF mobile manipulator Besides, I combine and use controllers alternately to overcome the disadvantage when using a single controller during the welding process The effectiveness of the proposed controller is shown by simulation results iv MỤC LỤC LỜI CÁM ƠN i TÓM TẮT LUẬN VĂN ii MỤC LỤC iv Chương 1: MỞ ĐẦU 1.1 Tên đề tài mục đích 1.2 Nhiệm vụ 1.3 Ý nghĩa khoa học thực tiễn đề tài .2 Chương 2: TỔNG QUAN 2.1 Giới thiệu .3 2.2 Tính cần thiết đề tài 2.3 Nghiên cứu tham khảo Chương 3: MƠ HÌNH ĐỘNG HỌC CỦA TAY MÁY BẬC TỰ DO .8 3.1 Mơ hình động học khung đế di động .8 3.2 Mô hình động học tay máy L1L2L3 .12 3.2.1 Động học vị trí tay máy L1L2L3 .12 3.2.2 Động học vận tốc tay máy L1L2L3 14 3.3 Mơ hình động học tay máy hàn di động bậc tự 16 3.3.1 Động học vị trí tay máy di dộng bậc tự 16 3.3.2 Động học vận tốc tay máy di động bậc tự .17 Chương 4: THIẾT KẾ BỘ ĐIỀU KHIỂN PHI TUYẾN VỚI CÁC THAM SỐ XÁC ĐỊNH 21 4.1 Định nghĩa sai số 21 4.2 Đo lường sai số 22 4.3 Bậc tự thừa phương án thiết kế điều khiển .23 4.4 Thiết kế điều khiển phi tuyến cho hệ thống thứ 24 v 4.4.1 Phương trình điều khiển .24 4.4.2 Sơ đồ khối điều khiển 29 4.5 Thiết kế điều khiển phi tuyến cho hệ thống thứ hai 30 4.5.1 Phương trình điều khiển .30 4.5.2 Sơ đồ khối điều khiển 33 Chương 5: THIẾT KẾ BỘ ĐIỀU KHIỂN THÍCH NGHI VỚI CÁC THAM SỐ CHƯA XÁC ĐỊNH .34 5.1 Thiết kế điều khiển thích nghi cho hệ thống thứ 34 5.1.1 Phương trình điều khiển 34 5.1.2 Sơ đồ khối điều khiển thích nghi 37 5.2 Thiết kế điều khiển thích nghi cho hệ thống thứ hai 37 5.2.1 Phương trình điều khiển .37 5.2.2 Sơ đồ khối điều khiển 41 Chương 6: KẾT QUẢ MÔ PHỎNG .42 6.1 Mô trình hàn việc sử dụng điều khiển thứ 42 6.1.1 Trường hợp tham số kích thước xác định (bộ điều khiển phi tuyến) 42 6.1.2 Trường hợp tham số kích thước chưa xác định (bộ điều khiển thích nghi) .49 6.2 Mơ q trình hàn việc sử dụng đồng thời điều khiển 53 6.2.1 Trường hợp tham số kích thước xác định (bộ điều khiển phi tuyến) 54 6.2.2 Trường hợp tham số kích thước chưa xác định (bộ điều khiển thích nghi) .58 Chương 7: KẾT LUẬN 63 PHỤ LỤC 64 TÀI LIỆU THAM KHẢO 85 Chương MỞ ĐẦU 1.1 Tên đề tài mục đích Tên đề tài: “Xây dựng điều khiển cho mơ hình động học tay máy bậc tự do” Mục đích đề tài đề giải thuật điều khiển (điều khiển phi tuyến điều khiển thích nghi) cho mơ hình động học tay máy bậc tự ứng dụng tay máy hàn di động 1.2 Nhiệm vụ + Mơ hình hóa động học tay máy di động bậc tự + Giải vấn đề bậc tự thừa, từ đưa phương án thiết kế điều khiển + Thiết kế điều khiển phi tuyến + Thiết kế điều khiển thích nghi + Mơ kết 1.3 Phương pháp nghiên cứu Để hoàn thành luận văn, người thực cần phải sử dụng phương pháp sau: + Phương pháp thu thập tài liệu + Phương pháp xử lý tài liệu + Ứng dụng phép tốn để mơ hình hóa tay máy hàn di động + Ứng dụng nguyên lý điều khiển để xây dựng điều khiển + Phương pháp thực mô 1.4 Ý nghĩa khoa học thực tiễn đề tài 1.4.1 Ý nghĩa khoa học Đề tài nghiên cứu việc ứng dụng giải thuật điều khiển phi tuyến điều khiển thích nghi lĩnh vực Robot di động lĩnh vực cịn quan tâm, tìm hiểu Đề tài hoàn thành sở cho nghiên cứu sau lĩnh vực robot di động kỹ thuật điều khiển 1.4.2 Ý nghĩa thực tiễn Đề tài có tính thực tiễn cao ứng dụng Robot di động đặc biệt Robot hàn di động Với việc sử dụng nhiều điều khiển khác q trình điều khiển Robot hàn, Robot di chuyển linh hoạt thực cơng việc xác bất chấp hình dáng quỹ đạo đường hàn 73 if Ldistance >= 0.53 mode = 1; end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %sai lech theo he truc toa deltax = xR - xE; deltay = yR - yE; deltaphi = phiR - phiE; %cac sai so e1 = cos(phiE)*deltax + sin(phiE)*deltay; e2 = -sin(phiE)*deltax + cos(phiE)*deltay; e3 = deltaphi; %%%%%%%%%%%%%%%%%%%cac ky hieu viet tat s23e3 = sin(theta2+theta3+e3); s3e3 = sin(theta3+e3); %van toc goc cua khau dieu khien if mode == Dthetar = (-s3/(r*c12))*(-k11*e1vR*cos(e3))+(c3/(r*c12))*(k21*e2+vR*sin(e3))+(e1*c3+e2*s3+L3*s3)/(r*c12)*(k31*e3+DphiR); Dtheta1 = 0; Dtheta2 = (c123/(L2*c12))*(-k11*e1vR*cos(e3))+(s123/(L2*c12))*(k21*e2+vR*sin(e3))(e1*s123+e2*c123+L3*c123)/(L2*c12)*(k31*e3+DphiR); Dtheta3 = (-c123/(L2*c12))*(-k11*e1-vR*cos(e3))+(s123/(L2*c12))*(k21*e2+vR*sin(e3))+(e1*s123+e2*c123+L3*c123+L2*c12)/(L2*c 12)*(k31*e3+DphiR); if (Dthetar eps*w xt = (u+w)/2; fx = (xt - xRsau)^2+(yR0 - yRsau + 1*sin(8*(xt-xR0)))^2 - (vR*deltaT)^2; if sign(fx) == sign(fu) u = xt; fu = fx; else w = xt; fw = fx; end end xRtruoc = xRsau; yRtruoc = yRsau; phiRtruoc = phiRsau; xRsau = xt; yRsau = yR0 + 1*sin(8*(xt-xR0)); phiRsau = atan((yRsau-yRtruoc)/(xRsau-xRtruoc)); DphiR = (phiRsau - phiRtruoc)/deltaT; xR = xRsau; yR = yRsau; phiR = phiRsau; %%%%%%%%%%%%%%%%%%%%%%%% ve thi if vequydao == plot(xR,yR) if mode == plot(xE,yE,'-r') else plot(xE,yE,'-g') end plot(xP,yP,'-b') end if vesaiso12 == plot(time,e1) plot(time,e2,'-r') end if vesaiso3 == plot(time,e3,'-r') end end %%%%%%%%%%%%%%%%%%%%%%%%truc toa do%%%%%%%%%%%%% if vequydao == axis([0 1]) plot(xR,yR,'-b') plot(xE,yE,'-r') plot(xE,yE,'-g') plot(xP,yP,'-p') xlabel('truc toa x (m)') ylabel('truc toa y (m)') title('QUY DAO HAN') legend('quy dao han tham chieu qR','quy dao dau han qE bo dieu khien 1','quy dao dau han bo dieu khien 2','quy dao cua tam de di dong') end if (vesaiso12 + vesaiso3) == xlabel('thoi gian (giay)') ylabel('sai so e (m)') title ('SU HOI TU CUA SAI SO SAU 10S DAU TIEN') if vesaiso12 ==1 legend('sai so e1','sai so e2') else if vesaiso3 ==1 75 ylabel('sai so e (rad)') legend('sai so e3') end end end hold off Phụ lục 4: Chương trình mơ trình hàn đoạn cong hình sin trường hợp tham số chưa xác định (sử dụng điều khiển thích nghi thứ nhất) vesaiso12 = 0; vesaiso3 = 0; vequydao = 0; ve_sai_lech_uocluong = 1; if (vesaiso12 + vesaiso3) == sovonglap = 1000; end if vequydao + ve_sai_lech_uocluong == sovonglap = 5000; end %khoang cach giua banh dan va truc doi xung b =0.1; %ban kinh banh xe r0 = 0.0205; %chieu dai khau L1 = 0.25; %chieu dai khau L20 = 0.251; %chieu dai khau L30 = 0.051; %van toc han vR = 007; %%%%%%%%%%kich thuoc ban dau%%%%%%%%%%% L2 = L20; L3 = L30; r = r0; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% phiR0 = 0; DphiR0 = 0; %cac thong so ban dau (t=0): deltax0 = 0.01; %sai lech giua diem han tham chieu va dau han theo phuong X (xR - xE) deltay0 = 0.01; %sai lech giua diem han tham chieu va dau han theo phuong Y (yR - yE) % delta_phi o day%%%%%%%%%%%%%%%%%%%% vE0 = 0; %van toc dau han omegaE0 = 0; % van toc goc cua khau phi0 = 5*pi/180; % goc lech cua de di dong so voi he quy chieu thetar0 = 0; %goc quay banh phai thetal0 = 0; %goc quay banh trai theta10 = 120*pi/180; %goc khau1 theta20 = -90*pi/180; %goc khau2 theta30 = 45*pi/180; %goc khau xP0 = 0.2; %hoanh tam de di dong 76 yP0 = 0.2; %tung tam de di dong xE0 = xP0 + L1*cos(phi0+theta10) + L2*cos(phi0+theta10+theta20) + L3*cos(phi0+theta10+theta20+theta30); yE0 = yP0 + L1*sin(phi0+theta10) + L2*sin(phi0+theta10+theta20) + L3*sin(phi0+theta10+theta20+theta30); phiE0 = phi0 + theta10 +theta20 + theta30 - pi/2; xR0 = xE0 + deltax0; yR0 = yE0 + deltay0; phiRtruoc = 0; phiRsau = 0; xRsau = xR0; yRsau = yR0; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%5 phiR = phiR0; DphiR = DphiR0; deltax = deltax0; deltay = deltay0; % delta_phi o day%%%%%%%%%%%%%%%%%%%% phi = phi0; thetar = thetar0; thetal = thetal0; theta1 = theta10; theta2 = theta20; theta3 = theta30; Dthetar = 0; Dtheta1 = 0; Dtheta2 = 0; Dtheta3 = 0; xP = xP0; yP = yP0 ; xR = xR0; yR = yR0; k1 = 1.5; k2 = 1.5; k3 = 2; gama1 = 0.23; gama2 = 0.28; gama3 = 0.27; deltaT = 0.01; time = 0; hold on % i = tuong ung o thoi diem ban dau va bang 1000 tuong ung o thoi diem 10s %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5 for k = 0:sovonglap %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %mot so dinh nghia viet tat time = time + deltaT; s1 = sin(theta1); s2 = sin(theta2); s3 = sin(theta3); s12 = sin(theta1+theta2); s13 = sin(theta1+theta3); s23 = sin(theta2+theta3); s123 = sin(theta1+theta2+theta3); c1 = cos(theta1); c2 = cos(theta2); 77 c3 = cos(theta3); c12 = cos(theta1+theta2); c13 = cos(theta1+theta3); c23 = cos(theta2+theta3); c123 = cos(theta1+theta2+theta3); tg1 = tan(theta1); tg2 = tan(theta2); tg3 = tan(theta3); tg12 = tan(theta1+theta2); tg13 = tan(theta1+theta3); tg23 = tan(theta2+theta3); tg123 = tan(theta1+theta2+theta3); %%%%%%%%%%%%%% %toa diem han: xE = xP + L1*cos(phi+theta1) + L2*cos(phi+theta1+theta2) + L3*cos(phi+theta1+theta2+theta3); yE = yP + L1*sin(phi+theta1) + L2*sin(phi+theta1+theta2) + L3*sin(phi+theta1+theta2+theta3); phiE = phi + theta1 +theta2 + theta3 - pi/2; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %sai lech theo he truc toa deltax = xR - xE; deltay = yR - yE; deltaphi = phiR - phiE; %cac sai so e1 = cos(phiE)*deltax + sin(phiE)*deltay; e2 = -sin(phiE)*deltax + cos(phiE)*deltay; e3 = deltaphi; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%cap nhat kich thuoc Dr_mu = 1/gama3*(e1*s123+e2*c123)*Dthetar; r = r+Dr_mu*deltaT; sailech_r = r0 - r; DL2_mu = 1/gama1*(e2*s3-e1*c3)*Dtheta2; L2 = L2+DL2_mu*deltaT; sailech_L2 = L20 - L2; DL3_mu = -1/gama2*e1*(Dtheta2+Dtheta3); L3 = L3+DL3_mu*deltaT; sailech_L3 = L30 - L3; %van toc goc cua khau dieu khien Dthetar = (-s3/(r*c12))*(-k1*e1vR*cos(e3))+(c3/(r*c12))*(k2*e2+vR*sin(e3))+(e1*c3+e2*s3+L3*s3)/(r*c12)*(k3*e3+DphiR); Dtheta2 = (c123/(L2*c12))*(-k1*e1vR*cos(e3))+(s123/(L2*c12))*(k2*e2+vR*sin(e3))(e1*s123+e2*c123+L3*c123)/(L2*c12)*(k3*e3+DphiR); Dtheta3 = (-c123/(L2*c12))*(-k1*e1-vR*cos(e3))+(s123/(L2*c12))*(k2*e2+vR*sin(e3))+(e1*s123+e2*c123+L3*c123+L2*c12)/(L2*c1 2)*(k3*e3+DphiR); thetar = thetar+ Dthetar*deltaT; % dao ham bang gia so cua goc thetar tren mot deltaT theta2 = theta2+ Dtheta2*deltaT; % dao ham bang gia so cua goc theta2 tren mot deltaT theta3 = theta3+ Dtheta3*deltaT; % dao ham bang gia so cua goc theta3 tren mot deltaT xP = xP + Dthetar*deltaT*r*cos(phi); yP = yP + Dthetar*deltaT*r*sin(phi); 78 %%%phan xac dinh toa cua diem han tham chieu %%%%%%%quy dao han: y = yR0 +200*sin(8*(x-xR0))(mm) u = xRsau; fu = -Inf; w = xRsau + vR*deltaT; fw = Inf; while w-u > eps*w xt = (u+w)/2; fx = (xt - xRsau)^2+(yR0 - yRsau + 1*sin(8*(xt-xR0)))^2 - (vR*deltaT)^2; if sign(fx) == sign(fu) u = xt; fu = fx; else w = xt; fw = fx; end end xRtruoc = xRsau; yRtruoc = yRsau; phiRtruoc = phiRsau; xRsau = xt; yRsau = yR0 + 1*sin(8*(xt-xR0)); phiRsau = atan((yRsau-yRtruoc)/(xRsau-xRtruoc)); DphiR = (phiRsau - phiRtruoc)/deltaT; xR = xRsau; yR = yRsau; phiR = phiRsau; %%%%%%%%%%%%%%%%%%%%%%%% ve thi %plot(time,e1) %plot(time,e2,' r') %plot(time,e3,' r') if vequydao == plot(xR,yR,'-b') plot(xE,yE,'-r') plot(xP,yP,'-g') end if vesaiso12 == plot(time,e1) plot(time,e2,'-r') end if vesaiso3 == plot(time,e3,'-r') end if ve_sai_lech_uocluong == plot(time,sailech_r) plot(time,sailech_L2,'-r') plot(time,sailech_L3,'-g') end %%%%%%ket thuc vong lap for end if vequydao == axis([0 1]) xlabel('truc toa x (m)') ylabel('truc toa y (m)') title('QUY DAO HAN') legend('quy dao han tham chieu qR','quy dao dau han qE','quy dao cua tam de di dong') end if (vesaiso12 + vesaiso3) == xlabel('thoi gian (giay)') ylabel('sai so e (m)') 79 title ('SU HOI TU CUA SAI SO SAU 10S DAU TIEN') if vesaiso12 ==1 legend('sai so e1','sai so e2') else if vesaiso3 ==1 ylabel('sai so e (rad)') legend('sai so e3') end end end if ve_sai_lech_uocluong ==1 xlabel('thoi gian (giay)') ylabel('cac gia tri uoc luong (m)') title ('CAC KICH THUOC UOC LUONG') legend('uoc luong cua r','uoc luong cua L2','uoc luong cua L3') end hold off Phụ lục 5: Chương trình mơ q trình hàn đoạn cong hình sin trường hợp tham số chưa xác định (sử dụng điều khiển thích nghi đồng thời) vesaiso12 = 0; vesaiso3 = 1; vequydao = 0; ve_sai_lech_uocluong = 0; if (vesaiso12 + vesaiso3) == sovonglap = 1000; end if (vequydao + ve_sai_lech_uocluong) == sovonglap = 25000; end %khoang cach giua banh dan va truc doi xung b =0.1; %ban kinh banh xe r0 = 0.0205; %chieu dai khau L10 = 0.251; %chieu dai khau L20 = 0.251; %chieu dai khau L30 = 0.051; %van toc han vR = 007; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%kich thuoc ban dau%%%%%%%%%%%%% L1 = L10; L2 = L20; L3 = L30; r = r0; %%%%%%%%%%%%%%%%%%%%%%%%%%%% phiR0 = 0; DphiR0 = 0; DphiE0 = 0; %cac thong so ban dau (t=0): 80 deltax0 = 0.01; %sai lech giua diem han tham chieu va dau han theo phuong X (xR - xE) deltay0 = 0.01; %sai lech giua diem han tham chieu va dau han theo phuong Y (yR - yE) % delta_phi o day%%%%%%%%%%%%%%%%%%%% vE0 = 0; %van toc dau han omegaE0 = 0; % van toc goc cua khau phi0 = 5*pi/180; % goc lech cua de di dong so voi he quy chieu thetar0 = 0; %goc quay banh phai thetal0 = 0; %goc quay banh trai theta10 = 120*pi/180; %goc khau1 theta20 = -110*pi/180; %goc khau2 theta30 = 45*pi/180; %goc khau xP0 = 0.2; %hoanh tam de di dong yP0 = 0.2; %tung tam de di dong xE0 = xP0 + L1*cos(phi0+theta10) + L2*cos(phi0+theta10+theta20) + L3*cos(phi0+theta10+theta20+theta30); yE0 = yP0 + L1*sin(phi0+theta10) + L2*sin(phi0+theta10+theta20) + L3*sin(phi0+theta10+theta20+theta30); phiE0 = phi0 + theta10 +theta20 + theta30 - pi/2; xR0 = xE0 + deltax0; yR0 = yE0 + deltay0; phiRtruoc = 0; phiRsau = 0; xRsau = xR0; yRsau = yR0; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%5 DphiE = DphiE0; phiR = phiR0; DphiR = DphiR0; deltax = deltax0; deltay = deltay0; % delta_phi o day%%%%%%%%%%%%%%%%%%%% phi = phi0; Dphi = 0; thetar = thetar0; thetal = thetal0; theta1 = theta10; theta2 = theta20; theta3 = theta30; Dtheta1 = 0; Dtheta2 = 0; Dtheta3 = 0; Dthetar = 0; xP = xP0; yP = yP0; xR = xR0; yR = yR0; k11 = 1.5; k21 = 1.5; k31 = 2; k12 = 1.5; k22 = 1.28; k32 = 2.0; gamar = 0.25; gama1 = 0.7; gama2 = 0.45; gama3 = 0.8; 81 deltaT = 0.01; time = 0; mode =0; hold on % i = tuong ung o thoi diem ban dau va bang 1000 tuong ung o thoi diem 10s %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5 for k = 0:1:sovonglap %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %mot so dinh nghia viet tat time = time + deltaT; s1 = sin(theta1); s2 = sin(theta2); s3 = sin(theta3); s12 = sin(theta1+theta2); s13 = sin(theta1+theta3); s23 = sin(theta2+theta3); s123 = sin(theta1+theta2+theta3); c1 = cos(theta1); c2 = cos(theta2); c3 = cos(theta3); c12 = cos(theta1+theta2); c13 = cos(theta1+theta3); c23 = cos(theta2+theta3); c123 = cos(theta1+theta2+theta3); tg1 = tan(theta1); tg2 = tan(theta2); tg3 = tan(theta3); tg12 = tan(theta1+theta2); tg13 = tan(theta1+theta3); tg23 = tan(theta2+theta3); tg123 = tan(theta1+theta2+theta3); %%%%%%%%%%%%%% %toa diem han so voi he mat dat: xE = xP + L1*cos(phi+theta1) + L2*cos(phi+theta1+theta2) + L3*cos(phi+theta1+theta2+theta3) yE = yP + L1*sin(phi+theta1) + L2*sin(phi+theta1+theta2) + L3*sin(phi+theta1+theta2+theta3) phiE = phi + theta1 +theta2 + theta3 - pi/2; %toa diem han so voi he quy chieu de banh xe xEp = L1*cos(theta1) + L2*cos(theta1+theta2) + L3*cos(theta1+theta2+theta3); yEp = L1*sin(theta1) + L2*sin(theta1+theta2) + L3*sin(theta1+theta2+theta3); %%%%%%%%%%%%toc de di dong vP = r*Dthetar; %khoang cach tu diem han den de banh xe Ldistance = sqrt(xEp^2+yEp^2); %%%%%%%%%%%% neu khoang cach vuot qua mot gioi han cho truoc thi se %%%%%%%%%%%% ht se chuyen sang bo dieu khien khac if Ldistance >= 0.53 mode = 1; end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %sai lech theo he truc toa deltax = xR - xE; deltay = yR - yE; 82 deltaphi = phiR - phiE; %cac sai so e1 = cos(phiE)*deltax + sin(phiE)*deltay; e2 = -sin(phiE)*deltax + cos(phiE)*deltay; e3 = deltaphi; %%%%%%%%%cap nhat kich thuoc%%%%%%%%%%%%%%%%%%%%% if mode == Dr_mu = 1/gama3*(e1*s123+e2*c123)*Dthetar; r = r+Dr_mu*deltaT; sailech_r = r0 - r; DL2_mu = 1/gama2*(e2*s3-e1*c3)*Dtheta2; L2 = L2+DL2_mu*deltaT; sailech_L2 = L20 - L2; DL3_mu = -1/gama3*e1*(Dtheta2+Dtheta3); L3 = L3+DL3_mu*deltaT; sailech_L3 = L30 - L3; else DL1_mu = 1/gama1*e1*(-c23-Dtheta1*c23) + 1/gama1*e2*(s23+Dtheta1*s23); L1 = L1+DL1_mu*deltaT; sailech_L1 = L10 - L1; DL2_mu = 1/gama2*e1*(-c3(Dtheta1+Dtheta2)*c3)+1/gama2*e2*(s3+(Dtheta1+Dtheta2)*s3); L2 = L2+DL2_mu*deltaT; sailech_L2 = L20 - L2; DL3_mu = 1/gama3*e1*(-1-Dtheta1-Dtheta2-Dtheta3); L3 = L3+DL3_mu*deltaT; sailech_L3 = L30 - L3; end %%%%%%%%%%%%%%%%%%%cac ky hieu viet tat s23e3 = sin(theta2+theta3+e3); s3e3 = sin(theta3+e3); %van toc goc cua khau dieu khien if mode == Dthetar = (-s3/(r*c12))*(-k11*e1vR*cos(e3))+(c3/(r*c12))*(k21*e2+vR*sin(e3))+(e1*c3+e2*s3+L3*s3)/(r*c12)*(k31*e3+DphiR); Dtheta1 = 0; Dtheta2 = (c123/(L2*c12))*(-k11*e1vR*cos(e3))+(s123/(L2*c12))*(k21*e2+vR*sin(e3))(e1*s123+e2*c123+L3*c123)/(L2*c12)*(k31*e3+DphiR); Dtheta3 = (-c123/(L2*c12))*(-k11*e1-vR*cos(e3))+(s123/(L2*c12))*(k21*e2+vR*sin(e3))+(e1*s123+e2*c123+L3*c123+L2*c12)/(L2*c 12)*(k31*e3+DphiR); if (Dthetar eps*w xt = (u+w)/2; fx = (xt - xRsau)^2+(yR0 - yRsau + 1*sin(8*(xt-xR0)))^2 - (vR*deltaT)^2; if sign(fx) == sign(fu) u = xt; fu = fx; else w = xt; fw = fx; end end xRtruoc = xRsau; yRtruoc = yRsau; phiRtruoc = phiRsau; xRsau = xt; yRsau = yR0 + 1*sin(8*(xt-xR0)); phiRsau = atan((yRsau-yRtruoc)/(xRsau-xRtruoc)); DphiR = (phiRsau - phiRtruoc)/deltaT; xR = xRsau; yR = yRsau; phiR = phiRsau; %%%%%%%%%%%%%%%%%%%%%%%% ve thi if vequydao == plot(xR,yR) if mode == plot(xE,yE,'-r') else plot(xE,yE,'-g') end plot(xP,yP,'-b') end if vesaiso12 == plot(time,e1) plot(time,e2,'-r') end if vesaiso3 == plot(time,e3,'-r') end if ve_sai_lech_uocluong == plot(time,sailech_r) plot(time,sailech_L1,'-b') plot(time,sailech_L2,'-r') plot(time,sailech_L3,'-g') end %%%%%%ket thuc vong lap for end 84 if vequydao == axis([0 1]) plot(xR,yR,'-b') plot(xE,yE,'-r') plot(xE,yE,'-g') plot(xP,yP,'-p') xlabel('truc toa x (m)') ylabel('truc toa y (m)') title('QUY DAO HAN') legend('quy dao han tham chieu qR','quy dao dau han qE bo dieu khien 1','quy dao dau han bo dieu khien 2','quy dao cua tam de di dong') end if (vesaiso12 + vesaiso3) == xlabel('thoi gian (giay)') ylabel('sai so e (m)') title ('SU HOI TU CUA SAI SO SAU 10S DAU TIEN') if vesaiso12 ==1 legend('sai so e1','sai so e2') else if vesaiso3 ==1 ylabel('sai so e (rad)') legend('sai so e3') end end end hold off 85 TÀI LIỆU THAM KHẢO [1] – Lại Khắc Liễm – Giáo trình Cơ Học Máy – Nhà xuất Đại Học Quốc Gia TP.HCM – 2000 [2] – Đinh Gia Tường – Nguyên Lý Máy – Nhà xuất Giáo Dục – 1998 [3] – Nguyễn Ngọc Cẩn – Kỹ thuật Điều Khiển Tự Động – Nhà xuất Đại Học Quốc Gia TP.HCM – 2001 [4] – Yutako Kanayama – Two Dimensional Wheeled Vehicle Kinematics – Proceedings of the IEEE International Conference on Robotics and Automation, Vol 4, pp 3079-3084, 1994 [5] – Meng.Pi Cheng, Ching.Chih Tsai – Dynamic Modeling and Tracking Control of a Nonholomic Wheeled Mobile Manipulator with Two Robot Arms – Proceedings of the IEEE Conference on Decision and Control, Vol 3, pp 29322937, 2003 [6] – Fukao – Adaptive Tracking Control of a Nonholonomic Mobile Robot – Transactions on Robotics and Automation, Vol 16, No 5, pp 609-615, 2000 [7] – Trong Hieu Bui, Tan Lam Chung, Jin Ho Suh, Sang Bong Kim– Adaptive Control for Tracking Trajectory of a Two-Wheel Welding Mobile Robot with Unknown Parameter – Proceedings of the International Conference on Control, Automation and Systems, pp 191-196, 2003 [8] – Trong Hieu Bui, Tan Lam Chung, Sang Bong Kim – A Simple Nonlinear Control of a Two-Wheeled Welding Mobile Robot – Transactions on International Journal of Control, Automation, and Systems (IJCAS), Vol 1, No 1, pp 35-42, 2003 [9] –Tan Tung Phan – Adaptive Control of DOF Mobile Manipulator – Ph.D Thesis – 2005 86 [10] – J.J.Craig, P Hsu and S S Sastry – Adaptive Control of Mechanical Manipulators – Proceedings of the IEEE International Conference on Robotics and Automation, Vol 2, pp 190-195, 1986 [11] – R.Colbaugh, H.Seraji – A New Approach to Adaptive Manipulator Control – Proceedings of the IEEE International Conference on Robotics and Automation, Vol 1, pp.604-611, May 1993 [12] – J.Craig – Introduction to Robotics – Mechanics and Control – Addison Wesley Publishing Company, 1989 [13] – John Mc Kerrow – In troduction to Robotics – Addison Wesley Publishing Company, 1993 [14] – Petros A Ioannou, Jing Sun – Robust Adaptive Control – Mc Graw Hill – năm 1996 87 LÝ LỊCH TRÍCH NGANG Họ tên: Nguyễn Phương Bắc Ngày, tháng, năm sinh: 21/11/2007 Nơi sinh: Tây Ninh Địa liên lạc: 1/22A Tân Sơn, Phường 12, Quận Gị Vấp, TP Hồ Chí Minh Q trình đào tạo: 1995 – 2000: Học tốt nghiệp Kỹ sư chun ngành Cơ Khí Hàng Khơng trường Đại Học Bách Khoa TP Hồ Chí Minh 2005 – 2007: Học Thạc Sĩ ngành Công Nghệ Chế Tạo Máy, trường Đại Học Bách Khoa TP Hồ Chí Minh Q trình cơng tác: 2000 – 2001: Làm việc môn Thiết Kế Máy, Đại Học Bách Khoa TP Hồ Chí Minh 2001 – 2007: Làm việc khoa Cơ Khí, trường Đại Học Cơng Nghiệp TP Hồ Chí Minh ... 3.3 Mô hình động học tay máy hàn di động bậc tự 16 3.3.1 Động học vị trí tay máy di dộng bậc tự 16 3.3.2 Động học vận tốc tay máy di động bậc tự .17 Chương 4: THIẾT KẾ BỘ ĐIỀU KHIỂN... cho mơ hình động học tay máy bậc tự ứng dụng tay máy hàn di động 1.2 Nhiệm vụ + Mơ hình hóa động học tay máy di động bậc tự + Giải vấn đề bậc tự thừa, từ đưa phương án thiết kế điều khiển + Thiết. .. mơ hình tay máy PUMA762 8 Chương MƠ HÌNH ĐỘNG HỌC CỦA TAY MÁY BẬC TỰ DO 3.1 Mô hình động học khung đế di động Xét tay máy di động hình 3.1: Giả sử, khung đế di động có bánh xe đồng trục dẫn động