Tài liệu Thiết kế bộ điều khiển cho quadcopter kết hợp thị giác máy tính bám đối tượng di động

ĈҤ,+Ӑ&48Ӕ&*,$73+&0 75ѬӠ1*ĈҤ,+Ӑ&%È&+.+2$ -------------------- 3+Ҥ07+$1+48<ӄ1 7+,ӂ7.ӂ%ӜĈ,ӄ8.+,ӆ1&+248$'COPTER .ӂ7+Ӧ37+ӎ*,È&0È<7Ë1+ %È0ĈӔ,7ѬӦ1*',ĈӜ1* DESIGN OF QUADCOPTER CONTROLLER WITH COMPUTER VISION TRACKING MOVING TARGET Chuyên ngành : .ӻWKXұW&ѫÿLӋQWӱ 0mVӕ8520114 /8Ұ19Ă17+Ҥ&6Ƭ 73+Ӗ&+Ë0,1+WKiQJ8 QăP &{QJWUuQKÿѭӧFKRjQWKjQKWҥL7UѭӡQJĈҥLKӑF%iFK.KRD± Ĉ+4*-HCM. &iQEӝKѭӟQJGүQNKRDKӑF3*6761JX\ӉQ'X\$QK............................................. &iQEӝFKҩPQKұQ[pW 3*6761J{4XDQJ+LӃX......... &iQEӝFKҩPQKұQ[pW 761JX\ӉQ+ӳX&ѭӡQJ.......... /XұQYăQWKҥFVƭÿѭӧFEҧRYӋWҥL7UѭӡQJĈҥLKӑF%iFK.KRDĈ+4*7S+&0 QJj\WKiQJQăPWUӵFWX\ӃQ 7KjQKSKҫQ+ӝLÿӗQJÿiQKJLiOXұQYăQWKҥFVƭJӗP &KӫWӏFK3*6761JX\ӉQ4XӕF&Kt 7KѭNê76/r7KDQK+ҧL 3KҧQELӋQ3*6761J{4XDQJ+LӃX 3KҧQELӋQ761JX\ӉQ+ӳX&ѭӡQJ Ӫ\YLrQ767UҫQ9LӋW+ӗQJ ;iFQKұQFӫD&KӫWӏFK+ӝLÿӗQJÿiQKJLi/9Yj7UѭӣQJ.KRDTXҧQOêFKX\rQ QJjQKVDXNKLOXұQYăQÿmÿѭӧFVӱDFKӳDQӃXFy &+Ӫ7ӎ&++Ӝ,ĈӖ1* 75ѬӢ1*KHOA &Ѫ.+Ë ĈҤ,+Ӑ&48Ӕ&*,$73+&0 75ѬӠ1*ĈҤ,+Ӑ&%È&+.+2$ &Ӝ1*+Ñ$;­+Ӝ,&+Ӫ1*+Ƭ$9,ӊ71$0 ĈӝFOұS± 7ӵGR± +ҥQKSK~F NHIӊM VӨ LUҰ19Ă17+Ҥ&6Ƭ +ӑYjWrQKӑFYLrQ3+Ҥ07+$1+48<ӄ1««««««06+9 1Jj\WKiQJQăPVLQK«««««««««« 1ѫLVLQK&j0DX &KX\rQQJjQK.ӻWKXұW&ѫÿLӋQWӱ«««««««««« 0mVӕ I. 7Ç1Ĉӄ7¬, 7KLӃWNӃEӝÿLӅXNKLӇQFKR4XDGFRSWHU NӃWKӧSWKӏJLiFPi\WtQK EiPÿӕLWѭӧQJGLÿӝQJ II. 1+,ӊ09Ө9¬1Ӝ,'81* - 7KLӃWNӃEӝ ÿLӅXNKLӇQ NӃWKӧS[ӱOêҧQKJL~S4XDGFRSWHU bám theo ÿӕLWѭӧQJ. - 1JKLrQFӭXSKѭѫQJWKӭFJLDRWLӃS[ӱOêWtQKLӋX YjJӱLWtQKLӋXÿLӅXNKLӇQWӯWUҥPPһW ÿҩW ÿӃQEӝÿLӅXNKLӇQED\ - ;k\GӵQJP{KuQKP{SKӓQJWUrQPDWODE - ;k\GӵQJP{KuQKWKӵFQJKLӋP NLӇPFKӭQJ ,,,1*¬<*,$21+,ӊ09Ө21/09/2020 ,91*¬<+2¬17+¬1+1+,ӊ09Ө01/08/2021 9&È1%Ӝ+ѬӞ1*'Ү13*6761JX\ӉQ'X\$QK 7S+͛&Kt0LQKQJj\«WKiQJ«QăP21 &È1%Ӝ+ѬӞ1*'Ү1 +ӑWrQYjFKӳNê &+Ӫ1+,ӊ0%Ӝ0Ð1Ĉ¬27Ҥ2 +ӑWrQYjFKӳNê 75ѬӢ1*.+2$ &Ѫ.+Ë +ӑWrQYjFKӳNê i LӠI CҦ0Ѫ1 Tôi muӕn bày tӓ lӡi cҧPѫQFKkQWKjQKÿӃQJLDÿuQKÿmKӛ trӧ tôi vӅ mһt tinh thҫn FNJQJQKѭYұt chҩWÿһc biӋt lӡi cҧPѫQVkXVҳc nhҩWGjQKÿӃn Thҫ\Kѭӟng dүn tôi, Giҧng viên PGS. TS NguyӉQ'X\$QKÿmFKRW{LQKӳng lӡi khuyên vô cùng quý giá trong suӕt quá trình thӵc hiӋn luұQYăQQj\ Tp. H͛ &Kt0LQKQJj\WKiQJQăP 3+Ҥ07+$1+48<ӄ1 ii TÏM TҲT LUҰ19Ă1 LuұQYăQQjy trunh bjy mӝt cich tiӃp cұn mӟi cho viӋc xiFÿӏnh vӏ trt cӫa miy bay NK{QJQJѭӡi lii (UAVs) thay thӃ cho ciFSKѭѫQJin truyӅn thӕQJQKѭVӱ dөQJÿӏnh vӏ GPS kӃt hӧp vӟi cҧm biӃn IMU cho viӋFѭӟFOѭӧng vӏ trt cӫa UAVs vӟi ciFÿӕLWѭӧng xung TXDQKĈӇ WăQJWtQKFѫÿӝng, UAVs nhҩt thiӃt phҧi nhұn diӋQÿѭӧc cic tiFQKkQP{LWUѭӡng, xӱ lê vj phҧn inh lҥi mӝt cich nhanh chyQJ'Rÿy luұQYăQQjy cNJQJÿӅ xuҩt hӋ thӕng nhұn diӋn hunh ҧQKÿѭӧc lҳp trӵc tiӃp trên UAVs vӟi cich ljm njy ta cy thӇ giҧi quyӃWÿѭӧc vҩQÿӅ ÿӝ trӉ bӣi vu ttn hiӋXÿLӅu khiӇQÿѭӧc truyӅn trӵc tiӃSÿӃn bӝ ÿLӅu khiӇn bay. Luұn YăQFNJQJÿm chӭng minh kӃt quҧ tӯ lê thuyӃt, mô phӓQJÿӃn thӵc nghiӋm, UAVs cy thӇ nhұn diӋQÿѭӧFÿӕLWѭӧng mөc tiêu, cұp nhұt dӳ liӋu hunh ҧnh, vj biPWKHRÿӕLWѭӧng theo thӡi gian thӵc. LuұQYăQÿѭӧc sҳp xӃp theo bӕ cөFFKѭѫQJ &KѭѫQJ7ӘNG QUAN &KѭѫQJ&Ѫ6Ӣ Lé THUYӂT &KѭѫQJ0Ð7Ҧ PHҪN CӬNG V¬ PHҪN MӄM &KѭѫQJ.ӂT QUҦ MÔ PHӒNG V¬ THӴC NGHIӊM &KѭѫQJ.ӂT LUҰN V¬ +ѬӞNG PHÈT TRIӆ1Ĉӄ T¬I iii ABSTRACT This thesis presents an approach for determine the position of unmanned aerial vehicles (UAVs) which can replace for traditional method such as use a global positioning system (GPS) and an inertial measurement unit (IMU) for position estimation from their surrounding environment. To achive its maneuverability, it is necessary to recognize the surrounded object of environment, handle and respond to it immediately. Therefore, the thesis also proposes a direct visual servoing system for UAVs, by this way, the issue of control latency can be effectively solved because the position control signals are transmitted to the flight controller directly. The thesis also demonstrates from theory, simualation and experiment, the UAV could recognize a target updating in real time and a target tracking task was successfully realized. The thesis is arranged into 5 chapters: Chapter 1: OVERVIEW Chapter 2: THEORETICAL BASIS Chapter 3: DESCRIBE HARDWARE AND SOFTWARE Chapter 4: SIMULATION AND EXPERIMENT RESULTS Chapter 5: CONCLUSION AND DEVELOPMENT ORIENTATION iv LӠ,&$0Ĉ2$1 7{L[LQFDPÿRDQUҵQJOXұQYăQQj\ÿѭӧFYLӃWEӣLFKtQKFiQKkQW{LWKHRQKӳQJOӡLNKX\rQ WӯJLҧQJYLrQKѭӟQJGүQFӫDW{L7Kҫ\3*6761JX\ӉQ'X\$QKYjFQJYӟLQJXӗQWjL QJX\rQÿѭӧFOLӋWNrWURQJPөFWjLOLӋXWKDPNKҧR 3KҥP7KDQK4X\ӅQ v 0Ө&/Ө& 1+,ӊ09Ө/8Ұ19Ă17+Ҥ&6Ƭ ..................................................................................... i /Ӡ,&Ҧ0Ѫ1....................................................................................................................... ii TÏM TҲT LUҰ19ĂN......................................................................................................iii ABSTRACT ........................................................................................................................ iv /Ӡ,&$0Ĉ2$1 ................................................................................................................. v '$1+0Ө&+Î1+Ҧ1+ .................................................................................................viii &È&.é+,ӊ89¬7Ӯ9,ӂ77Ҳ7 .................................................................................... x &+ѬѪ1*7ӘNG QUAN ................................................................................................ 1 *LӟLWKLӋX chung .......................................................................................................... 1 0өFWLrXYjSKҥPYLFӫDÿӅWjL .................................................................................... 2 &+ѬѪ1*&Ѫ6Ӣ/é7+8<ӂ7..................................................................................... 4 2.1. 1JX\rQOêOjPYLӋF mô hình Quadcopter. ................................................................... 4 2.2. 7KXұWWRiQ[ӱOêҧQK .................................................................................................. 10 7uPKLӇXYӅNK{QJJLDQPjX .............................................................................. 10 7uPKLӇXYӅSKѭѫQJSKiSOӑFQKLӉXҧQK ............................................................ 13 *LҧLWKXұWQKұQGҥQJÿӕLWѭӧQJWK{QJTXDPjXVҳFFӫDFKtQKQy ..................... 16 ѬӟFOѭӧQJYӏWUt3RVHHVWLPDWLRQ ............................................................................ 19 2.3.1. Mô hình pinhole.................................................................................................. 19 2.3.2. Camera calibration. ............................................................................................. 20 2.3.3. ThuұWWRiQѭӟFOѭӧQJWUҥQJWKiL ........................................................................ 23 2.4. 7KXұWWRiQÿLӅXNKLӇQ ................................................................................................ 25 &+ѬѪ1*0Ð7Ҧ PHҪN CӬNG V¬ PHҪN MӄM ................................................... 29 3KҫQFӭQJ .................................................................................................................. 29 3KҫQPӅP ................................................................................................................. 39 vi &+ѬѪ1*.ӂ748Ҧ0Ð3+Ӓ1*9¬7+Ӵ&1*+,ӊ0 .......................................... 42 .ӃWTXҧP{SKӓQJWUrQ0DWODE ................................................................................. 42 .ӃWTXҧWKӵFQJKLӋP................................................................................................. 45 .ӃWTXҧQKұQGLӋQÿӕLWѭӧQJ .............................................................................. 45 4.2.2. Quadcopter tracNLQJWKHRÿӕLWѭӧQJ .................................................................. 47 &+ѬѪ1*.ӂT LUҰN V¬ +ѬӞNG PHÈT TRIӆ1Ĉӄ T¬I ..................................... 54 .ӃWOXұQ...................................................................................................................... 54 ĈӏQKKѭӟQJSKiWWULӇQÿӅWjL ...................................................................................... 54 7¬,/,ӊ87+$0.+Ҧ2 .................................................................................................. 56 3+Ө/Ө& ........................................................................................................................... 57 vii '$1+0Ө&+Î1+Ҧ1+ Hình 1- .KiLQLӋPYӅFiFKWLӃSFұQWK{QJWKѭӡQJDYj[iFÿӏQKYӏWUtWѭѫQJÿӕLGӵDWUrQ FҧPELӃQKuQKҧQKE ........................................................................................................... 1 Hình 1- 7әQJTXDQP{KuQK4XDGFRSWHUEiPWKHRÿӕLWѭӧQJGLÿӝQJ ............................. 2 Hình 2- 1 Mô hình Quadrotor [1] ......................................................................................... 4 Hình 2- ĈӏQKQJKƭDFKX\ӇQÿӝQJFӫD4XDGURWRU>@ ........................................................ 4 Hình 2- ĈӝQJOӵFKӑFKӋWKӕQJ ......................................................................................... 5 Hình 2- 0RPHQWYjFiFOӵFWiFÿӝQJOrQPӝWFiQKTXҥWWӵGR>@ .................................... 6 Hình 2- 6ӵ[RD\FӫDWUөF;<= ..................................................................................... 7 Hình 2- 4XDGFRSWHUÿѭӧFÿLӅXNKLӇQWK{QJTXDVӵWKD\ÿәL ............................................ 8 Hình 2- 7 Mô hình không gian màu RGB .......................................................................... 10 Hình 2- &ҩXWU~FP{KuQK+69+XH6WDWXUDWLRQDQG9DOXH ....................................... 11 Hình 2- 7ӑDÿӝFiFÿLӇPPjXFӫDNrQK+XHWURQJKӋ+69 ........................................... 12 Hình 2- /ѭXÿӗJLҧLWKXұWQKұQGҥQJÿӕLWѭӧQJ ........................................................... 16 Hình 2- ĈӗWKӏELӇXWKӏWK{QJVӕNK{QJJLDQPjX+69WURQJWKѭYLӋQ2SHQ&9 ......... 17 Hình 2- ĈӕLWѭӧQJOjTXҧEyQJYӟLPjX[DQKFKXӕLYӟLEiQNtQKFP ........................ 17 Hình 2- 1KұQGLӋQÿӕLWѭӧQJWUѭӟFYjVDXNKLTXDEӝOӑF ............................................ 18 Hình 2- 14 Mô hình Pinhole ............................................................................................... 19 Hình 2- &iFEѭӟFWtQKWRiQWK{QJVӕFDPHUD ............................................................... 20 Hình 2- 7ұSGӳOLӋX&KHVVERDUGÿѭӧFFKөSӣQKLӅXJyFÿӝNKiFQKDX ....................... 21 Hình 2- 0{KuQK&DOLEUDWLRQFDPHUDWKӵFQJKLӋP ....................................................... 21 Hình 2- &DPHUDQKұQGLӋQFRUQHUVFӫDFKHVVERDUG ...................................................... 22 Hình 2- &iFEѭӟFWKD\ELӃQWӑDÿӝ ................................................................................ 23 Hình 2- 1JX\rQOêJLҧLWKXұW3Q3 ................................................................................. 24 Hình 2- &DPHUDQKұQGLӋQJyFWӑDÿӝFӫDFKHVVERDUG ................................................. 24 Hình 2- 22 6ѫÿӗNKӕLÿLӅXNKLӇQWәQJTXiW ...................................................................... 26 Hình 2- ;iFÿӏQKYӏWUtWѭѫQJÿӕLWKHRWKӡLJLDQWKӵFGӵDWUrQWKӏJLiFPi\WtQK ........ 27 Hình 2- 24 %ӝÿLӅX/RZOHYHOFRQWUROErQWURQJ3L[KDZN ................................................ 27 Hình 3- 1 6ѫÿӗNKӕLNӃWQӕLSKҫQFӭQJ ............................................................................ 29 Hình 3- 2 0ҥFKFkQEҵQJPixhawk 2.4.8 ........................................................................... 30 Hình 3- 3 &ҩXWU~FErQWURQJFӫDPӝW(6& ........................................................................ 31 viii Hình 3- 4 ĈӗWKӏVRViQKWKӡLJLDQWUӉJLӳDFiFJLDRWKӭFWUX\ӅQ....................................... 33 Hình 3- 5 *LDRGLӋQSKҫQPӅP%/+HOL ............................................................................. 33 Hình 3- 6 0RGHOFҧPELӃQ/DVHU'istance ToF Sensor GY-53L1..................................... 34 Hình 3- 7 5DVSEHUU\JLDRWLӃSYӟL$UGXLQRWK{QJTXDJLDRWKӭF6HULDO ............................ 35 Hình 3- 8 Arduino Pro Micro ............................................................................................. 35 Hình 3- 9 *LDRWLӃSJLӳDFҧPELӃQ7R)YӟL5DVSEHUU\WK{QJTXD$UGXQR3UR0LFUR ...... 36 Hình 3- 10 0{KuQK4XDGFRSWHUWKӵFQJKLӋP ................................................................... 37 Hình 3- 11 0{KuQKJLDRWLӃSJLӳDFiFSKҫQPӅP ............................................................. 39 Hình 3- 12 *LDRGLӋQOjPYLӋFFӫDSKҫQPӅP0LVVRQ3ODQQHU ......................................... 40 Hình 4- 0{KuQK4XDGURWRUÿѭӧFH[SRUWYjR6LPPHFKDQLFV ......................................... 42 Hình 4- 7әQJTXDQP{KuQKNKLӇQGӵDtrên Matlab-simulink ........................................ 43 Hình 4- 4XDGURWRUEiPWKHRTXӻÿҥRÿѭӧFÿӏQKQJKƭDWUѭӟF ........................................ 43 Hình 4- ĈiSӭQJFӫD4XDGURWRUWKHRWUөF[\Yj] ......................................................... 44 Hình 4- 7tQKLӋXJyF5ROOYj3LWFK.................................................................................. 44 Hình 4- ĈӕLWѭӧQJÿѭӧFELӇXGLӉQWUrQWӑDÿӝWUiLÿҩW ................................................... 45 Hình 4- ĈӕLWѭӧQJÿѭӧFFKLӃXÿӃQKӋWӑDÿӝҧQK ........................................................... 45 Hình 4- 0{KuQKWKӵFQJKLӋP ......................................................................................... 46 Hình 4- &DPHUDQKұQGLӋQÿӕLWѭӧQJ ............................................................................. 46 Hình 4- 4XDGFRSWHUJLӳÿӝFDRP ........................................................................... 47 Hình 4- 0jQKuQKÿLӅXNKLӇQWUҥPPһWÿҩWYjWKXWKұSGӳOLӋX .................................... 47 Hình 4- 9ӏWUtWѭѫQJÿӕLJLӳD4XDGFRSWHUYj ÿӕLWѭӧQJWKHRSKѭѫQJ[ ....................... 48 Hình 4- 9ӏWUtWѭѫQJÿӕLJLӳD4XDGFRSWHUYjÿӕLWѭӧQJWKHRSKѭѫQJ\ ....................... 49 Hình 4- 4XDGFRSWHUÿѭӧFÿLӅXNKLӇQÿӇGX\WUuÿӕLWѭӧQJQҵPWURQJNKXQJҧQK ........ 50 Hình 4- ;k\GӵQJP{KuQKWKtQJKLӋP ......................................................................... 50 Hình 4- ĈӗWKӏEiPÿӕLWѭӧQJÿDQJGLFKX\ӇQ ............................................................. 51 Hình 4- 9ӏWUtWѭѫQJÿӕLJLӳD4XDGFRSWHUYjÿӕLWѭӧQJÿѭӧFÿRWKHRSKѭѫQJ x ......... 51 Hình 4- 9ӏWUtWѭѫQJÿӕLJLӳD4XDGFRSWHUYjÿӕLWѭӧQJÿѭӧFÿRWKHRSKѭѫQJ\ ......... 52 Hình 4- 4XDGFRSWHUÿѭӧFÿLӅXNKLӇQJLӳÿӝFDRNKRҧQJPVRYӟLPһWÿҩW............. 52 Hình 5- 7KӵFKLӋQP{SKӓQJ6,7/WUrQ8EXQWX............................................................. 55 ix &È&.é+,ӊ89¬7Ӯ9,ӂ77Ҳ7 7KXұWQJӳYLӃWWҳW 'LӉQJLҧL UAVs Unmanned Aerial Vehicles IMU Inertial Mesurement Unit GPS Global Position system INS Inertial Navigation system ESC Electronic speed control LQ Linear Quadratic PID Proportional Integral Derivative BLDC Brush-Less Direct Current MAV Micro Aerial Vehicle PWM Pulse-with Modulation UART Universal Asynchronous Reception and Transmission x &+ѬѪ1* TӘNG QUAN 1.1. *LӟLWKLӋX chung 'URQHVKD\4XDGFRSWHUÿѭӧFELӃWÿӃQQKѭOjPӝWSKѭѫQJWLӋQED\NK{QJQJѭӡLOiL Qy Fy NKҧ QăQJ ED\ EҵQJ FiFK ÿLӅX NKLӇQ Wӯ [D KRһF WK{QJ TXD YLӋF Vӱ GөQJ Pi\ WtQK QK~QJ0i\ED\NK{QJQJѭӡLOiL8$9VÿmWUӣWKjQKPӝWFKӫÿӅQJKLrQFӭXSKәELӃQFӫD FiFQKjNKRDKӑFGRWtQKÿDQKLӋPYjOLQKÿӝQJFӫDQyEҵQJFKӭQJOjQyFyWKӇFҩWFiQK YjÿiSWKHRSKѭѫQJWKҷQJÿӭQJFyWKӇJLӳYӏWUtYjGLFKX\ӇQFKұPYӟLOӧLÿLӇPQj\FK~QJ UҩWWKtFKKӧSFKRYLӋFED\WURQJFiFNK{QJJLDQKҥQFKӃYjWKӵFKLӋQFiFWiFYөFKtQK[iF 8$9VQJj\QD\ÿmÿѭӧFWuPUDQKLӅXӭQJGөQJÿDGҥQJWӯOƭQKYӵFTXkQVӵÿӃQF{QJ QJKLӋSQ{QJQJKLӋSOkPQJKLӋSYjGkQGөQJFKRYLӋFNLӇPWUDKRҥFKÿӏQKJLiPViWTXDQ WUҳFYjFӭXKӝ Hình 1- 1 .KiLQL͏PY͉FiFKWL͇SF̵QWK{QJWK˱ͥQJDYj[iFÿ͓QKY͓WUtW˱˯QJÿ͙LG͹D WUrQF̫PEL͇QKuQK̫QKE 0һFGÿmFyVӵWLӃQEӝÿiQJNӇYӅF{QJQJKӋFNJQJQKѭVӭFPҥQKYӅPһWWtQKWRiQ QKѭQJYLӋFÿӏQKYӏFӫD4XDGFRSWRUYүQFzQOjPӝWWKiFKWKӭFOӟQÿһFELӋWWURQJFiFP{L WUѭӡQJEӏKҥQFKӃKRһFNK{QJQKұQÿѭӧF*36ÿLӅXQj\WKұWVӵTXDQWUӑQJÿӕLYӟLFiFWKLӃW EӏED\WӵÿӝQJEӣLOӁQyFҫQSKҧLELӃWYӏWUtFӫDFKtQKQyÿӇWѭѫQJWiFYӟLFiFFKӫWKӇ[XQJ TXDQKÿӇJLҧLTX\ӃWYҩQÿӅQj\ÿmFyUҩWQKLӅXSKѭѫQJiQÿѭӧFÿӅUDQKѭFҫQSKҧLFyVӵ KӛWUӧWӯFiFFҧPELӃQQJRҥLODVHUVFDQQHUVKD\OLGDUOjORҥLFҧPELӃQÿѭӧFVӱGөQJSKә ELӃQWURQJOƭQKYӵFÿӏQKYӏ5RERW+uQK-DWX\QKLrQQyNK{QJWKtFKKӧSYӟL8$9VEӣL YuWҧLWUӑQJYjÿLӋQQăQJWLrXWKөÿiQJNӇ0ӝWFiFKWLӃSFұQNKiFOjVӱGөQJFDPHUD+uQK 1-Eÿk\OjPӝWVӵOӵDFKӑQÿѭӧFVӱGөQJSKәELӃQEӣLJLiWKjQKWKҩSNKӕLOѭӧQJQKҽ ÿһFELӋWSKѭѫQJSKiSQj\JL~SJLҧPÿiQJNӇNKӕLOѭӧQJWtQKWRiQWK{QJTXDYLӋF[iFÿӏQK YӏWUtWѭѫQJÿӕLÿӃQÿӕLWѭӧQJWKD\YuSKҧLWKXWKұSKӃWFiFWK{QJWLQYӅP{LWUѭӡQJÿӇWKӵF KLӋQFiFFKLӃQOѭӧFÿLӅXNKLӇQ+uQK-1a). 1 1.2. 0өFWLrXYjSKҥPYLFӫDÿӅWjL 0өFWLrXOXұQYăQ 7KLӃWNӃYjWKӵFQJKLӋPKӋWKӕQJÿLӅXNKLӇQ4XDGURWRUGӵDWUrQFiFGӳOLӋXFҧPELӃQ GӳOLӋXҧQKÿҫXYjRJLҧLSKiSVӁEDRJӗPFiFWKLӃWEӏWKѭѫQJPҥLVҹQFyWUrQWKӏWUѭӡQJ FiFPmQJXӗQPӣW\FKӍQKYjFKLSKtWKҩS'ӳOLӋXWӯFҧPELӃQFDPHUDYjYLӋFWính toán ÿѭӧFWKӵFKLӋQKRjQWRjQWUrQPi\WtQKQK~QJYjÿѭӧFOҳSÿһWWKHRP{KuQK 0ͭFWLrXFKtQKFͯDÿ͉WjLJ͛PFy - 7KLӃWNӃEӝÿLӅXNKLӇQNӃWKӧSYӟLWKӏJLiFPi\WtQKÿӇÿLӅXNKLӇQ4XDGFRSWHU EiPWKHRÿӕLWѭӧQJGLÿӝQJ - 7uPKLӇXJLҧLWKXұW[ӱOêҧQK QKұQGҥQJÿӕLWѭӧQJJLҧLWKXұWѭӟFOѭӧQJYӏWUt FӫDÿӕLWѭӧQJWӟLFDPHUD - ;k\GӵQJP{KuQKYjP{SKӓQJWUrQSKҫQPӅP0DWODE - 7KtQJKLӋPWUrQP{KuQKWKӵF ܼா ܻா 0,8-1m ܺா Hình 1- 2 7͝QJTXDQP{KuQK4XDGFRSWHUEiPWKHRÿ͙LW˱ͫQJGLÿ͡QJ 2 6̫QSḴPÿ̩Wÿ˱ͫF - ;ӱOêҧQKYjSKiWKLӋQÿӕLWѭӧQJWӯWUrQFDR - 4XDGFRSWHUÿѭӧFÿLӅXNKLӇQӣWҫPFDR[iFÿӏQKNKRҧQJ-PVRYӟLPһWÿҩW - 4XDGFRSWHUFyWKӇEiPWKHRÿӕLWѭӧQJYӟLYұQWӕFNKRҧQg 0,3m/s. 7ͳFiFPRQJPX͙QWUrQP͡WV͙EjLWRiQÿ˱ͫFÿ̿WUDOj - Bài toán mô hình hóa Quadrotor. - 1JKLrQFӭXÿѭDUDSKѭѫQJiQ[iFÿӏQKÿӝFDRFӫDP{KuQKWtQKWӯPһWÿҩW - ;k\GӵQJJLҧLWKXұWÿLӅXNKLӇQYӏWUtFKR4XDGFRSWHU - %jLWRiQOӑFQKLӉXҧQKYjWtQKLӋXÿӑFWӯFҧPELӃQ - 1JKLrQFӭXFiFJLҧLWKXұW[ӱOêҧQKSKiWKLӋQÿӕLWѭӧQJJLҧLWKXұWѭӟFOѭӧQJYӏWUt FӫDÿӕLWѭӧQJWӟLFDPHUDÿӇÿLӅXNKLӇQ4XDGFRSWHUQҵPWURQJNKXQJҧQK - 1JKLrQFӭXSKѭѫQJSKiSJӱLWtQKLӋXÿLӅXNKLӇQWӯWUҥPPһWÿҩWÿӃQQuadcopter, ÿӗQJWKӡLQKұQGӳOLӋXWӯ4XDGFRSWHUWUҧYӅÿӇÿiQKJLiWUҥQJWKiLFKX\ӇQÿӝQJYj KLӋXQăQJFӫDFiFJLҧLWKXұWÿѭӧFQK~QJYjRKӋWKӕQJ - ;k\GӵQJP{KuQKP{SKӓQJWUrQSKҫQPӅP0DWODE &iFFKѭѫQJWLӃSWKHRWUuQKEj\WәQJTXDQFѫVӣOêWKX\ӃWYj FiFSKѭѫQJiQWKӵFKLӋQ ÿӇWjL 3 &+ѬѪ1* &Ѫ6Ӣ/é7+8<ӂ7 2.1. 1JX\rQOêOjPYLӋF mô hình Quadcopter. 1JX\rQOêKRҥWÿӝQJFKtQKFӫDP{KuQKQj\GӵDWUrQVӵFKX\ӇQÿӝQJFӫD dòng khí do cánh TXҥW WҥRUD OӵF KѭӟQJ [XӕQJOjPÿӕLWѭӧQJED\OrQFQJYӟLVӵÿLӅXFKӍQK WӕFÿӝ WӯQJÿӝQJFѫVӁOjPWKD\ÿәLKѭӟQJED\FӫD4XDGcoptor. dƌӇӀĐ ʘ1 ʘ2 ɽ Trái ƴ ʘ4 ʘ3 WŚңŝ Sau ʗ Hình 2- 1 Mô hình Quadrotor [1] ĈӇFyÿѭӧFFKX\ӇQÿӝQJWKҷQJYӅSKtDWUѭӟFWKuWӕFÿӝTXD\FӫDURWRUSKtDVDXSKҧL ÿѭӧFWăQJOrQYjÿӗQJWKӡLSKҧLJLҧPWӕFÿӝFӫDURWRUSKtDWUѭӟFYjWѭѫQJWӵNKL4XDGFRSWHU FKX\ӇQÿӝQJTXDWUiLKD\SKҧL *yF@ 4 F1 F2 M1 M2 Trái dƌӇӀĐ ɽ XB F3 M3 ƴ YB ZB Sau F4 M4 WŚңŝ ʗ OE YE ZE Hình 2- 3 Ĉ͡QJO͹FK͕FK͏WK͙QJ 0{KuQKÿӝQJOӵFKӑFFӫDKӋWKӕQJÿҥWÿѭӧFWKHRPӝWVӕJLҧÿӏQKWѭѫQJÿӕL - 7Kӭ QKҩW 4XDGFRSWHU Oj PӝW NKӕL FӭQJ WURQJ NK{QJ JLDQ Yj GR ÿy SKѭѫQJ WUuQK Newton-(XOHUFyWKӇÿѭӧFiSGөQJÿӇP{WҧÿӝQJOӵFKӑFFӫDKӋWKӕQJ - 7KӭKDL GRVӵWKD\ÿәLYұQWӕF FӫD 4XDGFRSWHUሺ‫ݔ‬ሶ ǡ ‫ݕ‬ሶ ǡ ‫ݖ‬ሶ ሻ WѭѫQJÿӕL QKӓ QrQҧQK KѭӣQJFӫDOӵFYjPRPHQW+XEFyWKӇEӓTXD - 7KӭED4XDGFRSWHUOjPӝWKӋÿӕL[ӭQJWѭѫQJӭQJYӟLEDWUөFWӑDÿӝ[\Yj] 0{KuQKVӁÿӕL[ӭQJTXDWUөF‫ݕ‬஻ , ‫ݔ‬஻ (Hình 2-3PӛLURWRUVӁVҧQVLQKUDPӝWOӵFÿҭ\ ሬሬԦ ‫ܨ‬ఫ và moment ሬሬሬሬԦ ‫ܯ‬ఫ +ӋTXLFKLӃXYұWWKӇ%ሺܱ஻ ǡ ܺ஻ ǡ ܻ஻ ǡ ܼ஻ ሻ OjKӋTXLFKLӃXÿѭӧFJҳQYӟLWKkQ 4XDGFRSWHUYӟLܱ஻ OjJӕFFӫDKӋWUөFWӑDÿӝÿѭӧFJҳQWҥLWUӑQJWkPFӫD4XDGFRSWHUYjKӋ TXLFKLӃXJҳQYӟL PһWÿҩWሺܱா ǡ ܺா ǡ ܻா ǡ ܼா ሻ. 5 Rj MJ Hj FJ v ʘj Gj X ƴ Y O ɽ Z ё ʗ Hình 2- 4 0RPHQWYjFiFO͹FWiFÿ͡QJOrQP͡WFiQKTX̩WW͹GR>@ &iQKTXҥWWKӭMTXD\WURQJPһWSKҷQJWӑDÿӝ2[\YӟLWӕF ÿӝJӕFOj߱௝ FiQKTXҥW FKX\ӇQÿӝQJWKHRSKѭѫQJ QJDQJWURQJNK{QJNKtYӟLYұQWӕF‫ݒ‬Ԧ YjTXD\WUrQPӝWWUөFWѭѫQJ ÿӕL /ӵFÿҭ\KѭӟQJOrQOjNӃWTXҧFӫDPRPHQ[RҳQ‫ܯ‬௝ QJѭӧFKѭӟQJYӟL߱௝ WUrQWUөF URWRUPRPHQWQj\ÿѭӧFWҥRUDEӣLQăQJOѭӧQJÿLӋQiSYjGzQJÿѭӧFFXQJFҩSYjRPӛL rotor. ̪QKK˱ͧQJY͉P̿WNKtÿ͡QJK͕F 6ѭGLFKX\ӇQQJDQJFӫD4XDGFRSWHUYӟLYұQWӕF‫ݒ‬Ԧ WҥRUDOӵF+XE‫ܪ‬௝ >@OӵFQj\Oj WәQJKӧSFiFOӵFWKHRSKѭѫQJQJDQJWiFGөQJOrQWҩWFҧFiFSKҫQWӱFӫDFiQKTXҥWQJѭӧF FKLӅXYӟLYұQWӕF‫ݒ‬Ԧ YjFyÿӝOӟQWӹOӋWKXұQYӟLGLӋQWtFKYjEiQNtQKFiQKTXҥWEuQKSKѭѫQJ Moment Hub ܴ௝ Jk\UDWUҥQJWKiLQkQJNK{QJFkQEҵQJJLӳDFiFFiQKWLӃQYjOLҦQKKѭӣQJ KӗLFKX\ӇQFӫDFiFFiQKTXҥWNKLTXD\Jk\UDPRPHQWKӗLFKX\ӇQ‫ܩ‬௝ WѭѫQJӭQJYӟLYұQWӕF JӕFȳ. 6 z z Z Z +ɽ Y +ƴ +ƴ z Z Y +ʗ y y Y +ɽ X X x +ʗ x x X Hình 2- 5 6͹[RD\FͯDWUͭFX, Y, Z 9ͣL Roll: Sӵ quay cӫa góc ߮ TXDQKWUөF[5x Pitch: Sӵ xoay cӫa góc ߠ quanh trөc y: Ry Yaw: Sӵ xoay cӫa góc ߰ quanh trөc z: Rz 7URQJÿy ͳ ܴ௫ ൌ ൭Ͳ Ͳ Ͳ ܿ‫߶ݏ݋‬ ‫߶݊݅ݏ‬ ܿ‫ߠݏ݋‬ ܴ௬ ൌ ൭ Ͳ െ‫ߠ݊݅ݏ‬ ܿ‫߰ݏ݋‬ ܴ௭ ൌ ൭ ‫߰݊݅ݏ‬ Ͳ ‫߶ܥ߰ܥ‬ ܴ ൌ ܴ௑ Ǥ ܴ௒ Ǥ ܴ௓ ൌ ൭ ܵ߰‫ߠܥ‬ െܵߠ Ͳ ͳ Ͳ Ͳ െ‫߶݊݅ݏ‬൱ ܿ‫߶ݏ݋‬ (2 - 2) ‫ߠ݊݅ݏ‬ Ͳ ൱ ܿ‫ߠݏ݋‬ (2 - 3) െ‫߰݊݅ݏ‬ ܿ‫߰ݏ݋‬ Ͳ ‫߶ܵߠܵ߰ܥ‬ ܵ߰ܵߠܵ߶ ‫߶ܵߠܥ‬ 7 Ͳ Ͳ൱ ͳ (2 - 1) ‫ ߶ܥߠܵ߰ܥ‬൅ ܵ߰ܵ߶ ܵ߰ܵߠ‫ ߶ܥ‬െ ܵ߶‫߰ܥ‬൱ ‫߶ܥߠܥ‬ (2 - 2) y Hình 2- 6 4XDGFRSWHUÿ˱ͫFÿL͉XNKL͋QWK{QJTXDV͹WKD\ÿ͝L W͙Fÿ͡ÿ͡FO̵SFͯDPRWRU CiFÿҫXYjRÿLӅXNKLӇQQKѭVDX: ‫ܨ‬ ‫ݑ‬ଵ ܾ ߬థ ‫ݑ‬ଶ Ͳ ቎‫ ݑ‬቏ ൌ ൦ ߬ ൪ ൌ ൦ ଷ ఏ െ݈ܾ ‫ݑ‬ସ ߬ట ݀ ܾ െ݈ܾ Ͳ െ݀ ܾ Ͳ ݈ܾ ݀ ଶ ܾ ‫߱ۍ‬ଵ ‫ې‬ ଶ ݈ܾ ‫߱ێ‬ଶ ‫ۑ‬ ൪ ‫ ێ‬ଶ‫ۑ‬ Ͳ ‫߱ێ‬ଷ ‫ۑ‬ െ݀ ‫߱ۏ‬ସଶ ‫ے‬ (2 - 6) 7URQJÿyܾ ൐ Ͳ và ݀ ൐ Ͳ OjKDLWK{QJVӕSKөWKXӝFYjRPұWÿӝNK{QJNKtKӋVӕKuQK KӑF, KӋVӕQkQJYjÿӝQJKLrQJFӫDFiQKTXҥW߱ଵǡଶǡଷǡସ lҫQOѭӧt là tӕFÿӝ góc cӫa 4 motor và ‫ ܨ‬ÿҥL GLӋQ FKR WәQJ OӵF QkQJ FӫD 4XDGcopter WKHR WUөF ]߬థ và ߬ఏ ÿҥL GLӋQ FKR PRPHQW [RҳQWKHR góc UROOYjSLWFKWѭѫQJӭQJYj߬ట ÿҥLGLӋQFKRPRPHQW[RҳQWKHR góc yaw. ÈS GөQJ ÿӏQK OXұW 1HZWRQ-(XOHU FKR KӋ FKR WD SKѭѫQJ WUuQK FKX\ӇQ ÿӝQJ FӫD Quadrotor QKѭVDX ቊ ݉ߞ ሷ ൌ ‫ܨ‬௧௛ ൅ ‫ܨ‬ௗ ൅ ‫ܨ‬௚ ‫ܬ‬ȳሶ ൌ ‫ ܯ‬െ ‫ܯ‬௚௣ െ ‫ܯ‬௚௕ െ ‫ܯ‬௔ (2 - 3) 7URQJÿy ‫ܨ‬௧௛ ൌ ܴሺ߶ǡ ߠǡ ߰ሻሾͲǡͲǡ σସ௜ୀଵ ‫ܨ‬௜ ሿ் ÿҥi diӋn cho lӵc nâng cӫa 4 rotors ‫ܨ‬ௗ ൌ ݀݅ܽ݃ሺ݇ଵ ǡ ݇ଶ ǡ ݇ଷ ǡ ݇ସ ሻߞ ்ሶ ÿҥi diӋn cho lӵc cҧn không khí. ‫ܨ‬௚ ൌ ሾͲǡͲǡ ݉݃ሿ் ÿҥi diӋn cho trӑng lӵc cӫa quadcopter. ் ‫ ܯ‬ൌ ൣ߬థ ǡ ߬ఏ ǡ ߬ట ൧ ÿҥi diӋn cho moment xoҳn roll, pitch và yaw. ‫ܯ‬௚௣ ǡ ‫ܯ‬௚௕ là moment quay hӗi chuyӇn. ் ‫ܯ‬௔ ൌ ݀݅ܽ݃ሺ݇ସ ǡ ݇ହ ǡ ݇଺ ሻൣ߶ሶ ଶ ǡ ߠሶ ଶ ǡ ߰ሶ ଶ ൧ là moment xoҳn gây ra do lӵc cҧn không khí. 8