Nghiên cứu thiết kế bộ điều khiển cho tay máy hàn di động với mô hình động lực học

1 Đại Học Quốc Gia Tp Hồ Chí Minh TRƯỜNG ĐẠI HỌC BÁCH KHOA LÊ KHẮC ĐẠI NGHIÊN CỨU THIẾT KẾ BỘ ĐIỀU KHIỂN CHO TAY MÁY HÀN DI ĐỘNG VỚI MƠ HÌNH ĐỘNG LỰC HỌC Chun ngành : CHẾ TẠO MÁY LUẬN VĂN THẠC SĨ TP HỒ CHÍ MINH, tháng 01 năm 2008 HVTH : Lê Khắc Đ i GVHD : TS Phan Tấn Tù ng LỜI CẢM ƠN Em xin chân nh m n sựhươ ùng dẫn vàgiúp đ õ tận tình củ a Thầy TS Phan Tấn Tùng , ngừ i đ ãđ ị nh hươ ùng vàhỗ trơ ïđ ểem cóthểhoà n nh luận văn nà y Em xin gơ û i lơ ø i cám n chân nh đ ến thầy cô môn Chế Tạ o Máy vàcác thầy cô đ ã tham gia quátrình giả ng y chuyên ngà nh , ngươ ø i đ ãđ ặt móng vững cho sựnghiên cứu củ a em đ ề tà i Xin chân nh m n sựủ ng hộ , giúp đ õ củ a Anh , Chị lơ ùp CH2005 , ngươ ø i bạ n đ ãhết lò ng đ ộng viên đ ểcóthểhoà n nh đ ề tà i nà y Vàcuối cù ng xin m n Cha , Mẹ vàcác ngươ ø i thân gia đ ình đ ã ûbên cạ nh đ ộng viên , hậu thuẫn đ ể cóthể hoà n nh bả n luận văn tốt nghiệp nà y Họ c viên thự c Lê Khắc Đại HVTH : Lê Khắc Đ i GVHD : TS Phan Tấn Tù ng TÓM TẮT LUẬN VĂN THẠC SĨ Luận văn tập trung nghiên cứu tay máy di đ ộng bao gồm tay máy ba khâu đ ươ ï c gắn xe robot hai bánh dẫn đ ộng Các mô hình đ ộng họ c , đ ộng lự c họ c đ ã đ ươ ï c xây dự ng vàđ ã đ ề phươ ng pháp đ iều khiển cho tay máy di đ ộng nà y Trong nghiên cứu trươ ùc đ ây , tay máy di đ ộng thươ ø ng đ ươ ï c khả o sát hệ thống vàcác mô hình đ ộng họ c , đ ộng lự c họ c củ a nóđ ươ ï c xây dự ng cho n hệ thống Các mô hình nà y phức tạ p việc tí nh toán Vì , luận văn nà y phươ ng pháp đ iều khiển không tập trung đ ã đ ươ ï c xây dự ng đ ể đ iều khiển tay máy di đ ộng Tay máy di đ ộng đ ươ ï c khả o sát hệ thống gồm hai hệ : tay máy vàmột xe robot ; từđ óviệc xây dự ng mô hình đ ộng họ c , đ ộng lự c họ c cho hệcon nà y trơ ûnên dễ dà ng hơ n Thuật toán đ iều khiển nà y dự a quy luật đ iều khiển kết hơ ï p phươ ng pháp đ iều khiển theo đ ộng họ c vơ ùi phươ ng pháp đ iều khiển theo moment tí nh toán Vơ ùi phươ ng pháp nà y thuật toán đ iều khiển đ ươ ï c đ n giả n hóa đ ưa đ ươ ï c c củ a tay máy di đ ộng o đ iều khiển hệ thống đ i lươ ï ng đ ộng lự c họ Các kết quảmô phỏ ng máy tí nh chúng minh tí nh đ úng đ ắn củ a phươ ng pháp đ iều khiển đ ươ ï c đ ề nghị HVTH : Lê Khắc Đ i GVHD : TS Phan Tấn Tù ng ABSTRACT This thesis focuses on studying a mobile manipulator , which consists of a three-linked manipulator and two-wheeled mobile platform The mobile manipulator is investigated with two aspects : kinematics and dynamics , furthermore a control method is studied In previous works , the mobile manipulator is usually considered as single system , its kinematic and dynamic models are established for the whole system These models are very complicated for computation So , in this thesis , a decentralized motion control for the mobile manipulator is proposed to achieve effective and practical control The mobile manipulator is considered as two subsystems such as a manipulator and a mobile platform , and its kinematic and dynamic models are established for each subsystem very simple This control algorithm is based on the concept of kinematic into dynamic method , so it makes easy the computation in applying The effectiveness of the proposed controller is shown through simulation results HVTH : Lê Khắc Đ i GVHD : TS Phan Tấn Tù ng MỤC LỤC Lời cảm ơn Tóm tắt luận văn thạc só Muïc luïc Danh mục hình vẽ Danh mục bảng biểu Chương : TỔNG QUAN 10 1.1 Tình hình nghiên cứu 10 1.2 Mụ c tiêu củ a đ ề tà i 12 Chương : NGHIÊN CỨU MÔ HÌNH ĐỘNG HỌC – ĐỘNG LỰC HỌC 14 2.1 Giơ ùi thiệu 14 2.2 Mô hình tay máy di đ ộng 14 2.3 Mô hình đ ộng họ c củ a tay máy di đ ộng 15 2.3.1 Mô hình đ ộng họ c củ a xe robot 15 2.3.2 Mô hình đ ộng họ c củ a tay máy 17 2.4 Mô hình đ ộng lự c họ c củ a tay máy di đ ộng 20 2.4.1 Moâ hình đ ộng lự c họ c củ a xe robot 20 2.4.2 Mô hình đ ộng lự c họ c củ a tay máy 28 Chương : NGHIÊN CỨU THIẾT KẾ BỘ ĐIỀU KHIỂN 44 3.1 Đ ị nh nghó a sai soá 45 3.2 Thiết kế luật đ iều khiển cho tay máy di đ ộng mô hình đ ộng họ c 47 HVTH : Lê Khắc Đ i GVHD : TS Phan Tấn Tù ng 3.2.1 Thiết kế luật đ iều khiển cho tay máy mô hình đ ộng họ c 47 3.2.2 Thieát kế luật đ iều khiển cho xe robot mô hình đ ộng họ c 50 3.3 Thiết kế bộđ iều khiển cho tay máy di đ ộng theo mô hình đ ộng lự c họ c 54 3.3.1 Thiết kế bộđ iều khiển cho tay máy 55 3.3.2 Thiết kế bộđ iều khiển cho xe robot 58 Chương : CÁC KẾT QUẢ MÔ PHỎNG 63 Chương : KẾT LUẬN 74 5.1 Kết luận 74 5.2 Hươ ùng phát triển đ ề tà i 75 TÀI LIỆU THAM KHẢO 76 PHUÏ LUÏC 79 HVTH : Lê Khắc Đ i GVHD : TS Phan Tấn Tù ng DANH MỤC HÌNH VẼ Hình 1.1: Robot hà n đ ươ ø ng thẳng córay dẫn Hình 1.2 : Các loạ i robot hà n di đ ộng Hình 2.1 : Tay máy hà n di đ ộng khâu , khơ ùp Hình 2.2 : Sơ đ đ ộng họ c tay máy di đ ộng Hình 2.3 : Khối tâm củ a tay máy vàxe robot Hình 3.1 : Sơ đồ xác định sai số (e1 ,e2 ,e3) Hình 3.2 : Sơ đồ xác định sai số (e4 ,e5 ,e6) Hình 3.3 Sơ đ bộđ iều khiển gia tốc phả n hồi phi tuyến Hình 3.4 Sơ đ bộđ iều khiển gia tốc phả n hồi củ a xe robot Hình 3.5 Sơ đ khối phươ ng pháp đ iều khiển đ ãxây dự ng Hình 4.1 Tay máy hà n di đ ộng chạ y bám theo đ ươ ø ng hà n tham chiếu Hình 4.2 Quỹđ o củ a đ ầu hà n lúc ban đ ầu o lúc ban đ ầu Hình 4.3 Sai số vị trí e1 , e2 , e3 Hình 4.4 Sai số vị trí e1 , e2 , e3 Hình 4.5 Sai số e4 , e5 , e6 o lúc ban đ ầu Hình 4.6 Sai số e4 , e5 , e6 Hình 4.7 Sai số vận tốc khâu tay máy e7 , e8 , e9 lúc ban đ ầu Hình 4.8 Sai số vận tốc khâu tay máy e7 , e8 , e9 Hình 4.9 Sai số vận tốc xe robot e10 , e11 lúc ban đ ầu HVTH : Lê Khắc Đ i GVHD : TS Phan Tấn Tù ng Hình 4.10 Sai số vận tốc xe robot e10 , e11 Hình 4.11 Góc quay khâu củ a tay máy θ1 ,θ ,θ3 Hình 4.12.Vận tốc góc khâu tay máy o lúc đ ầu Hình 4.13.Vận tốc củ a đ iểm P - tâm xe robot VP , ωP Hình 4.14.Vận tốc củ a hai bánh xe ωR , ωL Hình 4.15.Moment củ a khâu tay máy o lúc đ ầu τ ,τ ,τ Hình 4.16.Moment củ a bánh xe o lúc đ ầu τ R ,τ L HVTH : Lê Khắc Đ i GVHD : TS Phan Tấn Tù ng DANH MỤC BẢNG BIỂU Bảng 4.1 Giátrị thông số củ a hệ thống vàcác giátrị ban đ ầu củ a hệ thống mô phỏ ng HVTH : Lê Khắc Đ i GVHD : TS Phan Tấn Tù ng 10 Chương : TỔNG QUAN 1.1 Tình hình nghiên cứu : Ngà y , robot đ ươ ï c sửdụ ng rộng rãi ngà nh công nghiệp , mụ c đ í ch củ a việc sử dụ ng robot làđ ể thay cho ngươ ø i tránh m việc môi trươ ø ng ô nhiễm , đ ộc hạ i ả nh hươ û ng tơ ùi sức khỏ e ngươ ø i Robot di đ ộng đ ươ c sử dụ ng ngà nh khí chế tạ o , lắp ráp , khoáng sả n , … vàhà n làmột công nghệ quan trọ ng thiếu ngà nh công nghiệp , đ ặc biệt làtrong công nghiệp đ óng tà u , việc tựđ ộng hoáquátrình hà n làhết sức cần thiết Việc ứng dụ ng robot hà n nhằm thự c quátrình thao tác hơ ï p lýbằng hơ n ngươ ø i thơ ïlà nh nghề cách ổn đ ị nh suốt quátrình m việc đ ể nâng cao chất lươ ï ng mối hà n , giả m giáthà nh sả n phẩm vìgiả m đ ươ ï c đ kểchi phí cho ngươ ø i sửdụ ng lao đ ộng , i thiện đ iều kiện m việc vàtăng suất Đ ể giả i vấn đ ề , hệ thống robot hà n đ ã đ ươ ï c nghiên cứu phát triển robot di đ ộng Đ ãcórất nhiều nghiên cứu phươ ng pháp đ iều khiển m việc theo quỹđ o Sau đ ây làmột số đ ề tà i nghiên cứu robot hà n tựđ ộng đ ãđ t đ ươ ï c số nh tự u đ ị nh Fierro [11] đ ã xây dự ng quy luật đ iều khiển robot di đ ộng cách kết hơ ï p phươ ng pháp đ iều khiển theo thông số đ ộng họ c vàđ iều khiển theo moment sử dụ ng tí n hiệu phả n hồi (backstepping) , quy luật đ iều khiển nà y đ ự a thuyết ổn đ ị nh Lyapunov HVTH : Lê Khắc Đ i GVHD : TS Phan Tấn Tuø ng 77 [8] Tr H Bui, T L Chung, J H Suh and S B Kim, “Adaptive Control for Tracking Trajectory of a Two Wheeled Welding Mobile Robot with Unknown Parameters” , Proceedings of the International Conference on Control, Automation and Systems, pp 191-196, 2003 [9] Tr H Bui, T T Nguyen, T L Chung and S B 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 [10] X Yun and Y Yamamoto, “Internal Dynamics of a Wheeled Mobile Robot” , Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, Japan, pp 1288-1294, July 1993 [11] R Fierro and F L Lewis, “Control of a Non-holonomic Mobile Robot: Backstepping Kinematics into Dynamics” , Proceedings of the IEEE Conference on Decision and Control, Vol 4, pp 3805-3810, 1995 [12] B O Kam, Y B Jeon, and S B Kim, “Motion Control of Two-Wheeled Welding Mobile Robot with Seam Tracking Sensor” , Proceedings of the IEEE International Conference Symposium on Industrial Electronics, Vol 2, pp 851-856, June 2001 [13] W Dong, Y Xu and Q Wang, “On Tracking Control of Mobile Manipulators” , Proceedings of the IEEE International Conference on Robotics and Automation, Vol 4, pp 3455-3460, 2000 [14] M P Cheng and C C Tsai, “Dynamic Modeling and Tracking Control of a Non-holonomic Wheeled Mobile Manipulator with Two Robotic Arms” , HVTH : Lê Khắc Đ i GVHD : TS Phan Tấn Tù ng 78 Proceedings of the IEEE Conference on Decision and Control, Vol 3, pp 29322937, 2003 [15] Trần Thiên Phúc , “ Control of Mobile Manipulator for tracking Smooth 3D Curved Welding Trajectory “ , Ph.D Thesis , Department of Mechatronics Engineering , Pukyong national University , Korea , 2005 [16] Y Yamamoto , “ Control and Coordination of Locomotion and Manipulation of a Wheeled Mobile Manipulator “ , Ph.D Thesis , Department of Computer and Information Science , University of Pennsylvania , USA , 1994 [17] Lê Hoà i Quốc , “Robot công nghiệp “ , Nhàxuất bả n Đ i Họ c Quốc Gia TpHCM , 2005 HVTH : Lê Khắc Đ i GVHD : TS Phan Tấn Tù ng 79 PHỤ LỤC % De tai luan van tot nghiep cao hoc % Le Khac Dai - Co Khi Che Tao May % Chuong trinh tinh toan mo phong bo dieu khien tay may di dong close all; clear all; disp('Please wait'); load path4 xr yr phr wr; dt=0.1; t(1)=0; n=length(xr); %n=100; % === at beginning ================= %n=100; % n=100 => t=10s % ================================== vr=0.0075; r=0.025; b=0.105; m=7.9; mc=7.5; d=0.087; mw=0.2; Ic=0.08; Iw=0.0008; Im=0.0004; L1=0.2; L2=0.2; L3=0.2; m1=2; m2=2; m3=2; I1=0.03; I2=0.03; I3=0.03; k1=1.6; k2=1.9; k3=1.8; HVTH : Lê Khắc Đ i I=0.0852; GVHD : TS Phan Tấn Tù ng 80 k4=1.5; k5=7.2; k6=1.6; xe(1)=0.275; ye(1)=0.395; phe(1)=-15*pi/180; ve(1)=0; we(1)=0; dve(1)=0; dwe(1)=0; T1(1)=pi/4; T2(1)=pi/2; T3(1)=-pi/4; dT1(1)=0; dT2(1)=0; ddT1(1)=0; ddT2(1)=0; dT3(1)=0; ddT3(1)=0; L=0.2*(1+sqrt(2)); php(1)=phe(1); xp(1)=xe(1)+L*sin(php(1)); vp(1)=0; wp(1)=0; dvp(1)=0; dwp(1)=0; k1p=2; k2p=28500; yp(1)=ye(1)-L*cos(php(1)); k3p=1.5; k4p=1.3; k5p=6; h=0.226; beta=76*pi/180; e1p(1)=L1*cos(T1(1))+L2*cos(T1(1)+T2(1))+L3*cos(T1(1)+T2(1)+T3(1)); e2p(1)=L1*sin(T1(1))+L2*sin(T1(1)+T2(1))+L3*sin(T1(1)+T2(1)+T3(1))-L; e3p(1)=T1(1)+T2(1)+T3(1)-pi/2; e1(1)=(xr(1)-xe(1))*cos(phe(1))+(yr(1)-ye(1))*sin(phe(1)); e2(1)=-(xr(1)-xe(1))*sin(phe(1))+(yr(1)-ye(1))*cos(phe(1)); e3(1)=phr(1)-phe(1); de1(1)=vr*cos(phr(1)-phe(1))-ve(1)-we(1)*((xr(1)-xe(1))*sin(phe(1))-(yr(1)ye(1))*cos(phe(1))); HVTH : Lê Khắc Đ i GVHD : TS Phan Tấn Tuø ng 81 de2(1)=vr*sin(phr(1)-phe(1))-we(1)*((xr(1)-xe(1))*cos(phe(1))+(yr(1)ye(1))*sin(phe(1))); de3(1)=wr(1)-we(1); J0=[xp(1) yp(1)]; J1=[J0(1)+L1*cos(T1(1)+php(1)) J0(2)+L1*sin(T1(1)+php(1))]; J2=[J0(1)+L1*cos(T1(1)+php(1))+L2*cos(T1(1)+php(1)+T2(1)) J0(2)+L1*sin(T1(1)+php(1))+L2*sin(T1(1)+php(1)+T2(1))]; J3=[J0(1)+L1*cos(T1(1)+php(1))+L2*cos(T1(1)+php(1)+T2(1))+L3*cos(T1(1)+php (1)+T2(1)+T3(1)) J0(2)+L1*sin(T1(1)+php(1))+L2*sin(T1(1)+php(1)+T2(1))+L3*sin(T1(1)+php(1)+T 2(1)+T3(1))]; line([J0(1),J1(1),J2(1),J3(1)],[J0(2),J1(2),J2(2),J3(2)],'Color','m'); Cp1=[xp(1)+0.1768*cos(php(1)-pi/4) yp(1)+0.1768*sin(php(1)-pi/4)]; Cp2=[xp(1)+0.1768*cos(php(1)+pi/4) yp(1)+0.1768*sin(php(1)+pi/4)]; Cp3=[xp(1)+0.1768*cos(php(1)+(pi-pi/4)) yp(1)+0.1768*sin(php(1)+(pi-pi/4))]; Cp4=[xp(1)+0.1768*cos(php(1)-(pi-pi/4)) yp(1)+0.1768*sin(php(1)-(pi-pi/4))]; line([Cp1(1),Cp2(1),Cp3(1),Cp4(1),Cp1(1)],[Cp1(2),Cp2(2),Cp3(2),Cp4(2),Cp1(2)],' Color','B'); %================ main program================= for i=2:n disp(i); t(i)=(i-1)*dt; HVTH : Lê Khắc Đ i GVHD : TS Phan Tấn Tù ng 82 %================ TAY MAY ================== dT1d(i)=(vr*sin(T3(i-1)+e3(i-1))-vp(i-1)*cos(T1(i-1)+T2(i-1))+(e2(i-1)*k2e1(i-1)*we(i-1))*cos(T3(i-1))+(e1(i-1)*k1+e2(i-1)*we(i-1)+L3*(wr(i)+e3(i1)*(k3+we(i-1))))*sin(T3(i-1)))/(L1*sin(T2(i-1)))-wp(i-1); dT2d(i)=(-vr*(L1*sin(T2(i-1)+T3(i-1)+e3(i-1))+L2*sin(T3(i-1)+e3(i-1)))+vp(i1)*(L1*cos(T1(i-1))+L2*cos(T1(i-1)+T2(i-1)))+(we(i-1)*e1(i-1)-e2(i1)*k2)*(L1*cos(T2(i-1)+T3(i-1))+L2*cos(T3(i-1)))-(L1*sin(T2(i-1)+T3(i1))+L2*sin(T3(i-1)))*(L3*e3(i-1)*(k3+we(i-1))+L3*wr(i)+e1(i-1)*k1+e2(i1)*we(i-1)))/(L1*L2*sin(T2(i-1))); dT3d(i)=(vr*sin(T2(i-1)+T3(i-1)+e3(i-1))-vp(i-1)*cos(T1(i-1))+(e2(i-1)*we(i1)+ k1*e1(i-1)+L3*(wr(i)+e3(i-1)*(k3+we(i-1))))* sin(T2(i-1)+T3(i-1))+(e2(i1)*k2-e1(i-1)*we(i-1)) *cos(T2(i-1)+T3(i-1)))/(L2*sin(T2(i-1)))+( wr(i)+e3(i1)*(k3+we(i-1))); e4(i-1)=dT1(i-1)-dT1d(i); e5(i-1)=dT2(i-1)-dT2d(i); e6(i-1)=dT3(i-1)-dT3d(i); ddT1d(i)=(vr*(dT3(i-1)+de3(i-1))*cos(T3(i-1)+e3(i-1))-dvp(i-1)*cos(T1(i1)+T2(i-1))+vp(i-1)*(dT1(i-1)+dT2(i-1))*sin(T1(i-1)+T2(i-1))+(de2(i-1)*k2-de1(i1)*we(i-1)-e1(i-1)*dwe(i-1))*cos(T3(i-1))-(e2(i-1)*k2-e1(i-1)*we(i-1))*dT3(i1)*sin(T3(i-1))+(de1(i-1)*k1+de2(i-1)*we(i-1)+e2(i-1)*dwe(i-1)+L3*(de3(i1)*(k3+we(i-1))+e3(i-1)*dwe(i-1)))*sin(T3(i-1))+(e1(i-1)*k1+e2(i-1)*we(i1)+L3*(wr(i-1)+e3(i-1)*(k3+we(i-1))))*dT3(i-1)*cos(T3(i-1)))/(L1*sin(T2(i-1)))(vr*sin(T3(i-1)+e3(i-1))-vp(i-1)*cos(T1(i-1)+T2(i-1))+(e2(i-1)*k2-e1(i-1)*we(i- HVTH : Lê Khắc Đ i GVHD : TS Phan Tấn Tù ng 83 1))*cos(T3(i-1))+(e1(i-1)*k1+e2(i-1)*we(i-1)+L3*(wr(i)+e3(i-1)*(k3+we(i1))))*sin(T3(i-1)))*(dT2(i-1)*cos(T2(i-1)))/(L1*(sin(T2(i-1)))^2)-dwp(i-1); ddT2d(i)=(vr*(L1*(dT2(i-1)+dT3(i-1)+de3(i-1))*cos(T2(i-1)+T3(i-1)+e3(i1))+L2*(dT3(i-1)+de3(i-1))*cos(T3(i-1)+e3(i-1)))+dvp(i-1)*(L1*cos(T1(i1))+L2*cos(T1(i-1)+T2(i-1)))-vp(i-1)*(L1*dT1(i-1)*sin(T1(i-1))+L2*(dT1(i1)+dT2(i-1))*sin(T1(i-1)+T2(i-1)))+(de1(i-1)*we(i-1)+e1(i-1)*dwe(i-1)-de2(i1)*k2)*(L1*cos(T2(i-1)+T3(i-1))+L2*cos(T3(i-1)))-(e1(i-1)*we(i-1)-e2(i1)*k2)*(L1*(dT2(i-1)+dT3(i-1))*sin(T2(i-1)+T3(i-1))+L2*dT3(i-1)*sin(T3(i-1)))(L1*(dT1(i-1)+dT2(i-1))*cos(T1(i-1)+T2(i-1))+L2*dT3(i-1)*cos(T3(i-1)))*(e1(i1)*k1+e2(i-1)*we(i-1)+L3*wr(i-1)+L3*e3(i-1)*(k3+we(i-1)))-(L1*sin(T1(i1)+T2(i-1))+L2*sin(T3(i-1)))*(de1(i-1)*k1+de2(i-1)*we(i-1)+e2(i-1)*dwe(i1)+L3*de3(i-1)*(k3+we(i-1))+L3*e3(i-1)*dwe(i-1)))/(L1*L2*sin(T2(i-1)))-(vr*(L1*sin(T2(i-1)+T3(i-1)+e3(i-1))+L2*sin(T3(i-1)+e3(i-1)))+vp(i1)*(L1*cos(T1(i-1))+L2*cos(T1(i-1)+T2(i-1)))+(we(i-1)*e1(i-1)-e2(i1)*k2)*(L1*cos(T2(i-1)+T3(i-1))+L2*cos(T3(i-1)))-(L1*sin(T2(i-1)+T3(i1))+L2*sin(T3(i-1)))*(L3*e3(i-1)*(k3+we(i-1))+L3*wr(i)+e1(i-1)*k1+e2(i1)*we(i-1)))*(dT2(i-1)*cos(T2(i-1)))/(L1*L2*(sin(T2(i-1)))^2); ddT3d(i)=(vr*(dT2(i-1)+dT3(i-1)+de3(i-1))*cos(T2(i-1)+T3(i-1)+e3(i-1))dvp(i-1)*cos(T1(i-1))+vp(i-1)*dT1(i-1)*sin(T1(i-1))+(de2(i-1)*k2-de1(i-1)*we(i-1)e1(i-1)*dwe(i-1))*cos(T2(i-1)+T3(i-1))-(e2(i-1)*k2-e1(i-1)*we(i-1))*(dT2(i1)+dT3(i-1))*sin(T2(i-1)+T3(i-1))+(de1(i-1)*k1+de2(i-1)*we(i-1)+e2(i-1)*dwe(i1)+L3*(de3(i-1)*(k3+we(i-1))+e3(i-1)*dwe(i-1)))*sin(T2(i-1)+T3(i-1))+(e1(i1)*k1+e2(i-1)*we(i-1)+L3*(wr(i-1)+e3(i-1)*(k3+we(i-1))))*(dT2(i-1)+dT3(i1))*cos(T2(i-1)+T3(i-1)))/(L2*sin(T2(i-1)))-(vr*sin(T2(i-1)+T3(i-1)+e3(i-1))-vp(i- HVTH : Lê Khắc Đ i GVHD : TS Phan Tấn Tù ng 84 1)*cos(T1(i-1))+(e2(i-1)*we(i-1)+ k1*e1(i-1)+L3*(wr(i)+e3(i-1)*(k3+we(i-1))))* sin(T2(i-1)+T3(i-1))+(e2(i-1)*k2-e1(i-1)*we(i-1)) *cos(T2(i-1)+T3(i-1)))*(dT2(i1)*cos(T2(i-1)))/(L2*(sin(T2(i-1)))^2)+(de3(i-1)*(k3+we(i-1))+e3(i-1)*dwe(i-1)); u1(i)=ddT1d(i)-k4*e4(i-1); u2(i)=ddT2d(i)-k5*e5(i-1); u3(i)=ddT3d(i)-k6*e6(i-1); Tau1(i)=(((m1/4)+m2+m3)*(L1^2)+((m2/4)+m3)*(L2^2)+(m3/4)*(L3^2)+I1+I2+I3 +(m2+(2*m3))*L1*L2*cos(T2(i-1))+m3*L1*L3*cos(T2(i-1)+T3(i1)))*u1(i)+(((m2/4)+m3)*(L2^2)+(m3/4)*(L3^2)+I2+I3+((m2/2)+m3)*L1*L2*cos( T2(i-1))+(m3/2)*L1*L3*cos(T2(i-1)+T3(i-1))+m3*L2*L3*cos(T3(i1)))*u2(i)+((m3/4)*L3^2+I3+(m3/2)*L1*L3*cos(T2(i-1)+T3(i1))+(m3/2)*L2*L3*cos(T3(i-1)))*u3(i)-(m2/2+m3)*L1*L2*dT2(i-1)*(2*dT1(i1)+dT2(i-1))*sin(T2(i-1))-(m3/2)*L1*L3*(dT2(i-1)+dT3(i-1))*(2*dT1(i-1)+dT2(i1)+dT3(i-1))*sin(T2(i-1)+T3(i-1))-(m3/2)*L2*L3*dT3(i-1)*(2*dT1(i-1)+2*dT2(i1)+dT3(i-1))*sin(T3(i-1)); Tau2(i)=(((m2/4)+m3)*(L2^2)+(m3/4)*(L3^2)+I2+I3+((m2/2)+m3)*L1*L2*cos(T2( i-1))+(m3/2)*L1*L3*cos(T2(i-1)+T3(i-1))+m3*L2*L3*cos(T3(i1)))*u1(i)+(((m2/4)+m3)*(L2^2)+(m3/4)*(L3^2)+I2+I3+m3*L2*L3*cos(T3(i1)))*u2(i)+((m3/4)*(L3^2)+I3+(m3/2)*L2*L3*cos(T3(i1)))*u3(i)+((m2/2)+m3)*L1*L2*(dT1(i-1)^2)*sin(T2(i-1))+(m3/2)*L1*L3*(dT1(i1)^2)*sin(T2(i-1)+T3(i-1))-(m3/2)*L2*L3*dT3(i-1)*(2*dT1(i-1)+2*dT2(i-1)+dT3(i1))*sin(T3(i-1)); HVTH : Lê Khắc Đ i GVHD : TS Phan Tấn Tuø ng 85 Tau3(i)=((m3/4)*L3^2+I3+(m3/2)*L2*L3*cos(T2(i-1)+T3(i1))+(m3/2)*L2*L3*cos(T3(i-1)))*u1(i)+((m3/4)*L3^2+I3+(m3/2)*L2*L3*cos(T3(i1)))*u2(i)+((m3/4)*L3^2+I3)*u3(i)+(m3/2)*L1*L3*dT1(i-1)*(dT1(i-1)-dT3(i1))*sin(T2(i-1)+T3(i-1))+(m3/2)*L2*L3*sin(T3(i-1))*(dT1(i-1)+dT2(i-1))^2; %ma tran khoi luong tay may m11(i)=((m1/4)+m2+m3)*(L1^2)+(m2/4+m3)*(L2^2)+(m3/4)*L3^2+I1+I2+I3+(m2 +2*m3)*L1*L2*cos(T2(i-1))+m3*L1*L3*cos(T2(i-1)+T3(i-1)); m12(i)=(m2/4+m3)*(L2^2)+(m3/4)*L3^2+I2+I3+(m2/2+m3)*L1*L2*cos(T2(i1))+(m3/2)*L1*L3*cos(T2(i-1)+T3(i-1))+m3*L2*L3*cos(T3(i-1)); m13(i)=(m3/4)*L3^2+I3+(m3/2)*L2*L3*cos(T2(i-1)+T3(i1))+(m3/2)*L2*L3*cos(T3(i-1)); m21(i)=(m2/4+m3)*(L2^2)+(m3/4)*L3^2+I2+I3+(m2/2+m3)*L1*L2*cos(T2(i1))+(m3/2)*L1*L3*cos(T2(i-1)+T3(i-1))+m3*L2*L3*cos(T3(i-1)); m22(i)=(m2/4+m3)*(L2^2)+(m3/4)*L3^2+I2+I3+m3*L2*L3*cos(T3(i-1)); m23(i)=(m3/4)*L3^2+I3+(m3/2)*L2*L3*cos(T3(i-1)); m31(i)=(m3/4)*L3^2+I3+(m3/2)*L2*L3*cos(T2(i-1)+T3(i1))+(m3/2)*L2*L3*cos(T3(i-1)); m32(i)=(m3/4)*L3^2+I3+(m3/2)*L2*L3*cos(T3(i-1)); m33(i)=(m3/4)*L3^2+I3; M=[m11(i) m12(i) m13(i); m21(i) m22(i) m23(i); m31(i) m32(i) m33(i)]; %ma tran [V] HVTH : Lê Khắc Đ i GVHD : TS Phan Tấn Tù ng 86 v11(i)=-(m2/2+m3)*L1*L2*dT2(i-1)*(2*dT1(i-1)+dT2(i-1))*sin(T2(i-1))(m3/2)*L1*L3*(dT2(i-1)+dT3(i-1))*(2*dT1(i-1)+dT2(i-1)+dT3(i-1))*sin(T2(i1)+T3(i-1))-(m3/2)*L2*L3*dT3(i-1)*(2*dT1(i-1)+2*dT2(i-1)+dT3(i-1))*sin(T3(i1)); v21(i)=(m2/2+m3)*L1*L2*(dT1(i-1)^2)*sin(T2(i-1))+(m3/2)*L1*L3*(dT1(i1)^2)*sin(T2(i-1)+T3(i-1))-(m3/2)*L2*L3*dT3(i-1)*(2*dT1(i-1)+2*dT2(i-1)+dT3(i1))*sin(T3(i-1)); v31(i)=(m3/2)*L1*L3*dT1(i-1)*(dT1(i-1)-dT3(i-1))*sin(T2(i-1)+T3(i1))+(m3/2)*L2*L3*sin(T3(i-1))*(dT1(i-1)+dT2(i-1))^2; V=[v11(i); v21(i); v31(i)]; %ma tran [Tau] Tau=[Tau1(i); Tau2(i); Tau3(i)]; %ma tran ddT ddT=inv(M)*(Tau-V); ddT1(i)=ddT(1,:); ddT2(i)=ddT(2,:); ddT3(i)=ddT(3,:); dT1(i)=dT1(i-1)+ddT1(i)*dt; dT2(i)=dT2(i-1)+ddT2(i)*dt; dT3(i)=dT3(i-1)+ddT3(i)*dt; T1(i)=T1(i-1)+dT1(i)*dt; T2(i)=T2(i-1)+dT2(i)*dt; T3(i)=T3(i-1)+dT3(i)*dt; HVTH : Lê Khắc Đ i GVHD : TS Phan Tấn Tù ng 87 %============ XE ROBOT ============= e1p(i)=L1*cos(T1(i))+L2*cos(T1(i)+T2(i))+L3*cos(T1(i)+T2(i)+T3(i)); e2p(i)=L1*sin(T1(i))+L2*sin(T1(i)+T2(i))+L3*sin(T1(i)+T2(i)+T3(i))-L; e3p(i)=T1(i)+T2(i)+T3(i)-pi/2; de1p(i)=-L1*dT1(i)*sin(T1(i))-L2*(dT1(i)+dT2(i))*sin(T1(i)+T2(i))L3*(dT1(i)+dT2(i)+dT3(i))*sin(T1(i)+T2(i)+T3(i)); de2p(i)=L1*dT1(i)*cos(T1(i))+L2*(dT1(i)+dT2(i))*cos(T1(i)+T2(i))+L3*(dT1 (i)+dT2(i)+dT3(i))*cos(T1(i)+T2(i)+T3(i)); de3p(i)=dT1(i)+dT2(i)+dT3(i); wpd(i)=wr(i)+vr*k2p*e2p(i)+k3p*sin(e3p(i)); vpd(i)=L*wpd(i)+vr*cos(e3p(i))+k1p*e1p(i); e4p(i-1)=vp(i-1)-vpd(i); e5p(i-1)=wp(i-1)-wpd(i); dwpd(i)=k2p*de2p(i)*vr+k3p*de3p(i)*cos(e3p(i)); dvpd(i)=L*dwpd(i)-vr*de3p(i)*sin(e3p(i))+k1p*de1p(i); u4(i)=dvpd(i)-k4p*e4p(i-1); u5(i)=dwpd(i)-k5p*e5p(i-1); %ma tran khoi luong xe robot [Mv] mv11(i)=m+2*Iw/(r^2); mv12(i)=-h*sin(beta)*(m1+m2+m3); mv21(i)=0; mv22(i)=I+2*Iw/(r^2)+(m1*(L1^2)+m2*(L2^2)+m3*(L3^2)); Mv=[mv11(i) mv12(i); mv21(i) mv22(i)]; HVTH : Lê Khắc Đ i GVHD : TS Phan Tấn Tù ng 88 %ma tran [Cv] cv11(i)=0; cv12(i)=-mc*d*wp(i-1)-h*wp(i-1)*cos(beta)*(m1+m2+m3); cv21(i)=mc*d*wp(i-1); cv22(i)=0; Cv=[cv11(i) cv12(i); cv21(i) cv22(i)]; %ma tran [E] e11(i)=1/r; e12(i)=1/r; e21(i)=b/r; e22(i)=-bDi! E=[e11(i) e12(i); e21(i) e22(i)]; %ma tran [dv] dv1(i)=u4(i); dv2(i)=u5(i); dv=[dv1(i); dv2(i)]; %ma tran [v] v1(i)=vp(i-1); v2(i)=wp(i-1); v=[v1(i); v2(i)]; %ma tran [TauP] TauP=inv(E)*(Mv*dv+Cv*v); TauR(i)=TauP(1,:); TauL(i)=TauP(2,:); HVTH : Lê Khắc Đ i GVHD : TS Phan Tấn Tuø ng 89 %tinh dvp & dwp ddp=inv(Mv)*(E*TauP-Cv*v); dvp(i)=ddp(1,:); dwp(i)=ddp(2,:); vp(i)=vp(i-1)+dvp(i)*dt; wp(i)=wp(i-1)+dwp(i)*dt; we(i)=dT1(i)+dT2(i)+dT3(i)+wp(i-1); dwe(i)=ddT1(i)+ddT2(i)+ddT3(i)+dwp(i-1); php(i)=php(i-1)+wp(i)*dt; xp(i)=xp(i-1)+vp(i)*cos(php(i))*dt; yp(i)=yp(i-1)+vp(i)*sin(php(i))*dt; phe(i)=T1(i)+T2(i)+T3(i)+php(i)-pi/2; xe(i)=xp(i)+L1*cos(T1(i)+php(i))+L2*cos(T1(i)+T2(i)+php(i))+L3*cos(T1(i)+T2(i) +T3(i)+php(i)); ye(i)=yp(i)+L1*sin(T1(i)+php(i))+L2*sin(T1(i)+T2(i)+php(i))+L3*sin(T1(i)+T2(i)+ T3(i)+php(i)); e1(i)=(xr(i)-xe(i))*cos(phe(i))+(yr(i)-ye(i))*sin(phe(i)); e2(i)=-(xr(i)-xe(i))*sin(phe(i))+(yr(i)-ye(i))*cos(phe(i)); e3(i)=phr(i)-phe(i); de1(i)=vr*cos(phr(i)-phe(i))-vr-we(i)*((xr(i)-xe(i))*sin(phe(i))-(yr(i)ye(i))*cos(phe(i))); de2(i)=vr*sin(phr(i)-phe(i))-we(i)*((xr(i)-xe(i))*cos(phe(i))+(yr(i)ye(i))*sin(phe(i))); de3(i)=wr(i)-we(i); HVTH : Lê Khắc Đ i GVHD : TS Phan Tấn Tuø ng 90 if mod(i,400) == J0=[xp(i) yp(i)]; J1=[J0(1)+L1*cos(T1(i)+php(i)) J0(2)+L1*sin(T1(i)+php(i))]; J2=[J0(1)+L1*cos(T1(i)+php(i))+L2*cos(T1(i)+T2(i)+php(i)) J0(2)+L1*sin(T1(i)+php(i))+L2*sin(T1(i)+T2(i)+php(i))]; J3=[J0(1)+L1*cos(T1(i)+php(i))+L2*cos(T1(i)+T2(i)+php(i))+L3*cos(T1(i)+T2(i)+ T3(i)+php(i)) J0(2)+L1*sin(T1(i)+php(i))+L2*sin(T1(i)+T2(i)+php(i))+L3*sin(T1(i)+T2(i)+T3(i)+ php(i))]; line([J0(1),J1(1),J2(1),J3(1)],[J0(2),J1(2),J2(2),J3(2)],'Color','m'); Cp1=[xp(i)+0.1768*cos(php(i)-pi/4) yp(i)+0.1768*sin(php(i)-pi/4)]; Cp2=[xp(i)+0.1768*cos(php(i)+pi/4) yp(i)+0.1768*sin(php(i)+pi/4)]; Cp3=[xp(i)+0.1768*cos(php(i)+(pi-pi/4)) yp(i)+0.1768*sin(php(i)+(pi-pi/4))]; Cp4=[xp(i)+0.1768*cos(php(i)-(pi-pi/4)) yp(i)+0.1768*sin(php(i)-(pi-pi/4))]; line([Cp1(1),Cp2(1),Cp3(1),Cp4(1),Cp1(1)],[Cp1(2),Cp2(2),Cp3(2),Cp4(2),Cp1(2)],' Color','B'); end; end; ww=[1/r b/r;1/r -b/r]*[vp;wp]; wwr=ww(1,:);wwl=ww(2,:); hold on; plot(xr,yr,xp,yp,xe,ye); axis equal; figure;plot(t,e1*1e3,' ',t,e2*1e3,'-.',t,e3*180/pi); HVTH : Lê Khắc Đ i GVHD : TS Phan Tấn Tù ng 91 xlabel('Sai so e1 -, e2 -.-, e3 _'); figure;plot(t,e1p*1e3,' ',t,e2p*1e3,'-.',t,e3p*180/pi); xlabel('Sai so e4 -, e5 -.-, e6 _'); %figure;plot(t,e4,' ',t,e5,'-.',t,e6); xlabel('Sai so e7 -, e8 -.-, e9 _'); % figure; plot(t,e4p,' ',t,e5p,'-.'); xlabel('Sai so e10 -, e11 -.-'); figure;plot(t,T1*180/pi,' ',t,T2*180/pi,'-.',t,T3*180/pi); xlabel('Goc quay khau -, -.-., _'); figure;plot(t,dT1,' ',t,dT2,'-.',t,dT3); xlabel('Thoi gian (s)- Van toc goc (rad/s) khop -,2-.-,3 _ '); figure;plot(t,vp,' ',t,wp); xlabel('Van toc vp - , van toc goc wp _'); figure;plot(t,wwr*30/pi,' ',t,wwl*30/pi); xlabel('Thoi gian (s)- Van toc goc (rad/s) banh xe phai -va trai _)'); figure;plot(t,Tau1,' ',t,Tau2,'-.',t,Tau3); xlabel('Moment khop -,2-.-,3 _'); figure;plot(t,TauR,' ',t,TauL); xlabel('Moment banh xe phai -va trai _)'); disp('Done'); HVTH : Lê Khắc Đ i GVHD : TS Phan Tấn Tù ng ... ) X Hình 3.2 : Sơ đồ xác định sai số (e4 ,e5 ,e6) 3.2 .Thiết kế luật điều khiển cho tay máy di động mô hình động học : 3.2.1 Thiết kế luật điều khiển cho tay máy mô hình động học : Bộ đ iều khiển. .. 3.2.2 Thiết kế luật đ iều khiển cho xe robot mô hình đ ộng họ c 50 3.3 Thiết kế bộ? ? iều khiển cho tay máy di đ ộng theo mô hình đ ộng lự c họ c 54 3.3.1 Thiết kế bộ? ? iều khiển cho tay máy. .. 2.4 Mô hình động lực học tay máy di động : 2.4.1 Mô hình động lực học xe robot : Trong phần nà y sẽkhả o sát mô hình đ ộng lự c họ c củ a tay máy phươ ng pháp nhân tửLagrange Khi khả o sát mô hình