Điều khiển hệ tay máy và robot di động bám theo quỹ đạo

BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT THÀNH PHỐ HỒ CHÍ MINH LUẬN VĂN THẠC SĨ TRƯƠNG THIỆN QUÂN ĐIỀU KHIỂN HỆ TAY MÁY VÀ ROBOT DI ĐỘNG BÁM THEO QUỸ ĐẠO NGÀNH: KỸ THUẬT ĐIỆN TỬ - 605270 S K C0 3 Tp Hồ Chí Minh, tháng 08/2014 BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƢỜNG ĐẠI HỌC SƢ PHẠM KỸ THUẬT THÀNH PHỐ HỒ CHÍ MINH - LUẬN VĂN THẠC SĨ TRƯƠNG THIỆN QUÂN ĐIỀU KHIỂN HỆ TAY MÁY VÀ ROBOT DI ĐỘNG BÁM THEO QUỸ ĐẠO NGÀNH: KỸ THUẬT ĐIỆN TỬ - 60 52 70 Tp Hồ Chí Minh, tháng năm 2014 BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƢỜNG ĐẠI HỌC SƢ PHẠM KỸ THUẬT TP HCM - LUẬN VĂN THẠC SĨ TRƢƠNG THIỆN QUÂN ĐIỀU KHIỂN HỆ TAY MÁY VÀ ROBOT DI ĐỘNG BÁM THEO QUỸ ĐẠO NGÀNH: KỸ THUẬT ĐIỆN TỬ - 60 52 70 Hƣớng dẫn khoa học: TS NGUYỄN THANH PHƢƠNG Tp Hồ Chí Minh, tháng năm 2014 LÝ LỊCH KHOA HỌC I LÝ LỊCH SƠ LƢỢC: Họ & tên: TRƢƠNG THIỆN QUÂN Giới tính: Nam Ngày, tháng, năm sinh: 02 / 10 / 1979 Nơi sinh: TT- Huế Quê quán: Phong Sơn, Phong Điền, TT-Huế Dân tộc: Kinh Chỗ riêng địa liên lạc: Hải Tân, Long Hải, Long Điền, TT-Huế Điện thoại quan: Điện thoại nhà riêng: 0972060601 Fax: E-mail: truongthienquan@gmail.com II QUÁ TRÌNH ĐÀO TẠO: Trung học chuyên nghiệp: Hệ đào tạo: Nơi học (trƣờng, thành phố): Ngành học: Thời gian đào tạo từ ……/…… đến ……/ …… Đại học: Hệ đào tạo: Chính Quy Thời gian đào tạo từ Nơi học (trƣờng, thành phố): Trƣờng ĐHSPKT Tp HCM Ngành học: Kỹ thuật Điện – Điện tử Tên đồ án, luận án môn thi tốt nghiệp: 1999 đến /2004 Ngày & nơi bảo vệ đồ án, luận án thi tốt nghiệp: Trƣờng Đại học SPKT Tp HCM Ngƣời hƣớng dẫn: III Q TRÌNH CƠNG TÁC CHUYÊN MÔN KỂ TỪ KHI TỐT NGHIỆP ĐẠI HỌC: Thời gian Nơi công tác Công việc đảm nhiệm 2004 - 2005 Cty TNHH Minh Long Quản đốc 2005 - Trƣờng Cao Đẳng Công Đồng BRVT Giảng viên LỜI CAM ĐOAN Tơi cam đoan cơng trình nghiên cứu Các số liệu, kết nêu luận văn trung thực chƣa đƣợc công bố cơng trình khác Tp Hồ Chí Minh, ngày … tháng … năm 201… (Ký tên ghi rõ họ tên) Trƣơng Thiện Quân CẢM TẠ Em xin bày tỏ lời cảm ơn chân thành sâu sắc tới tiến sĩ Nguyễn Thanh Phương - người thầy tận tâm, nhiệt tình bảo, hướng dẫn em suốt trình thực luận văn thạc sĩ này! Em xin chân thành cảm ơn Ban Giám hiệu, Phịng Sau đại học, thầy khoa Điện – Điện tử trường ĐHSPKT Tp HCM quan tâm tạo điều kiện thuận lợi cho em hoàn thành khóa học! Xin gửi lời tri ân đến gia đình, bạn bè đồng nghiệp ln động viên, khích lệ, giúp đỡ tơi q trình học tập nghiên cứu! Tp HCM ngày 10 tháng năm 2014 Tác giả Trương Thiện Quân TÓM TẮT Trong năm qua, nhiều nghiên cứu đƣợc thực cho robot di động kèm cánh tay máy (tiêu biểu robot hàn) sử dụng nhiều điều khiển khác [1-3] Trong tài liệu tham khảo [1], điều khiển phi tuyến đơn giản đƣợc dùng để dò bám theo đƣờng hàn cho trƣớc, nhƣng vào mơ hình động học mà bỏ qua phần động lực học hệ thống khí nhiễu từ bên Chung [2] đề xuất điều khiển trƣợt mà kết hợp phần động học động lực học với ràng buộc nhiễu từ bên ngoài, nhằm giải vấn đề dò theo quỹ đạo thực đƣợc robot di động Ngo [3] đề xuất điều khiển trƣợt thích nghi cho loại robot hàn trên, điều khiển trên, robot di động đƣợc khảo sát dựa vào mơ hình động học với thông số cho trƣớc nhiễu từ bên ngồi Tuy nhiên vận tốc tuyến tính điểm hàn không đƣợc giữ cố định nhƣ thiết kế ban đầu Để giải vấn đề, luận thiết kế điều khiển trƣợt, tích hợp mơ hình động học mơ hình động lực học với ràng buộc nhiễu từ bên cho robot hàn để dị theo đƣờng hàn cho trƣớc Tính ổn định hệ thống đƣợc chứng minh dựa vào lý thuyết ổn định Lyapunov Kết mô kết thực nghiệm đƣợc trình bày nhằm minh họa tính hiệu điều khiển trƣợt tích phân cho đối tƣợng robot di động mà robot hàn mang tính tiêu biểu Nội dung luận văn gồm có: Nghiên cứu tổng quan robot hàn di động hai bánh xe Xây dựng mơ hình tốn cho robot di động có cánh tay máy (robot hàn) Mơ hình hình học robot mơ hình động học, mơ hình động lực học robot Thiết kế điều khiển phi tuyến cho robot di động Thiết kế robot hàn di động có cánh tay Kết luận hƣớng phát triển đề tài i MỤC LỤC TÓM TẮT i MỤC LỤC ii DANH MỤC CÁC TỪ VIẾT TẮT v DANH MỤC CÁC BẢNG vii DANH MỤC CÁC HÌNH ẢNH viii CHƢƠNG GIỚI THIỆU LUẬN VĂN 1.1 Tổng quan robot 1.2 Kỹ thuật điều hƣớng cho robot di động bánh xe 12 1.3 Tóm tắt số cơng trình nghiên cứu 14 1.4 Nhận xét chung hƣớng tiếp cận 20 1.5 Mục tiêu luận văn 22 1.6 Nhiệm vụ luận văn 22 1.7 Giới hạn luận văn 22 1.8 Điểm luận văn 22 1.9 Nội dung luận văn 23 CHƢƠNG CƠ SỞ LÝ THUYẾT 24 2.1 Tiêu chuẩn ổn định Lyapunov 24 2.2 Điều khiển trƣợt .25 2.2.1 Xuất phát điểm phƣơng pháp điều khiển trƣợt 25 2.2.2 Nguyên lý điều khiển trƣợt 29 2.2.3 Các bƣớc xây dựng điều khiển trƣợt .29 CHƢƠNG XÂY DỰNG MÔ HÌNH TỐN CHO ROBOT DI ĐỘNG CĨ CÁNH TAY MÁY 31 3.1 Mơ hình hình học robot di động có cánh tay máy hai bánh xe: 31 ii 3.1.1 Giới thiệu 32 3.1.2 Mô hình động học robot hàn có cánh tay máy di động hai bánh xe …………………………………………………………………………….32 3.1.3.Mơ hình động lực học robot hàn di động hai bánh xe 329 3.2 Mơ hình học robot hàn có cánh tay máy nhiều bậc có hai bánh xe:………………………………………………………………….………… 34 3.2.1.Giới thiệu 34 3.2.2.Mơ hình động học tay máy 36 3.2.3.Mơ hình hệ thống robot tay máy di động .37 CHƢƠNG THIẾT KẾ BỘ ĐIỀU KHIỂN TRƢỢT 37 4.1 Đặt vấn đề 38 4.2 Thiết kế điều khiển trƣợt .38 4.2.1 Giới thiệu 39 4.2.2 Kết mô 410 a Lƣu đồ giải thuật điều khiển .40 b Các kết mô 41 4.3 Giải thuật trƣợt cho cánh tay đa bậc 47 4.3.1 Giới thiệu 47 4.3.2 Kết mô 50 CHƢƠNG KẾT LUẬN VÀ HƢỚNG PHÁT TRIỂN 52 5.1 Những kết đạt đƣợc 52 5.2 Hạn chế đề tài 52 5.3 Hƣớng phát triển đề tài 52 TÀI LIỆU THAM KHẢO 54 PHỤ LỤC 1.Chứng minh mơ hình động học ,động lực học robot điều khiển trƣợt robot .56 1.1 Mơ hình động học 56 1.2 Thiết kế điều khiển trƣợt tích phân 60 Code matlab 56 iii 1.1 Điều khiển bậc 63 1.2 Điều khiển đa bậc 63 iv để đơn giản, giả sử r = n – m Đạo hàm (2) thay kết váo (13) nhân với ƮT, ma trận ràng buộc AT (q)λ bị lựơt bỏ động lực học robot với ràng buộc (1) nhƣ sau : J T MJ z  J T (MJ  VJ) z  J T Bτ (14) Nhân với ( J T B) 1 , từ (14) viết lại là: M(q) z  V(q, q ) z  τ (15) Trong luận văn này, trạng thái robot hàn có diện nhiễu bên τ d  R r1 để xem xét, chƣơng trình động học thực tế robot hàn với nhiễu bên đƣợc rút từ (15) nhƣ sau : M(q) z  V(q, q ) z  τ d  τ (16) Giả sử vecto nhiễu đƣợc diễn tả nhƣ ma trận cấp số nhân M(q) nhƣ sau: τ d  M(q)f Với f  R(n m)1 vecto nhiễu bên ngồi hệ thống Bằng cách tuyến tính hóa phản hồi hệ thống vecto điều khiển u  R(n m)1 đƣợc xác định cách tính momen xoắn nhƣ sau : τ  M(q) z d  V(q, q ) z  M(q) u (17) Với z d  R (n m)1 vecto đầu vào Từ (15), 18) ta có : f  u  z  z d (18) 58 Với q  x, y,  , n=3 ; m=1 ; r=2 Ta có hệ số nhƣ sau : T   V  r m d c  2b  r2 mc d 2b     r2  (mb  I )  I w M   4b  r (mb  I )  4b (19)  r2 (mb  I )  4b  r2 (mb  I )  I w  4b  I  mc d  2mwb  I c  2I m m  mc  2mw , z= [x  = [  rw f = [ f1 (20) T  ] , z d = [ vd T d ]  lw ]T f ]T 1.2 Thiết kế điều khiển trƣợt tích phân Bộ điều khiển trƣợt tích phân cho rơbốt đƣợc thiết kế mơ hình động học động lực học Điểm tham chiếu R( xr , yr , r ) di chuyển theo đƣờng dẫn cho trƣớc với vận tốc cố định Trong hình véc tơ sai số e  e1 , e2 , e3  đƣợc xác định sai số  điểm hàn w điểm tham chiếu R( xr , yr , r ) nhƣ sau:  e1   cos e  e2   sin     e3   sin cos   xr   yr  1 r xw yw w     (21) Mục đích tốn thiết kế điều khiển phi tuyến điểm hàn W dò theo điểm tham chiếu R nghĩa làm cho sai số e  , t  Xét trƣờng hợp chiều dài mỏ hàn điều khiển đƣợc sử dụng cấu trƣợt mỏ hàn Đạo hàm bậc phƣơng trình (10) ta đƣợc: 59  e1  1 e  l e  e     e1    1  e3    v v r cos e3      v sin e  i      r    r  (22) Hàm Lyapunov đƣợc định nghĩa nhƣ sau: V  V1  V2  (23) 2 V1  (e1  e2  e3 )  (24) đó: Từ (13) đạo hàm ta có : V1  e1e1  e2 e2  e3e3 Thay vào (11) ta có: V1  e1 (v  e2    vr cos e3 )  e2 (e1  vr sin e3  )  e3 (  r ) V2   S S 0 (25) Sv  Sv1 Sv  véc tơ mặt trƣợt Luật điều khiển động lực học đƣợc thiết kế nhƣ sau [1]: vd  l (R  C3e3 )  vR cos e3  C1e1  zd       R  C3e3 wd    (26) Chiều dài mỏ hàn thỏa mãn: l  vR sin e3  C2e2  (27) Với C1 , C2 , C3 giá trị dƣơng Với vận tốc điều khiển (15) V1 trở thành : V1  C1e12  C2e22  C3e32  (28) Véc tơ mặt trƣợt S v đƣợc định nghĩa nhƣ sau: Sv  ev  Kv  ev dt 60 (29) Với ev  zd  z  ev1 ev   R x1  vectơ sai số vận tốc K v ma trận đƣờng chéo dƣơng Đạo hàm Sv nhƣ sau:  + Kv ev S v = ev + Kv ev = (z d - z) (30) Thay (9) vào (19) rút gọn, ta đƣợc phƣơng trình sau: Sv  f  u   ( zd  z)  Kv ev (31) Luật điều khiển u  u1 u2  thiết kế nhƣ sau:  u  QSv  Psign( Sv )  Kv ev (32) Ta có m S  0 Q P   f  f  Sv   v1  ; Q   P f   1      ; ;  Sv   Q2   P2   f   f 2m  Qi Pi , i  1, số dƣơng f m i , i  1, giới hạn f i Với luật điều khiển (21), (20) trở thành: S v = QSv  Psign(Sv ) + f (33) Thay (22) vào phƣơng trình đạo hàm bậc V2 V2 = STv S v  -STv QSv - Sv1 ( P1  f1m )  Sv2 ( P2  f2m ) m Nếu chọn Qi ³ Pi  fi , i Theo bổ đề Barbalat, Sv   1.2 (34) V2  t   , nghĩa tồn luật điều khiển u ổn định mặt trƣợt tiến không Từ (12) đến (14) (17), (23), V  Nghĩa hai e  ev  , kết điểm W rô bốt bám theo điểm tham chiếu R chuyển động theo đƣờng cong tham chiếu với vận tốc không đổi nhƣ mong muốn 61 code matlab 2.1 điều khiển bậc close all clear all %**************************************** % trajectory %**************************************** dt=0.01; Vr=0.05; Kv1=0.3; Kv2=0.06; Rr=6240*Vr*dt*(4/(2*pi)); Lr=4000*Vr*dt; x1=0.25; y1=0.4; x2=x1+Lr; y2=y1; % ngang x3=x2+Rr; y3=y2+Rr; % cong trai x4=x3; y4=y3+Lr; % dung thang x5=x4+Rr; y5=y4+Rr; % cong phai x6=x5+Lr; y6=y5; % ngang xr1(1)=x1; yr1(1)=y1; phr1(1)=0; wr1(1)=0; n1=round(Lr/(Vr*dt)); for i=2:n1 xr1(i)=xr1(i-1)+Vr*dt; yr1(i)=yr1(i-1); % ngang phr1(i)=0; wr1(i)=0; end; xr2(1)=x2; yr2(1)=y2; phr2(1)=0; wr2(1)=Vr/Rr; n2=round(pi*Rr/(2*Vr*dt)); for i=2:n2 if i==2 i23=0; end; xr2(i)=x2+Rr*sin(Vr*i23*dt/Rr); yr2(i)=y2+Rr*(1-cos(Vr*i23*dt/Rr)); phr2(i)=Vr*i23*dt/Rr; % cong trai wr2(i)=Vr/Rr; i23=i23+1; end; xr3(1)=x3; yr3(1)=y3; phr3(1)=pi/2; 62 wr3(1)=0; n3=round(Lr/(Vr*dt)); for i=2:n3 xr3(i)=xr3(i-1); yr3(i)=yr3(i-1)+Vr*dt; phr3(i)=pi/2; % thang dung wr3(i)=0; end; xr4(1)=x4; yr4(1)=y4; phr4(1)=pi/2; wr4(1)=-Vr/Rr; n4=round(pi*Rr/(2*Vr*dt)); for i=2:n4 if i==2 i34=0; end; xr4(i)=x4+Rr*(1-cos(Vr*i34*dt/Rr)); yr4(i)=y4+Rr*sin(Vr*i34*dt/Rr); phr4(i)=pi/2-Vr*i34*dt/Rr; wr4(i)=-Vr/Rr; %cong phai i34=i34+1; end; xr5(1)=x5; yr5(1)=y5; phr5(1)=0; wr5(1)=0; %n5=400; n5=round(Lr/(Vr*dt)); for i=2:n5 xr5(i)=xr5(i-1)+Vr*dt; yr5(i)=yr5(i-1); %ngang phr5(i)=0; wr5(i)=0; end; plot([xr1,xr2,xr3,xr4,xr5],[yr1,yr2,yr3,yr4,yr5],'k-'); hold on plot(x1,y1,'k*',x2,y2,'k*',x3,y3,'k*',x4,y4,'k*',x5,y5,' k*',x6,y6,'k*');axis equal title('the reference trajectory of the AGV'); xlabel('X coordinate (m)'); ylabel('Y coordinate (m)'); xr=[xr1,xr2,xr3,xr4,xr5]; yr=[yr1,yr2,yr3,yr4,yr5]; phr=[phr1,phr2,phr3,phr4,phr5]; wr=[wr1,wr2,wr3,wr4,wr5]; 63 n=n1+n2+n3+n4+n5; dwr(1)=0; for i=2:n dwr(i)=(wr(i)-wr(i-1))/dt; end; vr=Vr; %**************************************** % Controller Parameters %**************************************** Q1=12;Q2=15; % kiem tra thay cung vay P1=1.5;P2=1.1; % kiem tra thay cung vay f1=0.5;f2=0.5; % gioi han cua nhieu Teta=0.3; C=[7.5 -8.2 0.27;35 -2.3 3.1]; tmax=244.8; %duong cu dai qua dt=0.01; n=round(tmax/dt); %n=12000; //day du la 24480 %**************************************** dxr(1)=0; % van toc d2xr(1)=0; % gia toc dyr(1)=0; % van toc d2yr(1)=0; % gia toc vr=0.05; % van toc dai %**************************************** for i=2:n dxr(i)=(xr(i)-xr(i-1))/dt; % dao ham bac cua ref de tinh o duoi d2xr(i)=(dxr(i)-dxr(i-1))/dt; % dao ham bac dyr(i)=(yr(i)-yr(i-1))/dt; % dao ham bac d2yr(i)=(dyr(i)-dyr(i-1))/dt; % dao ham bac end %***************************************************** %Main Program %***************************************************** %WMR Parameters h=0.0; %Distance from tracking point P to M [m] b=0.39; %Distance from Center to Wheel [m] d=0.45; %Distance from center of mass and center of geometry [m] r=0.16; %Radius of Wheel [m] d=0.1 cung vay ah mc=32.67; %Mass of only body [kg] mw=2.75; %Mass of wheel [kg] Jc=17.85; %Moment of inertia of the body [kgm2] Jw=0.0135; %Moment of inertia of wheels about their axes [kgm2] Jm=0.0068; %Moment of inertia of wheels about a diametter [kgm2] 64 m=mc+2*mw; %Mass of total WMR [kg] J=mc*d^2+2*mw*b^2+Jc+2*Jm; %Moment of inertia of total WMR [kgm2] t11=r*m+(2/r)*Jw; t12=(r/b)*J+(2*b/r)*Jw; t2=(r/b)*mc*d; %Initial Values=========================== %(1) For tracking Point xw(1)=0.22; %0.25;0.4 yw(1)=0.35; %W la diem dat camera P phi_w(1)=30*pi/180; %5s vw(1)=0; %van toc dai cua point P ww(1)=0; %van toc goc cua point P %(2) For point M of WMR phi(1)=phi_w(1); x(1)=xw(1)-h*cos(phi(1)); y(1)=yw(1)-h*sin(phi(1)); dx(1)=0; dy(1)=0; v(1)=0; %van toc dai w(1)=0; %van toc goc dv(1)=0; %dao ham luc dw(1)=0; %dao ham luc % lan dau h giam nho xuong cua cua dau dau AGV AGV cua cua luc luc van van dau dau toc dai toc goc %(3) Initial Error of Tracking (11) E0=[cos(phi(1)) sin(phi(1)) 0; -sin(phi(1)) cos(phi(1)) 0; 0 1]*[xr(1)-xw(1);yr(1)-yw(1);phr(1)-phi_w(1)]; e1(1)=E0(1); e2(1)=E0(2); % cac sai so luc dau e3(1)=E0(3); % giong may han E=E0; de1(1)=0; % dao ham cua sai so de2(1)=0; % dao ham luc dau bang de3(1)=0; dE=[de1(1);de2(1);de3(1)]; %(4)Initial Wheel Dynamics vwl(1)=0; %Velocity of Left Wheel vwr(1)=0; %Velocity of Right Wheel l=0.3;%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% P1=13;P2=15;Q1=15;Q2=22;C1=11;C2=3.5;C3=20;Kv=[0.3 0;0 0.6]; z=[v(1);w(1)]; % vecto trang thai //van toc vd(1)= l*(wr(1)+C3*e3(1))+vr*cos(e3(1))+C1*e1(1); wd(1)= wr(1)+C3*e3(1); dvd(1)= l*(dwr(1)+C3*de3(1))+vr*de3(1)*cos(e3(1))+C1*de1(1); %0 dwd(1)= dwr(1)+C3*de3(1); %0 zd=[vd(1);wd(1)]; z_tilde=zd-z; %ev 65 CE=C*E; S1(1)=z_tilde(1)+CE(1); %(13) S2(1)=z_tilde(2)+CE(2); u1(1)=Q1*S1(1)+P1*sign(S1(1))+Kv1*CE(1); %(19) u2(1)=Q2*S2(1)+P2*sign(S2(1))+Kv2*CE(2); %matrix H H=[t11 0; t12]; U=[u1(1); u2(1)]; %initial of momen Tw=H*U; tr=inv([1 1; -1])*Tw; trw(1)=tr(1); tlw(1)=tr(2); %Main Loop%%%%%%%%%%%%%%%%%%%%%%%%%%% for i=2:n %i1=i ; t(i)=(i-1)*dt; Dist(i)=rand(1)-0.5; %State dynamics dv(i)=(t2*b*w(i-1)*w(i-1)+trw(i-1)+tlw(i-1))/(t11); dw(i)=(-t2*w(i-1)*v(i-1)+trw(i-1)-tlw(i-1))+w(i-1)/(t12); v(i)=(dt/t11)*(t2*w(i-1)*w(i-1)+trw(i-1)+tlw(i-1))+v(i-1); w(i)=(dt/t12)*(-t2*w(i-1)*v(i-1)+trw(i-1)-tlw(i-1))+w(i1); %dw(i)=(w(i)-w(i-1))/dt; % dao ham cua w % dao ham cua sai so (12) dE=([-1;0;0]*v(i)+[e2(i-1);-e1(i-1)-h;1]*w(i)+[vr*cos(e3(i-1));vr*sin(e3(i-1));wr(i)]); de1(i)=dE(1); de2(i)=dE(2); % lap 2000 lan de3(i)=dE(3); E=dE*dt+E0; % gia tri moi cua sai so lap lai e1(i)=E(1); e2(i)=E(2); e3(i)=E(3); E0=E; % gan gia tri cu %tracking point dynamics P phi(i)=phr(i)-e3(i); Ae=[cos(phi(i)) -h*sin(phi(i));sin(phi(i)) h*cos(phi(i)); 1]; % giong ben dXW=Ae*[v(i);w(i)]; % dao ham tai diem han W cach tam quay doan L xw(i)=dXW(1)*dt+xw(i-1); yw(i)=dXW(2)*dt+yw(i-1); %vi tri tai diem tracking P o vong lap thu i phi_w(i)=dXW(3)*dt+phi_w(i-1); 66 vw(i)=v(i); ww(i)=w(i); % v,w da tinh o tren % van toc goc cua point P z=[v(i);w(i)]; % chon zw moi dung chu %z=[vw(i);ww(i)]; %van toc phu tro (32) vd(i)= l*(wr(i)+C3*e3(i))+vr*cos(e3(i))+C1*e1(i); wd(i)= wr(i)+C3*e3(i); dvd(i)= l*(dwr(i)+C3*de3(i))+vr*de3(i)*cos(e3(i))+C1*de1(i); dwd(i)= dwr(i)+C3*de3(i); %0 zd=[vd(i);wd(i)]; z_tilde=zd-z; CE=C*E; %mat phang truot S1(i)=z_tilde(1)+Kv1*CE(1); %(21) S2(i)=z_tilde(2)+Kv2*CE(2); %0 u1(i)=Q1*S1(i)+P1*sign(S1(i))+CE(1); u2(i)=Q2*S2(i)+P2*sign(S2(i))+CE(2); %tinh toan Tw=H*[dvd(i); dwd(i)]+t2*[0 -b*w(i); w(i) 0]*[v(i); w(i)]+H*[u1(i); u2(i)]; tr=inv([1 1; -1])*Tw; % hai phuong trinh thi duoc trw(i)=tr(1); tlw(i)=tr(2); %wheel dynamics [rpm] vlw(i)=(1/r)*(v(i)-b*w(i))*(180/pi); vrw(i)=(1/r)*(v(i)+b*w(i))*(180/pi); %robot point M x(i)=xw(i)-h*cos(phi(i)); y(i)=yw(i)-h*sin(phi(i)); end; %*************************************************** %Plot Simulation Results %*************************************************** figure;plot(t,e1*1000,'r-',t,e2*1000,'k-',t,e3*180/pi,'b');xlabel('Thoi gian (s)');ylabel('Sai so)'); %figure;plot(t,e1*1000,'k-',t,e2*1000,'k.',t,e3*180/pi,'k ');xlabel('Time (s)');ylabel('Tracking error vector e1(mm), e2(mm), e3 (deg)'); legend('Sai so e1 (mm)','Sai so e2 (mm)','Sai so e3 (mm)',1); %figure; plot(t,vlw,'k-',t,vrw,'k ');xlabel('Time (s)');ylabel('Wheel Velocity (rpm)'); figure; plot(t,vlw,'r-',t,vrw,'b-');xlabel('Thoi gian (s)');ylabel('Van toc banh xe phai va trai'); 67 legend('vlw','vrw',1); figure; plot(xr,yr,'k-',x,y,'r-');xlabel('Truc X');ylabel('Truc Y'); %title('xr,yr,x,y and xw,yw'); %figure; plot(xr,yr,'k-',x,y,'k ');xlabel('X coordinate (m)');ylabel('Y coordinate (m)'); %title('xr,yr,x,y and xw,yw'); figure; plot(t,v*1000,'k-');xlabel('Thoi gian (s)');ylabel('Van toc tuyen tinh v');%title('Linear velocity of AGV (mm/s)'); figure; plot(t,w,'k-');xlabel('Thoi gian (s)');ylabel('Van toc goc');%title('Angular velocity of AGV (rad/s)'); figure; plot(t,S1*1000,'k-');xlabel('Thoi gian (s)');ylabel('Mat truot S1 (mm/s)'); figure; plot(t,S2*180/pi,'k-');xlabel('Thoi gian (s)');ylabel('Mat truot S2 (deg/s)'); figure; plot(t,u1*1000,'r-',t,u2*1000,'b-');xlabel('Thoi gian (s)');ylabel('TIn hieu dieu khien'); legend('u1','u2',1); figure; plot(t,u1*1000,'k-');xlabel('Thoi gian (s)');ylabel('Tin hieu dieu khien u1'); legend('u1',1); figure; plot(t,u2*180/pi,'k-');xlabel('Thoi gian (s)');ylabel('Tin hieu dieu khien u2'); legend('u2',1); figure; plot(t,trw,'r-',t,tlw,'b-');xlabel('Thoi gian (s)');ylabel('Momen banh phai va banh trai'); %figure; plot(t,trw,'k-',t,tlw,'k ');xlabel('Time (s)');ylabel('Torque of wheels (N.mm)'); legend('trw','tlw',1); figure; plot(t,tlw,'k-');xlabel('Thoi gian (s)');ylabel('Momen banh trai'); legend('tlw (N.mm)',1); figure; plot(t,trw,'k-');xlabel('Thoi gian (s)');ylabel('Momen banh phai'); legend('trw (N.mm)',1); 2.2 điều khiển đa bậc clear all q_ini = [0 pi/2 0 0]; Xr_c Yr_c Xr_c1 Yr_c1 gama = = = = = 5; 0.5;% 5/1.4142; 0;% pi/4; opt=odeset('RelTol',1e-4); [t, q] = ode45(@file1, [5 60], q_ini, opt); path1 = t; 68 path2 = 5*sin(t); plot(path2,path1,'b-'); hold on axis equal axis([-10 65 -5 65]) grid on path = size(t); Na = path(1,1); for j = 1:1:Na, % change to M for sinewave, N for circle R1 = 5/1.414; R2 = 5/1.414; Xc = q(j,1); Yc = q(j,2); fai = q(j,3); robot_width = 2.5;% kích c? c?a robot Xr = Xc+Xr_c*cos(fai)-Yr_c*sin(fai); Yr = Yc+Xr_c*sin(fai)+Yr_c*cos(fai); Xr1 = Xc+Xr_c1*cos(fai+gama)-Yr_c1*sin(fai+gama); Yr1 = Yc+Xr_c1*sin(fai+gama)+Yr_c1*cos(fai+gama); theta_r_dot = q(j,6); theta_l_dot = q(j,7); sinfai(j) = sin(fai); cosfai(j) = cos(fai); linearvelocity(j) = (theta_l_dot+theta_r_dot)*0.75/2; theta_r_dotre(j) = theta_r_dot; theta_l_dotre(j) = theta_l_dot; theta_r(j) = q(j,4); theta_l(j) = q(j,5); Xcre(j) = q(j,1); Ycre(j) = q(j,2); faire(j) = fai; [Robot] = Robotplot(Xc, Yc, fai, robot_width); Robot1x = [Robot(1,1) Robot(1,3)]; Robot1y = [Robot(1,2) Robot(1,4)]; Robot2x = [Robot(1,3) Robot(1,5)]; Robot2y = [Robot(1,4) Robot(1,6)]; Robot3x = [Robot(1,5) Robot(1,7)]; Robot3y = [Robot(1,6) Robot(1,8)]; Robot4x = [Robot(1,7) Robot(1,1)]; Robot4y = [Robot(1,8) Robot(1,2)]; Robotlinkx Robotlinky = [Xr Xc]; = [Yr Yc]; 69 Robotlinkx1 = [Xr1 Xc]; Robotlinky1 = [Yr1 Yc]; Robotlinkx2 = [Xr1 Xr]; Robotlinky2 = [Yr1 Yr]; RobotBox(1,:) = [Robot1x Robot2x Robot3x Robot4x]; RobotBox(2,:) = [Robot1y Robot2y Robot3y Robot4y]; XrYr XcYc Xr1Yr1 XrYrtoXcYc Xr1Yr1toXcYc XrYrtoXr1Yr1 = = = = = = [Xr Yr]; [Xc Yc]; [Xr1 Yr1]; distance(XrYr,XcYc); distance(Xr1Yr1,XcYc); distance(XrYr,Xr1Yr1); MMcos = (-(XrYrtoXcYc*XrYrtoXcYc-Xr1Yr1toXcYc*Xr1Yr1toXcYcXrYrtoXr1Yr1*XrYrtoXr1Yr1)/(2*XrYrtoXr1Yr1*Xr1Yr1toXcYc)); MM(j) = sqrt(1 - MMcos^2); if j==1, h1 =plot(Robot1x, Robot1y,'b-','LineWidth',4);% bánh xe bên trái robot h2 =plot(Robot2x, Robot2y,'r-','LineWidth',1);%thành phía tr??c c?a robot h3 =plot(Robot3x, Robot3y,'b-','LineWidth',4);% bánh xe bên ph?i robot h4 =plot(Robot4x, Robot4y,'r-','LineWidth',1);%thành phía sau c?a robot h5 =plot(Robotlinkx, Robotlinky,'ro','LineWidth',1);% ??u dò tay máy h6 =plot(Robotlinkx1, Robotlinky1,'r-','LineWidth',2);% tay máy h7 =plot(Robotlinkx2, Robotlinky2,'r-','LineWidth',2); set(h1,'EraseMode','xor'); set(h2,'EraseMode','xor'); set(h3,'EraseMode','xor'); set(h4,'EraseMode','xor'); set(h5,'EraseMode','xor'); else set(h1,'XData', set(h2,'XData', set(h3,'XData', set(h4,'XData', set(h5,'XData', set(h6,'XData', set(h7,'XData', Robot1x,'YData',Robot1y) Robot2x,'YData',Robot2y) Robot3x,'YData',Robot3y) Robot4x,'YData',Robot4y) Robotlinkx,'YData',Robotlinky) Robotlinkx1,'YData',Robotlinky1) Robotlinkx2,'YData',Robotlinky2) 70 end pause(0.01) end 71 S K L 0 ... robot di động có cánh tay máy (robot hàn) Mơ hình hình học robot mơ hình động học, mơ hình động lực học robot Thiết kế điều khiển phi tuyến cho robot di động Thiết kế robot hàn di động có cánh tay. .. giải vấn đề dị theo quỹ đạo thực đƣợc robot di động Ngo [3] đề xuất điều khiển trƣợt thích nghi cho loại robot hàn trên, điều khiển trên, robot di động đƣợc khảo sát dựa vào mơ hình động học với... luận văn đƣa giải thuật điều khiển mà kết hợp điều khiển trƣợt sở điều khiển động học động lực học với giải thuật điều khiển trƣợt tích phân cho robot di động hai bánh bám theo chiến lƣợc mong muốn