BÁO CÁO MÔ HÌNH ĐIỀU KHIỂN QUADROCOPTER

Lôøi caûm ônKính göûi Thaày TS. Chung Taán Laâm !

Em laø Phan Leâ Sôn, sinh vieân naêm 4, ngaønh Cô Ñieän Töû ,

Tröôøng Ñaïi Hoïc Baùch Khoa Tp.HCM.

Hoïc kyø naøy, em raát vinh döï ñöôïc thöïc hieän ñeà taøi cuûa

ñoà aùn moân hoïc Thieát Keá Heä Thoáng Cô Ñieän Töû döôùi

söï höôùng daãn taän tình cuûa Thaày. Chính söï nhieät tình chæ

daãn cuûa Thaày ñaõ giuùp em hoaøn thaønh ñoà aùn khaù toát,

ñaït ñöôïc nhöõng yeâu caàu cô baûn maø ñeà taøi ñaët ra. Vaø

cuõng qua ñoù, em ñaõ hoïc hoûi, tieáp caän ñöôïc nhieàu vaán

ñeà thöïc teá hôn, boå sung nhöõng “khe hôû” kieán thöùc do

vieäc hoïc thieân veà lyù thuyeát ñeå laïi.

Em xin chaân thaønh caûm ôn Thaày.

Cuõng qua ñaây, em xin göûi lôøi caûm ôn chaân thaønh cuûa

mình ñeán gia ñình, baïn beø –nhöõng ngöôøi ñaõ ñoäng vieân,

quan taâm heát möïc ñeán em trong suoát quaù trình thöïc hieän

ñoà aùn. Ñoù laø nhöõng ñoäng löïc thuùc ñaåy em coá gaéng

hôn, quyeát taâm hôn, ñeå cuoái cuøng em coù ñöôïc keát quaû

nhö ngaøy hoâm nay.

Em xin göûi lôøi caûm ôn ñeán taát caû.

BÁO CÁO MÔ HÌNH ĐIỀU KHIỂN QUADROCOPTER

Nội dung

1. Giới thiệu về QuadroCopter2. Cấu tạo của QuadroCopter3. Nguyên lý hoạt động của QuadroCopter4. Xây dựng mô hình QuadroCopter trong VisualNastran5. Xây dựng mô hình điều khiển QuadroCopter trong Simulink của MatLab6. Lyù thuyeát veà boä loïc Kalman7. ÖÙng duïng boä loïc Kalman vaøo ño caùc goùc nghieâng cuûa

maùy bay8. Höôùng phaùt trieån cuûa ñeà taøi .9. Phuï Luïc

I. GIỚI THIỆU VỀ QUADROCOPTER:

Quadrocopter là một loại máy bay có 4 cánh cùng nằm trong một mặt phẳng và đối xứng qua tâm của máy bay. Nhôø coù 4 caùnh neân loaïi maùy bay naøy deã ñieàu khieån hôn vaø coù khaû naêng giöõ thaêng baèng toát hôn . Loaïi maùy bay naøy coù raát nhieàu öùng duïng thöïc teá, noù coù theå coù kích côõ lôùn ñeå chôû ngöôøi vaø haøng hoaù vaø cuõng coù theå coù kích côõ nhoû raát naêng ñoäng coù theå hoaït ñoäng ngay caû trong nhaø . Loaïi maùy bay kích côõ bay trong nhaø hieän nay ñang phaùt trieån raát raàm roä , nhieàu sinh vieân cuûa Ñöùc vaø Myõ ñaõ cheá taïo loaïi maùy bay naøy vôùi nhieàu hình daùng vaø tính naêng khaùc nhau ñeå nghieân cöùu . Trong phaàn ñoà aùn naøy nhoùm xin ñöôïc giôùi thieäu keát quaû nghieân cöùu veà loaïi Quadrocopter hoaït ñoäng trong nhaø .

Trong ñoà aùn naøy nhoùm ñaõ moâ phoûng maùy bay Quadrocopter baèng phaàn meàm MatLab keát hôïp vôùi Visual Nastran . Sau ñoù nhoùm ñaõ thieát keá phaàn cöùng maùy bay vaø hoaøn thaønh moät soá modul nhoû cuûa coâng vieäc ñieàu khieån maùy bay . Trong ñoà aùn naøy coù ñöa vaøo modul ño goùc nhieâng cuûa maùy bay baèng caûm bieán gyro vaø caûm bieán gia toác daøi ñöôïc öôùc löôïng baèng boä loïc Kalman .

Máy bay Quadrocopter

II. CẤU TẠO CỦA QUADROCOPTER :- Thân : thường có hình dáng giống dấu chữ thập , mỗi cánh của chữ thập đỡ một

cánh của máy bay.

- Cánh : có 4 cánh giống hệt nhau đặt trên 4 góc của thân , 2 cánh đối diện thì quay cùng chiều nhau và ngược chiều với 2 cánh đối diện còn lại . Sở dĩ máy bay có cấu tạo như vậy để làm hạn chế sự quay của thân khi cánh quay.

- Bộ điều khiển : bao gồm 2 phần , 1 phần đặt trên máy bay , một phần nằm ở dưới đất .+ Phần đặt trên máy bay : bao gồm mạch công suất điều khiển 4 động cơ , mạch truyền sóng để kết nối với bộ điều khiển ở dưới đất , mạch chính dùng để điều khiển chuyển động của động cơ ( có thể bao gồm cả 2 phần là bằng tay và tự động

) . Để điều khiển máy bay theo chế độ tự động ta còn phải có thêm các cảm biến như cảm biến gyro để đo độ nghiêng, cảm biển aùp suaát để đo độ cao , cảm biển gia tốc để biết hướng chuyển động , camera xử lý ảnh có nhiệm vụ theo dõi các đối tượng ở dưới đất . . .+ Phần dưới đất : bao gồm bộ điều khiển bằng tay có mạch thu phát tín hiệu sóng vô tuyến, mạch kết nối với máy tính hoặc là mạch giao tiếp với người sử dụng để họ có thể thay đổi các thông số và các chế độ điều khiển . . .

- Động cơ và bộ driver của động cơ : nếu máy bay chỉ hoạt động bay thông thường thì chỉ cẩn bốn động cơ dùng cho 4 cánh quạt là đủ.

Hình 2: Moâ hình QuadroCopter

III. NGUYEÂN LYÙ HOAÏT ÑOÄNG CUÛA QUADROCOPTER:

Ñeå ñieàu khieån ñöôïc hoaït ñoäng cuûa maùy bay ta phaûi ñieàu khieån tyû leå toác ñoä töông öùng giöõa 4 caùnh quaït . Sau ñaây laø caùch ñieàu khieån cuï theå:

Bay leân:Boán caùnh quay vôùi vaän toác ñeàu nhau gioáng nhö hình veõ. ÔÛ treân hình chuùng ta thaáy 2 caùnh ñoái dieän cuûa maùy bay quay cuøng chieàu vôùi nhau vaø ngöôïc vôùi 2 caùnh coøn laïi . Ñieàu naøy raát quan troïng vì nhö vaäy noù seõ khoâng laøm maùy bay xoay quanh truïc vuoâng goùc vôùi maët phaúng maùy bay, theo nguyeân lyù baûo toaøn momanh ñoäng löôïng thì 2 caëp caùnh quay ngöôïc nhau seõ taïo ra caëp momanh ñoäng löôïng ñoái nghòch trieät tieâu laãn nhau.

Hình 3 : Ñieàu khieån maùy bay bay leân

Bay theo moät höôùng :- Giöõ nguyeân toác ñoä 2 caùnh theo höôùng khoâng dòch

chuyeån .- Taêng toác ñoä caùnh naèm ôû goác vector theo phöông dòch

chuyeån .- Giaûm toác ñoä caùnh naèm ôû ñænh vector theo phöông dòch

chuyeån .

Hình 4 : Ñieàu khieån maùy bay bay theo 1 höôùng

Khi chuùng ta giöõ nguyeân toác ñoä 2 caùnh theo phöông khoâng dòch chuyeån va taêng, giaûm toác ñoä 2 caùnh theo phöông dòch chuyeån khi ñoù chuùng ta seõ taïo cho maùy bay moät goùc ngieâng theo phöông dòch chuyeån . Chính nhôø goùc nghieâng naøy maø löïc naâng cuûa caùnh quaït khoâng coøn naèm theo phöông thaúng ñöùng , vaø toàn taïi thaønh phaàn löïc höôùng theo phöông chuyeån ñoäng . Chính thaønh phaàn löïc naøy ñaõ laøm maùy bay bay theo phöông ngang .

Hình 5 : Löïc taùc duïng leân thaân maùy bay khi noù bò nghieâng

Neáu chuùng ta muoán maùy bay bay theo phöông naøo cuûa thaân thì chuùng ta aùp duïng nguyeân taéc ñieàu khieån treân vaøo phöông ñoù .

Xoay voøng quanh truïc vuoâng goùc vôùi maët phaúng maùy bay:- Khoâng maát tính toång quaùt , ta giaû söû caàn ñieàu khieån maùy bay xoay ngöôïc chieàu kim ñoàng hoà nhö hình veõ. Ta phaûi taêng vaän toác cuûa 2 caùnh ñoái dieän maø taêng momanh ñoäng löôïng khieán maùy bay xoay ngöôïc chieàu kim ñoâng hoà vaø giaûm vaän toác 2 caùnh coøn laïi ñeå giaûm momanh ñoäng löôïng theo phöông ngöôïc vôùi chieàu xoay mong muoán cuûa maùy bay .

Hình 6 : Ñieàu khieån maùy bay bay voøng quanh truïc vuoâng goùc vôùi maët phaúng maùy bay.

IV. XAÂY DÖÏNG MOÂ HÌNH QUADRO TRONG VISUAL NASTRAN :Phaàn meàm Visual Nastran laø phaàn meàm giuùp baïn taïo

ñöôïc moâi tröôøng vaät lyù gaàn gioáng vôùi thöïc teá ñeå moâ phoûng hoaït ñoäng cuûa robot . Vôùi vieäc duøng phaàn meàm Visual Nastran keát hôïp vôùi Simulink cuûa Matlab giuùp ta coù moät moâi tröôøng moâ phoûng gaàn vôùi thöïc teá nhaát. Trong Visual Nastran caùc yeáu toá troïng löôïng , vaän toác , gia toác , momanh quaùn tính ñeàu ñöôïc xeùt ñeán .

Ban ñaàu ta caàn phaûi xaây döïng moâ hình cô khí cho maùy bay tröôùc , vieäc xaây döïng moâ hình naøy ñöôïc thöïc hieän baèng phaàn meàm Pro/Engineer .

Hình 7: Moâ hình cô khí cuûa maùy bay ñöôïc xaây döïng baèng phaàn meàm Pro/Engineer .

Moâ hình treân laø moâ hình caûi tieán cuûa maùy bay maø nhoùm ñaõ xaây döïng sau nhaèm ñaùp öùng caùc yeâu caàu thöïc teá, coøn döôùi ñaây laø moâ hình maø nhoùm duøng ñeå moâ phoûng trong phaàn meàm Visual Nastran . Giöõa thaân maùy bay vaø 4 caùnh quaït ñöôïc gaén 4 khôùp quay .Khi caùnh quaït quay ta xaây döïng haøm truyeàn giöõa vaän toác caùnh quaït vaø löïc naâng cuûa noù . Vieäc tính toaùn haøm truyeàn naøy ñöôïc thöïc hieän trong moâi tröôøng MatLab , sau khi tính ñöôïc löïc naâng , ta ñöa löïc naøy vaøo trong moâi tröông Visual Nastran ( do trong Nastran khoâng coù chöùc naêng taïo löïc naâng khi caùnh quaït quay ) . Vieäc ñöa löïc vaøo 4 caùnh quaït chæ ñöôïc thöïc hieän vôùi ñoä chính xaùc töông ñoái , nghóa laø caùc löïc naøy khoâng ñoái xöùng nhau hoaøn toaøn . Laøm nhö vaäy ta coù theå kieåm tra ñöôïc ñaùp öùng cuûa luaät ñieàu khieån , bôûi vì khi maùy bay hoaït ñoäng trong moâi tröôøng thöïc teá thì caùc löïc naâng naøy cuõng khoâng hoaøn toaøn ñoái xöùng .

Hình 8 : Moâ hình maùy bay trong Visual Nastran

Trong caùc cöûa soå giao dieän cuûa Visual Nastran ta cho hieän leân caùc cöûa soå thoâng baùo löïc taùc duïng leân 4 caùnh maùy bay, goùc nghieâng theo 2 phöông x vaø y .

V. XAÂY DÖÏNG MOÂ HÌNH ÑIEÀU KHIEÅN CUÛA QUADROCOPTER TRONG SIMULINK CUÛA MATLAB :

Nhiệm vụ của phaàn naøy là xây dựng một mô hình điều khiển máy bay QuadroCopter . Nếu ta muốn điều khiển hướng di chuyển của QuadroCopter thì trước tiên ta phải điểu khiển được độ nghiêng của máy bay theo 2 phương x , y ( oxy là mặt phẳng song song với mặt đất ).

Hình vẽ ký hiệu các cánh của QuadroCopter

Nếu ta muốn cho máy bay nghiêng theo phương X thì ta phải thay đổi tỷ lệ vận tốc của cánh A và cánh C ( sự thay đổi vận tốc tương ứng với sự thay đổi lực nâng và lực vuông góc với trục ) . Tương tự nếu ta muốn thay đổi góc nghiêng theo phương Y thì ta phải thay đổi tỷ lệ vận tốc của 2 cánh B và D .

Công thức liên hệ giữa lực nâng F ( vuông góc với mặt phẳng chứa cánh quạt ) và vận tốc gốc ω của máy bay là :

F = k.ω2 với k là hằng số phụ thuộc vào môi trường và cấu tạo của cánh máy bay .

Mô hình điều khiển được xây dựng trên môi trường Simulink của MatLab kết hợp với VisualNastran .

Dưới đây là mô hình điều khiển độ nghiêng của máy bay :

X

Y

http://upload.wikimedia.org/wikipedia/commons/e/e2/Quadrocopter_schema.png

Sơ đồ điều khiển trên bao gồm 3 phần :- Phần có khối Controller dùng để điều khiển độ nghiêng theo phương X .- Phần có khối Controller1 dùng để điều khiển độ nghiêng theo phương Y .- Phaàn coù khoái controller2 duøng ñeå ñieàu khieån ñoä cao

theo phöông Z .Do 2 khối ñaàu tương tự như nhau nên ta chỉ cần chỉ ra các khối con trong 1 khối là đủ:

- Khối Step : chung cho cả 2 khối lớn , đây là khối tạo ra giá trị tham khảo

về độ nghiêng của máy bay theo 2 phương X, Y

Giá trị tham khảo của góc nghiêng được cho như đồ thị sau :

- Khối PID : Kp = 30 , KI = 15 , KD = 25

- Khối : Dùng để lượng tử hoá tín hiệu đầu vào và giữ nó

trong một chu kỳ tính toán . - Khối hàm truyền của động cơ : tín hiệu đầu vào là điện áp , tín hiệu đầu ra là vận

tốc góc .

- Khối bão hoà và khối giới hạn mức tăng nhằm giữ cho tín hiệu đầu ra nằm trong khoảng giới hạn vật lý cho phép của mô hình .

- Khối tính toán : với đầu vào là độ thay đổi vận tốc gốc và đầu ra là sự phân chia sự thay đổi giá trị đầu vào của vận tốc gốc cho 2 cánh .

Nội dung trong khối này như sau:function y = fcn(u)% This block supports an embeddable subset of the MATLAB language.% See the help menu for details. y1= (u+sqrt(16-u*u))/2;

y2= (-u+sqrt(16-u*u))/2 ;y = [y1 y2];

- Khối hàm truyền từ vận tốc gốc sang lực vuông góc với cánh :

- Khối VisualNastran :

Khối này gồm 4 đầu vào đó là lực nâng của 4 cánh quạt . Đầu ra là độ nghiêng của máy bay theo phương X và Y .Nếu muốn có cánh quạt quay để hình ảnh mô phỏng trực quan gần với thực tế hơn ta có thể thêm vào 4 đầu vào vận tốc cho khối VisualNastran .YÙ töôûng xaây döïng khoái naøy laø töø goùc nghieâng ño veà ta so saùnh vôùi giaù trò tham khaûo ñeå ñöa ra sai soá goùc nghieâng , töø sai soá goùc nghieâng ta ñöa vaøo khoái PID tính ra ñöôïc möùc vol cheânh leäch caàn ñöa vaøo ñeå ñieàu khieån 2 caùnh quaït . Töø möùc Vol naøy , sau khi qua haøm truyeàn cuûa ñoäng cô ta ñöôïc möùc cheânh leäch vaän toác cuûa 2 caùnh quaït . Sau ñoù ta ñöa ñoä cheânh leäch naøy vaøo khoái chia vaän toác cho moãi ñoäng cô sao cho hôïp lyù nhaát.

Sau đây là kết quả mô phỏng được thể hiện trên VisualNastran :

Đồ thị trên biểu diễn góc nghiêng của máy bay theo phương X và Y theo thời gian ( giá trị max và min của đồ thị góc trên là từ - 90 0 đến 90 0 ) . Trong đồ thị ta thấy trong 5 s đầu góc tương ứng theo 2 phương hướng tới trạng thái ổn định ( 00 ) và sau 5 s thì góc nghiêng chuyển sang 180 . Để thấy được kết quả của luật điều khiển ta so sánh đồi thị trên với đồ thị giá trị tham khảo của khối Step đã được đưa ra trong phần khối Step . Kết quả mô phỏng bằng hình ảnh cho ta thấy ban đầu ( trong 5s đầu ) 2 góc của máy bay là 00 tương ứng với việc giữ thăng bằng trên không của máy bay . Và sau 5s góc điều khiển theo 2 phương là 180 nên ta thấy được đáp ứng của máy bay là thay đổi hướng ngay lập tức ứng với phương nghiêng điều khiển.

Khoái ñieàu khieån ñoä cao :

Söï khaùc bieät cuûa khoái ñieàu khieån ñoä cao so vôùi 2 khoái kia laø ôû choã , trong moãi khoái ñieàu khieån goùc nghieâng chæ taùc ñoäng ñeán 2 caùnh quaït töông öùng vôùi goùc nghieâng ñoù.

Coøn trong khoái ñieàu khieån ñoä cao thì taùc ñoäng 1 luùc leân 4 caùnh quaït . Cuï theå nhö sau, sau khi ñoïc giaù trò ñoä cao veà ta so saùnh vôùi giaù trò tham khaûo thì thu ñöôïc sai soá ñoä cao . Sai soá naøy ñöôïc ñöa qua khoái PID ñeå tính ra toång möùc Vol caàn thay ñoåi . Giaù trò naøy laïi laø ñaàu vaøo cuûa khoái moâ hình hoaù ñoäng cô , ñaàu ra cuûa khoái moâ hình hoaù ñoäng cô chính laø vaän toác caàn thay ñoåi . Vaän toác naøy ñöôïc chia ñeàu cho 4 caùnh quaït ñeå ñaûm baûo khoâng gaây aûnh höôûng nhieàu tôùi ñoä nghieâng .

Ñeå vieäc moâ phoûng ñöôïc tröïc quan hôn vaø deã söû duïng hôn nhoùm ñaõ thieât keá giao dieän vôùi ngöôøi duøng trong moâi tröôøng Matlab :

Hình veõ giao dieän ñieàu khieån maùy bay moâ hìnhGiao dieän coù caùc thanh tröôït vaø caùc Edit box ñeå ngöôøi duøng coù theå nhaäp giaù trò ñieàu khieån vaøo maùy bay hoaëc quan saùt caùc giaù trò ño ñöôïc töø maùy bay .Caùc thanh tröôït X , Y laø ñeå ñieàu khieån goùc nghieâng theo phöông X vaø Y .Thanh tröôït Z duøng ñeå ñieån khieån ñoä cao theo phöông Z .Caùc thanh tröôït V1, V2, V3, V4 laø duøng ñeå ñieàu khieån vaän toác cuûa caùc caùnh A, B, C, D cuûa maùy bay .

Phaàn Code cuûa giao dieän ñöôïc ñeå ôû phaàn phuï luïc .

VI. LYÙ THUYEÁT VEÀ BOÄ LOÏC KALMAN :1. Giôùi thieäu boä loïc Kalman:

Naêm 1976 R.E. Kalman ñaõ ñöa ra baøi vieát mieâu taû phöông phaùp giaûi hoài quy cho nhöõng baøi toaùn loïc tín hieäu rôøi raïc. Keå töø ñoù, vôùi nhöõng öu ñieåm lôùn trong tính toaùn, boä loïc Kalman laø ñeà taøi cuûa nhieàu nghieân cöùu vaø öùng duïng, cuï theå laø trong lónh vöïc töï ñoäng hay hoã trôï doø ñöôøng.

Boä loïc kalman laø taäp hôïp nhöõng phöông trình toaùn hoïc nhaèm tính toaùn caùc giaù trò trung bình ñeå öôùc löôïng traïng thaùi cuûa quaù trình, baèng caùch giaûm thieåu giaù trò trung bình cuûa bình phöông sai soá. Boä loïc kalman raát maïnh ôû nhieàu maët: noù cung caáp giaù trò öôùc löôïng cuûa traïng thaùi tröôùc, hieän taïi, vaø keá tieáp, vaø noù coù theå hoaït ñoäng ngay caû khi khoâng xaùc ñònh ñöôïc traïng thaùi chính xaùc cuûa heä thoáng ñöôïc moâ hình hoaù.

Boä loïc kalman ñöôïc phaân laøm 2 loaïi : rôøi raïc (discrete) vaø môû roäng (extended) Trong ñoà aùn naøy nhoùm xin trình baøy nhöõng noäi dung cô baûn veà boä loïc kalman, vaø moät soá ví duï cuï theå.

2. Boä loïc kalman rôøi raïc ( Discrete Kalman filer)

Boä loïc kalman öôùc löôïng quaù trình rôøi raïc coù traïng thaùi , vôùi phöông trình update tuyeán tính sau:

xk = Axk-1 + Buk-1 + wk-1

Vaø vôùi giaù trò ño * laø:zk = Hxk + vk

Hai bieán ngaãu nhieân w vaø v ñaëc tröng cho nhieãu quaù trình vaø nhieãu thieát bò ño. Chuùng ñoäc laäp vôùi nhau, laø nhieãu traéng vaø ñeàu coù xaùc xuaát tuaân theo phaân phoái chuaån:

p(w) ~ N(0,Q)p(v) ~ N(0,R)Trong thöïc teá caùc ma traän covariance cuûa nhieãu quaù trình

Q vaø nhieãu thieát bò ño R laø thay ñoåi nhöng ôû ñaây ta giaû söû chuùng khoâng thay ñoåi.

Ma traän vuoâng nxn A trong phöông trình (1) bieåu dieãn moái quan heä giöõa traïng thaùi tröôùc ñoù vaø traïng thaùi hieän taïi trong tröôøng hôïp khoâng coù nhieãu quaù trình. Chuù yù raèng A thay ñoåi ñoái vôùi töøng traïng thaùi nhöng ôû ñaây ta giaû söû A khoâng ñoåi.

Time update (“predict”)Öôùc löôïng giaù trò keá tieáp:

xk = Axk-1 + Buk-1 + wk-1

Öôùc löôïng ma traän covariance keá tieáp:

Measurement update (“ corect”)1.Tính toaùn ñoä lôïi Kalman:

2.Caäp nhaäp laïi vôùi giaù trò ño zk

3.Caäp nhaät laïi ma traän covariance

Khôûi taïo va Pk-1

Ma traän B nxl bieåu dieãn quan heä giöõa bieán ñieàu khieån ngoõ vaøo vaø traïng thaùi. Ma traän H mxn bieåu dieãn quan heä giöõa giaù trò ño vaø traïng thaùi cuûa quaù trình. Trong thöïc theá, H thay ñoåi nhöng ta giaû söû H khoâng ñoåi.

Quaù trình tính toaùn cuûa boä loïc:

Ta ñònh nghóa laø traïng thaùi tröôùc öôùc löôïng taïi

böôùc k, cho bieát traïng thaùi cuûa quaù trình ngay tröôùc traïng

thaùi k, vaø laø traïng thaùi sau öôùc löôïng taïi böôùc k, vaø

cho ra giaù trò ño la zk . Ta ñònh nghóa caùc sai soá tröôùc vaø sau khi öôùc löôïng laø:

Ma traän covariance sai soá tröôùc vaø sau öôùc löôïng seõ laø:

Quaù trình xöû lyù cuûa boä loïc kalman chia laøm hai giai ñoaïn : caäp nhaäp thôøi gian ( time update ) vaø caäp nhaäp giaù trò ño ( mesurement update ). Quaù trình update tính toaùn caùc giaù trò

tröôùc öôùc löôïng: ma traän vaø cho neân ñaây coøn goïi laø

quaù trình tieân ñoaùn ( predictor). Quaù trình measurement update

seõ cho ra nhöõng giaù trò sau khi öôùc löôïng Pk vaø töø nhöõng

giaù trò coù ñöôïc ôû time update, cho neân coøn goïi laø quaù trình chænh söõa (corrector) . Caùc phöông trình tính toaùn nhö sau:

Quaù trình tính toaùn cuûa boä loïc DKFTrong ñoù L chính laø heä soá thaëng dö cuûa boä loïc Kalman, hay ñoä lôïi Kalman.

Quaù trình tính toaùn cuûa boä loïc kalman laø moät quaù trình qua laïi tuaàn hoaøn giöõa predict vaø corrector .

Trong quaù trình thöïc thi thöïc teá cuûa boä loïc, ma traän covariance cuûa nhieãu thieát bò ño R ñöôïc xaùc ñònh tröôùc. R ñöôïc ño off_line baêng caùch cho moät soá giaù trò ño maãu, khaûo saùt vaø xaùc ñònh söï bieán ñoåi cuûa nhieãu ño. Ta cuõng coù theå xaùc ñònh Q töø thöïc nghieäm nhöng khoù khaên hôn nhieàu bôûi vì ñoâi khi ta khoâng theå quan saùt ñöôïc tröïc tieáp quaù trình ñang xaûy ra.

Q vaø R chính laø hai thoâng soá ñeå hieäu chænh boä loïc Kalman . Quaù trình hieäu chænh thöôøng ñöôïc thöïc hieän off_line .

3. Moâ hình hoaù gyro:

Gyro laø caûm bieán ño vaän toác goùc .Goùc nghieâng theo phöông x cuûa maùy bay ôû traïng thaùi

hieän taïi baèng goùc theo phöông x ôû traïng thaùi tröôùc coäng vôùi tích phaân cuûa vaän toác goùc. Ta coù phöông trình sau:

Trong ñoù : laø giaù trò goùc nghieâng ôû traïng thaùi thöù k+1

laø traïng thaùi goùc nghieâng ôû traïng thaùi tröôùc ñoù

laø vaän toác goùc ño bôûi gyûo ôû traïng thaùi thöù k laø thôøi gian laáy maãu

laø ñoä troâi cuûa heä soá gyûo bias

3. Moâ hình hoaù Accelerometer :

Töông töï nhö gyro, giaù trò vò trí ta tính ñöôïc cuõng bò sai soá do hai laàn laáy tích phaân.Phöông trình traïng thaùi cuûa accelerometer nhö sau:

Trong ñoù:, laàn löôït laø vaän toác daøi cuûa maùy bay ôû 2

traïng thaùi keá tieáp k vaø k+1a(k) laø gia toác cuûa xe ôû traïng thaùi thöù k

bg(k) laø heä soá troâi cuûa accelerometerxa(k), xa(k) laàn löôït laø vò trí theo phöông x cuûa maùy bay ôû

2 traïng thaùi keá tieáp k vaø k+1ya(k), ya(k+1) laàn löôït laø vò trí theo phöông y cuûa maùy bay

ôû 2 traïng thaùi keá tieáp k vaø k+1.

4. Thieát keá sô ñoà maïch nguyeân lyù cuûa gyro vaø accelerometer

Caûm bieán gia toác maø nhoùm söû duïng laø caûm bieán gia toác 3 truïc , coù nghóa laø vôùi caûm bieán accelerometer naøy coù theå ño ñöôïc gia toác theo 3 phöông X, Y, Z .

g -s e le c t 2

X_DATA

3 . 3V _1

U 1

MMA7260

1234

5 6 7 8

91 01112

13141516

g -S e le c t 1g -S e le c t 2VD DVSS

NC1

NC2

NC3

NC4

N C 5N C 6N C 7

~s leep_m ode

ZOUT

YOUT

XOUT

NC8

Z_DATA

g -s e le c t 1

Y_DATA

Z O U T

R 3

1K

Y O U T

Z _D ATA

C 4

0 . 1uF

XO U T

Y _D ATAR 2

1k

C 3

0 . 1uF

C 5

0 . 1uF

R 1

1k

X_D ATA

Sô ñoà nguyeân lyù caûm bieán gia toác

Caûm bieán MMA7260 caàn nguoàn cung caáp laø 3.3 V . Caùc chaân chuùng ta söû duïng ôû ñaây laø chaân g-Select1 ( chaân soá 1) , g-Select2 ( chaân soá 2 ) duøng ñeå choïn cheá ñoä hoaït ñoäng caûm bieán . Ba chaân döõ lieäu XOUT (15), YOUT (14), ZOUT (13) laø 3 chaân ñöa ra möùc Vol tyû leä vôùi gia toác theo 3 phöông X, Y, Z .

Treân moâ hình thí nghieäm nhoùm söû duïng 2 caûm bieán gyro ño theo phöông X vaø Y.

5V

3 . 3V _1

U 6

G y ro

1 2

3 4

5 6

5V N C

3 .3V C +

G N D D ATAD ATA 1

C 1

C

Sô ñoà nguyeân lyù caûm bieán gyro

Caûm bieán gyro caàn 2 nguoàn cung caáp : 1 nguoàn laø 3.3 V , moät nguoàn laø 5 V . Moät chaân döõ lieäu cho ra möùc Vol tyû leä vôùi vaän toác goùc theo phöông maø gyro ño .

8.Phuï Luïc :

Code chöông trình thieát keá giao dieän baèng phaàn meàm MatLab

function varargout = GUI1(varargin)% GUI1 M-file for GUI1.fig% GUI1, by itself, creates a new GUI1 or raises the existing% singleton*.%% H = GUI1 returns the handle to a new GUI1 or the handle to% the existing singleton*.%% GUI1('CALLBACK',hObject,eventData,handles,...) calls the local% function named CALLBACK in GUI1.M with the given input arguments.%% GUI1('Property','Value',...) creates a new GUI1 or raises the% existing singleton*. Starting from the left, property value pairs are% applied to the GUI before GUI1_OpeningFunction gets called. An% unrecognized property name or invalid value makes property application% stop. All inputs are passed to GUI1_OpeningFcn via varargin.%% *See GUI Options on GUIDE's Tools menu. Choose "GUI allows only one% instance to run (singleton)".%% See also: GUIDE, GUIDATA, GUIHANDLES

% Copyright 2002-2003 The MathWorks, Inc.

% Edit the above text to modify the response to help GUI1

% Last Modified by GUIDE v2.5 28-Feb-2008 13:00:33

% Begin initialization code - DO NOT EDIT

gui_Singleton = 1;gui_State = struct('gui_Name', mfilename, ... 'gui_Singleton', gui_Singleton, ... 'gui_OpeningFcn', @GUI1_OpeningFcn, ... 'gui_OutputFcn', @GUI1_OutputFcn, ... 'gui_LayoutFcn', [] , ... 'gui_Callback', []);if nargin && ischar(varargin{1}) gui_State.gui_Callback = str2func(varargin{1});end

if nargout [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});else gui_mainfcn(gui_State, varargin{:});end% End initialization code - DO NOT EDIT% --- Executes just before GUI1 is made visible.function GUI1_OpeningFcn(hObject, eventdata, handles, varargin)% This function has no output args, see OutputFcn.% hObject handle to figure% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA)% varargin command line arguments to GUI1 (see VARARGIN)

% Choose default command line output for GUI1handles.output = hObject;

% Update handles structureguidata(hObject, handles);

% UIWAIT makes GUI1 wait for user response (see UIRESUME)% uiwait(handles.figure1);

% --- Outputs from this function are returned to the command line.function varargout = GUI1_OutputFcn(hObject, eventdata, handles) % varargout cell array for returning output args (see VARARGOUT);% hObject handle to figure% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA)

% Get default command line output from handles structurevarargout{1} = handles.output;

% --- Executes on slider movement.

function sliderX_Callback(hObject, eventdata, handles)x=get(handles.sliderX,'value');x=num2str(x);set(handles.editX,'string',x);set_param('son1/StepX','After',num2str(get(handles.sliderX,'value')/10000));% hObject handle to sliderX (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'Value') returns position of slider% get(hObject,'Min') and get(hObject,'Max') to determine range of slider

% --- Executes during object creation, after setting all properties.function sliderX_CreateFcn(hObject, eventdata, handles)% hObject handle to sliderX (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles empty - handles not created until after all CreateFcns called

% Hint: slider controls usually have a light gray background, change% 'usewhitebg' to 0 to use default. See ISPC and COMPUTER.usewhitebg = 1;if usewhitebg set(hObject,'BackgroundColor',[.9 .9 .9]);else set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor'));end

% --- Executes on slider movement.function sliderY_Callback(hObject, eventdata, handles)Y=get(handles.sliderY,'value');Y=num2str(Y);set(handles.editY,'string',Y);set_param('son1/StepY','After',num2str(get(handles.sliderY,'value')/10000));% hObject handle to sliderY (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'Value') returns position of slider

% get(hObject,'Min') and get(hObject,'Max') to determine range of slider

% --- Executes during object creation, after setting all properties.function sliderY_CreateFcn(hObject, eventdata, handles)% hObject handle to sliderY (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles empty - handles not created until after all CreateFcns called


% --- Executes on slider movement.function sliderZ_Callback(hObject, eventdata, handles)Z=get(handles.sliderZ,'value');Z=num2str(Z);set(handles.editZ,'string',Z);% hObject handle to sliderZ (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'Value') returns position of slider% get(hObject,'Min') and get(hObject,'Max') to determine range of slider

% --- Executes during object creation, after setting all properties.function sliderZ_CreateFcn(hObject, eventdata, handles)% hObject handle to sliderZ (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles empty - handles not created until after all CreateFcns called

% Hint: slider controls usually have a light gray background, change% 'usewhitebg' to 0 to use default. See ISPC and COMPUTER.

usewhitebg = 1;if usewhitebg set(hObject,'BackgroundColor',[.9 .9 .9]);else set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor'));end

function editx_Callback(hObject, eventdata, handles)% hObject handle to editx (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of editx as text% str2double(get(hObject,'String')) returns contents of editx as a double

% --- Executes during object creation, after setting all properties.function editx_CreateFcn(hObject, eventdata, handles)% hObject handle to editx (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.% See ISPC and COMPUTER.if ispc set(hObject,'BackgroundColor','white');else set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor'));end

function editY_Callback(hObject, eventdata, handles)% hObject handle to editY (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of editY as text

% str2double(get(hObject,'String')) returns contents of editY as a doubley=get(handles.editY,'string');y=str2num(y);set(handles.sliderY,'value',y);

% --- Executes during object creation, after setting all properties.function editY_CreateFcn(hObject, eventdata, handles)% hObject handle to editY (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles empty - handles not created until after all CreateFcns called


function editZ_Callback(hObject, eventdata, handles)% hObject handle to editZ (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of editZ as text% str2double(get(hObject,'String')) returns contents of editZ as a doublez=get(handles.editZ,'string');z=str2num(z);set(handles.sliderZ,'value',z);

% --- Executes during object creation, after setting all properties.function editZ_CreateFcn(hObject, eventdata, handles)% hObject handle to editZ (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.% See ISPC and COMPUTER.if ispc

set(hObject,'BackgroundColor','white');else set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor'));end

% --- Executes on slider movement.function sliderV1_Callback(hObject, eventdata, handles)% hObject handle to sliderV1 (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'Value') returns position of slider% get(hObject,'Min') and get(hObject,'Max') to determine range of sliderV1=get(handles.sliderV1,'value');V1=num2str(V1);set(handles.editV1,'string',V1);set_param('son1/Pulse Generator1','Amplitude',num2str(1000*get(handles.sliderV1,'value')));

% --- Executes during object creation, after setting all properties.function sliderV1_CreateFcn(hObject, eventdata, handles)% hObject handle to sliderV1 (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles empty - handles not created until after all CreateFcns called


% --- Executes on slider movement.function sliderV2_Callback(hObject, eventdata, handles)% hObject handle to sliderV2 (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB

% handles structure with handles and user data (see GUIDATA)








function editv1_Callback(hObject, eventdata, handles)% hObject handle to editv1 (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of editv1 as text% str2double(get(hObject,'String')) returns contents of editv1 as a double

% --- Executes during object creation, after setting all properties.function editv1_CreateFcn(hObject, eventdata, handles)% hObject handle to editv1 (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.% See ISPC and COMPUTER.if ispc set(hObject,'BackgroundColor','white');else set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor'));

end



% --- Executes during object creation, after setting all properties.function editv2_CreateFcn(hObject, eventdata, handles)% hObject handle to editv2 (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles empty - handles not created until after all CreateFcns called




% --- Executes during object creation, after setting all properties.function editv3_CreateFcn(hObject, eventdata, handles)% hObject handle to editv3 (see GCBO)

% eventdata reserved - to be defined in a future version of MATLAB% handles empty - handles not created until after all CreateFcns called





% Hint: slider controls usually have a light gray background, change% 'usewhitebg' to 0 to use default. See ISPC and COMPUTER.usewhitebg = 1;if usewhitebg set(hObject,'BackgroundColor',[.9 .9 .9]);else

set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor'));end

function editV4_Callback(hObject, eventdata, handles)% hObject handle to editV4 (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of editV4 as text% str2double(get(hObject,'String')) returns contents of editV4 as a doubleV4=get(handles.sliderV4,'value');V4=num2str(V4);set(handles.editV4,'string',V4);

% --- Executes during object creation, after setting all properties.function editV4_CreateFcn(hObject, eventdata, handles)% hObject handle to editV4 (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles empty - handles not created until after all CreateFcns called



% Hints: get(hObject,'String') returns contents of editV1 as text% str2double(get(hObject,'String')) returns contents of editV1 as a double






% Hint: edit controls usually have a white background on Windows.% See ISPC and COMPUTER.if ispc set(hObject,'BackgroundColor','white');else

set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor'));end





function editX_Callback(hObject, eventdata, handles)% hObject handle to editX (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of editX as text% str2double(get(hObject,'String')) returns contents of editX as a doublex=get(handles.editX,'string');x=str2num(x);set(handles.sliderX,'value',x);

% --- Executes during object creation, after setting all properties.function editX_CreateFcn(hObject, eventdata, handles)% hObject handle to editX (see GCBO)

% eventdata reserved - to be defined in a future version of MATLAB% handles empty - handles not created until after all CreateFcns called


% --- Executes on button press in SimulateButton.function model_open(handles) open_system('son1'); set_param('son1/Pulse Generator1','Amplitude',num2str(1000*get(handles.sliderV1,'value'))); set_param('son1/Pulse Generator2','Amplitude',num2str(1000*get(handles.sliderV2,'value'))); set_param('son1/Pulse Generator3','Amplitude',num2str(1000*get(handles.sliderV3,'value'))); set_param('son1/Pulse Generator4','Amplitude',num2str(1000*get(handles.sliderV4,'value'))); set_param('son1/StepX','After',num2str(get(handles.sliderX,'value')/10000)); set_param('son1/StepY','After',num2str(get(handles.sliderY,'value')/10000));function SimulateButton_Callback(hObject, eventdata, handles)% hObject handle to SimulateButton (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA) model_open(handles); set_param('son1', 'SimulationCommand', 'start')

function SimulateButton_CreateFcn(hObject, eventdata, handles)% hObject handle to SimulateButton (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles empty - handles not created until after all CreateFcns called %them ma set(hObject,'String','Simulate');

% --- Executes on button press in ExitButton.function ExitButton_Callback(hObject, eventdata, handles)% hObject handle to ExitButton (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA)

close(gui1); % --- Executes during object creation, after setting all properties.function ExitButton_CreateFcn(hObject, eventdata, handles)% hObject handle to ExitButton (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles empty - handles not created until after all CreateFcns called

% them ma set(hObject,'String','Exit');

Code cuûa haøm tính toaùn goùc nhieâng döïa vaøo thuaät toaùn Kalman extern float angle;extern float q_bias;extern float rate;

extern voidstate_update( const float q_m /* Pitch gyro measurement */);

extern voidkalman_update( const float ax_m, /* X acceleration */ const float az_m /* Z acceleration */);

#endif

Haøm kalman_update

#include <math.h>

/* * thôøi gian laáy maãu */static const float dt = ( 1024.0 * 256.0 ) / 8000000.0;

/* * Ma traän covariance , ma traän naøy ñöôïc caäp nhaäp sau moãi chu kyø tính * Ma traän naøy laø ma traän covarian sai soá tröôùc khi tính */static float P[2][2] = { { 1, 0 }, { 0, 1 },};

/* * goùc nhieâng * bias * toác ñoä */float angle;float q_bias;float rate;

/* * R laø ma traän covarian cuûa nhieuã ño . Trong tröôøng hôïp naøy * laø ma traän 1x1 , chuùng ta ñaët laø 0.3 rad ( thoâng soá naøy ño ñöôïc töø thöïc nghieäm) */static const float R_angle = 0.3;

/* * Q laø ma traän 2x2 trình baøy nhieãu quaù trình */static const float Q_angle = 0.001;static const float Q_gyro = 0.003;

/* * X = [ angle, gyro_bias ]

* Xdot = [ angle_dot, gyro_bias_dot ] * angle_dot = gyro - gyro_bias * gyro_bias_dot = 0 * Pdot = A*P + P*A' + Q * * A laø Jacobian cuûa Xdot ñöôïc tính nhu sau: * * A = [ d(angle_dot)/d(angle) d(angle_dot)/d(gyro_bias) ] * [ d(gyro_bias_dot)/d(angle) d(gyro_bias_dot)/d(gyro_bias) ] * * = [ 0 -1 ] * [ 0 0 ] */voidstate_update( const float q_m /* Pitch gyro measurement */){ const float q = q_m - q_bias;

/* * Pdot = A*P + P*A' + Q * * A = [ 0 -1, 0 0 ] * A' = [ 0 0, -1, 0 ] * Q_angle vaø Q_gyro coù trong ma traän Q */ const float Pdot[2 * 2] = { Q_angle - P[0][1] - P[1][0], /* 0,0 */ - P[1][1], /* 0,1 */ - P[1][1], /* 1,0 */ Q_gyro /* 1,1 */ };

rate = q;

/* * goùc sau khi öôùc löôïng * angle += angle_dot * dt * += (gyro - gyro_bias) * dt * += q * dt */ angle += q * dt;

/* Caäp nhaäp ma traän P */

P[0][0] += Pdot[0] * dt; P[0][1] += Pdot[1] * dt; P[1][0] += Pdot[2] * dt; P[1][1] += Pdot[3] * dt;}

/* * C = [ d(angle_m)/d(angle) d(angle_m)/d(gyro_bias) ] * = [ 1 0 ] */voidkalman_update( const float ax_m, /* X acceleration */ const float az_m /* Z acceleration */){ /* tính goùc ño vaø sai soá öôùc löôïng */ const float angle_m = atan2( -az_m, ax_m ); const float angle_err = angle_m - angle;

/* * C_0 trình baøy söï lieân quan giöõa giaù trò ño vaø traïng thaùi öôùc löôïng * * The C_1 trình baøy söï lieân quan giöõa traïng thaùi ño vôùi heä soá bias cuûa gyro */ const float C_0 = 1; /* const float C_1 = 0; */

/* * PCt<2,1> = P<2,2> * C'<2,1> */ const float PCt_0 = C_0 * P[0][0]; /* + C_1 * P[0][1] = 0 */ const float PCt_1 = C_0 * P[1][0]; /* + C_1 * P[1][1] = 0 */ /* * E = C P C' + R * E<1,1> = C<1,2> P<2,2> C'<2,1> + R<1,1> */ const float E = R_angle + C_0 * PCt_0 /* + C_1 * PCt_1 = 0 */

;

/* * K = P C' inv(E) * K<2,1> = P<2,2> C'<2,1> inv(E)<1,1> * E laø <1,1>, vì theá inverse cuûa E chæ laø 1/E. */ const float K_0 = PCt_0 / E; const float K_1 = PCt_1 / E; /* * P = P - K C P * P<2,2> -= K<2,1> C<1,2> P<2,2> * t[0,0] = C[0,0] * P[0,0] + C[0,1] * P[1,0] * t[0,0] = C[0,0] * P[0,0] = PCt[0,0] */ const float t_0 = PCt_0; /* C_0 * P[0][0] + C_1 * P[1][0] */ const float t_1 = C_0 * P[0][1]; /* + C_1 * P[1][1] = 0 */

P[0][0] -= K_0 * t_0; P[0][1] -= K_0 * t_1; P[1][0] -= K_1 * t_0; P[1][1] -= K_1 * t_1; /* * X += K * err * X<2> = X<2> + K<2,1> * err<1,1> */ angle += K_0 * angle_err; q_bias += K_1 * angle_err;}

Documents

BÁO CÁO MÔ HÌNH ĐIỀU KHIỂN QUADROCOPTER