Chương 1: Tổng quan Chương 2: Cơ sở lý thuyết Chương 3: Mô phỏng và khảo sát hệ trên phần mềm Matlab Simulink Chương 4: Xây dựng mô hình thực tế Chương 5: Kết luận và hướng phát triển 1.4 Giới hạn Đề tài chỉ tập trung điều khiển cân bằng hệ tại vị trí link 1 hướng lên và link 2 hướng xuông. Hệ thống chỉ đáp ứng khi góc lệch ban đầu của 2 thanh con lắc không quá 100. Chỉ điều khiển cân bằng được tại vị trí link 1 hướng lên link 2 hướng xuống, chưa điều khiển được cân bằng ở hai vị trí còn lại là 2 thanh cùng hướng lên và link 1 hướng xuống, link 2 hướng lên 2 CHƯƠNG 2.1 Mô hình hệ con lắc ngược quay kép Biểu đồ của hệ con lắc ngược quay kép được hiển thị như Hình 2.1 Hệ con lắc ngược quay kép Mô hình hệ con lắc ngược quay kép bao gồm 3 thanh rắn và 1 động cơ. Thanh thứ nhất được gọi là thanh cánh tay được gắn với trục động cơ theo phương nằm ngang, thanh thứ hai được gọi là thanh con lắc 1 được dựng theo phương thẳng đứng, thanh thứ 3 được gọi là thanh con lắc 2 nối tiếp với thanh con lắc 1.1 Mô tả ký hiệu được sử dụng Ký hiệu Mô tả Đơn vị 𝑚1 Khối lượng thanh cánh tay kg 𝑚2 Khối lượng thanh con lắc 1 kg 𝑚3 Khối lượng thanh con lắc 2 kg 𝜃1 Góc quay của thanh cánh tay rad 𝜃2 Góc quay của thanh con lắc 1 rad 3 𝜃3 Góc quay của thanh con lắc 2 rad 𝐿1 Chiều dài của thanh cánh tay m 𝐿2 Chiều dài của thanh con lắc 1 m 𝐿3 Chiều dài của thanh con lắc 2 m 𝑙1 Khoảng cách gốc tọa độ đến trọng tâm cánh tay m 𝑙2 Khoảng cách gốc tọa độ đến trọng tâm con lắc 1 m 𝑙3 Khoảng cách gốc tọa độ đến trọng tâm con lắc 2 m 𝐽1 Momen quán tính của thanh cánh tay Kgm2 𝐽2 Momen quán tính của thanh con lắc 1 Kgm2 𝐽3 Momen quán tính của thanh con lắc 2 Kgm2 g Gia tốc trọng trường m/s 2 𝐾𝑡 Hằng số momen điều khiển N.M/A 𝐾𝑣 Hằng số suất điện động V.s 𝑅𝑚 Điện trở động cơ Ohm 𝐶𝑚 Hệ số ma sát nhớt động cơ N.s e Điện áp điều khiển động cơ V 2.2 Phương trình phi tuyến hệ Con lắc ngược quay kép Trong chương này, chúng ta sẽ khảo sát hệ DRIP từ việc từng bước lập hệ phương trình vi phân của hệ DRIP trước tiên, rồi sau đó tuyến tính hóa tại điểm làm việc là cả 2 thanh cùng hướng lên và link 1 hướng lên, link 2 hướng xuống.
Dựa vào hệ phương trình khi tuyến tính hóa, ta sẽ xét đến yếu tố điều khiển được và thiết kế bộ điều khiển LQR cho mô phỏng và thực nghiệm. Mô hình của hệ DRIP được biểu diễn từ phương trình Euler-Lagrange như sau [7]: 𝑑 𝜕𝐿 𝜕𝐿 𝜕𝑊 (1) ( )− + = 𝑄𝑖 , 𝑖 = 1, …. 𝑚 𝑑𝑡 𝜕𝑞̇ 𝑖 𝜕𝑞𝑖 𝜕𝑞𝑖 Trong đó: 𝑞 (𝑡): vector vị trí góc 𝑞̇ (𝑡): vector vận tốc góc 𝑄𝑖 : vector lực tác dụng 4 𝐿: Lagrange 𝑊: Năng lượng tiêu hao Để có được phương trình Euler-Lagrange, tập hợp đạo hàm riêng của Lagrange, L được định nghĩa như sau: 𝐿(𝑞, 𝑞̇ ) = 𝑇𝑡𝑜𝑡𝑎𝑙 − 𝑉𝑡𝑜𝑡𝑎𝑙 (2) Trong đó: 𝑇𝑡𝑜𝑡𝑎𝑙 : Tổng động năng của hệ 𝑉𝑡𝑜𝑡𝑎𝑙 : Tổng thế năng của hệ Tổng động năng của hệ là tổng động năng của thanh cánh tay và 2 thanh con lắc, được tính như sau: 𝑇𝑡𝑜𝑡𝑎𝑙 = 𝑇𝑐á𝑛ℎ 𝑡𝑎𝑦 + 𝑇𝑐𝑜𝑛 𝑙ắ𝑐 1 + 𝑇𝑐𝑜𝑛 𝑙ắ𝑐 2 (3) Để sử dụng phương trình Euler-Lagrange cho việc mô tả hệ DRIP, ma trận 𝑞𝑖 được định nghĩa như sau: 𝜃1 𝑞𝑖 = [𝜃2 ] 𝜃3 (4) Ma trận 𝑞̇ 𝑖 cũng được định nghĩa như sau: 𝜃1̇ (5) 𝑞̇ 𝑖 = [𝜃̇2 ] 𝜃̇3 Và cuối cùng là ma trận 𝑄𝑖 : 𝜏 (6) 𝑄𝑖 = [0] 0 Các hệ tọa độ để mô tả hệ thống là 𝜃1 , 𝜃2 , 𝜃3. Do có 3 hệ tọa độ khác nhau nên phương trình Euler-Lagrange sẽ được áp dụng cho mỗi hệ tọa độ.
𝑥1 𝑙1 𝑐𝑜𝑠𝜃1 (7) 𝑦 [ 1 ] = [ 𝑙1 𝑠𝑖𝑛𝜃1 ] 𝑧1 0 5 Ta có: 1 1 (8) 𝑇𝑖 = 𝑚𝑖 𝑣𝑖2 + 𝐽𝑖 𝜃1̇ 2 2 2 𝑣12 = [𝑥̇12 𝑦̇12 𝑧̇12 ] = 𝑙12 𝜃1̇ 2 (9) 𝑑 (10) 𝑥̇12 = (𝑙 𝑐𝑜𝑠𝜃1 ) = −𝑙1 𝑠𝑖𝑛𝜃1 𝑑𝑡 1 𝑑 (11) 𝑦̇12 = (𝑙 𝑠𝑖𝑛𝜃1 ) = 𝑙1 𝑐𝑜𝑠𝜃1 𝑑𝑡 1 𝑑 (12) 𝑧̇12 = (0) = 0 𝑑𝑡 Nên tổng động năng của thanh cánh tay là: 1 1 1 (13) 𝑇𝑐á𝑛ℎ 𝑡𝑎𝑦 = 𝑚1 𝑙12 𝜃1̇ 2 + 𝐽1 𝜃̇12 = 𝐽1 𝜃1̇ 2 2 2 2 Tương tự cho thanh con lắc 1 và thanh con lắc 2, sau đó ta được tổng động năng của hệ như sau: 1 1 1 1 𝑇= 𝐽1 𝜃̇12 + 𝐽2 𝜃̇22 + 𝐽3 𝜃̇32 + 𝑚2 [(𝐿1 𝜃̇1 + 𝑙2 𝜃̇2 𝑐𝑜𝑠𝜃2 )2 + (𝑙2 𝜃̇2 𝑠𝑖𝑛𝜃2 )2 ] 2 2 2 2 (14) 1 + 𝑚3 [(𝐿1 𝜃1̇ + 𝐿2 𝜃̇2 𝑐𝑜𝑠𝜃2 + 𝑙3 𝜃̇3 𝑐𝑜𝑠𝜃3 )2 2 + (𝐿2 𝜃̇2 𝑠𝑖𝑛𝜃2 + 𝑙3 𝜃̇3 𝑠𝑖𝑛𝜃3 )2 ] Tổng thế năng của hệ là tổng thế năng của thanh cánh tay và 2 thanh con lắc, được tính như sau: 𝑉𝑡𝑜𝑡𝑎𝑙 = 𝑉𝑐á𝑛ℎ 𝑡𝑎𝑦 + 𝑉𝑐𝑜𝑛 𝑙ắ𝑐 1 + 𝑉𝑐𝑜𝑛 𝑙ắ𝑐 2 (15) Với: 𝑉𝑐á𝑛ℎ 𝑡𝑎𝑦 = 0 (16) 6 𝑉𝑐𝑜𝑛 𝑙ắ𝑐 1 = 𝑚2 𝑔𝑙2 𝑐𝑜𝑠𝜃2 (17) 𝑉𝑐𝑜𝑛 𝑙ắ𝑐 2 = 𝑚3 𝑔(𝐿2 𝑐𝑜𝑠𝜃2 + 𝑙3 𝑐𝑜𝑠𝜃3 ) (18) Từ đó ta được tổng thế năng của hệ là: 𝑉 = 𝑚2 𝑔𝑙2 𝑐𝑜𝑠𝜃2 + 𝑚3 𝑔(𝐿2 𝑐𝑜𝑠𝜃2 + 𝑙3 𝑐𝑜𝑠𝜃3 ) (19) Do bỏ qua ma sát, nên năng lượng tiêu hao xấp xỉ là 0 nên 𝑊=0 (20) Phương trình Lagrange có dạng: 𝐿 = 𝑇𝑡𝑜𝑡𝑎𝑙 − 𝑉𝑡𝑜𝑡𝑎𝑙 (21) Hệ phương trình động học của hệ thống là: 𝑑 𝜕𝐿 𝜕𝐿 ( )− =𝜏 𝑑𝑡 𝜕𝜃1̇ 𝜕𝜃1 𝑑 𝜕𝐿 𝜕𝐿 ( )− =0 (22) 𝑑𝑡 𝜕𝜃̇2 𝜕𝜃2 𝑑 𝜕𝐿 𝜕𝐿 ( )− =0 {𝑑𝑡 𝜕𝜃̇3 𝜕𝜃3 Quy đổi tín hiểu điều khiển từ momen lực về điện áp điều khiển động cơ. Phương trình diễn tả momen lực theo điện áp là [7]: 𝜏 = −𝐾3 𝜃1̈ − 𝐾2 𝜃1̇ + 𝐾1 𝑒 (23) Trong đó: 𝐾𝑡 (24) 𝐾1 = 𝑅𝑚 Và: 𝐾𝑡 𝐾𝑏 (25) 𝐾2 = 𝑅𝑚 Và: 𝐾3 = 𝐽𝑚 (26) Hệ phương trình động học dưới dạng vi phân của hệ thống là: 7 −𝐾3 𝜃̈1 − 𝐾2 𝜃̇1 + 𝐾1 𝑒 = ℎ1 𝜃1̈ + (ℎ2 + ℎ3 )𝑐𝑜𝑠𝜃2 𝜃̈2 + ℎ4 𝑐𝑜𝑠𝜃3 𝜃̈3 − (ℎ2 + ℎ3 )𝑠𝑖𝑛𝜃2 𝜃̇22 − (ℎ4 𝑠𝑖𝑛 { 0 = (ℎ2 + ℎ3 )𝑐𝑜𝑠𝜃2 𝜃1̈ + ℎ5 𝜃̈2 + ℎ6 cos(𝜃2 − 𝜃3 ) 𝜃̈3 + ℎ6 sin(𝜃2 − 𝜃3 ) 𝜃̇32 − (ℎ7 + ℎ8 )𝑠𝑖𝑛 (27) 0 = (ℎ4 𝑐𝑜𝑠𝜃3 )𝜃1̈ + ℎ6 cos(𝜃2 − 𝜃3 ) 𝜃̈2 + ℎ9 𝜃̈3 − ℎ6 sin(𝜃2 − 𝜃3 )𝜃̇22 − ℎ10 𝑠𝑖𝑛𝜃3 Trong đó: Bảng 2.2 Bảng tham số trung gian Ký hiệu Giá trị ℎ1 𝐽1 + 𝑚2 𝐿21 + 𝑚3 𝐿21 ℎ2 𝑚2 𝐿1 𝑙2 ℎ3 𝑚3 𝐿1 𝐿2 ℎ4 𝑚3 𝐿1 𝑙3 ℎ5 𝐽2 + 𝑚2 𝑙22 + 𝑚3 𝐿22 ℎ6 𝑚3 𝐿2 𝑙3 ℎ7 𝑚2 𝑔𝑙2 ℎ8 𝑚3 𝑔𝐿2 ℎ9 𝐽3 + 𝑚3 𝑙32 ℎ10 𝑚3 𝑔𝑙3 2.3 Khảo sát hệ tại điểm làm việc cả 2 thanh con lắc cùng hướng lên 2.1 Tuyến tính hóa hệ tại điểm làm việc Để tuyến tính hóa hệ, ta bắt đầu từ phương trình động lực học trước khi tuyến tính được trình bày dưới dạng vi phân. Khi thanh cánh tay ở vị trí gốc, hai thanh con lắc ở vị trí thẳng đứng hướng lên (𝜽𝟏 = 𝟎𝟎 , 𝜽𝟐 = 𝟎𝟎 , 𝜽𝟑 = 𝟎𝟎 ), ta có thể tuyến tính hóa bằng cách áp dụng xấp xỉ góc nhỏ: 𝒄𝒐𝒔𝜽 ≈ 𝟏, 𝒔𝒊𝒏𝜽 ≈ 𝟎. Từ đó, ta được hệ phương trình tuyến tính của hệ tại điểm làm việc: −𝐾3 𝜃̈1 − 𝐾2 𝜃̇1 + 𝐾1 𝑒 = ℎ1 𝜃1̈ + (ℎ2 + ℎ3 )𝜃̈2 + ℎ4 𝜃̈3 { 0 = (ℎ2 + ℎ3 )𝜃1̈ + ℎ5 𝜃̈2 + ℎ6 𝜃̈3 (28) 0 = ℎ4 𝜃1̈ + ℎ6 𝜃̈2 + ℎ9 𝜃̈3 2.2 Mô tả hệ thống dưới dạng phương trình trạng thái Để có thể mô tả hệ thống dưới dạng phương trình trạng thái, trước tiên ta cần đặt các biến trạng thái như sau: 8 𝑥1 = 𝜃1 , 𝑥2 = 𝜃1̇ , 𝑥3 = 𝜃2 , 𝑥4 = 𝜃̇2 , 𝑥5 = 𝜃3 , 𝑥6 = 𝜃̇3 (29) Và: 𝑥 = [𝑥1 𝑥2 𝑥3 𝑥4 𝑥5 𝑥6 ]𝑇 (30) Ta được: 𝜃1 𝜃1̇ 𝑥1 𝑓1 ̇ ̈ 𝑥2 𝜃1̇ 𝜃1 𝑓2 𝑑𝑥 𝑑 𝑥3 𝑑 𝜃2 𝜃2 ̇ 𝑓3 = 𝑓 (𝑥, 𝑢, 𝑡) = = = = (31) 𝑑𝑡 𝑑𝑡 𝑥4 𝑑𝑡 𝜃̇2 𝜃̇̈2 𝑓4 𝑥5 𝜃3 ̇3 𝑓5 𝜃 [𝑥6 ] [𝑓6 ] [𝜃̇3 ] [𝜃̇̈3 ] Phương trình trạng thái của hệ được viết như sau: 𝑥̇ = 𝐴𝑥 + 𝐵𝑢 (32) Với ma trận A, B được tính như sau: 𝜕𝑓1 𝜕𝑓1 𝜕𝑓1 𝜕𝑓1 𝜕𝑓1 𝜕𝑓1 𝜕𝑥1 𝜕𝑥2 𝜕𝑥3 𝜕𝑥4 𝜕𝑥5 𝜕𝑥6 𝜕𝑓2 𝜕𝑓2 𝜕𝑓2 𝜕𝑓2 𝜕𝑓2 𝜕𝑓2 𝜕𝑥1 𝜕𝑥2 𝜕𝑥3 𝜕𝑥4 𝜕𝑥5 𝜕𝑥6 𝜕𝑓3 𝜕𝑓3 𝜕𝑓3 𝜕𝑓3 𝜕𝑓3 𝜕𝑓3 (33) 𝜕𝑥1 𝜕𝑥2 𝜕𝑥3 𝜕𝑥4 𝜕𝑥5 𝜕𝑥6 𝐴= 𝜕𝑓4 𝜕𝑓4 𝜕𝑓4 𝜕𝑓4 𝜕𝑓4 𝜕𝑓4 𝜕𝑥1 𝜕𝑥2 𝜕𝑥3 𝜕𝑥4 𝜕𝑥5 𝜕𝑥6 𝜕𝑓5 𝜕𝑓5 𝜕𝑓5 𝜕𝑓5 𝜕𝑓5 𝜕𝑓5 𝜕𝑥1 𝜕𝑥2 𝜕𝑥3 𝜕𝑥4 𝜕𝑥5 𝜕𝑥6 𝜕𝑓6 𝜕𝑓6 𝜕𝑓6 𝜕𝑓6 𝜕𝑓6 𝜕𝑓6 [𝜕𝑥1 𝜕𝑥2 𝜕𝑥3 𝜕𝑥4 𝜕𝑥5 𝜕𝑥6 ] 𝜕𝑓 𝜕𝑓2 𝜕𝑓3 𝜕𝑓4 𝜕𝑓5 𝜕𝑓6 𝑇 (34) 𝐵=[ 1 ] 𝜕𝑢 𝜕𝑢 𝜕𝑢 𝜕𝑢 𝜕𝑢 𝜕𝑢 Ta có thông số mô hình như sau: Bảng 2.3 Bảng thông số mô hình Ký hiệu Giá trị Đơn vị 𝑚2 0.s Tại điểm cân bằng khi 2 thanh con lắc đều hướng lên, ta có ma trận A là: 0 1 0 0 0 0 0 −0.4989 0] Và ma trận B là: B = [0 0.3 Khảo sát tính điều khiển được Với giá trị A và B được tính ở trên, ma trận điều khiển được M được tính như sau: M = [B AB A2 B A3 B A4 B A5 B]T 0 0.4633 ] Ta tính được Rank(M) = 6 nên hệ điều khiển được tại điểm làm việc cả 2 thanh cùng hướng lên.4 Khảo sát hệ tại điểm làm việc link 1 hướng lên, link 2 hướng xuống 2.1 Tuyến tính hóa hệ tại điểm làm việc Tương tự như điểm làm việc trước đó, ta bắt đầu từ phương trình động lực học trước khi tuyến tính được trình bày dưới dạng vi phân.
Khi thanh cánh tay ở vị trí con lắc 1 ở vị trí hướng lên, con lắc 2 ở vị trí hướng xuống (𝜽𝟏 = 𝟎𝟎 , 𝜽𝟐 = 𝟎𝟎 , 𝜽𝟑 = 𝟏𝟖𝟎𝟎 ), ta có thể tuyến tính hóa bằng cách áp dụng xấp xỉ góc nhỏ: 𝒄𝒐𝒔𝜽𝟏 ≈ 𝟎, 𝒄𝒐𝒔𝜽𝟐 ≈ 𝟏, 𝒄𝒐𝒔𝜽𝟑 ≈ −𝟏, 𝒔𝒊𝒏𝜽 ≈ 𝟎 Từ đó ta được hệ phương trình tuyến tính của hệ: −𝐾3 𝜃̈1 − 𝐾2 𝜃̇1 + 𝐾1 𝑒 = ℎ1 𝜃1̈ + (ℎ2 + ℎ3 )𝜃̈2 − ℎ4 𝜃̈3 { 0 = (ℎ2 + ℎ3 )𝜃1̈ + ℎ5 𝜃̈2 − ℎ6 𝜃̈3 (38) 0 = −ℎ4 𝜃1̈ − ℎ6 𝜃̈2 + ℎ9 𝜃̈3 2.