LI MÔ HÌNH HỆ CON LẮC NGƯỢC
4.2.2Thiết lập thuộc tính đối tượng
4. ỉ GIỚI THIỆU PHÀN MÉM MÒ PHỎNG 3D: V1SUALNASTRAN DESKTOP
4.2.2Thiết lập thuộc tính đối tượng
Đ ể thiết lập thông số đối tuợng, người thiết kế chọn đối tượng trong Object m anager và m ở properties. Hình 4 0 chỉ ra thuộc tính cùa đối tượng W heeỉ_Ieft gồm các phần:
- Appearance: gồm tên đối tượng và hình thức xuất hiện - Pos: các tham số vị trí của đối tượng trong hệ toạ độ O xyz. - Vel: tham sổ vận tốc của đối tượng theo các hướng trục
- Material: tham sổ về vật liệu (loại vật liệu, phân bố khối lượng, khối lượng, tham số ma sát,...)
- Geom etry: kích thước hình học của đối tượng.
- N goài ra còn có thể thêm vào các tham số khác để điều chỉnh thuộc tính đối tượng thông qua sử dụng Properties list.
S U B
Appearance I Pos Ị Vel I Material ] Geometry I
Appearance
|v Shown
r Ttack I frames r~ Center of mass shown ,1 r Ĩ ranslucent
Color: j-|
Name wheel lef i Automatic
¿t, Properties of body[9] "wheeljeft“
Close Apply
Tham sổ các đối tượng trong m ô hình x e hai bánh dược thiết kế trong bản luận văn này thoả mãn tính điều khiển được cùa hệ thống thông qua v iệc xác định hệ phương trình trạng thái được chi ra trong chư ơng 1.
4 .2 .3 T h êm tham số đ iều k h iển và đ o lư ò n g
Quá trình thêm tham số điều khiển và đo lường được thực hiện như sau: - Chọn đối tượng
- Chọn meter hoặc control trong m enu Insert của chư ơ n g trình: menu con hiện ra cho phép ta chọn các tham sổ tác đ ộ n g điều khiển và đo lường.
Đ ể đàm bảo việc thực hiện thuật toán điều khiển x e hai bánh tự cân bằng, tham số cùa các đổi tượng sau được thiết lập:
- Tham số điều khiển m ôm en xoắn của hai m ôtơ lần lượt là: + Torque o f M otor_left: M ôm en xoẳn của m ôtơ trái + Torque o f M otor_R ight: M ôm en xoắn cùa m ôtơ phải. - Tham sổ được đo lường giám s á t :
+ Tham số vị trí cùa đế và hai bánh xe: X , x _ w h eel_ left, x_w heel_right
+ Tham sổ g ó c và vận tốc g ó c của khối gia trọng: R z và W z. V ới 2 tham số lối vào điều khiển và 5 tham số lối ra đo lường, phương trình toán được chi ra trong chư ơ ng 1 có thể dễ dàng dược thực hiện.
4.2 .4 T r u y ề n tham số g iữ a V isu a lN a s tr a n và M a tL a b
V iệ c truyền tham số giữa hai phầm m ềm V isualN astran và M atLab được thực hiện thông qua khối v N P la n í cùa M atLab. K hối này cho phép ta gọi một trương trình m ô phỏng 3D được thiết kế bằng V isualN atran với các tham sổ lối vào được liệt kê trong Input và tham số lối ra trong Output (hình 4 1 )
Block Parameters: vNPlant > balancing m
vN Plant
Define the input controls to the vNPIant and the output meteis from the vN Plant. Parameters
vN4D File: [C:\Luan van\Luan van Thac syVmophongMien ket voi matlabV Browse...
Inputs Outputs
Available input controls to vN Plant Available output meters from vN Plant
Input controls to vNPIant Output meters (rom vNPIant (adsbygoogle = window.adsbygoogle || []).push({});
Torque of MotorJeft Torque of Motor_right vNPIjnt Orientation of mass.R2 Orientation of mass.Wz Position of Sole x Position of Sole.x_wheel_left Position of Sole x_wheel_right
vNPIant Sample-Time Interval
Specify interval between I/O samples as an integer multiple of vN4D animation steps: ' 1
Cancel Help
H ình 41. K h ố i vN PIant th ự c hiện tru yền tham s ố g iữ a VisualN as tran v à M atL ab
H ình 41 cho thấy rằng khối vNPIant có hai tham số đầu vào điều khiển là: m ôm en xoắn của bánh x e trái và phải (Torque o f M o to r je ft, Torque o f M otor_right), 5 tham số đầu ra gồm : vị trí xe, bánh xe (Position o f so le .x , Position o f so le .x _ w h e el_ left, P osition o f sole.x_w h eel_righ t), g óc nghiêng của xe so với trục z (O rientation o f m a ss.R z), vận tốc g óc lắc của x e (Orientation o f m ass.W z). 5 tham số lối ra nêu trên được đưa tới môđun tính toán cùa MatLab thực hiện thuật toán LQ R với hệ số phản hồi K (hinh 42). Kết quả tính toán được đưa tới lối vào của khối vNPIant tác đ ộng trở lại m ô hình xe hai b á n h được thiết kế bởi
C S r H ầ . balancing LQR •+ t Scopt wh«*l left ÉẳẳÍ Mi
//m /? ¥2. S ơ dồ thực thi thuật thuật toán đ iều khiển xe hai bánh tự cân b ằn g đư ợc thiết ké bằn g cô n g cụ sim ulin k cùa M atL ab
Dưới đây là đồ thị phụ thuộc của các tham số: góc nghiêng, vận tốc góc của xe theo thời gian khi thực hiện chương trình điều khiển nêu trên.
9 Orientation of lĩìđtt BBS)
Rl(r«d) w v s bme(s)
K in h 43. Đ ồ thị quan hệ g iữ a g ó c nghiêng, vận lố c g ó c cùa x e hai bảnh theo thời gian
4. 3. T H Ử N G H IỆ M X Â Y D ự N G X E T ự C Â N BẢ N G T R Ê N H A I B Á N H 4.3.1 K ết cấ u c ơ k h í của xe
Kích thước: + Cao: 35 cm
+ Khoảng cách giữa hai bánh xe: 17cm Tổng khối lượng: 2kg
Cảm biến gia tốc ADXL202
Con quay vi cơ ADXRS150
Bộ xử lý trung tâm C Y 8 C 2 7 4 4 3 , họ PSoC Thu thập
dừ liệu
Môđun điều khiển động cơ DC
Encoder Môtơ 2
Encoder Môtơ 1
- Trọng tâm xe: cách mặt sàn 15cm
- Động cơ phát động gồm 2 động cơ DC loại Pittme GM913J820 24VDC, hê số giảm tốc 19,7:1, Encoder:500 CPR.
H ình 44. X e tự c â n b ă n g trẽn hai bánh đ ư ợ c x â y d ự n g tro n g bàn luận văn n à y
4.3.2 Thực thi phần cứng và phần mềm điều khiển
Hình 45 chi ra sơ đồ Hệ thống điều khiển xe hai bánh tự cân bằng bao gồm các phần sau:
- Khối cảm biến: sử dụng cảm biến gia tốc ADXL202, cảm biến Gyro ADXRS150 cung cấp các thông tin về góc nghiên và vận tốc góc.
- Khối điều khiển trung tâm: sử dụng chip vi điều khiển CY8C27443 thuộc họ PSoC của hãng Cypress có chức năng xử lý tín hiệu từ các cảm biến, tín toán các thông sổ điều khiển, xuất lệnh điều khiền các cơ cấu chấp hành.
- Khối chấp hành gồm hai động cơ bánh có gắn đĩa lập mã (Encoder) để thu tín hiệu phản hồi về vị trí phục vụ thuật toán điều khiển PID. (adsbygoogle = window.adsbygoogle || []).push({});
Bộ xử lý trung tâm sử dụng chip vi điều khiển PSoC (Program m able System on Chip). Chip này được cấu thành từ m ột lõi vi điều khiển 8 bít và các khối chức năng số và tương tự. M ồi khối chức năng có thể được lập trình ở các mức khác nhau, khối chức năng số có thể được thiết kế như bộ định thời, bộ đếm , bộ tạo xung PW M , bộ tạo chuỗi ngẫu nhiên,...M ỗi khối khi được đưa vào sử dụng sẽ được gọi là môđun người dùng. M ột đặc điểm nổi bật cùa PSoC là việc hồ trợ của các hàm API (A pplication Programming Interface) cho các môđun người dùng. Trong mỗi ứng dụng được thiết kế, chúng ta có thể thay đổi được cấu trúc cùa các môđun người dùng trong PSoC. Hình 4 6 chi ra các môđun người dùng bên trong chip PSoC được sứ dụng trong bản luận văn này gồm:
- Các môdun đếm xung: được sử dụng để đo thời gian xung của cảm biến A D X L 2 0 2 và tín hiệu từ Encoder cùa m ôtơ nhằm xác định g óc nghiêng cùa và vận tốc của xe.
- B ộ biến đổi A D C 12 bít: được sử dụng để biến đổi tín hiệu m ức điện áp tương tự của A D X R S 150 tương ứng thành giá trị vận tốc góc.
Với các nhiệm vụ đã được quy định trên đây, nhiệm vụ đặt ra phải thiết kế hệ thống mạch điện tử (phần cứ ng ) và phát triển chương trình nạp trên chip PSoC.
T h ự c th i p h ầ n cứ n g : S ơ đồ mạch điện tử của hệ thống được chi ra trong hình 47 gồm:
- B ộ điều khiển côn g suất: được thiết kể sử dụng mạch cầu H tích hợp trong chip L298. Đ ây là chip điều khiển động cơ DC chuyên dụng có khả năng chịu được dòng điện 4A . V ới thiết kế này, m ôtơ có thể được điều khiển ở chế độ đóng/m ở hoặc chế độ điều khiển độ rộng xung.
- M ạch vi xử lý PSoC.
T h ự c th i p h ầ n m ềm : Phần m ềm điều khiển nạp trên chip PSoC được viết bằng ngôn ngữ c dưới sự hỗ trợ cùa hàm API mỗi khi môđun người dùng được khởi tạo gồm các môđun chương trình sau:
- Môđun chương trinh đo g óc nghiêng: thực hiện đo thời gian xung của từ lối ra của chíp A D X L 2 0 2 và chuẩn hoá góc.
- Môđun chương trình đo vận tốc góc: thực hiện biến đổi tương tự sổ tín hiệu mức điện áp từ cảm biến A D X R S 1 5 0 .
- M ôđun chương trình xác định g ó c quay của m ôtơ thông qua v iệc đo tín hiệu từ Encoder.
- M ôđun chương trình điều khiển vị trí môtơ.
- Chương trình thử nghiệm điều khiển xe hai bánh tự cân bằng.
Trong bản luận văn này, tác giả đã bước đầu thử nghiệm thuật toán Kalman trong v iệc xử lý tín hiệu góc nghiêng từ hai cảm biến, tuy nhiên do khối lượng tính toán lớn, các phép toán chủ yếu thao tác trên ma trận đã gây khó khăn không nhỏ ảnh hướng đến kết quả thừ nghiệm xe hai bánh tự cân bằng. Bước đầu, xe chi có thể tự giữ cân bàng trong thời gian nhỏ. H y v ọn g trong thời gian tới, bằng v iệc sử dụng các chíp chuyên dụng thực hiện thuật toán Kalman kết hợp với các môđun chương trinh đã xâ y dựng, x e tự cân bằng trên hai bánh có thể hoạt động tốt.
ũebuo tSogim loak £rdo» 3*©
s o »0 t X? ■ R@ □ a • . .«QUÍ '4 teLĩt ** Gtt>«ỉR*iasc«t v*u * CPU.Ckx* i«.M» 39WS«K« PU_Mod» SlMP.TtMI 5»J_M vo-5ỹ*c*v* »2 VC2-VC1/N 10 ve? vao-v«*. 5 $r«c* San« l-4fn». Mt>_1 • Um> MortA. v«t« - Ox» vc» En*he Hự« Comp*»Oui ỉf€-mjtrnrềC)‘J * Uim f-* Sl«K - Pon 0 0 pqq ũubt P«._GJ «IM s«£> Port.a_2 «1*1 Por c 3 pq3| POL0.4 pqi| Gmx p<*_0_5 pq?j StđC» Pon.0.6 nnn iua.
M4 1«U| ‘rartHm 1 )Mn<MỈ mnUì VJ ị
i
Ẽ
XII —gj~
s i
*ĨAJ -Li-
Sau một thời gian nghiên cứu. được sự giúp đỡ, hướng dẫn và chi bảo tận tình của T S. Trần Q uang V inh, cùng với nồ lực cùa bản thân đến nay bản luận văn đã hoàn thành được những nội dung sau:
- Đ ã tìm hiểu về lý thuyết về hệ điều khiển tự cân bằng: lý thuyết con lẳc ngược, lý thuyết điều khiển xe hai bánh tự cân bằng.
- N gh iên cứu triển khai hai cảm biến vi cơ điện tử A D X L 2 0 2 và A D X R S 1 5 0 trong một số bài toán sử dụng thông sổ g óc nghiêng và vận tốc g ó c như: đo góc nghiêng của vật thể chuyển động,...
- N g h iên cứu lý thuyết xử lý tín hiệu góc nghiêng áp dụng cho bài toán c ó nhiễu rung động lớn: thuật toán lọc bù, thuật toán lọc thích nghi Kalm an.
- M ô phỏng thành côn g mô hình xe hai bánh tự cân bàng sử dụng phần m ềm VisualNastran 4 D và côn g cụ Sim ulink cùa MatLab.
- X ây dựng thử nghiệm xe hai bánh tự cân bằng sử dụng cảm biến A D X L 2 0 2 và A D X R S 1 5 0 trên cơ sở vi điều khiển PSoC: xây dụng các môđun chương trình, bước đầu thử nghiệm hệ thống.
M ặc dù đã c ó nhiều cổ gẳng, tuy nhiên với điều kiện trang thiết bị hiện tại, việc xâ y dựng một hệ thống xe tự cân bằng trên hai bánh còn gặp nhiều khó khăn. Trong thời gian tới, trên cơ sở những kết quả đã nghiên cứu cộ n g với việc tìm kiếm một số giải pháp để thực thi thuật toán lọc Kalman trên chíp chuyên dụng, v iệc xây dựng m ột hệ thống x e hai bánh tự cân bằng sẽ hứa hẹn nhiều thành côn g tiến tới xây dựng m ột robot đi bộ tự cân bằng.
1. Pham Duy Hung, Vu V iet Hung, N gu yen Vinh Quang, Tran Q uang Vinh and N g u y en Phu Tliuy, T elepresen ce virtu a l c o n tro l system fo r ro b o t using M E M S -b a sed (adsbygoogle = window.adsbygoogle || []).push({});
sen sor, Proceedings o f The 8th International Conference on M echatronics
T ech n o lo g y , H anoi, page 379 -3 8 4 , 2004.
2. Le Vu Ha, Pham Duy Hung, Ha Khanh Linh, Tran Q uang Vinh, Dang Anh V iet and N gu yen Thi Thanh Van, From M obile R o b o ts to Industrial A utom ation, Proceedings o f The 8th International Conference on M echatronics T echnology, Hanoi, page 4 5 9 - 4 6 3 , 2004.
3. Phạm D uy Hưng, Trần Thu H ồng, Đ ặng Cao Sơn, N gu yễn Thăng Long, Trần Q uang Vinh và N gu yễn Phú Thuỳ, ứ n g D ụng cảm biến g ia tố c M EM S đ ế đ o g ó c ngh iên g R oll và P itch cùa m áy bay, K ỳ yếu H ội nghị toàn quốc “ C ơ học và khí cụ bay có điều khiển” lần thứ nhất 200 5 , trang 162-167
4. Pham Duy Hung, Dang Cao Son, N gu yen Thang Long, Tran Q uang Vinh and N gu yen Phu Tliuy, An a p p lic a tio n o f P S O C in tilt-a n g le m easurem en t d evice using
M E M S -bu sed a c c ele ra tio n sensor, C D Proceedings o f The 9 lh International
C onfcrcnce on M echatronics Technology, Kuala Lumpur, M alaysia, 2005.
1. N g u y ễn Văn Chúc, Trần Tiến Đạt, Lê Anh Tuấn, N gu yễn Phú Thắng, Trần N g ọ c Thanh, Xu hướng p h á t triển và ứng dụng cùa càm biến quán tính vi c ơ (Con íỊuav, g ia
tố c k ể), Trung tâm Khoa học K ỹ thuật - Công nghệ Quân sự.
2. N g u y ễn Văn Chúc, N g u y ễn Phú Thắng, Lê Anh Tuấn, Đ ánh g iả ngư ờn g độ nhậy con q u a y vi cơ, Trung tâm K hoa học Kỹ thuật và C ông nghệ Quân sự.
3. M ai Tuấn Đạt, X e h a i bánh tự câ n b ằn g d i chuyển trên đ ịa hình b ằ n g p h ả n g, Luân văn Đ ại học hệ đào tạo kỹ sư chất lượng cao, tháng 7 /2005.
4. Phạm D uy Hưng, Trần Thu H ồng, Đặng Cao Sơn, N gu yễn Thăng Long, Trần Q uang V inh và N gu yễn Phủ Thuỳ, ứ n g D ụng cảm biến g ia tố c M E M S đ ế đ o g ó c n g h iên g R oll và P itch củ a m á y bay, Kỷ yếu Hội nghị toàn quốc “ C ơ học và khí cụ bay có điều khiển” lần thứ nhất 2005, trang 162-167
Tiếng Anh
5. h típ ://w w w . engin. um ich. edu /grou p/ctm /exam ples/pen d/in vpen . htm ỉ
6. F e lix Grasser, A ld o D 'A rrigo, S ilv io Colombi and A lfred C. Rufer, JO E : A m obile,
In v e rte d pendulum , IEEE Transactions on industrial electronics, V o l 4 9 , N o. 1,
February 2002.
7. Pham D uy Hung, Vu V iet H ung, N guyen Vinh Quang, Tran Q uang Vinh and N g u y en Phu Thuy, T elep resen ce virtu al control system f o r ro b o t using M E M S -b a sed
sen sor, Proceedings o f T he 8,h International C onference on M echatronics
T ech n o lo g y , H anoi, page 3 7 9 -3 8 4 , 2004.
8. Pham D uy Hung, D ang Cao Son, N guyen Thang Long, Tran Q uang V inh and N g u y en Phu Thuy, A n a p p lic a tio n o f P S O C in tilt-a n g le m easurem en t d e v ic e using
M E M S -b a sed a c c ele ra tio n sen sor, CD Proceedings o f The 9 lh International
C onference on M echatronics T echnology, Kuala Lumpur, M alaysia, 2005. TÀI LIỆU THAM KHẢO
9. Jacky Bakes, Sara McGrath and John Anderson, A ctive B alancing U sing G y r o s c o p e s f o r a S m a ll H u m an oid Robot, Department o f Com puter Scien ce. U n iversity o f Manitoba, W innipeg, M anitoba, Canada.
10. Albert-Jan Baerveidt and Robert K lang, A L o w -c o st a n d L o w -w eig h t A ttitude E stim a tio n System f o r a A u tonom ous H elic o p te r, Center for Com puter System s Architecture-H alm stad U niversity, B ox 823 S - 3 0 1 18 Halmstad, Sw eden.
11. Stephen B.Stancliff, Jennifer L. Laine, M ichael C. N echyba, L earn in g to F ly .D e s ig n a n d C on struction o f A u tonom ous A irp la n e, M achine Intelligence Laboratory, Electrical and Computer Engineering, U niversity o f Florida.
12. W. N ukulw uthiopas, S. Laowattana and T. M aneew am , D yn am ic M o d elin g o f a O n e-w h ee l R obot b y U sing K a n e ’s m eth o d, Institute o f Field R obotics(F IB O ), K ing M ongkut’s U niversity o f T echnology Thonburi, Bangkok, Thailand.
13. G reg W elch and Gary Bishop, An Introduction to the K alm an F ilter, D epartm ent o f Computer Scien ce-U niversity o f North Carolina at Chapel Hill.
14. V incent G abaglio, Quentin Ladetto and Bertrand M erm inod, K alm an F ilter A p p ro a c h f o r A u g m en ted G P S P ed estria n N aviagation, G eodetic Engineering Laboratory, Institute o f G eom atics, S w iss Federal Institute o f T echnology, Lausanne Swittzerland.
15. G reg W elch and Gary Bishop, C ou rse 8: An Introduction to th eK alm an filte r, (adsbygoogle = window.adsbygoogle || []).push({});
Departm ent o f Computer Scien ce-U niversity o f North Carolina at Chapel Hill. 16. H .W . Sorenson, L ea st-sq u a res estim ation : fro m G au ss to K a lm a n, U niversity o f California, San D ieg o , IEEE Spectrum V ol 7, pp 63-68.
17. Larry J. L evy, The K alm an F ilter: N a v ig a tio n ’s Intergation W orkhorse, A pplied P h ysics Laboratory- The Johns H opkins U niversity.
18. T on y Lacey, Tutorial: The K alm an F ilter, chapter 11.
19. Rich Chi O oi, B alancing a tw o -w h e e le d A u tonom ous Robot, The U niversity o f W estern Autralia