ĐẠI HỢC QUÓC GIA HÀ NỘI
TRƯỜNG ĐẠI HỌC CÔNG NGHỆ
Phạm Duy Hưng
N G H I Ê N C Ú Ư T H Ủ N G H I Ệ M M Ộ T H Ệ Đ I È U K H I Ẻ N
• • •
T Ụ C Â N B Ằ N G T R Ê N c o S Ỏ s ử D Ụ N G C Ả M B I Ế N
G I A T Ố C V À C Ả M B I É N V Ậ N T Ó C G Ó C
Ngành: Điện tử-Viễn thông
Chuyèn ngành: Kỳ thuật vô tuyến điện tử và thông tin liên lạc
Mã số: 2.07.00
LUẬN VÃN THẠC SỸ
NGƯỜI HƯỚNG DÀN KI IOA HỌC
TS. Trần Quang Vinh
HÀ NỘI - 2006
MỤC LỤC
LỜI CẢM ƠN 1
LỜI CAM ĐOAN 2
DANH MỤC HÌNH VẼ, ĐÒ THỊ 5
MỞ ĐÂU 8
Chương 1 - LÝ THUYẾT HỆ ĐIÊU KHIẾN Tự CÂN BẲNG

10
L I MÔ HÌNH HỆ CON LẮC NGƯỢC 11
1.1.1 Xây dựng phương trình toán
11
1.1.2 Thiết kế mô hình không gian trạng thái cho Hệ con lắc ngược

14
1.2. MỎ HÌNH XE T ự CÂN BẦNG TRÊN HAI BẢNH.'.
IX
1.3. TÍNH ĐIÊU KHIÊN Đ ư ợ c CỦA HỆ THÔNG 22

55
4.2.1 Các đối tượng cùa mô hình:
55
4.2.2 Thiết lập thuộc tính đối tượng 56
4.2.3 Thêm tham số điều khiển và đo lường 57
4.2.4 Truyền tham số giữa VisualNastran và MatLab 57
4. 3. THỬ NGHIỆM XÂY DỰNG XE Tự CÂN BẰNG TRÊN HAI BÁNH

59
4.3.1 Kết cấu cơ khí của xe
59
4.3.2 Thực thi phần cứng và phần mềm điều khiển 60
KẾT LUẬN 64
DANH MỤC CÁC CÔNG TRÌNH CỦA TÁC GIÀ ĐẢ CỔNG Bố 65
TÀI LIỆU THAM KHẢO 66
PHỤ LỤC 69
Phụ lục 1: Chương trình điều khiển viết trên chip PSoC 69
Phụ lục 2: Chương trình Matlab khảo sát thuật toán điều khiển LQR

76
Phụ lục 3: Chương trình Matlab mô phỏng thuật toán Kalman
78
Phụ lục 4: Chương trình chỉ thị góc nghiêng roll-pitch trên máy tính

80
4
DANH MỤC HÌNH VẼ, ĐÒ THỊ
Hình 1. Mô hình xe hai bánh tự cân bằng
Hình 2. Mô hình con lắc ngược được gẳn trên đế chuyển động được bằng hai bánh
Hình 3. Các lực tác dụng lên hệ con lẳc ngược

Hình 24. Sơ đồ khối đo vận tốc góc sử dụng cảm bién ADXR S150
Hình 25. đồ thị quan hệ giữa vận tốc góc và giá trị số thu được từ bộ biến đổi ADC
trong
2
trường hợp: quay theo chiều thuận (hình trái); quay theo chiều ngược(hình
phải)
Iỉình 26. a) Bàn xoay có gắn các vạch đen trắng
b) Sơ đồ khối Môđun đo vận tốc góc cùa bàn xoay
Hình 27. Ý tưởng thiết kế bộ lọc bù
Hình 28. Sơ đồ khối bộ lọc bù.
Hình 29. Giản đồ bode cùa hệ thống ước lượng với hằng số thời gian khác nhau.
Hình 30. Đáp ứng tần số và đáp ứng pha của bộ lọc thông ihap tại nhánh có cảm
biến góc nghiêng
Hình 31.Đáp ứng tần số và đáp ứng pha cùa bộ lọc thông cao nhánh có cảm biến
vận tốc góc
Minh 32. Tổ hợp của hai nhánh tới lối ra bộ lọc. Đường liên tục là lối ra bộ lọc và
dường đứt nét là tín hiệu các nhánh.
Hình 33. Thuật toán lọc Kalman.
Hình 34. Sơ đồ thực hiện thuật toán Kalman.
Hình 35 Bộ lọc Kalman.
Hình 36. Sơ dồ khối hệ thống khảo sát trong ví dụ
6
1 Hnh 38 Giao diện chương trình VisualNastran 4D
Hình 39. Mô hình xe hai bánh tự cân bằng
Hình 40. Cửa sổ thuộc tính cùa đối tượng w h ee ljeft
Hình 41. Khối vNPlant thực hiện truyền tham số giữa VisualNastran và MatLab
Hình 42. Sơ đồ thực thi thuật thuật toán điều khiển xe hai bánh tự cân bằng được
thiết
Hình 43. Đồ thị quan hệ giữa góc nghiêng, vận tốc góc của xe hai bánh theo thời
gian

cân bang trên hai bánh sừ dụng cảm biến gia tốc và cảm biến vận tốc góc chế tạo
MỞ ĐẦU
8
theo công nghệ MEM S, tác già hy vọng sẽ tiếp tục phát triển đề tài tiến dần đến việc
chế tạo một robot đi bộ tự giữ thăng bằng trên hai chân khi có một bộ xử lý mạnh
hơn, đủ đáp ứng thực hiện các thuật toán đòi hòi độ phức tạp cao hơn.
Vi thời gian và khả năng có hạn, bản luận văn không tránh khỏi những thiếu
sót. Tác giả rất mong nhận được sự góp ý của các thầy cô giáo, bè bạn, đồng nghiệp
và những ai đang quan tâm tới đề tài này.
9
Chương 1
LÝ THUYÉT HỆ ĐIÈU KHIẺN TỤ CÂN BẦNG
Hứa hẹn nhiều ứng dụng rộng rãi trong tương lai, Hệ thống điều khiển tự cân
bang từ đon giản (con lắc ngược ) đến phức tạp (hệ thống xe hai bánh tự cân bằng,
robot đi bộ bằng hai chân, ) đã và đang được nghiên cứu, triển khai tại nhiều
phòng thí nghiệm trên thế giới [19, 20, 21]. Với mục đích nghiên cứu, thừ nghiệm
một hệ điều khiển tự cân bằng, chương này đề cập lý thuyết điều khiển mô hình xe
hai bánh tự cân bằng được chỉ ra trong hình
1
với bản chất là một con lắc ngược.
Hình I. Mô hình xe hai bánh tự cân bằng
Đây là mô hình gồm 3 bậc tự do.
- Xe có thể quay quanh trục z (pitch) tạo góc nghiêng 0p so với phương thẳng
đứng tương ứng với vận tốc góc cop.
- Chuyển động thẳng theo trục X rm với vận tốc góc VRM
- Quay quanh trục thẳng đứng (yaw) với góc ô.
Việc xây dựng các phương trình toán và các tham sổ điều khiển liên quan sẽ
được làm sang tỏ ngay sau đây.
10
1.1 M Ô HÌNH HỆ CO N LẮ C NGƯ Ợ C

Mx+bx+N = F (1)
2
N = m x + m lG c o s0-m lớ s i n ớ (2)
Lấy (2) trừ (1) ta thu được :
2
(M + m )x + b x + m l0 c o s0 -m lO sin ớ = F (3)
Theo phương vuông góc, các lực tác dụng lên con lắc được chi ra trong
phương trình (4):
p s in ớ + N c o s ỡ - m g s in ớ = m ỉO + m :ccosớ (4)
Tổng mômen xung quanh trọng tâm của con lắc thoả mãn phương trình (5)
- p /s in ớ - yv/co sớ = 1 0 (5)
Biến đổi phương trình (4) và (5) ta thu được:
( / + ml2)ỡ+ mglsinỡ = -ml X CO S0 (6 )
Như vậy, đối với mô hình Hệ con lắc ngược được chi ra ờ trên ta có thể mô tả
bằng hệ hai phương trình (3) và (
6
) như sau:
( (M + m )x + b x + mlOcosG - m I0
2
sin0 = F
, (7)
!• (/ + ml2)0+ mgl sin ỡ = -mi xcosớ
12
Khi góc nghiêng 0 nhỏ (0<1O°), ta có thể tuyến tính hoá hệ như sau:
(M + m)x+bx+ mi 6 = u
(/ + ml2)ỡ+ mglỡ = —mi X
Với Ư là lối vào cùa hệ
Thực hiện biến đổi Laplace ta thu được hàm truyền của hệ thống như sau:
I (M + m)X(s)sĩ + bX(s)s + ml0(s).s2 = ư(s)
(/ + ml2)ỡ(s)s2 + mglO(s) = -mlX(s)s2

mỉ
(13)
U(s) 3 bụ + ml )s (M + m)mgls bmgl
s “f“ ’ — ——————— —
Q q q
Hệ thống còn có thể được biểu diễn thông qua Hệ phương trình không gian
trạng thái:
13
<
-
X
X
0
0
- ụ + mT)h
I(M + m) + Mml2
0 0
- mlb
0
0
m-gT
Ỉ(M + m) + MmV
0
mgl(M + m)
X
0
ụ + ml2)
X
e
[ớ

- Giới hạn (overshoot) nghiêng cùa góc 0 nhò hơn 20°(0.35 radian).
- Lỗi trạng thái ổn định 2 %
Luật phản hồi K có thể được xác định thông qua hàm lqr(A, D, Q, R) (bộ điều
chỉnh toàn phương tuyến tính-linear lỊuađratic regulator) cùa M atlab. Hàm này cho
phép lựa chọn hai tham số Q và R (tham số sẽ bù lối vào R) nhằm thực hiện tối ưu.
14
Trường hợp đơn giàn giả sừ R =l, cấu trúc cùa Q có thể được xác định như
sau Q = C ’ *C:
Q =
X 0 0 0
0 0 0 0
0 0 ớ 0
0 0 0 0
Phần từ ở vị trí 1,1 trong ma trận Q biểu diễn vị trí cùa xe còn phần từ tại vị
trí 3,3 biểu diễn cho góc nghiêng cùa con lắc. Từ các tham số bài toán cho trước ta
dễ dàng tìm ra A, B, Q, R. Như vậy, việc tìm ra K là hết sức đơn giàn.
Già sừ Hệ con lắc ngược có các thông số sau đây:
- Khối lượng xe: M = 0.5
- Khối lượng cùa con lắc: m= 0.2
- Hệ số ma sát cùa xe đối với mặt sàn:b= 0.1
- Hệ số quán tính cùa con lắc: i= 0.006
- Gia tốc trọng trường: g= 9.8
- Chiều dài của con lắc: 1= 0.3
Thực hiện thuật toán LQR cho con lắc ngược được mô tả trên đây thu được dồ
thị quan hệ giữa vị trí của xe, góc nghiêng cùa con lác biến đổi theo thời gian (hình 5).
Hình 5. Đồ thị biểu diễn quan hệ giữa vị tri của xe, góc nghiên cùa con lắc theo
thời gian dưới tác dụng của xung nháy bậc ử lối vào
15
Nhìn vào đồ thị ta thấy: góc nghiêng của con lắc đã ổn định theo thời gian.
Tuy nhiên, cần phải cải thiện quá trình quá độ để đáp ứng được tốt hơn nữa. Trong

V
) 1 2 3 4 5 E
stephfwt 020X1
-ns * 1 1 • 1 ns
06
£ OX
c
0
[~ Mrtuiịl Advance
(j ptt Sepjratfty
1 02
ß Refereice Irpui
>
0
Sydetii
Time (see)
Inverted Pendulum Animation
•0.6 -CU -0.2 0 0.2 0.4 06
X Position (m)
Hình 9. Sự tương quan giữa đáp ứng hệ vật lý với đồ thị
17
>ẠI HỌC QUÔC Gia NÒi '
IVUNG TÂM THÕNG TIN THU v :. M
V - l o / 0 £>
1.2. MÔ HÌNH XE T ự C Â N BẢNG T RÊN HAI BÁNH
Xem xét mô hình xe hai bánh tự cân bằng [6] được chỉ ra trong hình 1 gồm 3
bậc tự do với 6 biến trạng thái mô tà động học: lực tác dụng vào khối tâm của xe fdp,
lực tác dụng vào tâm của bánh trái fdRL, lực tác dụng vào tâm bánh phải fjRR, góc
rung động 0J, mômen xoắn của Cl, Cr tương ứng của hai bánh xe [19]
Phân tích động lực học [6, 12] của của mô hình được chỉ ra trong hình 10.

19
Lấy (15) trừ (21) sau đó tuyến tính hoá kết quả xung quanh điểm hoạt động
(X[<M=0, 0p=o, 5=0). phương trình không gian trạng thái cùa hệ thống có thể được
viết dưới dạng ma trận như sau:
XlỉM
0 ]
0 0 0 0
xm
0 0 0 0 0
c
VlM
0
0
An
0 0 0
VKW *2, Bn
B»
^ L
c „
0
0
0 1 0 0
e F 0
0 0
0 0
R
Ỡ r
—
0 0
•^43

65
được định nghĩa như là các tham sổ của vận tốc.
Các thông số lực phân bố trên bánh trái, bánh phải và thân xe fdRL, fdRR> f'dP
thường không thể sử dụng để điều khiển hệ thống. Thông số hay dùng để điều khiển
là CL, CR (mô men xoẳn cùa môtơ trái và m ôtơ p h ả i) [20, 21, 22].
Đổ thuận tiện trong quá trình thực hiện điều khiển ta có thể sử dụng mômen
xoắn quay quanh trục thẳng đứng Cg và m ômen xoắn quay quanh trục z c 0. Công
thức chuyển đổi giữa mômen của động cơ bánh và hai mômen này được chỉ ra như
sau:
CL Du
Da
c .
CK
Dĩt d 22
c ,
(23)
► CR
H ình II . Sơ đồ thực hiện chuyển đỏ i giữa hai loại m ôm en xo ắn
20
Với việc bò qua các phân bố lực, phương trình (22) có thể viết ]ại như sau:
+
0 0
b2
b2
0 0
c,
b4
c*
0
0

D =
0.5 0.5
0.5 -0.5
Với giả thiết này ta có thể tìm Ba, Bb, Bc như sau:
Ba=B2, Bị,=B4, Bc=B6.
Tới đây ta có thể tách rời hai thành hai bài toán điều khiển như sau:
Hệ con lấc ngược chuyển động quay quanh trục z (phương trình 26)
Hệ chuyển động quay quanh trục y (phương trình 27)
2 1
X HM
0 1
0 0
XHM 0
X RM
0 0
0
X RK1
* 2
Ỡr
0 0
0 1
0„
T
0
Or
0 0
^43
0
Ỡ,
£ 4

riêng X của ma trận A. X được tìm thông qua giải phương trình sau:
\XI -A \ = Q (29)
Nếu một trong các giá trị riêng x> 0 thì hệ điều khiển không ổn định.
Ngoài tính ổn định, khi thiết kể hệ thống, người thiết kế không thể không
tính đến khả năng điều khiển được cùa hệ thống. Một hộ chỉ điều khiển được nếu
ma trận điều khiển được P=[A AB A 2B A 3B

A n''Bj phải cỏ hạng n.
Hạng cùa p được xác định như một định thức khác 0 lớn nhất hình thành từ
p. Neu p là ma trận vuông thì định thức lớn nhât là |P|, ngược lại, nếu p không phải
ma trận vuông, định thức lớn nhất được hình thành bằng cách thêm các phần tử
0
vào hàng hoặc cột để tạo thành ma trận vuông |PC
cxl'
2 2
M atlab cung cấp lệnh Rank(P) giúp ta tìm hạng ma trận p nhanh gọn và
thuận tiện. Ngoài ra, M atlab còn cung cấp m ột công cụ để xây dựng m a trận p thông
qua lệnh sau: p = ctrb(A ,B ) [3].
N hư vậy, để kiểm tra tính điều khiển được cùa hệ thống được mô tả bằng hệ
phương trình trạng thái (28) ta chỉ cần thực hiện cặp lệnh sau trong M atlab với các
ma trận A, B cho trước:
>>P=ctrb(A,B);
> >n=Rank(P)
Độ m ạnh yếu cùa được đánh giá qua số điều khiển của p thực hiện thông qua
hàm M atlab cond(P), bản chất là khoảng cách từ p đến vị trí điều khiển được.
K hoảng cách này càng nhỏ thì khả năng điều khiển được càng cao.
23
Chương 2
Việc ứng dụng các hệ thống vi cơ điện (Micro Electro M echanical System)
dã tạo ra sự thay đổi lớn trong công nghệ chế tạo các cảm biến quán tính [

KX * r '
Do đó, chúng ta thu được a = — -Như vậy, đê đo gia tôc ta chỉ cân đo khoáng
dịch chuy ển X.
Có nhiều phương pháp khác nhau được sử dụng để đo khoảng dịch chuyển
này. T rong các loại cảm biến vi cơ người ta thường sử dụng thuộc tính điện cùa tụ
điện để thực hiện phép đo khoảng dịch chuyển. X ét hai bản tụ song song, khoảng
cách giữa hai bản tụ có thể thay đổi được (hình 13)
Hình 13. Mó hình một tụ điện đơn (bên trải) và hai tụ nối tiếp nhau (bên phài).
của m ôi trường nằm giữa hai bản tụ. Nếu biết k, điện dung cùa tụ điện c ta có thể
tính được x0. C ũng trong hình 13, nếu bản tụ nằm giữa CA và Cb dịch chuyển một
khoảng là X thì
m
c* Ch
k ' '
Điện dung của tụ điện đom là c=— , với k là hăng sô phụ thuộc vào thuộc tinh
k
k
(30)
x-x0
hay có thể viết lại là:
*0 -X
(31)
do đỏ:
A C = CA-CH=Cx0
(32)
x0 + X x0 -X
Với k h o ả n g X d ịc h c h u y ển n hỏ , p h ươn g trìn h trê n c ó thể rút gọ n thành:
25
AC « C — (33)
*0