ROBOT 2 BÁNH TỰ CÂN BẰNG FULL

I HC QUC GIA TP H CHÍ MINH TRNG I HC BÁCH KHOA KHOA C KHÍ LUN VN TT NGHIP I HC XE HAI BÁNH T CÂN BNG DI CHUYN TRÊN A HÌNH PHNG MÃ NGÀNH: 128 SVTH :MAI TUN T CBHD :KS. VÕ TNG QUÂN CHNG TRÌNH ÀO TO K S CHT LNG CAO KHÓA 2: 2000 – 2005 TP. H CHÍ MINH, 07/2005 I HC QUC GIA TP H CHÍ MINH TRNG I HC BÁCH KHOA KHOA C KHÍ LUN VN TT NGHIP I HC XE HAI BÁNH T CÂN BNG DI CHUYN TRÊN A HÌNH PHNG MÃ NGÀNH:128 SVTH :MAI TUN T MSSV :P0000016 CBHD :KS. VÕ TNG QUÂN CHNG TRÌNH ÀO TO K S CHT LNG CAO KHÓA 2: 2000 – 2005 TP. H CHÍ MINH, 07/2005 Lôøi caûm ôn Tôi không th theo đui và hoàn thành đ tài ca lun vn trong vòng 16 tun nu không có s giúp đ ca nhng ngi thân và ngi bn xung quanh. Do vy, vi s trân trng và cm kích, tôi xin gi li cm n đn ông bà và cha m, nhng ngi thân trong gia đình ht lòng chm sóc, an i khi gp tr ngi và đng viên tôi trong thi gian thc hin lun vn, xin cm n TS. Nguyn Vn Giáp và giáo viên tr c tip hng dn lun vn, thy Võ Tng Quân đã cho phép tôi theo đui đ tài và cho nhng li khuyên xác đáng, kp thi nhng lúc gp khó khn khi thc hin trong sut quá trình làm lun vn tt nghip đi hc. Ngoài ra, tôi cng xin chân thành cm n anh Quân và anh Kiên  công ty máy tính Bách Khoa đã h tr mt phn kinh phí và thit b đ thc hin đ tài; cm n ngi anh – Th.S Trn Công Binh, gi ng viên b môn Thit b in – nhit tình giúp đ v mt lý thuyt đ hoàn thành phn đin đng c công sut cao, mt phn khá hóc búa ca đ tài. Ngoài ra, cng xin cm n Thy và Tâm, hai ngi bn thân thit nht đã giúp tôi hoàn thành bn thuyt minh mà chúng ta đang có trên tay. Cui cùng em xin cm n tt c quý Thy Cô tham gia ging dy chng trình K s cht lng cao Vit Pháp khóa 2000-2005, và Khoa C Khí, b môn C in t, Trng i Hc Bách Khoa TP.HCM đã trang b cho em nhng kin thc c s cng nh đã giúp đ tôi trong thi gian làm Lun vn tt nghip. Tp. H Chí Minh, ngày 03 tháng 07 nm 2005 Mai Tun t SVTH: Mai Tun t Li cm n Mc lc i Tóm tt đ tài iv Abstract v CHNG 1 TNG QUAN 1 1.1 Li nói đu 1 1.2 Th nào là xe hai bánh t cân bng (two wheels self balancing) 2 1.3 Ti sao phi thit k xe hai bánh t cân bng 3 1.4 u nhc đim ca xe hai bánh t cân bng 4 1.4.1 u đim ca xe scooter t cân bng trên hai bánh 4 1.4.2 Nhc đim ca xe 4 1.5 Kh nng ng dng 5 1.6 Tình hình nghiên cu trong và ngoài nc 5 1.6.1 Mt s dng xe hai bánh t cân bng dùng trên robot 5 1.6.2 Mt s dng scooter hai bánh t cân bng 9 1.7 Nhu cu thc t 14 CHNG 2 NHIM V LUN VN 15 2.1 Mc tiêu đ tài 15 2.2 Phng pháp nghiên cu 15 CHNG 3 LÝ THUYT TIP CN 17 3.1 Phng pháp tính đng lc hc 17 3.2 Thut toán điu khin - K thut điu khin hin đi 24 3.3 Các phng pháp x lý tín hiu t cm bin 29 3.3.1 Lc b ph thông tn (complementaty filter) 29 3.3.2 Lc thích nghi - B lc Kalman 32 3.3.3 So sánh các b lc vi b lc Kalman 40 3.4 Mô hinh lý thuyt đng c  DC 43 CHNG 4 TÍNH TOÁN MÔ PHNG 45 4.1 Các thông s trong mô hình mô phng đc xây dng 45 i SVTH: Mai Tun t 4.2 Mô phng MatLAB 46 4.2.1 Gii thiu v phn mm MatLAB, công c Simulink 46 4.2.2 Kt qu tính bng MatLAB 46 4.3 Mô phng VisualNastran và Simulink 48 4.3.1 Gii thiu v phn mm VisualNastran 48 4.3.2 Cách thc hin mô phng bng vN Desktop 4D 49 4.3.4 Kt qu mô phng 50 CHNG 5 THC HIN 54 5.1 Thit k c khí 54 5.1.1 Tóm tt thit k 54 5.1.2 Tính toán sc bn 54 5.2 Mch đin t 59 5.2.1 Ngun đin 60 5.2.2 Mch công sut điu khin đng c 61 5.2.2.1 B đm (MOSFET driver) 61 5.2.2.2 MOSFET công sut – mc b ph 63 5.2.2.3 Mch Snubber 66 5.2.2.4 MOSFET thng 66 5.2.3 Cm bin 66 5.2.3.1 Thit b đo góc gyro Murata ENC-03 67 5.2.3.2 ADXL202A 68 5.2.3.3 Cm bin đo v trí- encoder 73 5.2.3.4 Cm bin đo dòng hi tip (in tr shunt) 75 5.2.4 B x lý trung tâm - vi điu khin PIC 18F452 76 5.2.4.1 Các kh nng ca vi điu khin Microchip PIC 18F452: 76 5.2.4.2 Mch điu khin trung tâm 79 5.2.5 Bng điu khin và hin th 80 5.2.6 ng c 80 5.2.7 Hình chp các mch đin t 85 5.3 Gii thut - Lu đ chng trình 88 5.3.1 Chng trình chính 88 5.3.2 Chng trình ngt 89 5.3.3 Cp nht encoder 91 5.3.4 iu khin đng c 92 5.4 Kt qu 94 CHNG 6 CÁCH VN HÀNH 95 6.1 Cách s dng 95 6.2 Bo dng 97 CHNG 7 KT LUN 98 ii SVTH: Mai Tun t 7.1 Nhng kt qu đt đc 98 7.2 Nhng kt qu cha đt đc 98 7.3 Nhng vn đ cha gii quyt 99 7.4 Hng phát trin 99 TÀI LIU THAM KHO 100 PH LC 102 1. GII THIU PHN MM VISUALNASTRAN 102 2. LC THÍCH NGHI – B LC KALMAN 105 3. GYRO MURATA ENC-03 118 4. CM BIN GIA TC ACCELEROMETER ADXL202 122 5. CHUN TRC CÁC CM BIN O GÓC 128 6. TÍNH NNG VI IU KHIN PIC 18FXX2 131 iii SVTH: Mai Tun t TÓM TT  TÀI  tài này có th xem là mt cu ni kinh nghim t mô hình thng bng con lc ngc đn vic nghiên cu và ch to các loi robot hai chân và robot ngi (humanoid robot) trong tng lai. Mc tiêu ca đ tài là thit k và ch to mt xe hai bánh t cân bng, da trên lý thuyt cân bng con lc ngc. Không ging nh các xe scooter hay xe 2 bánh thông thng có hai bánh xe nm trc sau, xe scooter trong đ tài có hai bánh nm song song vi nhau, giúp nó tr nên cc k gn gàng đ di chuy n bng nhng bánh xe trong nhng khong cht hp mà thng ch có th đi b.  tài này đc quan tâm t vic tính toán các thông s đu vào và ra, da trên đó đ xây dng các mô phng, đn vic thit k mô hình, thc hin phn đin t và điu khin, vit các chng trình điu khin vi mc đích cui cùng là to ra mt mô hình xe di chuyn cân bng trên hai bánh xe đ ng trc đc lp trên hai đng c da theo các đnh lut c hc Newton và c hc vt rn: điu khin đ luôn duy trì b mt chân đ (hai bánh xe)  v trí ngay di trng tâm ca xe khi đng yên, và to mt sai s nh v góc nghiêng ca thân xe vi nn khi mun xe chuyn đng. S cân bng đc tính toán và mô phng bng 2 phn mm MatLAB- SIMULINK và Visual Nastran, đ chng minh r ng hoàn toàn có kh nng đ điu khin mt mô hình xe t cân bng ch nh mt h thng điu khin hot đng ca đng c đin gn trên mi bánh xe. Mô hình bao gm mt thân mang hai đng c DC đc tích hp trong mi bánh xe đp đin 400 mm ph bin trong thi gian gn đây ti Vit Nam, bo mch s dng b đ iu khin trung tâm PIC18Fxxx ca hãng Microchip đ điu khin nhng mch khuch đi công sut, lái công sut (MOSFET driver) cho nhng đng c, điu khin nhng cm bin cn thit đ đo các giá tr góc và quãng đng đi. Các tín hiu đo góc t hai cm bin accelerometer và gyro đc thông qua mt b lc Kalman đc lp trình trên vi điu khin PIC đ có các thông s đo góc chính xác. Bng đi n kim soát và hin th chc nng hot đng ca xe. Bình đin đc lp di sàn xe bng nhôm đ cung cp toàn b nng lng cho xe hot đng. iv SVTH: Mai Tun t ABSTRACT This project can be an useful experiment to the research and manufacture in balancing robot and humanoid robot in future. The main purposes of my project are designing and manufacturing a self-balancing scooter, based on the theory of the balancing inverted pendulum. It is unlike the popular scooter or bicycle, which have two wheels being in a same surface (the wheel’s axes are parallel). Its parallel wheels configuration make it compact enough to be maneuvered through most pedestrian spaces that accommodate wheelchairs. Calculating parameters of the model to construct the simulation, designing the model, making electronic boards and controller, and programming the microcontroller are the missions in the project, to reach the main goal of building a scooter that could balance in its two coaxial wheels driven by two intergrated motors. The method analysing the auto-balancing scooter’s dynamic is roughly based on Newton’s laws and mechanics of solid. To keep the scooter remains balanced when scooter don’t move, it must drive the wheels staying under the scooter’s gravity, and making a small error in tilt angle (angle of the chassis with respect to the ground) when the scooter moves. The balance of scooter is also calculated and simulated by MatLAB- SIMULINK and Visual Nastran, to show that it is clearly possible to control such a system using an electric motor mounted on each of the two wheels. The self-balancing scooter is structured of a chassis carrying two wheels coupled a DC motors for each. The wheel which is used in my final project is a wheel of electric bicycle (400 mm of diameter), lately popular in Viet Nam. PIC18Fxxx, a micro-controller of Microchip’s family is used to implement as the main controller of scooter’s system, manages the works of the electric power amplifiers, MOSFET driver for the motors and of the necessary sensors to measure the vehicle’s states. To have the exact information of angle received from the noisy accelerometer and piezo-electric gyro, a discrete Kalman filter is implemented in PIC microcontroller. A control board is used to display the state of sensors, operation of scooter and to control the speed and steering. Batteries are bolted under the chassis of scooter, supply electric energies for scooter’s operation. v SVTH: Mai Tun t Chng 1 Tng quan Chng 1 TNG QUAN 1.1 Li nói đu Bài lun vn xut phát t ý tng đã đc thng mi hóa ca công ty Segway: kt hp ý tng v cách gi thng bng ca con ngi trên đôi chân và đ c đng trong di chuyn ca các loi xe di chuyn bng bánh. Thông qua bài nghiên cu, ta có th phn nào nm bt nhng ý tng gi thng bng cho các loi humanoid robot (robot dng ngi), cách phi hp và x  lý tín hiu tt nht t cm bin. Tuy vy, giá thành ca sn phm Segway không r (khong 5000USD/xe) do chi phí rt cao t các cm bin đã đc tích hp và x lý vi đ chính xác và tin cy cao (khong 900USD/b). Do vy, chúng ta s tìm cách kt hp các cm bin riêng l vi giá thành thp (4 - 40USD/cm bin) và x lý tín hiu cm bin ca chúng đ có đc các tín hiu tinh khit và chính xác nh mong mu n vi giá thành không cao. Mô hình là mt chic xe có hai bánh đc đt dc trc vi nhau (khác vi xe đp là trc ca hai bánh xe song song). Trên mô hình s dng các cm bin đ đo góc nghiêng ca thân xe, vn tc quay (lt) ca sàn xe quanh trc bánh và vn tc di chuyn ca xe so vi mt đt. Nh các cm bin này, xe s có th t gi thng bng và di chuyn. Vi cu trúc này, trng tâm ca mô hình phi luôn nm trong vùng đ ca bánh xe (supporting area) đ có th thng bng khi di chuyn  mi b mt t đn gin đn phc tp. Trong h thng các cm bin, đ loi tr các tín hiu nhiu t h thng và nhiu t tín hiu đo, sai s ca ngõ ra, đng thi có th c lng chính xác giá tr đo trong tng lai ca cm bin cng nh k t hp các tín hiu, b lc Kalman đc nghiên cu và s dng nhm cho mt kt qu ti u v tình trng ca xe gm góc nghiêng, vn tc quay ca xe t mô hình và các cm bin thành phn. Nói cách khác, h thng x lý tín hiu và lc Kalman là công c đ bin các cm bin đn gin, giá r thành tp hp cm bin có giá tr trong h thng. T các tín hiu đo, thông qua m t s đi lng đc trng ca mô hình (khi lng, chiu dài, chiu cao vt, đng kính bánh…) ta s tính đc momen quán tính nghiêng (lt ca mô hình), t đó đa ra các giá tr điu khin phù hp cho các bánh xe đ gi cho mô hình luôn đng vng hoc di chuyn vi mt vn tc n đnh. Toàn b mô hình đc điu khin bng mt vi điu khin PIC 18F452. ây là th h tng đi cao cp ca h PIC có th x lý và thc thi chng trình  tc đ cao (đt đn 10MIPs) trong vic tính toán các giá tr cm bin và đa ra b truyn đng (đng c đin). B vi điu khin đóng vai trò th nht trong đ tài nh mt b lc Kalman vi tín hiu vào t thit b inclinometer và gyro. Vi các d li u v góc đã x Trang 1 SVTH: Mai Tun t Chng 1 Tng quan lý và tín hiu hi tip v v trí đo encoder đa v (incremental encoder), vai trò th hai ca vi điu khin trong đ tài s tính toán và đa ra tín hiu điu khin b truyn đng, đn bánh xe đ gi thng bng/di chuyn, đi thng, quay, quo. ây là mt phng tin vn chuyn mi ti các thành ph trong tng lai vi nhiu u đim: gn, nh , ít chim din tích đng ph, d mang vác, tháo lp và vn chuyn, nhiên liu sch, d điu khin cho ngi ln và tr em, đi đc trên mt s đa hình phc tp. 1.2 Th nào là xe hai bánh t cân bng (two wheels self balancing) B nghiêng Cân bng Hình 1.1 Mô t nguyên lý gi thng bng i vi các xe ba hay bn bánh, vic thng bng và n đnh ca chúng là nh trng tâm ca chúng nm trong b mt chân đ do các bánh xe to ra. i vi các xe 2 bánh có cu trúc nh xe đp, vic thng bng khi không di chuyn là hoàn toàn không th, vì vic thng bng ca xe da trên tính cht con quay hi chuyn  hai bánh xe khi đang quay. Còn đi vi xe hai bánh t cân bng, là loi xe ch có hai bánh vi trc ca hai bánh xe trùng nhau, đ cho xe cân bng, tr ng tâm ca xe (bao gm c ngi s dng chúng) cn đc gi nm ngay gia các bánh xe. iu này ging nh ta gi mt cây gy dng thng đng cân bng trong lòng bàn tay. Thc ra, trng tâm ca toàn b scooter không đc bit nm  v trí nào, cng không có cách nào tìm ra nó, và có th không có kh nng di chuyn bánh xe đ nhanh đ gi nó luôn  di toàn b trng tâm. V mt k thu t, góc gia sàn scooter và chiu trng lc có th bit đc. Do vy, thay vì tìm cách xác đnh trng tâm nm gia các bánh xe, tay lái cn đc gi thng đng, vuông góc vi sàn xe (góc cân bng khi y là zero). Hình 1.2 Mô t cách bt đu di chuyn Trang 2 [...]... x bánhTB M bánh R ) [3-43] c: [3-44] C R x bánh TB 0 M thân ( M thân L (2 M bánh M thân ) Gi i h ph M bánh R C R 0 M thân ) x bánhTB ( M thân L C M thân L M thân x bánh TB C R x bánh TB LM thân cos 2 M thân x bánhTB 4 L g 3 [3- 42] gL M thân sin X p x các giá tr sin, (1+sin2), cos, ta ( 2 M bánh 1 M bánh R 2 2 2 R M thân L(sin ) M thân x bánhTB 2 M bánh x bánh TB LM thân cos ng trình, ta M bánh. .. thân L2 J thân gL M thân sin (1 sin )C 2 2 M bánh x bánhTB M thân L(sin ) [3-38] x bánhTB LM thân cos J 2 bánh R M thân L(cos ) C R M thân x bánhTB [3-39] Momen quán tính c a thân xem nh là m t o n th ng hình tr chi u dài 2L, bánh xe xem nh là m t d a tròn xoay: J thân [3-40] J bánh 1 M thân L2 3 1 M thân L2 3 1 M bánh R 2 2 [3-41] M thân L2 gL M thân sin (1 sin )C 2 2 M bánh 4 M thân L2 3... RL x RR ) [3 -24 ] 2 x RM M bánh ( x RL x RR ) (H L H R ) ( H TL [3 -25 ] H TR ) Mà RL J RL CL [3 -26 ] H TL R (C L C R ) ( M bánh ( x RL x RR ) x thân M thân RL R J RL RR J RR ) [3 -27 ] CL 1 C 2 1 C 2i [3 -28 ] CR 1 C 2 1 C 2i [3 -29 ] CL CR c o so v i trung t a bánh J thân [3-30] C Galilée t nh: M thân gL sin( ) (1 sin 2 ( ))(C L C R ) [3-31] M thân L y thân sin( ) x thân cos( ) 2 M bánh x RM (C L C... ta M bánh R ) c: Trang 22 M thân C (2 M bánh M thân ) R [3-45] SVTH: Mai Tu n Ch t C M thân L 4 L g 3 x bánh TB ng 3 Lý thuy t ti p c n M thân 4 ( M thân L M bánh R) M thân L 3 (2M bánh M thân ) M thân C (2M bánh M thân ) R gM thân [3-46] X M thân C (2 M bánh M thân ) R gM thân t: X gM thân X T ó, ta có h ph gM thân X x bánhTB 4 gM thân L X 3 x bánh x bánh 0 0 g 1 0 0 0 0 xbánh [3-48] [3-49] [3-50]... gM thân L X 3 x bánhTB [3-47] C L M thân 4 ( M thân L M bánh R ) M thân L 3 (2 M bánh M thân ) 1 M thân (2M bánh M thân ) R Y C M thân L 4 L g 3 x bánhTB [3- 52] g 4 LY 3X 0 0 4 M thân L X 3 0 gM thân X x bánh 1 0 0 0 x bánh 0 0 1 0 0 1 M thân L [3-53] C 0 x bánh x bánh 1 0 0 C 0 Trang 23 4 LY 3X 1 M thân L C 0 Y X [3-54] [3-55] C L SVTH: Mai Tu n Ch t ng 3 Lý thuy t ti p c n 3 .2 Thu t toán i u... hai bánh t cân b ng ch c thi t k cho m t ng i s d ng 1.3 T i sao ph i thi t k xe hai bánh t cân b ng Nh ng mobile robot xây d ng h u h t robot là nh ng robot di chuy n b ng ba bánh xe, v i hai bánh lái c l p ráp ng tr c, và m t bánh uôi nh Có nhi u ki u khác nhau, nh ng ây là ki u thông d ng nh t Còn i v i các xe 4 bánh, th ng m t u xe có hai bánh truy n ng và u xe còn l i c g n m t ho c hai bánh. .. ( x thân M thân RL J RL RR R J RR ) [3- 32] C R 2 M bánh x RM x thân M thân 2 J bánh bánh [3-33] R C là giá tr trung bình c a CL và CR ythân xthân L (1 cos ) xbánhTB [3-34] L sin y thân L(sin ) x thân [3-35] x bánhTB L (cos ) y thân L(sin ) L(cos ) x thân 2 [3-36] x bánh TB L (cos ) L (sin ) 2 Do ó: y thân sin x thân cos L x bánhTB cos Trang 21 [3-37] SVTH: Mai Tu n Ch t H ph ng 3 Lý... c a h th ng, ma tr n h s tr ng thái c a nó nh sau: 0 1 x bánh x bánh 0 0 0 g [3-64] 0 4 gM thân L 3 X 0 0 x bánh x bánh 0 0 0 0 0 gM thân X 1 4 LY 3X 0 1 M thân L C 0 Y X x bánh xbánh 1 0 0 0 x bánh 0 0 1 0 0 C 0 [3-65] v i X 4 M thân L 3 M thân ( M thân L (2 M bánh M thân ) Trang 27 [3-66] M bánh R ) SVTH: Mai Tu n Y Ch t 1 M thân (2M bánh M thân ) R ng 3 Lý thuy t ti p c n 1 L [3-67] Ngõ vào... t h p hai ph I mL2 NL cos [3-5] I ng trình trên: mgL sin mL x cos [3-6] T [3-3]và [3-6] x p x tuy n tính hóa t i 0o: M I m x b x mL mL2 mgL F [3-7] mL x [3-8] Trang 18 SVTH: Mai Tu n Ch t ng 3 Lý thuy t ti p c n Hàm tr ng thái (space-state): 0 x x I 0 I M 0 0 I M 1 mL2 b m MmL2 0 mLb m MmL2 0 m 2 gL2 I M m MmL2 0 mgL( M m) I M m MmL2 0 0 x x I I M 1 0 I M 0 mL2 m MmL2 F 0 mL m MmL2 [3-9] x x 1 0 0... [3 -20 ] L Th [3-19] và [3 -20 ] vào [3-18]: J thân J thân (C L C R ) ) L sin 2 ( ) x thân M thân L cos( ) (C L C R ) [3 -21 ] L ( y thân M thân M thân g M thân L y thân sin( ) x thân cos( ) M thân gL sin( ) (1 sin( Xét v i m i bánh trái và ph i: Trang 20 2 ))(C L C R ) [3 -22 ] SVTH: Mai Tu n Ch t ng 3 Lý thuy t ti p c n x RL M RL H TL HL H TR HR [3 -23 ] x RR M RR M RL M RR M bánh ; ( x RL x RR ) [3 -24 ]