ĐẠI HỌC QUỐC GIA TP.HCM
TRƯỜNG ĐẠI HỌC BÁCH KHOA
_____o0o_____

PHAN VINH HIẾU
ĐÊ TÀI

XÂY DỰNG BỘ ĐIỀU KHIỂN
NHÚNG CHO CON LẮC NGƯỢC
QUAY
CHUYÊN NGÀNH: T Ự ĐỘNG HĨA
MSHV:01507311

LUẬN ÁN THẠC SỸ

Tp. Hồ Chí Minh, ngày…tháng 06 năm 2010


Luận văn thạc sỹ - Phan Vinh Hiếu

CƠNG TRÌNH ĐƯỢC HOÀN THÀNH TẠI
TRƯỜNG ĐẠI HỌC BÁCH KHOA
ĐẠI HỌC QUỐC GIA TP.HỒ CHÍ MINH

Cán bộ hướng dẫn khoa học: ...............................................................
(ghi rõ họ tên, học hàm, học vị và chữ ký)

Cán bộ chấm nhận xét 1:......................................................................
(ghi rõ họ tên, học hàm, học vị và chữ ký)

Cán bộ chấm nhận xét 2:......................................................................

(ghi rõ họ tên, học hàm, học vị và chữ ký)

Luận văn Thạc Sỹ được bảo vệ tại HỘI ĐỒNG CHẤM BẢO VỆ LUẬN
VĂN THẠC SỸ
TRƯỜNG ĐẠI HỌC BÁCH KHOA, ngày ….tháng…năm 2010.


Luận văn thạc sỹ - Phan Vinh Hiếu

ĐẠI HỌC QUỐC GIA TP. HCM
TRƯỜNG ĐẠI HỌC BÁCH KHOA
----------------

CỘNG HOÀ XÃ HỘI CHỦ NGHIÃ VIỆT NAM
Độc Lập - Tự Do - Hạnh Phúc
---oOo--Tp. HCM, ngày . . . . . tháng . . . . . năm 2010

NHIỆM VỤ LUẬN VĂN THẠC SĨ
Họ và tên học viên:

PHAN VINH HIẾU

Giới tính : Nam

Ngày, tháng, năm sinh :

09-11-1982

Nơi sinh : TP HỒ CHÍ MINH


Chuyên ngành :

TỰ ĐỘNG HĨA

Khố (Năm trúng tuyển) :

2007

1- TÊN ĐỀ TÀI:

XÂY DỰNG BỘ ĐIỀU KHIỂN NHÚNG CHO CON LẮC NGƯỢC
QUAY
2- NHIỆM VỤ LUẬN VĂN:
•

Xây dựng phần cơ khí và điện tử mơ hình con lắc ngược quay.

•

Thu thập dữ liệu từ mơ hình thực để ước lượng các tham số của mơ hình con lắc

•

Xây dựng bộ điều khiển thời gian thực trên DSP để dựng ngược và giũ cân bằng
con lắc.

3- NGÀY GIAO NHIỆM VỤ : ...........................................................................................
4- NGÀY HOÀN THÀNH NHIỆM VỤ : ..........................................................................
5- HỌ VÀ TÊN CÁN BỘ HƯỚNG DẪN (Ghi đầy đủ học hàm, học vị ): ........................
............................................................................................................................................

............................................................................................................................................
............................................................................................................................................
............................................................................................................................................
Nội dung và đề cương Luận văn Thạc Sĩ đã được Hội Đồng Chuyên Ngành thông qua.
CÁN BỘ HƯỚNG DẪN
(Họ tên và chữ ký)

CHỦ NHIỆM BỘ MÔN
QUẢN LÝ CHUYÊN NGÀNH
(Họ tên và chữ ký)


Luận văn thạc sỹ - Phan Vinh Hiếu

Lời cảm ơn.
Trước tiên, tôi xin chân thành cảm ơn Thầy Nguyễn Đức Thành người đã
tận tình hướng dẫn và truyền đạt kiến thức giúp tơi hồn thành luận án này. Và quan
trọng hơn hết, Ts là người đã gợi mở cho tác giả một đề tài, có tính ứng dụng cao,
hồn tồn phù hợp với năng lực của tác giả.
Bên cạnh đó tôi xin chân thành cảm ơn đến quý thầy cô trong bộ môn Điều
Khiển Tự Động: thầy Nguyễn Vĩnh Hão, cơ Nguyễn Thị Phương Hà, thầy Huỳnh
Thái Hồng, thầy Dương Hoài Nghĩa, thầy Nguyễn Thiện Thành ….đã định
hướng và hỗ trợ một số phần quan trọng cho việc học tập, nghiên cứu và phát triển
đề tài.
Tôi cũng xin chân thành cảm ơn các bạn cao học khóa 2006 và 2007 đã
động viên giúp đỡ tơi trong suốt khóa học.
Cuối cùng, tôi xin chân thành cảm ơn sự quan tâm, hỗ trợ, tạo điều kiện và
động viên về vật chất lẫn tinh thần của các thành viên trong gia đình trong suốt thời
gian qua.
Tp.HCM, ngày….tháng….năm 2010.

Tác giả

PHAN VINH HIẾU


Luận văn thạc sỹ - Phan Vinh Hiếu

Tóm tắt luận văn thạc sỹ
Con lắc ngược quay là một đối tượng đại diện cho một lớp các đối tượng có
độ phi tuyến cao và không ổn định. Với ưu điểm hệ thống cơ khí khơng q phức
tạp (so với con lắc ngược đặt trên xe) con lắc ngược quay được sử dụng rộng rãi
trong việc giảng dạy và nghiên cứu lý thuyết điều khiển tự động cũng như xây dựng
các bộ điều khiển số. Hệ thống bao gồm một que kim loại (con lắc) quay quanh một
trục thẳng đứng. Que kim loại được gắn gián tiếp vào đầu của một cánh tay nằm
ngang thơng qua một encoder để đo góc, đầu còn lại của cánh tay được gắn vào trục
quay của một động cơ servo DC được đặt thẳng đứng để cánh tay có thể quay trong
một mặt phẳng nằm ngang. Mục tiêu của đề tài là thu thập dữ liệu rồi ước lượng các
tham số của con lắc bằng phương pháp Bình Phương Cực Tiểu, sau đó xây dựng
luật điều khiển dựng con lắc từ vị trí thẳng đứng hướng xuống đến vị trí thẳng đứng
hướng lên bằng phương pháp do K Futara - M Yamakita đề xuất và giữ con lắc cân
bằng tại vị trí này bằng phương pháp Điều Khiển Đặt Cực, LQR. Luật điều khiển
sẽ được xây dựng trên nền DSP xử lý dấu chấm động TMS320f28335. Ưu điểm khi
sử dụng DSP để thực thi bộ điều khiển là: kích thước nhỏ gọn, giá thành rẻ, sử dụng
linh hoạt và dễ dàng ứng dụng vào thực tế.

Abstract
The rotary inverted pendulum system is a highly nonlinear and unstable
system, The mechanism of this system is not complicated. Because of this, this
system provides a platform for verifying the effectiveness of different control
schemes. The rotary inverted pendulum consists of a rigid rod (pendulum) rotating

in a vertical plane. The rigid rod is attached to a pivot arm that is mounted on the
shaft of a DC motor such that the pivot arm can rotate in the horizontal plan. The
thesis objective is collect data and use Least Squares Estimation Methods to
estimate System’s Parameter.After that, Building a controller which will swing up
the pendulum from the downward position and to balance the pendulum in the
vertical-upright position. The Pole Replacement and LQR control scheme are used
to achieve balancing control, and the controller proposed by K Futara and M
Yamakita is used to swing up the pendulum. The control law is implemented
through a IC TMS320f28335 floating point digital signal processor. The key
features of this DSP-Based stand-alone control system include portability,
minimizing the hardware size and saving cost.


Luận văn thạc sỹ - Phan Vinh Hiếu

Mục lục
Chương 1: Tổng quan ......................................................................................01
1.1.

Đặt vấn đề .....................................................................................................01

1.2.

Tổng quan về đề tài.......................................................................................02

1.3.

Tóm tắt các nội dung các chương .................................................................06

Chương 2: Nhiệm vụ luận văn. ..................................................................07

Chương 3: Mơ hình hóa và ước lượng tham số .....................................08
3.1.

Mơ hình hố hệ thống con lắc ngược quay

08

3.2.

Ước lượng thông số hệ thống

13

3.3.

Kỹ thuật kiểm chứng mô hình

20

Chương 4: Thiết kế bộ điều khiển ..............................................................21
4.1. Giới thiệu........................................................................................................21
4.2

Thiết kế bộ điều khiển Lật ngược con lắc......................................................21

4.3

Thiết kế bộ điều khiển Cân bằng. ..................................................................22

4.4


Mô phỏng bộ điều khiển trên Simulink. ........................................................29

4.5

So sánh hai bộ điều khiển Đặt cực và LQR. ..................................................38

Chương 5: Ước lượng thông số từ dữ liệu thu thập .............................39
5.1.

Giới thiệu.......................................................................................................39

5.2.

Lập trình DSP tạo tín hiệu kích và thu thập dữ liệu.....................................40

5.3.

Tiến hành thu thập dữ liệu nhận dạng...........................................................45

5.4.

Đánh giá kết quả ước lượng tham số ............................................................46

Chương 6: Xây dựng bộ điều khiển thời gian thực trên nền DSP....57


Luận văn thạc sỹ - Phan Vinh Hiếu

6.1. Giới thiệu........................................................................................................57

6.2.

Xây dựng bộ điều khiển Lật ngược và giữ Cân bằng con lắc trên nền DSP
bằng Phương pháp đặt cực .......................................................................... 57

6.3.

Xây dựng bộ điều khiển Lật ngược và giữ Cân bằng con lắc trên nền DSP
bằng Phương pháp LQR rời rạc

68

6.4. So sánh hai bộ điều khiển Đặt cực và LQR ...................................................71

Chương 7: Kết luận và hướng phát triển đề tài. ……….........................68
7.1. Kết luận……… ..............................................................................................72
7.2. Hướng phát triển đề tài ..................................................................................73

Phụ lục ..................................................................................................................74
Phần A: Thành lập mơ hình tốn học phi tuyến. ...................................74
A.1 Hệ thống theo toạ độ ........................................................................................74
A.2 Phương trình Lagrange của chuyển động(các hệ thống quay)

75

A.3 Mơ hình động lực học (vị trí thẳng đứng) ...................................................... 75
A.4 Mơ hình động lực học(theo phương hướng xuống)

78


Phần B: Tuyến tính hố các mơ hình tốn học phi tuyến

80

B.1 Tuyến tính hố các mơ hình tốn học phi tuyến

80

B.2 Mơ hình tuyến tính (theo phương thẳng đứng)

.80

Phần C: Kết quả ước lượng sử dụng Phương Pháp Bình Phương
Cực Tiểu.
C.1 Kết quả ước lượng sử dụng Phương Pháp 1.

85

C.2 Kết quả ước lượng sử dụng Phương Pháp 2.

88

Phần D: Thiết kế cơ khí

91


Luận văn thạc sỹ - Phan Vinh Hiếu

Phần E: Thiết kế điện tử


94

E.1 Mạch nguồn

94

E.2 Mạch cầu H lái động cơ

94

E.3 Mạch điều khiển

96

E.4 Giới thiệu Encoder

100

E.5 Module kết nối encoder

100

Phần F: Phần mềm lập trình.

102

F.1 Giới thiệuTarget For TI C2000

102


F.2 Sử dụng Target Preferences Library

103

F.3 Sử dụng Chip Support Library

104

F.4 Code Composer Studio

105

Phần G: Mã lập trình
G.1 Chương trình Matlab ước lượng tham số của mơ hình

107

G.2 Chương trình CCS chạy độc lập trên DSP dữ cân bằng con lắc

112

Tài liệu tham khảo

126

Lý Lịch trích ngang:

127


Danh mục các hàm, ký hiệu, từ viết tắt
• Ký hiệu
• Từ viết tắt
LQR: Linear-quadratic regulator
Tồn phương tuyến tính.
DSP: Digital signal processing
Bộ xử lý tín hiệu số
CCS: Code Composer Studio
Trình soạn thảo mã lập trình cho DSP.

Danh mục các hình vẽ, đồ thị, bảng biểu


Luận văn thạc sỹ - Phan Vinh Hiếu

Hình 1.1. Sơ đồ khối hệ thống điều khiển bằng máy tính. .....................................01
Hình 1.2. Sơ đồ khối hệ thống điều khiển bằng hệ thống nhúng. ..........................02
Hình 1.6 Sơ đồ khối của hệ con lắc ngược quay....................................................04
Hình 1.7. Cấu trúc con lắc ngược 1 bậc tự do do tác giả chế tạo............................05
Hình 3.1. Hệ trục tọa độ của con lắc

08

Hình 3.2 Nguyên lý nhận dạng hệ thống.. .............................................................13
Hình 3.3. Tín hiệu nhận dạng mẫu. ........................................................................16
Hình 3.4. Mơ hình Simulink kiểm chứng kết quả ước lượng

20

Hình 4.1. Sơ đồ bộ điều khiển tổng quát.................................................................21

Hình 4.2. Trường vector chiếu... .............................................................................22
Hình 4.3. Sơ đồ điều khiển một hệ hở... ................................................................23
Hình 4.4. Điều khiển đặt cực... ..............................................................................23
Hình 4.6. Sơ đồ khối của bộ điều khiển rời rạc.. ...................................................26
Hình 4.7. Sơ đồ bộ điều khiển LQR.

27

Hình 4.8. Mơ hình Simulink mô phỏng bộ điều khiển cân bằng con lắc sử dụng
phương pháp đặt cực..

30

Hình 4.10. Kết quả đáp ứng của bộ điều khiển cân bằng con lắc mô phỏng trên
Simulink bằng phương pháp đặt cực.

31

Hình 4.11. Mơ hình Simulink mô phỏng bộ điều khiển cân bằng con lắc và vị trí
cánh tay bám theo tín hiệu đặt sử dụng phương pháp đặt cực.

32

Hình 4.12. Kết quả đáp ứng của bộ điều khiển cân bằng con lắc và vị trí cánh tay
bám theo tín hiệu đặt mơ phỏng trên Simulink bằng phương pháp đặt cực

33

Hình 4.13. Mơ hình Simulink mơ phỏng bộ điều khiển cân bằng con lắc sử dụng
phương pháp LQR.


34


Luận văn thạc sỹ - Phan Vinh Hiếu

Hình 4.14.Kết quả đáp ứng của bộ điều khiển cân bằng con lắc mơ phỏng trên
35

Simulink bằng phương pháp LQR.

Hình 4.15.Mơ hình Simulink mơ phỏng bộ điều khiển cân bằng và vị trí cánh tay
36

bám theo tín hiệu đặt sử dụng phương pháp LQR.

Hình 4.16.Kết quả đáp ứng của bộ điều khiển cân bằng con lắc và vị trí cánh tay
bám theo tín hiệu đặt mơ phỏng trên Simulink bằng phương pháp LQR.

37

Hình 5.1. Nguyên lý nhận dạng hệ thống................................................................39
Hình 5.2. Chương trình DSP tạo tín hiệu kích và thu thập dữ liệu

40

Hình 5.3 Cấu trúc bên trong của khối Motor Control Signal…………………...40
Hình 5.7. Cấu trúc bên trong của khối Arm Velocity Signal …………………...42
Hình 5.10. Cấu trúc bên trong của khối Pen Position Signal..................................43
Hình 5.12. Cấu trúc bên trong của khối RS232.


44

Hình 5.13. Đồ thị đáp ứng.

45

Hình 5.14. Đồ thị đáp ứng đánh gía kết quả ước lượng.......................................... 49
Hình 6.1. Chương trình điều khiển Lật ngược, Dữ cân bằng con lắc trên DSP sử
dụng phương pháp đặt cực.. ....................................................................................57
Hình 6.2. Sơ đồ khối chương trình của khối Swing Up-State Feeback Controller
.................................................................................................................................58
Hình 6.3. Khối Arm Module.. .................................................................................60
Hình 6.4..Khối Pen Module.

60

Hình 6.5. Khối Motor Module.. .............................................................................61
Hình 6.7. Đồ thị đáp ứng Swing Up của hệ thống.. .

………………………… 62

Hình 6.8. Đồ thị đáp ứng cân bằng của hệ thống với bộ hệ số K1d... ....................63
Hình 6.9. Đồ thị đáp ứng cân bằng của hệ thống với bộ hệ số K2d... ....................64


Luận văn thạc sỹ - Phan Vinh Hiếu

Hình 6.10. Đồ thị đáp ứng cân bằng của hệ thống với bộ hệ số K3d... ................65
Hình 6.11. Chương trình điều khiển Lật ngược, Giữ cân bằng con lắc và điều khiển

vị trí cánh tay trên DSP sử dụng Phương pháp đặt cực.. ........................................66
Hình 6.12. Đồ thị đáp ứng điều khiển vị trí cánh tay của hệ thống. .....................67
Hình 6.13. Chương trình điều khiển Lật ngược, Dữ cân bằng con lắc trên DSP sử
dụng Phương pháp LQR rời rạc.. ............................................................................ 68
Hình 6.14. Đồ thị đáp ứng cân bằng của hệ thống với bộ hệ số K1LQR

69

Hình 6.15. Đồ thị đáp ứng điều khiển vị trí cánh tay của hệ thống bằng Phương
pháp LQR rời rạc.. ....................................................................................... 70


Luận Văn Thạc Sỹ
Học Viên: Phan Vinh Hiếu

GVHD : TS Nguyễn Đức Thành.

CHƯƠNG I:

TỔNG QUAN

1.1 Đặt vấn đề
Hiện nay, việc áp dụng các lý thuyết điều khiển tự động vào thực tế được thực
hiện theo 2 cách:
Cách thứ nhất: là bằng máy tính thơng qua các phần mềm như Matlab, Labview
kết hợp với Card xuất nhập dữ liệu (PCI 17xx Advantech). Chương trình điều khiển sẽ
được lập trình trên các phần mềm, giá trị tín hiệu điều khiển từ chương trình sẽ được các
Card chuyển đổi sang giá trị điện áp và xuất ra mơ hình thực. Cách này có ưu điểm là tiết
kiệm được thời gian thiết kế, cho phép người sử dụng có một cái nhìn trực quan về điều
khiển học, tận dụng được các hàm toán học của phần mềm cũng như khả năng tính tốn

và lưu trữ mạnh của máy tính. Tuy nhiên nó gây ra một số trở ngại, vì người dùng phải
phụ thuộc vào chương trình của máy tính và Card chuyển đổi (giá thành cao, kích thước
lớn). Ngồi ra nó khơng cho phép chúng ta thấy được cấu trúc thực sự của một hệ thống
nên gây khó khăn cho việc triển khai ứng dụng vào thực tế. Cách này chỉ thuận tiện khi
sử dụng trong các phịng thí nghiệm với mục đích nghiên cứu.

MATLAB

CARD
GIAO
TIẾP

u(t)
y(t)

KHUYẾCH ĐẠI
CÔNG SUẤT
BỘ LỌC NHIỄU

HỆ
THỐNG
VẬT LÝ

Hinh 1.1 Sơ đồ khối hệ thống điều khiển bằng máy tính.
Cách thứ hai: là sử dụng hệ thống nhúng: ECP (Embedded Computing Platform),
FPGA/ CPLD (Field-programmable gate array), PSOC (Programmable System-on-Chip),
DSP (Digital signal processing), MCU (Microcontroller Unit) với các ngôn ngữ C/C++,
B#, Ada, Assembly. Chương điều khiển sẽ được lập trình bằng các phần mềm trên máy
tính, sau đó chương trình này sẽ được biên dịch ra mã nhị phân và nạp vào hệ nhúng để
chúng có thể hoạt động độc lập mà khơng cần tới máy tính nữa. Cách này có ưu điểm là

kích thước của bộ điều khiển cuối cùng sẽ nhỏ gọn, hoạt động độc lập, ổn định có tính
năng tự động hoá cao, giá thành rẻ nên dễ dàng triển khai vào thực tế.

-01-


Luận Văn Thạc Sỹ
Học Viên: Phan Vinh Hiếu

GVHD : TS Nguyễn Đức Thành.

u(t)
TRÌNH
BIÊN DỊCH

Rs232
USB

HỆ
THỐNG
NHÚNG

y(t)

KHUYẾCH ĐẠI
CƠNG SUẤT

HỆ
THỐNG
VẬT LÝ


BỘ LỌC NHIỄU

Hinh 1.2 Sơ đồ khối hệ thống điều khiển bằng hệ thống nhúng
Tuy nhiên, việc sử dụng hệ thống nhúng đòi hỏi người sử dụng phải có khả năng
tính tốn, thiết kế phần cứng tốt đồng thời phải nắm vững nguyên lý làm việc của trình
biên dịch để có thể tối ưu khả năng hoạt động của hệ thống. So với máy tính hệ thống
nhúng có khả năng tính tốn ú hơn nhưng với những tiến bộ mới trong lĩnh vực chế tạo
Chip , khả năng và tốc độ tính tốn cua các hệ thống nhúng ngày càng được cải thiện .
Đó là lý do tác giả quyết định thực hiện đề tài “Xây Dựng Bộ Điều Khiển Nhúng
Cho Con Lắc Ngược Quay” trên nền chip DSP TMS320F28335 .

1.2 Tổng quan về đề tài
1.2.1 Xây dựng mơ hình – Thuật tóan điều khiển:
Con lắc ngược là một đối tượng đại diện cho một lớp các đối tượng có độ phi
tuyến cao và không ổn định.

Hinh 1.3 Con lắc quay 2 bậc.Hinh 1.4 Con lắc quay 1 bậc. Hinh 1.5 Con lắc đặt trên xe.
tự do
tự do

-02-


Luận Văn Thạc Sỹ
Học Viên: Phan Vinh Hiếu

GVHD : TS Nguyễn Đức Thành.

Con lắc ngược quay 1 bậc tự do với ưu điểm có hệ thống cơ khí khơng q phức

tạp (so với con lắc ngược đặt trên xe) được sử dụng rộng rãi trong việc giảng dạy và
nghiên cứu lý thuyết điều khiển tự động cũng như xây dựng các bộ điều khiển số.
Con lắc ngược quay 1 bậc tự do bao gồm 3 phần chính: phần cơ khí, điện tử và
chương trình. Phần cơ khí bao gồm một que kim loại (con lắc) quay quanh một trục thẳng
đứng. Que kim loại được gắn gián tiếp vào đầu của một cánh tay nằm ngang thông qua
một encoder để đo góc, đầu cịn lại của cánh tay được gắn vào trục quay của một động cơ
servo DC được đặt thẳng đứng để cánh tay có thể quay trong một mặt phẳng nằm ngang.
Do trong quá trình vận hành cánh tay sẽ quay với tốc độ cao để lấy mẫu nên phần cơ khí
cần phải được tính tốn thiết kế chính xác, chắc chắn nhằm tránh rung gây nhiễu và hư
hỏng trong quá trình vận hành. Phần điện tử: gồm 3 phần nhỏ là: cảm biến đo góc cánh
tay và con lắc, mạch khuyếch đại công suất và mạch điều khiển. Phần cảm biến đóng 1
vai trị hết sức quan trọng, nó sẽ cung cấp cho bộ điều khiển: vị trí, hướng và vận tốc của
cánh tay và con lắc. Có nhiều loại cảm biến được sử dụng để đo góc, trong đề tài này tác
giả sử dụng Bộ mã hóa vịng quay (Rotary Encorder) có độ phân giải cao. Tín hiệu từ Bộ
mã hóa vịng quay sẽ được khuyếch đại, lọc nhiễu rồi kết nối vào module ngoai vi eQEP
(Enhanced Quadrature Encorder Pulse) của DSP. Tùy thuộc vào tín hiệu từ các Bộ mã
hóa vịng quay (Rotary Encorder) mà DSP được lập trình để xuất tín hiệu ngõ ra điều
khiển động cơ DC Servo qua một mạch khuyếch đại công suất. Phần chương trình:
Chương trình điều khiển con lắc tác giả không viết trực tiếp trên Code Composer Studio
mà kết hợp với Matlab2009a thông qua thư viện Target Support Package TC2 để tận
dụng các hàm tính tốn mạnh có sẵn trong Matlab. Cách viết này về lý thuyết sẻ giúp
người sử dụng tiết kiệm được thời gian vì khơng cần phải nắm vững cấu trúc phần cứng
của DSP và tập lệnh C của CCS nhưng thực tế do mã C sinh ra bởi Matlab sẽ không tối
ưu nên để chương trình hoạt động tốt địi hỏi người lập trình phải nắm thật vững cách
lập trình bằng Matlab, cấu trúc mã C do Matlab sinh ra, cấu trúc phần cứng của DSP để
có thể can thiệp và hiệu chỉnh trực tiếp vào mã C này.
Mục tiêu của đề tài là thu thập dữ liệu rồi ước lượng các tham số của con lắc bằng
phương pháp Bình Phương Cực Tiểu sau đó xây dựng luật điều khiển dựng con bằng
phương pháp do K Futara - M Yamakita đề xuất và giữ con lắc cân bằng bằng phương
pháp Điều Khiển Đặt Cực, LQR. Luật điều khiển sẽ được xây dựng trên nền DSP xử lý

dấu chấm động TMS320F28335.

-03-


Luận Văn Thạc Sỹ
Học Viên: Phan Vinh Hiếu

GVHD : TS Nguyễn Đức Thành.

Rs232

Hình 1.6 Sơ đồ khối của hệ con lắc ngược quay.

-04-


Luận Văn Thạc Sỹ
Học Viên: Phan Vinh Hiếu

GVHD : TS Nguyễn Đức Thành.

Cánh tay
Encoder đo góc con
lắc

Cổ góp tín hiệu.

Pulli đai truyền động.


Đơng cơ Servo DC
Con lắc
Encoder đo góc cánh
tay

Mạch điều khiển và
công suất

Hinh 1.7 Cấu trúc con lắc ngược 1 bậc tự do do tác giả chế tạo.

-05-


Luận Văn Thạc Sỹ
Học Viên: Phan Vinh Hiếu

GVHD : TS Nguyễn Đức Thành.

1.2.2 Tóm tắt nội dung các chương:
Nhiệm vụ của Chương 2 là trình bày các cơng việc cần thực hiện để hoàn tất đề tài.
Mục tiêu của Chương 3 là trình bày cách thành lập mơ hình tốn và giới thiệu phương
pháp bình phương cự tiểu để ước lượng các tham số của mơ hình con lắc.
Ở Chương 4, thuật toán Lật ngược và Giữ cân bằng con lắc dựa trên phương pháp tích
lũy năng lượng và đặt cực cũng như LQR sẽ được tác giả trình bày chi tiết.
Chương 5 trình bày quy trình nhận dạng các tham số mơ hình con lắc bằng ngơn ngữ lập
trình Matlab với dữ liệu đáp ứng của con lắc được thu thập về máy tính bằng DSP qua
cổng RS232. Tác giả sẽ trình bày và so sánh các kết quả nhân dạng bằng các phương
pháp khác nhau để tìm ra kết quả tốt nhất.
Tiếp theo, Chương 6 sẽ giới thiệu phần lập trình bộ điều khiển Swing Up và Cân Bằng
con lắc trên nền DSP với các tham số điều khiển khác nhau. Các đồ thị kết quả sẽ được

trình bày chi tiết để có thể phân tích và đánh giá nhằm đạt được kết quả điều khiển tốt
nhất.
Cuối cùng, Chương 7 sẽ trình bày các kết quả đạt được, ưu nhược điểm và hướng phát
triển của đề tài.

-06-


Luận Văn Thạc Sỹ
Học Viên: Phan Vinh Hiếu

CHƯƠNG II:

GVHD : TS Nguyễn Đức Thành.

NHIỆM VỤ LUẬN VĂN

Để thực hiện hoàn tất luận văn thì tác giả phải hồn thành các nhiệm vụ sau:
•

Thiết kế-thi cơng mới tồn bộ hệ thống con lắc ngược quay.

•

Nghiên cứu chip DSP TMS320F28335.

•

Nghiên cứu lập trình C và Matlab cho DSP.


•

Nghiên cứu thiết kế-thi cơng cơng phần mạch điều khiển, cơng suất

•

Tiến hành lập trình và thu thập dữ liệu từ mơ hình thực để ước
lượng.

•

Lập trình Matlab để ước lượng các thơng số của hệ thống từ dữ liệu
được thu thập bằng phương pháp Bình Phương Cực Tiểu.

•

Thiết kế và mơ phỏng bộ điều khiển đặt cực, LQR của hệ vừa nhận
dạng được trên Simulink.

•

Xây dựng bộ điều khiển Dựng Ngược, Đặt Cực Rời Rạc và LQR
trên nền Chip DSP TMS320F28335 nhằm giữ con lắc ổn định ở vị
trí cân bằng hướng lên.

•

Tiến hành chạy thử và phân tích đáp ứng của hệ thống.

•


Xây dựng bộ điều khiển giữ con lắc ổn định ở vị trí cân bằng hướng
lên đồng thời điều khiển thay đổi vị trí của cánh tay theo tín hiệu đặt.

-07-


Luận Văn Thạc Sỹ
Học Viên: Phan Vinh Hiếu

GVHD : TS Nguyễn Đức Thành.

CHƯƠNG III: MƠ

HÌNH HỐ VÀ ƯỚC
LƯỢNG THAM SỐ

3.1. Mơ hình hố hệ thống con lắc ngược quay:
3.1.1. Mơ hình thực tế của hệ thống con lắc ngược quay:

Hình 3.1 Hệ trục tọa độ của con lắc.
Các mơ hình trong chương này dựa trên hệ toạ độ được cho ở Hình 3.1 phía trên.
Vị trí góc quay của cánh tay α được giả thiết là tăng khi cánh tay quay quanh trục z theo
chiều từ trái sang phải. Giá trị của α được xác định so với trục x.
Vị trí góc quay của con lắc(β và θ) được giả thiết là tăng khi con lắc quay quanh 1
trục đi xuyên qua phần cánh tay gắn ở trục chốt quay của con lắc. β và θ có chiều theo
quy tắc bàn tay phải. β tính từ trục thẳng đứng theo chiều hướng lên cịn θ tính từ trục
thằng đứng theo chiều hướng xuống
-8-



Luận Văn Thạc Sỹ
Học Viên: Phan Vinh Hiếu

GVHD : TS Nguyễn Đức Thành.

Bảng tham số của mơ hình con lắc ngược quay:
Kí hiệu

Đơn vị

Định nghĩa

Thơng số thực tế

rad

Góc quay(vị trí) cánh tay quanh trục z(bắt đầu
tính từ trục x dương)

rad

Góc quay(vị trí) của con lắc(bắt đầu tính tính
từ trục thẳng đứng hướng lên)

θ

rad

Góc quay(vị trí) của con lắc(bắt đầu tính từ

trục thẳng đứng hướng xuống )

J0

kg-m2

Momen quán tính phần cánh tay

Ước lượng

J1

kg-m2

Momen quán tính phần con lắc

Ước lượng

C0

kg-m2/s

Hệ số ma sát trượt phần cánh tay

Ước lượng

C1

kg-m2/s


Hệ số ma sát trượt phần con lắc

Ước lượng

m1

kg

Khối lượng con lắc

0.0343

l1

m

Chiều dài con lắc

0.1883

L0

m

Chiều dài cánh tay

0.1735

Kt


N-m/A

Hằng số momen quay động cơ

0.081

Kb

V-s/rad

Hằng số sức điện động cảm ứng động cơ

0.0812

Ku

V/count

Hệ số khuếch đại tín hiệu điều khiển động cơ

0.03

Ra

Ω

Điện trở cuộn Rotor của động cơ

0.92


g

m/s2

Gia tốc trọng trường(≈9.81m/s2)

9.81

u

counts

Tín hiệu điều khiển động cơ

α
β

-9-


Luận Văn Thạc Sỹ
Học Viên: Phan Vinh Hiếu

GVHD : TS Nguyễn Đức Thành.

3.1.2. Mơ hình tốn học của hệ thống con lắc ngược quay:
3.1.2.1. Hệ phương trình phi tuyến:
Mơ hình động học phi tuyến diễn tả thực tế nhất một hệ thống. Nó cho ta mối quan
hệ chính xác của các phần tử liên quan trong hệ thống. Tất cả các mơ hình tuyến tính
dùng cho thiết kế bộ điều khiển đều được thành lập dựa trên mơ hình phi tuyến.

Ta có (Phụ lục A), mơ hình động học phi tuyến của con lắc ở vị trí thẳng đứng
hướng lên:
⎡ J 0 + m1 L20 + m1l12 sin 2 β − m1 L0l1 cos β ⎤ ⎡α ⎤
⎢
⎥ ⎢ ⎥ +
2
m
L
l
cos
J
m
l
+
θ
1 0 1
1
11
⎣
⎦ ⎣β ⎦
Kt Kb 1 2 
1 2
⎡
⎤

⎢C0 + R + 2 m1l1 β sin 2 β m1 L0l1β sin β + 2 m1l1 α sin 2β ⎥ ⎡α ⎤
a
⎢
⎥⎢ ⎥
⎢

⎥ ⎣ β ⎦
1 2

m
l
sin
2
C
α
β
−
11
1
⎢⎣
⎥⎦
2
⎡ Kt Kb ⎤
0
⎡
⎤ ⎢
+⎢
= Ra ⎥ u
⎥
⎣ −m1 gl1 sin β ⎦ ⎢⎢ 0 ⎥⎥
⎣
⎦

(3.1)

Mơ hình động học phi tuyến của con lắc ở vị trí thẳng đứng hướng xuống:


⎡ J 0 + m1 L20 + m1l12 sin 2 θ m1 L0l1 cos θ ⎤ ⎡α⎤
+
⎢
2 ⎥ ⎢ ⎥
θ
+
θ
m
L
l
cos
J
m
l
1 01
1
11 ⎦⎣ ⎦
⎣
Kt Kb 1 2 
1 2
⎡
⎤

⎢C0 + R + 2 m1l1 θ sin 2θ −m1 L0l1θ sin θ + 2 m1l1 α sin 2θ ⎥ ⎡α ⎤
a
⎢
⎥⎢ ⎥
⎢
⎥ ⎣θ ⎦

1 2

−
α
θ
m
l
sin
2
C
11
1
⎢⎣
⎥⎦
2
⎡ Kt Kb ⎤
0
⎡
⎤ ⎢
+⎢
= Ra ⎥ u
⎥
⎣ m1 gl1 sin θ ⎦ ⎢⎢ 0 ⎥⎥
⎣
⎦

- 10 -

(3.2)



Luận Văn Thạc Sỹ
Học Viên: Phan Vinh Hiếu

GVHD : TS Nguyễn Đức Thành.

3.1.2.2. Hệ phương trình tuyến tính (tuyến tính hoá quanh điểm dừng):
Trong luận văn này, tác giả xét mơ hình theo phuơng trình động học phi tuyến (3.1).
Mơ hình động học tuyến tính hóa được dẫn xuất trên
cơ sở tuyến. tính hóa mơ hình động
.
học phi tuyến ở vị trí hướng lên trên quanh điểm α = 0, β = 0, β = 0 (Phụ lục B) là:
⎡ J 0 + m1 L20
⎢
⎣ −m1L0l1

Kt Kb
⎡
−m1L0l1 ⎤ ⎡α ⎤ ⎢C0 +
Ra
⎥⎢ ⎥+
J1 + m1l12 ⎦ ⎣ β⎦ ⎢
⎢⎣
0

⎤
⎡K K ⎤
0 ⎥ ⎡α ⎤ ⎡0
0 ⎤ ⎡α ⎤ ⎢ t b ⎥
+

= R
u
⎥ ⎣⎢ β ⎦⎥ ⎢0 −m1 gl1 ⎥ ⎣⎢ β ⎦⎥ ⎢ a ⎥
⎣
⎦
C1 ⎥⎦
⎣⎢ 0 ⎦⎥

Ta đặt: a = J 0 + m1 L20 ; b = m1l12 ; c = m1L0l1 ; e =

Kt K a
KK
; d = C0 + t a ; f = J1 + m1l12 (3.4)
Ra
Ra

⎧ aα − cβ + dα − eu = 0
Lúc này : (2) ⇔ ⎨


⎩−cα + f β + C1β − hβ = 0

•

(3.5)

Để lập cơng thức mơ hình tuyến tính 3 trạng thái, ta định nghĩa biến trạng thái là
⎡α ⎤
x3 = ⎢⎢ β ⎥⎥ thì (2) ⇔ x3 = A3 x3 + B3u với :
⎢⎣ β ⎥⎦

⎡ − df
1 ⎢
A3 =
0
af − c 2 ⎢
⎢⎣ −cd

•

(3.3)

−cC1 ⎤
0 af − c 2 ⎥⎥ ;
ah − aC1 ⎥⎦
ch

⎡ef ⎤
1 ⎢ ⎥
B3 =
0
af − c 2 ⎢ ⎥
⎢⎣ ce ⎥⎦

(3.6)

Để lập cơng thức mơ hình tuyến tính 4 trạng thái, ta định nghĩa biến trạng thái là
⎡α ⎤
⎢α ⎥
x4 = ⎢ ⎥ thì (2) ⇔ x4 = A4 x4 + B4u với:
(3.7)

⎢β ⎥
⎢ ⎥
⎣β ⎦
⎡0 1
⎢0
A4 = ⎢
⎢0
⎢
⎣0

0
A3

0⎤
⎡0⎤
⎥
⎢ ⎥
⎥;B = ⎢ ⎥
⎥ 4 ⎢ B3 ⎥
⎢ ⎥
⎥
⎣ ⎦
⎦

(3.8)

- 11 -


Luận Văn Thạc Sỹ

Học Viên: Phan Vinh Hiếu
⎡ − df
1 ⎢
A3 =
0
af − c 2 ⎢
⎣⎢ −cd

và

GVHD : TS Nguyễn Đức Thành.

−cC1 ⎤
0 af − c 2 ⎥⎥ ;
ah − aC1 ⎦⎥
ch

⎡ef ⎤
1 ⎢ ⎥
B3 =
0
af − c 2 ⎢ ⎥
⎣⎢ ce ⎦⎥

(3.9)

Từ hệ phương trình tuyến tính thành lập được ở trên, ta có tiếp các mơ hình sau:
• Mơ hình khơng gian trạng thái liên tục
• Mơ hình không gian trạng thái rời rạc
Trong khuôn khổ luận văn này, tác giả sử dụng mơ hình khơng gian trạng thái rời rạc vì

tác giả đang sử dụng kĩ thuật điều khiển số dùng DSP.
• Mơ hình trạng thái liên tục: đã được trình bày ở trên
•

Mơ hình trạng thái rời rạc:

Ta tiếp tục đặt y=Cx (với C là ma trận ngõ ra tuỳ chọn), kết hợp với mơ hình trạng
thái liên tục đã tính sẽ rút ra được phương trình trạng thái rời rạc
Nếu ta đặt u(k)=u(k-1)+∆u(k) thì
⇔ x(k + 1) = Ad x(k ) + Bd u (k − 1) + Bd Δu (k )

(3.12)

⎡ x( k ) ⎤
⎥ . Phương trình trạng thái được viết như
⎣u (k − 1) ⎦

Định nghĩa biến trạng thái: xi (k ) = ⎢

⎧ xi ( k + 1) = Adi xi (k ) + Bdi Δu ( k )
y (k ) = Cdi xi ( k )
⎩

(3.13)

sau: ⎨

⎡A

với Adi = ⎢ d

⎣0

Bd ⎤
⎡B ⎤
; Bdi = ⎢ d ⎥ ; Cdi = [Cd
⎥
1⎦
⎣1⎦

0]

(3.14)

Phương trình trạng thái sử dụng một biến trạng thái bên ngồi là u(k-1). Việc phân
tích biến trạng thái này cho ta thấy nó tác động như một khâu tích phân lên tất cả các biến
trạng thái khác. Việc thêm khâu tích phân này sẽ đảm bảo tính đuổi bám của hệ

- 12 -


Luận Văn Thạc Sỹ
Học Viên: Phan Vinh Hiếu

GVHD : TS Nguyễn Đức Thành.

3.2 Ước lượng thông số hệ thống:
3.2.1.Giới thiệu:
Để xây dựng được một bộ điều khiển hiệu quả chúng ta cần có một mơ hình tốn
chính xác của hệ thống, để có một mơ hình tốn chính xác ta cần biết chính xác các thơng
số của hệ thống như khối lượng , chiều dài, hệ số ma sát, moment quán tính, các hằng số

của động cơ DC, v.v…..Tuy nhiên, vì 1 lý do nào đó chúng ta khơng thể xác định chính
xác một vài hay tồn bộ các thơng số này nên ta phải tiếng hành ước lượng.
Trong phần này, tác giả xin giới thiệu phương pháp ước lượng thơng số. Để có thể
ước lượng thơng số, ta kích thích hệ thống với tín hiệu thử đã biết và thu thập dữ liệu đáp
ứng của hệ thống.

Đánh giá kết quả

Hình 3.2 Ngun lý nhận dạng hệ thống.
3.2.2 Mơ hình toán học dùng để ước lượng:
Để ước lượng tham số, hệ thống vật lý phải được kích thích bằng một tín hiệu thử
đã biết. Vị trí bắt đầu là vị trí con lắc quay xuống phía dưới. Đáp ứng ngõ ra hệ thống vật
lý đối với tín hiệu thử tuân theo các định luật động lực học (được mô tả bằng mơ hình phi
tuyến tương ứng với vị trí cân bằng là con lắc hướng xuống dưới). Do đó, sẽ tiện hơn cho
chúng ta nếu xét mơ hình mà điểm dừng là vị trí con lắc hướng xuống để ước lượng tham
số:

- 13 -


Luận Văn Thạc Sỹ
Học Viên: Phan Vinh Hiếu

GVHD : TS Nguyễn Đức Thành.

⎡ J 0 + m1 L20 + m1l12 sin 2 θ m1 L0l1 cos θ ⎤ ⎡α ⎤
⎢
⎥⎢ ⎥+
m1 L0l1 cos θ
J1 + m1l12 ⎦ ⎣θ⎦

⎣
Kt Kb 1 2 
1 2
⎡
⎤

⎢C0 + R + 2 m1l1 θ sin 2θ − m1 L0l1θ sin θ + 2 m1l1 α sin 2θ ⎥ ⎡α ⎤
a
⎢
⎥⎢ ⎥
⎢
⎥ ⎣θ ⎦
1 2

m
l
sin
2
C
−
α
θ
11
1
⎢⎣
⎥⎦
2
⎡ Kt Kb ⎤
0
⎡

⎤ ⎢
+⎢
= Ra ⎥ u
⎥
⎣ m1 gl1 sin θ ⎦ ⎢⎢ 0 ⎥⎥
⎣
⎦

(3.15)

3.2.3 Xử lý tín hiệu nhận dạng:
Dữ liệu được thu thập để ước lượng tham số là:
α : vận tốc cánh tay

θ :vị trí con lắc

u :tín hiệu lái động cơ

Tín hiệu thơ thu được thường khơng chuẩn và bị nhiễu rất nhiều. Hơn nữa,việc lấy mẫu
và lưu giữ dữ liệu sẽ tạo đã bị lỗi do bị cắt xén. Sau khi chuyển về dạng chuẩn, dữ liệu
phải được lọc trước khi đem đi nhận dạng hệ thống. Bộ lọc được sử dụng là một bộ lọc số
đáp ứng xung vơ hạn (IIR) có hàm truyền đạt:

H ( z) =

0.15
1 − 0.85 z −1

(3.16)


Đáp ứng biên độ H (e jω ) của bộ lọc là:

0.15
1 − 0.85e − jω
0.15
=
1 − 0.85cos ω + j 0.85sin ω
0.15
| H (e jω ) |=
(1 − 0.85cos ω )2 + (0.85sin ω ) 2
H (e jω ) =

=

(3.17)

0.15
1 − 1.7 cos ω + 0.852

Khi sử dụng bộ lọc kiểu IIR, tín hiệu đã được lọc sẽ bị méo pha. Do đó, khi thực
hiện bộ lọc này, tín hiệu đã được lọc phải được đảo chiều trong chuỗi tín hiệu và tiếp tục
được đưa vào bộ lọc lần thứ 2. Bằng cách này, tất cả các thành phần tần số đi qua bộ lọc
2 lần, mỗi lần theo 1 hướng khác nhau. Sự trễ của mỗi thành phần riêng rẽ này là như
- 14 -