Khảo sát động lực học và điểu khiển vị trí của tay máy hai khâu đàn hồi phẳng theo phương pháp phần tử hữu hạn

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 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 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 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 LIST OF SYMBOLS AND ABBREVIATIONS L l , i e Length of link i , length of each element of link i i Angle between link i −1 and link i i Number of links of robot, number of elements of link i ) and joint variable of link i n, n , Arbitrary point on the element j of link i (t i i x (x ), m m = Shape functions of element j Elastic displacement at arbitrary point on element j of link i wij (t, x) u Flexural displacement, slope displacement of node j and u node j + of element j of link i , respectively u Flexural and slope displacement at end point of link i Flexural and slope displacement at node k u k + of element k of link i −1 u u(i −1) f ,u(i −1)s H r,r ij 0ij r ,r 02r 02f and node Flexural and slope displacement at the end point of link i −1 transforms from the coordinate system Oi XYii coordinate system Oi −1X i −1Yi −1 Position vector of arbitrary point on the element General d (t ), (t ) i i q Position vector of the end point of link in cases of rigid and flexible models in the coordinate system O0X 0Y0 C(q, q) Q F (t), i * q e KP,KI ,KD m,m Tij ,Ti ,T T,T,T P ije M M K j of link i in the coordinate systems Oi XYii and O0X 0Y0 homogeneous transformation matrix which ie to the id p Kinetic energy of element j of link i , kinetic energy of 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 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 energy of element j of link i , potential energy of link i and the system Mass matrix of element j , link i and system Mass matrices of the motor and the tip load Stiffness matrix of element j , link i and system Coriolis matrix Generalized force/torque vector of the system Driving force, torque at the joint i Joint variable error vector, error vector in objective function and Lyapunov function Cross matrix of control parameters in PID controller 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 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 LIST OF TABLES Table The parameters  i , u ( i  1)s , u (i 1)f , i ,ai depending on types of joints Table Table Table 3 The maximum elastic displacements at the ending points of the links The dy The m Table Table Table Table Table Table Table Table Table 10 11 12 The pa The len The ma The pa The pa The pa 159 function %#codegen [N,L2,L1,E,I,le,m,Jh,mt,m1,Ih] = etinfo(); %% M=zeros(14,14); M(1,1)=0.13e2 / m * x(6) le * m * x(8) * 0.210e3 + 0.13e2 x(8) * m * x(7) x(10) * 0.210e3 + 0.13e2 x(10) * * m * x(9) x(12) * 0.210e3 x(14) + m * x(12) * x(13) 0.105e3 x(14) + x(11) * x(6) / le ^ 0.210e3 + 0.13e2 x(4) * le * m m + 0.9e1 / 0.35e2 * * x(5) 0.70e2 * m * x(5) *L1/ le * m + 0.25e2 * m * x(4) 0.105e3 0.5e1 * / 0.35e2 0.26e2 + 0.2e1 x(8) ^ * x(7) * m * * le * m 0.105e3 0.26e2 + le ^ 0.105e3 * x(13) m + 0.5e1 * L2 0.125e3 0.25e2 x_d2dot = * x(10) / * le ^ / * * le ^ / * le ^ * le ^ * m * x(3) * le ^ x(5) + 0.10e2 - / le ^ 0.2e1 - Jh + L1 * m * le * * le ^ * le * / ^ + * / * le ^ + 0.13e2 / 0.35e2 * * m * le 160 m * le ^ 0.2e1 + x(13) ^ x(2) ^ * M(1,2)=-le - le * m * x(5) - le * m * x(9) - le * m * * x(14) / 0.12e2 - le * m * x(13) / 0.2e1 - 0.2e1 * M(1,3)=-(m x(2) - * M(1,4)=-(m x(2) - * M(1,5)= -m x(2) + m * M(1,6)=m * M(1,7)=-m * le * L2 + m * le * x(2) + * m * le ^ 2; M(1,8)=m * M(1,9)=-m * le * L2 + m * le * x(2) + * m * le ^ 2; M(1,10)= m M(1,11)=-m x(2) + * M(1,12)= m M(1,13)=m * le * L1 / 0.2e1 - m * le * L2 / 0.2e1 0.2e1 + 0.47e2 / 0.20e2 * m * le ^ + mt * L1 + mt * x(2) / 0.2e1; M(1,14)=(m x(2) - 23 * le)) / 0.60e2; for f=1:14 M(f,1)=M(1,f); end; % M(2,2)=(5 * m * % M(3,3)=0.13e2 / M(3,4)=0.11e2 / 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 * M(4,5)=0.13e2 / 2; M(4,6)=-m * le ^ / 0.140e3; for f=4:14 M(f,4)=M(4,f); end; % M(5,5)=0.26e2 / M(5,6)=0; 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; 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) - 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); 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 + P4.3 Declare the objective function (file: Jmuctieu) function [Jmuctieu] [N,~]=size(x);% );% Nx6 Jmuctieu=zeros(N,1 global Kp1 Ki1 Kd1 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); P4.4 GA algorithm codes global Kp1 Ki1 Kd1 Kp2 Ki2 Kd2; ff='Jmuctieu'; % npar=6; LB=[000000]; 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 0) {temp_old = temp; temp = Serial.read(); / } if (temp == '1') { DCmotor(1, 255); // Thuan Dieu khien dong co DC bang tay run_step = 0; } if (temp == '1') { DCmotor(1, 255); // Nhan Thuan } if (temp == '2') { DCmotor(2, 255); // Nguoc run_step = 0; run_step = 0; } if (temp == '2') { DCmotor(2, 255); // Nhan } if (temp == ' ') { DCmotor(0, 0); Nguoc run_step = 0; run_step = 0; } if (temp == ' ') { // Nhan dau Space de dung lai DCmotor(0, 0); } if (temp == 'a') { run_step = 1; run_step = 0; if ( temp != 'a') { } timer = millis(); // Lenh chay cac dong co cho bai DLH thuan } if (temp == 'a') { // Nhan 'a' de chay dong thoi if ((millis() - timer ) < 500) // 0.5s //dong co { timer =millis(); DCmotor(1, 255); // Thuan temp = 0; } run_auto=1; } else if (((millis() - timer ) >= 500) && ((millis() - if(run_auto==1) timer ) < 1000)) { { / DCmotor(2, 255); // Nguoc Dong co Step if (pul_step < 50) // Gia tri xung } / else if ((millis() - timer ) >= 1000) { 50 xung ung voi 0.16s timer = millis();encodertt=0; { run_step=1; } digitalWrite(DIR_step, LOW); } } else {run_step=0;} float angle1 = Flex_senssor(FLEX1); // Dong co DC float angle2 = Flex_senssor(FLEX2); 167 { float angle = 0; uint32_t if ((millis() - timer ) < 500) { DCmotor(1, 255); // Chay thuan dong co //DC muc cao 12V digitalWrite(DIR_step, LOW); } else / Stop cac dong co { run_auto=0; DCmotor(0, 0); pul_step=0; } } float angle1 = Flex_senssor(FLEX1); float angle2 = Flex_senssor(FLEX2); UART_senddata(encodertt, encoderxoay, angle1, //angle2, 15); } // Ket thuc vong lap loop void Timer1_ISR(void) { if (run_step == 1) { i++; if (i == 1) { digitalWrite(PUL_step, HIGH); } if (i == 2) { digitalWrite(PUL_step, LOW); i = 0; pul_step++; } } } //================================ doc flex sensor float Flex_senssor(uint8_t Flex_pin) tong = 0; } else if ((pul_step >= 50) && (pul_step < 100)) { UART_senddata(encoderxoay, encodertt, angle1, angle2, digitalWrite(DIR_step, HIGH); 35); } // Vong lap cho dong co step } void Timer1_ISR(void) { if (run_step == 1) { i++; if (i == 1) { digitalWrite(PUL_step, HIGH); } if (i == 2) { digitalWrite(PUL_step, LOW); i = 0; pul_step++; } if (pul_step < 50) // Gia tri xung la 50 { digitalWrite(DIR_step, LOW); else if (pul_step >= 100){ pul_step = 0;encoderxoay=0;} } } //================================ doc flex sensor float Flex_senssor(uint8_t Flex_pin) { float angle = 0; uint32_t tong = 0; / Read the ADC, and calculate voltage and resistance from it for (uint8_t i = 0; i < 5; i++) { tong += (uint32_t)analogRead(Flex_pin); } 168 / / Read the ADC, and calculate voltage and //resistance from it for (uint8_t i = 0; i < 30; i++) { tong += (uint32_t)analogRead(Flex_pin); } uint32_t flexADC = tong / 5; tong = 0; //float flexV = (float)flexADC * VCC / 1023.0; //float flexR = R_DIV * (VCC / flexV 1.0); / Serial.println("Resistance: " + String(flexR) + " //ohms"); //angle = (float)map(flexR, //TRAIGHT_RESISTANCE, //BEND_RESISTANCE, 0, 90); angle = (float)flexADC; return angle; } //=============================== interrupt void read_Encoder1(void) { if (digitalRead(encoder0PinA) == 1) { encoderxoay++; } else { encoderxoay-; } } //=========================interrupt void read_Encoder2(void) { if (digitalRead(encoder0PinB) == 1) { encodertt++; } else { encodertt-; } } ===================== } //=========================interrupt uint32_t flexADC = tong / 5; tong = 0; / float flexV = (float)flexADC * VCC / 1023.0; / float flexR = R_DIV * (VCC / flexV - 1.0); / void read_Encoder2(void) { if (digitalRead(encoder0PinB) == 1) { encodertt++; Serial.println("Resistance: " + String(flexR) } else + " //ohms"); / { encodertt angle = (float)map(flexR, ; //STRAIGHT_RESISTANCE, //BEND_RESISTANCE, 0, 9000) / 100; return angle= (float)flexADC; } //=============================== interrupt void read_Encoder1(void) { if (digitalRead(encoder0PinA) == 1) { encoderxoay++; } else { encoderxoay ; } } } / =====================dieu khien dong co void DCmotor(unsigned char DIR, unsigned char speed_DC) { if (DIR == 1) { digitalWrite(12, HIGH); digitalWrite(13, LOW); analogWrite(10, speed_DC); 169 void DCmotor(unsigned char DIR, unsigned char } speed_DC) if (DIR == 2) { { if (DIR == 1) digitalWrite(12, LOW); { digitalWrite(13, HIGH); analogWrite(10, speed_DC); digitalWrite(12, HIGH); digitalWrite(13, LOW); } analogWrite(10, speed_DC); if (DIR == 0) { } if (DIR == 2) digitalWrite(12, HIGH); { digitalWrite(13, HIGH); analogWrite(10, 0); digitalWrite(12, LOW); } 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"); } 170 dkSerial.print( encoder2 + 3000); dkSerial.print("f"); dkSerial.print("g"); dkSerial.print( flex2); dkSerial.print("h"); timer_buff = millis(); } } 10 11 12 13 14 ... 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