Điều khiển cân bằng hệ Pendubot

Nội dung text: Điều khiển cân bằng hệ Pendubot

1. ĐIỀU KHIỂN CÂN BẰNG HỆ PENDUBOT PENDUBOT CONTROL BALANCING Phạm Văn Lợi 1 Đại Học Sư Phạm Kỹ Thuật Tp.HCM Abstract – Balance control is interesting ngõ vào điều khiển nhỏ hơn số cơ cấu chấp hành. control problem for the Pendubot. In this paper, Trong đó, chiều dài vector cấu hình hệ thống nhỏ author applies Linear-Quadratic regulator hơn chiều dài của vector điều khiển. Những hệ thống method (LQR) to control balancing unstable máy móc như vậy gọi là hệ thống thiếu cơ cấu truyền inverted equilibrium Pedubot system. This động. Hệ thống này có thể thực hiện các nhiệm vụ project simulates LQR [1] control algorithm to phức tạp với số lượng cơ cấu truyền động ít và lại có balance system after the minimum time, những ưu điểm như: nhẹ, rẻ, ít tiêu hao năng lượng algorithms and appropriate parameters have và độ tin cậy rất cao. Pendubot là một ví dụ điển been selected during simulating mathematical hình cho hệ thống thiếu cơ cấu truyền động này, là model of the object which is embedded into DSP robot có hai bật tự do hoạt động trong mặt phẳng TMS320F28335 [2]. Actual model of Pendubot đứng. Có một cơ cấu truyền động nằm ở khớp nối after constructing the hardware and embedding thứ nhất, khớp thứ 2 nối với khớp thứ nhất bằng trục algorithm control will be kept in balance liên kết tự do. unstable inverted equilibrium position [3]. Keyword – Pendubot system, underactuated II. MÔ HÌNH HỆ THỐNG mechanical systems, underactuated robot. I. GIỚI THIỆU CHUNG Xét mô hình đơn giản hóa của hệ Pendubot như Trong những năm trở lại đây chưa bao giờ được thể hiện trong hình 1. việc đề xuất phát triển robot và thử nghiệm robot lại đa dạng như vậy. Cuộc sống đương đại đã gợi ý cho các nhà khoa học xu hướng chế tạo robot có tính ứng dụng vào thực tiễn rất cao. Trong lĩnh vực này phải kể đến dự án nghiên cứu các cánh tay robot được sử dụng trong những lĩnh vực điều khiển khác nhau như các lĩnh vực lắp đặt linh kiện IC vào trong các bo mạch, hàn khung xe, máy bay . . . Người ta bắt Hình 1: Mô hình hóa hệ Pendubot đầu quan tâm đến việc điều khiển máy móc với số Các thống số q1 và q2 mô tả góc giữa link 1, link 2 với mặt phẳng đứng. Mỗi link có khối lượng mi,
2. chiều dài li, và khoảng cách từ trục quay đến tâm lci dqcqq()[ (,) bq ()]+ dq ()[- cqqc (,)-12 (,)-() qqbq q 21 2 2 22 11 1 1 2 (3) và gia tốc trọng trường được ký hiệu là g. Áp dụng d11()()() q d 22 q d 12 q phương trình chuyển động Lagragian [1], ta mô tả dqcqq11()[ 2 (,) bq 2 ()]+ dq 21 ()[- cqqc 11 (,)-12 (,)- qqgq 1 () q2 2 được phương trình chuyển động của hệ Pendubot: d11()()() q d 22 q d 12 q . . D q q C() q , q q g q  (1) Hệ thống có thể được biểu diễn bằng biến trạng thái . ′ Ta có thể triển khai dưới dạng ′ . = [ 1 2 3 4] = [ 푞1 푞1 푞2 푞2 ] [4]. Do đó . . dqqdqq()()() cqcq bq  phương trình chuyể động của Pendubot có thể viết 111 12 2 11 1 12 2 1 (2) . lại dạng như sau: d( q ) q d ( q ) q c q b ( q ) 0 211 22 2 21 1 2 . fx x1 1 Trong đó: q(t), 푞̇(t), 푞̈ (t) € R2 biểu thị lần lượt . giá trị góc, vận tốc, gia tốc góc của hai thanh. x2 fx2 (4) . . x3 fx D(q) € R2x2, C(q, q ) € R2x2 và g(q) € R2x2 lần 3 . lượt là moment quán tính, ma trận centripedal- x4 fx4 coriolis, và vector gia tốc trọng trường τ€ R là . . moment đều khiển ngõ vào Trong đó: fx11 q , fx21 q , fx32 q , fx42 q dd cc Dq 11 12 ; Cq 11 12 ; III. BỘ ĐIỀU KHIỂN CÂN BẰNG LQR dd cc 21 22 21 22 b1 Để cân bằng hệ Pendubot ở điểm cân bằng ngược, g q b2 ta tiến hành tuyến tính hóa hệ thống quah điểm T Các thông số được thể hiện như sau: không cân bằng hướng lên x 000từ 22 2 dq11 ml 11c I 1 ml 222 c I ml 21 2 mllcosq 212 c 2 2 phương trình trạng thái (5). d22 q m 2 lc 2 I 2 . x Ax Bu (5) d q m q m l22 I m l 12 21 2c 2 2 2 1 Từ đó ta rút ra các ma trận hệ số của hệ Pendubot 2m l l cos q 2 1c 2 2 được tuyến tính hóa quanh điểm cân bằng không ổn định hướng lên c( q , q )11 m 2 l 1 lc 2 sin( q 2 ) q2 0 1 0 0 1 g 1  3 g 2  3 g 2 c( q , q )12 m 2 l 1 lc 2 sin( q 2 )( q12 q ) 2200 1  2  3  1  2  3 A 0 0 0 1 c( q , q )21 m 2 l 1 lc 2 sin( q 2 ) q1 21313212 (*)g  g  g  g g 213   2200 b1 q ml 1cc 1 m 2 l 1 gcos q 1 m 2 l 2 gcos q 1 q 2 1*t 2  3  1 *  2  3 0 b2 q m 2 lc 2 gcos q 1 q 2  B 2 (6) Thay vào hệ phương trình (2) ta thu được 0 phương quán tính của link 1 và link 2 như sau: ()23 Trong đó:
3. 22 số K được tính toán bằng Matlab sử dụng lênh lqr, 1 m 1 lc 1 I 1 m 2 l 1 (7) 2 ta được kết quả như sau 2 m 2 lc 2 I 2 K = [-9.8564 -2.2482 -8.7548 -1.5120] 3 m 2 l 1 lc 2 Sơ đồ thể hiện sự tính toán của bộ điều khiển 4 m 1 lc 1 m 2 l 1 LQR được thể hiện như Hình 2. 5 ml 2c 2 Giả sử mọi trạng thái trên đúng với mọi thời điểm, ta sử dụng lệnh lqr trong Matlab tối ưu hóa ma trận hồi tiếp trạng thái Klqr . Để tìm ra ma trận vector hệ số K của bộ điều khiển ta phải giải bài toán tối ưu, tối thiểu hóa chỉ tiêu chất lượng. J () xTT Qx u RU dt (8) lqr 0 Hình 2: Mô phỏng mô hình trạng thái hệ Ta giải phương trình đại số Ricati Pendubot bằng simulink ATT S SA SBR 1 B S Q 0 (7) Kết quả đáp ứng ngõ ra của góc link 1 và link 2 Ma trận trọng lượng cho hàm chi phí được thiết lập như sau w1 0 0 0 0 0 0 0 Q = ; R = 1; (9) 0 0w2 0 0 0 0 0 Trong đó w1 , w2 là hệ số trọng lượng, với phép lựa Hình 3: Đáp ứng ngõ ra góc link 1 chọn ma trận w1 1, w2 1. Sử dụng lệnh lqr trong matlab để giải phương trình đại số Riccati trên K,(,,,) S lqr A B Q R (10) Ta được kết quả như sau: K = [-100.5 -23.7 -101.6 -17.5] Từ đó ta suy ngõ ra tín hiệu điều khiển được xác định bằng công thứ u(t)=-Kx(t). Hình 4: Đáp ứng ngõ ra góc link 2 IV. KẾT QUẢ A. MÔ PHỎNG TRÊN MATLAB Sử dụng thời gian lấy mẫu liên tục T=0.01s, bộ điều khiển được thiết lập sử dụng Simulink và RealTime Workshop. Với bộ điều khiển cân bằng hệ
4. Hình 5: Đáp ứng ngõ ra bộ điều khiển Hình 6: Chương trình điều khiển hệ Pendubot thực tế trên matlab B. KẾT QUẢ THỰC NGHIỆM Sơ đồ giải thuật cho bộ điều khiển Trong đó các khối được thể hiện trong mô Bắt đầu hình có ý nghĩa như sau: + Khối eQEP1, có tác dụng đọc encoder gắn Khởi tạo hoạt động cho các thiết bị ngoại vi trên động cơ. Do encoder đọc được từ giá trị 0 đến giá trị tràn do ta đặt rồi quay về giá trị 0 (không có Đọc xung phát từ encoder 1 và giá trị <0 để xác định được góc âm) nên ta phải biến encoder 2 đổi quay về giá trị encoder đọc được giá trị góc tương ứng với thực tế. Do encoder loại 400 No -π/5 <goc_link1<π/5 -π/5<goc_link2<π/5 xung/vòng, sau khi đọc từ DSP, số xung đọc được Yes mỗi vòng là 1600 xung. Ở đây học viên lấy giá trị Luật điều khiển LQR thẳng đứng là , chiều dương của góc lệch của 2 link1 cùng chiều kim đồng hồ. Như vậy ta biến đổi Kết thúc sao cho vị trí thẳng đứng hướng lên là , quay theo Bộ điều khiển LQR sẽ được thiết lập khi các chiều kim đồng hồ thì giá trị dương, ngược kim đồng góc nằm trong tầm điều khiển sau: q1 55 hồ là giá trị âm. Việc biến đổi này được thực hiện và . Nếu các góc nằm ngoài giới hạn q2 trong khối xu ly goc link1. Sau khi tín hiệu từ 55 trên thì bộ điều khiển sẽ không giữ cân bằng được encoder đưa về được xử lý xong, chuyển qua bộ T “goc link1 thanh radian” chuyên đổi sang dạng và quay về vị trí ban đầu x 000 2 radian (vì trong định dạng của ta là radian). + Khối eQEP2 có tác dụng đọc encoder gắn trên link2.
5. + Các khối Embedded matlab “read2” có nhiệm vụ biến các giá trị góc lệch link1, link2, điện áp đặt vào động cơ thành các ký tự mã ASCII truyền lên cổng COM của máy tính, dùng cho việc thu thập dữ liệu + Khối Embedded matlab “motor”, chứa chương trình giải thuật thực hiện xuất tín hiệu điều khiển động cơ khi có tín hiệu tương ứng. Các tín hiệu của khối này là: - Ngõ vào (q1, q1_dot, q2, q2_dot) tương tướng với các giá trị biến trạng thái hệ thống. Hình 7: Mô hình Pendubot thực sau khi thi - Ngõ ra “u” giá trị độ lớn độ rộng xung để công phần cứng. điều khiển động cơ quay chậm hay nhanh. Kết quả thu được khi cho hệ thống chạy mô hình thật - Ngõ ra “sign u” giá trị chiều quay của động cơ. Nếu “sign u” = 0 thì động cơ quay ngược kim đồng hồ. Ngược lại nếu “sign u” = 1 thì động cơ quay cùng chiều kim đồng hồ - Ngõ ra “dien ap gui len” cung cấp giá trị điện áp đặt vào động cơ được chuyển đổi lên máy tính để thu thập giá trị của hệ thống. + Khối “SCI Transmit” có nhiệm vụ đưa các Hình 8: Đáp ứng thực tế góc link 1 giá trị góc lệch của link1 và link2, vận tốc góc link1, Trong khoảng thời gian bé hơn 5s thì link 1 link2 ( tính theo rad), điện áp cấp cho động cơ ( tính và link 2 nằm ngoài góc [ ] nên bộ bộ điều theo v). Tất cả các giá trị tham số được thu thập 5 5 thông qua cổng COM. khiển LQR chưa được thiết lập để giữ cân bằng, sau khoảng thời gian 5s khi ta đưa hai link vào vùng điều + Khối ePWM là khôi tạo xung. Việc điều xung này có tác dụng điều khiển động cơ thay đổi khiển [ ] thì bộ điều khiển LQR được thiết moment để cân bằng link1 và link2 ở tại vị trí cân lập để giữ link 1 nằm ở vị trí cân ngược [ 0 bằng trên. 2 ] như thể hiện ở hình 8 và hình 9.
6. [4] Z.Wang, Y.Guo. Unified control for Pendubot at four equilibrium points, IET Control Theory & Applications, Vol. 5, Issue 1, January 6 2011, pp 155 – 163. Thông tin liên hệ tác giả: Họ tên: Phạm Văn Lợi Điện thoại: 0937 907 468 Hình 9: Đáp ứng thực tế góc link 2 Email: vanloispkt@gmail.com Xác nhận của giảng viên hướng dẫn: TS. Trương Đình Nhơn Hình 10: Đáp ứng thực tế điện áp ngõ ra Tương tự, trong khoảng thời gian bé hơn 5s thì link 1 và link 2 nằm ngoài góc [ ] 5 5 thì tiến hiệu điều khiển u = 0 do bộ điều khiển LQR chưa được thiết lập. Khi bộ điều khiển được thiết lập thì tín hiệu điều khiển u xuất hiện liên tục trong khoảng [-20 20]V để giữ cân bằng hai link của hệ Pendubot. TÀI LIỆU THAM KHẢO [1] Nguyễn Thị Phương Hà, Huỳnh Thái Hoàng. Lý thuyết điều khiển tự động, Nhà xuất bản đại học Quốc gia thành phố Hồ Chí Minh, 2005. [2] Texa instruments, TMS320F28335 digital signail controllers. Data manual, 2007. [3] Guoyu Wang. B.Eng. Observer-Based Feedback control methods for an underactuated robot system, Tsinghua University, Beijing, P.R.China. 1995.
7. BÀI BÁO KHOA HỌC THỰC HIỆN CÔNG BỐ THEO QUY CHẾ ĐÀO TẠO THẠC SỸ Bài báo khoa học của học viên có xác nhận và đề xuất cho đăng của Giảng viên hướng dẫn Bản tiếng Việt ©, TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP. HỒ CHÍ MINH và TÁC GIẢ Bản quyền tác phẩm đã được bảo hộ bởi Luật xuất bản và Luật Sở hữu trí tuệ Việt Nam. Nghiêm cấm mọi hình thức xuất bản, sao chụp, phát tán nội dung khi chưa có sự đồng ý của tác giả và Trường Đại học Sư phạm Kỹ thuật TP. Hồ Chí Minh. ĐỂ CÓ BÀI BÁO KHOA HỌC TỐT, CẦN CHUNG TAY BẢO VỆ TÁC QUYỀN! Thực hiện theo MTCL & KHTHMTCL Năm học 2017-2018 của Thư viện Trường Đại học Sư phạm Kỹ thuật Tp. Hồ Chí Minh.