Tổng hợp hệ điều khiển tay máy có khớp đàn hồi ứng dụng cảm biến vi cơ quán tính

BỘ GIÁO DỤC VÀ ĐÀO TẠO BỘ QUỐC PHÒNG VIỆN KHOA HỌC VÀ CÔNG NGHỆ QUÂN SỰ TỔNG HỢP HỆ ĐIỀU KHIỂN TAY MÁY CÓ KHỚP ĐÀN HỒI ỨNG DỤ - NĂM 2016 BỘ GIÁO DỤC VÀ ĐÀO TẠO BỘ QUỐC PHÒNG VIỆN KHOA HỌC VÀ CÔNG NGHỆ QUÂN SỰ TỔNG HỢP HỆ ĐIỀU KHIỂN TAY MÁY CÓ KHỚP ĐÀN HỒI ỨNG DỤ Chuyên ngành: khiển T Mã số: 62.52.02.16 NGƢỜI HƢỚNG DẪN KHOA HỌC: PGS.TS TRẦN ĐỨC THUẬN - NĂM 2016 i Tôi cam đoan đâ 01 năm 2016 ii ần Đức Thuậ , ể K q m ơn c C – iii vi ix ầu Chƣơng 1: TỔNG QUAN VỀ ệp 1.1 1.1.1 Sơ lượ ệp 1.1.2 y 1.1.3 10 1.2 11 1.3 Tình hình nghiên cứu 14 ều khiển ch 1.4 17 20 Chƣơng 2: XÂY DỰNG THUẬT TOÁN XÁC ĐỊNH TƢ THẾ GÓC VÀ CÁC THUẬT TOÁN HIỆU CHỈNH KHI SỬ DỤNG CÁC VI CƠ QUÁN TÍNH 21 2.1 Giới thiệu thuật toán lọc Kalman 21 2.2 Xây dựng thuật toán xác đị ứng dụng phần tử ố 22 2.2.1 Ma trận cosin phương góc định hướng 22 2.2.2 Xây dựng thuật toán xác định tham s 27 2.3 Xây dựng thuật toán xác định tham số góc cho tay máy ứng dụng phần tử vi trường hợ 2.4 Xây dựng thuật toán hiệu ố 32 thông số đo 34 2.4 34 2.4.2 Phương pháp thuật toán hiệu chuẩn 37 2.5 Xây dựng thuật toán nhận dạng hiệu ch hệ đo 46 2.5.1 Phương pháp hiệu chỉnh vị trí 47 2.5.2 Phương pháp hiệu chỉnh cho hệ đo véc tơ tốc độ góc 49 2.5.3 Phương pháp hiệu chỉnh cho hệ đo véc tơ gia tốc 52 Kết luận chƣơng 54 iv Chƣơng 3: XÂY DỰNG THUẬT TOÁN ĐIỀU KHIỂN CHO TAY MÁY 55 3.1 Cơ 55 3.1.1 Tính ổn định Lyapunov 55 3.1.2 Hàm điều khiển Lyapunov 57 3.2 Hệ thống phản hồ 58 61 3.4 Tổng hợp điều khiển cho tay rô bốt 65 3.4.1 Mô hình đối tượng 65 3.4.2 Thiết kế điều khiển 66 Kết luận chƣơng 80 Chƣơng 4: ỀU KHIỂN 81 ậ 4.1 Mô 81 4.1.1 Mô kiểm nghiệm thuậ 81 4.1.2 83 4.1.3 85 4.2 87 4.2.1 87 4.2.2 90 Kết luận chƣơng 98 100 101 103 P1 v DANH MỤC CÁC CÁC KÝ HIỆ Ký hiệu Ý nghĩa A Ma trận CHỮ VIẾT TẮT Aˆ x , Aˆ y , Aˆ z x, y, z Axc , Ayc , Azc x, y, z ax, ay, az Bˆ x , Bˆ y , Bˆ z ] x, y, z Bxc , Byc , Bzc b1, b2, b3 Các tham số biến đổi chậm, thể độ trôi quay F1 , F2 Các số ma sát nhớt Fk Véctơ hàm số F bước thứ k-1 G Gx , Gy , Gz Các gia tốc kế x, y, z Hk hi(Xk xj I i Dòng cảm ứng [A] J1 , J2 ] K Hệ số lò xo Kb Hệ số phản hồi suất điện động Kt Hằng số mô men m M c1 , M c Mô men ma sát L Cảm kháng phần ứng (Ω) N Pk Pk ( ) Pk Pk ( ) Pk Q Ma trận đối xứng k vi Qk q1, q2 Điện trở phần ứng (Ω) R Rk Tx, Ty, Tz u Điện áp phần ứng [V] V Hàm Lyapunov Vi Hàm Lyapunov thứ i X Véc tơ trạng thái hệ thống Xk Trạng thái véctơ trạng thái X bước thứ k Xk Trạng thái véctơ trạng thái X bước thứ k-1 Xˆ k ( ) X k Xˆ k ( ) X k rạng thái thứ i hệ thống điều khiển xi Véc tơ đầu hệ thống Y Z Giá trị véctơ Zk Zˆ k k Z zi k Sai số hiệu chỉnh , k Véctơ nhiễu động lực nhiễu đo có dạng ồn trắng k fi(Xk-1 k xj k-1 Kỳ vọng toán học ớng , góc chúc ngóc góc Cren) , , x , o x , y , o y Tốc độ quay thực quay z , o z Các nhiễu đo thường có dạng ồn trắng wx , wy , wz , , , Chỉ Các tham số không xác định vii i ci , , p Sai số đánh giá Các số dương i Điều khiển ảo i Sai số quan sát Chữ viết tắt Ý Nghĩa AMF Công ty Đúc Máy American Machine and Foundry Company) BĐK Bộ điều khiển CLF n (Control Lyapunove Function) CNC Máy gia công CNC (Computer Numerical Control) DH Denavit-Hartenberg ĐK Điều khiển IMU Inertial Measurement Unit) KĐCS Khuếch đại công suất LF Hàm Lyapunove (Lyapunove Function) MEMs OSC P Micro Electro Mechanical Systems) Máy sóng (Oscilloscope) iểu khiển tỷ lệ (Proportional) PC Máy tính cá nhân (Personal Computer) PD Bộ điểu khiển tỷ lệ, đạo hàm (Proportional Derivative) PI Bộ điểu khiển tỷ lệ, tích phân (Proportional Integral) PID Bộ điểu khiển tỷ lệ, tích phân đạo hàm (Proportional Integral Derivative) PWM Bộ điều chế độ rộng xung (Pulse Width Modulation) viii DANH MỤC CÁC HÌNH VẼ, ĐỒ THỊ Trang Hình 1.3 Chiều dài góc xoắn khâu Hình 1.4 : Các thông số ai, αi, di θi rô 10 12 12 14 18 18 Hình 1.12 Độ lệch hệ quy chuẩn đo hệ quy chuẩ ệ tọa độ liên kết hệ tọa độ chuẩn ệ toạ độ liên kết hệ toạ độ 19 23 24 chuẩn 25 bố trí quay vi đo tốc độ góc, gia tốc kế từ kế Hình 2.5 Hình 2.6 27 33 ệ tọa độ liên kế 35 Hình 2.7 Hệ tọa độ liên kết gia tốc kế 38 Hình 2.8 Hệ tọa độ sở hệ toạ độ liên kết 46 vị trí cần có để xác định hệ số hiệu chuẩn 48 55 59 103 TÀI LIỆU THAM KHẢO Ti [1] Nguyễn Văn Chúc, Ngô Trọng Mại (2007), “Thuật toán dẫn đường quán tính có hiệu chỉnh ứng dụng thiết bị vi điện tử dùng cho hệ thống điều khiển thiết bị bay”, Hội nghị Khoa học ngành vũ khí, Trung tâm KHKT-CNQS [2] Tạ Văn Đĩnh (2000), “Phương pháp tính”, NXB Giáo dục, Hà Nội tr.7- 20 [3] Nguyễn Hoàng Hải, Nguyễn Khắc Kiểm (2003), “Lập trình Matlab”, NXB Khoa học Kỹ thuật Hà Nội, tr 114- 131 [4] Đ o Văn Hi p (2013), “Kỹ thuậ ọc Kỹ thuật N i (2012), “ [5] Nguy n ”, K K Q [6] [7] K Quâ ọc Kỹ thuậ [8] NXB Khoa học Kỹ thuật [9] n [10] NXB K Kỹ [11] Nguyễn Doãn Phước, Phan Xuân Minh, “Điều khiển tối ưu Bền vững”, NXB Khoa học K thuật [12] Nguy n ng”, NXB K Quang (2006), “MatLab Simulink nh cho sư u n t Kỹ [13] Trần Thế San (2003), “Cơ sở nghiên cứu sáng tạ ”, NXB Thống kê [14] Trần Đức Thuận, Đinh Văn Tuân, (2004), “Về thuật toán tổng hợp lệnh điều khiển tên lửa hành trình có thiết bị dẫn đường quán tính”, Tạp Nghiên cứu KHKT&CN Quân sự, Trung tâm KHKT-CNQS, số 104 [15] Trần Đức Thuận, Đinh Văn Tuân, Nguyễn Hải Long, (2006), “Về thuật toán nhận dạng tham số mô hình khí cụ bay có thiết bị dẫn đường quán tính”, Tạp chí Nghiên cứu KHKT&CN Quân sự, Trung tâm KHKT-CNQS, Số 14 [16] Ngô nh (2014), “Xây dựng Robot tự hành dạng Nonholonomic tổng hợp điều khiển bám quỹ đạo”, , Trường Đại học Bách khoa Hà Nội Tiếng Anh: [17] Bruno Siciliano, Lorenzo Sciavicco, Luigi Villani, Giuseppe Oriolo (2009): “Robotics (Modelling, Planning and Cotrol)”, Springer-Verlag London Limited [18] Denavit J., Hartenberg R S (1995), “A Kinematic Notation for Lower – Pair Mechanisms Based on Matrises” ASME Journal of Applied Mechanics [19] Fernaldo Passol (2009), “Applying RBF Neural Nets for Position Control of an Inter/Scara Robot”, Int J of Computer, Communications and Control, ISSN 18419836, E-ISSN 1841-9844 Vol IV 2009, No 2, Page 148-157 [20] Ghnia Debbache, Abdelhak Bennia, Noureddine Golea (2007), “Neural networks - Based Adaptive State Feedblack Control of Robot Manipulators”, International Journal of Computers, Communications & Control, Vol II (2007), No Pages 328-339 [21] H Asada, J J E Slotine (1995), “Robot Analysis and Control”, Massachusetts Institude of Technology [22] Huan Liu, Thea Iberall, George A Bekey (1988), “Neural network Architecture for robot hand control”, International Conference on Neural Network, San Diego, California, Juny 24-27, pp 38-42 [23] J Abadie and J Carpentier (1969), “Generalization of the Wolfe reduce gradient method to the case of nonlinear constraint, optimization”, Academic Press, London, pp.37-47 [24] John M Daly and Howard M Schwartz (2005), “Non-Linear Adaptive Output Feedback Control of Robot Manipulators”, Carleton University [25] Kim Chwee Ng (1995), “Switching Control Systems and Their Design Automation via Genetic Algorithms” Ph.D Thesis, Glasgow University, Faculty of Engineering, 1995 Supervised by Y.Li 105 [26] King, S.Y, J N Hwang (1989), “ Neural Network Architectures for Robot application”, IEEE trans Robotics and Automation, vol 35, pp 641-657 [27] Krstic M., Kanellakopoulos L., Kokotovic P.V (1995), “Nonlinear and Adaptive Control Design”, A Wiley Interscience Publication John Wiley & Sons, Inc [28] La Sall J and Lefchetz S (1961), “Stability by Liapunov’s direct method with applications”, Academic Press, New York [29] M S Grewal and A P Andrews (2001), “Kalman Filtering: Theory and Practice Using MATLAB”, Wiley Interscience [30] Ozgur Yeniay (2005), “A comparative study on optimization method for the constrained nonlinear programming problems, mathematical problems inEngineering” [31] P.M Taylor (1990), “Robotic control”, printed in the London [32] P.T Boggs and J.W Tolle (2000), “Sequential quadratic programming for Largge - scale nonlinear optimization”, J Comput Appl Math [33] Parviz E Nikravesh (1988), “Computer -Aided Analysis of Mechnical Systems” printed in USA, pp.19-250 [34] Paul R.P., Robot Manipulators: Mathematics, Programming and Control [35] Salim Ibrir (2009), “Algebraic observer design for a class of Uniformly- observable nonlinear systems, Application to 2-link Robotic manipulator”, Proceedings of the 7th Asian Control Conference, Hong Kong, China [36] Salychev O.S (1998) “Inertial Systems in Navigation and Geophysics”, Bauman MSTU Press, Moscow [37] T.Wang (2001), “Global optimization for Constrained nonlinear programming” Department of Computer Science , University of Illinois , Illinois, pp 1-40 [38] W Khalil, E.Dombre (2004), “Modeling, Identification and Control of Robots”, Kogan page Science, London [39] Wei Sun, Yaonan Wang (2004), “A Recurrent Fuzzy Neural Network Based Adaptive Control and Its Application on Robotic Tracking Control”, Neural [40] Withit Chatlatanagulchai, Peter H Meckl (2005), “Backstepping High-Order Differential Neural Network Control of Flexible-Joint Manipulator, American Control Conference 106 Tiếng Nga: [41] В В Матвеев, В Я Распопов «Основы Построения Бесплатформенных Инерциальных Навигационных Систем» - СПб: ГНЦ РФ ОАО «Концерн «ЦНИИ «Электроприбор», 2009 [42] Распопов В.Я Микросистемная Авионика, учебное Пособие.Тула "Гриф и к" 2010 [43] Фильтрация и Стохастическое Управление в Динамических Системах Под Редакциѐй К Т Леондеса, перевод с английского Издательство "Мир" Москва 1980 [44] Шаврин В.В., Коналов А.С., Тисленко В.И, “Калибровка микроэлектроме- ханических датчиков ускорений и угловых скоростей в бесплатформенных инерциальных навигационных системах”, Доклады ТУСУРа, № (25), часть 2, июнь 2012 107 PHỤ LỤC Ph n m m mô đun lọc Kalman function [quatout,bias_out,V_inertial_out,euler_out,pqr_out,unfiltered_euler,bodyAcc_out,P_out,yaw1, gyro3, counter_out1, counter_out2, counter_out3] = fcn(gyros,accels,mags,dt_gps,gps_update,XYZ_inertial, time_05) %%************************************************************ %%***************** initialize variables ********************* %%************************************************************ persistent xhat; persistent P; persistent XYZ_inertial_prev; persistent XYZ_inertial_prev_prev; persistent bodyAcc; persistent V_inertial; persistent counter; persistent counter2; persistent counter3; persistent gyros_old; persistent out_test; persistent bodyV_prev; persistent bodyV; persistent bodyV_out; persistent bodyAcc_prev; persistent euler_out_prew; persistent euler_out_prew_prew; persistent accels_prew; persistent accels_prew_prew; persistent gyros_prew_prew; persistent gyros_prew; persistent gyros_tb; persistent mags_prew; persistent yaw_old_1; persistent yaw_old_1_prew; persistent yaw_old_1_prew_prew; persistent mags_prew_prew; persistent va; if isempty(counter) counter = 0; 108 end if isempty(counter2) counter2 = 0; end if isempty(xhat) xhat =[0.999998858349755; 0.000871902307597; 0.000873425393729; 0.000871902307597; [1e-6;1e-6;1e-6];]; P=diag( [1^2*ones(1,3) 1^2*ones(1,3)] ); XYZ_inertial_prev = [0;0;0]; XYZ_inertial_prev_prev = [0;0;0]; euler_out_prew = [0;0;0]; euler_out_prew_prew =[0;0;0]; accels_prew = [0;0;0]; accels_prew_prew =[0;0;0]; gyros_prew_prew =[0;0;0]; gyros_prew =[0;0;0]; gyros_tb =[0;0;0]; mags_prew = [0;0;0]; mags_prew_prew =[0;0;0]; unfiltered_euler = [0;0;0]; bodyAcc = [0;0;0]; bodyAcc_prev = [0;0;0]; V_inertial = [0;0;0]; bodyV_out=[0;0;0]; bodyV_prev = [0;0;0]; bodyV = [0;0;0]; gps_alive = 0; gyros_old = gyros; %gyros_old(3) = gyros(3)-57.2*0.01745; yaw_old_1 = 0; gyro3 = 0; yaw_old_1_prew_prew = 0; yaw1=0; out_test =[0;0;0;0;0;0]; counter3 = 0; end dt = 0.01; % fundamental time step g = 9.81; 109 % B = [0.38848; % -0.00953; % 0.22781]; %NED B = [0.23092; 0.00920; 0.25392]; %NED a_n = 0.005; m_n = 005; % a_n = 50; % m_n = 5; Q_quat = 1e-2; Q_bias = 1e-2; tau_gyro = 100; exptau = exp(-.01/tau_gyro); R = diag([a_n^2*ones(1,3) m_n^2*ones(1,3)]); Q = diag([Q_quat^2*ones(1,3) dt*Q_bias^2*ones(1,3)]); x_old=xhat; %%************************************************************ %%******************** propogate state *********************** %%************************************************************ counter = counter+1; counter2 = counter2+1; counter3 = counter3+1; if accels(1)>0 accels(1)=accels(1)*0.94; % accels(1)=accels(1) - (2/104)*accels(1); end if accels(1)0 accels(2)=accels(2)*0.96; % end accels(2)=accels(2)-(2/104)*accels(2); 110 if accels(2)0 accels(3)=accels(3)*0.95; end if accels(3)