Nghiên cứu thử nghiệm một hệ điều khiển tự cân bằng trên cơ sở sử dụng cảm biến gia tốc và cảm biến vận tốc góc

ĐẠ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

Chương 2 - CẢM BIẾN VI cơ ĐIỆN TỪ 24
2.1. CẢM BI ÉN GIA TÔC ADXL202: 24
2.1.1 Nguyên tắc đo gia tốc: 24
2.1.2 Đặc trưng cùa cảm biến ADXL202 26
2.1.3 Một sổ khảo sát trên ADXL202 27
2.2. CON QUAY VI c o A DXRSI50 30
2.2.1 Đặc trưng cùa Gyroscope ADXRS150 31
2.2.2 Một sổ khảo sát trên cảm biến ADXRS150 34
Chương 3 - x ử LÝ TÍN HIỆU TỪ CẢM BIẾN 37
3. ỉ. THỈÉT KÉ B ộ LỌC BÙ ĐỂ x ử LÝ TÍN HIỆU GÓC NGHIÊNG 37
3.1.1 Thiết kế bộ lọc với cảm biến lý tưởng 39
3.1.2 Thiết kế các bộ lọc với động lực học cảm biến gia tốc: 40
3. 2. B ộ LỌC THÍCH NGHI KALMAN 43
3.2.1 Bản chất tính toán của bộ lọc: 43
3.2.2 Bàn chất thống kê của bộ lọc 45
3
3.2.3 Khảo sát thuật toán Kalman thông qua ngôn ngừ Matlab

47
3.2.4 Thuật toán Kalinan xử lý tín hiệu góc nghiêng
50
3.3. ĐẢNH GIẢ Bộ LỌC BÙ VÀ Bộ LỌC KALMAN.

52
Chương 4 - MÔ PHỎNG VÀ THỪ NGHIỆM MÔ HÌNH XE HAI BÁNH Tự CÂN
BẲNG 53
4. ỉ. GIỚI THIỆU PHÀN MÉM MÒ PHỎNG 3D: V1SUALNASTRAN DESKTOP
53
4.2 THIẾT KÉ MÔ HÌNH XE HAI BẢNH Tự CẦN BẰNG

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 4. Sơ đồ điều khiển phản hồi vòng kín
Hình 5. Đồ thị biểu diễn quan hệ giữa vị trí 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
Hình 6. Đáp ứng điều khiển LQR với các giá trị khởi đầu lớn
Hình 7. Sơ đồ điều khiển phàn hồi với lối vào tham chiếu
Hình 8. Đáp ứng thời gian với lối vào tham chiếu
Hình 9. Sụ tương quan giữa đáp ứng hệ vật lý với đồ thị
Hình 10. Phân tích lực và mômen cùa mô hình xe tự cân bằng trên hai bánh
Hình 11. Sơ đồ thực hiện chuyển đổi giữa hai loại mômen xoắn
Hình 12. Hệ khối lượng - lò xo được sử dụng để đo gia tốc
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).
Hình 14. Cấu trúc của cảm biến gia tốc ADXL202.
Hình 15. Hình chiếu cùa véctơ gia tổc trọng trường lên hai trục cảm biến gia tổc sử
dụng đổ đo góc nghiêng.
Hình 16. Tín hiệu độ rộng xung tại lối ra số của cảm biến gia tốc.
Hình 17. Đồ thị biểu diễn sự phụ thuộc độ rộng xung và góc nghiêng
Hình 18. Môđun đo góc nghiêng cùa vật thể chuyển động:
a) Môđun đo gắn trên vật thể chuyển động.
b) Giao diện đồ hoạ của phần mềm chi thị góc nghiêng roll và pitch.
Hình 19. Hệ điều khiển thực tại ảo cho tay máy 5 bậc tự do sử dụng cảm biến
ADXL202:
a) Mô hình bài toán điều khiển thực tại ảo
5
b) Giao diện đồ hoạ cùa phần mềm chi thị tại trạm điều khiển
c) Ảnh chụp người điều khiển ra lệnh cho Robot bàng tay (cảm biển được
gắn trên tay người điều khiển).
Mình 20. Ảnh chụp cảm biến ADX RS150.
Iỉình 21. Tín hiệu lối ra RATEOUT tăng theo chiều quay thuận chiều kim đồng hồ.
Iỉình 22 Cấu trúc của AD XRS150.
Hình 23. Sơ đồ đấu nối AD X RS150 với các linh kiện phụ

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

Hình 45. Sơ dồ khối bộ điều khiển xe tự cân bằng trên hai bánh
Hình 46. Thiết lập tài nguyên người dùng bên trong chip PSoC
Hỉnh 47. Sơ đồ nguyên lý mạch điều khiển xe cân bằng trên hai bánh
I lình 37. Ket quà thực hiện chương trình(m file) Matlab khảo sát thuật toán Kalman
7
Trong những năm gần đây, việc ứng dụng các hệ thống vi cơ điện từ MEMS
(Micro Electro Mechanical System) đã tạo ra sự thay dổi lớn trong công nghệ chế
tạo các cảm biến quán tính (cảm biến gia tốc, vận tốc góc, ). Mặc dù, các cảm biến
này chưa đạt độ chính xác của các cảm biến cơ học cổ điển nhưng nó có các ưu
điểm hom hẳn về mặt kính thước, công suất, giá thành.
Cùng với xu hướng đó, nhiều phòng thí nghiệm trên thế giới đã triển khai
nghiên cứu ứng dụng các cảm biến này trong việc xây dựng và chế tạo các hệ điều
khiển tự cân bằng, khởi đầu cho sự ra đời cùa các robot dạng người (humanoid
robot), một lĩnh vực hứa hẹn nhiều thành công và khả năng úng dụng rộng rãi trong
tương lai. Tại Khoa Cơ khí, trường Đại học Bách khoa Thành phố Hồ Chí Minh,
một nghiên cứu chế tạo xe hai tự cân bằng có thể trờ một người (mô hình Segway)
đã được thực hiện thu được kết quả tốt [3].
Nhàm mục đích nghiên cứu và bước đầu thừ nghiệm lý thuyết điều khiển cân
bằng áp dụng cho mô hình xe tự cân bằng trên hai bánh kích thước nhỏ sử dụng
cảm biến MEMS, Bản luận văn tập trung làm sáng tỏ các vấn đề và cũng chính là
nội dung dược trình bày trong các chương sau:
C h ư on g 1: Nghiên cứu lý thuyết điều khiển cân bàng của con lắc ngược và
mô hình xe tự cân bằng trên hai bánh xe.
C hưo-ng 2: Nghiên cứu, triển khai cảm biến gia tốc và vận tổc góc của hãng
Analog D evice ứng dụng đo góc nghiêng.
C h u ong 3: Nghiên cứu các phương pháp xử lý tín hiệu góc nghiêng kết hợp
hai loại cảm biến gia tốc và vận tốc góc.
C hương 4: Thực hiện mô phỏng và thử nghiệm xây dựng một hệ điều khiển
tự cân bằng trên hai bánh xe.
Với những kết quả đã đạt được trong việc thừ nghiệm xây dựng một hệ xe

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

Xét mô hình con lắc ngược [5] được chi ra trong hình 2 gồm con lắc có chiều
dài 1, khối lượng m, một đầu được gắn với trục quay trên đế xe khối lượng M. Đế
này có thể chuyển động được trên mặt sàn thông qua hệ thống bánh xe.
-► X
Hình 2. Mô hình con lắc ngược được gan trẽn để chuyên động được bằng hai bánh
Các tham số cùa hệ có thể chi ra như sau:
- M : k h ố i lượng c ủ a x e
- m: khối lượng cùa con lắc
-
1
: chiếu dài cùa con lắc
- b: h ệ số ma s á t c ù a x e đổi với mặt s à n
- ỉ: hệ số quán tính của con lắc
- F: lực tác dụng vào xe
- x: vị trí cùa xe trong hệ toạ độ
-
0
: góc của con lắc so với phương thẳng đứng.
1.1.1 Xây dựng phương trình toán
Hình 3 chi ra các lực tác dụng lên mô hình con lắc ngược theo hai phần riêng biệt:
- Tác dụng lên con lắc
- Tác dụng lên thân xe
11
Hình 3. Các lực tác dụng lên hệ con lắc ngược
Theo phương ngang, phương trình [ực tác dụng lên thân xe và con lắc lần lượt
được biểu diễn thông qua phương trình (
1
) và (
2
)

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

Biến đổi phương trình thứ hai của hệ (9) ta thu được
( / + mỉ2) g
(8)
(9)
X(s) =
0(s)
mỉ s
Thay phương trình (10) vào phương trình th ử nhẩt cùa hệ (9) ta được:
(10)
(A/ + m)
ự + ml2)
g
ỡ(s)s2+b
" (/ + ml2) g '
mỉ
s2
1
1 <N
1 ^5
s
1
Ỡ(s)s + ml0(s)s2 =U(s) (11)
Biến đổi phương trình (11) ta có:
ỡ(s)
ml
U(s) 4 b(I + ml2)sì (M + m)mgls2 bmgls
(12)
Với q = (M + m)(Ị + ml2) + ( ml)2
Rút gọn s ta được:
0ự)_

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
[ớ

+
I(M + m) + MmV'
0
mỉ
Ỉ(M + m) + Mmf'
(14)
y =
X
o
o
o
X '0 '
+
0

0

1 0
e
0
e
1.1.2 Th iết kc mô hình không gian trạng thái cho hệ con lắc ngượ c
Xét sơ đồ điều khiển phản hồi vòng kín được chỉ ra trong hình 4
R
Hình 4. Sơ đồ điều khiến phàn hồi vỏng kín
Bổn trạng thái được xác định trong hệ điều khiển này gồm: vận tổc và vị trí
của xe, góc nghiêng và tốc độ nghiêng của con lắc. Lối ra y gồm: vị trí của xe và
góc nghiêng cùa con lấc. Tiêu chuẩn thiết kế cho hệ thống có thể đặt ra như sau:
- Thời gian giảm (settling tim e) cùa X và 0 nhỏ hơn 5 giây.
- Thời gian tăng (rise tim e) cùa X nhỏ hơn 1 giây.

- 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

trường hợp này, mô h ìn h m ắ c phải lỗi điều khiển vị trí. Lỗi này sẽ được giải quyết
thông qua việc hiệu c h in h quá trình quá độ (thời gian tăng và g iả m cùa đáp ứng).
Nếu tă n g giá trị k hở i đ ầu X và 0 ta sẽ thu đ ư ợc đáp ứng tốt hơn như hình 6.
Hình 6. Đáp ứng điều khiến LQR với các giá trị khởi đầu lớn
Lý d o đ ồ thị nà y d ư ợ c lựa ch ọ n b ở i v ì n ỏ đ ã đ áp ứ ng d ư ợc c ác y ê u c ầu th iết
k ế k h i giữa h ai g iá trị X v à 0 nhỏ đến m ứ c có thể.
Đ ể loại bỏ các lỗi trạng thái ổn định, ta có thể thêm vào sơ đồ điều khiển một
phần tử tham chiếu được chi ra trong hình 7.
Hình 7. S ơ đồ điều khiến phản hồi với lối vào tham chiếu
Đồ thị đáp ứng vị trí của xe, góc nghiên cùa con lăc theo thời gian được biêu
diễn trong hình
8
.
16
step response wild LOR and Nbat control
Hình 8. Đ áp ứng thời gian với loi vào tham chiểu.
Bằng việc sử dụng giao diện dồ hoạ GUI (Graphical User Interface) sẽ cho
phép ta minh hoạ Hệ con lắc ngược trực quan hơn (thấy rõ sự tương quan giữa đồ
thị và đáp ứng vật lý của hệ thống) [25].
04
03
02
ữ.t
0
-01
Step Response lo 0 2(00 cm ir;|Hji
Perduhjm Angle (rad.)
Cart Posllran (cm.)
/
ì /

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.

Hình 10. Phân tích lực và mômen của mô hình xe tự cân bằng trên hai bánh
Các biến được chọn để mô tả xe gồm:
J rl> Jrr: Mômen quan tính cùa chuyển động quay tương ứng với trục z
Jpội Mômen quán tính cùa để xe.
J[>5: Mômen quán tính cùa thân xe
Mrl. Mrr : Khối lượng cùa bánh trái và bánh phải
18
M|>: Khối lượng cùa thân xe
R: bán kính của bánh xe
D: Khoảng cách giữa hai bánh xe
L: K hoảng cách từ trục z đến trọng tâm xe
Phân tích động lực học đối với bánh phải và bánh trái của hệ là tương tự vì
vậy sau đây chỉ trình bày các phương trình động lực học đối với bánh trái.
XrlM rl = f dRl - H l + H tl (15)
yRLMRL =VTL-M Rlg-VL (16)
ỚRL Jrl = CL (17)
Đối với thân xe ta có:
XpMp = f đP + H r + H l (18)
ỴpMp = VR +VL -M Rl g + FC0 (19)
Grho =(K + y/')Lsin0,,- (H r + H, )Lcosớr ~ (C R + c , ) (20)
SJ„I =(H,-H„) I (21)
Trong đó, H l , H r, Htl, Ht r , Vl , V r , Vt l , Vtr biểu diễn các lực tương tác
giữa các bộ phận khác nhau của xe.
Chú ý rằng môm en quán tính Jp§phụ thuộc vào góc 0p, bởi vậy khi tuyến tính
hoá phương trình với độ lệch 0p nhỏ xung quanh vị trí 0p=o, chúng ta giả sử Jp6 là
hằng số tại 0p=o túc là J ị,8 |0 =0 = c o n s t
Già sử rằng các bánh xe luôn luôn tiếp xúc với m ặt đất và không bị trượt.
Bời vậy, sẽ không có sự di chuyển theo hướng z và không quay quanh trục X.
Khi hằng số thời gian của các m ôtơ điện nhỏ hơn hằng số thời gian của hệ
thống, thì động lực học của các rnôtơ coi là không đáng kể trong mô hình.

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

0 0 0
t
Cữp
+
B»
5 «
Bu
fdRL
/
ũ)r
0 0 0
0 0 1
ổ 0 0
0 0
0
JjRR
Ổ
ổ
0
0
0 0 0
0
Õ
Đ6i
*62
b m
fdi'
(22)
Với Á23, A24, B21, B22, B23, B24, B25, B41, B42, B43, B44, B45, B 6I, B62, B63, B64,
B

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

b6.=
-1^62
(24)
Thay phương trình (23) vào (24) và cân bằng kết quà thông qua phương trình
(25) ta thu được phương trinh (26):
0 0 0 0
b2
K
0
0 0
Du
Da
0 0
Du
Dn
Bt
0
0 0 0 0
-Bb
0
Bc
(25)
0
0
0
0 B.
Ce
c x
(26)
Ma trận D có thể được chọn như sau:

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

[CA
(26)
0

1
5
0
+
0

0
5
[c A
(27)
ơ đây, A
23
, A
43
, Bi, B4, B
6
được xác định thông qua các tham số cho trước
của hệ thống.
Như vậy, ta có thề thiết kế hai bộ điều khiển độc lập cho mỗi hệ con và khả
năng tương tác giữa chúng với nhau.
1.3. T ÍN H ĐIÈ U K HIÉN ĐƯ Ợ C CỦ A HỆ TH Ố NG
Xét hệ điều khiển được mô tà bởi hệ phương trình trạng thái sau:
x’ = Ax+Bu
y = Cx+Du
(28)
Để kiểm tra tính ổn định của hệ thống người ta tiến hành kiểm tra giá trị

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 [

1
] (con
quay, gia tốc kế).
Cảm biến quán tính MEMS chưa đạt độ chính xác của các cảm biến cơ học
cổ điển nhưng nó có những ưu điểm vượt trội về kích thước, công suất tiêu thụ, giá
thành và đặc biệt trong xừ lý tín hiệu [2], Nhờ những ưu điểm này, ngay sau khi ra
đời các cảm biến quán tính vi cơ điện tử đã được ứng dụng rộng rãi trong nhiều lĩnh
vực cà quân sự lẫn dân sự. Chương này đề cập đến 2 loại cảm biến quán tính được
sử dụng phổ biến hiện nay là cảm biến gia tốc và cám biển vận tốc góc. Một số đặc
tính liên quan cũng sẽ được nghiên cứu. khảo sát trong chương này.
2.1. CẢ M BIÉN GIA TÓC ADX L202:
2.1.1 Nguyên tắc đo gia tốc:
V iệc do gia tốc thông qua cảm biến gia tốc M EM S có thể được mô tà nhờ một
sơ đồ trên hình vẽ 12 như một hệ gồm một khối lượng m và một lò xo [4, 7,
8
]:
CẢM BIẾN VI CO ĐIỆN TỬ
F = lev = ma
acccl
k z z !
m
—vMMML—
X 0
(Rcmj
Hình 12. Hệ khối lưcmg - lò xo được sừ dụng để đo gia tốc
Theo định luật Hooke, lực kéo khối lượng m ti lệ với độ biến dạng cùa lò xo
F=kx, với k là hệ số tỉ lệ hay độ cứng của lò xo, X là khoảng dịch chuyển so với vị
trí cân bằng. Theo định luật II Newton, lực F này cung cấp cho khối lượng m có
một gia tốc a theo công thức: F=ma.
24

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

Như vậy, nếu gắn khối lượng m của cảm biến vào bản tụ nằm giữa hệ hai tụ
điện nối tiếp thì có thể xác định được độ dịch chuyển của nó dưới tác dụng cùa lực
F, tức là xác định được gia tốc thông qua việc xác định giá trị AC.
2.1.2 Đặc trư ng của cảm biến ADX L202
A DXL 202 là cảm biến gia tốc được chế tạo theo công nghệ M EM S, một sàn
phẩm của Hãng A nalog Device. Đây là loại cảm biết gia tốc hai chiều kiểu tụ cho
phép xác định một cách độc lập các gia tốc theo các phương trục toạ độ X và Y.
Cảm biến này có hai loại lối ra đó là lối ra số (độ rộng xung ti lệ với gia tốc) và lối
ra tương tự (mức điện áp tương tự tỉ lệ với gia tốc). Hình 14 cho thấy nếu lấy tín
hiệu ra tại các chân X|.j|i và Ypii, sõ cho ta tín hiệu tương tự, cách khác nó sẽ là chân
đổ thiết kế bộ lọc tín hiệu RC. Lối ra X oul, Youl có dạng xung với chu kỳ có thể được
thay đổi bời điện trở Rse,.
H ình 14. Cấu trúc cùa càm biến g ia tốc ADXL 202
26