0

Nghiên cứu phát triển hệ thống hil (hardware in the loop) thủy lực trong mô phỏng chuyển động

72 2 0
  • Nghiên cứu phát triển hệ thống hil (hardware in the loop) thủy lực trong mô phỏng chuyển động

Tài liệu hạn chế xem trước, để xem đầy đủ mời bạn chọn Tải xuống

Tài liệu liên quan

Thông tin tài liệu

Ngày đăng: 12/05/2022, 10:54

I H C QU C GIA TP HCM TR NG I H C BÁCH KHOA NG HU NH NGH A NGHIÊN C U PHÁT TRI N H TH NG HIL (HARDWARE - IN THE - LOOP) TH Y L C TRONG MÔ PH NG CHUY N NG STUDY AND DEVELOP HYDRAULIC HIL (HARDWARE – IN THE – LOOP) SYSTEM IN MOTION SIMULATION Chuyên ngành: K thu t C Khí Mã s : 8520103 LU N V N TH C S TP H CHÍ MINH, tháng n m 2022 Cơng trình đ Cán b h c hồn thành t i: Tr ng i h c Bách Khoa HQG-HCM ng d n khoa h c: TS H TRI T H NG Cán b ch m nh n xét 1: PGS.TS L U THANH TÙNG Cán b ch m nh n xét 2: PGS.TS LÊ TH TRUY N Lu n v n th c s đ c b o v t i Tr ng đ i h c Bách Khoa, HQG Tp HCM ngày 15 tháng 01 n m 2022 Thành ph n H i đ ng đánh giá lu n v n th c s g m: PGS.TS TR N ANH S N – Ch t ch h i đ ng TS BÀNH QU C NGUYÊN – Th ký PGS.TS L U THANH TÙNG – Ph n bi n PGS.TS LÊ TH TRUY N – Ph n bi n PGS.TS PH M S N MINH – y viên Xác nh n c a Ch t ch H i đ ng đánh giá LV Tr chuyên ngành sau lu n v n đ CH T CH H I NG PGS.TS TR N ANH S N ng Khoa qu n lý c s a ch a (n u có) TR NG KHOA C KHÍ i I H C QU C GIA TP HCM TR NG C NG HÒA XÃ H I CH NGH A VI T NAM I H C BÁCH KHOA c l p – T – H nh phúc NHI M V LU N V N TH C S H tên h c viên: NG HU NH NGH A MSHV: 1870045 Ngày, tháng, n m sinh: 15/03/1995 N i sinh: Cam Ranh, Khánh Hòa Chuyên ngành: K Thu t C Khí Mã s : 8520103 I TÊN TÀI: Nghiên c u phát tri n h th ng HIL (Hardware - In the - Loop) th y l c mô ph ng chuy n đ ng Study and develop hydraulic HIL (Hardware – In the – Loop) system in motion simulation NHI M V VÀ N I DUNG: - Tìm hi u t ng quan v HIL dùng h th ng truy n đ ng th y l c - Phân tích đ ng h c thu n ngh ch c a h th ng HIL k t c u song song b c t - Mơ hình h th ng HIL k t c u song song b c t - Thi t k h th ng u n m tr t thích nghi cho c c u - Mơ ph ng đánh giá k t qu mô ph ng h th ng HIL k t c u song song b c t v i h th ng u n m tr t thích nghi II NGÀY GIAO NHI M V : 22/02/2021 III NGÀY HOÀN THÀNH NHI M V : 12/12/2021 IV.CÁN B H NG D N: TS H TRI T H NG ii Tp HCM, ngày … tháng … n m 202 CÁN B H NG D N CH NHI M B TR NG KHOA C KHÍ MƠN ÀO T O iii L IC M Tr N c tiên xin g i l i c m n chân thành nh t đ n TS H Tri t H ng ng t n tâm h ng d n t o đ ng l c cho tơi su t q trình th c hi n lu n v n C m n th y khoa C Khí nói riêng th y cô tr Khoa – i ng đ i h c Bách HQG Tp H Chí Minh nói chung ln vui v ch d n t o nh ng u ki n thu n l i nh t đ tơi có th hồn thành đ tài lu n v n c a C m n gia đình b n bè ln ch d a v ng ch c nh t đ ng l c đ ph n đ u m i ngày Tuy có nhi u c g ng nh ng không th tránh kh i nh ng thi u sót q trình trình bày lu n v n, r t mong nh n đ c s góp ý chân thành t q th y đ ng nghi p Trân tr ng c m n! TP.HCM, ngày 12 tháng 12 n m 2021 H c viên th c hi n ng Hu nh Ngh a iv TÓM T T LU N V N TH C S Mô ph ng HIL (Hardware – In the – Loop) m t công ngh m i đ y ti m n ng H th ng HIL giúp cung c p đ n nhà thi t k m t ph ng đoán tr c quan v kh n ng đáp ng c a s n ph m c ng nh nh ng nh h ng b t l i c a mơi tr ng q trình v n hành, t đ a nh ng s a ch a, nâng c p k p th i Nghiên c u s th c hi n tìm hi u t ng quan v h th ng HIL th y l c, đ ng th i đ xu t ng d ng b truy n đ ng n th y l c (EHA – Electro-Hydrostatic Actuator) giúp ti t ki m n ng l ng cho h th ng Song song v i đó, nghiên c u c ng th c hi n đánh giá thi t k b u n m tr t thích nghi đ đ m b o đ xác c a h th ng v n hành ABSTRACT HIL (Hardware – In the – Loop) is a promisingly new technology HIL provides the designers with practical predictions about future product performance as well as potentially unfavourable effects from operating environment So that, the designer can take troubleshooting and improvement timely This study will perform a general research about hydraulic HIL system as well as propose an EHA (Electro-Hydrostatic Actuator) application to help the system save its energy In parallel, the study also evaluates and designs the adaptive fuzzy sliding mode controller for HIL system to ensure system accuracy in opertation v L I CAM OAN C A TÁC GI Tôi xin cam đoan đ tài “Nghiên c u phát tri n h th ng HIL (Hardware - In the Loop) th y l c mô ph ng chuy n đ ng” cơng trình nghiên c u c a Các n i dung nghiên c u k t qu đ tham kh o đ c trình bày hồn tồn trung th c Các tài li u c trích d n rõ ràng Tơi xin ch u hồn tồn trách nhi m n u nh ng u sai s th t TP.HCM, ngày 12 tháng 12 n m 2021 H c viên th c hi n ng Hu nh Ngh a vi M CL C NHI M V LU N V N TH C S i L I C M N iii TÓM T T LU N V N TH C S iv ABSTRACT iv L I CAM OAN C A TÁC GI .v DANH M C HÌNH NH viii DANH M C B NG BI U x DANH SÁCH CÁC T Ch VI T T T xi ng T NG QUAN 1.1 Gi i thi u chung v h th ng Hardware – In the – Loop (HIL) th y l c thu t toán u n m tr t thích nghi .1 1.1.1 H th ng Hardware – In the – Loop (HIL) th y l c 1.1.2 Thu t toán u n m tr t thích nghi (AFSMC - Adaptive Fuzzy Sliding Mode Control) 1.1.3 M t s h th ng HIL mô ph ng chuy n đ ng n i b t hi n .2 1.2 it ng m c tiêu nghiên c u 1.2.1 1.2.2 it ng nghiên c u M c tiêu nghiên c u 1.3 Ph m vi nghiên c u .6 Ch ng K T C U C KHÍ C A H TH NG HIL TH Y L C .7 2.1 Khái quát k t c u c khí c a h th ng 2.2 Các thành ph n c khí c a h th ng 2.2.1 B m / ng c servo 2.2.2 Xy lanh th y l c 2.2.3 Kh p n i 10 Ch ng PHÂN TÍCH NG H C 12 3.1 Phân tích b c t c a h th ng 12 3.2 Bài toán đ ng h c ngh ch 13 3.3 Bài toán đ ng h c thu n 16 vii Ch ng MƠ HÌNH HĨA CÁC THÀNH PH N CHÍNH C A H TH NG HIL TH Y L C .19 4.1 Mơ hình hóa b m / đ ng c th y l c 19 4.1.1 Ch đ b m .19 4.1.2 Ch đ đ ng c .20 4.2 Mơ hình hóa b truy n đ ng chung .20 Ch ng THI T K H TH NG I U KHI N 23 5.1 Thi t k b u n cho xy lanh 23 5.1.1 C s lý thuy t 23 5.1.2 K t qu mô ph ng b u n cho xy lanh .31 5.2 Thi t k b u n cho xy lanh 37 5.2.1 C s lý thuy t 37 5.2.2 K t qu mô ph ng b u n cho xy lanh .40 TÀI LI U THAM KH O 46 PH L C 50 viii DANH M C HÌNH NH Hình 1.1 Mơ hình HIL c a hãng Toyota Hình 1.2 Mơ hình HIL VR5 c a hãng Exsim Hình 1.3 Mơ hình HIL mô ph ng chuy n đ ng c a hãng Olsen .5 Hình 2.1 Khái quát k t c u c khí c a mơ hình HIL hexapod Hình 2.2 ng c servo AC c u t o c b n c a Hình 2.3 C u trúc c a m t xy lanh th y l c Hình 2.4 Kh p n i đ ng 10 Hình 2.5 Kh p n i c u .11 Hình 3.1 S đ khái quát chi ti t đ ng h c c a h th ng 12 Hình 3.2 Chuy n đ ng t nh ti n chuy n đ ng góc c a h th ng 13 Hình 3.3 S đ khái quát liên k t c a k t c u hexapod .14 Hình 3.4 V trí kh p n i đ c đ nh (a) n n t i d ch chuy n (b) 14 Hình 4.1 S đ chung c a c c u truy n đ ng n th y l c EHA 21 Hình 5.1 S đ kh i c a b u n m tr t thích nghi [23] 23 Hình 5.2 Các bi n ngôn ng ngõ vào s 27 Hình 5.3 Các bi n ngôn ng ngõ u 28 Hình 5.4 Mơ hình o c a c c u truy n đ ng n - th y l c xây d ng b ng Amesim .32 Hình 5.5 S đ t ng quát c a h th ng u n 33 Hình 5.6 Ph n h i đa b c c a h th ng v i b u n m tr t thích nghi 34 Hình 5.7 Sai s v trí c a h th ng theo th i gian 34 Hình 5.8 Áp su t theo th i gian t i c ng (p u n m tr c ng (p t thích nghi cho ph n h i đa b c a xy lanh v i b c 34 Hình 5.9 Giá tr l c t i theo th i gian 35 Hình 5.10 So sánh gi a giá tr v trí th c t c a t i tham chi u 36 Hình 5.11 Sai s gi a v trí th c t tham chi u 36 Hình 5.12 Tr ng thái áp su t c a hai c ng xy lanh 36 45 Ch ng K T LU N VÀ H NG PHÁT TRI N Quá trình nghiên c u phát tri n h th ng HIL th y l c g m ph n chính: mơ ph ng th c nghi m Tuy nhiên, th i gian không cho phép, b c đ u tiên lu n v n ch t p trung gi i quy t n i dung c s lý thuy t bao g m nghiên c u t ng quan, phân tích đ ng h c, mơ ph ng đánh giá k t qu mô ph ng b u n s d ng cho h th ng Ti p theo, c n xây d ng mơ hình v t lý th c hi n ki m nghi m l i d li u mô ph ng thu đ c t ph n m m K t qu mô ph ng c a h th ng HIL k t c u song song b c t b u n m tr t thích nghi thu đ c t t B u n không ch đ m b o đ xác cao cho qu đ o mơ ph ng mà h n ch t i đa nh ng nh h ng b t l i c a y u t nhi u lên q trình mơ ph ng V i k t qu đó, ta kh ng đ nh b u n m tr t thích nghi hồn tồn phù h p đ s d ng cho h th ng HIL Song song v i vi c phân tích mơ ph ng h th ng HIL c b n c n m r ng tìm hi u h th ng HIL tiên ti n s d ng cho ngành công nghi p hi n đ i nh hàng không v tr , hàng h i, cơng nghi p qu c phịng Qua n m b t đ c n n t ng ki n th c, công ngh m i giúp ích cho q trình nghiên c u phát tri n h th ng t ng lai 46 TÀI LI U THAM KH O [1] M Chen, and D Zhao, “The gravitational potential energy regeneration system with closed-circuit of boom of hydraulic excavator,” Mechanical system and Signal Processing, vol 82, pp 178-192, 2017 [2] J Choi, “Robust position control of electro-hydrostatic actuator systems with radial basis function neural networks,” Journal of Advanced Mechanical Design Systems & Manufacturing, vol 7, pp 257-267, 2013 [3] H Razmi, and S Afshinfa, “Neural network-based adaptive sliding mode control design for position and attitude control of a quadrotor UAV,” Aerospace Science and Technology, vol 91, pp 12-27, 2019 [4] Z Xu, and Y Guo, “Fuzzy control method for earthquake mitigation structure with magnetorheological damper,” Journal of Intelligent Material systems and Structure, vol 17, pp 871-881, 2006 [5] V Qu c Huy et al., “T ng h p b u n tr t m c u trúc bi n đ i kháng nhi u (asf-vsc) cho h phi n b t đ nh,” T p chí Nghiên c u KH&CN quân s , vol 55, pp 17-25, 2015 [6] A Tumasov et al., “The Application of Hardware-in-the-Loop (HIL) Simulation for Evaluation of Active Safety of Vehicles Equipped with Electronic Stability Control (ESC) Systems,” Procedia Computer Science, vol 150, pp 309-315, 2019 [7] Staff Writer “Toyota's driving simulator: Not all fun and games.” Internet: https://www.motortrend.com/news/news260711-toyotas-driving-simulator, 2007 [8] Bsimracing “Introducing The EXSIM VR5 Motion Racing Simulator.” Internet: https://www.bsimracing.com/introducing-the-exsim-vr5-motion-racing-simulator, 2007 [9] Olsen Actuation UK Ltd “Motion Simulation Platforms.” Internet: https://www.olsenactuation.com/motion-platforms.html, 2017 47 [10] Mesidas Group “T ng quan v đ ng c Servo.” Internet: https://mesidas.com/servo-motor-la-gi, 2020 [11] HASS “Xy lanh th y l c.” Internet: https://haasvn.com/xi-lanh-thuy-luc, 2020 [12] Ngo Phan Group “Kh p n i cardan gì? Cách l a ch n kh p n i cardan.” Internet: https://www.cpclinear.com/vi/tin-tuc/tin-nganh-co-khi/khop-noi- cardan-la-gi-126.html, 2020 [13] Kanavn “Các lo i kh p n i, kh p n i tr c đ nh ngh a.” Internet: https://kanavn.vn/cac-loai-khop-noi-khop-noi-truc-va-dinh-nghia.html, 2020 [14] O Albayrak “Modeling and real-time control system implementation for a Stewart platform,” M.A thesis, Middle East Technical University, Turkey, 2005 [15] C M McCann, and A M Dollar, “Design of a Stewart Platform-Inspired Dexterous Hand for 6-DOF Within-Hand Manipulation,” presented at IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Vancouver, BC, Canada, 2017 [16] A Filabi, and M Yaghoobic “Fuzzy Adaptive Sliding Mode Control of DOF Parallel Manipulator with Electromechanical Actuators in Cartesian Space Coordinates,” Communications on Advanced Computational Science with Applications, vol 1, pp 1-21, 2015 [17] D Zhang “Parallel Robotic Machine Tools,” Springer Science & Business Media, vol 10, pp 20-35, 2010 [18] H M Alwan, and Sarhan, “Kinematics Simulation of Gough-Stewart Parallel Manipulator by Using Simulink Package in Matlab Software,” Journal of University of Babylon for Engineering Sciences, vol 27, pp 10-20, 2019 48 [19] T H Ho, and K K Ahn, “Speed Control of a Hydraulic Pressure Coupling Drive Using an Adaptive Fuzzy Sliding-Mode Control,” IEEE/Asme Transactions On Mechatronics, vol 17, pp 976-986, 2012 [20] N Noroozi et al., “Adaptive fuzzy sliding mode control scheme for uncertain systems,” Commun Nonlinear Sci Numer Simul., vol 14, pp 3978–3992, 2009 [21] Tri Nguyen et al, “Trajectory control of an electro-hydraulic actuator using an iterative backstepping control scheme,” Mechatronics, vol 29, pp 96-102, 2015 [22] J Choi, “Robust position control of electro-hydrostatic actuator systems with radial basis function neural networks,” Journal of Advanced Mechanical Design Systems & Manufacturing, vol 7, pp 257-267, 2003 [23] C Guan, and S Pan, “Adaptive sliding mode control of electrohydraulic system with nonlinear unknown parameters,” Control Engineering Practice, vol 16, pp 1275–1284, 2008 [24] S Huang, and H Chen, “Adaptive sliding controller with self-tuning fuzzy compensation for vehicle suspension control,” Mechatronics, vol 16, pp 607622, 2006 [25] H.-C Lu, and T.-H Hung, “Fuzzy sliding mode controller design with variable sliding surface,” IEEE 3rd International Conference on Computational Cybernetics, Mauritius, 2005 [26] K M Passino “Fuzzy control,” presented at Addison-Wesley, Monlo Park, CA, 1998 [27] R Palm “Robust control by fuzzy sliding mode,” Automatica, vol 9, pp 1429-1437, 1994 [28] L X Wang “Adaptive fuzzy systems and control: Design and stability analysis,” presented at Prentice–Hall, Englewood Cliffs, NJ, 1994 49 [29] J Y Chen “Rule regulation of fuzzy sliding mode controller design: direct adaptive approach,” Fuzzy Sets and System, vol 120, pp 159-168, 2001 [30] M Manceur et al “MIMO second order sliding mode fuzzy type-2 control,” presented at IEEE International Conference on Systems, Man and Cybernetics, Mauritius, 2005 50 PH L C Ph l c A Matlab code cho toán đ ng h c ngh ch function [L1,L2,L3,L4,L5,L6] = fcn(X,Y,Z,alpha,beta,gama) deg2rad=pi/180; theta_b=15*deg2rad; theta_p=30*deg2rad; R_b=1; R_p=0.9; B1=[R_b*cos(15*deg2rad);R_b*sin(15*deg2rad);0]; B2=[R_b*cos(105*deg2rad);R_b*sin(105*deg2rad);0]; B3=[R_b*cos(135*deg2rad);R_b*sin(135*deg2rad);0]; B4=[R_b*cos(225*deg2rad);R_b*sin(225*deg2rad);0]; B5=[R_b*cos(255*deg2rad);R_b*sin(255*deg2rad);0]; B6=[R_b*cos(345*deg2rad);R_b*sin(345*deg2rad);0]; Bx1=B1(1,:); Bx2=B2(1,:); Bx3=B3(1,:); Bx4=B4(1,:); Bx5=B5(1,:); Bx6=B6(1,:); By1=B1(2,:); By2=B2(2,:); By3=B3(2,:); By4=B4(2,:); By5=B5(2,:); By6=B6(2,:); Bz1=B1(3,:); Bz2=B2(3,:); Bz3=B3(3,:); Bz4=B4(3,:); Bz5=B5(3,:); Bz6=B6(3,:); P1=[R_p*cos(30*deg2rad);R_b*sin(30*deg2rad);0]; P2=[R_p*cos(90*deg2rad);R_b*sin(90*deg2rad);0]; P3=[R_p*cos(150*deg2rad);R_b*sin(150*deg2rad);0]; P4=[R_p*cos(210*deg2rad);R_b*sin(210*deg2rad);0]; P5=[R_p*cos(270*deg2rad);R_b*sin(270*deg2rad);0]; 51 P6=[R_p*cos(330*deg2rad);R_b*sin(330*deg2rad);0]; Px1=P1(1,:); Px2=P2(1,:); Px3=P3(1,:); Px4=P4(1,:); Px5=P5(1,:); Px6=P6(1,:); Py1=P1(2,:); Py2=P2(2,:); Py3=P3(2,:); Py4=P4(2,:); Py5=P5(2,:); Py6=P6(2,:); Pz1=P1(3,:); Pz2=P2(3,:); Pz3=P3(3,:); Pz4=P4(3,:); Pz5=P5(3,:); Pz6=P6(3,:); L1=((X+((cos(gama)*cos(beta))*Px1)+(((cos(gama)*sin(beta)*sin(alpha))(sin(gama)*cos(alpha)))*Py1)+(((cos(gama)*sin(beta)*cos(alpha))+(sin(gama )*sin(alpha)))*Pz1)Bx1)^2+(Y+((sin(gama)*cos(beta))*Px1)+(((sin(gama)*sin(beta)*sin(alpha))+ (cos(gama)*cos(alpha)))*Py1)+(((sin(gama)*sin(beta)*cos(alpha))(cos(gama)*sin(alpha)))*Pz1)-By1)^2+(Z+((- sin(beta))*Px1)+((cos(beta)*sin(alpha))*Py1)+((cos(beta)*cos(alpha))*Pz1) -Bz1)^2)^0.5; L2=((X+((cos(gama)*cos(beta))*Px2)+(((cos(gama)*sin(beta)*sin(alpha))(sin(gama)*cos(alpha)))*Py2)+(((cos(gama)*sin(beta)*cos(alpha))+(sin(gama )*sin(alpha)))*Pz2)Bx2)^2+(Y+((sin(gama)*cos(beta))*Px2)+(((sin(gama)*sin(beta)*sin(alpha))+ (cos(gama)*cos(alpha)))*Py2)+(((sin(gama)*sin(beta)*cos(alpha))(cos(gama)*sin(alpha)))*Pz2)-By2)^2+(Z+((- sin(beta))*Px2)+((cos(beta)*sin(alpha))*Py2)+((cos(beta)*cos(alpha))*Pz2) -Bz2)^2)^0.5; 52 L3=((X+((cos(gama)*cos(beta))*Px3)+(((cos(gama)*sin(beta)*sin(alpha))(sin(gama)*cos(alpha)))*Py3)+(((cos(gama)*sin(beta)*cos(alpha))+(sin(gama )*sin(alpha)))*Pz3)Bx3)^2+(Y+((sin(gama)*cos(beta))*Px3)+(((sin(gama)*sin(beta)*sin(alpha))+ (cos(gama)*cos(alpha)))*Py3)+(((sin(gama)*sin(beta)*cos(alpha))(cos(gama)*sin(alpha)))*Pz3)-By3)^2+(Z+((- sin(beta))*Px3)+((cos(beta)*sin(alpha))*Py3)+((cos(beta)*cos(alpha))*Pz3) -Bz3)^2)^0.5; L4=((X+((cos(gama)*cos(beta))*Px4)+(((cos(gama)*sin(beta)*sin(alpha))(sin(gama)*cos(alpha)))*Py4)+(((cos(gama)*sin(beta)*cos(alpha))+(sin(gama )*sin(alpha)))*Pz4)Bx4)^2+(Y+((sin(gama)*cos(beta))*Px4)+(((sin(gama)*sin(beta)*sin(alpha))+ (cos(gama)*cos(alpha)))*Py4)+(((sin(gama)*sin(beta)*cos(alpha))(cos(gama)*sin(alpha)))*Pz4)-By4)^2+(Z+((sin(beta))*Px4)+((cos(beta)*sin(alpha))*Py4)+((cos(beta)*cos(alpha))*Pz4) -Bz4)^2)^0.5; L5=((X+((cos(gama)*cos(beta))*Px5)+(((cos(gama)*sin(beta)*sin(alpha))(sin(gama)*cos(alpha)))*Py5)+(((cos(gama)*sin(beta)*cos(alpha))+(sin(gama )*sin(alpha)))*Pz5)Bx5)^2+(Y+((sin(gama)*cos(beta))*Px5)+(((sin(gama)*sin(beta)*sin(alpha))+ (cos(gama)*cos(alpha)))*Py5)+(((sin(gama)*sin(beta)*cos(alpha))(cos(gama)*sin(alpha)))*Pz5)-By5)^2+(Z+((sin(beta))*Px5)+((cos(beta)*sin(alpha))*Py5)+((cos(beta)*cos(alpha))*Pz5) -Bz5)^2)^0.5; L6=((X+((cos(gama)*cos(beta))*Px6)+(((cos(gama)*sin(beta)*sin(alpha))(sin(gama)*cos(alpha)))*Py6)+(((cos(gama)*sin(beta)*cos(alpha))+(sin(gama )*sin(alpha)))*Pz6)Bx6)^2+(Y+((sin(gama)*cos(beta))*Px6)+(((sin(gama)*sin(beta)*sin(alpha))+ (cos(gama)*cos(alpha)))*Py6)+(((sin(gama)*sin(beta)*cos(alpha))(cos(gama)*sin(alpha)))*Pz6)-By6)^2+(Z+((sin(beta))*Px6)+((cos(beta)*sin(alpha))*Py6)+((cos(beta)*cos(alpha))*Pz6) -Bz6)^2)^0.5; 53 Ph l c B Matlab code cho toán đ ng h c thu n function [X,Y,Z,alpha,beta,gama] = fcn(L1,L2,L3,L4,L5,L6) R_b = 1; R_p = 0.9; L0 = 1; angle_B = [deg2rad(15) deg2rad(105) deg2rad(135) deg2rad(255) deg2rad(345)]; deg2rad(225) angle_P = [deg2rad(30) deg2rad(90) deg2rad(150) deg2rad(210) deg2rad(270) deg2rad(330)]; [B1, B2, B3, B4, B5, B6] = matrixB(R_b, angle_B); Bx1 = B1(1,:); Bx2 = B2(1,:); Bx3 = B3(1,:); Bx4 = B4(1,:); Bx5 = B5(1,:); Bx6 = B6(1,:); By1 = B1(2,:); By2 = B2(2,:); By3 = B3(2,:); By4 = B4(2,:); By5 = B5(2,:); By6 = B6(2,:); Bz1 = B1(3,:); Bz2 = B2(3,:); Bz3 = B3(3,:); Bz4 = B4(3,:); Bz5 = B5(3,:); Bz6 = B6(3,:); [P1, P2, P3, P4, P5, P6] = matrixP(R_p, angle_P); Px1 = P1(1,:); Px2 = P2(1,:); Px3 = P3(1,:); Px4 = P4(1,:); Px5 = P5(1,:); Px6 = P6(1,:); Py1 = P1(2,:); Py2 = P2(2,:); Py3 = P3(2,:); Py4 = P4(2,:); 54 Py5 = P5(2,:); Py6 = P6(2,:); Pz1 = P1(3,:); Pz2 = P2(3,:); Pz3 = P3(3,:); Pz4 = P4(3,:); Pz5 = P5(3,:); Pz6 = P6(3,:); Px = [Px1 Px2 Px3 Px4 Px5 Px6]; Py = [Py1 Py2 Py3 Py4 Py5 Py6]; Pz = [Pz1 Pz2 Pz3 Pz4 Pz5 Pz6]; Bx = [Bx1 Bx2 Bx3 Bx4 Bx5 Bx6]; By = [By1 By2 By3 By4 By5 By6]; Bz = [Bz1 Bz2 Bz3 Bz4 Bz5 Bz6]; [f1, f2, f3, f] = getCompleteF(X, Z, beta, Px, Py, Pz, Bx, By, Bz, L); df = [diff(f1,X) diff(f1,Z) diff(f1,beta);diff(f2,X) diff(f2,beta);diff(f3,X) diff(f3,Z) diff(f3,beta)]; x = newTonRaphson(f, df, 100); X = x(1, 1); Y=0; Z = x(2, 1); alpha=0; beta = x(3, 1); gama=0; function r = deg2rad(x) end r = x * pi / 180; function m = getMatrixRow(c1, angle) m = [c1*cos(angle); c1*sin(angle); 0]; end function [B1, B2, B3, B4, B5, B6] = matrixB(R_b, angle_B) B1 = getMatrixRow(R_b, angle_B(1, 1)); B2 = getMatrixRow(R_b, angle_B(1, 2)); B3 = getMatrixRow(R_b, angle_B(1, 3)); B4 = getMatrixRow(R_b, angle_B(1, 4)); B5 = getMatrixRow(R_b, angle_B(1, 5)); B6 = getMatrixRow(R_b, angle_B(1, 6)); end function [P1, P2, P3, P4, P5, P6] = matrixP(R_p, angle_P) diff(f2,Z) 55 P1 = getMatrixRow(R_p, angle_P(1, 1)); P2 = getMatrixRow(R_p, angle_P(1, 2)); P3 = getMatrixRow(R_p, angle_P(1, 3)); P4 = getMatrixRow(R_p, angle_P(1, 4)); P5 = getMatrixRow(R_p, angle_P(1, 5)); end P6 = getMatrixRow(R_p, angle_P(1, 6)); function x = newTonRaphson(f, df, n) x0 = [0; 2; 0]; x1 = [0; 0; 0]; i = 1; while i
- Xem thêm -

Xem thêm: Nghiên cứu phát triển hệ thống hil (hardware in the loop) thủy lực trong mô phỏng chuyển động ,