專利名稱：無(wú)人機(jī)的導(dǎo)航方法及裝置的制作方法
技術(shù)領(lǐng)域：
本發(fā)明涉及航空領(lǐng)域，具體而言，涉及一種小型無(wú)人機(jī)的導(dǎo)航方法及裝置。
背景技術(shù)：
小型無(wú)人機(jī)以其體積小，在執(zhí)行任務(wù)時(shí)隱蔽性好，機(jī)動(dòng)靈活，便于部署，成本低廉， 便于攜帶等特點(diǎn)，在低空軍事偵察、火力支援、目標(biāo)搜索、中繼通訊、航空攝影、氣象災(zāi)害監(jiān)測(cè)以及道路交通監(jiān)控等各領(lǐng)域都有著廣泛的應(yīng)用前景。隨著應(yīng)用領(lǐng)域向無(wú)人機(jī)小型化上的拓展，對(duì)小型無(wú)人機(jī)導(dǎo)航系統(tǒng)也提出了更高的要求，為小型無(wú)人機(jī)設(shè)計(jì)一種高精度、高可靠性的導(dǎo)航系統(tǒng)是小型無(wú)人機(jī)發(fā)展的關(guān)鍵技術(shù)之一。目前，在導(dǎo)航系統(tǒng)中，大多以慣性導(dǎo)航為基準(zhǔn)導(dǎo)航系統(tǒng)，其他導(dǎo)航系統(tǒng)輔助慣性導(dǎo)航來(lái)提高導(dǎo)航精度。常用的組合導(dǎo)航系統(tǒng)有慣性/多普勒、慣性/天文、INSdnertial Navigation System，慣性導(dǎo)航系統(tǒng))/GPS (Global Positioning System，全球定位系統(tǒng))以及慣性/地形匹配組合導(dǎo)航系統(tǒng)等。其中以INS/GPS組合導(dǎo)航系統(tǒng)應(yīng)用最為廣泛，是大多數(shù)組合導(dǎo)航系統(tǒng)的最佳選擇方案之一。慣性導(dǎo)航系統(tǒng)(INS)是一種不依賴于外部信息，也不向外部輻射能量的完全自主式導(dǎo)航系統(tǒng)，這使它具有很好的隱蔽性，不受干擾，工作環(huán)境適應(yīng)性好，而且提供的導(dǎo)航參數(shù)信息多，數(shù)據(jù)更新率高，短期精度和穩(wěn)定性好。但是慣性導(dǎo)航系統(tǒng)并非十全十美，由于慣性導(dǎo)航系統(tǒng)的定位誤差隨時(shí)間增加不斷增大，因而難以滿足長(zhǎng)時(shí)間精確導(dǎo)航的要求。此外，由于利用GPS接收機(jī)提供的信息只能對(duì)水平姿態(tài)信息得到很好的估計(jì)，而對(duì)航向角誤差估計(jì)的效果不好。針對(duì)相關(guān)技術(shù)中的慣性導(dǎo)航系統(tǒng)的定位誤差隨時(shí)間增加不斷增大，難以滿足長(zhǎng)時(shí)間精確導(dǎo)航的問(wèn)題，目前尚未提出有效的解決方案。

發(fā)明內(nèi)容
針對(duì)相關(guān)技術(shù)中的慣性導(dǎo)航系統(tǒng)的定位誤差隨時(shí)間增加不斷增大的問(wèn)題，本發(fā)明提供了一種小型無(wú)人機(jī)的導(dǎo)航方法及裝置，以至少解決上述問(wèn)題。根據(jù)本發(fā)明的一個(gè)方面，提供了一種無(wú)人機(jī)的導(dǎo)航方法，該方法包括接收GPS接收機(jī)和三軸電子羅盤(pán)輸出的測(cè)量參數(shù)；根據(jù)上一個(gè)周期輸出的導(dǎo)航參數(shù)、測(cè)量參數(shù)生成校正參數(shù)；根據(jù)校正參數(shù)輸出當(dāng)前周期的導(dǎo)航參數(shù)；根據(jù)當(dāng)前周期的導(dǎo)航參數(shù)對(duì)無(wú)人機(jī)系統(tǒng)進(jìn)行導(dǎo)航。優(yōu)選的，根據(jù)上一個(gè)周期輸出的導(dǎo)航參數(shù)、所述測(cè)量參數(shù)生成校正參數(shù)的步驟包括將所述上一個(gè)周期輸出的導(dǎo)航參數(shù)、所述測(cè)量參數(shù)作為輸入?yún)?shù)進(jìn)行卡爾曼濾波操作， 將卡爾曼濾波操作得到的結(jié)果作為所述校正參數(shù)。優(yōu)選的，根據(jù)上一個(gè)周期輸出的導(dǎo)航參數(shù)、所述測(cè)量參數(shù)生成校正參數(shù)的步驟還包括在進(jìn)行卡爾曼濾波操作之前，判斷是否到達(dá)濾波周期，若到達(dá)濾波周期，則通知系統(tǒng)執(zhí)行所述卡爾曼濾波操作。優(yōu)選的，根據(jù)所述校正參數(shù)輸出當(dāng)前周期的導(dǎo)航參數(shù)的步驟包括獲取當(dāng)前的角速度參數(shù)和加速度參數(shù)；通過(guò)所述加速度參數(shù)與所述校正參數(shù)對(duì)所述上一個(gè)周期輸出的導(dǎo)航參數(shù)中的速度參數(shù)進(jìn)行校正，得到所述當(dāng)前周期的導(dǎo)航參數(shù)中的速度參數(shù)；通過(guò)所述角速度參數(shù)與所述校正參數(shù)對(duì)所述上一個(gè)周期輸出的導(dǎo)航參數(shù)中的姿態(tài)參數(shù)進(jìn)行校正，得到所述當(dāng)前周期的導(dǎo)航參數(shù)中的姿態(tài)參數(shù)；通過(guò)所述當(dāng)前周期的導(dǎo)航參數(shù)中的姿態(tài)參數(shù)和速度參數(shù)對(duì)所述上一個(gè)周期輸出的導(dǎo)航參數(shù)中的位置參數(shù)進(jìn)行校正，得到所述當(dāng)前周期的導(dǎo)航參數(shù)中的位置參數(shù)。優(yōu)選的，根據(jù)所述校正參數(shù)輸出當(dāng)前周期的導(dǎo)航參數(shù)的步驟包括在進(jìn)行獲取當(dāng)前的角速度參數(shù)和加速度參數(shù)之前，判斷是否到達(dá)校正周期，若到達(dá)校正周期，則通知系統(tǒng)執(zhí)行所述校正操作。優(yōu)選的，在接收GPS接收機(jī)和三軸電子羅盤(pán)輸出的測(cè)量參數(shù)之前，無(wú)人機(jī)的導(dǎo)航方法還包括使用加速度參數(shù)、所述三軸電子羅盤(pán)輸出的測(cè)量參數(shù)對(duì)導(dǎo)航參數(shù)進(jìn)行初始對(duì)準(zhǔn)操作。根據(jù)本發(fā)明的另一方面，提供了一種無(wú)人機(jī)的導(dǎo)航裝置，該裝置包括捷連貫導(dǎo)系統(tǒng)，GPS接收機(jī)和三軸電子羅盤(pán)，其中，捷連貫導(dǎo)系統(tǒng)包括接收單元，用于接收GPS接收機(jī)和三軸電子羅盤(pán)輸出的測(cè)量參數(shù)；生成單元，用于根據(jù)上一個(gè)周期輸出的導(dǎo)航參數(shù)、測(cè)量參數(shù)生成校正參數(shù)；輸出單元，用于根據(jù)校正參數(shù)輸出當(dāng)前周期的導(dǎo)航參數(shù)；導(dǎo)航單元，根據(jù)當(dāng)前周期的導(dǎo)航參數(shù)對(duì)無(wú)人機(jī)系統(tǒng)進(jìn)行導(dǎo)航。優(yōu)選的，生成單元包括濾波模塊，用于將所述上一個(gè)周期輸出的導(dǎo)航參數(shù)、所述測(cè)量參數(shù)作為輸入?yún)?shù)進(jìn)行卡爾曼濾波操作，將卡爾曼濾波操作得到的結(jié)果作為所述校正參數(shù)。優(yōu)選的，輸出單元包括獲取模塊，用于獲取當(dāng)前的角速度參數(shù)和加速度參數(shù)；第一校正子模塊，用于通過(guò)獲取模塊獲取的所述加速度參數(shù)與所述校正參數(shù)對(duì)所述上一個(gè)周期輸出的導(dǎo)航參數(shù)中的速度參數(shù)進(jìn)行校正，得到所述當(dāng)前周期的導(dǎo)航參數(shù)中的速度參數(shù)； 第二校正子模塊，用于通過(guò)所述獲取模塊獲取的角速度參數(shù)與所述校正參數(shù)對(duì)所述上一個(gè)周期輸出的導(dǎo)航參數(shù)中的姿態(tài)參數(shù)進(jìn)行校正，得到所述當(dāng)前周期的導(dǎo)航參數(shù)中的姿態(tài)參數(shù)；第三校正子模塊，用于通過(guò)所述當(dāng)前周期的導(dǎo)航參數(shù)中的姿態(tài)參數(shù)和速度參數(shù)對(duì)所述上一個(gè)周期輸出的導(dǎo)航參數(shù)中的位置參數(shù)進(jìn)行校正，得到所述當(dāng)前周期的導(dǎo)航參數(shù)中的位置參數(shù)。優(yōu)選的，無(wú)人機(jī)的導(dǎo)航裝置還包括初始化單元，用于在所述接收單元接收所述GPS 接收機(jī)和所述三軸電子羅盤(pán)輸出的測(cè)量參數(shù)之前，使用加速度參數(shù)、所述三軸電子羅盤(pán)輸出的測(cè)量參數(shù)對(duì)導(dǎo)航參數(shù)進(jìn)行初始對(duì)準(zhǔn)操作。通過(guò)本發(fā)明，采用捷連貫導(dǎo)系統(tǒng)輸出導(dǎo)航參數(shù)信息，使用GPS接收機(jī)和三軸電子羅盤(pán)接收的信息參數(shù)輔助校正，解決了相關(guān)技術(shù)中的慣性導(dǎo)航系統(tǒng)的定位誤差隨時(shí)間不斷增加，難以滿足長(zhǎng)時(shí)間精確導(dǎo)航的問(wèn)題，進(jìn)而達(dá)到了精確導(dǎo)航的技術(shù)效果。


此處所說(shuō)明的附圖用來(lái)提供對(duì)本發(fā)明的進(jìn)一步理解，構(gòu)成本申請(qǐng)的一部分，本發(fā)明的示意性實(shí)施例及其說(shuō)明用于解釋本發(fā)明，并不構(gòu)成對(duì)本發(fā)明的不當(dāng)限定。在附圖中圖1是根據(jù)本發(fā)明實(shí)施例的無(wú)人機(jī)的導(dǎo)航方法的一種優(yōu)選的流程圖2是根據(jù)本發(fā)明實(shí)施例的無(wú)人機(jī)的導(dǎo)航裝置的一種優(yōu)選的結(jié)構(gòu)框圖；圖3是根據(jù)本發(fā)明實(shí)施例的無(wú)人機(jī)的導(dǎo)航裝置的另一種優(yōu)選的結(jié)構(gòu)框圖；圖4是根據(jù)本發(fā)明實(shí)施例的無(wú)人機(jī)的導(dǎo)航裝置的一種優(yōu)選的原理圖；圖5是根據(jù)本發(fā)明實(shí)施例的無(wú)人機(jī)的導(dǎo)航方法的另一種優(yōu)選的流程圖。
具體實(shí)施例方式下文中將參考附圖并結(jié)合實(shí)施例來(lái)詳細(xì)說(shuō)明本發(fā)明。需要說(shuō)明的是，在不沖突的情況下，本申請(qǐng)中的實(shí)施例及實(shí)施例中的特征可以相互組合。實(shí)施例1本發(fā)明提供了一種優(yōu)選的無(wú)人機(jī)的導(dǎo)航方法，如圖1所示，包括S102，接收GPS接收機(jī)和三軸電子羅盤(pán)輸出的測(cè)量參數(shù)；S104，根據(jù)上一個(gè)周期輸出的導(dǎo)航參數(shù)、測(cè)量參數(shù)生成校正參數(shù)；S106，根據(jù)校正參數(shù)輸出當(dāng)前周期的導(dǎo)航參數(shù)；S108，根據(jù)當(dāng)前周期的導(dǎo)航參數(shù)對(duì)無(wú)人機(jī)系統(tǒng)進(jìn)行導(dǎo)航。在上述優(yōu)選的實(shí)施例中，借助接收GPS接收機(jī)和三軸電子羅盤(pán)輸出的測(cè)量參數(shù)對(duì)導(dǎo)航輸出的參數(shù)進(jìn)行周期性校正，克服了導(dǎo)航的定位誤差隨時(shí)間不斷增加，難以滿足長(zhǎng)時(shí)間精確導(dǎo)航的問(wèn)題，達(dá)到了精確導(dǎo)航的技術(shù)效果。為了對(duì)輸出導(dǎo)航參數(shù)進(jìn)行校正，本發(fā)明還提供了一種校正方法，具體地，根據(jù)上一個(gè)周期輸出的導(dǎo)航參數(shù)、測(cè)量參數(shù)生成校正參數(shù)的步驟包括將上一個(gè)周期輸出的導(dǎo)航參數(shù)、測(cè)量參數(shù)作為輸入?yún)?shù)進(jìn)行卡爾曼濾波操作，將卡爾曼濾波操作得到的結(jié)果作為校正參數(shù)。通過(guò)上述卡爾曼濾波參數(shù)，完成濾波運(yùn)算，實(shí)現(xiàn)對(duì)導(dǎo)航輸出參數(shù)的精確校正。優(yōu)選的，在進(jìn)行上述卡爾曼濾波操作之前，判斷是否到達(dá)濾波周期，若到達(dá)濾波周期，則通知系統(tǒng)執(zhí)行卡爾曼濾波操作。通過(guò)濾波周期來(lái)判斷是否進(jìn)行卡爾曼濾波，保證濾波的周期性，進(jìn)而實(shí)現(xiàn)對(duì)導(dǎo)航輸出參數(shù)的精確校正。在本發(fā)明優(yōu)選的實(shí)施例中，還提供了一種優(yōu)選的根據(jù)校正參數(shù)輸出當(dāng)前周期的導(dǎo)航參數(shù)的方法，具體包括獲取當(dāng)前的角速度參數(shù)和加速度參數(shù)；通過(guò)加速度參數(shù)與校正參數(shù)對(duì)上一個(gè)周期輸出的導(dǎo)航參數(shù)中的速度參數(shù)進(jìn)行校正，得到當(dāng)前周期的導(dǎo)航參數(shù)中的速度參數(shù)；通過(guò)角速度參數(shù)與校正參數(shù)對(duì)上一個(gè)周期輸出的導(dǎo)航參數(shù)中的姿態(tài)參數(shù)進(jìn)行校正，得到當(dāng)前周期的導(dǎo)航參數(shù)中的姿態(tài)參數(shù)；通過(guò)當(dāng)前周期的導(dǎo)航參數(shù)中的姿態(tài)參數(shù)和速度參數(shù)對(duì)上一個(gè)周期輸出的導(dǎo)航參數(shù)中的位置參數(shù)進(jìn)行校正，得到當(dāng)前周期的導(dǎo)航參數(shù)中的位置參數(shù)。通過(guò)上述優(yōu)選的方法，可以實(shí)現(xiàn)同時(shí)對(duì)飛行器的速度參數(shù)、姿態(tài)參數(shù)以及位置參數(shù)的校正，進(jìn)一步保證導(dǎo)航的精確性。優(yōu)選的，在進(jìn)行獲取當(dāng)前的角速度參數(shù)和加速度參數(shù)之前，判斷是否到達(dá)校正周期，若到達(dá)校正周期，則通知系統(tǒng)執(zhí)行校正操作。通過(guò)校正周期來(lái)判斷是否進(jìn)行校正，保證校正的周期性，進(jìn)而實(shí)現(xiàn)對(duì)導(dǎo)航輸出參數(shù)的精確校正。在本發(fā)明優(yōu)選的實(shí)施例中，還提供了一種優(yōu)選的導(dǎo)航初始化的方法，具體的，在接收GPS接收機(jī)和三軸電子羅盤(pán)輸出的測(cè)量參數(shù)之前，無(wú)人機(jī)的導(dǎo)航方法還包括使用加速度參數(shù)、三軸電子羅盤(pán)輸出的測(cè)量參數(shù)對(duì)導(dǎo)航參數(shù)進(jìn)行初始對(duì)準(zhǔn)操作。通過(guò)對(duì)導(dǎo)航參數(shù)進(jìn)
6行初始對(duì)準(zhǔn)，保證下一步的校正操作運(yùn)算過(guò)程的精確性，進(jìn)而達(dá)到導(dǎo)航輸出參數(shù)的精確校正的技術(shù)效果。實(shí)施例2本發(fā)明提供了一種優(yōu)選的無(wú)人機(jī)的導(dǎo)航裝置，具體的，如圖2所示，包括捷連貫導(dǎo)系統(tǒng)202，GPS接收機(jī)204和三軸電子羅盤(pán)206，其中，捷連貫導(dǎo)系統(tǒng)202包括接收單元2022，用于接收GPS接收機(jī)和三軸電子羅盤(pán)輸出的測(cè)量參數(shù)；生成單元20M，用于根據(jù)上一個(gè)周期輸出的導(dǎo)航參數(shù)、測(cè)量參數(shù)生成校正參數(shù)；輸出單元20 ,用于根據(jù)校正參數(shù)輸出當(dāng)前周期的導(dǎo)航參數(shù)；導(dǎo)航單元20 ，根據(jù)當(dāng)前周期的導(dǎo)航參數(shù)對(duì)無(wú)人機(jī)系統(tǒng)進(jìn)行導(dǎo)航。在上述優(yōu)選的實(shí)施例中，借助接收GPS接收機(jī)204和三軸電子羅盤(pán)206輸出的測(cè)量參數(shù)對(duì)導(dǎo)航輸出的參數(shù)進(jìn)行周期性校正，克服了導(dǎo)航的定位誤差隨時(shí)間不斷增加，難以滿足長(zhǎng)時(shí)間精確導(dǎo)航的問(wèn)題，達(dá)到了精確導(dǎo)航的技術(shù)效果。優(yōu)選的，上述無(wú)人機(jī)的導(dǎo)航裝置采用基于MIMU的捷聯(lián)慣導(dǎo)系統(tǒng)作為組合導(dǎo)航系統(tǒng)的主輸出通道，每間隔預(yù)定時(shí)間(如20ms)進(jìn)行一次捷聯(lián)慣導(dǎo)解算輸出導(dǎo)航參數(shù)信息，使用GPS接收機(jī)204輸出的GPS位置速度信息對(duì)慣性導(dǎo)航系統(tǒng)進(jìn)行周期校正。但利用GPS接收機(jī)提供的信息，只能對(duì)水平姿態(tài)誤差信息得到很好的估計(jì)，而對(duì)航向角誤差估計(jì)效果不好。為了提高對(duì)航向誤差角的估計(jì)精度，還使用了一個(gè)三軸電子羅盤(pán)206，利用其輸出的磁航向角信息與GPS接收機(jī)204提供的位置速度信息一起作為量測(cè)信息對(duì)捷聯(lián)慣導(dǎo)系統(tǒng)進(jìn)行周期校正。這樣就構(gòu)成了以捷聯(lián)慣導(dǎo)系統(tǒng)為主，GPS接收機(jī)204和三軸電子羅盤(pán)206為輔的多傳感器信息融合的組合導(dǎo)航系統(tǒng)來(lái)消除捷聯(lián)慣導(dǎo)系統(tǒng)的誤差積累，實(shí)現(xiàn)對(duì)微小型無(wú)人機(jī)長(zhǎng)時(shí)間精確導(dǎo)航。為了對(duì)輸出導(dǎo)航參數(shù)進(jìn)行校正，本發(fā)明還對(duì)上述生成單元20M進(jìn)行了改進(jìn)，具體的，如圖3所示，該生成單元20M包括濾波模塊20M2，用于將上一個(gè)周期輸出的導(dǎo)航參數(shù)、測(cè)量參數(shù)作為輸入?yún)?shù)進(jìn)行卡爾曼濾波操作，將卡爾曼濾波操作得到的結(jié)果作為校正參數(shù)。優(yōu)選的，在上述濾波模塊20242進(jìn)行卡爾曼濾波操作之前，判斷是否到達(dá)濾波周期，若到達(dá)濾波周期，則通知系統(tǒng)執(zhí)行卡爾曼濾波操作。通過(guò)濾波周期來(lái)判斷是否進(jìn)行卡爾曼濾波，保證濾波的周期性，進(jìn)而實(shí)現(xiàn)對(duì)導(dǎo)航輸出參數(shù)的精確校正。本發(fā)明還對(duì)上述輸出單元20 進(jìn)行了改進(jìn)，以達(dá)到精確校正的效果。具體的，如圖3所示，輸出單元20 包括獲取模塊2(^62，用于獲取當(dāng)前的角速度參數(shù)和加速度參數(shù)；第一校正子模塊2(^64，用于通過(guò)獲取模塊獲取的加速度參數(shù)與校正參數(shù)對(duì)上一個(gè)周期輸出的導(dǎo)航參數(shù)中的速度參數(shù)進(jìn)行校正，得到當(dāng)前周期的導(dǎo)航參數(shù)中的速度參數(shù)；第二校正子模塊2(^66，用于通過(guò)獲取模塊獲取的角速度參數(shù)與校正參數(shù)對(duì)上一個(gè)周期輸出的導(dǎo)航參數(shù)中的姿態(tài)參數(shù)進(jìn)行校正，得到當(dāng)前周期的導(dǎo)航參數(shù)中的姿態(tài)參數(shù)；第三校正子模塊2(^68，用于通過(guò)當(dāng)前周期的導(dǎo)航參數(shù)中的姿態(tài)參數(shù)和速度參數(shù)對(duì)上一個(gè)周期輸出的導(dǎo)航參數(shù)中的位置參數(shù)進(jìn)行校正，得到當(dāng)前周期的導(dǎo)航參數(shù)中的位置參數(shù)。通過(guò)上述優(yōu)選的實(shí)施方式，可以實(shí)現(xiàn)同時(shí)對(duì)飛行器的速度參數(shù)、姿態(tài)參數(shù)以及位置參數(shù)的校正，進(jìn)一步保證導(dǎo)航的精確性。優(yōu)選的，在獲取模塊20262獲取當(dāng)前的角速度參數(shù)和加速度參數(shù)之前，判斷是否到達(dá)校正周期，若到達(dá)校正周期，則通知系統(tǒng)執(zhí)行校正操作。通過(guò)校正周期來(lái)判斷是否進(jìn)行校正，保證校正的周期性，進(jìn)而實(shí)現(xiàn)對(duì)導(dǎo)航輸出參數(shù)的精確校正。在本發(fā)明優(yōu)選的實(shí)施例中，還提供了一種優(yōu)選的初始化的實(shí)施方式，具體的，無(wú)人機(jī)的導(dǎo)航裝置還包括初始化單元，用于在接收單元接收GPS接收機(jī)和三軸電子羅盤(pán)輸出的測(cè)量參數(shù)之前，使用加速度參數(shù)、三軸電子羅盤(pán)輸出的測(cè)量參數(shù)對(duì)導(dǎo)航參數(shù)進(jìn)行初始對(duì)準(zhǔn)操作。通過(guò)對(duì)導(dǎo)航參數(shù)進(jìn)行初始對(duì)準(zhǔn)，保證下一步的校正操作運(yùn)算過(guò)程的精確性，進(jìn)而達(dá)到導(dǎo)航輸出參數(shù)的精確校正的技術(shù)效果。實(shí)施例3圖4示出本優(yōu)選實(shí)施例的無(wú)人機(jī)的導(dǎo)航裝置的原理圖，使用GPS接收機(jī)提供的位置速度信息與三軸電子羅盤(pán)輸出的磁航向角信息一起作為量測(cè)信息對(duì)捷聯(lián)慣導(dǎo)系統(tǒng)進(jìn)行周期校正。這樣就構(gòu)成了以捷聯(lián)慣導(dǎo)系統(tǒng)為主，GPS接收機(jī)和三軸電子羅盤(pán)為輔的多傳感器信息融合的組合導(dǎo)航系統(tǒng)來(lái)消除捷聯(lián)慣導(dǎo)系統(tǒng)的誤差積累，實(shí)現(xiàn)對(duì)微小型無(wú)人機(jī)長(zhǎng)時(shí)間精確導(dǎo)航。在上述無(wú)人機(jī)的導(dǎo)航裝置中，將導(dǎo)航參數(shù)的誤差量作為系統(tǒng)狀態(tài)向量，捷聯(lián)慣導(dǎo)輸出的信息與量測(cè)信息相減作為卡爾曼濾波的量測(cè)量，采用反饋校正的卡爾曼濾波器，經(jīng)過(guò)濾波計(jì)算后估計(jì)出的誤差對(duì)捷聯(lián)慣導(dǎo)進(jìn)行校正。具體的，微慣性測(cè)量組合模塊通過(guò)溫度誤差補(bǔ)償模塊和初始校準(zhǔn)模塊與捷連貫導(dǎo)解算模塊相連，卡爾曼濾波模塊根據(jù)捷連貫導(dǎo)解算模塊解算得到的參數(shù)、以及電子羅盤(pán)和GPS接收機(jī)接收的測(cè)量參數(shù)進(jìn)行卡爾曼濾波操作，將經(jīng)過(guò)濾波操作得到的誤差量輸出給捷連貫導(dǎo)解算模塊，捷連貫導(dǎo)解算模塊使用上述誤差量進(jìn)行反饋校正以生成校正后的導(dǎo)航參數(shù)，并把校正后的導(dǎo)航參數(shù)傳輸給導(dǎo)航處理模塊。此外，利用微慣性測(cè)量組合模塊輸出的加速度信息和電子羅盤(pán)輸出的信息完成捷聯(lián)慣導(dǎo)系統(tǒng)的初始對(duì)準(zhǔn)。圖5示出了上述無(wú)人機(jī)的裝置工作的方法流程圖，具體的，包括(S502-S550)S502，完成使用參數(shù)的初始化。進(jìn)入導(dǎo)航子程序后，先完成使用參數(shù)的初始化，優(yōu)選的，這些參數(shù)包括捷聯(lián)慣導(dǎo)解算周期、卡爾曼濾波周期，地球半徑、自轉(zhuǎn)角速率、量測(cè)噪聲方差陣、驅(qū)動(dòng)噪聲方差陣以及程序執(zhí)行時(shí)間長(zhǎng)度等，以及在捷聯(lián)慣導(dǎo)解算和卡爾曼濾波運(yùn)算中使用變量的初始化。S504，進(jìn)行捷連貫導(dǎo)系統(tǒng)的初始化對(duì)準(zhǔn)。S506，判斷是否是第一次執(zhí)行程序，若是，則執(zhí)行步驟S508，若否，則轉(zhuǎn)而執(zhí)行步驟 S510。S508，賦予位置、速度初值，即第一次執(zhí)行程序，就對(duì)位置速度賦初值，由位置初值計(jì)算出位置矩陣，利用初始對(duì)準(zhǔn)得到的姿態(tài)角建立姿態(tài)矩陣并完成四元數(shù)的初始化工作。S510，獲取當(dāng)前的角速度信息和加速度信息。在獲取當(dāng)前的角速度信息和加速度信息之后，執(zhí)行步驟S512-S534。S512-S534，由加速度信息進(jìn)行速度更新計(jì)算出當(dāng)前時(shí)刻的速度，并判斷濾波標(biāo)志位是否為1，如果濾波標(biāo)志位為1就對(duì)速度進(jìn)行校正；由角速度信息完成四元數(shù)更新并計(jì)算當(dāng)前的姿態(tài)矩陣，并判斷濾波標(biāo)志位是否為1，如果濾波標(biāo)志位為1，則對(duì)速度進(jìn)行校正；由角速度信息完成四元數(shù)更新并計(jì)算當(dāng)前的姿態(tài)矩陣，并判斷濾波標(biāo)志位是否為1，如果濾波標(biāo)志位為1，則對(duì)姿態(tài)陣進(jìn)行修正并計(jì)算修正后的四元數(shù)值；由當(dāng)前的姿態(tài)矩陣提取出飛機(jī)的姿態(tài)角，由計(jì)算出的當(dāng)前速度更新位置矩陣，計(jì)算出當(dāng)前位置，并判斷濾波標(biāo)志位是否為1，若濾波標(biāo)志位為1，則對(duì)位置進(jìn)行校正。至此，通過(guò)步驟S502-S534，就完成了捷連貫導(dǎo)解算任務(wù)。在此之后，進(jìn)行卡爾曼濾波，卡爾曼濾波的具體步驟包括(S536-S550)S536，判斷捷連貫導(dǎo)系統(tǒng)是否正常工作；若是，執(zhí)行步驟S538-S550，否則，返回主程序。S538，判斷是否到達(dá)濾波周期；若是，執(zhí)行步驟S540-SM4，否則，轉(zhuǎn)而執(zhí)行步驟 SM6。SM0，進(jìn)行濾波計(jì)算。S542,濾波標(biāo)志位置1。SM4，更新G陣和Q陣，其中，G陣和Q陣為卡爾曼濾波中使用到的參數(shù)。SM6，清除濾波標(biāo)志位；計(jì)算F陣、狀態(tài)轉(zhuǎn)移矩陣，其中，F(xiàn)陣為卡爾曼濾波中使用到的參數(shù)。SM8，輸出估計(jì)值。S550，判斷程序是否結(jié)束；若結(jié)束，則返回主程序；否則，轉(zhuǎn)至步驟S506進(jìn)行判斷。通過(guò)利用捷聯(lián)慣導(dǎo)解算后的位置、速度以及姿態(tài)角信息對(duì)系統(tǒng)狀態(tài)方程進(jìn)行離散化處理計(jì)算出狀態(tài)轉(zhuǎn)移矩陣，如果到了濾波周期就進(jìn)行卡爾曼濾波計(jì)算，置濾波標(biāo)志位為 1，并輸出估計(jì)誤差值。優(yōu)選的，捷聯(lián)慣導(dǎo)解算周期為20毫秒，卡爾曼濾波周期為2秒。在完成濾波運(yùn)算后判斷程序是否結(jié)束，如果沒(méi)有結(jié)束則返回捷聯(lián)慣導(dǎo)解算的入口繼續(xù)運(yùn)行， 否則子程序結(jié)束，返回主程序。從以上的描述中，可以看出，本發(fā)明采用捷連貫導(dǎo)系統(tǒng)輸出導(dǎo)航參數(shù)信息，使用 GPS接收機(jī)和三軸電子羅盤(pán)接收的信息參數(shù)輔助校正，解決了相關(guān)技術(shù)中的慣性導(dǎo)航系統(tǒng)的定位誤差隨時(shí)間不斷增加，難以滿足長(zhǎng)時(shí)間精確導(dǎo)航的問(wèn)題，進(jìn)而達(dá)到了精確導(dǎo)航的技術(shù)效果。顯然，本領(lǐng)域的技術(shù)人員應(yīng)該明白，上述的本發(fā)明的各模塊或各步驟可以用通用的計(jì)算裝置來(lái)實(shí)現(xiàn)，它們可以集中在單個(gè)的計(jì)算裝置上，或者分布在多個(gè)計(jì)算裝置所組成的網(wǎng)絡(luò)上，可選地，它們可以用計(jì)算裝置可執(zhí)行的程序代碼來(lái)實(shí)現(xiàn)，從而，可以將它們存儲(chǔ)在存儲(chǔ)裝置中由計(jì)算裝置來(lái)執(zhí)行，并且在某些情況下，可以以不同于此處的順序執(zhí)行所示出或描述的步驟，或者將它們分別制作成各個(gè)集成電路模塊，或者將它們中的多個(gè)模塊或步驟制作成單個(gè)集成電路模塊來(lái)實(shí)現(xiàn)。這樣，本發(fā)明不限制于任何特定的硬件和軟件結(jié)合。以上所述僅為本發(fā)明的優(yōu)選實(shí)施例而已，并不用于限制本發(fā)明，對(duì)于本領(lǐng)域的技術(shù)人員來(lái)說(shuō)，本發(fā)明可以有各種更改和變化。凡在本發(fā)明的精神和原則之內(nèi)，所作的任何修改、等同替換、改進(jìn)等，均應(yīng)包含在本發(fā)明的保護(hù)范圍之內(nèi)。
9
權(quán)利要求
1.一種無(wú)人機(jī)的導(dǎo)航方法，其特征在于，包括 接收GPS接收機(jī)和三軸電子羅盤(pán)輸出的測(cè)量參數(shù)；根據(jù)上一個(gè)周期輸出的導(dǎo)航參數(shù)、所述測(cè)量參數(shù)生成校正參數(shù)； 根據(jù)所述校正參數(shù)輸出當(dāng)前周期的導(dǎo)航參數(shù)； 根據(jù)所述當(dāng)前周期的導(dǎo)航參數(shù)對(duì)無(wú)人機(jī)系統(tǒng)進(jìn)行導(dǎo)航。
2.根據(jù)權(quán)利要求1所述的方法，其特征在于，根據(jù)上一個(gè)周期輸出的導(dǎo)航參數(shù)、所述測(cè)量參數(shù)生成校正參數(shù)的步驟包括將所述上一個(gè)周期輸出的導(dǎo)航參數(shù)、所述測(cè)量參數(shù)作為輸入?yún)?shù)進(jìn)行卡爾曼濾波操作，將卡爾曼濾波操作得到的結(jié)果作為所述校正參數(shù)。
3.根據(jù)權(quán)利要求2所述的方法，其特征在于，根據(jù)上一個(gè)周期輸出的導(dǎo)航參數(shù)、所述測(cè)量參數(shù)生成校正參數(shù)的步驟還包括在進(jìn)行卡爾曼濾波操作之前，判斷是否到達(dá)濾波周期，若到達(dá)濾波周期，則通知系統(tǒng)執(zhí)行所述卡爾曼濾波操作。
4.根據(jù)權(quán)利要求1所述的方法，其特征在于，根據(jù)所述校正參數(shù)輸出當(dāng)前周期的導(dǎo)航參數(shù)的步驟包括獲取當(dāng)前的角速度參數(shù)和加速度參數(shù)；通過(guò)所述加速度參數(shù)與所述校正參數(shù)對(duì)所述上一個(gè)周期輸出的導(dǎo)航參數(shù)中的速度參數(shù)進(jìn)行校正，得到所述當(dāng)前周期的導(dǎo)航參數(shù)中的速度參數(shù)；通過(guò)所述角速度參數(shù)與所述校正參數(shù)對(duì)所述上一個(gè)周期輸出的導(dǎo)航參數(shù)中的姿態(tài)參數(shù)進(jìn)行校正，得到所述當(dāng)前周期的導(dǎo)航參數(shù)中的姿態(tài)參數(shù)；通過(guò)所述當(dāng)前周期的導(dǎo)航參數(shù)中的姿態(tài)參數(shù)和速度參數(shù)對(duì)所述上一個(gè)周期輸出的導(dǎo)航參數(shù)中的位置參數(shù)進(jìn)行校正，得到所述當(dāng)前周期的導(dǎo)航參數(shù)中的位置參數(shù)。
5.根據(jù)權(quán)利要求4所述的方法，其特征在于，根據(jù)所述校正參數(shù)輸出當(dāng)前周期的導(dǎo)航參數(shù)的步驟包括在進(jìn)行獲取當(dāng)前的角速度參數(shù)和加速度參數(shù)之前，判斷是否到達(dá)校正周期，若到達(dá)校正周期，則通知系統(tǒng)執(zhí)行所述校正操作。
6.根據(jù)權(quán)利要求1所述的方法，其特征在于，在接收GPS接收機(jī)和三軸電子羅盤(pán)輸出的測(cè)量參數(shù)之前，還包括使用加速度參數(shù)、所述三軸電子羅盤(pán)輸出的測(cè)量參數(shù)對(duì)導(dǎo)航參數(shù)進(jìn)行初始對(duì)準(zhǔn)操作。
7.一種無(wú)人機(jī)的導(dǎo)航裝置，包括捷連貫導(dǎo)系統(tǒng)，GPS接收機(jī)和三軸電子羅盤(pán)，其中，所述捷連貫導(dǎo)系統(tǒng)包括接收單元，用于接收所述GPS接收機(jī)和所述三軸電子羅盤(pán)輸出的測(cè)量參數(shù)； 生成單元，用于根據(jù)上一個(gè)周期輸出的導(dǎo)航參數(shù)、所述測(cè)量參數(shù)生成校正參數(shù)； 輸出單元，用于根據(jù)所述校正參數(shù)輸出當(dāng)前周期的導(dǎo)航參數(shù)； 導(dǎo)航單元，根據(jù)所述當(dāng)前周期的導(dǎo)航參數(shù)對(duì)無(wú)人機(jī)系統(tǒng)進(jìn)行導(dǎo)航。
8.根據(jù)權(quán)利要求7所述裝置，其特征在于，所述生成單元包括濾波模塊，用于將所述上一個(gè)周期輸出的導(dǎo)航參數(shù)、所述測(cè)量參數(shù)作為輸入?yún)?shù)進(jìn)行卡爾曼濾波操作，將卡爾曼濾波操作得到的結(jié)果作為所述校正參數(shù)。
9.根據(jù)權(quán)利要求7所述裝置，其特征在于，所述輸出單元包括獲取模塊，用于獲取當(dāng)前的角速度參數(shù)和加速度參數(shù)；第一校正子模塊，用于通過(guò)獲取模塊獲取的所述加速度參數(shù)與所述校正參數(shù)對(duì)所述上一個(gè)周期輸出的導(dǎo)航參數(shù)中的速度參數(shù)進(jìn)行校正，得到所述當(dāng)前周期的導(dǎo)航參數(shù)中的速度參數(shù)；第二校正子模塊，用于通過(guò)所述獲取模塊獲取的角速度參數(shù)與所述校正參數(shù)對(duì)所述上一個(gè)周期輸出的導(dǎo)航參數(shù)中的姿態(tài)參數(shù)進(jìn)行校正，得到所述當(dāng)前周期的導(dǎo)航參數(shù)中的姿態(tài)參數(shù)；第三校正子模塊，用于通過(guò)所述當(dāng)前周期的導(dǎo)航參數(shù)中的姿態(tài)參數(shù)和速度參數(shù)對(duì)所述上一個(gè)周期輸出的導(dǎo)航參數(shù)中的位置參數(shù)進(jìn)行校正，得到所述當(dāng)前周期的導(dǎo)航參數(shù)中的位置參數(shù)。
10.根據(jù)權(quán)利要求7所述裝置，其特征在于，還包括初始化單元，用于在所述接收單元接收所述GPS接收機(jī)和所述三軸電子羅盤(pán)輸出的測(cè)量參數(shù)之前，使用加速度參數(shù)、所述三軸電子羅盤(pán)輸出的測(cè)量參數(shù)對(duì)導(dǎo)航參數(shù)進(jìn)行初始對(duì)準(zhǔn)操作。
全文摘要
本發(fā)明公開(kāi)了一種無(wú)人機(jī)的導(dǎo)航方法及裝置，其中，該方法包括接收GPS接收機(jī)和三軸電子羅盤(pán)輸出的測(cè)量參數(shù)；根據(jù)上一個(gè)周期輸出的導(dǎo)航參數(shù)、測(cè)量參數(shù)生成校正參數(shù)；根據(jù)校正參數(shù)輸出當(dāng)前周期的導(dǎo)航參數(shù)；根據(jù)當(dāng)前周期的導(dǎo)航參數(shù)對(duì)無(wú)人機(jī)系統(tǒng)進(jìn)行導(dǎo)航。本發(fā)明解決了相關(guān)技術(shù)中的慣性導(dǎo)航系統(tǒng)的定位誤差隨時(shí)間不斷增加，難以滿足長(zhǎng)時(shí)間精確導(dǎo)航的問(wèn)題，達(dá)到了精確導(dǎo)航的技術(shù)效果。
文檔編號(hào)G01C21/24GK102506875SQ201110390809
公開(kāi)日2012年6月20日 申請(qǐng)日期2011年11月30日 優(yōu)先權(quán)日2011年11月30日
發(fā)明者吳佳楠, 谷新宇 申請(qǐng)人:中國(guó)南方航空工業(yè)(集團(tuán))有限公司