Tài liệu hạn chế xem trước, để xem đầy đủ mời bạn chọn Tải xuống
1
/ 185 trang
THÔNG TIN TÀI LIỆU
Thông tin cơ bản
Định dạng
Số trang
185
Dung lượng
7,16 MB
Nội dung
MINISTRY OF EDUCATION AND TRAINING MINISTRY OF NATIONAL DEFENCE MILITARY TECHNICAL ACADEMY DUONG XUAN BIEN DYNAMIC MODELLING AND CONTROL OF TWO-LINK FLEXIBLE ROBOTS BY USING FINITE ELEMENT METHOD DOCTOR OF PHILOSOPHY HA NOI, 2019 LUAN VAN CHAT LUONG download : add luanvanchat@agmail.com MINISTRY OF EDUCATION AND TRAINING MINISTRY OF NATIONAL DEFENCE MILITARY TECHNICAL ACADEMY DUONG XUAN BIEN DYNAMIC MODELLING AND CONTROL OF TWO-LINK FLEXIBLE ROBOTS BY USING FINITE ELEMENT METHOD Major: Technical mechanic Code: 9.52.01.03 DOCTOR OF PHILOSOPHY SCIENCE SUPERVISORS: Associate Prof, Dr Chu Anh My Associate Prof, Dr Phan Bui Khoi HA NOI, 2019 LUAN VAN CHAT LUONG download : add luanvanchat@agmail.com ACKNOWLEDGMENTS I would like to express my deepest gratitude to Professor Chu Anh My and Professor Phan Bui Khoi for their support, dedicated guide and research orientation on this work I wish to thank all my colleagues from Advanced Technology Center, Faculty of Mechanical Engineering, Faculty of Aerospace in Military Technical Academy and School of Mechanical Engineering in Hanoi University of Science and Technology for the help they gave me in the many different occasions The greatly appreciation is to my family for their love and support Last but not least, I would like to thank all the others that are not mentioned and helped me on this thesis LUAN VAN CHAT LUONG download : add luanvanchat@agmail.com CONFIRMATION BY AUTHOR I pled that this thesis is my own research work The results presented in this work are honest and has not been published by anyone in any other works The information cited in this thesis is clearly stated origins August, 2019 Duong Xuan Bien LUAN VAN CHAT LUONG download : add luanvanchat@agmail.com LIST OF SYMBOLS AND ABBREVIATIONS Li , lie Length of link i , length of each element of link i i Angle between link i − and link i Number of links of robot, number of elements of link i n, ni ,i (t ) and joint variable of link i x Arbitrary point on the element j of link i m (x ), m = Shape functions of element j Elastic displacement at arbitrary point on element j of wij (t, x ) link i ui (2 j −1), ui(2 j ), ui (2 j +1), ui (2 j +2) Flexural displacement, slope displacement of node j ui (2n +1), ui (2n + 2) Flexural and slope displacement at end point of link i i i and node j + of element j of link i , respectively u(i −1)(2k −1), u(i −1)(2k ), Flexural and slope displacement at node k and node u(i −1)(2k +1), u(i −1)(2k +2) k + of element k of link i − u(i −1)f , u(i −1)s Flexural and slope displacement at the end point of link i −1 General homogeneous transformation matrix which Hif(i −1) transforms from the coordinate system Oi XY to the i i coordinate system Oi −1Xi −1Yi −1 rij , r0ij r02r , r02 f Position vector of arbitrary point on the element j of link i in the coordinate systems Oi XY and O0X 0Y0 i i Position vector of the end point of link in cases of rigid and flexible models in the coordinate system O0X 0Y0 LUAN VAN CHAT LUONG download : add luanvanchat@agmail.com di (t ), i (t ) qijcv (t ), qicv (t ) qij (t ), qi (t ), q(t ) mi , mdci , mt Tij ,Ti ,T Tie ,Tid ,Tp Translational and rotational joint variable of link i Elastic displacement vector of the element j of link i and elastic displacement vector of link i Generalized elastic displacement vectors of the element j , of the link i and of the system Mass per length unit of link i , mass of motor i , mass of the tip load Kinetic energy of element j of link i , kinetic energy of link i and kinetic energy of system Elastic deforming kinetic energy of link i , kinetic energy of motor driving link i and the tip load Elastic deforming potential and gravitational potential Pije , Pijg , Pi , P energy of element j of link i , potential energy of link i and the system Mj , Mi , M Mass matrix of element j , link i and system Mdc , Mtt Mass matrices of the motor and the tip load Kj , Ki , K Stiffness matrix of element j , link i and system C(q, q) Coriolis matrix Qex (t ) Generalized force/torque vector of the system Fi (t), i (t ) Driving force, torque at the joint i e*(t ), e(t ),V KP , KI , KD Joint variable error vector, error vector in objective function and Lyapunov function Cross matrix of control parameters in PID controller LUAN VAN CHAT LUONG download : add luanvanchat@agmail.com TABLE OF CONTENTS Pages PREFACE CHAPTER LITERATURE REVIEW OF FLEXIBLE ROBOT DYNAMIC AND CONTROL 1.1 Applications of flexible robots 1.2 Classifying joint types of flexible robots 1.3 Classifying flexible robots 11 1.4 Modeling methods 13 1.5 Differential motion equations 14 1.6 Recent works on flexible robots 15 1.7 Position accuracy of motion of flexible robots 19 1.8 Comments and problems 20 Conclusion of chapter 21 CHAPTER DYNAMIC MODELING OF THE PLANAR FLEXIBLE ROBOTS 22 2.1 Kinematic of the planar flexible robots 22 2.2 Dynamics of the planar flexible robots 38 Conclusion of chapter 58 CHAPTER DYNAMIC ANALYSIS AND POSITION CONTROL OF THE PLANAR TWO-LINK FLEXIBLE ROBOTS 59 3.1 Boundary conditions 59 3.2 Forward dynamic 61 3.3 Inverse dynamic 79 3.4 Position control system of the planar serial multi-link flexible robots 86 Conclusion of chapter 99 CHAPTER EXPERIMENT 101 4.1 Objective and experimental model 101 4.2 Parameters, equipment and method of measuring 103 4.3 System connection diagram 105 LUAN VAN CHAT LUONG download : add luanvanchat@agmail.com 4.4 Experimental orders 107 4.5 Method of handling the measurement data 108 4.6 Experimental results 110 Conclusion of chapter 115 CONCLUSION AND SUGGESION 116 LIST OF THE RESEARCH PAPERS OF THE AUTHOR 118 REFERENCES 121 APPENDICES 139 LUAN VAN CHAT LUONG download : add luanvanchat@agmail.com LIST OF TABLES Table The parameters i , u(i 1)s , u(i 1) f , i , a i depending on types of joints 26 Table The dynamic parameters of flexible robot type I (continuous) 65 Table The mass ratios between the flexible links and tip load 67 Table 3 The maximum elastic displacements at the ending points of the links 67 Table The parameters of the flexible robot type IV 70 Table The length of the links in two cases 71 Table The maximum values in two cases 74 Table The parameters of flexible robot type III 75 Table The parameters of the flexible robot type IV 92 Table The parameters of the GA and the position PID controller 93 Table 10 The comparative results the control quality between two cases 94 Table 11 The parameters of the GA and the position PID controller 96 Table 12 The comparative results the control quality between two cases 97 LUAN VAN CHAT LUONG download : add luanvanchat@agmail.com LIST OF FIGURES Figure The structure of the thesis Figure The order executing the thesis Figure 1 Flexible robots Figure The flexible robot in space Figure Flexible robot in medicine Figure Rotational joint Figure Translational joint type Pa Figure Translational joint type Pb Figure The single-link flexible robot with rotational joint 11 Figure The single-link flexible robot with translational joint 11 Figure The two-link flexible robots with only rotational joints 12 Figure 10 The two-link flexible robots consist translational joints 12 Figure 11 The planar serial multi-link flexible robots 12 Figure 12 The parallel-link flexible robots 13 Figure 13 The mobile fiexlible robots 13 Figure 14 Flexible planar closed mechanism [8] 15 Figure 15 Spring-mass system [45] 16 Figure 16 The single-link flexible robot with joint Pa [133] 17 Figure 17 The two-link flexible robot Quanser 17 Figure 18 The two-link flexible robot with rotational joints 17 Figure 19 The flexible robot with rotational and translational joints 18 Figure A generalized schematic of an arbitrary pair of flexible links 23 Figure 2 Structure I 29 Figure Structure II 30 Figure Structure III 31 Figure Structure IV 32 Figure Structure V 33 Figure Structure VI 34 Figure Structure VII 35 LUAN VAN CHAT LUONG download : add luanvanchat@agmail.com 157 Appendix The elements of matrix Mtt of flexible robot type I LUAN VAN CHAT LUONG download : add luanvanchat@agmail.com 158 Appendix Extended PID controller and codes program in MATLAB of flexible robot type III (varied boundary conditions) P4.1 Extended PID controller in SIMULINK The PID control law in SIMULINK is shown as Fig P1.14 P1 14 PID control law in SIMULINK P4.2 MATLAB codes of the dynamic model Declare the parameters (Title: einfor) function [N, L2, L1, E, I, le, m, mt, m1, Ih] = etinfo() N=5;% So phan tu khau dan hoi L2=0.8;% Chieu dai tong khau dan hoi L1=0.2; % Chieu dai khau cung rho=7850; % kg/m3 b2=0.02; % m h2=0.002; % m A2=b*h; % m2 m=rho*A2; % kg/m Khoi luong theo don vi chieu dai khau I=b2*h2^3/12; % m4 E=2*10^10; % N/m2 le=L2/N;% (m/kg) Chieu dai phan tu khau mt=0.02; %20g b1=0.02; h1=0.02; A1=b1*h1; m1=L1*A1*rho; % Khoi luong khau Ih=b1*h1^3/12; end Matrices M, C, K and technique of handling the varied boundary conditions (Noted that the codes below are entered into blocks of SIMULINK which is presented in Fig 3.32, Fig 3.57 and Fig 3.58) LUAN VAN CHAT LUONG download : add luanvanchat@agmail.com 159 function x_d2dot = fcn(Tau,F,x,dx) %#codegen [N,L2,L1,E,I,le,m,Jh,mt,m1,Ih] = etinfo(); %% M=zeros(14,14); M(1,1)=0.13e2 / 0.210e3 * le ^ * m * x(6) * x(7) + 0.9e1 / 0.35e2 * le * m * x(5) * x(7) - le ^ * m * x(8) * x(10) / 0.70e2 - 0.13e2 / 0.210e3 * le ^ * m * x(7) * x(10) + 0.13e2 / 0.210e3 * le ^ * m * x(8) * x(9) + 0.9e1 / 0.35e2 * le * m * x(7) * x(9) - le ^ * m * x(10) * x(12) / 0.70e2 - 0.13e2 / 0.210e3 * le ^ * m * x(9) * x(12) + 0.13e2 / 0.210e3 * le ^ * m * x(10) * x(11) + 0.9e1 / 0.35e2 * le * m * x(9) * x(11) - le ^ * m * x(12) * x(14) / 0.70e2 - 0.13e2 / 0.210e3 * le ^ * m * x(11) * x(14) + 0.13e2 / 0.210e3 * le ^ * m * x(12) * x(13) - 0.11e2 / 0.105e3 * le ^ * m * x(13) * x(14) + 0.9e1 / 0.35e2 * le * m * x(11) * x(13) - le ^ * m * x(4) * x(6) / 0.70e2 + 0.11e2 / 0.105e3 * le ^ * m * x(3) * x(4) - 0.13e2 / 0.210e3 * le ^ * m * x(3) * x(6) + 0.13e2 / 0.210e3 * le ^ * m * x(4) * x(5) + 0.10e2 * L1 * x(2) * le * m - 0.10e2 * L2 * x(2) * le * m + 0.9e1 / 0.35e2 * le * m * x(3) * x(5) - le ^ * m * x(6) * x(8) / 0.70e2 - 0.13e2 / 0.210e3 * le ^ * m * x(5) * x(8) + Ih / 0.2e1 + m1 * L1 / 0.2e1 - 0.10e2 * L1 * L2 * le * m + Jh + L1 * x(2) * mt + 0.25e2 * m * le ^ * x(2) + le ^ * m * x(4) ^ / 0.105e3 + 0.2e1 / 0.105e3 * le ^ * m * x(6) ^ + 0.5e1 * x(2) ^ * le * m + 0.13e2 / 0.35e2 * le * m * x(3) ^ + 0.26e2 / 0.35e2 * le * m * x(5) ^ + 0.2e1 / 0.105e3 * le ^ * m * x(8) ^ + 0.26e2 / 0.35e2 * le * m * x(7) ^ + 0.2e1 / 0.105e3 * le ^ * m * x(10) ^ + 0.26e2 / 0.35e2 * le * m * x(9) ^ + 0.2e1 / 0.105e3 * le ^ * m * x(12) ^ + 0.26e2 / 0.35e2 * le * m * x(11) ^ + le ^ * m * x(14) ^ / 0.105e3 + 0.13e2 / 0.35e2 * le * m * x(13) ^ + 0.5e1 * L1 ^ * le * m + 0.5e1 * L2 ^ * le * m + 0.125e3 / 0.3e1 * m * le ^ + 0.25e2 * m * le ^ * L1 - 0.25e2 * (dx(1)*dx(5)*le*m*(54*x(3) + 312*x(5) + 54*x(7) + 13*le*x(4) 13*le*x(8)))/420 + (dx(1)*dx(7)*le*m*(54*x(5) + 312*x(7) + 54*x(9) + 13*le*x(6) 13*le*x(10)))/420 + (dx(1)*dx(9)*le*m*(54*x(7) + 312*x(9) + 54*x(11) + 13*le*x(8) 13*le*x(12)))/420 + (dx(1)*dx(11)*le*m*(54*x(9) + 312*x(11) + 54*x(13) + 13*le*x(10) - 13*le*x(14)))/420 + (dx(1)*dx(3)*le*m*(156*x(3) + 54*x(5) + 22*le*x(4) 13*le*x(6)))/420 + (dx(1)*dx(4)*le^2*m*(22*x(3) + 13*x(5) + 4*le*x(4) 3*le*x(6)))/420 (dx(1)*dx(14)*le^2*m*(13*x(11) + 22*x(13) + 3*le*x(12) 4*le*x(14)))/420; C(2)=-(dx(1)*(12*dx(13)*mt + 4*L1*dx(1)*mt + 6*dx(3)*le*m + 12*dx(5)*le*m + 12*dx(7)*le*m + 12*dx(9)*le*m + 12*dx(11)*le*m + 6*dx(13)*le*m + 4*dx(1)*mt*x(2) + 100*dx(1)*le^2*m + dx(4)*le^2*m dx(14)*le^2*m + 40*dx(1)*le*m*x(2) + 40*L1*dx(1)*le*m 40*L2*dx(1)*le*m))/8; C(3)=-(dx(1)*le*m*(156*dx(1)*x(3) 315*dx(2) + 54*dx(1)*x(5) + 22*dx(1)*le*x(4) 13*dx(1)*le*x(6)))/420; C(4)=-(dx(1)*le^2*m*(44*dx(1)*x(3) - 105*dx(2) + 26*dx(1)*x(5) + 8*dx(1)*le*x(4) 6*dx(1)*le*x(6)))/840; C(5)=-(dx(1)*le*m*(54*dx(1)*x(3) 630*dx(2) + 312*dx(1)*x(5) + 54*dx(1)*x(7) + 13*dx(1)*le*x(4) 13*dx(1)*le*x(8)))/420; C(6)=(dx(1)^2*le^2*m*(13*x(3) 13*x(7) + 3*le*x(4) - 8*le*x(6) + 3*le*x(8)))/420; C(7)=-(dx(1)*le*m*(54*dx(1)*x(5) 630*dx(2) + 312*dx(1)*x(7) + 54*dx(1)*x(9) + 13*dx(1)*le*x(6) 13*dx(1)*le*x(10)))/420; LUAN VAN CHAT LUONG download : add luanvanchat@agmail.com 160 m * le ^ * L2 + L1 ^ * mt / 0.2e1 + x(13) ^ * mt / 0.2e1 + x(2) ^ * mt / 0.2e1; M(1,2)=-le ^ * m * x(4) / 0.12e2 - le * m * x(3) / 0.2e1 - le * m * x(5) - le * m * x(7) - le * m * x(9) - le * m * x(11) + le ^ * m * x(14) / 0.12e2 - le * m * x(13) / 0.2e1 - 0.2e1 * mt * x(13); M(1,3)=-(m * le * (10 * L2 - 10 * x(2) - * le)) / 0.20e2; M(1,4)=-(m * le ^ * (5 * L2 - * x(2) - * le)) / 0.60e2; M(1,5)= -m * le * L2 + m * le * x(2) + m * le ^ 2; M(1,6)=m * le ^ / 0.15e2; M(1,7)=-m * le * L2 + m * le * x(2) + * m * le ^ 2; M(1,8)=m * le ^ / 0.15e2; M(1,9)=-m * le * L2 + m * le * x(2) + * m * le ^ 2; M(1,10)= m * le ^ / 0.15e2; M(1,11)=-m * le * L2 + m * le * x(2) + * m * le ^ 2; M(1,12)= m * le ^ / 0.15e2; M(1,13)=m * le * L1 / 0.2e1 - m * le * L2 / 0.2e1 + m * le * x(2) / 0.2e1 + 0.47e2 / 0.20e2 * m * le ^ + mt * L1 + mt * x(2) / 0.2e1; M(1,14)=(m * le ^ * (5 * L2 - * x(2) - 23 * le)) / 0.60e2; for f=1:14 M(f,1)=M(1,f); end; % M(2,2)=(5 * m * le) + mt / 0.2e1; % M(3,3)=0.13e2 / 0.35e2 * m * le; M(3,4)=0.11e2 / 0.210e3 * m * le ^ 2; M(3,5)=0.9e1 / 0.70e2 * m * le; M(3,6)=-0.13e2 / 0.420e3 * m * le ^ 2; for f=3:14 M(f,3)=M(3,f); end; % M(4,4)=m * le ^ / 0.105e3; M(4,5)=0.13e2 / 0.420e3 * m * le ^ 2; M(4,6)=-m * le ^ / 0.140e3; for f=4:14 M(f,4)=M(4,f); end; % M(5,5)=0.26e2 / 0.35e2 * m * le; M(5,6)=0; C(8)=(dx(1)^2*le^2*m*(13*x(5) 13*x(9) + 3*le*x(6) - 8*le*x(8) + 3*le*x(10)))/420; C(9)=-(dx(1)*le*m*(54*dx(1)*x(7) 630*dx(2) + 312*dx(1)*x(9) + 54*dx(1)*x(11) + 13*dx(1)*le*x(8) 13*dx(1)*le*x(12)))/420; C(10)=(dx(1)^2*le^2*m*(13*x(7) 13*x(11) + 3*le*x(8) - 8*le*x(10) + 3*le*x(12)))/420; C(11)=-(dx(1)*le*m*(54*dx(1)*x(9) 630*dx(2) + 312*dx(1)*x(11) + 54*dx(1)*x(13) + 13*dx(1)*le*x(10) - 13*dx(1)*le*x(14)))/420; C(12)=(dx(1)^2*le^2*m*(13*x(9) 13*x(13) + 3*le*x(10) - 8*le*x(12) + 3*le*x(14)))/420; C(13)=-(dx(1)*(210*dx(1)*mt*x(13) 315*dx(2)*le*m - 945*dx(2)*mt + 54*dx(1)*le*m*x(11) + 156*dx(1)*le*m*x(13) + 13*dx(1)*le^2*m*x(12) 22*dx(1)*le^2*m*x(14)))/420; C(14)=(dx(1)*le^2*m*(26*dx(1)*x(11) - 105*dx(2) + 44*dx(1)*x(13) + 6*dx(1)*le*x(12) 8*dx(1)*le*x(14)))/840; %% Ma tran K K=zeros(14,14); % K(3,3)=(12*I) * E / le ^ 3; K(3,4)=(6*I) * E / le ^ 2; K(3,5)=(-12*I) * E / le ^ 3; K(3,6)=(6*I) * E / le ^ 2; for f=3:14 K(f,3)=K(3,f); end; % K(4,4)=(4*I) * E / le; K(4,5)=(-6*I) * E / le ^ 2; K(4,6)=(2*I) * E / le; for f=4:14 K(f,4)=K(4,f); end; % K(5,5)=(24*I) * E / le ^ 3; K(5,6)=0; K(5,7)=(-12*I) * E / le ^ 3; K(5,8)=(6*I) * E / le ^ 2; for f=5:14 LUAN VAN CHAT LUONG download : add luanvanchat@agmail.com 161 M(5,7)=0.9e1 / 0.70e2 * m * le; M(5,8)=-0.13e2 / 0.420e3 * m * le ^ 2; for f=5:14 M(f,5)=M(5,f); end; % M(6,6)=0.2e1 / 0.105e3 * m * le ^ 3; M(6,7)=0.13e2 / 0.420e3 * m * le ^ 2; M(6,8)=-m * le ^ / 0.140e3; for f=6:14 M(f,6)=M(6,f); end; % M(7,7)=0.26e2 / 0.35e2 * m * le; M(7,8)=0; M(7,9)=0.9e1 / 0.70e2 * m * le; M(7,10)=-0.13e2 / 0.420e3 * m * le ^ 2; for f=7:14 M(f,7)=M(7,f); end; % M(8,8)=0.2e1 / 0.105e3 * m * le ^ 3; M(8,9)=0.13e2 / 0.420e3 * m * le ^ 2; M(8,10)=-m * le ^ / 0.140e3; for f=8:14 M(f,8)=M(8,f); end; % M(9,9)=0.26e2 / 0.35e2 * m * le; M(9,10)=0; M(9,11)=0.9e1 / 0.70e2 * m * le; M(9,12)=-0.13e2 / 0.420e3 * m * le ^ 2; for f=9:14 M(f,9)=M(9,f); end; % M(10,10)=0.2e1 / 0.105e3 * m * le ^ 3; M(10,11)=0.13e2 / 0.420e3 * m * le ^ 2; M(10,12)=-m * le ^ / 0.140e3; for f=10:14 M(f,10)=M(10,f); end; % M(11,11)=0.26e2 / 0.35e2 * m * le; M(11,12)=0; M(11,13)=0.9e1 / 0.70e2 * m * le; M(11,14)=-0.13e2 / 0.420e3 * m * le ^ 2; K(f,5)=K(5,f); end; % K(6,6)=(8*I) * E / le; K(6,7)=(-6*I) * E / le ^ 2; K(6,8)=(2*I) * E / le; for f=6:14 K(f,6)=K(6,f); end; % K(7,7)=(24*I) * E / le ^ 3; K(7,8)=0; K(7,9)=(-12*I) * E / le ^ 3; K(7,10)=(6*I) * E / le ^ 2; for f=7:14 K(f,7)=K(7,f); end; % K(8,8)=(8*I) * E / le; K(8,9)=(-6*I) * E / le ^ 2; K(8,10)=(2*I) * E / le; for f=8:14 K(f,8)=K(8,f); end; % K(9,9)=(24*I) * E / le ^ 3; K(9,10)=0; K(9,11)=(-12*I) * E / le ^ 3; K(9,12)=(6*I) * E / le ^ 2; for f=9:14 K(f,9)=K(9,f); end; % K(10,10)=(8*I) * E / le; K(10,11)=(-6*I) * E / le ^ 2; K(10,12)=(2*I) * E / le; for f=10:14 K(f,10)=K(10,f); end; % K(11,11)=(24*I) * E / le ^ 3; K(11,12)=0; K(11,13)=(-12*I) * E / le ^ 3; K(11,14)=(6*I) * E / le ^ 2; for f=11:14 K(f,11)=K(11,f); end; % K(12,12)=(8*I) * E / le; K(12,13)=(-6*I) * E / le ^ 2; K(12,14)=(2*I) * E / le; for f=12:14 K(f,12)=K(12,f); end; % K(13,13)=(12*I) * E / le ^ 3; K(13,14)=(-6*I) * E / le ^ 2; LUAN VAN CHAT LUONG download : add luanvanchat@agmail.com 162 for f=11:14 M(f,11)=M(11,f); end; % M(12,12)=0.2e1 / 0.105e3 * m * le ^ 3; M(12,13)=0.13e2 / 0.420e3 * m * le ^ 2; M(12,14)=-m * le ^ / 0.140e3; for f=12:14 M(f,12)=M(12,f); end; % M(13,13)=0.13e2 / 0.35e2 * m * le + mt / 0.2e1; M(13,14)=-0.11e2 / 0.210e3 * m * le ^ 2; for f=13:14 M(f,13)=M(13,f); end; % M(14,14)=m * le ^ / 0.105e3; %% Cq=zeros(14,1); % Bieu thuc C*Q C(1)=dx(1)*dx(2)*((L1*mt)/2 + (mt*x(2))/2 + (25*le^2*m)/2 + 5*L1*le*m - 5*L2*le*m + 5*le*m*x(2)) + dx(1)*dx(13)*((mt*x(13))/2 + (13*le^2*m*x(12))/420 (11*le^2*m*x(14))/210 + (9*le*m*x(11))/70 + (13*le*m*x(13))/35) dx(2)*dx(13)*(mt + (le*m)/4) + dx(2)*dx(13)*(mt/4 + (le*m)/4) (dx(1)*dx(6)*le^2*m*(13*x(3) 13*x(7) + 3*le*x(4) - 8*le*x(6) + 3*le*x(8)))/420 (dx(1)*dx(8)*le^2*m*(13*x(5) 13*x(9) + 3*le*x(6) - 8*le*x(8) + 3*le*x(10)))/420 (dx(1)*dx(10)*le^2*m*(13*x(7) 13*x(11) + 3*le*x(8) - 8*le*x(10) + 3*le*x(12)))/420 (dx(1)*dx(12)*le^2*m*(13*x(9) 13*x(13) + 3*le*x(10) - 8*le*x(12) + 3*le*x(14)))/420 + for f=13:14 K(f,13)=K(13,f); end; % K(14,14)=(4*I) * E / le; %% Ky thuat xu ly dieu kien bien thay doi theo thoi gian dhist = x(2); l=round((L2-dhist)*N/L2); Ffem=zeros(2*N+4,1); Ffem(1)=Tau; Ffem(2)=F; Md=zeros(2*N+2,2*N+2); Kd=zeros(2*N+2,2*N+2); Fd=zeros(2*N+2,1); xd=zeros(2*N+2,1); dxd=zeros(2*N+2,1); % Md(1:2*l+1,1:2*l+1) = M(1:2*l+1,1:2*l+1); Md(2*l+2:2*N+2,2*l+2:2*N+2) = M(2*l+4:2*N+4,2*l+4:2*N+4); % Kd(1:2*l+1,1:2*l+1) = K(1:2*l+1,1:2*l+1); Kd(2*l+2:2*N+2,2*l+2:2*N+2) = K(2*l+4:2*N+4,2*l+4:2*N+4); % Fd(1:2*l+1,1) = Ffem(1:2*l+1,1); Fd(2*l+2:2*N+2,1) = Ffem(2*l+4:2*N+4,1); % xd(1:2*l+1,1) = x(1:2*l+1,1); xd(2*l+2:2*N+2,1) = x(2*l+4:2*N+4,1); % dxd(1:2*l+1,1) = dx(1:2*l+1,1); dxd(2*l+2:2*N+2,1) = dx(2*l+4:2*N+4,1); %% He Phuong trinh dong luc hoc x_d2dot =inv(Md)*(Fd-Cq-Kd*xd); P4.3 Declare the objective function (file: Jmuctieu) function [Jmuctieu] = Jmuctieu(x) [N,~]=size(x);% );% x la ma tran Nx6 Jmuctieu=zeros(N,1 global Kp1 Ki1 Kd1 Kp2 Ki2 Kd2 t ; % tinh gia tri ham muc tieu [xrow,~]=size(e13); tf=length(xrow); t= 0:0.01:tf; Jmt=0; LUAN VAN CHAT LUONG download : add luanvanchat@agmail.com 163 for j=1:1:N Kp1=x(j,1); Ki1=x(j,2); Kd1=x(j,3); Kp2=x(j,4); Ki2=x(j,5); Kd2=x(j,6); model='control'; load_system(model); sim(model); for i=1:1:xrow Jmt=Jmt+50*e11(i,2)^2+50*e12(i,2)^2 +10*e13(i,2)^2+10*e14(i,2)^2+0.01*u pid1(i,2)^2+0.01*upid2(i,2)^2; end Jmt=Jmt/xrow; Jmuctieu(j,1)=Jmt; end end P4.4 GA algorithm codes global Kp1 Ki1 Kd1 Kp2 Ki2 Kd2; ff='Jmuctieu'; % npar=6; LB = [0 0 0 0]; UB =[30 30 30 30 30 30]; maxit=10; mincost=-9999999; popsize=20; mutrate=0.05; selection=0.5; Nt=npar; keep=floor(selection*popsize); nmut=ceil((popsize-1)*Nt*mutrate); M=ceil((popsize-keep)/2); % iga=0; par=rand(popsize,npar)*diag(UBLB)+ones(popsize,npar)*diag(LB); cost=feval(ff,par); [cost,ind]=sort(cost par=par(ind,:); minc(1)=min(cost); meanc(1)=mean(cost) % while iga= 500) && ((millis() - if(run_auto==1) timer ) < 1000)) { { // Dong co Step DCmotor(2, 255); // Nguoc if (pul_step < 50) // Gia tri xung } // 50 xung ung voi 0.16s else if ((millis() - timer ) >= 1000) { { run_step=1; timer = millis();encodertt=0; digitalWrite(DIR_step, LOW); } } } else {run_step=0;} float angle1 = Flex_senssor(FLEX1); // Dong co DC float angle2 = Flex_senssor(FLEX2); LUAN VAN CHAT LUONG download : add luanvanchat@agmail.com 167 if ((millis() - timer ) < 500) { UART_senddata(encoderxoay, encodertt, angle1, angle2, 35); DCmotor(1, 255); // Chay thuan dong co //DC muc cao 12V digitalWrite(DIR_step, LOW); // Vong lap cho dong co step } void Timer1_ISR(void) { } if (run_step == 1) else { // Stop cac dong co i++; { if (i == 1) { run_auto=0; digitalWrite(PUL_step, HIGH); DCmotor(0, 0); } pul_step=0; if (i == 2) { } digitalWrite(PUL_step, LOW); } i = 0; pul_step++; float angle1 = Flex_senssor(FLEX1); } float angle2 = Flex_senssor(FLEX2); if (pul_step < 50) // Gia tri xung la 50 UART_senddata(encodertt, encoderxoay, angle1, { //angle2, 15); digitalWrite(DIR_step, LOW); } // Ket thuc vong lap loop } void Timer1_ISR(void) { else if ((pul_step >= 50) && (pul_step < 100)) if (run_step == 1) { { digitalWrite(DIR_step, HIGH); i++; } if (i == 1) { else if (pul_step >= 100){ pul_step = digitalWrite(PUL_step, HIGH); 0;encoderxoay=0;} } } if (i == 2) { } digitalWrite(PUL_step, LOW); //================================ doc i = 0; pul_step++; flex sensor } float Flex_senssor(uint8_t Flex_pin) } { float angle = 0; } uint32_t tong = 0; //================================ doc // Read the ADC, and calculate voltage and flex sensor resistance from it float Flex_senssor(uint8_t Flex_pin) for (uint8_t i = 0; i < 5; i++) { float angle = 0; { uint32_t tong = 0; tong += (uint32_t)analogRead(Flex_pin); } LUAN VAN CHAT LUONG download : add luanvanchat@agmail.com 168 // Read the ADC, and calculate voltage and //resistance from it uint32_t flexADC = tong / 5; tong = 0; // float flexV = (float)flexADC * VCC / 1023.0; for (uint8_t i = 0; i < 30; i++) // float flexR = R_DIV * (VCC / flexV - 1.0); { // Serial.println("Resistance: " + String(flexR) + " tong += (uint32_t)analogRead(Flex_pin); //ohms"); } // angle = (float)map(flexR, uint32_t flexADC = tong / 5; tong = 0; //STRAIGHT_RESISTANCE, //float flexV = (float)flexADC * VCC / 1023.0; //BEND_RESISTANCE, 0, 9000) / 100; //float flexR = R_DIV * (VCC / flexV - 1.0); return angle= (float)flexADC; // Serial.println("Resistance: " + String(flexR) + " } //ohms"); //angle = (float)map(flexR, //=============================== interrupt //TRAIGHT_RESISTANCE, void read_Encoder1(void) //BEND_RESISTANCE, 0, 90); { angle = (float)flexADC; if (digitalRead(encoder0PinA) == 1) { return angle; encoderxoay++; } } else { //=============================== encoderxoay ; interrupt } void read_Encoder1(void) } { //=========================interrupt if (digitalRead(encoder0PinA) == 1) { encoderxoay++; void read_Encoder2(void) { } else { if (digitalRead(encoder0PinB) == 1) { encoderxoay ; encodertt++; } } else { } encodertt ; //=========================interrupt } void read_Encoder2(void) } { // =====================dieu khien dong co if (digitalRead(encoder0PinB) == 1) { encodertt++; } else { encodertt ; } void DCmotor(unsigned char DIR, unsigned char speed_DC) { if (DIR == 1) { } digitalWrite(12, HIGH); // ===================== digitalWrite(13, LOW); analogWrite(10, speed_DC); LUAN VAN CHAT LUONG download : add luanvanchat@agmail.com 169 void DCmotor(unsigned char DIR, unsigned char } speed_DC) if (DIR == 2) { { if (DIR == 1) digitalWrite(12, LOW); { digitalWrite(13, HIGH); digitalWrite(12, HIGH); analogWrite(10, speed_DC); digitalWrite(13, LOW); } analogWrite(10, speed_DC); if (DIR == 0) } { if (DIR == 2) digitalWrite(12, HIGH); { digitalWrite(13, HIGH); digitalWrite(12, LOW); analogWrite(10, 0); digitalWrite(13, HIGH); analogWrite(10, speed_DC); } } } ===================================== if (DIR == 0) ===== { void UART_senddata(int32_t encoder1, int32_t digitalWrite(12, HIGH); encoder2, float flex1, float flex2, uint32_t digitalWrite(13, HIGH); timer_send) analogWrite(10, 0); { } static uint32_t timer_buff = millis(); } if ((millis() - timer_buff) > timer_send) ====================================== { === Xuat du lieu sang Labview dkSerial.print("a"); void UART_senddata(int32_t encoder1, int32_t dkSerial.print( flex1); encoder2, float flex1, float flex2, uint32_t dkSerial.print("b"); timer_send) dkSerial.print("c"); { dkSerial.print( encoder1 + 10000); static uint32_t timer_buff = millis(); dkSerial.print("d"); if ((millis() - timer_buff) > timer_send) dkSerial.print("e"); { dkSerial.print( encoder2 + 3000); dkSerial.print("a"); dkSerial.print("f"); dkSerial.print( flex1); dkSerial.print("g"); dkSerial.print("b"); dkSerial.print( flex2); dkSerial.print("c"); dkSerial.print("h"); dkSerial.print( encoder1 + 10000); timer_buff = millis(); dkSerial.print("d"); } dkSerial.print("e"); } LUAN VAN CHAT LUONG download : add luanvanchat@agmail.com 170 dkSerial.print( encoder2 + 3000); dkSerial.print("f"); dkSerial.print("g"); dkSerial.print( flex2); dkSerial.print("h"); timer_buff = millis(); } } LUAN VAN CHAT LUONG download : add luanvanchat@agmail.com 10 11 12 13 14 15 16 17 18 B NG K STT S l Gi c Gi 4 Gi g n tr c tr n Th p C45 Gi g n tr c v t me Th p C45 Tr c tr n Th p C45 Tr c v t me Th p C45 D nh Th p C45 Gi d i kh p t nh ti n 10 Gi d i encoder v 11 Tr c n i 12 B cc 13 T m gi 14 Kh u nh i 15 G c nh tr c 16 Gi 17 18 Nh a ABS nh ng c Th p C45 ng c DC ng kh p t nh ti n Nh a ABS ng c Th p C45 Th p C45 Th p C45 nh encoder Nh a ABS ng Th p C45 ng c b Th p C45 ng c tr n encoder v ng c Th p C45 c NEMA 17 Encoder DOCTORAL DISSERTATION V t li u T n chi ti t ng No Qty Document No Signature Date Designer Duong Xuan Bien Inspector Duong Xuan Bien FLEXIBLE ROBOT TYPE IV Stamp Sheet: Mass Scale Sheet No: MILITARY TECHNICAL ACADEMY LUAN VAN CHAT LUONG download : add luanvanchat@agmail.com ... of planar flexible robots based multi-bodies dynamic, mechanically deformed body, finite element theory, control and numerical computation method The results of this research are referenced in... position of planar flexible robots The control law is determined and stably proved based on Lyapunov’s theory The parameters of controller are found by using genetic algorithm - A flexible robot is designed... evaluate results of calculations The contents can be shown as Fig 0.1 Methodology The researching theory, numerical calculation and experimental method are used to execute the contents of dissertation