Nội dung
1. Giới thiệu về QuadroCopter
2. Cấu tạo của QuadroCopter
3. Nguyên lý hoạt động của QuadroCopter
4. Xây dựng mô hình QuadroCopter trong VisualNastran
5. Xây dựng mô hình điều khiển QuadroCopter trong Simulink của MatLab
6. Lý thuyết về bộ lọc Kalman
7. Ứng dụng bộ lọc Kalman vào đo các góc nghiêng của máy bay
8. Hướng phát triển của đề tài .
9. Phụ Lụ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ờ có 4 cánh nên loại máy bay này dễ điều khiển hơn
và có khả năng giữ thăng bằng tốt hơn . Loại máy bay này có rất nhiều ứng dụng thực
tế, nó có thể có kích cỡ lớn để chở người và hàng hoá và cũng có thể có kích cỡ nhỏ
rất năng động có thể hoạt động ngay cả trong nhà . Loại máy bay kích cỡ bay trong
nhà hiện nay đang phát triển rất rầm rộ , nhiều sinh viên của Đức và Mỹ đã chế tạo
loại máy bay này với nhiều hình dáng và tính năng khác nhau để nghiên cứu . Trong
phần đồ án này nhóm xin được giới thiệu kết quả nghiên cứu về loại Quadrocopter
hoạt động trong nhà .
Trong đồ án này nhóm đã mô phỏng máy bay Quadrocopter bằng phần mềm
MatLab kết hợp với Visual Nastran . Sau đó nhóm đã thiết kế phần cứng máy bay và
hoàn thành một số modul nhỏ của công việc điều khiển máy bay . Trong đồ án này có
đưa vào modul đo góc nhiêng của máy bay bằng cảm biến gyro và cảm biến gia tốc
dài được ước lượng bằng bộ lọc Kalman .
CHÚ THÍCH : TÀI LIỆU TRÊN GỒM FILE PDF + WORD
41 trang |
Chia sẻ: lvcdongnoi | Lượt xem: 3417 | Lượt tải: 5
Bạn đang xem trước 20 trang tài liệu Mô hình điều khiển Quadrocopter, để xem tài liệu hoàn chỉnh bạn click vào nút DOWNLOAD ở trên
Lôøi caûm ôn
Kí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ề QuadroCopter
2. Cấu tạo của QuadroCopter
3. Nguyên lý hoạt động của QuadroCopter
4. Xây dựng mô hình QuadroCopter trong VisualNastran
5. Xây dựng mô hình điều khiển QuadroCopter trong Simulink của MatLab
6. Lyù thuyeát veà boä loïc Kalman
7. ÖÙng duïng boä loïc Kalman vaøo ño caùc goùc nghieâng cuûa maùy bay
8. 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ờ có 4 cánh nên loại máy bay này dễ điều khiển hơn
và có khả năng giữ thăng bằng tốt hơn . Loại máy bay này có rất nhiều ứng dụng thực
tế, nó có thể có kích cỡ lớn để chở người và hàng hoá và cũng có thể có kích cỡ nhỏ
rất năng động có thể hoạt động ngay cả trong nhà . Loại máy bay kích cỡ bay trong
nhà hiện nay đang phát triển rất rầm rộ , nhiều sinh viên của Đức và Mỹ đã chế tạo
loại máy bay này với nhiều hình dáng và tính năng khác nhau để nghiên cứu . Trong
phần đồ án này nhóm xin được giới thiệu kết quả nghiên cứu về loại Quadrocopter
hoạt động trong nhà .
Trong đồ án này nhóm đã mô phỏng máy bay Quadrocopter bằng phần mềm
MatLab kết hợp với Visual Nastran . Sau đó nhóm đã thiết kế phần cứng máy bay và
hoàn thành một số modul nhỏ của công việc điều khiển máy bay . Trong đồ án này có
đưa vào modul đo góc nhiêng của máy bay bằng cảm biến gyro và cảm biến gia tốc
dài được ước lượng bằng bộ lọ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 ).
X
Y
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 :
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ình
Giao 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 :
Ta ñònh nghóa nkx R∈ 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ø nkx R∈ laø traïng thaùi sau öôùc löôïng taïi
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 x ∈ Rn , 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. 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:
^−
^
ek k − x −K
= x
Pk− = E ⎡⎣ek k−T ⎤⎦
e
vaø xk töø nhöõng giaù trò coù ñöôïc ôû time update, cho neân coøn goïi laø quaù trình chænh söõa
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ø:
^
−
^
ek = xk − xk
Ma traän covariance sai soá tröôùc vaø sau öôùc löôïng seõ laø:
−
k ⎣ k ⎦
P = E ⎡ekeT ⎤
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 Pk− vaø xk 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
^
(corrector) . Caùc phöông trình tính toaùn nhö sau:
Pk = APk−1AT + Q
Time update (“predict”)
1. Öôùc löôïng giaù trò keá tieáp:
xk = Axk-1 + Buk-1 + wk-1
2. Öôùc löôïng ma traän covariance
keá tieáp:
−
Measurement update (“ corect”)
1.Tính toaùn ñoä lôïi Kalman:
Kk = Pk− H T (HPk− H T + R)−1
2.Caäp nhaäp laïi vôùi giaù trò ño zk
^ ^ − ^ −
xk = xk + Kk (zk − H xk )
3.Caäp nhaät laïi ma traän covariance
Pk k k−= (I − K H )P
Khôûi taïo
^ −
xk−1 va Pk-1
Quaù trình tính toaùn cuûa boä loïc DKF
Trong ñ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:
èg (k +1) = èg (k) + ù(k).Ät + bg (k)
Trong ñoù :
èg (k +1) laø giaù trò goùc nghieâng ôû traïng thaùi thöù k+1
èg (k) laø traïng thaùi goùc nghieâng ôû traïng thaùi tröôùc ñoù
ù(k) laø vaän toác goùc ño bôûi gyûo ôû traïng thaùi thöù k
Ät laø thôøi gian laáy maãu
bg (k) 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:
v(k +1) = v(k) + a(k).Ät + ba (k)
xa (k +1) = xa (k ) + v(k).cosè (k ).Ät
ya (k +1) = ya (k) + v(k).sinè (k ).Ät
Trong ñoù:
v(k ) , v(k +1) 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+1
a(k) laø gia toác cuûa xe ôû traïng thaùi thöù k
bg(k) laø heä soá troâi cuûa accelerometer
xa(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+1
ya(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.
X_DATA
Y_DATA
Z_DATA
NC8
XOUT
YOUT
ZOUT
5
6
7
8
NC1
NC2
NC3
NC4
16
15
14
13
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 .
R1
Z_DATA
1k
C3
ZOUT
0.1uF
R2
U1
MMA7260
Y_DATA
1k
YOUT
C4
0.1uF
g-select1
g-select2
3.3V_1
1
2
3
4
g-Select1
g-Select2
VDD
VSS
~sleep_mode
NC7
NC6
NC5
12
11
10
9
X_DATA
R3
1K
XOUT
C5
0.1uF
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.
U6
5V
3.3V_1
1 2
5V NC
3 4
3.3V C+
C1
GND DATA DATA1
5 6
C
Gy ro
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 GUI1
handles.output = hObject;
% Update handles structure
guidata(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 structure
varargout{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
% 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 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 double
y=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
% 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 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 double
z=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 slider
V1=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
% 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 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)
% Hints: get(hObject,'Value') returns position of slider
% get(hObject,'Min') and get(hObject,'Max') to determine range of slider
V2=get(handles.sliderV2,'value');
V2=num2str(V2);
set(handles.editV2,'string',V2);
set_param('son1/Pulse
Generator2','Amplitude',num2str(1000*get(handles.sliderV2,'value')));
% --- Executes during object creation, after setting all properties.
function sliderV2_CreateFcn(hObject, eventdata, handles)
% hObject handle to sliderV2 (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 sliderV3_Callback(hObject, eventdata, handles)
% hObject handle to sliderV3 (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
V3=get(handles.sliderV3,'value');
V3=num2str(V3);
set(handles.editV3,'string',V3);
set_param('son1/Pulse
Generator3','Amplitude',num2str(1000*get(handles.sliderV3,'value')));
% --- Executes during object creation, after setting all properties.
function sliderV3_CreateFcn(hObject, eventdata, handles)
% hObject handle to sliderV3 (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 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
function editv2_Callback(hObject, eventdata, handles)
% hObject handle to editv2 (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 editv2 as text
% str2double(get(hObject,'String')) returns contents of editv2 as a double
% --- 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
% 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 editv3_Callback(hObject, eventdata, handles)
% hObject handle to editv3 (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 editv3 as text
% str2double(get(hObject,'String')) returns contents of editv3 as a double
% --- 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: 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 sliderV4_Callback(hObject, eventdata, handles)
% hObject handle to sliderV4 (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
V4=get(handles.sliderV4,'value');
V4=num2str(V4);
set(handles.editV4,'string',V4);
set_param('son1/Pulse
Generator4','Amplitude',num2str(1000*get(handles.sliderV4,'value')));
% --- Executes during object creation, after setting all properties.
function sliderV4_CreateFcn(hObject, eventdata, handles)
% hObject handle to sliderV4 (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 double
V4=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
% 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 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
function editV3_Callback(hObject, eventdata, handles)
% hObject handle to editV3 (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 editV3 as text
% str2double(get(hObject,'String')) returns contents of editV3 as a double
% --- 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: 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 editV2_Callback(hObject, eventdata, handles)
% hObject handle to editV2 (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 editV2 as text
% str2double(get(hObject,'String')) returns contents of editV2 as a double
% --- 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
% 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 double
x=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
% 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 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 void
state_update(
);
const float q_m /* Pitch gyro measurement */
extern void
kalman_update(
const float ax_m, /* X acceleration */
const float az_m /* Z acceleration */
);
#endif
Haøm kalman_update
#include
/*
* 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]
*/
void
state_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) ]
* =[10]
*/
void
kalman_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 = P * C'
*/
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 = C P C' + R
*/
const float E =
R_angle
+ C_0 * PCt_0
/* + C_1 * PCt_1 = 0 */
;
/*
* K = P C' inv(E)
* K = P C' inv(E)
* E laø , 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-KCP
* P -= K C P
* 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 = X + K * err
*/
angle += K_0 * angle_err;
}
q_bias += K_1 * angle_err;
Các file đính kèm theo tài liệu này:
- MO_HINH_DIEU_KHIEN_QUADROCOPTER.docx
- MO_HINH_DIEU_KHIEN_QUADROCOPTER.pdf