BỘ GIÁO DỤC VÀ ĐÀO TẠO
TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT
THÀNH PHỐ HỒ CHÍ MINH
ĐỒ ÁN TỐT NGHIỆP
ĐỀ TÀI: THIẾT KẾ BỘ ĐIỀU KHIỂN TUYẾN TÍNH HÓA VÀO
RA CHO HỆ CON LẮC NGƯỢC QUAY
GVHD: TS. Nguyễn Văn Đông Hải
SVTH: Trần Minh Hiển – 19151220
Tp.Hồ Chí Minh, tháng 7/2023
TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP HỒ CHÍ MINH
KHOA ĐÀO TẠO CHÍNH QUY
HCMUTE
ĐỒ ÁN TỐT NGHIỆP
ĐỀ TÀI:
THIẾT KẾ BỘ ĐIỀU KHIỂN TUYẾN TÍNH HÓA VÀO RA
CHO HỆ CON LẮC NGƯỢC QUAY
GVHD
SVTH
MSSV
LỚP
NGÀNH
HỆ ĐÀO TẠO
KHÓA
: TS. NGUYỄN VĂN ĐÔNG HẢI
: TRẦN MINH HIỂN
: 19151220
: 1915153A
: ĐIỀU KHIỂN VÀ TỰ ĐỘNG HÓA
: CHÍNH QUY
: 2019-2023
TP. Hồ Chí Minh, tháng 7/2023
i
PHẦN I: GIỚI THIỆU
ii
TRƯỜNG ĐH SPKT TP. HỒ CHÍ MINH
KHOA ĐIỆN-ĐIỆN TỬ
BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN
CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT
NAM
ĐỘC LẬP - TỰ DO - HẠNH PHÚC
----o0o---Tp. HCM, ngày tháng năm
NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP
Họ và tên sinh viên 1: Trần Minh Hiển
MSSV: 19151220
Chuyên ngành:
Công nghệ Kỹ thuật Điều khiển và Tự động hóa
Hệ đào tạo:
Đại học chính quy
Khóa: 2019
Lớp: 191513A
I. TÊN ĐỀ TÀI: THIẾT KẾ BỘ ĐIỀU KHIỂN TUYẾN TÍNH HÓA VÀO RA CHO
HỆ CON LẮC NGƯỢC QUAY
II. NHIỆM VỤ
1. Các số liệu ban đầu:
......................................................................................................................................
......................................................................................................................................
......................................................................................................................................
......................................................................................................................................
2. Nội dung thực hiện:
......................................................................................................................................
......................................................................................................................................
......................................................................................................................................
......................................................................................................................................
III. NGÀY GIAO NHIỆM VỤ: 01/02/2023
IV. NGÀY HOÀN THÀNH NHIỆM VỤ: 30/06/2023
V. HỌ VÀ TÊN CÁN BỘ HƯỚNG DẪN: TS. Nguyễn Văn Đông Hải
CÁN BỘ HƯỚNG DẪN
BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN
iii
TRƯỜNG ĐH SPKT TP. HỒ CHÍ MINH
KHOA ĐIỆN-ĐIỆN TỬ
BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN
CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT
NAM
ĐỘC LẬP - TỰ DO - HẠNH PHÚC
----o0o---Tp. HCM, ngày tháng năm
LỊCH TRÌNH THỰC HIỆN ĐỒ ÁN TỐT NGHIỆP
Họ và tên sinh viên 1: Trần Minh Hiển
MSSV: 19151220
Tên đề tài: Thiết kế bộ điều khiển tuyến tính hóa và ra cho hệ con lắc ngược quay
Tuần/ngày
(Ký và ghi rõ họ và tên)
Nội dung
Xác nhận
GVHD
GV HƯỚNG DẪN
iv
TRƯỜNG ĐH SPKT TP. HỒ CHÍ MINH
KHOA ĐIỆN-ĐIỆN TỬ
BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN
CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT
NAM
ĐỘC LẬP - TỰ DO - HẠNH PHÚC
----o0o---Tp. HCM, ngày tháng năm
PHIẾU NHẬN XÉT CỦA GIÁO VIÊN HƯỚNG DẪN
Họ và tên Sinh viên:……………………………………MSSV:………………………..
Ngành:……………………………………………………………………………………
Tên đề tài:………………………………………………………………...........................
……………………………………………………………………………………………
NHẬN XÉT:
1. Về nội dung đề tài & khối lượng công việc thực hiện:
……………………………………………………………………………………………
……………………………………………………………………………………………
……………………………………………………………………………………………
2. Ưu điểm:
…………………………………………………………………………………………
……………………………………………………………………………………………
……………………………………………………………………………………………
3. Khuyết điểm:
……………………………………………………………………………………………
……………………………………………………………………………………………
……………………………………………………………………………………………
4. Đề nghị cho bảo vệ hay không?
……………………………………………………………………………………………
5. Đánh giá loại:
……………………………………………………………………………………………
6. Điểm:…………………………..(Bằng chữ:………………………………………….)
……………………………………………………………………………………………
Tp.Hồ Chí Minh, ngày……tháng……năm 20….
Giáo viên hướng dẫn
(Ký & ghi rõ họ tên)
Khoa ĐT Đại Trà – ĐH SPKT TP.HCM
v
TRƯỜNG ĐH SPKT TP. HỒ CHÍ MINH
KHOA ĐIỆN-ĐIỆN TỬ
BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN
CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT
NAM
ĐỘC LẬP - TỰ DO - HẠNH PHÚC
----o0o---Tp. HCM, ngày tháng năm
PHIẾU NHẬN XÉT CỦA GIÁO VIÊN PHẢN BIỆN
Họ và tên Sinh viên:……………………………………MSSV:………………………..
Ngành:……………………………………………………………………………………
Tên đề tài:………………………………………………………………...........................
……………………………………………………………………………………………
NHẬN XÉT:
1. Về nội dung đề tài & khối lượng công việc thực hiện:
……………………………………………………………………………………………
……………………………………………………………………………………………
……………………………………………………………………………………………
2. Ưu điểm:
…………………………………………………………………………………………
……………………………………………………………………………………………
……………………………………………………………………………………………
3. Khuyết điểm:
……………………………………………………………………………………………
……………………………………………………………………………………………
……………………………………………………………………………………………
4. Đề nghị cho bảo vệ hay không?
……………………………………………………………………………………………
5. Đánh giá loại:
……………………………………………………………………………………………
6. Điểm:…………………………..(Bằng chữ:………………………………………….)
……………………………………………………………………………………………
Tp.Hồ Chí Minh, ngày……tháng……năm 20….
Giáo viên hướng dẫn
(Ký & ghi rõ họ tên)
Khoa ĐT Đại Trà – ĐH SPKT TP.HCM
vi
TRƯỜNG ĐH SPKT TP. HỒ CHÍ MINH
KHOA ĐIỆN-ĐIỆN TỬ
BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN
CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT
NAM
ĐỘC LẬP - TỰ DO - HẠNH PHÚC
----o0o---Tp. HCM, ngày tháng năm
LỜI CAM ĐOAN
Tôi xin cam kết đề tài này là do tôi tự thực hiện dựa vào một số tài liệu trước đó
và không sao chép từ tài liệu hay công trình đã có trước đó.
Người thực hiện đề tài
vii
TRƯỜNG ĐH SPKT TP. HỒ CHÍ MINH
KHOA ĐIỆN-ĐIỆN TỬ
BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN
CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT
NAM
ĐỘC LẬP - TỰ DO - HẠNH PHÚC
----o0o---Tp. HCM, ngày tháng năm
LỜI MỞ ĐẦU
Hệ con lắc ngược quay (Rotary Inverted Pendulum) là một hệ thống kinh điển, phi
tuyến đã được điều khiển ổn định trên mô phỏng và thực tế. Từ hệ thống này các kỹ sư
đã xây dựng và chế tạo không những các sản phẩm ứng dụng trong sinh hoạt mà còn là
tiền đề để phát triển các công nghệ trong nhiều lĩnh vực như hàng không, vũ trụ, nghiên
cứu.
Một mô hình chuẩn dành cho sinh viên khảo sát giải thuật điều khiển tự động
(ĐKTĐ) đã được phát minh bởi các kỹ sư ở hãng Quanser. Ở nước ngoài đã có nhiều
nhóm tác giả thực hiện khảo sát thành công trên hệ thống này như như bộ điều khiển
PID , so sánh độ hiểu quả giữa hai giải thuật Neuron và LQR , bộ điều khiển H-infinify,…
Còn ở Việt Nam ta có một số bài báo thiết kế bộ điều khiển cho hệ tiêu biểu như bộ điều
khiển trượt SMC cân bằng, bộ điều khiển back-stepping, bộ điều khiển tuyến tính hóa
vào ra cho RIP.
Ở nghiên cứu giới thiệu bộ điều khiển tuyến tính hóa vào ra đầu tiên cho hệ con lắc
ngược nhưng có điều chưa tối ưu bởi bộ điều khiển này được xây dựng trên hệ phương
trình trạng thái phi tuyến trong khi người thực hiện đề tài nhận thấy rằng khi hệ RIP làm
việc ở vị trí cân bằng ổn định thì có thể giả sử cả hệ thống không còn là hệ phi tuyến nữa
mà là hệ tuyến tính vì thế việc phát triển bộ điều khiển tuyến tính hóa vào ra dựa trên hệ
phương trình không gian trạng thái tuyến tính là điều cần thiết bởi không những bộ điều
khiển sẽ được tối ưu mà vi điều khiển tính toán các thông số sẽ đơn giản hơn.
Mặc dù người thực hiện đề tài đã cố gắng hoàn thành nhiệm vụ đề tài đặt ra và
đúng thời hạn quy định nhưng chắc chắn sẽ không tránh khỏi những thiếu sót, mong
quý Thầy/Cô và các bạn sinh viên thông cảm. Người thực hiện đề tài rất mong nhận
được những ý kiến đóng góp của quý Thầy/Cô và các bạn sinh viên để đề tài được
hoàn thiện hơn nữa.
Xin chân thành cảm ơn!
Người thực hiện đề tài
Trần Minh Hiển
viii
TRƯỜNG ĐH SPKT TP. HỒ CHÍ MINH
KHOA ĐIỆN-ĐIỆN TỬ
BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN
CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT
NAM
ĐỘC LẬP - TỰ DO - HẠNH PHÚC
----o0o---Tp. HCM, ngày tháng năm
LỜI CẢM ƠN
Để hoàn thành luận văn này với kết quả tốt đẹp nhất, tôi đã nhận được rất nhiều sự
hỗ trợ cả về vật chất và tinh thần của quý Thầy/Cô và các bạn sinh viên ngành tự động
hóa. Với lòng biết ơn và tình cảm sâu sắc, cho phép tôi được gửi lời cảm ơn chân thành
đến tất cả các cá nhân đã tạo điều kiện giúp đỡ tôi trong quá trình nghiên cứu và thực
hiện đồ án tốt nghiệp.
Đặc biệt, tôi xin gửi lời cảm ơn sâu sắc nhất đến thầy TS. Nguyễn Văn Đông Hải,
thầy không những giúp đỡ tôi thiết kế mô hình đề tài mà còn tận tình hướng dẫn ngay
những ngày đầu tôi thực hiện đồ án. Thầy là một trong những sự trợ giúp to lớn nhất để
tôi có thể thực hiện thành công luận văn tốt nghiệp.
Tôi xin bày tỏ lòng biết ơn đến quý thầy cô Khoa Điện – Điện Tử, hệ Đại Trà trực
tiếp và gián tiếp giúp đỡ tôi trong suốt quá trình học tập và nghiên cứu đề tài.
Với điều kiện thời gian cũng như kinh nghiệm còn non nớt của một sinh viên, luận
văn này không thể tránh khỏi những thiếu sót. Tôi rất mong được nhận sử chỉ bảo, đóng
góp của các thầy cô để tôi có điều kiện bổ sung, nâng cao trình độ của bản thân, phục vụ
công tác nghiên cứu sau này tốt hơn.
Tôi xin chân thành cảm ơn
Tp.Hồ Chí Minh,ngày…….tháng……năm2022
Sinh viên thực hiện
Trần Minh Hiển
ix
MỤC LỤC
ĐỀ TÀI: .............................................................................................................................i
PHẦN I: GIỚI THIỆU .....................................................................................................ii
NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP .............................................................................. iii
LỊCH TRÌNH THỰC HIỆN ĐỒ ÁN TỐT NGHIỆP .....................................................iv
PHIẾU NHẬN XÉT CỦA GIÁO VIÊN HƯỚNG DẪN ................................................ v
PHIẾU NHẬN XÉT CỦA GIÁO VIÊN PHẢN BIỆN ..................................................vi
LỜI CAM ĐOAN ..........................................................................................................vii
LỜI MỞ ĐẦU .............................................................................................................. viii
LỜI CẢM ƠN .................................................................................................................ix
MỤC LỤC ........................................................................................................................ x
DANH MỤC HÌNH ẢNH .............................................................................................xii
DANH MỤC BẢNG BIỂU ..........................................................................................xix
DANH MỤC TỪ VIẾT TẮT ......................................................................................... xx
TÓM TẮT .....................................................................................................................xxi
ABSTRACT ..................................................................................................................xxi
PHẦN II: NỘI DỤNG ................................................................................................ xxii
CHƯƠNG 1: TỔNG QUAN ........................................................................................ 1
1.1 Đặt vấn đề ............................................................................................................ 1
1.2 Lý do chọn đề tài ................................................................................................. 5
1.3 Đối tượng nghiên cứu. ......................................................................................... 5
1.4 Giới hạn đề tài ..................................................................................................... 5
1.5 Mục tiêu nghiên cứu ............................................................................................ 5
1.6 Phương pháp nghiên cứu ..................................................................................... 6
1.7 Giới thiệu nội dung .............................................................................................. 6
CHƯƠNG 2: CƠ SỞ LÝ THUYẾT ............................................................................. 8
2.1 Giới thiệu về hệ con lắc ngược quay ................................................................... 8
2.2 Giới thiệu về đạo hàm Lie ................................................................................. 11
2.3 Giới thiệu về bậc tương đối của hệ thống ......................................................... 12
x
2.4 Giới thiệu về giải thuật tuyến tính hóa vào ra ................................................... 13
CHƯƠNG 3: MÔ TẢ TOÁN HỌC HỆ CON LẮC NGƯỢC QUAY ....................... 16
3.1 Cơ sở khoa học .................................................................................................. 16
3.2 Thành lập phương trình động học cho hệ con lắc ngược quay ......................... 16
3.3 Hệ phương trình trạng thái phi tuyến của mô hình ........................................... 24
CHƯƠNG 4: THIẾT KẾ BỘ ĐIỀU KHIỂN TUYẾN TÍNH HÓA VÀO RA .......... 26
4.1 Thiết kế bộ điều khiển tuyến tính hóa vào ra trên mô hình con lắc ngược ....... 26
4.2 Áp dụng giải thuật LQR rời rạc dựa trên phương pháp tuyến tính hóa vào ra . 31
CHƯƠNG 5: THI CÔNG HỆ THỐNG...................................................................... 34
5.1 Thiết kế hệ thống phần cứng ............................................................................. 34
5.2 Phần mềm .......................................................................................................... 42
5.3 Lưu đồ giải thuật ............................................................................................... 44
CHƯƠNG 6: KẾT QUẢ THỰC HIỆN ...................................................................... 45
6.1 Kết quả mô phỏng ............................................................................................. 45
6.2 Kết quả thực nghiệm ......................................................................................... 63
CHƯƠNG 7: KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN ........................................... 97
7.1 Kết luận ............................................................................................................. 97
7.2 Hướng phát triển ................................................................................................ 97
TÀI LIỆU THAM KHẢO .......................................................................................... 98
PHỤ LỤC ................................................................................................................. 100
xi
DANH MỤC HÌNH ẢNH
Hình 2.1 Mô hình hệ xe-con lắc ngược ......................................................................... 8
Hình 2.2 Mô hình hệ con lắc ngược quay Furuta, chế tạo bởi hãng Quanser ............... 10
Hình 2.3 Hệ con lắc ngược quay kép ............................................................................ 10
Hình 2.4 Hệ con lắc ngược quay song song .................................................................. 11
Hình 2.5 Hệ bánh xe con lắc ngược .............................................................................. 11
Hình 3.1 Mô hình toán học của hệ con lắc ngược quay ................................................ 17
Hình 3.2 Cấu trúc động cơ DC ...................................................................................... 22
Hình 4.1 Sơ đồ khối giải thuật LQR.............................................................................. 32
Hình 4.2 Sơ đồ bộ điều khiển tuyến tính hóa vào ra có kết hợp giải thuật LQR ........... 33
Hình 5.1 Mô hình con lắc ngược thực tế ....................................................................... 34
Hình 5.2 Vi mạch điều khiển Arduino Mega 2560 ....................................................... 36
Hình 5.3 Sơ lược các chân tín hiệu của Arduino Mega2560 ......................................... 37
Hình 5.4 Động cơ DC kèm Encoder Nisca NF5475E ................................................... 38
Hình 5.5 Cầu H L298N ................................................................................................. 39
Hình 5.6 Encoder thực tế ............................................................................................... 41
Hình 5.7 Mô tả tín hiệu 2 kênh của Encoder ................................................................. 41
Hình 5.8 Giao diện phần mềm Arduino IDE................................................................. 43
Hình 5.9 Giao diện phần mềm Terminal ....................................................................... 43
Hình 5.10 Lưu đồ giải thuật điều khiển LQR con lắc ngược quay ............................... 44
Hình 6.1 Khối mô phỏng bộ điều khiển hệ thống con lắc ngược .................................. 45
Hình 6.2 Khối mô phỏng mô tả hệ thống con lắc ngược .............................................. 46
Hình 6.3 Khối mô phỏng bộ điều khiển ........................................................................ 46
Hình 6.4 Kết quả mô phỏng đáp ứng góc cánh tay và con lắc khi h(x) = x1, khi hiệu
chỉnh ma trận Q và R lần thứ nhất ................................................................................. 47
Hình 6.5 Kết quả mô phỏng đáp ứng vận tốc góc con lắc và góc cánh tay khi h(x) = x1,
khi hiệu chỉnh ma trận Q và R lần thứ nhất ................................................................... 47
Hình 6.6 Kết quả mô phỏng đáp ứng góc cánh tay và góc con lắc khi h(x) = x1 sau khi
hiệu chỉnh ma trận Q và R lần thứ hai ........................................................................... 48
Hình 6.7 Kết quả mô phỏng đáp ứng vận tốc góc con lắc và góc cánh tay khi h(x) = x1,
sau khi hiệu chỉnh ma trận Q và R lần thứ hai ............................................................... 48
Hình 6.8 Kết quả mô phỏng đáp ứng góc cánh tay và góc con lắc khi h(x) = x1 sau khi
hiệu chỉnh ma trận Q và R lần thứ ba............................................................................. 49
Hình 6.9 Kết quả mô phỏng đáp ứng vận tốc góc con lắc và góc cánh tay khi h(x) = x1,
sau khi hiệu chỉnh ma trận Q và R lần thứ ba ................................................................ 49
xii
Hình 6.10 Kết quả mô phỏng đáp ứng góc cánh tay và góc con lắc khi h(x) = x1 sau
khi hiệu chỉnh ma trận Q và R lần thứ tư ....................................................................... 50
Hình 6.11 Kết quả mô phỏng đáp ứng vận tốc góc con lắc và góc cánh tay khi h(x) =
x1, sau khi hiệu chỉnh ma trận Q và R lần thứ tư........................................................... 50
Hình 6.12 Kết quả mô phỏng đáp ứng góc cánh tay và góc con lắc khi h(x) = 3x1 -3x3
khi hiệu chỉnh ma trận Q và R lần thứ nhất ................................................................... 51
Hình 6.13 Kết quả mô phỏng đáp ứng vận tốc góc cánh tay và vận tốc góc con lắc khi
h(x) = 3x1 -3x3 khi hiệu chỉnh ma trận Q và R lần thứ nhất ........................................... 51
Hình 6.14 Kết quả mô phỏng đáp ứng góc cánh tay và góc con lắc khi h(x) = 3x1 -3x3
khi hiệu chỉnh ma trận Q và R lần thứ hai ..................................................................... 52
Hình 6.15 Kết quả mô phỏng đáp ứng vận tốc góc cánh tay và vận tốc góc con lắc khi
h(x) = 3x1 -3x3 khi hiệu chỉnh ma trận Q và R lần thứ ................................................... 52
Hình 6.16 Kết quả mô phỏng đáp ứng góc cánh tay và góc con lắc khi h(x) = 3x1 -3x3
khi hiệu chỉnh ma trận Q và R lần thứ ba ...................................................................... 53
Hình 6.17 Kết quả mô phỏng đáp ứng vận tốc góc cánh tay và vận tốc góc con lắc khi
h(x) = 3x1 -3x3 khi hiệu chỉnh ma trận Q và R lần thứ ba .............................................. 53
Hình 6.18 Kết quả mô phỏng đáp ứng góc cánh tay và góc con lắc khi h(x) = 3x1 -3x3
khi hiệu chỉnh ma trận Q và R lần thứ tư ....................................................................... 54
Hình 6.19 Kết quả mô phỏng đáp ứng vận tốc góc cánh tay và vận tốc góc con lắc khi
h(x) = 3x1 -3x3 khi hiệu chỉnh ma trận Q và R lần thứ tư .............................................. 54
Hình 6.20 Kết quả mô phỏng đáp ứng góc cánh tay và góc con lắc khi h(x) = 3x1 - 5x3
khi hiệu chỉnh ma trận Q và R lần thứ nhất .................................................................. 55
Hình 6.21 Kết quả mô phỏng đáp ứng vận tốc góc cánh tay và vận tốc góc con lắc khi
h(x) = 3x1 - 5x3 hiệu chỉnh ma trận Q và R lần thứ nhất ................................................ 55
Hình 6.22 Kết quả mô phỏng đáp ứng góc cánh tay và góc con lắc khi h(x) = 3x1 - 5x3
hiệu chỉnh ma trận Q và R lần thứ hai ........................................................................... 56
Hình 6.23 Kết quả mô phỏng đáp ứng vận tốc góc cánh tay và vận tốc góc con lắc khi
h(x) = 3x1 - 5x3 hiệu chỉnh ma trận Q và R lần thứ hai ................................................. 56
Hình 6.24 Kết quả mô phỏng đáp ứng góc cánh tay và góc con lắc khi h(x) = 3x1 - 5x3
khi hiệu chỉnh ma trận Q và R lần thứ ba ..................................................................... 57
Hình 6.25 Kết quả mô phỏng đáp ứng vận tốc góc cánh tay và vận tốc góc con lắc khi
h(x) = 3x1 - 5x3 khi hiệu chỉnh ma trận Q và R lần thứ ba ............................................ 57
Hình 6.26 Kết quả mô phỏng đáp ứng góc cánh tay và góc con lắc khi h(x) = 3x1 - 5x3
khi hiệu chỉnh ma trận Q và R lần thứ tư ...................................................................... 58
Hình 6.27 Kết quả mô phỏng đáp ứng vận tốc góc cánh tay và vận tốc góc con lắc 3x1 5x3 khi hiệu chỉnh ma trận Q và R lần thứ tư .............................................................. 58
xiii
Hình 6.28 Kết quả mô phỏng đáp ứng góc cánh tay và góc con lắc khi h(x) = 3x1 - 7x3
hiệu chỉnh ma trận Q,R lần thứ nhất .............................................................................. 59
Hình 6.29 Kết quả mô phỏng đáp ứng vận tốc góc cánh tay và vận tốc góc con lắc khi
h(x) = 3x1 - 7x3 hiệu chỉnh ma trận Q và R lần thứ nhất .............................................. 59
Hình 6.30 Kết quả mô phỏng đáp ứng góc cánh tay và góc con lắc khi h(x) = 3x1 - 7x3
hiệu chỉnh ma trận Q và R lần thứ hai ........................................................................... 60
Hình 6.31 Kết quả mô phỏng đáp ứng vận tốc góc cánh tay và góc con lắc khi h(x) =
3x1 - 7x3 hiệu chỉnh ma trận Q và R lần thứ hai ........................................................... 60
Hình 6.32 Kết quả mô phỏng đáp ứng góc cánh tay và góc con lắc khi h(x) = 3x1 - 7x3
hiệu chỉnh ma trận Q và R lần thứ ba............................................................................. 61
Hình 6.33 Kết quả mô phỏng đáp ứng vận tốc góc cánh tay và góc con lắc khi h(x) =
3x1 - 7x3 hiệu chỉnh ma trận Q và R lần thứ ba ........................................................... 61
Hình 6.34 Kết quả mô phỏng đáp ứng góc cánh tay và góc con lắc khi h(x) = 3x1 - 7x3
hiệu chỉnh ma trận Q và R lần thứ tư ............................................................................. 62
Hình 6.35 Kết quả mô phỏng đáp ứng vận tốc góc cánh tay và góc con lắc khi h(x) =
3x1 - 7x3 hiệu chỉnh ma trận Q và R lần thứ tư ............................................................. 62
Hình 6.36 Kết quả thực nghiệm đáp ứng góc con lắc khảo sát hàm h(x) = x1 khi hiệu
chỉnh ma trận Q,R lần thứ nhất ...................................................................................... 64
Hình 6.37 Kết quả thực nghiệm đáp ứng góc cánh tay khảo sát hàm h(x) = x1 khi hiệu
chỉnh ma trận Q,R lần thứ nhất ...................................................................................... 64
Hình 6.38 Kết quả thực nghiệm đáp ứng vận tốc góc con lắc khảo sát hàm h(x) = x1
khi hiệu chỉnh ma trận Q,R lần thứ nhất ........................................................................ 65
Hình 6.39 Kết quả thực nghiệm đáp ứng vận tốc góc cánh tay khảo sát hàm h(x) = x1
khi hiệu chỉnh ma trận Q,R lần thứ nhất ........................................................................ 65
Hình 6.40 Kết quả thực nghiệm đáp ứng góc con lắc khảo sát hàm h(x) = x1 khi hiệu
chỉnh ma trận Q,R lần thứ hai ........................................................................................ 66
Hình 6.41 Kết quả thực nghiệm đáp ứng góc cánh tay khảo sát hàm h(x) = x1 khi hiệu
chỉnh ma trận Q,R lần thứ hai ........................................................................................ 66
Hình 6.42 Kết quả thực nghiệm đáp ứng vận tốc góc con lắc khảo sát hàm h(x) = x1
khi hiệu chỉnh ma trận Q,R lần thứ hai .......................................................................... 67
Hình 6.43 Kết quả thực nghiệm đáp ứng vận tốc góc cánh tay khảo sát hàm h(x) = x1
khi hiệu chỉnh ma trận Q,R lần thứ hai .......................................................................... 67
Hình 6.44 Kết quả thực nghiệm đáp ứng góc con lắc khảo sát hàm h(x) = x1 khi hiệu
chỉnh ma trận Q,R lần thứ ba ......................................................................................... 68
Hình 6.45 Kết quả thực nghiệm đáp ứng góc cánh tay khảo sát hàm h(x) = x1 khi hiệu
chỉnh ma trận Q,R lần thứ ba ......................................................................................... 68
xiv
Hình 6.46 Kết quả thực nghiệm đáp ứng vận tốc góc con lắc khảo sát hàm h(x) = x1
khi hiệu chỉnh ma trận Q,R lần thứ ba ........................................................................... 69
Hình 6.47 Kết quả thực nghiệm đáp ứng vận tốc góc cánh tay khảo sát hàm h(x) = x1
khi hiệu chỉnh ma trận Q,R lần thứ ba ........................................................................... 69
Hình 6.48 Kết quả thực nghiệm đáp ứng góc con lắc khảo sát hàm h(x) = x1 khi hiệu
chỉnh ma trận Q,R lần thứ tư .......................................................................................... 70
Hình 6.49 Kết quả thực nghiệm đáp ứng góc cánh tay khảo sát hàm h(x) = x1 khi hiệu
chỉnh ma trận Q,R lần thứ tư .......................................................................................... 70
Hình 6.50 Kết quả thực nghiệm đáp ứng vận tốc góc con lắc khảo sát hàm h(x) = x1
khi hiệu chỉnh ma trận Q,R lần thứ tư ............................................................................ 71
Hình 6.51 Kết quả thực nghiệm đáp ứng vận tốc góc cánh tay khảo sát hàm h(x) = x1
khi hiệu chỉnh ma trận Q,R lần thứ tư ............................................................................ 71
Hình 6.52 Kết quả thực nghiệm đáp ứng góc con lắc khảo sát hàm h(x) = 3x1 - 3x3 khi
hiệu chỉnh ma trận Q,R lần thứ nhất .............................................................................. 72
Hình 6.53 Kết quả thực nghiệm đáp ứng góc cánh tay khảo sát hàm h(x) = 3x1 - 3x3
khi hiệu chỉnh ma trận Q,R lần thứ nhất ........................................................................ 72
Hình 6.54 Kết quả thực nghiệm đáp ứng vận tốc góc con lắc khảo sát hàm h(x) = 3x1 3x3 khi hiệu chỉnh ma trận Q,R lần thứ nhất ................................................................. 73
Hình 6.55 Kết quả thực nghiệm đáp ứng vận tốc góc cánh tay khảo sát hàm h(x) = 3x1
- 3x3 khi hiệu chỉnh ma trận Q,R lần thứ nhất ............................................................... 73
Hình 6.56 Kết quả thực nghiệm đáp ứng góc con lắc khảo sát hàm h(x) = 3x1 - 3x3 khi
hiệu chỉnh ma trận Q,R lần thứ hai ................................................................................ 74
Hình 6.57 Kết quả thực nghiệm đáp ứng góc cánh tay khảo sát hàm h(x) = 3x1 - 3x3
khi hiệu chỉnh ma trận Q,R lần thứ hai .......................................................................... 74
Hình 6.58 Kết quả thực nghiệm đáp ứng vận tốc góc con lắc khảo sát hàm h(x) = 3x1 3x3 khi hiệu chỉnh ma trận Q,R lần thứ hai ................................................................... 75
Hình 6.59 Kết quả thực nghiệm đáp ứng vận tốc góc cánh tay khảo sát hàm h(x) = 3x1
- 3x3 khi hiệu chỉnh ma trận Q,R lần thứ hai ................................................................. 75
Hình 6.60 Kết quả thực nghiệm đáp ứng góc con lắc khảo sát hàm h(x) = 3x1 - 3x3 khi
hiệu chỉnh ma trận Q,R lần thứ ba ................................................................................. 76
Hình 6.61 Kết quả thực nghiệm đáp ứng góc cánh tay khảo sát hàm h(x) = 3x1 - 3x3
khi hiệu chỉnh ma trận Q,R lần thứ ba ........................................................................... 76
Hình 6.62 Kết quả thực nghiệm đáp ứng vận tốc góc con lắc khảo sát hàm h(x) = 3x1 3x3 khi hiệu chỉnh ma trận Q,R lần thứ ba .................................................................... 77
Hình 6.63 Kết quả thực nghiệm đáp ứng vận tốc góc cánh tay khảo sát hàm h(x) = 3x1
- 3x3 khi hiệu chỉnh ma trận Q,R lần thứ ba .................................................................. 77
xv
Hình 6.64 Kết quả thực nghiệm đáp ứng góc con lắc khảo sát hàm h(x) = 3x1 - 3x3 khi
hiệu chỉnh ma trận Q,R lần thứ tư .................................................................................. 78
Hình 6.65 Kết quả thực nghiệm đáp ứng góc cánh tay khảo sát hàm h(x) = 3x1 - 3x3
khi hiệu chỉnh ma trận Q,R lần thứ tư ............................................................................ 78
Hình 6.66 Kết quả thực nghiệm đáp ứng vận tốc góc con lắc khảo sát hàm h(x) = 3x1 3x3 khi hiệu chỉnh ma trận Q,R lần thứ tư .................................................................... 79
Hình 6.67 Kết quả thực nghiệm đáp ứng vận tốc góc cánh tay khảo sát hàm h(x) = 3x1
- 3x3 khi hiệu chỉnh ma trận Q,R lần thứ tư .................................................................. 79
Hình 6.68 Kết quả thực nghiệm đáp ứng góc con lắc khảo sát hàm h(x) = 3x1 - 5x3 khi
hiệu chỉnh ma trận Q,R lần thứ nhất .............................................................................. 80
Hình 6.69 Kết quả thực nghiệm đáp ứng góc cánh tay khảo sát hàm h(x) = 3x1 - 5x3
khi hiệu chỉnh ma trận Q,R lần thứ nhất ........................................................................ 80
Hình 6.70 Kết quả thực nghiệm đáp ứng vận tốc góc con lắc khảo sát hàm h(x) = 3x1 5x3 khi hiệu chỉnh ma trận Q,R lần thứ nhất ................................................................. 81
Hình 6.71 Kết quả thực nghiệm đáp ứng vận tốc góc cánh tay khảo sát hàm h(x) = 3x1
- 5x3 khi hiệu chỉnh ma trận Q,R lần thứ nhất ............................................................... 81
Hình 6.72 Kết quả thực nghiệm đáp ứng góc con lắc khảo sát hàm h(x) = 3x1 - 5x3 khi
hiệu chỉnh ma trận Q,R lần thứ hai ................................................................................ 82
Hình 6.73 Kết quả thực nghiệm đáp ứng góc cánh tay khảo sát hàm h(x) = 3x1 - 5x3
khi hiệu chỉnh ma trận Q,R lần thứ hai .......................................................................... 82
Hình 6.74 Kết quả thực nghiệm đáp ứng vận tốc góc con lắc khảo sát hàm h(x) = 3x1 5x3 khi hiệu chỉnh ma trận Q,R lần thứ hai ................................................................... 83
Hình 6.75 Kết quả thực nghiệm đáp ứng vận tốc góc cánh tay khảo sát hàm h(x) = 3x1
- 5x3 khi hiệu chỉnh ma trận Q,R lần thứ hai ................................................................. 83
Hình 6.76 Kết quả thực nghiệm đáp ứng góc con lắc khảo sát hàm h(x) = 3x1 - 5x3 khi
hiệu chỉnh ma trận Q,R lần thứ ba ................................................................................. 84
Hình 6.77 Kết quả thực nghiệm đáp ứng góc cánh tay khảo sát hàm h(x) = 3x1 - 5x3
khi hiệu chỉnh ma trận Q,R lần thứ ba ........................................................................... 84
Hình 6.78 Kết quả thực nghiệm đáp ứng vận tốc góc con lắc khảo sát hàm h(x) = 3x1 5x3 khi hiệu chỉnh ma trận Q,R lần thứ ba .................................................................... 85
Hình 6.79 Kết quả thực nghiệm đáp ứng vận tốc góc cánh tay khảo sát hàm h(x) = 3x1
- 5x3 khi hiệu chỉnh ma trận Q,R lần thứ ba .................................................................. 85
Hình 6.80 Kết quả thực nghiệm đáp ứng góc con lắc khảo sát hàm h(x) = 3x1 - 5x3 khi
hiệu chỉnh ma trận Q,R lần thứ tư .................................................................................. 86
Hình 6.81 Kết quả thực nghiệm đáp ứng góc cánh tay khảo sát hàm h(x) = 3x1 - 5x3
khi hiệu chỉnh ma trận Q,R lần thứ tư ............................................................................ 86
xvi
Hình 6.82 Kết quả thực nghiệm đáp ứng vận tốc góc con lắc khảo sát hàm h(x) = 3x1 5x3 khi hiệu chỉ nh ma trận Q,R lần thứ tư .................................................................... 87
Hình 6.83 Kết quả thực nghiệm đáp ứng vận tốc góc cánh tay khảo sát hàm h(x) = 3x1
- 5x3 khi hiệu chỉnh ma trận Q,R lần thứ tư .................................................................. 87
Hình 6.84 Kết quả thực nghiệm đáp ứng góc cánh tay khảo sát hàm h(x) = 3x1 - 7x3
khi hiệu chỉnh ma trận Q,R lần thứ nhất ........................................................................ 88
Hình 6.85 Kết quả thực nghiệm đáp ứng góc cánh tay khảo sát hàm h(x) = 3x1 - 7x3
khi hiệu chỉnh ma trận Q,R lần thứ nhất ........................................................................ 88
Hình 6.86 Kết quả thực nghiệm đáp ứng vận tốc góc con lắc khảo sát hàm h(x) = 3x1 7x3 khi hiệu chỉnh ma trận Q,R lần thứ nhất ................................................................. 89
Hình 6.87 Kết quả thực nghiệm đáp ứng vận tốc góc cánh tay khảo sát hàm h(x) = 3x1
- 7x3 khi hiệu chỉnh ma trận Q,R lần thứ nhất ............................................................... 89
Hình 6.88 Kết quả thực nghiệm đáp ứng góc con lắc khảo sát hàm h(x) = 3x1 - 7x3 khi
hiệu chỉnh ma trận Q,R lần thứ hai ................................................................................ 90
Hình 6.89 Kết quả thực nghiệm đáp ứng góc cánh tay khảo sát hàm h(x) = 3x1 - 7x3
khi hiệu chỉnh ma trận Q,R lần thứ hai .......................................................................... 90
Hình 6.90 Kết quả thực nghiệm đáp ứng vận tốc góc con lắc khảo sát hàm h(x) = 3x1 7x3 khi hiệu chỉnh ma trận Q,R lần thứ hai ................................................................... 91
Hình 6.91 Kết quả thực nghiệm đáp ứng vận tốc góc cánh tay khảo sát hàm h(x) = 3x1
- 7x3 khi hiệu chỉnh ma trận Q,R lần thứ hai ................................................................. 91
Hình 6.92 Kết quả thực nghiệm đáp ứng góc con lắc khảo sát hàm h(x) = 3x1 - 7x3 khi
hiệu chỉnh ma trận Q,R lần thứ ba ................................................................................. 92
Hình 6.93 Kết quả thực nghiệm đáp ứng góc cánh tay khảo sát hàm h(x) = 3x1 - 7x3
khi hiệu chỉnh ma trận Q,R lần thứ ba ........................................................................... 92
Hình 6.94 Kết quả thực nghiệm đáp ứng vận tốc góc con lắc khảo sát hàm h(x) = 3x1 7x3 khi hiệu chỉnh ma trận Q,R lần thứ ba .................................................................... 93
Hình 6.95 Kết quả thực nghiệm đáp ứng vận tốc góc con cánh tay khảo sát hàm h(x) =
3x1 – 7x3 khi hiệu chỉnh ma trận Q,R lần thứ ba .......................................................... 93
Hình 6.96 Kết quả thực nghiệm đáp ứng góc con lắc khảo sát hàm h(x) = 3x1 - 7x3 khi
hiệu chỉnh ma trận Q,R lần thứ tư .................................................................................. 94
Hình 6.97 Kết quả thực nghiệm đáp ứng góc cánh tay khảo sát hàm h(x) = 3x1 - 7x3
khi hiệu chỉnh ma trận Q,R lần thứ tư ............................................................................ 94
Hình 6.98 Kết quả thực nghiệm đáp ứng vận tốc góc con lắc khảo sát hàm h(x) = 3x1 7x3 khi hiệu chỉnh ma trận Q,R lần thứ tư ..................................................................... 95
Hình 6.99 Kết quả thực nghiệm đáp ứng vận tốc góc cánh tay khảo sát hàm h(x) = 3x1
- 7x3 khi hiệu chỉnh ma trận Q,R lần thứ tư .................................................................. 95
xvii
xviii
DANH MỤC BẢNG BIỂU
Bảng 3.1 Ký hiệu mô hình con lắc ngược quay ............................................................ 21
Bảng 3.2 Ký hiệu và thông số động cơ DC Nisca ......................................................... 22
Bảng 4.5.1 Chân của encoder trong động cơ ................................................................. 38
Bảng 4.5.2 Các thông số cơ bản của động cơ do NSX quy định..Error! Bookmark not
defined.
Bảng 4.5.3 Thông số kỹ thuật của cầu H L298N .......... Error! Bookmark not defined.
Bảng 4.5.4 Thông số kỹ thuật của Encoder ................... Error! Bookmark not defined.
xix
DANH MỤC TỪ VIẾT TẮT
Từ viết tắt
Từ nguyên nghĩa
PID
Proportional-Integral-Derivative
LQR
Linear Quadratic Regulator
SIMO
Single Input Multiple Output
DC
Direct Circuit
LQR
Linear Quadratic Regulator
ANN
Artificial Neural Network
IOL
Input-Output Linearization
SMC
Sliding Mode Control
PD
Proportional-Derivative
RIP
Rotary Inverted Pendulum
FSF
Full State Feedback
FLC
Fuzzy Logic Controller
xx
TÓM TẮT
Luận văn tập trung về trình bày phương pháp xây dựng phương trình động lực học,
thiết kế bộ điều khiển cân bằng cho con lắc ngược quay trên phần mềm mô phỏng
Matlab/Simulink. Con lắc ngược quay đơn là hệ thống phi tuyến khó ổn định, có một
ngõ vào và hai ngõ ra (Single Input – Multi Output). Trong bài luận văn này, tác giả trình
bày phương pháp xây dựng phương trình động lực học và thuật toán tuyến tính hóa vào
ra để thiết kế bộ điều khiển sau đó khảo sát sự ổn định khi áp dụng bộ điều khiển lên con
lắc ngược ở vị trí cân bằng bất ổn định (mô phỏng Matlab/Simulink). Việc đánh giá kết
quả mô phỏng sẽ cho thấy mức độ hiệu quả của bộ điều khiển trên hệ thống.
Từ khoá: Con lắc ngược quay; tuyến tính hóa vào ra; bộ điều khiển hệ SIMO;
điều khiển cân bằng; điều khiển ổn định
ABSTRACT
The thesis presents method of developing dynamic equation, balancing controller for the
rotary inverted pendulum (RIP) system in the simulation environment
(MATLAB/Simulink). RIP is a unstable non-linear model, it’s also a SIMO (Single
Input-Multi Output) and underactuated system, in foreign and domestic. In this thesis,
the author will be presenting method of developing system dynamic equation and
controller using input-output linearization algorihtm based on linear state space model,
then examine the performance of the system applied the balancing controller when the
pendulum at the uprigth position (simulation Matlab/Simulink). The evaluation of the
simulation results will show the effectiveness of the controller on this system.
Keywords: Rotary inverted pendulum; SIMO controller; balancing control;
Input-Output Linearization
xxi
PHẦN II: NỘI DỤNG
xxii
CHƯƠNG 1: TỔNG QUAN
1.1 Đặt vấn đề
Trong bối cảnh đất nước đang tiến vào thời kỳ cách mạng công nghiệp tự động hóa
4.0 thì việc nghiên cứu và phát triển công nghệ, máy móc luôn giành được sự quan tâm
ưu đãi lớn từ nhà nước. Đi cùng với cách mạng 4.0 đó là sự phát triển của các ngành kỹ
thuật điện tử, công nghệ thông tin và đặc biệt nhất chính là ngành kỹ thuật điều khiển và
tự động hóa. Tự động hóa là quá trình nghiên cứu, thiết kế giải thuật và áp dụng bộ điều
khiển lên các hệ mô hình đa bậc, phi tuyến tính như thiết bị vận chuyển máy bay không
người lái, hệ thống điều khiển hành trình cho oto, cánh tay robot, hệ thống tự động cân
bằng BKM8 (Automatic balance machine), hệ thống ổn định trước khi đáp cánh của tên
lửa vũ trụ. Ngày nay, chúng ta không những đòi hỏi máy móc vận hành một cách tự động
(không có sự can thiệp của con người) mà còn phải tối ưu hóa quá trình làm việc, nâng
cao hiệu suất, giảm thiểu sai sót, và để làm được vậy cần đến các kỹ sư tự động hóa.
Nhưng trước khi một người kỹ sư có thể thiết kế cho những hệ thống quá đỗi phức
tạp và khó khăn thì anh ta cần phải có sự tiếp cận thích hợp, các thuật toán điều khiển,
đặc biệt là giải thuật cân bằng luôn được giới kỹ sư quan tâm bởi sự quan trọng để xây
dựng nền tảng và ứng dụng rộng rãi của nó.
Hệ con lắc ngược quay (Rotary Inverted Pendulum) là một hệ phi tuyến, đa bậc,
“underactuated” điển hình, một tín hiệu ngõ vào – bốn tín hiệu ngõ ra (Single Input –
Multi Output), hệ thống gồm một thanh con lắc rơi tự do theo phương thẳng đứng quanh
trục của mình được kết nối với một thanh cánh tay hoặc xe chuyển động theo phương
ngang. Hệ thống có độ ổn định thấp, và phải hoạt động liên tục nhằm cân bằng con lắc
ngược ở trạng thái thẳng đứng hướng lên. Hệ RIP luôn là một đối tượng thử nghiệm hấp
dẫn để kiểm tra các định luật điều khiển tuyến tính và phi tuyến, nhằm đánh giá chất
lượng cho các giải thuật điều khiển từ cơ bản đến phức tạp. Dựa trên nền tảng hệ RIP,
các kỹ sư từ đó đã phát triển các mô hình đại diện cho hệ như chuyển động cánh tay
người, hệ thống ổn định cho tàu thuyền, vệ tinh vũ trụ,…
❖ Các kết quả nghiên cứu ngoài nước
Các nhà kỹ sư của viện tiêu chuẩn toàn cầu về thiết bị phòng thí nghiệm kỹ thuật
phục vụ cho việc giảng dạy và nghiên cứu chuyên về điều khiển, robot, và cơ điện tử
Quanser đã nghiên cứu về đối tượng này, đồng thời họ cũng thiết kế và sản xuất hệ RIP,
phân phối sản phẩm đến khắp các trường đại học trên thế giới với mục đích nghiên cứu
và dạy học dành cho các sinh viên, kỹ sư tự động hóa, đối tượng mà sản phẩm của
Quanser nhắm đến là các sinh viên cho nên bộ điều khiển họ thiết kế tương đối đơn giản
và dễ nắm bắt, đó là bộ điều khiển phản hồi trạng thái (state feedback controller) và bộ
1
điều khiển swing-up bằng phương pháp năng lượng [1]. Vào năm 2005, Nhóm tác giả
Yung-Chih Fu và Jung-Shan Lin đã thành công phát triển bộ điều khiển phi tuyến
backstepping nhằm cân bằng hệ con lắc ngược Furuta, mục tiêu của bài nghiên cứu
không những swing-up con lắc từ vị trí thẳng đứng hướng xuống thành thẳng đứng hướng
lên mà cánh tay còn phải đáp ứng yêu cầu quay trở về vị trí ban đầu [2]. Vào năm 2007,
nhóm các tác giả Mojtaba Ahmadieh Khanesar, Mohammad Teshnehlab, Mahdi Aliyari
Shoorehdeli đã cùng nhau thực hiện nghiên cứu thiết kế bộ điều khiển trượt (SMC) cho
hệ RIP, bộ điều khiển trượt đã thành công cân bằng và duy trì con lắc ở vị trí bất ổn định
thẳng đứng hướng lên nhưng nhóm tác giả nhận xét việc tính toán mặt trượt cho của hệ
thống khá khó khăn, vì hệ thống là hệ 2 bậc tự do tương đương với 2 mặt trượt, mặt trượt
được định nghĩa bằng hàm Lyapunov [3]. Vào năm 2010, hai tác giả Md Akhtaruzzaman
và A.A. Shafie đã thực hiện nghiên cứu con lắc ngược quay, trong bài nghiên cứu này,
họ xây dựng mô hình hóa, thiết kế và áp dụng nhiều bộ điều khiển khác nhau lên hệ
thống RIP nhằm so sánh và phân tích độ hiệu quả của từng giải thuật điều khiển, họ xây
dựng 3 bộ giải thuật đó là PID, FSF dựa trên công thức Ackerman, cuối cùng là giải thuật
LQR, kết quả họ thu được là tuy bộ điều khiển PID ổn định đối tượng ở vị trí thẳng đứng
hướng lên nhưng có biểu hiện đáp ứng kém khi có nhiễu đến từ môi trường, trong khi đó
giải thuật LQR và FSF có thể khắc phục sự ảnh hưởng của nhiễu và nhóm tác giả đánh
giá giải thuật LQR có độ tin cậy và đáp ứng tốt hơn so với FSF [4]. Vào năm 2013, tác
giả Rahul Agrawal và R.Mitra đã thiết kế thành công bộ điều khiển lai mạng thần kinh
mờ (neuron - fuzzy) cho hệ RIP, nhóm nghiên cứu xây dựng hai bộ điều khiển PID và
mạng thần kinh mờ để so sánh ưu điểm, và kết quả thực nghiệm cho thấy bộ điều khiển
lai đã thành công cân bằng con lắc, bộ điều khiển mạng thần kinh được huấn luyện nhờ
vào công cụ ANFIS trên MATLAB, sau đó lai với luật mờ mà tác giả tìm được, nhóm
tác giả nhận xét rằng bộ điều khiển lai có vài ưu điểm vượt trội so với bộ điều khiển PID
[5]. Cũng trong năm 2013, tác giả Jen-Hsing Li đã nghiên cứu hệ cân bằng bất ổn định
RIP, ông đề xuất thiết kế bộ điều khiển dựa theo phương pháp lai giữa hai bộ điều khiển
mờ (fuzzy) mamdani và PD, dựa theo kết quả khảo sát ông đánh giá rằng bộ điều khiển
lai fuzzy PD mang lại kết quả tương tự như bộ điều khiển hồi tiếp [6]. Vào năm 2016,
tác giả Arya Sunil và Manju G đã thiết kế thành công bộ điều khiển ứng dụng luật mờ
(fuzzy) cho hệ thống RIP, nhóm tác giả xây dựng phương trình động học và thiết kế luật
mờ sao cho con lắc ổn định ở vị trí cân bằng [7]. Vào năm 2017, nhóm tác giả S.
Balamurugan, P. Venkatesh, M. Varatharajan của trường đại học kỹ thuật K.L.N ở Ấn
Độ đã phát triển thành công bộ điều khiển lai trượt mờ (fuzzy – sliding mode) với bộ lọc
thống thấp để giảm thiểu ảnh hưởng dao động tần số cao, thực nghiệm thành trên mô
hình con lắc ngược của Quanser, bộ điều khiển trượt có nhiều ưu điểm nhưng đi cùng
với đó là hiện tượng nhiễu dao động ở ngõ vào, vì thế nhóm tác giả nghĩ ra phương án
2
áp dụng luật mờ để phân tích và triệt tiêu tín hiệu không mong muốn, tuy nhiên nhiễu
dao động vẫn chưa được triệt tiêu hết, nhóm tác giả liền thiết kế một bộ lọc thông thấp
để lọc các thành phần tín hiệu nhiễu ngõ vào ở tần số cao, kết quả đánh giá cho thấy bộ
điều khiển trượt mờ có bộ lọc thông thấp đã thành công loại bỏ tối đa nhiễu dao động và
thành công cân bằng con lắc [8]. Vào năm 2018, nhóm các tác giả Mehmet ÖKSÜZ,
Mehmet Burak ÖNAL, Recep HALICIOĞLU, Lale Canan DÜLGER đã nghiên cứu và
xây dựng bộ điều khiển hồi tiếp trạng thái dùng phương pháp đặt cực lên đối tượng RIP
và khảo sát hệ số giảm chấn tác động lên sự ổn định hệ thống, trong bài báo này, nhóm
tác giả thực hiện mô phỏng và thực nghiệm trên hệ thống để khả sát tính bền vững của
bộ điều khiển và độ hiệu quả của giải thuật [9]. Năm 2021, tác giả Belgacem Bekkar và
Khale Ferkous đã phát triển một giải thuật lai giữa bộ điều khiển toàn phương tuyến tính
LQR và mờ để con lắc ngược ổn định tại vị trí cân bằng và bám quỹ đạo cho cánh tay,
giải thuật này thay vì chọn ma trận Q và R bằng tay sau nhiều lần thử nghiệm kiểm tra
đúng sai mà sẽ sử dụng luật điều khiển mờ để chọn dựa vào kết quả đáp ứng mong muốn
và trạng thái của hệ thống, kết quả cho thấy bộ điều khiển lai cho đáp ứng nhanh hơn,
ổn định tốt, và khắc phục nhiễu tốt hơn trong môi trường thực nghiệm [10]. Vào năm
2022, tác giả Fatma Nur Sen1 and Ulas Beldek đã phát triển thành công bộ điều khiển
hồi tiếp trạng thái sử dụng giải thuật di truyền (GA) để tìm thông số tối ưu, bài nghiên
cứu trình bày hai phương pháp, một là thiết kế tiêu chuẩn cho bộ điều khiển hồi tiếp dựa
trên phương trình động học hệ thống, hai là sử dụng giải thuật di truyền để tìm thông số
tối ưu K cho bộ điều khiển hồi tiếp, sau đó tác giả tiến hành thực nghiệm và so sánh kết
quả giữa hai bộ điều khiển, cho thấy bộ điều khiển hồi tiếp sử dụng GA đáp ứng tốt hơn
[11]. Và cũng trong năm 2022, nhóm tác giả Anthony Gutarra, Styven Palomino, Elvis
J. Alegria, Jose Cisneros đã thiết kế thành công bộ điều khiển lai mờ tối ưu bằng giải
thuật di truyền, nhằm nâng cao đáp ứng so với một bộ điều khiển mờ thông thường, trong
bài báo thì một bộ điều khiển LQR được thiết kế dùng để so sánh hiệu suất, kết quả cho
thấy, bộ điều khiển mờ - di truyền có thể hoạt động trong một phạm vi lớn hơn so với bộ
điều khiển LQR [12].
❖ Các kết quả nghiên cứu trong nước
Vào năm 2011, tác giả Nguyễn Văn Đông Hải của trường đại học sư phạm kỹ thuật
đã giới thiệu thành công giải thuật tuyến tính hóa vào-ra (input-output linearization) cho
hệ phi tuyến xe-con lắc ngược, mục tiêu là biến đổi hệ thống từ dạng phi tuyến sang
tuyến tính, đặt biến ngõ ra để hệ thống có dạng tuyến tính những vẫn giữ được đặc trưng
của hệ phi tuyến [13]. Vào năm 2012, tác giả Nguyễn Đức Quyên và Ngô Văn Thuyên
của trường đại học sư phạm kỹ thuật cùng các cộng sự đã thành công xây dựng bộ điều
khiển mạng neuron nhân tạo (ANN) để điều khiển cân bằng con lắc ngược RIP, khả năng
3
học hỏi, tiếp thu qua quá trình thử nghiệm đúng sai và tự động thích nghi, điều chỉnh các
thông số của bộ điều khiển dựa vào thay đổi ở ngõ vào [14]. Cũng trong năm 2012, nhóm
tác giả Nguyễn Văn Đông Hải và Ngô Văn Thuyên của trường đại học sư phạm kỹ thuật
đã thành công xây dựng bộ điều khiển lai PID kết hợp mạng thần kinh neruon để điều
khiển ổn định hệ thống con lắc ngược, bài báo trình bày kết quả đáp ứng của hệ thống
trong mô phỏng và thực nghiệm [15]. Vào năm 2014, tác giả Nguyễn Văn Khanh và các
cộng sự của trường đại học Cần Thơ thực hiện bài nghiên cứu xây dựng bộ điều khiển
ổn định thời gian thực sử dụng phương pháp cuốn chiếu (backstepping), nhóm tác giả
nhúng giải thuật điều khiển trên vi điều khiển DSP STM32, bộ điều khiển sau đó được
so sánh với giải thuật LQR và thực nghiệm cho thấy bộ điều khiển cuốn chiếu cho ra kết
quả đáp ứng khả quan hơn so với bộ điều khiển LQR ngay cả khí tác động nhiễu từ bên
ngoài thay đổi khối lượng con lắc [16]. Vào năm 2017, tác giả Trần Văn Hào của trường
đại học HUTECH công bố nghiên cứu xây dựng bộ điều khiển trượt (SMC) cho hệ thống
con lắc ngược RIP, trong bài nghiên cứu, tác giả đã xây dựng thành công bộ điều khiển
dựa trên hàm Lyapunov, ngoài ra tác giả còn thiết kế bộ điều khiển swing-up dựa trên
cơ sở phân tích quỹ đạo chuyển động của con lắc trong miền không gian trạng thái [17].
Vào năm 2018, tác giả Lê Thị Thanh Hoàng của trường đại học sư phạm kỹ thuật và các
cộng sự đã xây dựng thành công bộ điều khiển mờ (FLC) và áp dụng thành công lên mô
hình, con lắc được cân bằng tại ví trí bất ổn định, luật mờ trong bài nghiên cứu được lựa
chọn theo tiêu chuẩn của các chuyên gia theo phương pháp Takegi-Sugeno gồm 81 luật,
các tham số bộ điều khiển được tính toán dựa trên thử nghiệm và kiểm tra lỗi thông qua
quan sát đáp ứng của hệ thống, bài báo trình bày kết quả mô phỏng và thực nghiệm cho
thấy hệ thống hoạt động ổn định [18]. Vào năm 2019, tác giả Hồng Gia Bảo của trường
đại học sư phạm kỹ thuật và các cộng sự đã thành công xây dựng bộ điều khiển trượt
bám quỹ đạo (trajectory tracking sliding mode control) cho hệ thống xe-con lắc, bài báo
đề xuất một giải thuật xuất tín hiệu điều khiển động cơ chiếc xe sao cho bám theo một
quỹ đạo xác định trong khi vừa đảm bảo cân bằng con lắc ổn định, tác giả so sánh đối
chiếu bộ điều khiển trượt so với bộ điều khiển LQR trong thực nghiệm và kết luận rằng
điều khiển trượt có đáp ứng tốt hơn so với LQR [19]. Vào năm 2022, tác giả Võ Minh
Tài và các cộng sự của trường đại học sư phạm kỹ thuật đã công bố trình bày nghiên cứu
của mình về xây dựng giải thuật điều khiển hồi tiếp tuyến tính hóa vào ra cho hệ con lắc
ngược RIP, nhóm tác giả đã duy trì thanh con lắc của hệ thống quanh vùng cân bằng ở
vị trí thẳng đứng hướng lên [20].
Trong các nghiên cứu được liệt kê ở trên thì tác giả nhận ra giải thuật tuyến tính
hóa là một giải thuật độc đáo nhưng đáng tiếc là ít được các nhà kỹ sư nghiên cứu và áp
dụng vào hệ thống. Ngoài ra ở nghiên cứu [20] của tác giả Võ Minh Tài, tác giả đề tài
4
nhận thấy tuy hệ thống đã được ổn định nhưng vẫn còn thiếu sót và bộ giải thuật có thể
được cải thiện hơn nữa.
1.2 Lý do chọn đề tài
Như đã được đề cập ở trên, ngành điều khiển tự động đang trở thành một trong
những ngành quan trọng trong bối cảnh cuộc cách mạng công nghiệp 4.0 đang đến
rất gần. Các giải pháp điều khiển thông minh, điều khiển hiện đại ngày càng được
quan tâm nghiên cứu để thực hiện các ứng dụng mà các giải pháp điều khiển thông
thường không thực hiện được. Đề tài “THIẾT KẾ BỘ ĐIỀU KHIỂN TUYẾN TÍNH
HÓA VÀO RA CHO HỆ CON LẮC NGƯỢC QUAY” sẽ là bước đệm để nghiên cứu
các hệ phi tuyến phức tạp áp dụng giải thuật điều khiển cao cấp hơn.
1.3 Đối tượng nghiên cứu.
Trong giới hạn của đề tài, người thực hiện chọn hệ con lắc ngược quay – Rotart
Inverted Pendulum (RIP) là một hệ phi tuyến bậc cao. Sau đó áp dụng giải thuật tuyến
tính hóa vào ra để điều khiển mô hình.
Để xây dựng và vận hành hệ thống, đề tài sử dụng vi xử lý Arduino Mega 2560 và
mạch công suất L298N, Encoder. Xây dựng phương trình động lực học và mô phỏng
đáp ứng của hệ thống trong phần mềm Matlab/Simulink. Lập trình giải thuật điều khiển
ngôn ngữ C bằng phần mềm Arduino IDE và nhúng giải thuật trên vi điều khiển.
Do sự hạn chế về thời gian cũng như đặc tính làm việc của các linh kiện, chi tiết
cơ học chưa thực sự tốt cho nên chưa thể hoàn chỉnh đề tài một cách hoàn hảo, tuy
nhiên, đề tài đã cố gắng hoàn thành được những mục tiêu đặt ra.
1.4 Giới hạn đề tài
• Đề tài chỉ giải quyết đến các vấn đề động học của con lắc ngược
• Thuật toán áp dụng lên hệ thống chỉ có tác dụng ổn định con lắc
• Mô hình con lắc ngược được xây dựng tương đối đơn giản
• Chương trình điều khiển tuyến tính hóa vào ra được xây dựng bằng ngôn ngữ C
và nhúng vào vi điều khiển Arduino
1.5 Mục tiêu nghiên cứu
• Thiết kế mô hình con lắc ngược quay
• Nhận dạng và tính toán thành công phương trình động lực học của con lắc
ngược quay áp dụng cơ học Lagrange
• Xây dựng được bộ điều khiển tuyến tính hóa vào ra, mô phỏng trên Matlab
• Xây dựng được giải thuật bằng ngôn ngữ C và nhúng trên Arduino
5
• So sánh kết quả giữa mô phỏng và thực tế
1.6 Phương pháp nghiên cứu
Để thực hiện việc nghiên cứu đề tài này, tác giả tuân theo các quá trình sau:
• Phân tích con lắc ngược khi hệ thống chưa có bộ điều khiển
• Phân tích công thức toán học với sự hỗ trợ của phần mềm Matlab
• Xây dựng giải thuật điều khiển và sử dụng phần mềm Matlab để mô phỏng. Phân
tích kết quả đáp ứng, giám sát tín hiệu ngõ vào, ngõ ra trong môi trường
Matlab/Simulink.
• Nhúng giải thuật lên Arduino. Phân tích kết quả đáp ứng thực tế của hệ thống
bằng dữ liệu thu thập theo thời gian thực để đánh giá độ hiệu quả của bộ điêu
khiển.
• Ổn định cân bằng của hệ thống là tiêu chí đánh giá trọng điểm của giải thuật.
1.7 Giới thiệu nội dung
• Chương 1: Tổng quan
Chương 1 sẽ trình bày lý do vì sao tác giả thực hiện đề tài, mục tiêu, giới hạn đề
tài, phương pháp nghiên cứu sử dụng trong đề tài.
• Chương 2: Cơ sở lý thuyết
Chương 2 sẽ giới thiệu tổng quan về hệ thống con lắc ngược, mô tả các lý thuyết
liên quan sẽ được áp dụng trpng đề tài tác giả tiêu biểu như: đạo hàm Lie, bậc
tương đối hệ thống, giải thuật tuyến tính hóa vào ra.
• Chương 3: Mô tả toán học hệ con lắc ngược quay
Chương 3 sẽ trình bày cơ sở khoa học, thành lập phương trình động học cho con
lắc ngược quay và cuối cùng là xây dựng hệ phương trình trạng thái phi tuyến của
hệ thống.
• Chương 4: Thiết kế bộ điều khiển tuyến tính hóa vào ra
Chương 4 sẽ trình bày tường tận các bước phương pháp xây dựng bộ điều khiển
tuyến tính hóa vào ra và đề cập đến việc áp dụng giải thuật LQR vào giải thuật
tuyến tính hóa vào ra.
• Chương 5: Thi công hệ thống
Chương 5 sẽ giới thiệu về phần cứng và phần mềm của mô hình bao gồm các linh
kiện điện tử và phần đế hệ thống, phần mềm để lập trình C và thu thập dữ liệu,
lưu đồ giải thuật.
• Chương 6: Kết quả thực hiện
Chương 6 sẽ trình bày các kết quả đáp ứng mô phỏng, thực nghiệm của hệ thống
và nhận xét của tác giả.
6
• Chương 7: Kết luận và hướng phát triển
7
CHƯƠNG 2: CƠ SỞ LÝ THUYẾT
2.1 Giới thiệu về hệ con lắc ngược quay
❖ Hệ xe-con lắc ngược (Cart and pole)
Hệ con lắc ngược quay là hệ thống mà trong đó vị trí của điểm trọng tâm khối
lượng thanh con lắc nằm ở trên trục quay của nó. Là một hệ thống phi tuyến kinh điển
bất ổn định, có một ngõ vào – nhiều ngõ ra (SIMO), là nền tảng thử nghiệm nổi tiếng để
đánh giá các giải thuật điều khiển khác nhau, từ hệ con lắc ngược nhiều nhà khoa học đã
phát triển lên thành nhiều ứng dụng trong cuộc sống như lĩnh vực hàng không, người
máy, máy bay không người lái,… Hệ thống này có đặc điểm nếu không có lực hỗ trợ tác
động từ bên ngoài vào hệ thống thì ngay lập tức con lắc sẽ ngã xuống. Trong hình 2.1,
thanh con lắc chỉ có thể được ổn định ở vị trí thẳng đứng hướng lên hoặc còn gọi là vị
trí bất ổn định với sự hỗ trợ từ hệ thống điều khiển tự động, hệ thống sẽ giám sát sự biến
thiên của góc quay thanh con lắc và gửi tín hiệu điều khiển xuống động cơ để để chiêc
xe di chuyển theo phương ngang, tác động của ngoại lực này khiến cho thanh con lắc tự
động di chuyển, hoặc có thể nói là dưới tác động có tính toán ta sẽ khiến con lắc luôn
dao động quanh vị trí cân bằng, từ đó con lắc luôn ổn đinh tại vị trí mong muốn theo lực
quán tính, bộ điều khiển ở đây sẽ tính toán giải thuật sao cho con lắc luôn cân bằng.
Trong các phòng thí nghiệm, ta thường xuyên nhìn thấy hệ con lắc ngược được gắn trên
một chiếc xe di chuyển theo phương ngang dưới sự điều khiển của động cơ servo, hệ này
được gọi là hệ xe-con lắc ngược.
Hình 2.1 Mô hình hệ xe-con lắc ngược
8
❖ Hệ con lắc ngược quay (Rotary Inverted Pendulum)
Hệ con lắc ngược quay còn được gọi là Rotary Inverted Pendulum hay Furuta
Pendulum, là một hệ under-actuated có ngõ vào điều khiển ít hơn số bậc tự do và rất khó
để điều khiển, đồng thời cũng là một hệ SIMO, hệ bao gồm một thanh cánh tay và một
thanh con lắc, trục quay của thanh cánh tay được gắn với động cơ quay theo phương
ngang, và thanh con lắc được gắn vào thanh cánh tay quay tự do trong mặt phẳng thẳng
đứng. Hệ thống này được phát minh bởi kỹ sư Katsuhisa Furuta và các đồng nghiệp của
ông, công bố lần đầu tiên vào năm 1992 tại học viện công nghệ Tokyo, một trường hợp
về bộ dao động phi tuyến phức tạp được quan tâm trong lý thuyết hệ thống điều khiển.
Bản thân hệ thống hoạt động kém hiệu quả và mang đặc tính cực kỳ phi tuyến do lực hấp
dẫn và ngẫu lực sinh ra từ hiệu ứng Coriolis và lực hướng tâm. Kể từ đó, đã có hàng
chục, hàng trăm bài báo nghiên cứu khoa học và luận văn đã sử dụng hệ thống này để
thiết kế và thử nghiệm các bộ điều khiển tuyến tính và phi tuyến.
Mô hình con lắc ngược quay được ứng dụng trong rất nhiều phòng thí nghiệm
nhằm nghiên cứu các giải thuật điều khiển thông minh, điều khiển hiện đại, điều khiển
tối ưu. Hệ con lắc ngược Furuta sẽ là hệ thống mà bài luận văn này tập trung nghiên
cứu gồm 3 thành phần chính như sau.
Phần cơ khí: gồm 1 cây kim loại (thanh con lắc) quay quanh 1 trục thẳng đứng.
Thanh con lắc được gắn gián tiếp vào thanh cánh tay thông qua một encoder để đo góc
con lắc. Một encoder được gắn lên trục động cơ để đo góc cánh tay. Do trong quá trình
vận hành thanh cánh tay phải quay với tốc độ cao để lấy mẫu nên phần cơ khí cần phải
được tính toán thiết kế chính xác, chắc chắn nhằm tránh gây nhiễu và hư hỏng trong
quá trình vận hành.
Điện tử: gồm cảm biến encoder đo góc cánh tay và góc con lắc, mạch khuếch đại
công suất (cầu H) L298N và mạch điều khiển trung tâm Arduino . Cảm biến đƣợc sử
dụng trong đề tài là encoder quay có độ phân giải cao. Tín hiệu từ encoder sẽ đượcc
truyền về bộ điều khiển. Tùy thuộc vào tín hiệu đọc được từ các encoder mà Arduino
được lập trình để xuất tín hiệu ngõ ra điều khiển động cơ DC qua một mạch khuếch đại
công suất (mạch cầu H).
Chương trình: Chương trình được viết trên môi trường phát triển tích hợp
Arduino bằng ngôn ngữ C và nhúng vào vi điều khiển. Tốc độ điều khiển hệ thống thực
phụ thuộc rất nhiều vào tốc độ xử lí và tần số lấy mẫu của bộ điều khiển trung tâm.
9
Hình 2.2 Mô hình hệ con lắc ngược quay Furuta, chế tạo bởi hãng Quanser
❖ Một số hệ thống phức tạp tiêu biểu được dựa trên hệ con lắc ngược
− Hệ con lắc ngược quay kép
− Hệ con lắc ngược quay song song
− Hệ bánh xe con lắc ngược quay
Hình 2.3 Hệ con lắc ngược quay kép
10
Hình 2.4 Hệ con lắc ngược quay song song
Hình 2.5 Hệ bánh xe con lắc ngược
2.2 Giới thiệu về đạo hàm Lie
Giả sử hệ phương trình trạng thái của một hệ thống phi tuyến bất kì có dạng như
sau:
 x = f ( x ) + g ( x ) u

 y = h ( x )
(2.1)
Trong đó:
f(x), g(x): các hàm phi tuyến
11
x: các biến trạng thái của hệ thống
u: tín hiệu điều khiển
y, h(x): tín hiệu ngõ ra
Định nghĩa: Đạo hàm Lie theo phương f(x) của hàm vô hướng g(x) là một vô
hướng, kí hiệu là L f h ( x ) được trình bày dưới dạng công thức như sau:
Lf h ( x) =
h
f ( x)
x
(2.2)
Đạo hàm Lie bậc n có thể được định nghĩa một cách đệ quy như sau:
(
Lf h ( x) = Lf Lf
( n)
( n −1)
)
h ( x) =
(
 Lf (
n −1)
h ( x)
x
) f ( x); L
0
f
h ( x) = h ( x)
(2.3)
Ta còn có thể mở rộng định nghĩa đạo hàm Lie như sau:
Lg L f h ( x ) = Lg ( L f h ( x ) ) =
 ( Lf h ( x ))
x
g ( x)
(2.4)
2.3 Giới thiệu về bậc tương đối của hệ thống
Điều khiển hồi tiếp tuyến tính là một phương pháp thông dụng được áp dụng để
điều khiển hệ phi tuyến, nếu ta có một hệ thống phi tuyến được diễn tả bởi hệ phương
trình sau:
 x = f ( x ) + g ( x ) u

 y = h ( x )
(2.5)
Bậc tương đối của hệ thống là  (1    n ) nếu thỏa điều kiện tín hiệu điều khiển
u không tồn tại trong các phương trình y, y,
, y(
 −1)
và y
()
, với n là số biến trạng
thái của hệ thống.
Ta có thể diễn tả toán học lại như sau:
y=
h ( x )
h ( x )
x=
( f ( x) + g ( x)u )
x
x
(2.6)
y = L f h ( x ) + Lg h ( x ) u , nếu Lg h ( x ) = 0 thì y = L f h ( x ) → hệ thống có bậc tương đối
lớn hơn 1
 ( Lf h ( x ))
 ( Lf h ( x ))
(2.7)
( f ( x) + g ( x)u )
x
y = L f 2 h ( x ) + Lg L f h ( x ) u , nếu Lg L f h ( x ) = 0 thì y = L f 2 h ( x ) → hệ thống có bậc
y=
x
x=
tương đối lớn hơn 2
12
y(
y(
 −1)
= Lf (
 −1)
h ( x ) + Lg L f (
 −1)
 − 2)
= Lf (
 −1)
h ( x ) + Lg L f (
h ( x ) u , nếu Lg L f (
 − 2)
 − 2)
h ( x)u
(2.8)
h ( x ) = 0 thì y (
 −1)
= Lf (
 −1)
h ( x) →
hệ thống có bậc tương đối lớn hơn  − 1
y ( ) = L f ( ) h ( x ) + Lg L f (


 −1)
h ( x)u
(2.9)
nếu Lg L f (  −1) h ( x )  0 thì y (  ) = L f (  ) h ( x ) + Lg L f (  −1) h ( x ) u → hệ thống có bậc tương
đối là 
Định nghĩa: Hệ thống H có bậc tương đối là  nếu và chỉ nếu 2 điều kiện sau
được thỏa mãn:
 Lg h ( x ) = Lg L f h ( x ) = Lg L f 2 h ( x ) =

(  −1)
h ( x)  0
 Lg L f
= Lg L f (
 − 2)
h ( x) = 0
(2.10)
2.4 Giới thiệu về giải thuật tuyến tính hóa vào ra
Xét đối tượng điều khiển H có phương trình trạng thái phi tuyến như sau:

x = f ( x) + g ( x)u
H =

 y = h ( x)
(2.11)
Trong đó:
x : các biến trạng thái của hệ thống
f ( x ) , g ( x ) : các hàm phi tuyến của hệ thống
u : tín hiệu điều khiển hệ thống
y, h ( x ) : tín hiệu, hàm ngõ ra hệ thống
Nếu hệ thống có bậc tương đối  = n thì nhiệm vụ của ta là đặt biến trạng thái lại
dưới dạng đạo hàm Lie như sau:
 h ( x) 


Lf h ( x) 

z = T ( x) =


 n −1

 L f h ( x ) 
Phương trình trạng thái của đối tượng điều khiển H được viết lại dưới dạng:
 z = AC z + BC  ( x ) u −  ( x ) 
H'=
 y = CC z
(2.12)
(2.13)
Luật điều khiển được xác định u =  ( x ) +  ( x ) v , sao cho hệ thống mới H’ tuyến
−1
tính:
13
 z = AC z + BC v
H'=
 y = CC z
(2.14)
Trong đó:
0 1
0 0

AC = 


0
CC = 1 0
0
1
1
0
0
0
0
0 
0 
0 
 , BC =  
 

1
 
1 
0 
Hệ thống có bậc tương đối  = n khi và chỉ khi thỏa 2 điều kiện sau:
 Lg L f ( i −1) h ( x ) = 0, i = 1, 2,

( n −1)
h ( x)  0
 Lg L f
, n −1
(2.15)
Theo (2.12) thì hệ phương trình trạng thái của H’ được diễn giải như sau:
z1 = h ( x )
h ( x )
h ( x )
( f ( x ) + g ( x ) u ) = L f h ( x ) + Lg h ( x ) u = L f h ( x ) = z2
x
x
 ( Lf h ( x ))
 ( Lf h ( x ))
z2 =
x=
( f ( x ) + g ( x ) u ) = L f 2h ( x ) + Lg L f h ( x ) u = L f 2h ( x ) = z3
x
x
z1 =
zn −1 =
zn =
(
x=
 Lf (
(
 Lf (
n − 2)
h ( x)
x
n −1)
h ( x)
x
) x = (L (
n − 2)
f
) x = (L (
h ( x)
x
n −1)
f
x
) ( f ( x) + g ( x)u ) = L (
n −1)
f
h ( x)
h ( x ) + Lg L f (
) ( f ( x ) + g ( x ) u ) = L ( )h ( x ) + L L (
n
f
g
n −1)
f
n −2)
h ( x)u = Lf (
n −1)
h ( x)u
(2.16)
Từ (2.16), ta biểu diễn dưới dạng ma trận như sau:
 z1  0
 z  0
 2  
 =
  
 zn −1  0
 zn  0
1 0
0 1
0 0
0 0
0   z1  0 
0   z2  0 
n
 
L f ( )h ( x )  
   +   Lg L f ( n −1) h ( x )  u −  −

  Lg L f ( n −1) h ( x )  
   

 
0   zn −1  0 
0   zn  1 
(2.17)
Từ (2.17), ta rút gọn lại thành:
z = AC z + BC  ( x ) u −  ( x ) 
(2.18)
14
h ( x ) = zn
Luật điều khiển được xác định u =  ( x ) +  ( x ) v , suy ra:
−1
z = AC z + BC v
(2.19)
Ta xem v như một tín hiệu điều khiển giả định, dùng luật LQR để tính toán tín
hiệu Q, ta được v = − Kz
z = ( AC − BC K ) z
 u =  ( x ) −  ( x ) Kz
Trong đó:
 ( x) = −
(2.20)
Lf nh ( x )
Lg L f n −1h ( x )
 ( x ) = Lg L f n −1h ( x )
 ( x) =  ( x)
−1
15
CHƯƠNG 3:MÔ TẢ TOÁN HỌC HỆ CON LẮC NGƯỢC QUAY
3.1 Cơ sở khoa học
Tất cả hệ thống điều khiển tự động đều được mô tả bằng một phương trình toán
học dạng vi phân. Hệ con lắc ngược quay cũng vậy, để điều khiển được hệ con lắc
ngược quay, việc đầu tiên là phải nắm rõ được đối tượng, từ đó mới đưa ra phương
pháp điều khiển.
Đối với hệ con lắc ngược quay, để mô tả được hệ dưới dạng một phương trình
toán học thì đó là sự kết hợp giữa vật lý và toán học với định luật cổ điển Lagrange
(dựa trên tổng năng lượng của hệ và lực tổng quát trong một hệ trục tọa độ
Descarts )
Phương trình Lagrange loại 1:
f
L d  L  C
−   +  i i = 0
rk dt  rk  i =1 rk
(3.1)
Trong đó:
rk là vị trí của chất điểm thứ i trong hệ
C là số lượng liên kết trong hệ
i là nhân tử Lagrange thứ i
fi là phương trình liên kết thứ i
L là hàm Lagrange của một hệ các hạt (chất điểm):
Toán tử Lagrange được tính như sau:
L = T −V
(3.2)
Trong đó:
T là tổng động năng của các chất điểm có trong hệ
V là tổng thế năng của các chất điểm có trong hệ
3.2 Thành lập phương trình động học cho hệ con lắc ngược quay
16
Hình 3.1 Mô hình toán học của hệ con lắc ngược quay
Mô hình hệ RIP bao gồm 2 thanh (con lắc, cánh tay) 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 được được dựng theo phương thẳng đứng nối với thanh
cánh tay. Góc quay của thanh cánh tay, thanh con lắc được ký hiệu lần lượt là  ,  ( rad )
Mục đích của chúng ta là thiết kế bộ điều khiển cân bằng sao cho thanh con lắc cân bằng
ở vị trí thẳng đứng hướng lên.
Trong đó, M r ( Kg ) là khối lượng thanh cánh tay, M p ( Kg ) là khối lượng thanh con
lắc, M en ( Kg ) là khối lượng của encoder, Lr ( m ) là chiều dài thanh cách tay, L p ( m ) là
chiều dài thanh con lắc
J r ( Kgm 2 ) là mô-men quán tính của thanh cánh tay, J p ( Kgm 2 )
là mô-men quán tính của thanh con lắc, g ( m / s 2 ) là gia tốc trọng trường,  là mô-men lực
điều khiển của động cơ, K t ( N .M / A ) là hằng số mô-men, K b (V .s ) là hằng số suất phản
điện động, Rm (  ) là điện trở động cơ Cm ( N .m.s ) hệ số ma sát nhớt động cơ, e (V ) là điện
áp điều khiển động cơ. Xét ma sát thanh cánh tay và thanh con lắc 1,2 Cr = C p  0 ( NA)
Điểm B là điểm trọng tâm của thanh con lắc: AB =
Lp
2
Giả sử A là giao điểm của tọa độ xyz được đặt trên cách tay, trọng lực của con
lắc đặt tại điểm giữa B, như vậy B sẽ có tọa độ,vận tốc so với A là:
17
(
)
1
Lp − Sin  i + Cos  j
2
.
1
 vB / A = rB / A = Lp − Cos  i − Sin  j
2
rB / A =
(
(3.3)
)
Đồng thời với chuyển động quay quanh điểm A thì con lắc còn thực hiện
chuyển động quay quanh điểm O với vận tốc Lr  . Do vậy, vận tốc của điểm A so với
điểm O cố định có thể được mô tả bởi phương trình :
vA/ O = Lr  i
(3.4)
Từ hai phương trình (2.3) và (2.4), vận tốc của điểm B, trọng tâm của con lắc
được mô tả theo hai phương x,y như sau:
1

 1
vB / O = vB / A + vA/ O =  Lr  − Lp Cos  iˆ − Lp Sin ˆj
2

 2
(3.5)
Xét điểm O là gốc thế năng, vậy tổng thế năng của hệ thống là:
Vsystem =
1
Lp M p gCos
2
(3.6)
Tổng động năng của hệ thống: Tsystem = Tpendulum + Tarm
Trong đó:
2
1
1
J p 2 + M p vB / O
2
2
2
1
1
= J r  2 + M en vA/ O
2
2
Tpendulum =
Tarm
(3.7)
(3.8)
Momen quán tính của thanh cánh tay là:
1
J r = M r Lr 2
3
(3.9)
Momen quán tính của thanh con lắc về trọng tâm khối lượng là :
Jp =
1
M p Lp 2
12
(3.10)
Vậy tổng động năng của hệ thống là: Tsystem = Tpendulum + Tarm
Tsystem =
(
1
1
1
J r  2 + J p 2 + M en Lr 
2
2
2
)
2
2
2

1
1
  1
 
+ M p  Lr  − Lp Cos  +  − Lp Sin   (3.11)
2
2
  2
 

Hàm Lagrange L = Tsystem − Vsystem
L=
Lp 2 M p  2
6
+
Lr 2 M p  2
2
+
Lr 2 M r  2 Lp M p g Cos Lp Lr M p   Cos 
−
−
6
2
2
(3.12)
18
Từ phương trình Lagrange Loại 1:
(
L  ,  ,  , 
 ( ,  )
) −   L ( , ,  ,  )  +  = 0 , ta
t 

 ( ,  )

áp dụng cho hệ con lắc ngược quay như sau:
   L  L
= 
 
−
 t    

   L  − L = 0
 t    
(3.13)
Trong đó:
 L
  = 0

 L Lr 2 M r 
Lp Lr M p Cos ( ) 
=
+ Lr 2 M p  −

3
2
 
   L 
L p Lr M p Cos L p Lr M p Sin 2 Lr 2 M r 
2
 
=
L
M

−
+
+
+ Lr 2 M en 
r
p

2
2
3
 t   
(3.14)

 L L p M p gSin L p Lr M p gSin
+
  =
2
2

2
 L L p M p L p M p Lr Cos
=
−



3
2

2
   L  L p M p L p Lr M p Cos L p Lr M p Sin
−
+
 
=
2
2
2
 t   
Từ (3.13) và (3.14), ta tìm được phương trình động lực học của hệ con lắc ngược
quay:
1
1
1
 2
( Lr M en + Lr 2 M p + Lr 2 M r )  − Lp Lr M pCos + Lp Lr M p Sin 2 = 


3
2
2

..
− 1 L L M Cos + 1 L 2 M  − 1 L M gSin = 0
p r
p
p
p
p
p

3
2
 2
(3.15)
Giải hệ phương trình (3.15) ta được:
19

−9 Lp Lr M p Cos ( ) Sin ( )  2 + 18 Cos ( ) + 18Lr M en gSin ( ) + 18Lr M p gSin ( ) + 6 Lr M r gSin ( )
 =
2
12 Lp Lr M p + 4 Lp Lr M r − 9 Lp Lr M pCos ( )

(3.16)

−12 Lp Lr M p Sin ( )  2 + 18Lr M p gCos ( ) Sin ( ) + 24

 =
24 Lr 2 M p + 8Lr 2 M r − 18Lr 2 M p Cos( ) 2

Từ (3.15), ta viết lại dưới dạng ma trận, phương trình động học của hệ con lắc
ngược quay với ngõ vào là momen động cơ 𝜏, ngõ ra là góc α,β.
(
)
D( ,  )  ,  + C ( ,  ,  ,  )( ,  ) + G( ,  ) = T
(3.17)
Trong đó:
 L p Lr M p Cos
−
2
D ( ,  ) = 
2
Lp M p


3


1
Lr 2 M en + Lr 2 M p + Lr 2 M r 
3

Lp Lr M p Cos

−

2

 Lp Lr M p Sin ( alpha ) 
C  , ,  ,  = 
2

0

0



G ( ,  ) = Lp M p gSin 
−



2
(
)
(3.18)

0

0 
(3.19)
(3.20)
 
T = 
0 
Kí hiệu
(3.21)
Mô tả
Đơn vị

Góc của thanh con lắc so với phương thẳng
đứng
rad

Góc của thanh cánh tay so với vị trí mốc
rad

Vận tốc góc của thanh con lắc
rad/s

Vận tóc góc của thanh cánh tay
rad/s

Gia tốc góc của thanh con lắc
rad/s2
Giá trị
Đại lượng
tính toán
Đại lượng
tính toán
Đại lượng
tính toán
Đại lượng
tính toán
Đại lượng
tính toán
20

Gia tốc góc của thanh cánh tay
rad/s2
M en
Khối lượng của encoder
kg
Đại lượng
tính toán
0.2
Mp
Khối lượng của thanh con lắc
kg
0.01392
Lp
Chiều dài của thanh con lắc
m
0.2
Jp
Momen quán tính của thanh con lắc
kgm2
0.0001856
Mr
Khối lượng của thanh cánh tay
kg
0.04872
Lr
Chiều dài của thanh cánh tay
m
0.2
Jr
Momen quán tính của thanh cánh tay
kgm2
0.0006496
e
Điện áp động cơ
V
g
Gia tốc trọng trường
m/s2
rB / A
Khoảng cách từ vị trí B so với vị trí A
m
vB / A
Vận tốc từ vị trí B so với vị trí A
m/s
v A/ O
Vận tốc từ vị trí A so với vị trí O
m/s
vB / O
Vận tốc từ vị trí B so với vị trí O
m/s
Đại lượng
tính toán
9.81
Đại lượng
tính toán
Đại lượng
tính toán
Đại lượng
tính toán
Đại lượng
tính toán
Bảng 3.1 Ký hiệu mô hình con lắc ngược quay
Tuy nhiên, ở hai công thức (3.15) và (3.16) mô tả phương trình động học của hệ
con lắc với ngõ vào là momen 𝜏, để thuận tiện cho việc điều khiển hệ, cần phải biến đổi
phương trình động học của hệ thống về dạng ngõ vào là điện áp, phù hợp cho việc sử
dụng động cơ. Như vậy, việc điều khiển của chúng ta sẽ đơn giản hơn.
Đồng thời, ta có thể thấy rằng, bản thân động cơ cũng là một hệ phi tuyến và
điện áp mới là thành phần điều khiển mà người điều khiển sử dụng cho hệ thống.
Moment động cơ chỉ là thành phần được tạo ra do quá trình sử dụng điện áp. Do đó
việc chuyển đổi phương trình toán học về dạng ngõ vào điện áp (thay vì moment) là
cần thiết.
21
Hình 3.2 Cấu trúc động cơ DC
Ký
hiệu
Mô tả
Đơn vị
Giá trị
Rm
Điện trở động cơ (ohm)
Ω
11.944421124154792
Lm
Cuộn cảm động cơ (H)
H
NSX cung cấp
Kb
Hằng số suất phản điện động
V/rad/sec
Kt
Hằng số moment (Nm/A)
Nm/A
Jm
Moment quán tính của rotor động
cơ DC
kgm2
Jm
Hệ số ma sát nhớt
Nm/rad/sec
f
Moment ma sát
Nm
NSX cung cấp
0.086164500636167
0.086164500636167
0.000059833861116
0.000067435629646
m

i
Moment xoắn cản
Nm
Đại lượng tính toán
Vận tốc động cơ DC
rad/s
Đại lượng tính toán
Moment xoắn nội
Nm
Đại lượng tính toán
m
Góc xoay trục động cơ
rad
Đại lượng tính toán
Bảng 3.2 Ký hiệu và thông số động cơ DC Nisca
Thông số của động cơ Nisca được cung cấp bởi bài báo nghiên cứu
Cấu trúc của động cơ được chia ra thành 2 phần: Phần điện và Phần cơ
Phân tích phần điện:
e = Lm
di
+ Rmi + Eb
dt
(3.22)
22
Mà Eb = Kb , thế vào (3.22) ta được
e = Lm
di
+ Rmi + Kb
dt
(3.23)
Phân tích phần cơ:
Jm
d
=  i − Cm −  m
dt
(3.24)
Mà  i = K t i , thế vào (3.24), ta được
d
Jm
= Kt i − Cm −  m
(3.25)
Pe = Ebi (W )
(3.26)
Pm =  i (W )
(3.27)
dt
Công suất điện:
Công suất cơ:
Áp dụng định luật bảo toàn năng lượng:
Pe = Pm = Ebi =  i
 K bi = K ti
(3.28)
 Kb = Kt
Trong trường hợp 𝐾𝑏 và 𝐾𝑡 dùng đơn vị MKS (Kb có đơn vị V/rad/s) và Kt có
đơn vị Nm/A. Lúc này, Kb = Kt
Vì tốc độ điện nhanh hơn tốc so với tốc độ cơ khí 𝑒 >> 𝐿𝑚
yếu tố điện cảm trong công thức
Lm
𝑑𝑖
𝑑𝑡
nên có thể bỏ qua
e − Kb
di
= 0 = e = Rmi + Kb = i =
dt
Rm
(3.29)
Kt
K
K
(e − Kb ) = t e − t Kb
Rm
Rm
Rm
(3.30)
 i = Kt i =
.
Ta có:  =  m   =  m =  , thay vào (3.30) ta được:
i =
Kt
K
e − t Kb 
Rm
Rm
(3.31)
Thay (3.31) vào (3.25), ta được:
 m = − J m  − (Cm +
Kt
K
Kb )  + t e
Rm
Rm
(3.32)
Ta đặt:
23
K1 =
Kt
K
; K 2 = Cm + t Kb ; K3 = J m
Rm
Rm
(3.33)
=  m = − K3  − K 2  + K1e = 
(3.34)
Ta kết hợp các phương trình sau (3.15) và (3.34), suy ra được ma trận phương trình
động học của hệ con lắc ngược quay với ngõ vào là điện áp e , ngõ ra là góc  ,  .
(
)
D( ,  )  ,  + C ( ,  ,  ,  )( ,  ) + G( ,  ) = V
(3.35)
Trong đó:
 L p Lr M p Cos
−
2
D ( ,  ) = 
2
Lp M p


3


1
K 3 + Lr 2 M en + Lr 2 M p + Lr 2 M r 
3

L p Lr M p Cos

−

2

 Lp Lr M p Sin ( alpha ) 
C  , ,  ,  = 
2

0

(
)

K2 

0 
0



G ( ,  ) = Lp M p gSin 
−



2
 K e
V = 1 
 0 
(3.36)
(3.37)
(3.38)
(3.39)
Mô tả toán học hệ con lắc ngược quay dưới dạng phương trình vi phân. Từ công
thức (3.35), đưa vào matlab tính toán, ta xác định được:
 = (3(6 K 3gSin( ) - 6K 2 L r  Cos( ) + 6 K1L r eCos( ) + 6L r 2 M en gSin( ) +
6L r 2 M p gSin( ) + 2L r M r gSin( ) - 3L p Lr 2 M p 2 Cos( )Sin( )))
(3.40)
/(Lp (12 K 3 + 12L r 2 M en + 12L r 2 M p + 4L r 2 M r - 9L r 2 M p Cos( ) 2 ))
 = 3(2Lp Lr M pSin( ) 2 2 + 4K 2  - 4K1e - 3L r M p gCos( )Sin( ))
/ (12 K 3 + 12 Lr 2 M en + 12 Lr 2 M p + 4 Lr 2 M r − 9 Lr 2 M p Cos( ) 2 )
(3.41)
3.3 Hệ phương trình trạng thái phi tuyến của mô hình
Đặt biến trạng thái của hệ thống như sau:
24
T
x =  x1 x2 x3 x4  =     
(3.42)
Từ (2.11), hệ phương trình biến trạng thái phi tuyến của hệ có dạng như sau:
 x1 
   t1 
x 
  t 
d
d
d 
(3.43)
x = f ( x, u , t ) =  2  =   =  2 
dt
dt  x3  dt     t3 
 
   
   t4 
 x4 
Trong đó:
x2




2
2
2
2
2
18
K
gSin
(
x
)
−
18
K
L
x
Cos
(
x
)
+
18
L
M
gSin
(
x
)
+
18
L
M
gSin
(
x
)
+
6
L
M
gSin
(
x
)
−
9
L
L
M
x
C
os
(
x
)
Sin
(
x
)
 x2 
3
1
2 r 4
1
r
en
1
r
p
1
r
r
1
p r
p 2
1
1 


 f 
Lp (12 K 3 + 12 L r 2 M en + 12 Lr 2 M p + 4 Lr 2 M r − 9 Lr 2 M p cos( x1 ) 2 )
 =  2
f ( x) = 
x4

  x4 

 f 
2
6 Lp Lr M p Sin( x1 ) x2 + 12 K 2 x4 − 9 Lr M p gCos ( x1 ) Sin( x1 )

  4
−
2
2
2
2
2


12
K
+
12
L
M
+
12
L
M
+
4
L
M
−
9
L
M
Cos
(
x
)
3
r
en
r
p
r
r
r
p
1


(3.44)
0




18 K1 Lr Cos ( x1 )

 0
2
2
2
2
2
 Lp (12 K 3 + 12 Lr M en + 3Lr M p + 4 Lr M r − 9 Lr M p Cos ( x1 ) )   g 2 
g ( x) = 
 =   (3.45)
0

 0

  g 4 
12 K1


2
2
2
2
2
 12 K 3 + 12 Lr M en + 3Lr M p + 4 Lr M − 9 Lr M p Cos ( x1 )

(3.44) và (3.45) góp phần tạo nên hệ phương trình trạng thái hệ thống:
 x = f ( x ) + g ( x ) u
H =
 y = h ( x )
25
CHƯƠNG 4: THIẾT KẾ BỘ ĐIỀU KHIỂN TUYẾN TÍNH HÓA VÀO RA
4.1 Thiết kế bộ điều khiển tuyến tính hóa vào ra trên mô hình con lắc ngược
Hệ con lắc ngược quay là hệ phi tuyến SIMO(Single Input Multi Output) phức tạp
với độ khó điều khiển cao. Ở bài toán này, u(t) chính là tín hiệu điều khiển điện áp e và
c(t) là tín hiệu ngõ ra góc lệch thanh con lắc và góc lệch cánh tay con lắc.
Với một ngõ vào điện áp cấp cho động cơ DC, và hai ngõ ra là góc lệch
của thanh cánh tay và góc lệch thanh con lắc. Vấn đề đặt ra là phải điều khiển cân
bằng cho hệ ở vị trí cân bằng thẳng đứng hướng lên trên.
Việc thiết kế bộ điều khiển tuyến tính hóa vào ra và mô phỏng được thực hiện trên
phần mềm Matlab/Simulink.
So với bài báo nghiên cứu khoa học của tác giả Võ Minh Tài đã thực hiện xây dựng
bộ điều khiển dựa trên hệ phương trình trạng thái phi tuyến thì đề tài này sẽ tiếp thu
hướng nghiên cứu của tác giả Tài và xây dựng bộ điều khiển dựa trên hệ phương trình
trạng thái phi tuyến, nghĩa là sẽ thực hiện việc biến đổi hệ phương trình trạng thái phi
tuyến thành hệ tuyến tính.
 x = f ( x ) + g ( x ) u
H =
 y = h ( x )
Các biến trạng thái được đặt như sau:
 x1 (t ) = 
 x (t ) =   x (t ) = 
 2
2

 x3 (t ) = 
(4.1)
 x (t ) =   x (t ) = 
 4
4
Diễn tả hệ phương trình vi phân dưới dạng
 x1 
   t1 
x 
   
d
d  2  d   t2 
x = f ( x, u , t ) =
=
=
dt
dt  x3  dt     t3 
 
   
   t 4 
 x4 
(4.2)
Trong đó:
26
t1 = x1 = x2

 6 K 3 gSin ( x1 ) − 6 K 2 Lr  Cos ( x1 ) + 6 K1 Lr eCos ( x1 ) + 6 Lr 2 M en gSin ( x1 ) + 

3


 6 L 2 M gSin ( x ) + 2 L M gSin ( x ) − 3L L 2 M x 2Cos ( x ) Sin ( x )

r
p
1
r
r
1
p
r
p
2
1
1


t =  =
2
Lp (12 K 3 + 12 L r 2 M en + 12 Lr 2 M p + 4 Lr 2 M r − 9 Lr 2 M p cos ( x1 ) 2 )


t3 = x3 = x4

6 L L M Sin( x1 ) x2 2 + 12 K 2 x4 − 9 Lr M p gCos ( x1 ) Sin( x1 ) − 12 K1e
t4 =  = − p r p
12 K 3 + 12 Lr 2 M en + 12 Lr 2 M p + 4 Lr 2 M r − 9 Lr 2 M pCos ( x1 ) 2

Trong các hàm từ t1 đến t 4 vẫn còn tồn tại các hàm lượng giác vì thế công việc
của chúng ta là triệt tiêu các hàm đấy đi hay còn gọi là tuyến tính hóa bằng cách khảo
sát phương trình quanh điểm làm việc cân bằng của chúng.
Phương trình tuyến tính hóa của hệ khi làm việc quanh điểm cân bằng có dạng:
 x = Ax ( t ) + Bu ( t )

 y = Cx ( t )
(4.3)
Trong đó:

 t1

 x

 1

 t2


 A( x , u ) =  x1
0
0

 t3



 x1

 t

 4

 x1


 t1 

 u 

 

 t2 

 
 B ( x0 , u0 ) =  u 

 t3 

 u 

 t 

 4

 u 
t1
x2
t1
x3
t2
x2
t2
x3
t3
x2
t3
x3
t4
x2
t4
x3
t1 
x4 

t2 
x4 

t3 

x 4 
t4 

x4 
27
Xét tại các điểm làm việc xác lập của hệ con lắc ngược quay
x0 ( = 0,  = 0,  = 0,  = 0 ) , u0 ( e = 0 ) dựa vào phần mềm Matlab/Simulink tính toán ta
tìm được các ma trận A và B :
0


2
2
2
18 K 3 g + 18Lr M en g + 18Lr M p g + 6 Lr M r g
 L (12K + 12L 2 M + 3L 2 M + 4 L 2 M )
p
3
r
en
r
p
r
r
A ( x0 , u0 ) = 

0

9 Lr M p g

 12 K + 12 L 2 M + 3L 2 M + 4 L 2 M
3
r
en
r
p
r
r



18 K 2 Lr

0 0 −
Lp (12 K 3 + 12 Lr 2 M en + 3Lr 2 M p + 4 Lr 2 M r ) 


0 0
1

12 K 2

0 0
−
12 K 3 + 12 Lr 2 M en + 3Lr 2 M p + 4 Lr 2 M r 
1 0
0
0




18 K1 Lr


 Lp (12 K 3 + 12 Lr 2 M en + 12 Lr 2 M p + 3Lr 2 M p + 4 Lr 2 M r ) 
B ( x0 , u0 ) = 

0




12 K1


2
2
2
12 K 3 + 12 Lr M en + 3Lr M p + 4 Lr M r


(4.4)
Thay (4.4) vào (4.3) ta có được phương trình tuyến tính hóa quanh điểm cân
bằng:
0
1 0
0




2
2
2
18K 2 Lr
 x1  18 K 3 g + 18Lr M en g + 18Lr M p g + 6 Lr M r g 0 0 −

2
2
2
 x   L 12K + 12L 2 M + 3L 2 M + 4 L 2 M

L
(12
K
)
+
12
L
M
+
3
L
M
+
4
L
M
(
)
p
3
r
en
r
p
r
r
3
r
en
r
p
r
r
 2 =  p

 x3  

0
0 0
1
  

x
9 Lr M p g
 4 
12 K 2

0 0
−
2
2
2
 12 K + 12 L 2 M + 3L 2 M + 4 L 2 M

12
K
+
12
L
M
+
3
L
M
+
4
L
M
3
r
en
r
p
r
r
3
r
en
r
p
r
r


0




18K1 Lr


 Lp (12 K 3 + 12 Lr 2 M en + 12 Lr 2 M p + 3Lr 2 M p + 4 Lr 2 M r ) 
+
u
0




12 K1


12 K 3 + 12 Lr 2 M en + 3Lr 2 M p + 4 Lr 2 M r


 x1 
x 
 2
 x3 
 
 x4 
(4.5)
Xét tính ổn định ở vị trí thẳng đứng hướng lên:
28
Với các thông số mô hình ta có được ở chương 3, ta xác định được ma trận A,B
là:
0
1 0
0 

 0 
 77.0473 0 0 -0.1168 
1.2229 



A=
;B = 

 0 
0
0 0
1 




 2.3149 0 0 -0.0779 
0.8152 
Ma trận điều khiển có dạng:
C =  B A2 B A3 B An −1B  , với n là số nghiệm của phương trình trạng thái
rank ( C ) = 4 = n . Vậy hệ thống có thể điều khiển cân bằng ổn định.
Từ (4.5), ta một lần nữa biến đổi hệ phương trình trạng thái tuyến tính về dạng
như sau:
 x = Ax ( t ) + Bu ( t )
 x = F ( x ) + Gu
H :

 y = Cx ( t )
 y = h ( x )
(4.6)
Trong đó:
x2




2
2
2
 6(3K 3 x1 g − 3K 2 Lr x4 + 3Lr M en x1 g + 3Lr M p x1 g + Lr M r x1 g )   x2 

 F 
Lp (12 K 3 + 12 Lr 2 M en + 3Lr 2 M p + 4 Lr 2 M r )
 =  2
F ( x) = 
x

  x4 
4

 F 
9 Lr M p gx1

  4
2
2
2


12 K3 + 12 Lr M en + 3Lr M p + 4 Lr M
0




18 K1 Lr

 0
2
2
2
 L p (12 K 3 + 12 Lr M en + 3Lr M p + 4 Lr M r )  G2 
G=
= 
0

 0

 G4 
12 K1


2
2
2
 12 K 3 + 12 Lr M en + 3Lr M p + 4 Lr M 
Để xây dựng bộ điều khiển tuyến tính hóa vào ra, ngoài xây dựng và tính toán các
hàm F ( x ) , G ( x ) , ta cần phải xác định biến ngõ ra cần điều khiển, đó là hàm h ( x ) , trong
đề tài này thì tác giả quan tâm đến 2 biến là x1 và x3 . RIP là hệ SIMO (một ngõ vào và
29
nhiều ngõ ra), mặt khác thì giải thuật tuyến tính hóa vào ra được xây dựng để áp dụng
lên các hệ có số ngõ vào bằng số ngõ ra. Chính vì vậy chúng ta cần lựa chọn hàm h ( x )
sao cho vừa là hàm bao gồm các biến ngõ ra vừa thỏa mãn yêu cầu về mặt toán học của
giải thuật theo phương trình (2.15)
Tác giả chọn hàm h ( x ) = mx1 + nx3 để thỏa mãn yêu cầu trên, ta sẽ xem như hệ
phương trình của hệ con lắc ngược hệ thống có bậc tương đường bằng 4 và cả 4 biến
quan sát được thỏa Lg h ( x ) = Lg L f h ( x ) = Lg L f ( 2) h ( x ) = 0 và Lg L f (3) h ( x )  0
Từ hệ phương trình trạng thái tuyến tính
 x = F ( x ) + Gu
H :
 y = h ( x )
Ta đặt các biến trạng thái theo phương pháp tuyến tính hóa vào ra như sau
 z1 = y = h ( x )

 z = y = h ( x ) x = h ( x ) f x + g x u = L h x + L h x u
( ( ) ( ) ) f ( ) g ( )
 2
x
x

(4.7)
 ( Lf h ( x ))
 ( Lf h ( x ))

2
z
=
y
=
x
=
f
x
+
g
x
u
=
L
h
x
+
L
L
h
x
u
(
)
(
)
(
)
(
)
(
)
 3
f
g f
x
x


 ( L f 2h ( x ))
 ( L f 2h ( x ))
 z4 = y =
x=
f ( x ) + g ( x ) u ) = L f 3h ( x ) + Lg L f 2 h ( x ) u
(
x
x

Thỏa mãn điều kiện (2.15), các biến trạng thái mới sẽ tiếp tục tương đương với
hệ:
 z1 = y = h ( x )

 z2 = y = L f h ( x )

2
 z3 = y = L f h ( x )

3
 z4 = y = L f h ( x )
Hệ phương trình trạng thái mới của hệ thống có dạng:
 z = AC z + BC  ( x ) u −  ( x ) 
H'=
 y = CC z
(4.8)
(4.9)
Tiếp tục biễu diễn hệ phương trình trạng thái của hệ thống:
30
 z1 = L f h ( x ) = z2

2
 z 2 = L f h ( x ) = z3

3
 z3 = L f h ( x ) = z4

4
3
 z4 = L f h ( x ) + Lg L f h ( x ) u
(4.10)
Kết hợp (4.9) vào (4.10) ta được hệ thống mới H’ tuyến tính:
 z1  0
 z  0
 2 = 
 z3   0
  
 z4   0
Trong đó:
 ( x) = −
1 0 0   z1  0 

 L f 4h ( x )  
0 1 0   z2  0 
3
+
Lg L f h ( x )  u −  −

 L L 3 h ( x )  

0 0 1   z3   0 
 g f


   
0 0 0   z4  1 
(4.11)
L f 4h ( x )
Lg L f 3h ( x )
 ( x ) = Lg L f 3h ( x )
Luật điều khiển được xác định:
u =  ( x) +  ( x) v = −
−1
Lf 4h ( x )
Lg L f 3h ( x )
+
v − L f 4h ( x )
v
=
Lg L f 3h ( x ) Lg L f 3h ( x )
(4.12)
Thay (4.12) vào (4.11), ta rút ra được hệ phương trình trạng thái với tín hiệu điều
khiển giả v
 z = AC z + BC v
H'=
 y = CC z
(4.13)
Trong đó:
0
0
AC = 
0

0
CC = 1
1 0 0
0 

0 
0 1 0
; BC  
0 
0 0 1

 
0 0 0
1 
0 0 0
4.2 Áp dụng giải thuật LQR rời rạc dựa trên phương pháp tuyến tính hóa vào ra
LQR là thuật toán điều khiển được xây dựng dựa trên cơ sở nguyên lý phản hồi
trạng thái, còn gọi là phương pháp tuyến tính hóa dạng toàn phương. Bộ điều khiển
31
LQR thường được áp dụng trên các hệ phi tuyến với nhiều ngõ vào ra. Bộ điều khiển
nhận tín hiệu vào là trạng thái của hệ thống và tín hiệu mẫu sau đó tính toán và chuyển
thành tín hiệu điều khiển cho hệ thống.
Một hệ điều khiển được thiết kế ở chế độ làm việc tốt nhất là hệ luôn ở trạng
thái tối ưu theo một tiêu chuẩn chất lượng nào đó (đạt giá trị cực trị). Trạng thái tối
ưu có đạt được hay không tùy thuộc vào yêu cầu chất lượng đặt ra, sự hiểu biết về đối
tượng và các tác động lên đối tượng, điều kiện làm việc của hệ,…
r
+
x = Ax + Bu
y = Cx + Du
−
x
State – Space
Kx
State Feedback Gain
Hình 4.1 Sơ đồ khối giải thuật LQR
Xét hệ thống có tác động bên ngoài với tín hiệu u khác 0, đối tượng tuyến tính được
mô tả bởi phương trình trạng thái như sau, hệ thống chúng ta không tồn tại ma trận D:
 x(t ) = Ax(t ) + Bu (t )

 y = Cx(t )
Chúng ta cần tìm ma trận K của vector điều khiển tối ưu: u(t) = -Kx(t) thỏa mãn chỉ
tiêu chất lượng dạng toàn phương trong đó thời điểm cuối tf = ∞
J (u ) =
1 
[ xT (t )Qx(t ) + uT (t ) Ru (t )]dt
2 0
Trong đó Q là ma trận xác định dương(hoặc bán xác định dương), R là ma trận
xác định dương
Ma trận K tối ưu được xác định từ phương trình Ricatti có dạng:
K = R −1 BT P
Và P là nghiệm bán xác định dương cảu phương trình đại số Ricatti:
PA + AT P + Q − PBR −1 BT P = 0
Với luật điều khiển u =
v − L f 3h ( x )
Lg L f 3h ( x )
và hệ phương trình trạng thái với tín hiệu điều
 z = AC z + BC v
 y = CC z
khiển giả định v , H ' = 
32
Ở đây, tác giả ý định sử dụng trận tối ưu K của giải thuật LQR lên hệ thống H’ có
tín hiệu giả định v vừa tìm được. Lúc này tín hiệu điều khiển giải định:
v = − K1 z1 − K 2 z2 − K 3 z3 − K 4 z4
(4.14)
Ma trận tối ưu K =  K1 K 2 K3 K 4  được xác định dùng giải thuật LQR rời rạc (thời
gian lấy mẫu là 0.01 giây) với các ma trận sau khi rời rạc hóa với thời gian lấy mẫu là
0.001s thì ta có:
0 
1 0.01 0.0001
0
1
0.01 0.0001
Ad = 
; Bd
0
0
1
0.01 


0
0
1 
0
 0 
 0 

 và ma trận Q, R hiệu chỉnh
0.0001


 0.01 
Lúc này, ta có thể viết hệ phương trình trạng thái về dạng:
z = ( Ad − Bd K ) z
Lf 4h ( x )
− L f 4 h ( x ) − Kz
Kz
 u =  ( x ) −  ( x ) Kz = −
−
=
Lg L f 3h ( x ) Lg L f 3h ( x )
Lg L f 3h ( x )
(4.15)
(4.16)
Hình 4.2 Sơ đồ bộ điều khiển tuyến tính hóa vào ra có kết hợp giải thuật LQR
33
CHƯƠNG 5: THI CÔNG HỆ THỐNG
5.1 Thiết kế hệ thống phần cứng
5.1.1 Thiết kế hệ thống cơ khí
Hình 5.1 Mô hình con lắc ngược thực tế
Trong đó:
1. Thanh con lắc
2. Encoder thanh con lắc
3. Động cơ
4. Encoder thanh cánh tay
5. Module driver cầu H L298N
6. Nguồn xung 5VDC – 2.5A
7. Board Arduino mege 2560
34
Hình 5.2 Mô hình bộ điều khiển
5.1.2 Thiết kế hệ thống điện tử
Hệ thống điện tử sẽ có vai trò đảm nhận việc thu thập dữ liệu ghi nhận được của
các encoder, vi điều khiển sẽ tiếp nhận các dữ liệu này và thực hiện tính toán giải thuật
và xuất ra tín hiệu điều khiển để điều khiển động cơ thông qua driver công suất, sơ đồ
kết nối hệ thống điện tử được trình bày như sau:
5.1.2.1 Lựa chọn vi điều khiển
Mạch Arduino Mega 2560 là một board vi điều khiển sử dụng vi xử lý
ATmega2560. Board này có 54 chân I/O (14 chân PWM ), 16 analog đầu hàng vào, 4
UARTs,sử dụng thạch anh 16 MHz, kết nối cổng USB, một Jack cắm điện, chân ICSP.
Board có tất cả mọi thứ cần thiết để hỗ trợ vi điều khiển.
35
Hình 5.3 Vi mạch điều khiển Arduino Mega 2560
Lý do dùng Arduino Mega là vài đây phiên bản cao cấp của Arduino Uno R3 và
Arduino Nano với số chân giao tiếp, ngoại vi và bộ nhớ nhiều hơn, mạch được thiết kế
và sử dụng trên IDE Arduino.Phù hợp cho các ứng dụng trong việc học tập. Arduino có
cộng đồng hỗ trợ lớn giúp sinh viên thận tiện cho việc tìm hiểu. ngoài ra arduino còn hỗ
trợ nhúng matlab vào bô vi điều khiển giúp cho việc lập trình dễ dàng hơn.
Arduino Mega2560 khác với tất cả các vi xử lý trước giờ vì không sử dụng FTDI
chip điều khiển chuyển tín hiệu từ USB để xử lý. Thay vào đó, nó sử dụng ATmega16U2
lập trình như là một công cụ chuyển đổi tín hiệu từ USB. Ngoài ra, Arduino
Mega2560 cơ bản vẫn giống Arduino Uno R3, chỉ khác số lượng chân và nhiều tính
năng mạnh mẽ hơn, nên các bạn vẫn có thể lập trình cho con vi điều khiển này bằng
chương trình lập trình cho Arduino Uno R3.
Arduino mege 2560
Thông số
Điện áp hoạt động
5V
Tần số hoạt động
16 MHz
Bộ nhớ Flash
128 KB
Bộ nhớ SRAM
8 KB
Bộ đếm Timer
6 Timer
Số chân ngõ vào Analog (ADC)
16 kênh
Số chân ngõ ra (Digital)
54 (15 chân có thể được dùng làm PWM)
Cổng nối tiếp phần cứng
4
(UARTs)
Bảng 5.1 Thông số cơ bản của Arduino Mega2560
36
Hình 5.4 Sơ lược các chân tín hiệu của Arduino Mega2560
5.1.3 Lựa chọn động cơ điều khiển
Động cơ là cơ cấu chấp hành chính trong hệ thống, để giữ cho hệ con lắc được cân
bằng, động cơ sẽ được gắn trực tiếp vào thanh cánh tay theo phương thẳng đứng. Một
encoder sẽ có nhiệm vụ đọc xung trả về bộ điều khiển giúp động cơ làm việc đúng theo
yêu cầu.
Yêu cầu lựa chọn động cơ:
• Momen lực động cơ phải đủ mạnh để tải thanh cánh tay và thanh con lắc
• Điều khiển ở chế độ xung PWM
• Động cơ có tích hợp với encoder
37
• Làm việc với điện áp tối thiểu là 5V
Với các tiêu chí trên, tác giả làm luận văn đã lựa chọn động cơ Nisca NF5475E.
Đậy là loại động cơ phổ biến, giá thành rẻ và bán tại khu vực. Động cơ có dãy điện áp
làm viễc lớn , momen xoắn lớn. Do là động cơ DC nên việc điều khiển điện áp của động
cơ cũng rất dễ dàng. Trên hết là thời gian đáp ứng của động cơ rất nhanh, tốt hơn so với
động cơ có bô giảm tốc cùng loại
Hình 5.5 Động cơ DC kèm Encoder Nisca NF5475E
Trên hệ thống thực của tác giả, vì Encoder của động cơ bị hư nên sinh viên thực
hiện thay thế bằng một Encoder khác có số xung là 200 pulse/cycle. Các thông số kĩ
thuật của cảm biến Encoder được đồng trục với động cơ Servo:
Model
VCC
Ecoder
5v-Đỏ
Xanh
Nâu
Vàng
Bảng 5.2 Chân của encoder trong động cơ
Model
Động cơ
GND
KÊNH A
KÊNH B
VCC
Momen lực
Tốc độ
Dòng điện
5v – Đỏ
74.8 mN*m
4162 rpm
1.256 A
Bảng 5.3 Các thông số cơ bản của động cơ do NSX quy định
ĐỘ PHÂN
GIẢI
200
Công suất
32 W
5.1.4 Lựa chọn driver cầu H L298N
Mạch điều khiển động cơ DC L298N có khả năng điều khiển 2 động cơ DC, dòng
tối đa 2A mỗi động cơ, mạch tích hợp diod bảo vệ và IC nguồn 7805 giúp cấp nguồn
5VDC cho các module khác (chỉ sử dụng 5V này nếu nguồn cấp <12VDC).
38
Mạch điều khiển động cơ DC L298N dễ sử dụng, chi phí thấp, dễ lắp đặt, là sự lựa
chọn tối ưu trong tầm giá.
Hình 5.6 Cầu H L298N
Điện áp đầu vào
Công suất tối đa
Dòng tối đa cho mỗi cầu H
Mức điện áp logic
Kích thước
5~30VDC
25W
2A
Thấp - 0.3V~1.5V, Cao: 2.3V~Vss
43x43x27mm
Bảng 5.4 Thông số kỹ thuật của cầu H L298N
•
•
•
•
Chân 12V, 5V và chân gnd để cấp nguồn cho động cơ.
Output A và Output B để cấp nguồn cho động cơ
A enable và B enable đùng để điều xung PWM cho ngõ ra cầu H
4 chân Input IN1, IN2, IN3, IN4 dùng để điều hướng cho động cơ
5.1.5 Lựa chọn nguồn điện cung cấp cho động cơ
Tác giả lựa chọn nguổn tổ ong 5V-5A, sỡ dĩ tác giả lựa chọn nguồn bé bởi vì để
cân bằng con lắc ngược quanh vị trí ổn định của nó thì nguồn 5V là đủ thỏa mãn yêu cầu
đặt ra, nếu tác giả có sử dụng giải thuật Swing-Up thì sẽ chọn nguồn 12V phù hợp với
công việc đề ra.
39
Hình 5.7 Nguồn cung cấp cho động cơ
5.1.6 Lựa chọn encoder
Encoder gắn trên con lắc cần có số xung/vòng đủ lớn để độ phân giải của góc quay
thanh con lắc là tốt nhất. Vị trí chính xác của thanh con lắc được hồi tiếp tốt góp phần
rất lớn cho việc điều khiển thành công con lắc ngược.
Tác giả thực hiện đã chọn encoder 200 xung để đọc góc quay của động cơ và
encoder 400 xung để đọc góc quay góc con lắc. Sau đó tác giả dùng một giải thuật đọc
xung encoder để tăng độ phân giải lên gấp 4 lần thực có, cụ thể đối với encoder đọc góc
cánh tau là 800 xung (tương đương với 0.00785radian/xung), đối với encoder đọc góc
con lắc là 1600 xung (tương đương với 0.003925radian/xung). Đối với tác giả thì độ
phân giải này là đủ để thõa mãn yêu cầu điều khiển con lắc ngược, chưa kể nếu độ phân
giải càng mịn hơn nữa thì vi điều khiển Arduino rất có khả năng đọc luột mất xung.
40
Hình 5.8 Encoder thực tế
Bên trong cảm biến Encoder là một Opto quang. Encoder có 4 dây, bao gồm: 2 dây
kênh (A,B), dây Vcc, dây GND. Thông số kĩ thuật của cảm biến Enocoder đọc giá trị từ
thanh con lắc.
Model
Ecoder
VCC
GND
KÊNH A
KÊNH B
5v – Đỏ
Đen
Trắng
Lục
Bảng 5.5 Thông số kỹ thuật của Encoder
RESOLUTION
400-200
Bởi vì encoder mà trong luận văn tác giả chọn có 2 kênh tín hiệu, ta có thể áp dụng
phương pháp đọc cả 2 kênh cùng lúc và nhờ vào phương pháp này, ta có thể cho ra kết
quả số xung tín hiệu trên một vòng gấp 4 lần thông thường.
Hình 5.9 Mô tả tín hiệu 2 kênh của Encoder
41
5.1.7 Sơ đồ kết nối
5.2 Phần mềm
5.2.1 Giới thiệu phần mềm Arduino
Arduino IDE là một chương trình phần mềm mã nguồn mở cho phép người dùng
viết và tải lên mã trong một môi trường làm việc thời gian thực. Vì mã này sau đó sẽ
được lưu trữ trong đám mây, nó thường được sử dụng bởi những người đã tìm kiếm thêm
một mức dư thừa. Hệ thống này hoàn toàn tương thích với bất kỳ bo mạch phần mềm
Arduino nào.
Arduino IDE sử dụng ngôn ngữ lập trình C/C++ rất phổ biến trong giới lập trình.
Bất kỳ đoạn code nào của C/C++ thì Arduino IDE đều có thể nhận dạng, giúp các lập
trình viên thuận tiện trong việc thiết kế chương trình lập cho các bo mạch Arduino
42
Hình 5.10 Giao diện phần mềm Arduino IDE
5.2.2 Giới thiệu phần mềm Terminal
Trong bài luận văn này, tác giả sử dụng phần mềm Terminal để thu thập dữ
liệu từ Arduino đọc về để vẽ biểu đồ và nghiên cứu kết quả hiệu suất của bộ điều
khiển.
Hình 5.11 Giao diện phần mềm Terminal
43
5.3 Lưu đồ giải thuật
Chương trình trong vi điều khiển arduino được lập trình bằng ngôn ngữ C và được
hỗ trợ từ những hàm được viết, các thư viện có sẵn của arduino
Chương trình do dinh viên lập trình trong arduino được trình bày theo lưu đồ giải
thuật bên dưới:
Hình 5.12 Lưu đồ giải thuật điều khiển tuyến tính hóa vào ra con lắc ngược quay
Khối set up:khai báo biến ,khai báo timer khai báo ngắt ngoài , khai báo các hằng
số giá trị.
Khối kiểm tra ngắt encoder: khiển tra sự thay đổi của tín hiệu từ 2 kênh A và B
của cả 2 encoder bằng các chân ngắt ngoài để đọc xong và tính toán gõc.
Khối kiểm tra ngắt của timer2: ngắt khi đạt 1ms để tính toán vận tốc
Khối kiểm tra ngắt của timer1: ngắt khi đạt 10ms để thực hiện tính toán giải thuật
tuyến tính hóa vào ra và cấp xung PWM để điều khiển động cơ
44
CHƯƠNG 6: KẾT QUẢ THỰC HIỆN
6.1 Kết quả mô phỏng
Trong phần này, sinh viên thực hiện thay đổi lần lượt các giá trị Q11 Q22 Q33 Q44
trong ma trận Q và ma trận R để phân tích sự tác động vào hệ thống khi điều khiển cân
bằng hệ con lắc ngược quay bằng giải thuật điều khiển tối ưu toàn phương tuyến tính.
Sinh viên tiến hành chọn lựa ma trận Q có khả năng ổn định rất tốt làm giá trị mẫu
để khảo sát cho các trường hợp. Từ đó rút ra các kết luận về mục tiêu mà mục này muốn
hướng tới.
Hình 6.1 Khối mô phỏng bộ điều khiển hệ thống con lắc ngược
Bên trong khối “Mô hình hệ con lắc ngược quay” là các phương trình trạng thái mô
phỏng hệ thống.
Bên trong khối “Bộ điều khiển tuyến tính hóa vào ra” là các hàm công thức tính
toán.
45
Hình 6.2 Khối mô phỏng mô tả hệ thống con lắc ngược
Hình 6.3 Khối mô phỏng bộ điều khiển
46
6.1.1 Khảo sát hàm h(x) = x1
Các giá trị ban đầu trong quá trình mô phỏng như sau:
 = −0.1( rad ) ;  = 0 ( rad / s ) ;  = 0 ( rad ) ;  = 0 ( rad / s )
6.1.1.1 Hiệu chỉnh ma trận Q và R lần thứ nhất
103

0
Chọn R = 0.00001 và Q = 
 0

 0
0
0
1
0
0 100
0
0
0

0
0

1
Giá trị ma trận K =  2805.2 1822.3 590.5 95.053
Hình 6.4 Kết quả mô phỏng đáp ứng góc cánh tay và con lắc khi h(x) = x1, khi hiệu chỉnh ma
trận Q và R lần thứ nhất
Hình 6.5 Kết quả mô phỏng đáp ứng vận tốc góc con lắc và góc cánh tay khi h(x) = x1, khi
hiệu chỉnh ma trận Q và R lần thứ nhất
47
6.1.1.2 Hiệu chỉnh ma trận Q và R lần thứ hai
104

0
Chọn R = 0.00001 và Q = 
 0

 0
0
0
1
0
0 101
0
0
0

0
0

1
Giá trị ma trận K = 8716.8 3987.9 911.79 96.895
Hình 6.6 Kết quả mô phỏng đáp ứng góc cánh tay và góc con lắc khi h(x) = x1 sau khi hiệu
chỉnh ma trận Q và R lần thứ hai
Hình 6.7 Kết quả mô phỏng đáp ứng vận tốc góc con lắc và góc cánh tay khi h(x) = x1, sau
khi hiệu chỉnh ma trận Q và R lần thứ hai
48
6.1.1.3 Hiệu chỉnh ma trận Q và R lần thứ ba
103

0
Chọn R = 0.0000001 và Q = 
 0

 0
0
0
1
0
0 103
0
0
0

0
0

1
Giá trị ma trận K =  2708.6 3943.4 2869.2 114.21
Hình 6.8 Kết quả mô phỏng đáp ứng góc cánh tay và góc con lắc khi h(x) = x1 sau khi hiệu
chỉnh ma trận Q và R lần thứ ba
Hình 6.9 Kết quả mô phỏng đáp ứng vận tốc góc con lắc và góc cánh tay khi h(x) = x1, sau
khi hiệu chỉnh ma trận Q và R lần thứ ba
49
6.1.1.4 Hiệu chỉnh ma trận Q và R lần thứ tư
104

0
Chọn R = 0.0000001 và Q = 
 0

 0
0
0
1
0
0 103
0
0
0

0
0

1
Giá trị ma trận K = 8518.6 7124.6 2978.9 114.7
Hình 6.10 Kết quả mô phỏng đáp ứng góc cánh tay và góc con lắc khi h(x) = x1 sau khi hiệu
chỉnh ma trận Q và R lần thứ tư
Hình 6.11 Kết quả mô phỏng đáp ứng vận tốc góc con lắc và góc cánh tay khi h(x) = x1, sau
khi hiệu chỉnh ma trận Q và R lần thứ tư
50
6.1.2 Khảo sát hàm y = h(x) = 3x1 – 3x3
Các giá trị ban đầu trong quá trình mô phỏng như sau:
 = −0.1( rad ) ;  = 0 ( rad / s ) ;  = 0 ( rad ) ;  = 0 ( rad / s )
6.1.2.1 Hiệu chỉnh ma trận Q và R lần thứ nhất
103

0
Chọn R = 0.00001 và Q = 
 0

 0
0
0
1
0
0 100
0
0
0

0
0

1
Giá trị ma trận K =  2805.2 1822.3 590.5 95.053
Hình 6.12 Kết quả mô phỏng đáp ứng góc cánh tay và góc con lắc khi h(x) = 3x1 -3x3 khi hiệu
chỉnh ma trận Q và R lần thứ nhất
Hình 6.13 Kết quả mô phỏng đáp ứng vận tốc góc cánh tay và vận tốc góc con lắc khi h(x) =
3x1 -3x3 khi hiệu chỉnh ma trận Q và R lần thứ nhất
51
6.1.2.2 Hiệu chỉnh ma trận Q và R lần thứ hai
104

0
Chọn R =0.00001 và Q = 
 0

 0
0
0
1
0
0 101
0
0
0

0
0

1
Giá trị ma trận K = 8716.8 3987.9 911.79 96.895
Hình 6.14 Kết quả mô phỏng đáp ứng góc cánh tay và góc con lắc khi h(x) = 3x1 -3x3 khi hiệu
chỉnh ma trận Q và R lần thứ hai
Hình 6.15 Kết quả mô phỏng đáp ứng vận tốc góc cánh tay và vận tốc góc con lắc khi h(x) =
3x1 -3x3 khi hiệu chỉnh ma trận Q và R lần thứ
52
6.1.2.3 Hiệu chỉnh ma trận Q và R lần thứ ba
103

0
Chọn R = 0.0000001 và Q = 
 0

 0
0
0
1
0
0 103
0
0
0

0
0

1
Giá trị ma trận K =  2708.6 3943.4 2869.2 114.21
Hình 6.16 Kết quả mô phỏng đáp ứng góc cánh tay và góc con lắc khi h(x) = 3x1 -3x3 khi hiệu
chỉnh ma trận Q và R lần thứ ba
Hình 6.17 Kết quả mô phỏng đáp ứng vận tốc góc cánh tay và vận tốc góc con lắc khi h(x) =
3x1 -3x3 khi hiệu chỉnh ma trận Q và R lần thứ ba
53
6.1.2.4 Hiệu chỉnh ma trận Q và R lần thứ tư
104

0
Chọn R = 0.0000001 và Q = 
 0

 0
0
0
1
0
0 103
0
0
Giá trị ma trận K = 8518.6 7124.6 2978.9 114.7
0

0
0

1
Hình 6.18 Kết quả mô phỏng đáp ứng góc cánh tay và góc con lắc khi h(x) = 3x1 -3x3 khi hiệu
chỉnh ma trận Q và R lần thứ tư
Hình 6.19 Kết quả mô phỏng đáp ứng vận tốc góc cánh tay và vận tốc góc con lắc khi h(x) =
3x1 -3x3 khi hiệu chỉnh ma trận Q và R lần thứ tư
54
6.1.3 Khảo sát hàm y = h(x) = 3x1 - 5x3
Các giá trị ban đầu trong quá trình mô phỏng như sau:
 = −0.1( rad ) ;  = 0 ( rad / s ) ;  = 0 ( rad ) ;  = 0 ( rad / s )
6.1.3.1 Hiệu chỉnh ma trận Q và R lần thứ nhất
103

0
Chọn R = 0.00001 và Q = 
 0

 0
0
0
1
0
0 100
0
0
0

0
0

1
Giá trị ma trận K =  2805.2 1822.3 590.5 95.053
Hình 6.20 Kết quả mô phỏng đáp ứng góc cánh tay và góc con lắc khi h(x) = 3x1 - 5x3 khi
hiệu chỉnh ma trận Q và R lần thứ nhất
Hình 6.21 Kết quả mô phỏng đáp ứng vận tốc góc cánh tay và vận tốc góc con lắc khi h(x) =
3x1 - 5x3 hiệu chỉnh ma trận Q và R lần thứ nhất
55
6.1.3.2 Hiệu chỉnh ma trận Q và R lần thứ hai
104

0
Chọn R =0.00001 và Q = 
 0

 0
0
0
1
0
0 101
0
0
0

0
0

1
Giá trị ma trận K = 8716.8 3987.9 911.79 96.895
Hình 6.22 Kết quả mô phỏng đáp ứng góc cánh tay và góc con lắc khi h(x) = 3x1 - 5x3 hiệu
chỉnh ma trận Q và R lần thứ hai
Hình 6.23 Kết quả mô phỏng đáp ứng vận tốc góc cánh tay và vận tốc góc con lắc khi h(x) =
3x1 - 5x3 hiệu chỉnh ma trận Q và R lần thứ hai
56
6.1.3.3 Hiệu chỉnh ma trận Q và R lần thứ ba
103

0
Chọn R = 0.0000001 và Q = 
 0

 0
0
0
1
0
0 103
0
0
0

0
0

1
Giá trị ma trận K =  2708.6 3943.4 2869.2 114.21
Hình 6.24 Kết quả mô phỏng đáp ứng góc cánh tay và góc con lắc khi h(x) = 3x1 - 5x3 khi
hiệu chỉnh ma trận Q và R lần thứ ba
Hình 6.25 Kết quả mô phỏng đáp ứng vận tốc góc cánh tay và vận tốc góc con lắc khi h(x) =
3x1 - 5x3 khi hiệu chỉnh ma trận Q và R lần thứ ba
57
6.1.3.4 Hiệu chỉnh ma trận Q và R lần thứ tư
104

0
Chọn R = 0.0000001 và Q = 
 0

 0
0
0
1
0
0 103
0
0
0

0
0

1
Giá trị ma trận K = 8518.6 7124.6 2978.9 114.7
Hình 6.26 Kết quả mô phỏng đáp ứng góc cánh tay và góc con lắc khi h(x) = 3x1 - 5x3 khi
hiệu chỉnh ma trận Q và R lần thứ tư
Hình 6.27 Kết quả mô phỏng đáp ứng vận tốc góc cánh tay và vận tốc góc con lắc 3x1 - 5x3
khi hiệu chỉnh ma trận Q và R lần thứ tư
58
6.1.4 Khảo sát hàm y = h(x) = 3x1 – 7x3
Các giá trị ban đầu trong quá trình mô phỏng như sau:
 = −0.1( rad ) ;  = 0 ( rad / s ) ;  = 0 ( rad ) ;  = 0 ( rad / s )
6.1.4.1 Hiệu chỉnh ma trận Q và R lần thứ nhất
103

0
Chọn R = 0.00001 và Q = 
 0

 0
0
0
1
0
0 100
0
0
0

0
0

1
Giá trị ma trận K =  2805.2 1822.3 590.5 95.053
Hình 6.28 Kết quả mô phỏng đáp ứng góc cánh tay và góc con lắc khi h(x) = 3x1 - 7x3 hiệu
chỉnh ma trận Q,R lần thứ nhất
Hình 6.29 Kết quả mô phỏng đáp ứng vận tốc góc cánh tay và vận tốc góc con lắc khi h(x) =
3x1 - 7x3 hiệu chỉnh ma trận Q và R lần thứ nhất
59
6.1.4.2 Hiệu chỉnh ma trận Q và R lần thứ hai
103

0
Chọn R =0.00001 và Q = 
 0

 0
0
0
1
0
0 101
0
0
0

0
0

1
Giá trị ma trận K =  2793.8 1927.9 663.8 95.482
Hình 6.30 Kết quả mô phỏng đáp ứng góc cánh tay và góc con lắc khi h(x) = 3x1 - 7x3 hiệu
chỉnh ma trận Q và R lần thứ hai
Hình 6.31 Kết quả mô phỏng đáp ứng vận tốc góc cánh tay và góc con lắc khi h(x) = 3x1 7x3 hiệu chỉnh ma trận Q và R lần thứ hai
60
6.1.4.3 Hiệu chỉnh ma trận Q và R lần thứ ba
103

0
Chọn R = 0.0000001 và Q = 
 0

 0
0
0
1
0
0 103
0
0
0

0
0

1
Giá trị ma trận K =  2708.6 3943.4 2869.2 114.21
Hình 6.32 Kết quả mô phỏng đáp ứng góc cánh tay và góc con lắc khi h(x) = 3x1 - 7x3 hiệu
chỉnh ma trận Q và R lần thứ ba
Hình 6.33 Kết quả mô phỏng đáp ứng vận tốc góc cánh tay và góc con lắc khi h(x) = 3x1 7x3 hiệu chỉnh ma trận Q và R lần thứ ba
61
6.1.4.4 Hiệu chỉnh ma trận Q và R lần thứ tư
103

0
Chọn R = 0.0000001 và Q = 
 0

 0
0
0
1
0
0 10 2
0
0
0

0
0

1
Giá trị ma trận K =  2971.3 2673.5 1201.3 105.87 
Hình 6.34 Kết quả mô phỏng đáp ứng góc cánh tay và góc con lắc khi h(x) = 3x1 - 7x3 hiệu
chỉnh ma trận Q và R lần thứ tư
Hình 6.35 Kết quả mô phỏng đáp ứng vận tốc góc cánh tay và góc con lắc khi h(x) = 3x1 7x3 hiệu chỉnh ma trận Q và R lần thứ tư
62
Nhận xét: Thông qua các kết quả mô phỏng khảo sát các hàm h(x) với các thông
số m,n khác nhau và việc hiệu chỉnh ma trận Q,R tác giả có nhận xét như sau:
Đầu tiên, bộ điều khiển mà tác giả áp dụng thực tế chỉ có hiệu quả rất tốt đối với
các hệ SISO. Nhưng trong trường hợp mà tác giả đang khảo sát là hệ SIMO nên kết quả
đáp ứng sẽ không thỏa mãn yêu cầu của tác giả. Dựa trên kết quả mô phỏng ta dễ dàng
nhìn ra bộ điều khiển ưu tiên ổn định thanh con lắc rồi sau đó mới đến thanh cánh tay,
hay nói cách khác là hệ thống bỏ qua việc ổn định thanh cánh tay để tập trung điều khiển
con lắc.
Thứ hai, việc chọn thông số m,n cho hàm khảo sát h(x) và sự hiện diện của cả hai
biến trạng thái x1 và x3 có tác động rất lớn đến đáp ứng cân bằng của hệ thống. Dựa trên
kết quả mô phỏng, đáp ứng hệ thống khi khảo sát hàm h(x) = x1 (tác giả chọn khảo sát
hàm h(x) = x1 và các hàm khác, sau đó so sánh các kết quả với nhau để cho thấy sự cải
tiến trong bài báo [20]) tuy chưa đạt đến kết quả tác giả mong muốn nhưng rõ ràng cho
thấy bộ điều khiển luôn ưu tiên cân bằng con lắc và có sự bất ổn định của thanh cánh
tay. Sau đó tác giả cho khảo sát hàm h(x) = 3x1 - 3x3, h(x) = 3x1 – 5x3, h(x) = 3x3 – 7
x3 có sự hiện diện của 2 biến trạng thái mà hệ thống khảo sát rõ ràng đã ảnh hưởng tích
cực rất lớn đối với đáp ứng hệ thống, cụ thể là con lắc và cánh tay được cân bằng ổn
định. Trong hệ thống mà tác giả khảo sát ta nên chọn hệ số m dương và n âm, thông qua
kết quả mô phỏng, ta đã thấy sự cải thiện hơn về đáp ứng của hệ thống khi thanh con lắc
và thanh cánh tay đều được ổn định so với bài nghiên cứu [20]
Thứ ba, việc chọn thông số cho ma trận Q,R ảnh hưởng rất lớn đến đáp ứng của bộ
điều khiển, trong các trường hợp mô phỏng. Tác giả lựa chọn giảm ma trận R rất bé cụ
thể là 10−4 và 10−6 giúp cải thiện thời gian đáp ứng của hệ thống. Đối với ma trận Q, tác
giả thay đổi chủ yếu hai thông số là Q11 và Q33 , vì hai thông số này ảnh hưởng trực tiếp
đến đáp ứng lần lượt là góc con lắc và góc cánh tay. Nhận thấy rằng nên đặt hai thông
số này có giá trị lớn, cụ thể từ 102 trở lên, thông số Q11 nên bé hơn hoặc bằng Q33 để hệ
thống có thể đáp ứng cân bằng thanh con lắc.
6.2 Kết quả thực nghiệm
Ở trong tất cả các kết quả khảo sát thực nghiệm, bởi vì vi điều khiển mà tác giả sử
dụng chỉ có thể đáp ứng thu thập dữ liệu với thời gian lấy mẫu là 0.00667 giây một lần.
Cho nên đối với hàm h(x) = x1, tác giả thu thập 1500 mẫu dữ liệu, tương ứng với 10 giây
khảo sát, các hàm còn lại tác giả thu thập 3000 mẫu, tương ứng với 20 giây khảo sát
63
6.2.1 Khảo sát hàm h(x) = x1
6.2.1.1 Hiệu chỉnh thông số ma trận lần thứ nhất
103

0
Chọn R = 0.00001 và Q = 
 0

 0
0
0
1
0
0 100
0
0
0

0
0

1
Hình 6.36 Kết quả thực nghiệm đáp ứng góc con lắc khảo sát hàm h(x) = x1 khi hiệu chỉnh
ma trận Q,R lần thứ nhất
Hình 6.37 Kết quả thực nghiệm đáp ứng góc cánh tay khảo sát hàm h(x) = x1 khi hiệu chỉnh
ma trận Q,R lần thứ nhất
64
Hình 6.38 Kết quả thực nghiệm đáp ứng vận tốc góc con lắc khảo sát hàm h(x) = x1 khi hiệu
chỉnh ma trận Q,R lần thứ nhất
Hình 6.39 Kết quả thực nghiệm đáp ứng vận tốc góc cánh tay khảo sát hàm h(x) = x1 khi hiệu
chỉnh ma trận Q,R lần thứ nhất
6.2.1.2 Hiệu chỉnh thông số ma trận lần thứ hai
104

0
Chọn R = 0.00001 và Q = 
 0

 0
0
0
1
0
0 101
0
0
0

0
0

1
65
Hình 6.40 Kết quả thực nghiệm đáp ứng góc con lắc khảo sát hàm h(x) = x1 khi hiệu chỉnh
ma trận Q,R lần thứ hai
Hình 6.41 Kết quả thực nghiệm đáp ứng góc cánh tay khảo sát hàm h(x) = x1 khi hiệu chỉnh
ma trận Q,R lần thứ hai
66
Hình 6.42 Kết quả thực nghiệm đáp ứng vận tốc góc con lắc khảo sát hàm h(x) = x1 khi hiệu
chỉnh ma trận Q,R lần thứ hai
Hình 6.43 Kết quả thực nghiệm đáp ứng vận tốc góc cánh tay khảo sát hàm h(x) = x1 khi hiệu
chỉnh ma trận Q,R lần thứ hai
6.2.1.3 Hiệu chỉnh thông số ma trận lần thứ ba
103

0
Chọn R = 0.0000001 và Q = 
 0

 0
0
0
1
0
0 103
0
0
0

0
0

1
67
Hình 6.44 Kết quả thực nghiệm đáp ứng góc con lắc khảo sát hàm h(x) = x1 khi hiệu chỉnh
ma trận Q,R lần thứ ba
Hình 6.45 Kết quả thực nghiệm đáp ứng góc cánh tay khảo sát hàm h(x) = x1 khi hiệu chỉnh
ma trận Q,R lần thứ ba
68
Hình 6.46 Kết quả thực nghiệm đáp ứng vận tốc góc con lắc khảo sát hàm h(x) = x1 khi hiệu
chỉnh ma trận Q,R lần thứ ba
Hình 6.47 Kết quả thực nghiệm đáp ứng vận tốc góc cánh tay khảo sát hàm h(x) = x1 khi hiệu
chỉnh ma trận Q,R lần thứ ba
6.2.1.4 Hiệu chỉnh thông số ma trận lần thứ tư
104

0
Chọn R = 0.0000001 và Q = 
 0

 0
0
0
1
0
0 103
0
0
0

0
0

1
69
Hình 6.48 Kết quả thực nghiệm đáp ứng góc con lắc khảo sát hàm h(x) = x1 khi hiệu chỉnh
ma trận Q,R lần thứ tư
Hình 6.49 Kết quả thực nghiệm đáp ứng góc cánh tay khảo sát hàm h(x) = x1 khi hiệu chỉnh
ma trận Q,R lần thứ tư
70
Hình 6.50 Kết quả thực nghiệm đáp ứng vận tốc góc con lắc khảo sát hàm h(x) = x1 khi hiệu
chỉnh ma trận Q,R lần thứ tư
Hình 6.51 Kết quả thực nghiệm đáp ứng vận tốc góc cánh tay khảo sát hàm h(x) = x1 khi hiệu
chỉnh ma trận Q,R lần thứ tư
6.2.2 Khảo sát hàm h(x) = 3x1 – 3x3
6.2.2.1 Hiệu chỉnh thông số ma trận lần thứ nhất
103

0
Chọn R = 0.00001 và Q = 
 0

 0
0
0
1
0
0 100
0
0
0

0
0

1
71
Hình 6.52 Kết quả thực nghiệm đáp ứng góc con lắc khảo sát hàm h(x) = 3x1 - 3x3 khi hiệu
chỉnh ma trận Q,R lần thứ nhất
Hình 6.53 Kết quả thực nghiệm đáp ứng góc cánh tay khảo sát hàm h(x) = 3x1 - 3x3 khi hiệu
chỉnh ma trận Q,R lần thứ nhất
72
Hình 6.54 Kết quả thực nghiệm đáp ứng vận tốc góc con lắc khảo sát hàm h(x) = 3x1 - 3x3
khi hiệu chỉnh ma trận Q,R lần thứ nhất
Hình 6.55 Kết quả thực nghiệm đáp ứng vận tốc góc cánh tay khảo sát hàm h(x) = 3x1 - 3x3
khi hiệu chỉnh ma trận Q,R lần thứ nhất
6.2.2.2 Hiệu chỉnh thông số ma trận lần thứ hai
104

0
Chọn R =0.00001 và Q = 
 0

 0
0
0
1
0
0 101
0
0
0

0
0

1
73
Hình 6.56 Kết quả thực nghiệm đáp ứng góc con lắc khảo sát hàm h(x) = 3x1 - 3x3 khi hiệu
chỉnh ma trận Q,R lần thứ hai
Hình 6.57 Kết quả thực nghiệm đáp ứng góc cánh tay khảo sát hàm h(x) = 3x1 - 3x3 khi hiệu
chỉnh ma trận Q,R lần thứ hai
74
Hình 6.58 Kết quả thực nghiệm đáp ứng vận tốc góc con lắc khảo sát hàm h(x) = 3x1 - 3x3
khi hiệu chỉnh ma trận Q,R lần thứ hai
Hình 6.59 Kết quả thực nghiệm đáp ứng vận tốc góc cánh tay khảo sát hàm h(x) = 3x1 - 3x3
khi hiệu chỉnh ma trận Q,R lần thứ hai
6.2.2.3 Hiệu chỉnh thông số ma trận lần thứ ba
103

0
Chọn R = 0.0000001 và Q = 
 0

 0
0
0
1
0
0 103
0
0
0

0
0

1
75
Hình 6.60 Kết quả thực nghiệm đáp ứng góc con lắc khảo sát hàm h(x) = 3x1 - 3x3 khi hiệu
chỉnh ma trận Q,R lần thứ ba
Hình 6.61 Kết quả thực nghiệm đáp ứng góc cánh tay khảo sát hàm h(x) = 3x1 - 3x3 khi hiệu
chỉnh ma trận Q,R lần thứ ba
76
Hình 6.62 Kết quả thực nghiệm đáp ứng vận tốc góc con lắc khảo sát hàm h(x) = 3x1 - 3x3
khi hiệu chỉnh ma trận Q,R lần thứ ba
Hình 6.63 Kết quả thực nghiệm đáp ứng vận tốc góc cánh tay khảo sát hàm h(x) = 3x1 - 3x3
khi hiệu chỉnh ma trận Q,R lần thứ ba
6.2.2.4 Hiệu chỉnh thông số ma trận lần thứ tư
104

0
Chọn R = 0.0000001 và Q = 
 0

 0
0
0
1
0
0 103
0
0
0

0
0

1
77
Hình 6.64 Kết quả thực nghiệm đáp ứng góc con lắc khảo sát hàm h(x) = 3x1 - 3x3 khi hiệu
chỉnh ma trận Q,R lần thứ tư
Hình 6.65 Kết quả thực nghiệm đáp ứng góc cánh tay khảo sát hàm h(x) = 3x1 - 3x3 khi hiệu
chỉnh ma trận Q,R lần thứ tư
78
Hình 6.66 Kết quả thực nghiệm đáp ứng vận tốc góc con lắc khảo sát hàm h(x) = 3x1 - 3x3
khi hiệu chỉnh ma trận Q,R lần thứ tư
Hình 6.67 Kết quả thực nghiệm đáp ứng vận tốc góc cánh tay khảo sát hàm h(x) = 3x1 - 3x3
khi hiệu chỉnh ma trận Q,R lần thứ tư
6.2.3 Khảo sát hàm h(x) = 3x1 - 5x3
6.2.3.1 Hiệu chỉnh thông số ma trận lần thứ nhất
103

0
Chọn R = 0.00001 và Q = 
 0

 0
0
0
1
0
0 100
0
0
0

0
0

1
79
Hình 6.68 Kết quả thực nghiệm đáp ứng góc con lắc khảo sát hàm h(x) = 3x1 - 5x3 khi hiệu
chỉnh ma trận Q,R lần thứ nhất
Hình 6.69 Kết quả thực nghiệm đáp ứng góc cánh tay khảo sát hàm h(x) = 3x1 - 5x3 khi hiệu
chỉnh ma trận Q,R lần thứ nhất
80
Hình 6.70 Kết quả thực nghiệm đáp ứng vận tốc góc con lắc khảo sát hàm h(x) = 3x1 - 5x3
khi hiệu chỉnh ma trận Q,R lần thứ nhất
Hình 6.71 Kết quả thực nghiệm đáp ứng vận tốc góc cánh tay khảo sát hàm h(x) = 3x1 - 5x3
khi hiệu chỉnh ma trận Q,R lần thứ nhất
6.2.3.2 Hiệu chỉnh thông số ma trận lần thứ hai
104

0
Chọn R =0.00001 và Q = 
 0

 0
0
0
1
0
0 101
0
0
0

0
0

1
81
Hình 6.72 Kết quả thực nghiệm đáp ứng góc con lắc khảo sát hàm h(x) = 3x1 - 5x3 khi hiệu
chỉnh ma trận Q,R lần thứ hai
Hình 6.73 Kết quả thực nghiệm đáp ứng góc cánh tay khảo sát hàm h(x) = 3x1 - 5x3 khi hiệu
chỉnh ma trận Q,R lần thứ hai
82
Hình 6.74 Kết quả thực nghiệm đáp ứng vận tốc góc con lắc khảo sát hàm h(x) = 3x1 - 5x3
khi hiệu chỉnh ma trận Q,R lần thứ hai
Hình 6.75 Kết quả thực nghiệm đáp ứng vận tốc góc cánh tay khảo sát hàm h(x) = 3x1 - 5x3
khi hiệu chỉnh ma trận Q,R lần thứ hai
6.2.3.3 Hiệu chỉnh thông số ma trận lần thứ ba
103

0
Chọn R = 0.0000001 và Q = 
 0

 0
0
0
1
0
0 103
0
0
0

0
0

1
83
Hình 6.76 Kết quả thực nghiệm đáp ứng góc con lắc khảo sát hàm h(x) = 3x1 - 5x3 khi hiệu
chỉnh ma trận Q,R lần thứ ba
Hình 6.77 Kết quả thực nghiệm đáp ứng góc cánh tay khảo sát hàm h(x) = 3x1 - 5x3 khi hiệu
chỉnh ma trận Q,R lần thứ ba
84
Hình 6.78 Kết quả thực nghiệm đáp ứng vận tốc góc con lắc khảo sát hàm h(x) = 3x1 - 5x3
khi hiệu chỉnh ma trận Q,R lần thứ ba
Hình 6.79 Kết quả thực nghiệm đáp ứng vận tốc góc cánh tay khảo sát hàm h(x) = 3x1 - 5x3
khi hiệu chỉnh ma trận Q,R lần thứ ba
6.2.3.4Hiệu chỉnh thông số ma trận lần thứ tư
104

0
Chọn R = 0.0000001 và Q = 
 0

 0
0
0
1
0
0 103
0
0
0

0
0

1
85
Hình 6.80 Kết quả thực nghiệm đáp ứng góc con lắc khảo sát hàm h(x) = 3x1 - 5x3 khi hiệu
chỉnh ma trận Q,R lần thứ tư
Hình 6.81 Kết quả thực nghiệm đáp ứng góc cánh tay khảo sát hàm h(x) = 3x1 - 5x3 khi hiệu
chỉnh ma trận Q,R lần thứ tư
86
Hình 6.82 Kết quả thực nghiệm đáp ứng vận tốc góc con lắc khảo sát hàm h(x) = 3x1 - 5x3
khi hiệu chỉ nh ma trận Q,R lần thứ tư
Hình 6.83 Kết quả thực nghiệm đáp ứng vận tốc góc cánh tay khảo sát hàm h(x) = 3x1 - 5x3
khi hiệu chỉnh ma trận Q,R lần thứ tư
6.2.4 Khảo sát hàm y = h(x) = 3x1 – 7x3
6.2.4.1 Hiệu chỉnh thông số ma trận lần thứ nhất
103

0
Chọn R = 0.00001 và Q = 
 0

 0
0
0
1
0
0 100
0
0
0

0
0

1
87
Hình 6.84 Kết quả thực nghiệm đáp ứng góc cánh tay khảo sát hàm h(x) = 3x1 - 7x3 khi hiệu
chỉnh ma trận Q,R lần thứ nhất
Hình 6.85 Kết quả thực nghiệm đáp ứng góc cánh tay khảo sát hàm h(x) = 3x1 - 7x3 khi hiệu
chỉnh ma trận Q,R lần thứ nhất
88
Hình 6.86 Kết quả thực nghiệm đáp ứng vận tốc góc con lắc khảo sát hàm h(x) = 3x1 - 7x3
khi hiệu chỉnh ma trận Q,R lần thứ nhất
Hình 6.87 Kết quả thực nghiệm đáp ứng vận tốc góc cánh tay khảo sát hàm h(x) = 3x1 - 7x3
khi hiệu chỉnh ma trận Q,R lần thứ nhất
6.2.4.2 Hiệu chỉnh thông số ma trận lần thứ hai
103

0
Chọn R =0.00001 và Q = 
 0

 0
0
0
1
0
0 101
0
0
0

0
0

1
89
Hình 6.88 Kết quả thực nghiệm đáp ứng góc con lắc khảo sát hàm h(x) = 3x1 - 7x3 khi hiệu
chỉnh ma trận Q,R lần thứ hai
Hình 6.89 Kết quả thực nghiệm đáp ứng góc cánh tay khảo sát hàm h(x) = 3x1 - 7x3 khi hiệu
chỉnh ma trận Q,R lần thứ hai
90
Hình 6.90 Kết quả thực nghiệm đáp ứng vận tốc góc con lắc khảo sát hàm h(x) = 3x1 - 7x3
khi hiệu chỉnh ma trận Q,R lần thứ hai
Hình 6.91 Kết quả thực nghiệm đáp ứng vận tốc góc cánh tay khảo sát hàm h(x) = 3x1 - 7x3
khi hiệu chỉnh ma trận Q,R lần thứ hai
6.2.4.3 Hiệu chỉnh thông số ma trận lần thứ ba
103

0
Chọn R = 0.0000001 và Q = 
 0

 0
0
0
1
0
0 103
0
0
0

0
0

1
91
Hình 6.92 Kết quả thực nghiệm đáp ứng góc con lắc khảo sát hàm h(x) = 3x1 - 7x3 khi hiệu
chỉnh ma trận Q,R lần thứ ba
Hình 6.93 Kết quả thực nghiệm đáp ứng góc cánh tay khảo sát hàm h(x) = 3x1 - 7x3 khi hiệu
chỉnh ma trận Q,R lần thứ ba
92
Hình 6.94 Kết quả thực nghiệm đáp ứng vận tốc góc con lắc khảo sát hàm h(x) = 3x1 - 7x3
khi hiệu chỉnh ma trận Q,R lần thứ ba
Hình 6.95 Kết quả thực nghiệm đáp ứng vận tốc góc con cánh tay khảo sát hàm h(x) = 3x1 –
7x3 khi hiệu chỉnh ma trận Q,R lần thứ ba
6.2.4.4 Hiệu chỉnh thông số ma trận lần thứ tư
103

0
Chọn R = 0.0000001 và Q = 
 0

 0
0
0
1
0
0 10 2
0
0
0

0
0

1
93
Hình 6.96 Kết quả thực nghiệm đáp ứng góc con lắc khảo sát hàm h(x) = 3x1 - 7x3 khi hiệu
chỉnh ma trận Q,R lần thứ tư
Hình 6.97 Kết quả thực nghiệm đáp ứng góc cánh tay khảo sát hàm h(x) = 3x1 - 7x3 khi hiệu
chỉnh ma trận Q,R lần thứ tư
94
Hình 6.98 Kết quả thực nghiệm đáp ứng vận tốc góc con lắc khảo sát hàm h(x) = 3x1 - 7x3
khi hiệu chỉnh ma trận Q,R lần thứ tư
Hình 6.99 Kết quả thực nghiệm đáp ứng vận tốc góc cánh tay khảo sát hàm h(x) = 3x1 - 7x3
khi hiệu chỉnh ma trận Q,R lần thứ tư
Nhận xét: Thông qua các kết quả thực nghiệm khảo sác các hàm h(x) với các thông số
m,n khác nhau và việc hiệu chỉnh ma trận Q,R tác giả có nhận xét như sau:
Đầu tiên, giải thuật tuyến tính hóa vào ra vốn được thiết kế để điều khiển cho các
hệ SISO. Trong bài luận văn này, tác giả đã cố gắng vận dụng giải thuật này trên một hệ
SIMO nên kết quả đáp ứng sẽ không thỏa mãn với yêu cầu đặt ra. Tiêu biểu khi ta khảo
sát hàm h(x) = x1, kết quả thực nghiệm cho thấy thanh con lắc tuy được ổn định nhưng
95
thanh cánh tay lại quay bất ổn định, không quay về vị trí cân bằng và sớm muộn sẽ khiến
cho thanh con lắc rơi xuống.
Thứ hai, việc chọn thông số m,n cho cho hàm khảo sát h(x) và sự hiện diện của hai
biến trạng thái hệ thống là x1 và x3 có tác động rất lớn đáp ứng cân bằng của hệ thống.
Trong các lần khảo nghiệm tác giả nhận thấy rằng, đối với hệ SIMO con lắc ngược mà
tác giả đang khảo sát, hàm h(x) phải luôn chứa hai biến là x1 và x3, như thế mới đảm
bảo cho giải thuật có thể điều khiển cân bằng thành công. Và có một quy luật khi chọn
hệ số m và n đó là, m luôn dương và n luôn âm. Thông qua kết thực nghiệm khi khảo sát
hàm h(x) = x1, ta thấy thanh con lắc tuy ổn định nhưng thanh cánh tay lại quay bất ổn
định không thỏa mãn yêu cầu đặt ra. Nhưng khi ta xem xét kết quả đáp ứng khi khảo sát
các hàm h(x) = 3x1 – 3x3, h(x) = 3x1 – 3x3, h(x) = 3x1 – 7x3, đáp ứng của hệ thống
được cải thiện một cách rõ ràng, không những thanh con lắc mà thanh cánh tay được ổn
định.
Thứ ba, nên chọn các bộ thông số Q,R sao cho phù hợp đối với yêu cầu điều khiển.
Thông qua các kết quả thực nghiệm, tác giả nhận thấy để hệ thống có thời gian xác lập
−4
−6
ngắn, ta nên chọn ma trận R bé, cụ thể nằm trong khoảng 10 10  . Đối với ma trận
Q, ta nên lựa chọn hiệu chỉnh 2 thông số đó là Q11 và Q33 2 thông số này ảnh hương trực
tiếp đến ổn định thanh con lắc và cánh tay. Khi hiệu chỉnh 2 thông số trên, ta nên chọn
thông số có giá trị lớn, cụ thể là từ 101 hoặc 102 trở lên. Tác giả khảo sát 2 trường hợp
sẽ xảy ra khi ta hiệu chỉnh:
• Trường hợp Q11 > Q33 : Thời gian xác lập ngắn, thanh cánh tay và thanh con lắc dao
động bé
• Trường hợp Q11 = Q33: Thời gian xác lập dài hơn, thanh con lắc ổn định ngay lập
tức nhưng thanh cánh tay dao động một khoảng khá lớn.
96
CHƯƠNG 7:KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN
7.1 Kết luận
Qua đề tài này, tác giả luận văn đã thành công xây dựng bộ điều khiển tuyến tính
hóa vào ra. Bộ điều khiển đã thành công điều khiển cân bằng ổn định hệ thống con lắc
ngược. Sau thời gian thực hiện đồ án, tác giả đã đạt được hầu hết các mục tiêu đặt ra:
• Thiết kế mô hình con lắc ngược quay
• Xây dựng phương trình động lực học cho con lắc
• Xây dựng bộ điều khiển và thu thập kết quả mô phỏng
• Xây dựng giải thuật nhúng trên Arduino và thu thập dữ liệu thực nghiệm
• So sánh kết quả đáp ứng giữa mô phỏng và thực tế
Kết quả mô phỏng và kết quả trên thí nghiệm thực tế có khác nhau sỡ dĩ do nhiễu
và sự dao động của con lắc. Trong kết quả mô phỏng, cả thanh con lắc và thanh cánh tay
đều ổn định về vị trí cân bằng. Nhưng trong kết quả thực tế, đáp ứng ngõ ra của thanh
con lắc như chúng ta thấy có sự dao động một khoảng rất bé quanh vị trí cân bằng và
thanh cánh tay chuyển động theo dạng sóng hình Sin phi tuyến để luôn cân bằng con lắc.
Nguyên nhân có thể bắt nguồn từ hệ thống cơ khí chưa chuẩn xác, nhiễu động cơ, độ trễ
từ tính, v.v. Điều này có thể cải thiện bằng việc tăng các các hệ số của ma trận Q hoặc
R, hoặc mô hình hóa hệ con lắc ngược tốt hơn, hoặc tìm ra ma trận khảo sát h(x) có thể
bao quát các trường hợp tác động bởi nhiễu tốt hơn.
7.2 Hướng phát triển
Mục tiêu mà sinh viên muốn đề xuất hướng phát triên là tiếp tục hoàn thiện bộ điều
khiển tuyến tính hóa vào ra, loại trừ nhiễu số hệ thống hoặc thích nghi với nhiễu, thiết
kế bộ điều khiển phi tuyến, thiết kế cân bằng hệ con lắc ngược kép 3 bậc tự do, thiết kế
bộ điều khiển Swing-Up bằng phương pháp năng lượng, thiết kế các giải thuật điều khiển
nâng cao khác từ đó làm nền tảng cho việc học thuật trở thành nghiên cứu sinh.
97
TÀI LIỆU THAM KHẢO
[1]
User Manual of Quanser: Inverted Pendulum Experimenr for LabVIEW Users.
Link: https://www.quanser.com/products/rotary-inverted-pendulum/
[2] Yung-Chih Fu and Jung-Shan Lin, "Nonlinear backstepping control design of the
furuta pendulum," Proceedings of 2005 IEEE Conference on Control Applications,
2005. CCA 2005., Toronto, ON, Canada, 2005, pp. 96-101, doi:
10.1109/CCA.2005.1507107.
[3] Mojtaba Ahmadieh Khanesar, Mohammad Teshnehlab and Mahdi Aliyari
Shoorehdeli, "Sliding mode control of Rotary Inverted Pendulm," 2007
Mediterranean Conference on Control & Automation, Athens, Greece, 2007, pp.
1-6, doi: 10.1109/MED.2007.4433653.
[4] M. Akhtaruzzaman and A. A. Shafie, "Modeling and control of a rotary inverted
pendulum using various methods, comparative assessment and result analysis,"
2010 IEEE International Conference on Mechatronics and Automation, Xi'an,
China, 2010, pp. 1342-1347, doi: 10.1109/ICMA.2010.5589450.
[5] Agrawal, R., Mitra, R. (2013). Adaptive Neuro Fuzzy Inference Structure
Controller for Rotary Inverted Pendulum. In: Kumar M., A., R., S., Kumar, T. (eds)
Proceedings of International Conference on Advances in Computing. Advances in
Intelligent Systems and Computing, vol 174. Springer, New Delhi.
https://doi.org/10.1007/978-81-322-0740-5_141
[6] J. -H. Li, "Composite fuzzy control of a rotary inverted pendulum," 2013 IEEE
International Symposium on Industrial Electronics, Taipei, Taiwan, 2013, pp. 1-5,
doi: 10.1109/ISIE.2013.6563769.
[7] Sunil A., Manju G.: “Quanser Rotary Inverted Pendulum Stabilization with Fuzzy
Logic Controller”, International Research Journal of Engineering and Technology
(IRJET) Volume: 03 Issue: 06 | June-2016
[8] Balamurugan, S., Venkatesh, P. & Varatharajan, M. Fuzzy sliding-mode control
with low pass filter to reduce chattering effect: an experimental validation on
Quanser SRIP. Sādhanā 42, 1693–1703 (2017). https://doi.org/10.1007/s12046017-0722-9
[9] Halicioglu, Recep & Öksüz, Mehmet. (2018). Alternative Controller Design for
Rotary Inverted Pendulum. Tehnički glasnik. 12. 139-145. 10.31803/tg20180208152214.
[10] Bekkar, B., Ferkous, K. Design of Online Fuzzy Tuning LQR Controller Applied
to Rotary Single Inverted Pendulum: Experimental Validation. Arab J Sci Eng 48,
6957–6972 (2023). https://doi.org/10.1007/s13369-022-06921-3
98
[11] ŞEN FN, Beldek U. Genetic Algorithm Based Stabilization of Rotary Inverted
Pendulum by State Feedback. Research Square; 2022. DOI: 10.21203/rs.3.rs2373423/v1.
[12] A. Gutarra, S. Palomino, E. J. Alegria and J. Cisneros, "Fuzzy Controller Design
for Rotary Inverted Pendulum System Using Genetic Algorithms," 2022 IEEE
ANDESCON,
Barranquilla,
Colombia,
2022,
pp.
1-6,
doi:
10.1109/ANDESCON56260.2022.9989988.
[13] Nguyễn Văn Đông Hải, Xây dựng bộ điều khiển nhúng tuyến tính hóa vào ra cho
hệ xe con lắc ngược, Luận văn Thạc sỹ, 2011.
[14] Nguyen Duc Quyen, Ngo Van Thuyen, Nguyen Quang Hoc, Nguyen Duc Hien,
Rotary inverted pendulum and control of rotary inverted pendulum by artificial
neural network, Proc. Natl. Conf. Theor. Phys. 37 (2012), pp. 243-249.
[15] Nguyễn Văn Đông Hải and Ngô Văn Thuyên, “PID-neuron controller design for
rotary inverted pendulum system”, JTE, no. 23, pp. 37–43, Dec. 2012.
[16] Khanh Nguyễn Văn, Nguyễn Vĩnh Hảo, và Nguyễn Ngô Phong. 2014. “Điều khiển
cân bằng Con lắc ngược sử dụng bộ điều khiển cuốn chiếu”. Tạp Chí Khoa học Đại
học cần Thơ, số p.h 31 (Tháng Sáu):18-25.
[17] Trần Văn Hào, "Điều khiển con lắc ngược quay", Luận văn thạc sỹ, 2017.
[18] Thi-Thanh- Hoang Le, Anh-Khoa Vo, Thien-Van Nguyen, Van-Khanh Doan, DucHa Vu, Manh-Son Tran, Van-Dong-Hai Nguyen, “Fuzzy Controller for Rotary
Inverted Pendulum”, Robotica & Management, Vol. 23, Issue 2, pp. 16-20, 2018.
[19] M. T. Nguyen, “Trajectory tracking sliding mode control for cart and pole system”,
JTE, no. 55, pp. 56–63, Dec. 2019.
[20] M. T. Vo, “Design of Input-Output Feedback Linearization Control for Rotary
Inverted Pendulum System”, JTE, no. 69, pp. 26–35, Apr. 2022.
99
PHỤ LỤC
Chương trình chính
#include<TimerOne.h>
#include<math.h>
float u = 0;
float K11 = 8518.6;
float K22 = 7124.6;
float K33 = 2978.9;
float K44 = 114.7;
float z1, Lf_h, Lf2_h, Lf3_h, Lf4_h, LgLf3_h = 0;
float x1, x2, x3, x4 = 0;
float m = 1, n = 0;
double Output, LastOutput;
float Men = 0.2;
float Mp = 0.01392;
float Lp = 0.2;
float Jp = 0.0001856;
float Mr = 0.04872;
float Lr = 0.2;
float Jr = 0.0006496;
float g = 9.81;
float Rm = 11.944421124154792;
float Kt = 0.086164500636167;
float Kb = 0.086164500636167;
float Jm = 0.000059833861116;
float Cm = 0.000067435629646;
float K1 = Kt / Rm;
float K2 = Cm + Kt * Kb / Rm;
float K3 = Jm;
100
float u1, u2, u3, u4;
#define PWM 5 //xung pwm
#define L298_IN1_QUAY_THUAN 6 //quay cung chieu kim dong ho
#define L298_IN2_QUAY_NGHICH 7 // quay nguoc chieu kim dong hoi
// for encoder 0, encoder đọc vận tốc góc con lắc
#define encoder0PinA 2 // Khai báo chân đọc channel A
#define encoder0PinB 3 // Khai báo chân đọc channel B
volatile int encoder0Count = 0;
// Khai báo biến đếm xung của encoder 0
volatile float angle0 = 0;
// Khai báo biến đo góc của encoder 0
volatile float velocity0 = 0;
// Khai báo biến vận tốc góc của encoder 0
volatile float angle0_previous = 0; // Khai báo biến đo góc lần trước
volatile float angle0_post = 0;
// Khai báo biến đo góc lần trức nũa
// for encoder 1, encoder đọc vận tốc góc động cơ
#define encoder1PinA 18 // Khai báo chân đọc channel A
#define encoder1PinB 19 // Khai báo chân đọc channel B
volatile int encoder1Count = 0;
volatile float angle1 = 0;
volatile float velocity1 = 0;
volatile float angle1_previous = 0;
volatile float angle1_post = 0;
// for Timer2
volatile int tcnt2 = 131;
volatile int t = 0;
void setup() {
// for encoder 0 con lắc
pinMode(encoder0PinA, INPUT_PULLUP);
pinMode(encoder0PinB, INPUT_PULLUP);
attachInterrupt(0, doEncoder0A, CHANGE);
attachInterrupt(1, doEncoder0B, CHANGE);
// for encoder 1 động cơ
pinMode(encoder1PinA, INPUT);
101
pinMode(encoder1PinB, INPUT);
attachInterrupt(4, doEncoder1A, CHANGE);
attachInterrupt(5, doEncoder1B, CHANGE);
// setup for Timer2: interrupts every 1 ms
TIMSK2 &= ~(1 << TOIE2);
TCCR2A &= ~((1 << WGM21) | (1 << WGM20));
TCCR2B &= ~(1 << WGM22);
ASSR &= ~(1 << AS2);
TIMSK2 &= ~(1 << OCIE2A);
TCCR2B |= (1 << CS22) | (1 << CS20);
TCCR2B &= ~(1 << CS21);
TCNT2 = tcnt2;
TIMSK2 |= (1 << TOIE2);
Serial.begin (57600); // for debugging
// LQR
Timer1.initialize(10000); //don vi us
Timer1.attachInterrupt(input_output_linearization);
}
void loop() {
Serial.print(angle0);
Serial.print(" ");
Serial.print(angle1);
Serial.print(" ");
Serial.print(velocity0);
Serial.print(" ");
Serial.print(velocity1);
Serial.print(" ");
Serial.println(u);
}
void input_output_linearization() // giải thuật tuyến tính hóa vào ra
{
x1 = angle0; x2 = velocity0; x3 = angle1; x4 = velocity1;
z1 = m*x1 + n*x3;
Lf_h = m*x2 + n*x4;
102
Lf2_h = m * ((x1 * (18 * K3 * g + 18 * Lr * Lr * Men * g + 18 * Lr * Lr * Mp *
g + 6 * Lr * Lr * Mr * g)) / (Lp * (12 * K3 + 12 * Lr * Lr * Men + 3 * Lr * Lr * Mp +
4 * Lr * Lr * Mr)) - (18 * K2 * Lr * x4) / (Lp * (12 * K3 + 12 * Lr * Lr * Men + 3 * Lr
* Lr * Mp + 4 * Lr * Lr * Mr))) - n * ((12 * K2 * x4) / (12 * K3 + 12 * Lr * Lr * Men
+ 3 * Lr * Lr * Mp + 4 * Lr * Lr * Mr) - (9 * Lr * Mp * g * x1) / (12 * K3 + 12 * Lr *
Lr * Men + 3 * Lr * Lr * Mp + 4 * Lr * Lr * Mr));
Lf3_h = ((12 * K2 * x4) / (12 * K3 + 12 * Lr * Lr * Men + 3 * Lr * Lr * Mp + 4
* Lr * Lr * Mr) - (9 * Lr * Mp * g * x1) / (12 * K3 + 12 * Lr * Lr * Men + 3 * Lr * Lr
* Mp + 4 * Lr * Lr * Mr)) * ((12 * K2 * n) / (12 * K3 + 12 * Lr * Lr * Men + 3 * Lr *
Lr * Mp + 4 * Lr * Lr * Mr) + (18 * K2 * Lr * m) / (Lp * (12 * K3 + 12 * Lr * Lr *
Men + 3 * Lr * Lr * Mp + 4 * Lr * Lr * Mr))) + x2 * ((m * (18 * K3 * g + 18 * Lr * Lr
* Men * g + 18 * Lr * Lr * Mp * g + 6 * Lr * Lr * Mr * g)) / (Lp * (12 * K3 + 12 * Lr
* Lr * Men + 3 * Lr * Lr * Mp + 4 * Lr * Lr * Mr)) + (9 * Lr * Mp * g * n) / (12 * K3
+ 12 * Lr * Lr * Men + 3 * Lr * Lr * Mp + 4 * Lr * Lr * Mr));
Lf4_h = ((m * (18 * K3 * g + 18 * Lr * Lr * Men * g + 18 * Lr * Lr * Mp * g +
6 * Lr * Lr * Mr * g)) / (Lp * (12 * K3 + 12 * Lr * Lr * Men + 3 * Lr * Lr * Mp + 4 *
Lr * Lr * Mr)) + (9 * Lr * Mp * g * n) / (12 * K3 + 12 * Lr * Lr * Men + 3 * Lr * Lr *
Mp + 4 * Lr * Lr * Mr)) * ((x1 * (18 * K3 * g + 18 * Lr * Lr * Men * g + 18 * Lr * Lr
* Mp * g + 6 * Lr * Lr * Mr * g)) / (Lp * (12 * K3 + 12 * Lr * Lr * Men + 3 * Lr * Lr
* Mp + 4 * Lr * Lr * Mr)) - (18 * K2 * Lr * x4) / (Lp * (12 * K3 + 12 * Lr * Lr * Men
+ 3 * Lr * Lr * Mp + 4 * Lr * Lr * Mr))) - (12 * K2 * ((12 * K2 * x4) / (12 * K3 + 12 *
Lr * Lr * Men + 3 * Lr * Lr * Mp + 4 * Lr * Lr * Mr) - (9 * Lr * Mp * g * x1) / (12 *
K3 + 12 * Lr * Lr * Men + 3 * Lr * Lr * Mp + 4 * Lr * Lr * Mr)) * ((12 * K2 * n) / (12
* K3 + 12 * Lr * Lr * Men + 3 * Lr * Lr * Mp + 4 * Lr * Lr * Mr) + (18 * K2 * Lr *
m) / (Lp * (12 * K3 + 12 * Lr * Lr * Men + 3 * Lr * Lr * Mp + 4 * Lr * Lr * Mr)))) /
(12 * K3 + 12 * Lr * Lr * Men + 3 * Lr * Lr * Mp + 4 * Lr * Lr * Mr) - (9 * Lr * Mp *
g * x2 * ((12 * K2 * n) / (12 * K3 + 12 * Lr * Lr * Men + 3 * Lr * Lr * Mp + 4 * Lr *
Lr * Mr) + (18 * K2 * Lr * m) / (Lp * (12 * K3 + 12 * Lr * Lr * Men + 3 * Lr * Lr *
Mp + 4 * Lr * Lr * Mr)))) / (12 * K3 + 12 * Lr * Lr * Men + 3 * Lr * Lr * Mp + 4 * Lr
* Lr * Mr);
LgLf3_h = (144 * K1 * K2 * ((12 * K2 * n) / (12 * K3 + 12 * Lr * Lr * Men + 3
* Lr * Lr * Mp + 4 * Lr * Lr * Mr) + (18 * K2 * Lr * m) / (Lp * (12 * K3 + 12 * Lr *
Lr * Men + 3 * Lr * Lr * Mp + 4 * Lr * Lr * Mr)))) / ((12 * K3 + 12 * Lr * Lr * Men +
3 * Lr * Lr * Mp + 4 * Lr * Lr * Mr) * (12 * K3 + 12 * Lr * Lr * Men + 3 * Lr * Lr *
Mp + 4 * Lr * Lr * Mr)) + (18 * K1 * Lr * ((m * (18 * K3 * g + 18 * Lr * Lr * Men * g
+ 18 * Lr * Lr * Mp * g + 6 * Lr * Lr * Mr * g)) / (Lp * (12 * K3 + 12 * Lr * Lr * Men
+ 3 * Lr * Lr * Mp + 4 * Lr * Lr * Mr)) + (9 * Lr * Mp * g * n) / (12 * K3 + 12 * Lr *
103
Lr * Men + 3 * Lr * Lr * Mp + 4 * Lr * Lr * Mr))) / (Lp * (12 * K3 + 12 * Lr * Lr *
Men + 3 * Lr * Lr * Mp + 4 * Lr * Lr * Mr));
u = (-K11*z1 - K22*Lf_h - K33*Lf2_h - K44*Lf3_h - Lf4_h)/LgLf3_h;
if (u > 5) {
u = 5;
}
if (u < -5) {
u = -5;
}
if (abs(angle1) > 1.2) {u = 0;}
Output = (u / 5) * 255;
LastOutput = Output;
if (LastOutput >= 0)
{
digitalWrite(L298_IN1_QUAY_THUAN, HIGH);
digitalWrite(L298_IN2_QUAY_NGHICH, LOW);
analogWrite(PWM, abs(LastOutput));
}
else
{
digitalWrite(L298_IN1_QUAY_THUAN, LOW);
digitalWrite(L298_IN2_QUAY_NGHICH, HIGH);
analogWrite(PWM, abs(LastOutput));
}
}
104
Chương trình đọc xung encoder (giải thuật tăng 4 lần độ phân giải)
//-------------------// for encoder 0
//-------------------void doEncoder0A(){ // interrupt 0 function
if (digitalRead(encoder0PinA) == HIGH) { // look for a low-to-high on channel
A
if (digitalRead(encoder0PinB) == LOW) { // check channel B to see which
way encoder is turning
encoder0Count = encoder0Count + 1;
}
else {
encoder0Count = encoder0Count - 1;
}
}
else {
// must be a high-to-low edge on channel A
if (digitalRead(encoder0PinB) == HIGH) { // check channel B to see which
way encoder is turning
encoder0Count = encoder0Count + 1;
}
else {
encoder0Count = encoder0Count - 1;
}
}
angle0 = (0.003925*encoder0Count) - 3.14; // công thức mới
//angle0 = 0.00105*encoder0Count; // unit: radian công thức cũ
}
void doEncoder0B(){ // interrupt 1 function
if (digitalRead(encoder0PinB) == HIGH) { // look for a low-to-high on channel
B
if (digitalRead(encoder0PinA) == HIGH) { // check channel A to see which
way encoder is turning
encoder0Count = encoder0Count + 1;
}
else {
105
encoder0Count = encoder0Count - 1;
}
}
else {
// must be a high-to-low edge on on channel B
if (digitalRead(encoder0PinA) == LOW) { // check channel B to see which
way encoder is turning
encoder0Count = encoder0Count + 1;
}
else {
encoder0Count = encoder0Count - 1;
}
}
angle0 = (0.003925*encoder0Count) - 3.14; // công thức mới
//angle0 = 0.00105*encoder0Count; // unit: radian công thức cũ
}
//-------------------// for encoder 1
//-------------------void doEncoder1A(){
if (digitalRead(encoder1PinA) == HIGH) {
if (digitalRead(encoder1PinB) == LOW) {
encoder1Count = encoder1Count + 1;
}
else {
encoder1Count = encoder1Count - 1;
}
}
else {
if (digitalRead(encoder1PinB) == HIGH) {
encoder1Count = encoder1Count + 1;
}
else {
encoder1Count = encoder1Count - 1;
}
}
angle1 = 0.00785*encoder1Count;
106
//angle1 = 0.00126*encoder1Count; //công thức cũ
}
void doEncoder1B(){
if (digitalRead(encoder1PinB) == HIGH) {
if (digitalRead(encoder1PinA) == HIGH) {
encoder1Count = encoder1Count + 1;
}
else {
encoder1Count = encoder1Count - 1;
}
}
else {
if (digitalRead(encoder1PinA) == LOW) {
encoder1Count = encoder1Count + 1;
}
else {
encoder1Count = encoder1Count - 1;
}
}
angle1 = 0.00785*encoder1Count; // công thức mới
//angle1 = 0.00126*encoder1Count; // công thức cũ
}
//-------------------// estimate velocity
//-------------------ISR(TIMER2_OVF_vect) {
TCNT2 = tcnt2; // reload the timer
t++;
if (t == 1){
angle0_previous = angle0;
angle1_previous = angle1;
}
else if (t == 10){
angle0_post = angle0;
angle1_post = angle1;
107
}
else if (t == 11){
velocity0 = (angle0_post - angle0_previous)*100; // unit: rad/s
velocity1 = (angle1_post - angle1_previous)*100;
t = 0;
}
}
108