MƠ HÌNH HỐ VÀ ĐIỀU KHIỂN TỐI ƯU CHO HỆ XE HAI BÁNH TỰ CÂN BẰNG MODELLING AND OPTIMAL CONTROL FOR TWO-WHEELED SELF-BALANCING ROBOT Nguyễn Minh Tâm, Nguyễn Văn Đông Hải, Nguyễn Phong Lưu, Lê Văn Tuấn Trường đại học Sư phạm Kỹ thuật TP.HCM TÓM TẮT Hệ xe hai bánh tự cân hệ thống áp dụng nhiều thí nghiệm điều khiển tự động Đây hệ thống MIMO (multi-input and multi-output) mang tính lý thuyết thực tiễn cao, áp dụng sống Tuy nhiên, đa phần nghiên cứu xoay quanh việc điều khiển cân dùng giải thuật tìm kiếm thử sai phương trình toán học đơn giản Các nghiên cứu chi tiết phương trình tốn học hệ thống giải thuật ứng dụng hiểu biết mơ hình khơng nhiều Bài báo phân tích phương trình tốn học hệ xe hai bánh tự cân Qua đó, tác giả áp dụng thành công giải thuật điều khiển tối ưu cho hệ Giải thuật điều khiển tối ưu thủ nghiệm với trường hợp di chuyển khác xe Kết điều khiển tốt mơ Matlab/Simulink thực nghiệm Từ khóa: xe hai bánh tự cân bằng, điều khiển tối ưu, LQR, Matlab/Simulink, MIMO, thực nghiệm ABSTRACT Two-wheeled Self-balancing Cart is a popular model in automatic experiments This is a MIMO system (multi-input and multi-output) which is theoretical, practical and has been applied in normal life Anyway, most research just focus on balancing this model through try-on experiments or simple form of mathematical model There are still so few research focus on modelling algorithm and designing a mathematical model-based controller for this system This paper analized mathematical equations of the system Then, the authors successfully applied optimal control for this system Optimal controller was tested with different cases of cart motion Controlling results was proved to work well through both experiments and simulation on Matlab/Simulink Keywords: Two-wheeled Self-balancing Robot/Cart, LQR, optimal control, Matlab/Simulink, experiment ĐẶT VẤN ĐỀ Hệ xe hai bánh tự cân đối tượng quen thuộc toán lý thuyết điều khiển Thế nhưng, phần lớn nghiên cứu dựa phương trình toán học dạng đơn giản với ngõ vào hệ thống momen bánh trái momen bánh phải hồn thành mơ phỏng[1], [2], [3] mơ hình thực tế ngõ vào điện áp cấp cho động bánh trái bánh phải (phương trình toán học hệ thống phức tạp hơn) Nhiều nghiên cứu lại áp dụng giải thuật điều khiển thử sai (PID, FUZZY,…), khơng dựa mơ hình tốn học hệ thống nên tính ổn định hệ thống khơng đảm bảo nhờ toán học [4], [5], [6] Do đó, việc thiết lập mơ hình tốn học chi tiết với ngõ vào điều khiển điện áp cần thiết Và phương trình tốn học chi tiết thành lập, giải thuật điều khiển tối ưu (LQR) thiết kế để điều khiển hệ thống Việc ổn định hệ thống đảm bảo nhờ phường trình Riccati Nhờ đó, điều khiển tối ưu ổn định hệ thống mô đảm bảo việc áp dụng thực tế Các mục báo trình bày sau: Mục II đề cập cách thức mơ hình hóa đối tượng hệ xe hai bánh tự cân Tiếp đó, mục III, trình bày giải thuật điều khiển tối ưu áp dụng cho hệ xe Sau đó, mục IV V trình bày kết điều khiển mô thực nghiệm Cuối cùng, mục VI đưa kết luận điều khiển LQR cho hệ thống MƠ HÌNH TỐN HỌC Sơ đồ hệ quy chiếu hệ xe hai bánh tự cân thể hình 1, 2, dưới: Hình 1: Hình ảnh mơ tả hệ Hình 2: hệ xe hai bánh tự Hình 3: hệ xe hai bánh tự cân cân nhìn nghiêng nhìn từ xuống xe hai bánh tự cân Trong đó, thành phần hệ liệt lê bảng 1: Kí hiệu Đơn vị Ý nghĩa N m kg Khối lượng bánh xe G M kg Khối lượng robot  R W m m Bán kính bánh xe Chiều rộng robot rad D m m m rad rad m m Góc nghiêng phần thân robot Góc xoay robot H L l ,r   xl , yl , zl xr , y r , z r xm , ym , zm m Tọa độ trung bình F F , F Nm Moment phát động theo phương khác Chiều ngang robot Tỉ số giảm tốc Gia tốc trọng trường m s2 rad Góc trung bình bánh trái phải Góc bánh trái phải sss J m kgm2 Chiều cao robot Khoảng cách từ trọng tâm robot đến trục bánh xe Hệ số ma sát bánh xe mặt phẳng di chuyển Hệ số ma sát robot động DC Moment quán tính động DC Rm  Điện trở động DC Fl ,r Nm Moment phát động động bánh trái, phải Dòng điện động bánh trái, phải Điện áp động bánh trái, phải fw fm , Kb Vs rad Hệ số EMF động DC il , ir A Kt Nm A Moment xoắn động DC vl , vr V Bảng 1: Ký hiệu ý nghĩa đại lượng Tọa độ bánh trái Tọa độ bánh phải Ta sử dụng phương pháp Euler-Lagrange để xây dựng mơ hình động học Giả sử thời điểm t = 0, robot di chuyển theo chiều dương trục x, ta có góc tịnh tiến trung bình hai bánh xe góc xoay robot xác định sau:     r      l      R    l   r      W  (1) 1 J wl2  J w r2  J w 2 2 1  J    n J m  l   2  n J m  r  T2    x   xm    m  y    y   m   m   zm   R    (2)  xm   R cos   Và       ym   R sin   (3) Tọa độ bánh trái hệ qui chiếu : (4) Tọa độ bánh phải hệ qui chiếu : W    xm  sin    xr     y    y  W cos    r  m W   zr    z m     (7) Phương trình động chuyển động quay : Trong tọa độ trung bình Robot hệ qui chiếu: W    xm  sin    xl     y    y  W cos    l  m W    zl   zm     T1  m  xl2  yl2  zl2    m  xr2  yr2  zr2    m  xb2  yb2  zb2  (5) Tọa độ tâm đối xứng hai động hệ qui chiếu :  xb   xm  L sin cos    y    y L sin cos    b  m   zb   zm  L cos  Phương trình động chuyển động tịnh tiến: (6)   (8)  Với 2 (9) n J m  l  ; n J m  r  2 động quay phần ứng động trái phải Phương trình năng: (10) U  mgzl  mgzr  mgzb Phương trình Lagrange :     L  T1  T2  U (11) d  L  L  F   dt     (12) d  L  L  F   dt     (13) d  L  L  F   dt     (14) Lấy đạo hàm L theo biến ta được:  m  M  R       2 J w  2n J m    MLR cos  2n J m   (15)  MLR sin  F  ML2  J    MLRcos          2   2n J m   2n J m  2  MgLsin  ML  sin cos  F (16)  2m  M  R  J w  2n J m    1   mW  J       W   J  n J    w m  2R2    2   ML sin       2 ML  sin cos  F (17) 2    f w    2      Fl  Fr    F     W    Fl  Fr   R  (18) Và:   F  nK i  f      f  F  nK i  nK i  f       f     Fl  nKt il  f m   l  f wl r t r  m t l m r t r w r m l (19) (20) (21) r Sử dụng phương pháp PWM để điều khiển động nên chuyển từ dòng điện sang điện áp động cơ:   Lmil ,r  vl ,r  Kb   l ,r  Rmil ,r (22) Xem điện cảm phần ứng tương đối nhỏ (gần 0), bỏ qua, suy ra: il ,r   vl ,r  Kb   l ,r  Rm (23) Từ đó, moment lực sinh ra: F    vl  vr      f w   2 (24) F    vl  vr   2  2 (25) nKt nKt Kb    fm Rm Rm (26) W W2   vr  vl   (   f w ) 2R 2R (27) Với   F  (28)  MLR sin    vl  vr   Momen động lực động DC sinh ra:  F   F  F    MLRcos  2n J m   Thu phương trình động lực học mơ tả chuyển động robot sau:  ML2  J    MLR cos          2   2n J m   2n J m  2  MgL sin  ML  sin cos (29)    vl  vr   2  2 1   mW  J       W  2  2R2  J w  n J m        2   ML sin     2 ML2 sin cos  (30) W W2   vr  vl      fw  2R 2R2 ĐIỀU KHIỂN TỐI ƯU Từ (28), (29), (30), ta có hệ phương trình mơ tả hệ thống xe hai bánh cân có dạng sau: x1  x2  x  f x , x , x , x , x , x ,v ,v 1 l r   x3  x4   x4  f  x1 , x2 , x3 , x4 , x5 , x6 , vl , vr   x5  x6   x6  f  x1 , x2 , x3 , x4 , x5 , x6 , vl , vr  (31) Với : x   x1 x2 x3 x4 x5 x6  T         32) T Nếu chọn điểm làm việc là: x0  0 0 0 0 ; u0   0 T T (33) Ta tuyến tính hố hệ thống (31) dạng: x  Ax  Bu (34)    f  vl x  x0  u  u0   vl   Với: u    ;  f v B   r   vl ux ux0     f  vl x  x0  u u0     f  f   x1 x  x0 vr x  x0  u  u0 u  u0     0   ;  A   f f   vr x  x0   x1 ux ux0 u  u0   0     f  f   x1 x  x0  vr x  x0   u u0 u  u0  f x2 x  x0 u  u0 f x3 f x2 f x4 x  x0 u  u0 x  x0 u  u0 f x3 f x2 x  x0 u  u0 f x5 x  x0 u  u0 f x4 x  x0 u  u0 f x3 x  x0 u  u0 f x4 x  x0 u  u0 x  x0 u  u0 f x5 x  x0 u  u0 f x5 x  x0 u  u0   f  x6 x  x0  u  u0    (35)  f  x6 x  x0  u  u0    f  x5 x  x0   u  u0  x  x0 u  u0 Lúc này, ta có ma trận trọng số sau: Q1 0 Q  0 Q 0 0   0 0 0 Q3 0 Q4 0 0 0 0 Q5 0  0  0 0  Q6   R1  and R     R2  Với thông số Q1 , Q2 , … Q5 , Q6 , R1 R2 để tinh chỉnh cho điều khiển LQR Trong đó, thơng Q1 , Q2 , Q3 , Q4 , Q5 , Q6 trọng số tối ưu tương ứng cho giá trị  ,  ,  ,  ,  ,  , tín hiệu điều khiển cho góc tiến tới tín hiệu điều khiển cho góc xoay Nếu muốn biến trạng thái ưu tiên việc điều khiển ta cho thơng số trọng số tương ứng lớn tương đối so với thông số trọng số khác Sau chọn thông số điều khiển tương ứng, ta tiến hành xây dựng thông số hồi tiếp K với tín hiệu điều khiển u= - K*x Thơng số K tính dựa vào phương trình Riccati Tuy nhiên, Matlab hỗ trợ ta hàm lqr() để tính thông số K biết ma trận A, B, Q, R Hình 4: Sơ đồ chi tiết điều khiển LQR cho hệ xe hai bánh tự cân KẾT QUẢ MƠ PHỎNG Thơng số mơ hình chọn sau: m  ; M  ; R  0.0725 ; W  0.24 ; D  0.2 ; H  0.5 ; L  0.18 ; f w  0.18 ; J m  0.01 ; fm  0.002 ; J  2ML2 ; J  M W  D2  12 ; Rm  50 ; Kb  0.468 ; Kt  0.317 ; n  40 ; g  9.81 ; T  0.01 Ma trận trọng số LQR chọn sau: 1 0  0 Q 0 0  0 0 0 0 0 0 0 0 0 0 0  0  0 0  1 ; 1  R  0  Lúc này, ta ma trận K theo lệnh K=lqr(A,B,Q,R) 0.7 236.3 6066 1390.9 0.7 10.4 K   0.7 236.3 6066 1390.9 0.7 10.4  4.1 Giá trị đặt Hình 5: Góc bánh xe  (rad) Hình 6: Góc nghiêng  (rad) Hình 7: Góc xoay  (rad) 4.2 Giá trị đặt khác Chọn  đặt =  (rad),  đặt =  (rad) (xe tiến tới đoạn chiều quay vòng hai bánh xe quay góc 90 độ quanh trục xe) Hình 8: Góc bánh xe  (rad) Hình 9: Góc nghiêng  (rad) Hình 10: Góc xoay  (rad) 4.3 Nhận xét Như kết mơ hình 5-9, ta thấy trạng thái xe ln bám sát tín hiệu đặt, điều khiển LQR đáp ứng tốt Điều chứng tỏ xe tới, lui, quẹo phải, trái tùy giá trị đặt Thời gian độ dài cải thiện thơng qua việc chọn ma trận trọng số phù hợp KẾT QUẢ THỰC TẾ Mơ hình thực nghiệm thể hình 11 sau: Hình 11: Mơ hình phần cứng thực nghiệm Nguồn Pin Mạch cầu H Vi điều khiển DSP Cảm biến MPU 6050 Bánh xe mơ hình Hình 12: Sơ đồ kết nối phần cứng Trong khuôn khổ điều khiển LQR, ta chọn ma trận sau: 0 0  1000  100 ; 0   R  0 100 0  Q  0 0.1 0    0 0 1000    0 0 100    20     20  Như vậy, với thơng số hệ thống thực tế, ta tính tốn ra: Hình 18: Giá trị góc xoay  (độ) Thí nghiệm 2: Điều khiển quẹo phải  0.7 236.3 6066 1390.9 0.7 10.4 K    0.7 236.3 6066 1390.9 0.7 10.4  5.1 Điều khiển đứng yên vị trí cân Hình 13: Giá trị góc tới  (độ) Hình 14: Giá trị góc nghiệng  (độ) Hình 19: Giá trị góc tới  (độ) Hình 20: Giá trị góc nghiệng  (độ) Hình 21: Giá trị góc xoay  (độ) Thí nghiệm 3: Điều khiển lùi Hình 15: Giá trị góc xoay  (độ) 5.2 Điều khiển với vị trí đặt khác Thí nghiệm 1: Điều khiển quẹo trái Hình 16: Giá trị góc tới  (độ) Hình 17: Giá trị góc nghiệng  (độ) Hình 22: Giá trị góc tới  (độ) Hình 23: Giá trị góc nghiệng  (độ) Hình 24: Giá trị góc xoay  (độ) Thí nghiệm 4: Điều khiển tới Hình 25: Giá trị góc tới  (độ) Hình 26: Giá trị góc nghiệng  (độ) Hình 27: Giá trị góc xoay  (độ) 5.3 Nhận xét Qua hình 16-27, ta nhận thấy hệ thống xe hai bánh cân thực tế điều khiển theo trạng thái tới, lui, quẹo trái, quẹo phải để đạt giá trị đặt cho trước Tuy nhiên, biến trạng thái thực tế khơng bám hồn tồn giá trị đặt Đó tính chất thỉa hiệp điều khiển LQRsự sai lệch biến trạng thái bù đắp sai lệch biến trạng thái tương ứng khác đủ để đảm bảo tính hiệu điều khiển giá trị thiết kế Đây khuyết điểm điều khiển LQR so với giải thuật điều khiển phi tuyến khác Tuy nhiên, giải thuật điều khiển LQR đơn giản nhiều dễ thiết kế cho hệ MIMO phức tạp so với giải thuật điều khiển phi tuyến khác vốn phù hợp với đa số hệ thống SISO KẾT LUẬN Bài báo tính tốn mơ hình hóa phương trình tốn học hệ xe hai bánh tự cân với ngõ vào điện áp để áp dụng cho đối tượng mô thực tế Từ đó, dựa mơ hình vừa tính tốn hệ thống, nhóm tác giả thiết kế giải thuật điều khiển tối ưu cho hệ thống Kết điều khiển cho kết tốt mô thực tế TÀI LIỆU THAM KHẢO [1] A N K Nasir, M A Ahmad, R M T Raja Ismail, The Control of a Highly Nonlinear Two-wheeled Balancing Robot: A Comparative Assessment between LQR and PID-PID Control Schemes, International Journal of Mechanical, Aerospace, Industrial, Mechatronic and Manufacturing Engineering, Vol 4, No 10, 2010 [2] Nguyen Ngoc Son, Ho Pham Huy Anh, Adaptive Backstepping Self-balancing Control of a Two-wheel Electric Scooter, International Journal of Advanced Robotic Systems, Vol 11, No 1, 2014 [3] Ming Yue, Wei Sun, Ping Hu, Sliding Mode Robust Control for Two-wheeled Mobile Robot with Lower Center of Gravity, International Journal of Innovative Computing, Information and Control, Vol 7, No 2, 2011 [4] Samer Miasa, Mohammad Al-Mjali, Anas Al-Haj Ibrahim, and Tarek A Ttunji, Fuzzy Control of a Two-Wheel Balancing Robot using DSPIC, 7th International Multi-Conference on Systems, Signals and Devices, 2010 [5] Unluturk, A, Aydogdu, O, Guner, U., Design and PID control of two wheeled autonomous balance robot, International Conference on Electronics, Computer and Computation (ICECCO), (IEEE), pp 260-264, 2013 [6] Congying Qiu, Yibin Huang, The Design of Fuzzy Adaptive PID Controller of Two-Wheeled Self-Balancing Robot, International Journal of Information and Electronics Engineering, Vol 5, No 3, 2015 Thơng tin liên hệ tác giả (người chịu trách nhiệm viết): Họ tên: Nguyễn Phong Lưu Đơn vị: Bộ môn Điều khiển tự động, Khoa Điện-điện tử, ĐH SPKT TPHCM Điện thoại: 0918 119 083 Email: luunp@hcmute.edu.vn; aphongluu@gmail.com View publication stats ... hai bánh tự cân thể hình 1, 2, dưới: Hình 1: Hình ảnh mơ tả hệ Hình 2: hệ xe hai bánh tự Hình 3: hệ xe hai bánh tự cân cân nhìn nghiêng nhìn từ xuống xe hai bánh tự cân Trong đó, thành phần hệ. .. điều khiển tối ưu áp dụng cho hệ xe Sau đó, mục IV V trình bày kết điều khiển mô thực nghiệm Cuối cùng, mục VI đưa kết luận điều khiển LQR cho hệ thống MƠ HÌNH TOÁN HỌC Sơ đồ hệ quy chiếu hệ xe hai. . .tối ưu ổn định hệ thống mô đảm bảo việc áp dụng thực tế Các mục báo trình bày sau: Mục II đề cập cách thức mô hình hóa đối tượng hệ xe hai bánh tự cân Tiếp đó, mục III, trình bày giải thuật điều