- _ Xây dựng mô hình thực tế hệ Robot hai bánh tự cân bang.- _ Xây dựng chương trình nhúng cho vi điều khiến STM32F4.- Điều khiến cân bang hé o ot hai ánh và điều khiến bám quỹ đạo trên
TONG QUAN VE DE TAI
Từ lâu, con người đã mơ ước sở hữu một người máy làm trợ lý cá nhân, được tạo ra dé thực hiện mọi nhiệm vụ của mình Một trong những yêu cầu là Robot chế tạo ra phải có kích thước và hình dạng tương tự như một con người Nhiều Robot di động hiện nay sử dụng ba hoặc bốn bánh và được thiết lập bánh xe cơ sở đủ lớn để Robot có thể đứng thăng đứng được Tuy nhiên, dựa vào kích thước của bánh xe cơ
Sở sẽ giới hạn việc thực hiện các công việc của Robot, bánh xe cơ sở phải nhỏ hơn đáng kế so với chiều cao hình dạng giống như người Vì vậy, chi cần một thay đôi nhỏ ở vi trí trọng tâm là du để làm cho Robot trở nên không ôn định. Điều nay có thé được khắc phục bang cách hạ thấp trọng tâm của Robot dé giảm sự bất ôn định này Tuy nhiên điều này thường đi kèm với chi phí đáng kể Ngoài ra, do sự mat ôn định của Robot, nên tốc độ mà các Robot có thé di chuyển thường bị hạn chế vì quán tính của Robot sẽ làm cho nó bị lật up.
Robot hai bánh là một Robot trong đó sử dụng các khái niệm về sự 6n định động lực học đứng thăng và cân băng trên hai bánh Điều này liên quan đến việc sử dụng ly thuyết điều khiến để điều khiến di chuyển hai bánh và cân bang hơn là dựa vào trọng lực Hy vọng rang với sự phát triển của Robot hai bánh và công nghệ liên quan, các Robot có thé hoạt động như con người và hỗ trợ được con người nhiều hơn nữa.
Dự án Tbot tại IHMC (hstitute for
Quân đội Hoa K (DARPA) tai trợ dé thử nghiệm các loại robot lính khác nhau có thể thay thế binh lính của con người ở Iraq va Afghanistan.
Tbot có thé biến đổi linh hoạt từ việc sử dung 4 bánh dé di chuyển khi can di chuyén ở địa hình có độ cao hạn chế sang 2 bánh để di chuyển ở địa hình hẹp.
Hình 1.2: Tbot tr O tai IHMC Đối với các Robot ba và bốn bánh, việc thăng băng và ôn định của chúng là nhờ trọng tâm năm trong mặt phăng chân dé do các bánh Robot tạo ra Còn đối với Robot hai bánh thì việc thăng bằng được là khi trọng tâm của Robot năm ngay giữa các bánh Robot Do đặc điểm trọng tâm nên Robot ba hoặc bốn bánh sẽ không ồn định dé bị ngã nếu trọng lượng đặt nhiều vào bánh lái hoặc mat kha năng bám khi đặt nhiều vào bánh đuôi Nó có thé di chuyển tốt trên địa hình phăng nhưng không thé di chuyển lên xuống trên địa hình lỗi lõm (mặt phăng nghiêng): mat kha năng bám khi lên dốc và có thé lật úp khi xuống dốc.
Ngược lại, Robot hai bánh đồng trục lại thăng bang rat linh động khi di chuyển trên địa hình phức tap mặc dù bản thân là một hệ thống không 6n định Khi leo dốc, nó tự động nghiêng ra trước và giữ trọng tâm dồn về hai bánh lái chính Tương tự, khi bước xuống dốc, nó nghiêng ra sau và giữ trọng tâm rơi vào các bánh lái Trọng tâm của Robot không bao giờ rơi ra khỏi v ng đỡ của các bánh Robot để có thé gây ra sự lật úp. Đối với những địa hình lỗi lõm và những ứng dụng thực tế, sự thăng băng củaRobot hai bánh có thể sẽ mang lại nhiều ý nghĩa thực tiễn hơn. Địa hình phăng Địa hình dốc
1.2 Một số công trình nghiên cứu liên quan Trên thế giới đã có nhiều nghiên cứu ứng dụng mô hình Robot con lắc ngược để điều khiến cân bang cho Robot hai bánh Trong nghiên cứu [1], nhóm tác giả đã điều khiến Robot cân băng dùng DSP trên căn bản con lắc ngược với trọng lượng gin ở hệ thống giả lập như con người Hệ điều khiến trạng thái tuyến tính có biến trạng thái là thông tin cảm biến từ gyros và encoder motor (Grasser al.2002)
Giải thuật điều khiến sử dụng phương pháp điều khiển đặt cực trên các biến trạng thái mô tả hệ thông
Position, Speed [SI Units] —_ Đáp ứng của hệ thông
Hỡnh 1.5: Cụng trỡnh nghiờn cứu ở Th y ùT[]l]
Trong nghiên cứu [2], David P Anderson đã thiết kế Nbot dùng các sensor quán tính và encoder motor đê cân băng Robot Y tưởng điêu khiên dé cân băng: các bánh Robot sẽ phải chạy theo hướng mà phân trên Robot sắp ngã Các thông số ngõ vào dé xác định họat động và vị trí của Robot cân băng hai bánh là: e Cảm biến góc nghiêng. e Cảm biến vận tốc góc nghiêng. e Vi trí bánh Robot. e Vận tốc bánh Robot.
Còn Steven Hassenplug thì với mô hình Leway dùng Lego Mindstorms
Robotics kit được lập trình trong BrickOS, ngôn ngữ lập trình giống C/C++ dùng trong LEGO Mindstorm.
Giải thuật sử dung hai khối điều khién PD: một khối PD điều khiến vi trí bánh Robot, một khối PD điều khiến góc nghiêng Robot Tổng hai khối điều khiển PD này d ng để điều khiến hệ cân bang Robot Robot hai bánh.
Trong nghiên cứu [3] dé cập tới Balibot, là một trong các mẫu đầu tiên về Robot hai bánh có trọng tâm phía trên các bánh Robot Khi hệ thống điều khiến không hoạt động, Robot sẽ bị ngã Khi Robot có nhận biết hướng mà nó sắp ngã, các bánhRobot sẽ di chuyển về phía ngã và thăng góc với chính nó.
Hệ thông sử dụng cảm biến góc nghiêng, gia tốc kế Motorola MMA260 vi điều khiến là PIC 16F876 PIC tích hợp một bộ bién đổi A/D nhiều kênh dé đo cảm biến góc nghiêng Nguon điện được dat gần đỉnh và họat động như trọng lượng của con lac ngược.
Một phiên bản khác của Balibot sử dụng các cảm biến hồng ngọai để đo khoảng cách thay vì dùng các cảm biến đo góc.
Hình 1.7: BaliBot trong nghiên wu
Thuật toán điều khiến là LỌR trong simulink và việc nối giữa máy tính và mô hình bằng card MF 624 và dùng Real Time Toolbox for matlab với động lực driver tương ứng được dé cập trong nghiên cứu [4] Dữ liệu cam biến (encoder và cặp accelerometer được đọc băng encoder built-in và các kênh analog Động cơ DC được điều khiến bang bộ biến đôi điều rộng xung (PWM) với điện tử công suất.
Thuật toán điều khiến là LỌR.
Selector System output dx y = [phi, x]
Hinh 1.8: O i ui iu ur a ong SimMechanics, ki aiu i lộ eo a i trong nghiên cứu [4]
Hình 1.9: Mô hình i dung Matlab Real Time Toolbox trong nghiên cuu [4]
1.3 Muc tiéu dé tai Đề hệ Robot hoạt động 6n định ngoài phan cơ cấu hop lý và phương trình toán hệ thống sát với thực tế, Robot còn phải được thiết kế bộ điều khiến thích hợp Đối với hệ Robot hai bánh tự cân bang thì quá trình điều khiển có thé được chia thành ba phan: điều khiến góc nghiêng của hệ Robot so với phương thang đứng điều khiến vị trí di chuyển của Robot so với phương nằm ngang và cuối c ng là điều khiến góc xoay của Robot Ban thân hệ Robot là hệ không 6n định, nên quá trình điều khiến sẽ gặp rất nhiều khó khăn khi phải thực hiện điều khiến ba thành phân: góc nghiêng, vi tri và góc xoay cùng một thời điểm.
Kết hợp những ưu điểm của các phương pháp điều khiến tuyến tính và điều khiển phi tuyến khả năng tính toán mạnh mẽ của vi điều khiến ARM-STM32F4- DISCOVERY, luận văn này trình bày cách giải quyết bài toán trên cơ sở lý thuyết điều khiến tuyến tinh là: điều khiến LQR và điều khiến phi tuyến là: điều khiến trượt Sau đó nhúng các thuật toán này trên ARM-STM32F4-DISCOVERY dé điều khiến đối tượng thực, nhận x t kết quả điều khiến khi sử dụng các thuật toán này.
Các van đê cơ bản trong luận văn g6m có:
Nghiên cứu lý thuyết điều khiến LQR Nghiên cứu lý thuyết điều khiến trượt Áp dụng phương pháp điều khiến LQR cho đối tượng Robot hai bánh Áp dụng phương pháp điều khiến trượt cho đối tượng Robot hai bánh Áp dụng phương pháp điều khiển LQR và điều khiến trượt để cho đối tượng Robot hai bánh cân bằng và bám theo quỹ đạo đặt.
THIET KE VA MÔ HÌNH HÓA ROBOT HAI BÁNH TỰ CÂN BẰNG
2.1 Thiết kế Robot hai bánh tự cân bằng
2.1.1 Phần cơ khí Hệ thống cơ khí Robot hai bánh được thiết kế bang phần mém Solid Work Từ cơ sở kích thước của Robot hai bánh xây dựng trên Solid Work ta có thể có được thông sô gan đúng về tọa độ trọng tâm va moment quán tính của xe.
Hình 2.1: Thiết kế o i Robot hai bánh dùng phan m m SolidWorks
Hình 2.2: Mô hình Robot ai t cân bằng sau khi hoàn thành
Thiết kế cơ khí của hệ Robot hai bánh tự cân bang nham dap ứng mục tiêu nghiên cứu là giữ cân bằng cho robot ở trạng thái đứng yên hoặc di chuyền.
Tác giả lựa chọn vật liệu chính cho mô hình là mica có độ dày 4mm và §mm với ưu điểm dễ gia công.
2.1.2 Phần mạch điện Sơ đồ khối phan mach dién cua hé thong:
UART [ uiais }*| DC Servo motor }
Hình23 0 6kh iphanma — i ncủa Robot hai bánht cân bằng
Y Mạch điều khiến trung tâm sử dụng vi điều khiển STM32F4-DISCOVERY làm nhiệm vụ nhúng thuật toán điều khiến LQR và điều khiến trượt, thu thập tín hiệu từ cảm biến gia tốc Accelerometer, cảm biến vận tốc góc Gyroscope, Encoder.
Từ đó xuất xung PWM điều khiến động cơ DC Servo.
Y Động cơ sử dụng dé điều khiến bánh xe là động cơ DC Servo của hãng Yaskawa với encoder 400 xung không giảm tốc.
* Cảm biến đo góc nghiêng MPU6050:
ITG/MPU
Hình 2.6: C m biến MPU6050 Cam biến GY-521 6DOF IMU MPU6050 là san phẩm của hãng InvenseSense.
MPU6050 có kha năng do 6 thông số: vận tóc góc quanh ba trục Góc quay (Gyro), gia tốc trên trục gia tốc hướng (Accelerometer) MPU6050 giao tiếp với vi điều khiến thông qua giao tiếp I2C.
Trong dé tài này MPU6050 được d ng để đo góc nghiêng so với phương thang đứng của Robot hai bánh tự cân bằng.
Y Mạch điều khiến công suất động cơ HI216:
Mạch d ng để điều khiến động cơ hai bánh xe của hệ Robot.
Hình 2.7: Mạch công sud i u khi o I lố
- _ Dòng liên tục 15A dòng đỉnh 20A công suất 600W, tại 25oC.
- _ Điện áp công suất từ 12V đến 48V.
- (C6 cầu chì bảo vệ ngăn mạch.
- C6 Led báo nguồn (POW), tín hiệu xung đưa vào PWM _ và va tín hiệu dao chiều (DIR).
- _ oard được thiết kế nhỏ gon với kích thước 52x64x22mm.
- Tan số hoạt động lên tới 100 Khz, sử dụng Opto HCPL-0631 cho tan số hoạt động cao.
- - Độ rộng xung 100%. v¥ Module Bluetooth HC05
Module HC05 được d ng dé hỗ trợ truyền nhận dữ liệu giữa KIT STM32F4 và máy tính HC05 giao tiếp với vi điều khiến theo chuẩn UART.
Với điện áp trung bình của mỗi cell là 3.7V pin Lipo 3s có điện áp trung bình là 11.1 V Khi sạc đầy điện áp của pin có thé lên đến 12.6V Ngoài ra, khả năng xả dòng lớn là ưu điểm lớn của pin Lipo, giúp đáp ứng dòng điện cần thiết để động cơ hoạt động khi chịu tải lớn.
2.2 Mô hình hóa hệ Robot hai bánh tự cân bang
2.2.1 ựngm h nh toánh c hệ Robot hai bánh
Hệ Robot hai bánh tự cân bằng dựa trên nguyên lý của hệ xe con lắc ngược, mô hình toán học đơn giản của hệ Robot hai bánh tự cân bằng có thể thu được nhờ phương pháp Euler — Lagrange [5] À v
Dưới day là bang ky hiệu các thông số của hệ thống: m Khối lượng bánh Robot M Khôi lượng Robot
W Chiêu rộng Robot D Chiêu sâu Robot H Chiêu cao Robot
L Khoảng cách từ trọng tâm Robot đến trục bánh Robot
J, Moment quan tinh cua banh Robot Jy Moment quan tính cua Robot quanh trục động co Jy Moment quan tính cua Robot quanh trục góc phi ty Hệ số ma sát giữa bánh Robot va mặt phăng di chuyển Tin Hệ số ma sát tại trục quay động co DC
Jin Moment quán tinh của động co DC R,, Điện trở động co DC
K, Hệ số EMF của động co DC K, Moment xoắn của động co DC n Tỉ số giảm tốc g Gia tộc trọng trường
8 Góc trung bình của bánh trái và phải
0, , Góc của bánh trái phải
Góc nghiêng của phân than Robot Ộ Góc xoay của Robot
%*;.,;sZ¡ | Tọa độ bánh trái x„;Y„›Z„ | Tọa độ bánh phải
FasFy›Fœ | Moment phát động theo các phương khác nhau Fi, Moment phát động của động cơ bánh trái phải ty ol, Dòng điện động cơ bánh trái phải
W„,V Điện áp động cơ bánh trái phải r
B ng2.1: ¥hi u các thông s cuah th ng
Với hệ trục toa độ như hình vẽ, sử dụng phương pháp Euler-Lagrange, chúng ta có thể tìm được mô hình động lực học của Robot Giả sử tại thời điểm t = 0, Robot di chuyền theo chiêu trục x chúng ta có các phương trình sau: rà -
Phương trình động năng của chuyên động tịnh tiến:
Phương trình động năng của chuyền động quay:
La động năng quay của phân ứng động co trái và phải Phương trình thé năng:
L=T,+T,-U (2.11) a(S |- San (2.12) dt\ 00) ỉ8 bã OL _ (2.13) dt\ Ow] Ow kã ob Cup, (2.14) dt\ 0b} Oo
Lay đạo hàm L theo các bién @, ự va ở ta có các biểu thức sau:
| (2m+M)R? +2J,,+2n7J,, lồ+(MRLcosu —2n”J„) ~ MLR" sin = Fo
(MLR cosy —2n7J, ử+ (ML? +Jy+ 2n”J„}Ù — MgLsiny — ML?" sin cosy = F,
2mw + Jy +a (Jy tn J„)+ ML sin” y |o+ 2M wosin y cosy = F,
Moment luc do dong co DC sinh ra:
F, =nK,i, + ƒ„(Ù—9,)— f,9, (2.19) fy, = 1K, 1, = 1K, i, ~ fn (WW -9;)- fn (WW -9,)
Dé tiện dung phương pháp PWM cho việc điều khiến động cơ nên ta sẽ chuyén đôi từ dòng điện sang điện áp động cơ:
Có thé xem như là điện cảm phân cứng tương đôi nhỏ (gân bang 0), có thé bỏ qua.
Chúng ta suy ra được: i), = R (2.21)
Từ đó các moment lực sinh ra:
Suy ra phương trình động luc học mô tả chuyển động của Robot như sau:
(MLR cosw — 2n J,,)0-+(ML +J\yy+2n J.) ~ MgLsin y - ML" sin YW COSY
— mW +Jy+—5(J, +n J„)+ ML sin“ yw lò + 2ML wosin y cosy
> Phương trình 2.26 và 2.27 thể hiện mối quan hệ giữa giá trị điện áp điêu khiến hai động cơ đối với độ nghién, vi tri, vận tốc va sóc xoay cua hệ Robot: Gia tri điện áp hai động cơ vị và v, tác động lên các thông số còn lại trong phương trình dưới dạng tổng (vị + v,).
> Trong khi đó phương trình 2.28 thể hiện mối quan hệ giữa góc xoay với điện áp hai động cơ dưới dạng hiệu SỐ (V, - VỊ).
Vì vậy bài toán có thé được tách thành hai bài toán nhỏ hon với hai tín hiệu điều khiến là V„và Vy với:
Robot m | Khôi lượng bánh Robot 36* 104-3
Khối lượng Robot 1837.5*10^-3 L | Khoang cách từ trọng tam Robot đến trục bánh 29.05* 104-3
Moment quan tinh cua banh Robot 1.57198* 104-5 Jy Moment quan tính cua Robot quanh trục động co 4147432.77* 104-9 J Moment quán tính của Robot quanh trục góc phi 11742250.25* 104-9 ƒ„ | Hệ sô ma sát tại trục quay động cơ DC 7.63611675S11e-05 Ở | Moment quán tính của động co DC 0.004757719385720 R„ | Điện trở động co DC 4.576550118093287
K, | Hệ sô EME của động cơ DC 0.064359009905374 K, | Moment xoắn của động co DC 0.064359009905374
B ng 2.2: B ng thông s h Robot hai bánh
2.2.2 Đánh giám hnhtoanhé o othai anhd ung e Mô phỏng lan 1:
Trong trường hop không có nhiễu và các trạng thái ban đầu bang 0:
(Theta_O Theta_dot_0 Psi_0 Psi_dot_O Phi_O Phi doi 0)=(000000)
> XE HAI BANH TU CAN BANG œ 1 T T T T T T T T
Hình 2.12: Kết qu mô phỏng lan 1 mô hình toán xe hai bánh
Nhận x t đánh giá: Ở vị trí ban đầu =0 khi không có tác động của ngoại lực, đáp ứng hệ thông vẫn giữ nguyên vị trí cũ Như vậy (0 00000) là một vị trí cân bang của hệ robot xe hai bánh. e M6 phỏng lần 2:
Trong trường hợp có nhiễu với công suất 10°, các trạng thái ban đầu khác khéng:(Theta_O Theta_dot_0 Psi_0 Psĩ_dot_0 Phi_O Phi_dot_0) = (2 0.5 0.05 0 pi/4 pi/8) Và không có tác động từ bộ điều khiến LOR: w(t) (same as v(t)) so ỉ s ® p= ke
Time (sec) 10 T T T T T T T T T ne]£ st [——Psi] |
Hình 2.13: Kế gu mô phỏng lan 2 mô hình toán xe hai bánh
Nhận x t đánh giá: Khi không có tác động của bộ điều khiến, các góc theta và Phi lệch khỏi vị trí cân bằng và mat 6n định Riêng góc Psi tiễn đến vi trí cân bang mới (=3.14 —truc xe bị chúi xuống dưới).
Kết lu n: Đã mô hình hóa được đặc tinh động học của hệ Robot hai bánh.
THIẾT KE BỘ DIEU KHIEN CHO HE ROBOT HAI NH TU CAN BANG
sign( a of + kyr
Suy ra luật điêu khiến trượt là:
3.1.2.4 Chương trình Matla /Simulink mô phỏng bộ điêu khiển trượt Sơ đồ điêu khiến tổng quát:
Hình 3.7: ơ 6 ¡ ukhi n tổ t i ukhi ut Bộ điều khiến trượt: h i zwo e 8
Hình 38: ơ 6b iukhi wt e © ết quả mô phỏng:
Các trạng thái ban dau băng không và góc nghiêng Robot Psi_0=0.1 rad : (Theta_0
Theta_dot_0 Psi_O Psi_do(_0 Phi_O Phí dot_0)=(0 0 01 0 0 0) Và có sự tác động từ bộ điều khiến trượt:
XE HAI BANH TU CAN BANG 0.1
— 0.05 _Ằ : ơ———— na ơ— eee e eee e eee e ee eeeeeees _ Ww : : : :
Hình 3.9: Kếtqu mô phỏngh Robot ai MỚI S ngcuab 1 ukhi n ut
Nh é i Khi đặt góc nghiêng ban đầu của Robot ở 0.1 rad (khoảng 3.6) Robot nhanh chóng trở về vị trí cân bang sau | 5(s) e Mô phóng so sánh đáp ứng bang (góc lật Psi) cân giữa hai bộ điều khiến LOR và điều khiến trượt
Hình 3.10: Kết qu mô phỏng so ứng bằng (góc | t Psi cân giữa hai b i M khi G i ukhi ut
Kết quả mô phỏng điều khiển cân bằng hệ Robot hai bánh bang thuật toán điều khiến trượt cho kết quả tốt, tín hiệu ra bám sát theo tín hiệu đặt mong muốn, hệ thống đáp ứng nhanh hơn phương pháp LQR.
3.2 Bộ điều khiến ám qui đạo cho hệ Robot hai ánh tực n ang
3.2.1 Thiết kế qu' đạo dat choo ot [8][9][10]
Thiết kế quỹ đạo và thiết lập bộ điều khiến dé bám theo quỹ đạo này là nhiệm vụ rat quan trọng của robot di động Trong các nghiên cứu gan đây thường giải quyết các vấn dé sau: điều khiển robot bám vạch bám tường, bám quỹ đạo, tránh vật cản, đi tới điểm đích
Robot di động được nghiên cứu là mô hình robot 6 biến điều khiến trong đó điều khiến vi tri ảnh hưởng đến điều khiến góc lái Phi của Robot quy đối mối liên hệ theo công thức 2.l tức là ta điều khiến 2 biến w,V„ (vận tốc bánh trái và vận tốc bảnh phải), vị trí của robot được xác định thông qua ba biến trạng thái (x,y, 9.
Tông kết ta cân biên đôi quỹ đạo đặt vê dạng vận toc góc và van toc dài hai biên @
Couavatien Module | e | Controller [—” 2WMR veneration :Vioduie
Hình 3.11: o 6 ii ou uo’ i u aod
Từ lý thuyết trên dé áp dụng vào mô hình ta cần tìm mối quan hệ giữa các biến trạng thái 0 và @; x„ (1) va Vụ, (z) 5 Xy (1) va yy (1) Quy đạo đặt trước được tính dựa vào phương trình sau:
Trong đó A, va A, là biên độ tùy ý
Từ vi trí x,(t) va y„ (r)ta suy ra góc 0, va g, theo công thức sau
3.2.2 Chương tr nh Matla /Simulinkm_ phỏng bộ điều khiến ám qui: đạo
Chương trình mô phỏng bộ điều khiến LQR bám quỹ đạo được xây dựng bằng phan mém Matlab/Simulink theo sơ đồ dưới đây:
TWO-WHEEL BALANCINNG SYSTEM XY Graph2 G
Subtract xy — fe) Scope2Xd i 100 |
Target Trajectory Gain4 XY Graph1
Hình 3.12: o 6b uaoa oii u iukhinLQR Từ bộ điều khién LQR như trên ta thu được những kết quả như sau.
Kết qua mô phỏng chạy theo quỹ đạo thu được sau khi Robot đã giữ thăng băng
XE HAI BANH TU CAN BANG
Hình 3.13: Êê u O tứ i avy CO 1 aod x 1073 XE HAI BANH TU CAN BANG 20 LJ LJ Ỉ I Ỉ I Ỉ I
120 XE HAI BANH TU CAN BANG
Hinh3.15 é@ u O tứ ea i ay eo u
2 XE HAI BANH TU CAN BANG
Tiến hành so sánh quỹ đạo đặt và quỹ đạo đáp ứng của Robot
Hình 3.17: ê u O O u aoa u qO “u ua Ooo
Nhận x t đánh giá: Khi sử dung bộ điều khiến LOR để điều khiển cho xe bam quỹ đạo đặt khi xe đã cân băng xe đã chạy được theo quỹ đạo đặt ra tuy nhiên có bị lệch đi khi chạy liên tục.
3.2.3 Thiết kế 6 điều khiến trượt ám qu' đạo đặtvàm phỏng
3.2.3.1 Thiết kế bộ điều khiển trượt m quỹ đạo Đặt lại các biến trạng thái như sau:
Do thiết kế luật điều khiến cho hệ thông MIMO bậc cao như hệ thông Robot 2 bánh tự cân bang rat phức tạp nên ta chia đầu vào của hệ thống thành 2 phan:
VY Phần thứ 1 sử dụng bộ điều khiến trượt tong hop dé suy ra luat diéu khién cho góc / & 8
Y Phần thứ 2 là thành phân chênh lệch điện áp của bánh trái và bánh phải (offset) dé điều khiến góc ¢, thành phan này ta dùng bộ điều khiến PID để hệ thống dé dàng bám theo quỹ đạo, thông số điều khiến sẽ được tìm kiếm dựa trên giải thuật di truyén.[1 1]
Hệ thông phi tuyên được biểu diễn dưới dạng
Với #(X).gĂ(X)./s(X).sứ;(X) là những hàm phi tuyờn mụ tả hệ thụng. u=VL+VR (3.45)
Luật điều khiến u sẽ được tim dựa trên lý thuyết điều khiến trượt.
Câu trúc của mặt trượt được thé hiện như
18 : Cấu trúc của các mặ wu t ước dau tiên ta định nghĩa các mặt trượt như sau:
Với e, =x,-x,, (i=1,2,3,4) là sai số quỹ đạo, c, (¡=1.2.3.4) là những thông số điều khiến.
Tín hiệu điêu khiến u cân tìm dé điều khiến hệ Robot hai bánh tự cân băng bám quỹ đạo được chia làm hai phân: u=u,.+u Với 1= 1,2,3 eql swi
H là điện áp dé lái tat cả các sai sô quỹ đạo e, =x,—x,, (¡= l,2,3,4) về cại mặt trượt tương ứng.
là điện áp dé giữ các sai số quỹ đạo trên mặt trượt tương ứng.
—C)e _£(yx | | sy = C5 fi J ea (Xx) Cy8,(X)
| = -(¢, (x, — ig) +e (Sf, (X) 44) +0; (x, — ig) +e4(f (X)-%4,)) 3.53) an c;8¡(X)+ca8a (X) | Định nghĩa ham Lyapunov như sau: v(t) _ I (3.54)
Dao ham V(t) theo thoi gian:
= ¢,(%) — #4) + (4i(X)+ $1(X)(How3 + tags) ~ a) +@ (x4 — a) +e4(fe (X)+ 2, (X)(uyy3 + Ueg3) ~ X42)
Theo nguyên lý 6n định của Lyapunov, do V(t )là hàm xác định dương nên V(t) trong (3.55) phai la ham xac dinh am Do do tac gia dat:
Với k,7 là những hăng số dương V (t) trong (3.55) tuong duong:
= —ks3 —71|8|