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

Đá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 3 1 .Đá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

ĐẠI HỢC QUÓC GIA HÀ NỘI TRƯỜNG ĐẠI HỌC CÔNG NGHỆ

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

L U Ậ N V Ã N T H Ạ C S Ỹ

NGƯỜI HƯỚNG DÀN KI IOA HỌC

T S T r ầ n Q u a n g V i n h

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 ÌN H H Ệ C O N L Ắ C N G Ư Ợ 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 ÌN H X E T ự C Â N B Ầ N G T RÊN H A I BẢNH.' IX 1.3 TÍNH Đ IÊU K H IÊ N Đ ư ợ c CỦA H Ệ T H Ô N G 22

Chương 2 - CẢM BIẾN VI c ơ ĐIỆN TỪ 24

2.1 C Ả M BI É N GIA TÔC A D X L 2 0 2 : 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 C O N Q U A Y VI c o A D X R S I 5 0 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 H IỆ U G Ó C N G H IÊ N G 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 N G H I K A L M A N 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.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

D A N H M Ụ C H Ì N H V Ẽ , Đ Ò T H Ị

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

b) G iao 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 A D X R S 1 5 0

Iỉình 2 1 Tín hiệu lối ra R A TE O U T tăng theo chiều quay thuận chiều kim đồng hồ Iỉình 22 Cấu trúc của A D X R S 1 5 0

H ình 23 Sơ đồ đấu nối A D X R S 1 5 0 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 A D X R S 1 5 0

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 A D C 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 3 1 Đá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ụ

1 Hnh 38 G iao 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 e e lj e f t

Hình 41 Khối vNPlant thực hiện truyền tham số giữa VisualNastran và MatLab Hình 4 2 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 4 6 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

Trong những năm gần đây, việc ứng dụng các hệ thống v i cơ điện từ M EM S (M icro Electro M echanical System ) đã tạo ra sự thay dổi lớn trong côn g 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ô n g suất, giá thành.

C ùng với xu hướng đó, nhiều phòng thí nghiệm trên thế g iớ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ôn g 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 S egw ay)

đã đượ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 M EM S, 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 ư o n g 1: N gh iê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 x e tự cân bằng trên hai bánh xe.

C h ư o -n g 2: N gh iên cứu, triển khai cảm biến gia tốc và vận tổc góc của hãng

A n a lo g D ev ice ứng dụng đo g óc nghiêng.

C h u o n g 3: N gh iê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 ư ơ n g 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 v iệ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

theo c ô n g nghệ M EM 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 m ong nhận được sự góp ý của các thầy cô giáo, bè bạn, đ ồ n g nghiệp

và những ai đang quan tâm tới đề tài này.

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.

- X e 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.

- C huyển động thẳng theo trục X r m với vận tốc g óc V RM

- Q uay quanh trục thẳng đứng (yaw ) với g óc ô.

V iệ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.

1.1 M Ô H ÌN H H Ệ C O N L Ắ C N G Ư Ợ 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 đ ộ n g đ ư ợ c b ằ n g h a i 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 ự n g p h ư ơ n g 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

H ình 3 C á c lực tá c dụng lên hệ con lắc ngược

T heo 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)

2

N = m x + m l G c o s 0 - m l ớ s i n ớ (2) Lấy (2) trừ (1) ta thu được :

Khi góc n g h iên g 0 nhỏ (0<1O°), ta có th ể tu y ến tính hoá h ệ n h ư sau:

(M + m)x+bx+ mi 6 = u

(/ + m l 2)ỡ+ mgl ỡ = —mi X

V ới Ư là lối vào cùa hệ

T h ự c h iện b iến đổi L aplace ta thu đ ư ợ c hàm tru y ền c ủ a hệ th ố n g n h ư sau:

1.1.2 T h iế t kc m ô h ình k h ô n g gian trạ n g thái c h o hệ con lắc n g ư ợ c

X ét sơ đồ điều khiển phản hồi v òn g kín được chỉ ra trong hình 4

R

H ình 4 S ơ đồ điều khiến p h à n hồi v ỏ n g kín

B ổn trạ n g thái đượ c xác định tro n g hệ điều khiển này gồm : vận tổc và vị trí

củ a xe, g ó c n g h iê n g và tốc độ n g h iên g của con lắc L ối ra y gồm : vị trí c ủ a xe và

gó c n g h iê n g c ù a c o n lấc T iêu chu ẩn thiết kế ch o hệ th ố n g có th ể đặt ra n h ư sau:

- T h ờ i gian giảm (se ttlin g tim e) cùa X và 0 nhỏ hơn 5 giây

- Thời g ian tăn g (rise tim e) cùa X nhỏ hơ n 1 giây

- Giới hạn (o v e rsh o o t) nghiêng cùa góc 0 nhò hơn 20°(0.35 radian).

- Lỗi trạ n g thái ổn định 2 %

L uật p h ả n hồi K có thể đ ư ợ c x ác đ ịn h th ô n g q u a hàm lqr(A, D, Q, R) (bộ điều

c h ỉn h to àn p h ư ơ n g tuyến tín h -lin ear lỊuađratic regu lator) cùa M atlab H àm này cho

p h é p lựa c h ọ n hai tham số Q và R (tham số sẽ bù lối vào R) nhằm thự c h iện tối ưu

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:

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.

G ià 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ố 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 LQ R 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ệ g iữ a vị tri c ủ a xe, g ó c nghiên cù a con lắ c theo

N hì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 g iá trị khởi đầu lớn

Lý d o đ ồ th ị n à y d ư ợ c lựa c h ọ n b ở i v ì n ỏ đ ã đ á p ứ n g d ư ợ c c á c y ê u c ầ u th iế t

k ế k h i giữa h a i 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 p h ả 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.

Hình 8 Đ á p ứng th ờ i g ia n với lo i vào tham chiểu.

B ằng việc sử dụng giao diện dồ hoạ GUI (Graphical U ser 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].

0403

02

ữ.t

0

-01

Step Response lo 0 2(00 cm ir;|Hji

Perduhjm Angle (rad.)Cart Posllran (cm.)

/

ì V/

stephfwt 020X1 -ns * 1 1 • 1 ns

06

£ OX c

0 [~ M rtuiịl Advance

0 Sydetii

1.2 M Ô H ÌN H X E T ự C Â N B Ả N G T R Ê N H A I B Á N H

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 P h ân tích lự c v à m ôm en củ a m ô hình xe tự câ n b ằn g trên hai bánh

Các biến được chọn để mô tả xe gồm:

J r l > 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 Mr r : Khối lượng cùa bánh trái và bánh phải

M|>: K hối lư ợ ng c ù a th ân xe

T ro n g đ ó, H l , Hr, Htl, H t r , V l , V r , V t l , Vtr b iểu d iễn các lực tư ơ n g tác

g iữ a các bộ ph ận khác nhau c ủ a xe

C h ú ý rằn g m ô m en q u á n tính Jp§phụ thuộc vào gó c 0p, bởi vậy khi tu y ến tính

ho á p h ư ơ n g trìn h với đ ộ lệch 0p nhỏ x u n g q u a n h vị trí 0p=o, ch ú n g ta g iả s ử Jp6 là

hằng số tại 0p=o túc là J ị,8 |0 =0 = c o n s t

G ià sử rằn g các b án h xe luôn luôn tiếp xúc với m ặt đất và k h ô n g bị trượt

B ời v ậy, sẽ k h ô n g có sự di ch u y ển theo h ư ớ n g z và k h ô n g qu ay q u a n h trục X.

K hi h ằ n g số thời gian của c ác m ô tơ đ iện nhỏ hơ n h ằ n g số thời g ian của hệ

th ố n g , thì đ ộ n g lực học của các rnôtơ coi là k h ô n g đ á n g kể tro n g m ô hình

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:

B65 đượ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 x e f dRL, 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à C L, C R (m ô m en 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 ôm en xoắn quay quanh trục thẳng đứng Cg và m ôm en 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 ôm en này được chỉ ra như sau:

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:

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)

Đ ể 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:

N ếu một trong các giá trị riêng x > 0 thì hệ điều khiển không ổn định.

N goà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 A B 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 N eu 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'

M a tla b c u n g c ấ p lệnh R a n k (P ) g iú p ta tìm hạng m a trận p n h a n h g ọ n và

th u ận tiện N g o à i ra, M a tla b c ò n c u n g cấp m ột c ô n g cụ để xây d ự n g m a trận p th ô n g

q u a lệnh sau: p = c trb (A ,B ) [3].

N h ư v ậ y , đ ể k iểm tra tín h đ iều k hiển đượ c cùa hệ th ố n g đ ư ợ c m ô tả b ằn g hệ

p h ư ơ n g trìn h trạ n g th ái (2 8 ) ta c h ỉ cần thự c hiện c ặ p lệnh sau tro n g M a tla b với các

Chương 2

V iệc ứng dụng các hệ thống vi c ơ điện (M icro Electro M echanical System )

dã tạo ra sự thay đổi lớn trong côn g 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 M EM S 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ô n g suất tiêu thụ, giá thành và đặc biệt trong xừ lý tín hiệu [2], N h ờ 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 B IÉ N G IA T Ó C A D X L 2 0 2 :

2 1.1 N g u y ê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ò x o [4, 7, 8]:

T heo định luật H ooke, lực kéo khối lượng m ti lệ với độ biến dạng cùa lò xo

F = k x, 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 N ew ton , lực F này cung cấp cho khối lượng m có

m ột gia tốc a theo côn g thức: F=ma.

đ iện đ ể th ự c h iện ph ép đo k h o ả n g dịch ch u yển X ét hai bản tụ so n g song, k h oảng

c ách g iữ a hai bản tụ có thể th ay đổi đượ c (h ìn h 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ư ờ n g nằm giữ a hai bản tụ N ế u biết k, điện d u n g cùa tụ điện c ta có thể tính đ ư ợ c x 0 C ũ n g tro n g hình 13, nếu bản tụ nằm g iữ a C A v à Cb d ịch c h u y ể n m ột

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, Y oul có dạng xung với chu kỳ có thể được thay đổi bời điện trở Rse,.

H ìn h 14 C ấ u trú c cù a càm b iến g ia tố c A D X L 2 0 2

Cảm biến có thể được sừ dụng để đo cà gia tốc tĩnh và gia tốc động (chịu được độ số c lên đến lOOOg) Đ iều thú vị cùa cảm biến gia tốc hai chiều này là thông qua v iệc đo gia tốc tĩnh ta có thể đo được g ó c nghiêng của cảm biến so với phương gia tốc trong trường Hình 15 chỉ ra rằng giá trị gia tổc tĩnh đo được tại lối ra kênh

X và Y của càm biến chính là hình chiếu độ lớn gia tốc trọng trường l g lên hai phương này.

A(g) = <T1,T2 - 0.5)/12.5%

0g=509ó xuũg T2(s)=Rset/125 Míi

H ìn h 16 Tín h iệu đ ộ rộ n g x u n g tại lo i ra s ố c ủ a c ả m b iến g ia tốc.

Đo sự phụ thuộc giữa số xung thu được từ bộ đếm cùa vi điều khiển vào góc nghiêng cùa cảm biến so với phương trọng trường, ta thu được một dạng tín hiệu diều hoà hình sin [7, 8 ] như minh hoạ trong hình 17.

Hình ỉ 7 Đồ thị biếu diễn sự phụ thuộc độ rộng xung và góc nghiêng

Có nhiều cách để chuyển đổi thời gian xung đo được thành góc nghiêng Một trong những cách đó được trình bày trong bản luận văn này là việc sử dụng hàm ngược để tính góc nghiêng.

a,=asm(p' - (f se!) - ạ. (35)

A

Cách làm này đã được tác giả sử dụng để xây dựng hệ đo g ó c roll và pitch của vật thể chuyển dộng [4, 8] (hình 18) và hệ điều khiển thực tại ảo để điều khiển cánh tay m áy 5 bậc tự do [7] (hình 19).

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 đ ộ n g b) G ia o diện đồ hoọ của p h ầ n m ềm ch ỉ thị g ó c nghiênX ro ỉl và

p iic h

Hình 19 H ệ đ iều khiển thự c tạ i ả o cho la y m ả y 5 b ậ c lự d o s ử dụng cảm biến A D X L 202:

a ) M ỏ hình b à i to á n điểu khiển thực tạ i ảo b) G ia o d iện đ ồ hoạ của p h ầ n m ềm ch ỉ thị tạ i trạm điểu khiến c) A nh chụp n gư ời đ iều khiển r a lệnh cho R o b o t b ằ n g ta y (cảm biến

đ ư ợ c g a n trên ta y ngư ời điểu khiển)

2 2 C O N Q U A Y V I c ơ A D X R S 1 5 0

Cảm biến vận tốc g ó c A D X R S 1 5 0 hoạt động trên nguyên tắc con quay vi cơ

D e v ic e, gồm con q u ay vi c ơ đư ợ c chế tạo bàn g quá trin h vi c ơ bề m ặt (su ríầc e -

m ic ro m a c h in in g p ro ce ss) và các m ạch x ử lý tín hiệu trên m ột đ ơ n ch íp H ai cấ u trúc

c ả m n h ận p o ly silic o n ch ứ a k h u n g dao đ ộ n g đư ợ c đ iều k hiển bởi bộ c ô n g h ư ở n g tĩnh

đ iệ n tạo ra th àn h phần vận tốc cần th iết ch o lực C o rio lis khi có c h u y ể n đ ộ n g quay

T ại hai đầu cuối cùa khung, v u ô n g góc với p h ư ơ n g dao đ ộ n g là các c ấ u trúc răng lượ c linh đ ộ n g gắn xen k ẽ với các răn g lược cố định đ ể tạo lên cấu trúc kiểu tụ dùng

đ ê cả m n h ận ch u y ển đ ộ n g C o rio lis [9] L ư ỡ n g cảm biến đượ c chế tạo bên trong

A D X R S 150 nhằm loại bò rung đ ộ n g và ngoại lực tác dụng

2.2.1 Đặc trưng của Gyroscope ADXRS150

- D ặc đ iểm nổi b ật c ủa A D X R S 150 là kích th ư ớ c nhò (7 m m X 7 m m X 3

m m ), độ c h ín h xác, tin c ậ y cao, g iá th àn h rẻ, d ò n g tiêu thụ th ấp (5m A )

- T íc h hợ p hoàn chinh con qu ay vi c ơ trên 1 đ ơ n chíp

Hình 20 Anh chụp cảm biến A D X R S Ỉ5 0

- Đ á p ứ n g th eo trục z.

- C h ố n g n h iễ u d u n g động lớn trên m iền tần rộng.

- Đ ộ sh o ck ch ịu đ ự n g được d ể n lOOOg

- C h ế độ tự kiểm tra theo lệnh số

- Lối ra cảm biến nh iệt

- Lối ra th am c h iế u đ iện th ế ch ín h x á c

- Đ iện áp h o ạ t đ ộng: 5V

- Siêu m ỏ n g v à n hẹ (< 1 5 0 m m 2, <1 gam )

Tín hiệu lối ra R A T E O U T ( 1 B, 2A ) có diện áp ti lệ với vận tốc g ó c quanh trục trục cảm nhận z vuông góc với bề mặt chíp dược đóng gói Đ iện áp lối ra sẽ dương nếu chuyển động quay theo chiều dương quy ước và ngược lại M ột điện trở ngoài có thể được sử dụng để hiệu chinh hệ số tỉ lệ Tần số hoạt động cũng có thể hiệu chỉnh bởi v iệc lẳp thêm một tụ bên ngoài.

nuôi lúc này nối vào chân C P5(chân 7 D ) với m ột tụ lOOnF mắc thay vị trí cùa tụ 47nF.

M ột mạch lọc thông thấp (sử dụng tụ mắc thêm bên ngoài C m in = 100n F ) được thiết kế ngay sau khối giải điều chế dùng để giớ i hạn ảnh hường của tần số cao lên tầng khuếch đại sau cùng c 0 ut có tác dụng giới hạn dải thông (sơ đồ chỉ ra trong hình 23) Tẩn sổ cắt tại - 3 d B được xác định bởi côn g thức sau:

M ở rộ n g dải đo: dài đo của một cảm biển vận tốc g ó c là g iớ i hạn đ o vận tốc g ó c của cảm biến Tuy nhiên, dải đo này có thể được điều chỉnh thống qua

v iệc thay đổi điện trở mắc thêm bên ngoài giữa hai chân R A T E O U T (lB ,2 A ) và

S U M J(1C ,2C ) tức là mắc song song với trở nội Rout- V í dụ, m ột trở bên ngoài có giá trị 330 KÍ2 tạo ra độ nhạy khoảng 8.1 m V /đ ộ/giây và độ tăng tương ứng khoảng 50% toàn dải N ó có thể m ở rộng toàn dải tới 4 lần (g iá trị điện trờ ngoài nhỏ nhất cho phép là 45 K í}) V iệc m ở rộng dải đo kéo theo sự gia tăng độ trôi điểm “0” lối ra (tương ứng với khoảng 2°/s theo nhiệt độ) do vậy cần

có sự điều chinh lại độ trôi điểm “0”

Điều chỉnh trôi điểm “ 0” : V iệc điều chinh điểm “0 ” đ ư ợ c thực hiện bằng cách đưa m ột dòng thích hợp vào chân SU M J(1C ,2C ) Đ ể thực hiện điều đó chỉ

cần mắc thêm một điện trờ giữa nguồn cung cấp h o ặ c đất với chân SU M J Giá trị điện trờ này được chỉ ra trong cô n g thức (37) Đ iện áp “0 ” 2 5 V nằm đổi xứng giữa dải điện áp lối ra Tuy nhiên đối với một số ứng dụng mức điện áp “0 ” không đối xứng lại thích hợp hem c ầ n lưu ý rằng khi mắc điện trở với nguồn cu n g cấp thì khi nguồn không ổn định có thể gây sự thăng giáng điểm “0 ” Trong trường hợp này, nhiễu cùa nguồn số phải được đặc biệt loại trừ.

m ong m uốn thi nối điện trở vào nguồn cung cấp Giá trị thường gặp của điện trở nằm trong khoảng 1 -5 MQ.

2 2 2 M ộ t số k h á o sát trên cảm b iến A D X R S 1 5 0

N hư ta đã biết, lối ra của cảm biến A D X R S 1 5 0 là mức điện áp tương tự tỉ lệ với vận tốc góc của cảm biến quay quanh trục cảm nhận z vuông góc với v ề mặt chip Tín hiệu này phải được đưa tới bộ biến đổi tương tự sổ A D C biến đội m ứ c điện áp thành giá trị sổ tương ứng với giá trị vận tốc góc Đ ể thực hiện điều này, trong bản luận văn sử dụng chip vi điều khiển PSoC (Program m able System on C hip) có chứa

bộ biến đổi A D C 12 bít M ô hình hệ đo vận tốc g ó c được chỉ ra trên hình 24.

bít của vi điều khiển PSoC C Y 8C 27443 Đ ây là loại vi điều khiển sử dụng bộ tài nguyên động trên chíp để thiết kế các ứng dụng D ừ liệu số tương ứng với vận tổc góc được truyền về máy tính thông qua chuẩn nối tiếp RS 232 M áy tính PC có chức năng ghi các mẫu số liệu cùa vận tốc g ó c tương ứng vào file V iệc lấy trung bình giá trị các mẫu trong từng file cho ta 1 tập số liệu tương ứng với dải vận tốc

g óc rời rạc của đĩa quay Trong miền tuyến tính của đồ thị quan hệ giữa vận tốc góc đĩa quay và dữ liệu sổ thu dược, vận tổc g óc được xác định qua c ô n g thức:

« = (P t-PoiTset)*tgỌ (38)

V ới tgcp là hệ số góc cùa đường thẳng tuyển tính, p, là sổ xung đo được tại thời điểm t Ponsci là số xung thu được tương ứng với vận tốc g ó c co=0 D ễ dàng nhận ra rằng, nếu Pi>PotTsci đĩa quay chuyển động theo chiều thuận và ngược lại nếu

Pi < Ponrset thì đĩa quay chuyển động theo chiều ngược.

ỉỉìn h 25 đ ồ thị p h ụ thuộc vận tốc g ó c và g iá trị s ố thu đ ư ợc từ bộ b iến đ ô i A D C tron g

2 trư ờ n g hợp: q u a y theo chiều thuận (hình trái); q u a y theo chiều ngư ợc (ìvnh p h ả i)

Bàn xoay được sử dụng trong bản luận văn này gồm một m ôtơ xoay chiều với hệ thống bánh răng cùa hộp số c ó hệ số giảm tốc giảm tốc đủ lớn để đảm bảo tính ổn định cùa bàn xoay Bàn xoay này được nhập từ Liên X ô cũ không có bộ chỉ thị vận tốc góc quay Vì vậy, việc thiết kế m ột m ôđun đo vận tốc g ó c cùa bàn xoay

là cần thiết M ôđun này bao gồm 36 0 vạch đen trắng xen kẽ tương ứng với 360° gẳn

trên đĩa quay Cảm biến hồng ngoại phát và thu tín hiệu phản xạ trên dãy vạch đen tráng tạo ra một chuỗi xung vuông B ộ đếm của vi điều khiển PSoC C Y 8C 27443 sẽ đếm sổ x u n g này trên 1 đơn vị thời gian từ đó xác định được vận tốc góc của đĩa quay Vận tốc g óc này được chỉ thị trên màn hình tinh thể lỏng LCD.

H ình 26 a) Bùn x o a y có g a n cá c vạch đen tra n g b) S ơ đ ồ khối M ôđun đ o vận tố c g ó c cùa bàn x o a y

N hư ta đã biết, đạo hàm bậc nhất của góc quay cho ta vận tốc góc, vì vậy, góc quay có thể được tính thông qua việc lấy tích phân tín hiệu vận tốc góc Phép tính tích phân này c ó thể là số hoặc tương tự.

- Phép tính gần đúng tích phân số được thực hiện từ tập dữ liệu thu được thông qua phép biến đổi A D C theo các phương pháp sau: phương pháp ơ le , phương pháp hình thang.

- Phép tính tích phân tương tự được thực hiện thông qua m ạch cứng được thiết

kế xử lý trực tiếp lối ra của cảm biến vận tốc góc.

Giả trị g óc quay được tính theo các phương pháp trên không được đề cập và khảo sát cụ thể trong bản luận văn này.

Chương 3

Trong thực tế triển khai các ứng dụng đo góc nghiêng và vận tổc góc các kết quả đo thu được trong chương 2 tuy có độ ổn định và độ chính xác cao nhưng vẫn chưa thể đáp ứng giải quyết m ộ t số bài toán thực hiện trong môi trường có nhiễu rung động lớn như: đo góc nghiêng của máy bay không người lái, Việc thiết kế một bộ lọc loại bỏ ảnh hưởng của nhiễu này là vô cùng cần thiết Chương 3 tìm hiểu

m ộ t sổ bộ lọc được thiết ké để xử lý tín hiệu góc nghiêng từ hai lối vào của cảm biển gia tốc và vận tốc góc Ưu điềm vuợt trội đã được chứng tỏ thông qua bộ lọc thích nghi Kalman.

lọc Gj(s) để ước lượng giá trị trong miền tần số thấp, vận tốc góc của Gyro được đưa tới bộ lọc Gg(s) cốt để ước lượng giá trị trong miền tần số cao Các yêu cầu cơ bản cho hai bộ lọc có thể dược chi ra như sau:

- Hệ số khuếch đại toàn dải là hằng số, pha tổn thất nhỏ tại vùng giao nhau

giữa hai dài tần số.

- Cảm biến góc nghiêng phải được sử dụng trong vùng băng thông rộng nhất

có thổ để giữ độ dịch offset của Gyro là nhỏ nhất.

- Số lượng tham số thiết kế nhỏ để dễ dàng điều chỉnh và thực hiện bộ lọc

Hình 28 Sơ đồ khối bộ lọc bù

Để đảm bảo các yêu cầu trên, bộ lọc được thiết kế với hàm hàm truyền Gj(s),

Gg(s) như sau:

Với Hj(s), Hg(s) theo thứ tự là hàm truyền của cảm biến gia tốc và cảm biến

vận tốc góc khi loại bò các yếu tổ phi tuyến Việc chọn hai hàm này có thể được

tiến hành hai cách sau đây:

Cách thú’ nhất: giả thiết các cảm biến là lý tưởng:

Cách thứ hai: giả thiết biết đặc trưng động của cảm biến gia tốc sử dụng

trong miền tần sổ thấp có hằng số thời gian T, thường các cảm biến gia tốc bán trên

thị trường hiện nay có hằng số thời gian 1 = 0.53 khi sản phẩm được xuất xưởng.

TS + 1 và Hg(s)= 1 (41)

Sau đây xin được đề cập thiết kế bộ lọc bù theo hai cách tiếp cận trên.

3.1.1 T h iế t k ế bộ lọc vớ i cảm biến lý tư ở n g

Với giả thiết các cảm biến là lý tưởng, việc kết hợp hai phương trình (39) và (40) cho ta sự phụ thuộc sau:

Dê giữ cho số lượng tham số của bộ lọc nhỏ, chúng ta chọn bộ lọc bậc hai với hai cực như sau:

Bộ lọc thông thấp bậc nhất Gj(s) được chi ra trong phương trình (43) lọc tín hiệu từ cảm biến góc nghiêng, trong khi nhánh còn lại là bộ lọc thông cao bậc hai Gg(s) lọc tín hiệu từ cảm biến vận tốc góc Bộ lọc bù cũng có thể được thiết kế sử dụng bộ lọc thông thấp bậc hai cho cảm biến góc nghiêng, mạch lọc thông cao bậc một cho cảm biến vận tổc góc bàng việc hoán đổi 2 giá trị 2 x và X 2 trong hai phương trình (43) và (44) cho nhau Giá trị góc nghiêng (p thu dược là như nhau trong hai trường hợp.

Ta thấy ràng, bộ lọc bù có hằng số thời gian X là tham số thiết kế duy nhất Hằng số thời gian này có thể được chọn thấp tới mức có thể với mục đích tối thiểu ảnh hường độ lệch offset của cảm biến vận tốc góc Vì vậy, khi chúng ta giả sừ rằng dùng cảm biển lý tưởng, hàng số thời gian cùa bộ lọc sẽ được chọn phải lớn hơn hàng sổ thời gian cùa cảm biến góc nghiêng( 0.53 s, tham số do nhà sản xuất quy dịnh trên sản phầm) Hình 29 chi ra rằng đáp ứng tần số của bộ lọc bù ước lượng góc nghiêng cp đối với hai giá trị hằng số thời gian T= 2 s và T=4s trong trường hợp sử dụng bộ lọc thông thấp bậc nhất cho cảm biến góc nghiêng lý tưởng (hầng số thời

(43)

(44)

gian đáp ứng do nhà sản xuất quy định là 0.53 s) và gyro lý tường như phương trình (41) Có thể thấy rằng, nếu chọn hằng số thời gian x=4s hoặc cao hơn sẽ cho dáp ứng tốt hơn đặc biệt việc tổn thất pha giữ hai miền tần sổ.

Gain dB

H ình 29 G iản đ ồ bo d e của hệ th ố n g ước lư ợ n g với h ằng s ổ thời g ia n khác nhau

Tuy nhiên, với x=4s thì lỗi dịch offset của cảm biển vận tốc góc đã không dược lọc một cách hiệu quả Bởi vậy, việc thiết kế bộ lọc bù theo cách thứ 2 sẽ được

đề cập ngay sau đây.

3.1.2 Thiết kế các bộ lọc với các đặc tính động cảm biến góc nghiêng

Nếu biết các đặc trưng động của cảm biến gia tốc ( t = 0 5 3 ) , đồng thời kết họp hai phương trình (39) và (41) ta thu được:

1

(45) (0.535 + 1)

Bên cạnh đó, bằng việc chọn mạch lọc bậc hai và bằng việc tối thiểu hoá ảnh hưởng của dộ lệch offset của Gyro, chúng ta thu được các bộ lọc sau:

(46)

(47)

Hang so thai gian bo Iqc trong trirong hgp nay bang vdi hang so thoi gian cua cam bien gia toe Trong gian do bode hinh 30 bieu dien dap img tan so cua bo loc nhanh (phirang trinh 46) co cam bien goc nghieng Bo loc nay la mot bo Iqc

dimg dau cai thien ton that pha cua cam bien gia toe He so cue dai a mot tan so khoang 1.5 rad/s la 1.15 db.

so 0.1 rad/s da chung to la du de loai bo loi djch offset cua cam bien van toe goc.

Bang viec so sanh gian do bode a hai nhanh, co the thay rang trong dai tan tir

1 den 10 rad/s (0.15 Hz den 1.5 Hz) ca hai cam bien deu gop phan dang ke vao loi

ra 0 tan so cao, cam bien van toe goc chi phoi nhieu hon loi ra bo loc, ngugc lai, cr mien tan so thap cam bien goc nghieng se chi phoi Hinh 32 bieu dien goc nghieng uoc lugng q> tai loi ra bo loc so vai hai tin hieu loi ra cua cam bien.