多旋翼飛行器gps和ins低成本組合導(dǎo)航系統(tǒng)的制作方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于組合導(dǎo)航技術(shù)領(lǐng)域，涉及基于GPS和INS松耦合方式的多旋翼飛行器 低成本組合導(dǎo)航系統(tǒng)。
【背景技術(shù)】
[0002] 在已有的多種導(dǎo)航技術(shù)中，慣性導(dǎo)航、衛(wèi)星導(dǎo)航以及二者的組合導(dǎo)航在當(dāng)前研究 和應(yīng)用中最為廣泛。慣性導(dǎo)航系統(tǒng)（INS)以經(jīng)典物理學(xué)理論為基礎(chǔ)，利用慣性傳感器和導(dǎo) 航計(jì)算機(jī)推算用戶的姿態(tài)角度、速度和位置，是一種三維航位推算（DR)導(dǎo)航系統(tǒng)。INS的 優(yōu)點(diǎn)在于其高帶寬的導(dǎo)航結(jié)果，具有短期精度，缺點(diǎn)是INS的誤差會(huì)隨時(shí)間累積，精度較高 的INS價(jià)格也非常昂貴。全球?qū)Ш叫l(wèi)星系統(tǒng)（GNSS)是與INS的航位推算原理根本區(qū)別的 一種直接定位系統(tǒng)，它測(cè)量用戶相對(duì)導(dǎo)航衛(wèi)星構(gòu)成星座的坐標(biāo)，再由衛(wèi)星運(yùn)行軌道計(jì)算衛(wèi) 星相對(duì)地球的坐標(biāo)，最后計(jì)算出用戶在地球上的位置。GNSS的誤差不隨時(shí)間累積，具有長(zhǎng)期 精度，但易受干擾，工作頻率較低。根據(jù)INS和GNSS的互補(bǔ)特性，將GNSS和INS組合起來(lái)， 可以實(shí)現(xiàn)超越二者單獨(dú)工作的導(dǎo)航性能。
[0003] 然而，精度較高的INS成本極高，低成本的慣性傳感器由于噪聲較大，漂移嚴(yán)重， 在實(shí)際使用中面臨極大挑戰(zhàn)。低成本的單點(diǎn)GNSS接收機(jī)通常工作頻率極低（<5Hz)，無(wú)法獲 取用戶的姿態(tài)信息，對(duì)于飛行器等對(duì)姿態(tài)要求很高的系統(tǒng)而言無(wú)法單獨(dú)使用，而差分GNSS 接收機(jī)、多天線接收機(jī)等通常成本也很高，對(duì)大多數(shù)民用系統(tǒng)而言很難承受。因此，結(jié)合低 成本INS和低成本GNSS對(duì)于要求姿態(tài)、低成本和實(shí)時(shí)性的民用導(dǎo)航用戶而言幾乎是唯一的 選擇。低成本組合導(dǎo)航系統(tǒng)也已成為國(guó)內(nèi)外學(xué)術(shù)研究和產(chǎn)品化的熱點(diǎn)，得到了科研單位和 導(dǎo)航企業(yè)的極大重視，具有豐富的學(xué)術(shù)研究?jī)r(jià)值和市場(chǎng)價(jià)值。目前，多旋翼飛行器已經(jīng)被廣 泛應(yīng)用于許多領(lǐng)域，如軍事、交通和物流等。作為實(shí)現(xiàn)其自主任務(wù)飛行的關(guān)鍵技術(shù)之一，多 旋翼飛行器的低成本導(dǎo)航已成為國(guó)內(nèi)外研究的熱點(diǎn)。單純的低成本GPS無(wú)法提供飛行器的 姿態(tài)信息，輸出頻率低且精度易受環(huán)境因素影響，而單純的低成本INS則噪聲較大，漂移嚴(yán) 重，因此，將二者有效地進(jìn)行數(shù)據(jù)融合和優(yōu)勢(shì)互補(bǔ)是低成本導(dǎo)航應(yīng)用的必然選擇。
[0004] 多旋翼飛行器通常具有低動(dòng)態(tài)特性，但在飛行過(guò)程中，電機(jī)轉(zhuǎn)動(dòng)引起的機(jī)身振動(dòng) 會(huì)極大降低導(dǎo)航系統(tǒng)性能。大量研究表明，多旋翼飛行器的振動(dòng)對(duì)導(dǎo)航系統(tǒng)的陀螺儀、加速 度計(jì)、磁力計(jì)和氣壓計(jì)均會(huì)有不容忽略的影響。因此，需要針對(duì)性的對(duì)多旋翼飛行器設(shè)計(jì)有 效的濾波算法。

【發(fā)明內(nèi)容】

[0005] 本發(fā)明的目的在于解決現(xiàn)有的導(dǎo)航系統(tǒng)精度差或成本高的問(wèn)題。
[0006] 本發(fā)明所采用的技術(shù)方案是包括數(shù)據(jù)采集處理器，數(shù)據(jù)采集處理器通過(guò)IIC總線 分別連接陀螺儀、加速度計(jì)、磁力計(jì)、氣壓計(jì)，數(shù)據(jù)采集處理器控制陀螺儀、加速度計(jì)、磁力 計(jì)、氣壓計(jì)采集數(shù)據(jù)，并將采集到的傳感器數(shù)據(jù)發(fā)送給DSP控制器，GPS模塊將定位數(shù)據(jù)發(fā) 送給DSP控制器，DSP控制器將傳感器數(shù)據(jù)和定位數(shù)據(jù)進(jìn)行導(dǎo)航解算。
[0007] 進(jìn)一步，所述氣壓計(jì)的采樣頻率為10Hz，所述陀螺儀、加速度計(jì)、磁力計(jì)的采樣頻 率均為50Hz。
[0008] 進(jìn)一步，所述陀螺儀型號(hào)為L(zhǎng)3G4200D，加速度計(jì)型號(hào)為ADXL345,磁力計(jì)型號(hào)為 HMC5883L，氣壓計(jì)型號(hào)為BMP085,GPS模塊型號(hào)為UbloxNE0-6M，數(shù)據(jù)采集處理器型號(hào)為 STM32F103VET6,DSP控制器型號(hào)為TMS320F28335。
[0009]GPS和INS松耦合方式多旋翼飛行器組合導(dǎo)航系統(tǒng)進(jìn)行導(dǎo)航的方法，按照以下步 驟進(jìn)行：
[0010] 步驟1:預(yù)處理；
[0011] a)低成本組合導(dǎo)航系統(tǒng)安裝到飛行器，Y軸指向飛行器機(jī)身的正前方，X軸指向機(jī) 身的右方，Z軸指向機(jī)身的正上方；
[0012] b)離地1米以上固定飛行器，保證飛行器不會(huì)起飛；
[0013]C)從靜止時(shí)刻開(kāi)始，間隔lmin增大電調(diào)的PWM控制信號(hào)高電平持續(xù)時(shí)間0. 02ms， 并實(shí)時(shí)采集并保存陀螺儀、加速度計(jì)、磁力計(jì)和氣壓計(jì)的數(shù)據(jù)；
[0014]d)分別對(duì)磁力計(jì)和氣壓計(jì)估計(jì)每個(gè)PWM控制信號(hào)變化的時(shí)間間隔內(nèi)采集的數(shù)據(jù) 的方差，并作圖，縱坐標(biāo)為方差數(shù)據(jù)，橫坐標(biāo)為對(duì)應(yīng)的PWM高電平持續(xù)時(shí)間，使用MATLAB的 cfto〇l工具箱對(duì)每條曲線進(jìn)行擬合，得到磁力計(jì)和氣壓計(jì)每個(gè)軸的數(shù)據(jù)方差與控制信號(hào)之 間的關(guān)系模型MCI和MC2。
[0015] 組合導(dǎo)航系統(tǒng)加電啟動(dòng)，點(diǎn)亮LED狀態(tài)燈，數(shù)據(jù)采集處理器以50Hz頻率通過(guò)IIC 總線讀取陀螺儀、加速度計(jì)和磁力計(jì)的數(shù)據(jù)，得到飛行器當(dāng)前的三軸角速率（wx、wy、wz)、三 軸加速度（ax、ay、az)和三軸磁場(chǎng)強(qiáng)度（mx、my、mz);
[0016] 步驟2 :重復(fù)步驟1并存儲(chǔ)100組三軸角速率和三軸加速度，檢測(cè)判斷是否為飛行 器靜止?fàn)顟B(tài)采樣的數(shù)據(jù)；
[0017] 步驟3:若步驟2中的靜止?fàn)顟B(tài)檢測(cè)不通過(guò)，重復(fù)步驟1~步驟2,直到滿足條件1 和條件2同時(shí)成立條件為止；
[0018] 步驟4:步驟2的靜止?fàn)顟B(tài)檢測(cè)通過(guò)后，估計(jì)陀螺儀的零偏，零偏估計(jì)完成后，點(diǎn)亮 LED狀態(tài)燈；
[0019] 步驟5:在10s時(shí)間內(nèi)，手動(dòng)將組合導(dǎo)航系統(tǒng)繞其X、Y、Z軸旋轉(zhuǎn)一次，計(jì)算磁力計(jì) 零偏，計(jì)算完成后，點(diǎn)亮LED狀態(tài)燈；
[0020] 步驟6:數(shù)據(jù)采集處理器以10Hz讀取氣壓計(jì)采樣數(shù)據(jù)，連續(xù)讀取10組，得到初始 狀態(tài)初始?xì)鈮焊撸˙aro_InitHeight);
[0021]步驟7:DSP控制器通過(guò)串口USART3接收當(dāng)前的GPS定位數(shù)據(jù)：Aeps(經(jīng)度）、 LSPS(煒度）、HSPS(高度）、VESPS(東向速度）、VNSPS(北向速度）、VUSPS(天向速度）；
[0022] 步驟8:重復(fù)步驟7接收30次有效GPS數(shù)據(jù)，將最新的GPS定位數(shù)據(jù)發(fā)送至DSP控 制器；
[0023] 步驟9 :DSP控制器修正陀螺儀和磁力計(jì)的零偏，將氣壓高修正到與GPS高同步，得 到當(dāng)前的氣壓高采樣；
[0024] 步驟10 :分別以50HZ采集陀螺儀、加速度計(jì)和磁力計(jì)數(shù)據(jù)，以10Hz采集氣壓高度 計(jì)數(shù)據(jù)，每一次采樣數(shù)據(jù)經(jīng)過(guò)步驟9處理；
[0025]步驟11 :對(duì)陀螺儀和加速度計(jì)數(shù)據(jù)做前置低通濾波；
[0026] 步驟12 :計(jì)算慣性導(dǎo)航系統(tǒng)的粗對(duì)準(zhǔn)初始參數(shù)；
[0027] 步驟13 :重復(fù)步驟9和步驟10直到任務(wù)完成為止，更新姿態(tài)、速度和位置；
[0028] 步驟14 :DSP控制器發(fā)送當(dāng)前解算的姿態(tài)、速度、位置以及采集的傳感器數(shù)據(jù)， 包括三軸加速度、三軸角速度、三軸磁場(chǎng)強(qiáng)度和氣壓高，以及解算的當(dāng)前姿態(tài)角觀測(cè)量 列々）、和丫 〇〇,發(fā)送完成后，慣導(dǎo)計(jì)算機(jī)修正姿態(tài)、速度和位置誤差；
[0029] 步驟15 :DSP控制器接收數(shù)據(jù)采集處理器的數(shù)據(jù)，并始終只保留最新一次解算的 結(jié)果，接收GPS定位數(shù)據(jù)，每一次接收完成后，對(duì)GPS做前置濾波；
[0030] 步驟16 :組合導(dǎo)航計(jì)算機(jī)利用慣導(dǎo)計(jì)算機(jī)發(fā)送的數(shù)據(jù)和步驟15處理后的數(shù)據(jù)，計(jì) 算慣性導(dǎo)航系統(tǒng)的修正量；
[0031] 步驟17 :將步驟16的組合導(dǎo)航結(jié)果發(fā)送至慣導(dǎo)計(jì)算機(jī)并用步驟14的方法修正慣 導(dǎo)參數(shù)，發(fā)送完成后重復(fù)執(zhí)行步驟15-16 ;
[0032] 步驟18 :慣導(dǎo)計(jì)算機(jī)判斷，接收到的三個(gè)角度修正量的變化均小于1度時(shí)，點(diǎn)亮 LED，表示初始對(duì)準(zhǔn)完成，并通知飛控系統(tǒng)可以起飛，同時(shí)將當(dāng)前的姿態(tài)角、速度和位置信息 發(fā)送至飛控系統(tǒng)。飛行系統(tǒng)收到后將當(dāng)前的最大控制脈寬發(fā)送至導(dǎo)航系統(tǒng)，導(dǎo)航系統(tǒng)根據(jù) 預(yù)處理建立的MCI和MC2模型分別計(jì)算磁航向角、氣壓高度的方差估計(jì)值；
[0033] 步驟19 :重復(fù)執(zhí)行步驟17和步驟13,即可實(shí)現(xiàn)完整的多旋翼飛行器導(dǎo)航應(yīng)用。
[0034] 本發(fā)明的有益效果是：根據(jù)多旋翼飛行器的動(dòng)態(tài)特性及低成本傳感器的誤差特 點(diǎn)，為飛行器提供準(zhǔn)確的姿態(tài)、速度和位置信息。
【附圖說(shuō)明】
[0035] 圖1是本發(fā)明硬件系統(tǒng)示意圖；
[0036] 圖2是本發(fā)明導(dǎo)航方法流程示意圖；
[0037] 圖3是本發(fā)明STM32程序處理流程圖；
[0038] 圖4是本發(fā)明DSP程序處理流程圖。
[0039] 圖中，1?數(shù)據(jù)采集處理器，2?陀螺儀，3?加速度計(jì)，4?磁力計(jì)，5?氣壓計(jì)，6.DSP控 制器，7.GPS模塊。
【具體實(shí)施方式】
[0040] 下面結(jié)合【具體實(shí)施方式】對(duì)本發(fā)明進(jìn)行詳細(xì)說(shuō)明。
[0041] 本發(fā)明組合導(dǎo)航系統(tǒng)針對(duì)多旋翼飛行器而設(shè)計(jì)，多旋翼飛行器最常見(jiàn)的是四旋翼 飛行器（Quad-Rotor)。四旋翼飛行器也叫四軸飛行器，是一種垂直起降（VT0L)的無(wú)人駕駛 航空器，其衍生產(chǎn)品還包括六軸、八軸甚至十六軸等多旋翼飛行器。這種飛行器通常由機(jī)械 結(jié)構(gòu)、飛行控制及導(dǎo)航系統(tǒng)、動(dòng)力系統(tǒng)、通信系統(tǒng)和任務(wù)模塊組成，可以實(shí)現(xiàn)遙控或自主飛 行。相比于固定翼無(wú)人機(jī)，四軸飛行器擁有旋翼無(wú)人機(jī)靈活、可懸停的優(yōu)勢(shì)；而相比傳統(tǒng)的 旋翼無(wú)人機(jī)，四軸飛行器具有更大的載重、更簡(jiǎn)單的機(jī)械結(jié)構(gòu)和更靈活的飛行方式。
[0042] 硬件系統(tǒng)如圖1所示：數(shù)據(jù)采集處理器1控制陀螺儀2、加速度計(jì)3、磁力計(jì)4、氣 壓計(jì)5采集數(shù)據(jù)，并將采集到的傳感器數(shù)據(jù)發(fā)送給DSP控制器6,GPS模塊7將定位數(shù)據(jù)發(fā)