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 Length of link i , length of each element of link i ie i Angle between link i −1 and link i n, n , Number of links of robot, number of elements of link i ) and joint variable of link i (t i Arbitrary point on the element j of link i i =1 x (x ), m Shape functions of element j Elastic displacement at arbitrary point on element j m of link i wij (t, x) , u u i (2 j −1) i (2 j ) i (2 j + 1) i (2 j +2) , u u (i − 1)(2 k − 1) (i − 1)(2 k + 1) Flexural and slope displacement at node k , u ,u H if(i −1) r,r 0ij r ,r 02r 02f k + of element k of link i −1 , (i−1)(2 k ) (i −1)(2 k +2) u(i −1) f ,u(i −1)s ij Flexural and slope displacement at end point of link i + 1) i (2n i +2) i (2 n i u node j + of element j of link i , respectively , u u u Flexural displacement, slope displacement of node j and , and node Flexural and slope displacement at the end point of link transforms from the coordinate system Oi XYii i −1 Position vector of arbitrary point on the element General coordinate system Oi −1X i −1Yi −1 i Position vector of the end point of link in cases of rigid and flexible models in the coordinate system O0X 0Y0 Q ex (t ) d (t ), (t ) i F (t), (t ) i q ijcv (t ), q icv (t ) i * e (t ), e(t ),V q (t ), q (t ), q(t ) ij i m , m ,m dci t Tij ,Ti ,T T,T,T ie id p P , P ,P,P ije ijg i M , M, M j i M ,M dc tt K ,K ,K j j of link i in the coordinate systems Oi XYii and O0X 0Y0 homogeneous transformation matrix which i to the i C(q, q) KP,KI ,KD 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 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 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) 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 % tinh gia tri ham muc tieu [xrow,~]=size(e13); tf=length(xrow); t= 0:0.01:tf; global Kp1 Ki1 Kd1 Kp2 Ki2 Kd2 t ; Jmt=0; 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=[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= 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;} } // Dong co DC float angle1 = Flex_senssor(FLEX1); 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 } else if (pul_step >= 100){ pul_step = 0;encoderxoay=0;} void Timer1_ISR(void) { } if (run_step == 1) } { //================================ i++; doc flex sensor if (i == 1) { float Flex_senssor(uint8_t Flex_pin) digitalWrite(PUL_step, HIGH); } { float angle = 0; uint32_t tong = 0; if (i == 2) { / Read the ADC, and digitalWrite(PUL_step, LOW); calculate voltage and i = 0; pul_step++; resistance from it } for (uint8_t i = 0; i < 5; i++) if (pul_step < 50) // Gia tri xung la 50 { { tong += (uint32_t)analogRead(Flex_pin); digitalWrite(DIR_step, LOW); } 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; void read_Encoder2(void) { if (digitalRead(encoder0PinB) == 1) / float flexR = R_DIV * (VCC / flexV - 1.0); { encodertt++; / Serial.println("Resistance: " + String(flexR) + " } else //ohms"); { encodertt / angle = (float)map(flexR, ; //STRAIGHT_RESISTANCE, //BEND_RESISTANCE, 0, 9000) / 100; return angle= (float)flexADC; } } } / u khien dong co void DCmotor(unsigned //=============================== interrupt void read_Encoder1(void) { if (digitalRead(encoder0PinA) == 1) { encoderxoay++; } else { encoderxoay ; } =====================die 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 15 16 17 18 STT Số lượng 1 Giá cố định Nhựa ABS Giá đỡ động Thép C45 Động DC 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ẫn hướng khớp tịnh tiến Thép C45 Giá đỡ khớp tịnh tiến Nhựa ABS 10 Giá đỡ encoder động Thép C45 11 Trục nối Thép C45 12 Bạc cố định encoder Thép C45 13 Tấm giá đứng Nhựa ABS 14 Khâu đàn hồi Thép C45 15 Gá cố định trục động Thép C45 16 Giá đỡ encoder động Thép C45 17 Động bước NEMA 17 18 Encoder No Qty Design er Inspect or BẢNG KÊ Document No Signature Date Duong Xuan Bien Tên chi tiết Vật liệu DOCTORAL DISSERTATION FLEXIBLEROB OT TYPEIV Stamp Mass Scale Sheet: Sheet No: Duong Xuan Bien MILITARY TECHNICAL ACADEMY ... 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