Đồ Án Tốt Nghiệp Quadcopter Có Khả Năng Vận Chuyển Hàng Hóa, Đồ Án Tốt Nghiệp

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ơnvà 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ựctế, 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 trongnhà 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ạoloạ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 . Trongphần đồ án này nhóm xin được giới thiệu kết quả nghiên cứu về loại Quadrocopterhoạt động trong nhà .Trong đồ án này nhóm đã mô phỏng máy bay Quadrocopter bằng phần mềmMatLab 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ốcdài được ước lượng bằng bộ lọc Kalman .

Đang xem: đồ án tốt nghiệp quadcopter

Xem thêm: mẫu danh sách cấp phát văn phòng phẩm

Bạn đang xem nội dung tài liệu Đồ án Mô hình điều khiển Quadrocopter, để tải tài liệu về máy bạn click vào nút DOWNLOAD ở trên

Xem thêm: viết bài tập làm văn số 1 nghị luận xã hội

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ïcBaù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ättì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öõngyeâ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 ñöôïcnhieà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ânveà 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ìnhthöï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âmhô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ỂNQUADROCOPTERNội dung1. 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ïcI. 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ơnvà 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ựctế, 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 trongnhà 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ạoloạ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 . Trongphần đồ án này nhóm xin được giới thiệu kết quả nghiên cứu về loại Quadrocopterhoạt động trong nhà .Trong đồ án này nhóm đã mô phỏng máy bay Quadrocopter bằng phần mềmMatLab 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ốcdài được ước lượng bằng bộ lọc Kalman .Máy bay QuadrocopterII. 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ộtcá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ì quaycù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ạchtruyền sóng để kết nối với bộ điều khiển ở dưới đất , mạch chính dùng để điềukhiể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ếnnhư cảm biến gyro để đo độ nghiêng, cảm biển aùp suaát để đo độ cao , cảm biểngia 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óngvô 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ôngthường thì chỉ cẩn bốn động cơ dùng cho 4 cánh quạt là đủ.Hình 2: Moâ hình QuadroCopterIII. 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áy2 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ønlaï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ïcvuoâng goùc vôùi maët phaúng maùy bay, theo nguyeân lyù baûo toaøn momanh ñoäng löôïngthì 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ättieâu laãn nhau.Hình 3 : Ñieàu khieån maùy bay bay leânBay 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öôùngKhi 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ûmtoá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ùcngieâ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ùnhquaï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 theophöông chuyeån ñoäng . Chính thaønh phaàn löïc naøy ñaõ laøm maùy bay bay theo phöôngngang .Hình 5 : Löïc taùc duïng leân thaân maùy bay khi noù bò nghieângNeá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ïngnguyeâ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ønlaïi ñeå giaûm momanh ñoäng löôïng theo phöông ngöôïc vôùi chieàu xoay mong muoán cuûamaù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àmVisual Nastran keát hôïp vôùi Simulink cuûa Matlab giuùp ta coù moät moâi tröôøng moâ phoûnggaà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âydöï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 trongphaà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ûanoù . Vieäc tính toaùn haøm truyeàn naøy ñöôïc thöïc hieän trong moâi tröôøng MatLab , sau khitính ñöôïc löïc naâng , ta ñöa löïc naøy vaøo trong moâi tröông Visual Nastran ( do trongNastran 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 4caù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àukhieå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øycuõng khoâng hoaøn toaøn ñoái xöùng .Hình 8 : Moâ hình maùy bay trong Visual NastranTrong 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ùolöï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 TRONGSIMULINK 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ểukhiển được độ nghiêng của máy bay theo 2 phương x , y ( oxy là mặt phẳng song songvới mặt đất ).XYHình vẽ ký hiệu các cánh của QuadroCopterNế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ủacá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ônggó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ảithay đổ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ốcgốc ω của máy bay là :F = k.ω2vớ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ớiVisualNastran .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ảovề độ nghiêng của máy bay theo 2 phương X, YGiá 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ậntố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 trongkhoả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 chiasự 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 = ;-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áybay 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 ñöôïcmöù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 khiqua 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 đưara 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øylaï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ûokhoâ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änvôù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àukhieå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óankxR∈laø traïng thaùi tröôùc öôùc löôïng taïi böôùc k, cho bieát traïngthaùi cuûa quaù trình ngay tröôùc traïng thaùi k, vaønkxR∈laø traïng thaùi sau öôùc löôïng taïi1. 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 chonhöõ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ò trungbì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ìnhupdate tuyeán tính sau:xk = Axk-1 + Buk-1 + wk-1Vaø vôùi giaù trò ño * laø:zk = Hxk + vkHai 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áichuaå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ïngthaù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 Bnxl 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 mxnbieå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:^−^ekk − x −K= xPk− = E ⎡⎣ekk−T ⎤⎦evaø xk töø nhöõng giaù trò coù ñöôïc ôû time update, cho neân coøn goïi laø quaù trình chænh söõaböôù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öôïnglaø:^−^ek = xk − xkMa 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 ( timeupdate ) 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 + QTime update (“predict”)1. Öôùc löôïng giaù trò keá tieáp:xk = Axk-1 + Buk-1 + wk-12. Öôùc löôïng ma traän covariancekeá tieáp:−Measurement update (“ corect”)1.Tính toaùn ñoä lôïi Kalman:Kk = Pk− H T (HPk− H T + R)−12.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 covariancePkkk−= (I − K H )PKhôûi taïo^ −xk−1 va Pk-1Quaù 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öõapredict 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átbò ñ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öïcnghieä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ápquaù 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äuchæ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 theophöô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ìnhsau:è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ãubg (k) laø ñoä troâi cuûa heä soá gyûo bias3. 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 ).Ätya (k +1) = ya (k) + v(k).sinè (k ).ÄtTrong ñ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+1a(k) laø gia toác cuûa xe ôû traïng thaùi thöù kbg(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 kvaø 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.X_DATAY_DATAZ_DATANC8XOUTYOUTZOUT5678NC1NC2NC3NC4161514134. Thieát keá sô ñoà maïch nguyeân lyù cuûa gyro vaø accelerometerCaû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ûmbieán accelerometer naøy coù theå ño ñöôïc gia toác theo 3 phöông X, Y, Z .R1Z_DATA1kC3ZOUT0.1uFR2U1MMA7260Y_DATA1kYOUTC40.1uFg-select1g-select23.3V_11234g-Select1g-Select2VDDVSS~sleep_modeNC7NC6NC51211109X_DATAR31KXOUTC50.1uFSô ñoà nguyeân lyù caûm bi