Thiết kế bộ điều khiển PID trượt cánh tay mềm dẻo hai bậc tự do

Đại Học Quốc Gia Tp Hồ Chí Minh TRƯỜNG ĐẠI HỌC BÁCH KHOA PHẠM VĂN TRỌNG THIẾT KẾ BỘ ĐIỀU KHIỂN PID TRƯỢT CÁNH TAY MỀM DẺO BẬC TỰ DO Chuyên ngành: Tự động hóa LUẬN VĂN THẠC SĨ TP HỒ CHÍ MINH, tháng 07 năm 2010 CƠNG TRÌNH ĐƯỢC HOÀN THÀNH TẠI TRƯỜNG ĐẠI HỌC BÁCH KHOA ĐẠI HỌC QUỐC GIA TP HỒ CHÍ MINH Cán hướng dẫn khoa học:…………………………………………………………… Cán chấm nhận xét 1………………………………………………………………… Cán chấm nhận xét 2………………………………………………………………… Luận văn thạc sĩ bảo vệ HỘI ĐỒNG CHẤM BẢO VỆ LUẬN VĂN THẠC SĨ TRƯỜNG ĐẠI HỌC BÁCH KHOA, ngày … tháng … năm 2010 TRƯỜNG ĐẠI HỌC BÁCH KHOA PHÒNG ĐÀO TẠO SĐH CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM ĐỘC LẬP - TỰ DO - HẠNH PHÚC Tp HCM, ngày … tháng … năm 2010 NHIỆM VỤ LUẬN VĂN THẠC SĨ Họ tên học viên: PHẠM VĂN TRỌNG Phái: Ngày, tháng, năm sinh: 18 – – 1982 Nơi sinh: Gia Lai Chuyên ngành: MSHV: Tự động hóa Nam 01506380 I TÊN ĐỀ TÀI: THIẾT KẾ BỘ ĐIỀU KHIỂN PID TRƯỢT CÁNH TAY MỀM DẺO BẬC TỰ DO II NHIỆM VỤ VÀ NỘI DUNG: Thiết kế điều khiển PID trượt Lập trình mơ hệ cánh tay máy mềm dẻo Simulink file_M Matlab điều khiển mơ mơ hình tốn điều khiển thực mơ hình Kiểm chứng giải thuật so sánh điều khiển mơ hình thực mơ hình tốn III NGÀY GIAO NHIỆM VỤ : 25 / 06 / 2009 IV NGÀY HOÀN THÀNH NHIỆM VỤ:…… /………/ 2010 V CÁN BỘ HƯỚNG DẪN: PGS.TSKH HỒ ĐẮC LỘC CÁN BỘ HƯỚNG DẪN CN BỘ MÔN QL CHUYÊN NGÀNH PGS.TSKH Hồ Đắc Lộc TS Nguyễn Đức Thành Nội dung đề cương luận văn thạc sĩ Hội đồng chuyên ngành thông qua Ngày … tháng … năm 2010 TRƯỞNG PHÒNG ĐT – SĐH TRƯỞNG KHOA QL NGÀNH TĨM TẮT Các robot thơng thường thiết kế với cánh tay máy đủ cứng (độ biến dạng khơng đáng kể hoạt động) cho phép điều khiển đơn giản với khớp nên kích thước trọng lượng lớn Điều làm cho robot hoạt động tiêu tốn lượng đồng thời tốc độ hoạt động khơng cao có qn tính lớn Robot với cánh tay mềm dẻo phát triển để khắc phục nhược điểm trên, cho phép hoạt động với tốc độ cao hiệu suất kích thước khối lượng nhỏ Tuy vậy, để điều khiển cánh tay mềm dẻo phức tạp nên chưa sử dụng nhiều thực tế Luận văn đưa điều khiển PID có cấu trúc thay đổi mặt trượt PID Bộ điều khiển thừa hưởng ưu điểm phương pháp điều khiển PID triệt tiêu sai số xác lập, đáp ứng nhanh, giảm vọt lố, kết hợp điều khiển có cấu trúc thay đổi mặt trượt PID để điều khiển góc quay cánh tay mềm dẻo Luận văn thạc sĩ i MỤC LỤC Chương 1: Tổng quan Chương 2: Cơ sở lý thuyết 2.1 Bộ điều khiển PID 2.2 Bộ điều khiển trượt Chương 3: Thiết kế điều khiển .12 3.1 Khảo sát mơ hình cánh tay mềm dẻo bậc tự 12 3.1.1 Phương trình động học 15 3.1.1.1 Phương trình động học thuận 15 3.1.1.2 Phương trình động học ngược .17 3.1.2 Phương trình động lực học .18 3.2 Bộ điều khiển PID trượt 35 3.2.1 Điều kiện trượt .36 3.2.2 Ổn định tiệm cận toàn cục hệ cánh tay mềm dẻo bậc tự 38 Chương 4: Mô hệ thống điều khiển dùng Matlab 47 4.1 Mô cánh tay mềm dẻo bậc tự 47 4.1.1 Mô cánh tay mềm dẻo với trục X .48 4.1.1.1 Sơ đồ Simulink 49 4.1.1.2 Kết mô .51 Luận văn thạc sĩ ii 4.1.2 Mô cánh tay mềm dẻo với trục Y .60 4.1.2.1 Sơ đồ Simulink 60 4.1.2.2 Kết mô .62 4.2 Mô cánh tay mềm dẻo bậc tự 72 4.2.1 Sơ đồ Simulink 72 4.2.2 Kết mô 73 Chương 5: Kết luận hướng phát triển đề tài 77 Tài liệu tham khảo .78 Phụ lục .81 Luận văn thạc sĩ CHƯƠNG TỔNG QUAN Đất nước ta q trình cơng nghiệp hóa đại hóa, việc nghiên cứu thiết kế chế tạo robot cơng nghiệp để ứng dụng vào sản xuất có ý nghĩa quan trọng Việc tự động hoá trình sản xuất với có mặt robot làm tăng khả mềm dẻo hệ thống sản xuất, tăng chất lượng sản phẩm đặc biệt làm giảm giá thành sản phẩm để tăng tính cạnh tranh Ngồi Robot cơng nghiệp cịn có tính quan trọng khác làm việc mơi trường khắc nghiệt mà người tham gia vào như: mơi trường nhiều khói bụi, mơi trường độc hại hố chất, mơi trường nhiệt độ cao Hiện giới có nhiều nghiên cứu robot có nhiều robot chế tạo ứng dụng vào trình sản xuất robot hàn nhà máy sản xuất ô tô, robot lắp ráp linh kiện dây chuyền sản xuất board mạch, robot cấp phôi máy gia cơng chi tiết khí Tuy nhiên, Việt Nam việc nghiên cứu chế tạo robot giai đoạn bắt đầu robot chưa có tính thích ứng với mơi trường thay đổi mà chủ yếu hoạt động theo chương trình định trước Việc nghiên cứu điều khiển để nâng cao độ xác robot cịn nhà khoa học nước quan tâm nhiều Tay máy mềm dẻo đối tượng có độ phi tuyến cao việc thiết kế điều khiển cho đối tượng phức tạp Như biết, điều khiển với cấu trúc thay đổi (sliding mode control) phương pháp hữu hiệu để điều Chương Luận văn thạc sĩ khiển cho đối tượng phi tuyến luật điều khiển hồi tiếp phi tuyến Trong năm gần đây, hệ thống điều khiển với cấu trúc thay đổi ứng dụng rộng rãi để ổn định hoá cho chuyển động robot Luận văn giới thiệu điều khiển PID có cấy trúc thay đổi kết hợp điều khiển có cấu trúc thay đổi mặt trượt PID để điều khiển cho góc quay cánh tay mềm dẻo bậc tự bám theo góc đặt Điều kiện tồn mặt trượt tính ổn định tiệm cận tồn cục hệ thống thiết lập dạng toàn phương hàm Lyapunov Tính khả thi điều khiển kiểm chứng thông qua kết mô phần mềm Matlab Luận văn chia làm chương với nội dung sau: + Chương 1: Tổng quan Nội dung chương trình bày tổng quan hệ thống robot ứng dụng cơng nghiệp đồng thời giới thiệu sơ lược điều khiển thiết kế luận văn + Chương 2: sở lý thuyết Nội dung chương trình bày lý thuyết sở điều khiển PID điều khiển trượt + Chương 3: Thiết kế điều khiển PID trượt Nội dung chương trình bày điều khiển PID trượt kết hợp với mặt trượt PID, điều kiện tồn mặt trượt điều kiện ổn định hệ thống + Chương 4: Mô hệ thống điều khiển môi trường Matlab Chương Luận văn thạc sĩ Nội dung chương trình bày sơ đồ mô cánh tay mềm dẻo bậc tự điều khiển điều khiển PID trượt thiết kế chương dùng simulink kết mô Chương 5: Kết luận hướng phát triển Chương Luận văn thạc sĩ CHƯƠNG CƠ SỞ LÝ THUYẾT 2.1 Bộ điều khiển PID: Bộ điều khiển PID sử dụng rộng rãi thực tế có khả làm triệt tiêu sai số xác lập, tăng tốc độ đáp ứng độ, giảm vọt lố thông số điều khiển chọn lựa thích hợp Do tính thơng dụng nên nhiều hãng sản xuất thiết bị điều khiển cho đời điều khiển PID thương mại tiện dụng Với điều khiển PID, ta tích hợp luật điều khiển khác điều khiển tỉ lệ P, điều khiển PI, điều khiển PD Sự có mặt khâu PID vịng hồi tiếp dẫn đến dao động hệ thống đáp ứng độ bị vọt lố hàm dirac  (t ) Các hiệu chỉnh PID ứng dụng nhiều công nghiệp dạng thiết bị điều khiển hay thuật toán phần mềm Hàm truyền hiệu chỉnh PID có dạng : G (s)  K P  K I s    KD  K P 1   TD s  s  T1s   2.1 K P , K I , K D số thực Phương trình vi tích phân mơ tả mối tương quan tín hiệu u(t ) với tín hiệu vào e(t ) điều khiển PID : u (t )  K P e(t )  K I  e(t )dt  K D de(t ) dt  2.2  r (t ) tín hiệu đặt y(t ) đáp ứng đầu hệ thống e(t )  r (t )  y(t ) sai lệch tín hiệu đặt ngõ Vấn đề thiết kế cần xác định giá trị K P , K I , K D cho hệ thỏa mãn yêu cầu chất lượng Chương Luận văn thạc sĩ 82 #include "math.h" /* Error handling * -* * You should use the following technique to report errors encountered within * an S-function: * * ssSetErrorStatus(S,"Error encountered due to "); * return; * * Note that the 2nd argument to ssSetErrorStatus must be persistent memory * It cannot be a local variable For example the following will cause * unpredictable errors: * * mdlOutputs() * { * char msg[256]; * sprintf(msg,"Error due to %s", string); * ssSetErrorStatus(S,msg); * return; * {ILLEGAL: to fix use "static char msg[256];"} } * * See matlabroot/simulink/src/sfunctmpl.doc for more details */ /*====================* Luận văn thạc sĩ 83 * S-function methods * *====================*/ /* Function: mdlInitializeSizes =============================================== * Abstract: * The sizes information is used by Simulink to determine the S-function * block's characteristics (number of inputs, outputs, states, etc.) */ static void mdlInitializeSizes(SimStruct *S) { /* See sfuntmpl.doc for more details on the macros below */ ssSetNumSFcnParams(S, 0); /* Number of expected parameters */ if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) { /* Return if number of expected != number of actual parameters */ return; } ssSetNumContStates(S, 0); ssSetNumDiscStates(S, 0); if (!ssSetNumInputPorts(S, 2)) return; ssSetInputPortWidth(S, 0, 1); ssSetInputPortWidth(S, 1, 1); ssSetInputPortDirectFeedThrough(S, 0, 1); ssSetInputPortDirectFeedThrough(S, 1, 1); if (!ssSetNumOutputPorts(S, 2)) return; Luận văn thạc sĩ 84 ssSetOutputPortWidth(S, 0,1); ssSetOutputPortWidth(S, 1,1); ssSetNumSampleTimes(S, 1); ssSetNumRWork(S, 0); ssSetNumIWork(S, 0); ssSetNumPWork(S, 0); ssSetNumModes(S, 0); ssSetNumNonsampledZCs(S, 0); ssSetOptions(S, 0); } /* Function: mdlInitializeSampleTimes ========================================= * Abstract: * This function is used to specify the sample time(s) for your * S-function You must register the same number of sample times as * specified in ssSetNumSampleTimes */ static void mdlInitializeSampleTimes(SimStruct *S) { ssSetSampleTime(S, 0, CONTINUOUS_SAMPLE_TIME); ssSetOffsetTime(S, 0, 0.0); Luận văn thạc sĩ 85 } #define MDL_INITIALIZE_CONDITIONS /* Change to #undef to remove function */ #if defined(MDL_INITIALIZE_CONDITIONS) /* Function: mdlInitializeConditions ======================================== * Abstract: * In this function, you should initialize the continuous and discrete * states for your S-function block The initial states are placed * in the state vector, ssGetContStates(S) or ssGetRealDiscStates(S) * You can also perform any other initialization activities that your * S-function may require Note, this routine will be called at the * start of simulation and if it is present in an enabled subsystem * configured to reset states, it will be call when the enabled subsystem * restarts execution to reset the states */ static void mdlInitializeConditions(SimStruct *S) { } #endif /* MDL_INITIALIZE_CONDITIONS */ #define MDL_START /* Change to #undef to remove function */ #if defined(MDL_START) /* Function: mdlStart ======================================================= * Abstract: * This function is called once at start of model execution If you * have states that should be initialized once, this is the place Luận văn thạc sĩ * 86 to it */ static void mdlStart(SimStruct *S) { } #endif /* MDL_START */ /* Function: mdlOutputs ======================================================= * Abstract: * In this function, you compute the outputs of your S-function * block Generally outputs are placed in the output vector, ssGetY(S) */ static void mdlOutputs(SimStruct *S, int_T tid) { real_T *y1 = ssGetOutputPortRealSignal(S,0); real_T *y2 = ssGetOutputPortRealSignal(S,1); InputRealPtrsType uPtrs1 = ssGetInputPortRealSignalPtrs(S,0); InputRealPtrsType uPtrs2 = ssGetInputPortRealSignalPtrs(S,1); double Px,Py,l,pi,t_x,t_y; l = 10.0; pi = 3.14159; t_x = *uPtrs1[0]; t_y = *uPtrs2[0]; Px = 2*cos(t_x)*l*cos(t_y)+2*sin(t_x)*l*sin(t_y)+l*sin(t_x) ; Py = -2*sin(t_x)*l*cos(t_y)+2*cos(t_x)*l*sin(t_y)+l*cos(t_x) ; Luận văn thạc sĩ 87 *y1 = Px; *y2 = Py; } #define MDL_UPDATE /* Change to #undef to remove function */ #if defined(MDL_UPDATE) /* Function: mdlUpdate ====================================================== * Abstract: * This function is called once for every major integration time step * Discrete states are typically updated here, but this function is useful * for performing any tasks that should only take place once per * integration step */ static void mdlUpdate(SimStruct *S, int_T tid) { } #endif /* MDL_UPDATE */ #define MDL_DERIVATIVES /* Change to #undef to remove function */ #if defined(MDL_DERIVATIVES) /* Function: mdlDerivatives ================================================= * Abstract: * In this function, you compute the S-function block's derivatives * The derivatives are placed in the derivative vector, ssGetdX(S) */ static void mdlDerivatives(SimStruct *S) { Luận văn thạc sĩ 88 } #endif /* MDL_DERIVATIVES */ /* Function: mdlTerminate ===================================================== * Abstract: * In this function, you should perform any actions that are necessary * at the termination of a simulation For example, if memory was * allocated in mdlInitializeConditions, this is the place to free it */ static void mdlTerminate(SimStruct *S) { } /*====================================================== * * See sfuntmpl.doc for the optional S-function methods * *======================================================* / /*=============================* * Required S-function trailer * *=============================*/ #ifdef MATLAB_MEX_FILE */ #include "simulink.c" /* Is this file being compiled as a MEX-file? /* MEX-file interface mechanism */ #else #include "cg_sfun.h" #endif /* Code generation registration function */ Luận văn thạc sĩ 89 P.2 Chương trình S-function động học ngược “rob2d_i.c” cho cánh tay mềm dẻo bậc tự do: /* * sfuntmpl.c: C template for a level S-function * * * | See matlabroot/simulink/src/sfuntmpl.doc for a more detailed template | * * * Copyright (c) 1990-1998 by The MathWorks, Inc All Rights Reserved * $Revision: 1.18 $ */ /* * You must specify the S_FUNCTION_NAME as the name of your Sfunction * (i.e replace sfuntmpl with the name of your S-function) */ #define U(element) (*uPtrs[element]) /* Pointer to Input Port0 */ #define S_FUNCTION_LEVEL #define S_FUNCTION_NAME rob2d_i /* * Need to include simstruc.h for the definition of the SimStruct and * its associated macro definitions */ #include "simstruc.h" #include "math.h" Luận văn thạc sĩ 90 /* Error handling * -* * You should use the following technique to report errors encountered within * an S-function: * * ssSetErrorStatus(S,"Error encountered due to "); * return; * * Note that the 2nd argument to ssSetErrorStatus must be persistent memory * It cannot be a local variable For example the following will cause * unpredictable errors: * * mdlOutputs() * { * char msg[256]; * sprintf(msg,"Error due to %s", string); * ssSetErrorStatus(S,msg); * return; * {ILLEGAL: to fix use "static char msg[256];"} } * * See matlabroot/simulink/src/sfunctmpl.doc for more details */ /*====================* * S-function methods * *====================*/ Luận văn thạc sĩ 91 /* Function: mdlInitializeSizes =============================================== * Abstract: * The sizes information is used by Simulink to determine the S-function * block's characteristics (number of inputs, outputs, states, etc.) */ static void mdlInitializeSizes(SimStruct *S) { /* See sfuntmpl.doc for more details on the macros below */ ssSetNumSFcnParams(S, 0); /* Number of expected parameters */ if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) { /* Return if number of expected != number of actual parameters */ return; } ssSetNumContStates(S, 0); ssSetNumDiscStates(S, 0); if (!ssSetNumInputPorts(S, 2)) return; ssSetInputPortWidth(S, 0, 1); ssSetInputPortWidth(S, 1, 1); ssSetInputPortDirectFeedThrough(S, 0, 1); ssSetInputPortDirectFeedThrough(S, 1, 1); if (!ssSetNumOutputPorts(S, 2)) return; ssSetOutputPortWidth(S, 0,1); ssSetOutputPortWidth(S, 1,1); Luận văn thạc sĩ 92 ssSetNumSampleTimes(S, 1); ssSetNumRWork(S, 0); ssSetNumIWork(S, 0); ssSetNumPWork(S, 0); ssSetNumModes(S, 0); ssSetNumNonsampledZCs(S, 0); ssSetOptions(S, 0); } /* Function: mdlInitializeSampleTimes ========================================= * Abstract: * This function is used to specify the sample time(s) for your * S-function You must register the same number of sample times as * specified in ssSetNumSampleTimes */ static void mdlInitializeSampleTimes(SimStruct *S) { ssSetSampleTime(S, 0, CONTINUOUS_SAMPLE_TIME); ssSetOffsetTime(S, 0, 0.0); } #define MDL_INITIALIZE_CONDITIONS /* Change to #undef to remove function */ #if defined(MDL_INITIALIZE_CONDITIONS) /* Function: mdlInitializeConditions ======================================== * Abstract: * In this function, you should initialize the continuous and discrete Luận văn thạc sĩ 93 * states for your S-function block The initial states are placed * in the state vector, ssGetContStates(S) or ssGetRealDiscStates(S) * You can also perform any other initialization activities that your * S-function may require Note, this routine will be called at the * start of simulation and if it is present in an enabled subsystem * configured to reset states, it will be call when the enabled subsystem * restarts execution to reset the states */ static void mdlInitializeConditions(SimStruct *S) { } #endif /* MDL_INITIALIZE_CONDITIONS */ #define MDL_START /* Change to #undef to remove function */ #if defined(MDL_START) /* Function: mdlStart ======================================================= * Abstract: * This function is called once at start of model execution If you * have states that should be initialized once, this is the place * to it */ static void mdlStart(SimStruct *S) { } #endif /* MDL_START */ Luận văn thạc sĩ 94 /* Function: mdlOutputs ======================================================= * Abstract: * In this function, you compute the outputs of your S-function * block Generally outputs are placed in the output vector, ssGetY(S) */ static void mdlOutputs(SimStruct *S, int_T tid) { real_T *y1 = ssGetOutputPortRealSignal(S,0); real_T *y2 = ssGetOutputPortRealSignal(S,1); InputRealPtrsType uPtrs1 = ssGetInputPortRealSignalPtrs(S,0); InputRealPtrsType uPtrs2 = ssGetInputPortRealSignalPtrs(S,1); double pi,l,X,Y,t_y,t_x,a,b; X = *uPtrs1[0]; Y = *uPtrs2[0]; l = 10.0; pi = 3.14159; t_y = asin((X*X+Y*Y-5*l*l)/l/l/4); a = 4*l*sin(t_y)+2*l; b = 4*l*l*sin(t_y)*sin(t_y)+4*l*l*sin(t_y)+l*l-X*X+4*l*l*cos(t_y)*cos(t_y); t_x = 2*atan((a-2*sqrt(b))/2/(X+2*l*cos(t_y))); *y1 = t_x; *y2 = t_y; } Luận văn thạc sĩ 95 #define MDL_UPDATE /* Change to #undef to remove function */ #if defined(MDL_UPDATE) /* Function: mdlUpdate ====================================================== * Abstract: * This function is called once for every major integration time step * Discrete states are typically updated here, but this function is useful * for performing any tasks that should only take place once per * integration step */ static void mdlUpdate(SimStruct *S, int_T tid) { } #endif /* MDL_UPDATE */ #define MDL_DERIVATIVES /* Change to #undef to remove function */ #if defined(MDL_DERIVATIVES) /* Function: mdlDerivatives ================================================= * Abstract: * In this function, you compute the S-function block's derivatives * The derivatives are placed in the derivative vector, ssGetdX(S) */ static void mdlDerivatives(SimStruct *S) { Luận văn thạc sĩ 96 } #endif /* MDL_DERIVATIVES */ /* Function: mdlTerminate ===================================================== * Abstract: * In this function, you should perform any actions that are necessary * at the termination of a simulation For example, if memory was * allocated in mdlInitializeConditions, this is the place to free it */ static void mdlTerminate(SimStruct *S) { } /*====================================================== * * See sfuntmpl.doc for the optional S-function methods * *======================================================* / /*=============================* * Required S-function trailer * *=============================*/ #ifdef MATLAB_MEX_FILE */ #include "simulink.c" /* Is this file being compiled as a MEX-file? /* MEX-file interface mechanism */ #else #include "cg_sfun.h" #endif /* Code generation registration function */ ... TÀI: THIẾT KẾ BỘ ĐIỀU KHIỂN PID TRƯỢT CÁNH TAY MỀM DẺO BẬC TỰ DO II NHIỆM VỤ VÀ NỘI DUNG: Thiết kế điều khiển PID trượt Lập trình mô hệ cánh tay máy mềm dẻo Simulink file_M Matlab điều khiển. .. chuẩn Lyapunov 3.1 Khảo sát mơ hình cánh tay mềm dẻo bậc tự do: Cánh tay mềm dẻo mà khảo sát cánh tay mềm dẻo bậc tự hình 3.1 Hình 3.1: Cánh tay mềm dẻo bậc tự Với tham số cho bảng 3.1 sau: Chương... tượng SISO Chương Luận văn thạc sĩ 12 CHƯƠNG THIẾT KẾ BỘ ĐIỀU KHIỂN Trong chương này, điều khiển PID trượt cho cánh tay mềm dẻo thiết kế để điều khiển cánh tay bám theo quỹ đạo cho trước, tính ổn