基于MPU的INS慣性導(dǎo)航和實(shí)時(shí)姿態(tài)檢測系統(tǒng)(完整資料)

基于MPU的INS慣性導(dǎo)航和實(shí)時(shí)姿態(tài)檢測系統(tǒng)(完整資料）(可以直接使用，可編輯優(yōu)秀版資料，歡迎下載）

基于MPU６050的INS慣性導(dǎo)航和實(shí)時(shí)姿態(tài)檢測系統(tǒng)基于MPU的INS慣性導(dǎo)航和實(shí)時(shí)姿態(tài)檢測系統(tǒng)(完整資料）(可以直接使用，可編輯優(yōu)秀版資料，歡迎下載）項(xiàng)目目標(biāo)及功能說明項(xiàng)目目標(biāo)學(xué)習(xí)使用正點(diǎn)原子探索者開發(fā)板,并熟悉開發(fā)板上的MPU60５0六軸傳感器的工作原理和各函數(shù)的調(diào)用過程.同時(shí)學(xué)習(xí)開發(fā)板的擴(kuò)展接口，嘗試在開發(fā)板上擴(kuò)展藍(lán)牙模塊，并實(shí)現(xiàn)開發(fā)板與手機(jī)等含有藍(lán)牙模塊的電子設(shè)備通過藍(lán)牙連接并進(jìn)行數(shù)據(jù)的傳輸。在完成上述內(nèi)容的基礎(chǔ)上，實(shí)現(xiàn)將MＰU605０六軸傳感器的加速度計(jì)和陀螺儀的數(shù)據(jù)傳送到手機(jī)上，在手機(jī)上實(shí)現(xiàn)陀螺儀的變化效果展示。同時(shí)通過串口將MＰU６050數(shù)據(jù)傳送到電腦上，通過Mａt(yī)ｌab編程處理數(shù)據(jù)，實(shí)現(xiàn)慣性導(dǎo)航的簡單展示。系統(tǒng)功能說明系統(tǒng)最主要的功能有兩個(gè)：一個(gè)是在手機(jī)端能夠展示開發(fā)板上MPU6050陀螺儀的姿態(tài)變化，通過一個(gè)立方體的轉(zhuǎn)動來表示陀螺儀的轉(zhuǎn)動；另一個(gè)是在電腦端能夠讀取ＭＰＵ60５0的數(shù)據(jù),并通過對數(shù)據(jù)的處理還原數(shù)據(jù)中存儲的MPＵ6050的姿態(tài)變化，簡單展現(xiàn)出慣性導(dǎo)航的效果.在實(shí)現(xiàn)系統(tǒng)最主要的兩個(gè)功能過程中,還需要實(shí)現(xiàn)一些基礎(chǔ)功能.開發(fā)板能夠通過藍(lán)牙與手機(jī)連接并傳輸數(shù)據(jù);開發(fā)板能夠通過串口將數(shù)據(jù)發(fā)送出去;在電腦端能夠讀取開發(fā)板上串口輸出的數(shù)據(jù)等。需求分析慣性導(dǎo)航系統(tǒng)用于各種運(yùn)動機(jī)具中,包括飛機(jī)、潛艇、航天飛機(jī)等運(yùn)輸工具及導(dǎo)彈,然而成本及復(fù)雜性限制了其可以應(yīng)用的場合.但是，存在一種情形:衛(wèi)星一旦突然因故障、敵方打擊或干擾（如太陽風(fēng)暴)等原因無法提供服務(wù)，這對依賴GPS、北斗等衛(wèi)星導(dǎo)航系統(tǒng)作為唯一PNT（Positioｎ、Naｖigatｉon、Ｔime)信息來源的系統(tǒng)來說可能是致命的災(zāi)難。作為目前為止衛(wèi)星導(dǎo)航系統(tǒng)最好的備援—-慣性導(dǎo)航系統(tǒng)（INS），將于屆時(shí)發(fā)揮出巨大的作用,其精度完全可以媲美ＧPS等衛(wèi)星導(dǎo)航系統(tǒng)。并且它不需要外部參考就可確定當(dāng)前位置、方向及速度,從而使它自然地不受外界的干擾和欺騙。定位、導(dǎo)航和授時(shí)服務(wù)對軍隊(duì)而言就像氧氣對人類一樣不可或缺，因此通過研究新機(jī)理、研制新設(shè)備、開發(fā)新算法,以擺脫人員和系統(tǒng)設(shè)備對GPS的依賴,具有極大的戰(zhàn)略意義。姿態(tài)監(jiān)測系統(tǒng)可廣泛應(yīng)用于關(guān)鍵資產(chǎn)姿態(tài)變化的無線實(shí)時(shí)監(jiān)控。由于目前移動智能終端設(shè)備的數(shù)量和質(zhì)量逐步提升，因此,通過計(jì)算機(jī)上傳統(tǒng)的上位機(jī)軟件進(jìn)行姿態(tài)監(jiān)測,逐漸暴露出了自身的缺點(diǎn)——串口傳輸無法實(shí)現(xiàn)無線監(jiān)測、計(jì)算機(jī)相比智能終端便攜性極差.因此，使用無線傳輸（藍(lán)牙、紅外、WＩFI、GSM等)的技術(shù)，開發(fā)一款在移動智能終端可以實(shí)時(shí)顯示物體姿態(tài)的應(yīng)用,具有很高的實(shí)用價(jià)值和廣泛的市場應(yīng)用前景。開發(fā)環(huán)境移動終端操作系統(tǒng)：?Androｉd4.4．４KｉtKaｔ計(jì)算機(jī)操作系統(tǒng)：Ｗiｎｄows８．1Proｘ６4串口開發(fā)：?MＡTLABＲ2014a開發(fā)板IDE: KｅilｕVｉsioｎ5AndrｏidIDＥ：EcliｐseJａvaEEIDEforWebDevｅlopｅrsAｎdroiｄDｅvｅlopｍentTｏolkiｔ2３。0.4。14６8518項(xiàng)目進(jìn)展情況到目前為止，我組已實(shí)現(xiàn)了以下功能：STM32F4開發(fā)板上MPU6050六軸傳感器的數(shù)據(jù)獲取并顯示在ＬCD屏幕上。在LCD屏幕上繪出圓形圖案,且圓形圖案能根據(jù)MＰＵ6０50六軸傳感器的姿態(tài)變化而運(yùn)動，傳感器傾斜角度越大，圖案運(yùn)動速度越快。擴(kuò)展藍(lán)牙模塊，能通過藍(lán)牙模塊與手機(jī)連接并進(jìn)行數(shù)據(jù)通信。根據(jù)函數(shù)提供的幀格式定義數(shù)據(jù)幀,并通過ＵSＡRＴ接口將數(shù)據(jù)幀傳給PＣ端。在手機(jī)端能根據(jù)藍(lán)牙獲取的MPＵ60５0六軸傳感器的陀螺儀數(shù)據(jù)繪出立方體，立方體能在可接受的時(shí)間延遲內(nèi)實(shí)時(shí)展現(xiàn)MPU６0５0的姿態(tài)變化(轉(zhuǎn)動方向和角度）。在PC端能通過對從USAＲT接口獲取的數(shù)據(jù)幀進(jìn)行解析獲?。停校?050加速度傳感器和陀螺儀的數(shù)據(jù)，并根據(jù)數(shù)據(jù)幀中設(shè)置的校驗(yàn)位進(jìn)行數(shù)據(jù)校驗(yàn)。在ＰC端能根據(jù)解析出的加速度傳感器和陀螺儀數(shù)據(jù),在可接受的誤差范圍內(nèi)還原ＭPＵ６０５0的姿態(tài)變化(包括位移、轉(zhuǎn)動方向和角度）,實(shí)現(xiàn)一個(gè)簡單的慣性導(dǎo)航系統(tǒng).系統(tǒng)設(shè)計(jì)IIC總線工作原理總線的構(gòu)成及信號類型Ｉ2C總線是由數(shù)據(jù)線ＳＤA和時(shí)鐘ＳCＬ構(gòu)成的串行總線，可發(fā)送和接收數(shù)據(jù)。在ＣＰＵ與被控ＩC之間、IC與ＩＣ之間進(jìn)行雙向傳送,最高傳送速率100kbｐs。各種被控制電路均并聯(lián)在這條總線上，但就像電話機(jī)一樣只有撥通各自的號碼才能工作，所以每個(gè)電路和模塊都有唯一的地址，在信息的傳輸過程中,I2Ｃ總線上并接的每一模塊電路既是主控器（或被控器），又是發(fā)送器(或接收器),這取決于它所要完成的功能。CPU發(fā)出的控制信號分為地址碼和控制量兩部分，地址碼用來選址,即接通需要控制的電路,確定控制的種類;控制量決定該調(diào)整的類別(如對比度、亮度等）及需要調(diào)整的量。這樣,各控制電路雖然掛在同一條總線上，卻彼此獨(dú)立，互不相關(guān)。I2Ｃ總線在傳送數(shù)據(jù)過程中共有三種類型信號，它們分別是：開始信號、結(jié)束信號和應(yīng)答信號。開始信號:ＳCL為高電平時(shí),SDA由高電平向低電平跳變，開始傳送數(shù)據(jù)。結(jié)束信號：ＳCＬ為低電平時(shí),SDA由低電平向高電平跳變，結(jié)束傳送數(shù)據(jù)。應(yīng)答信號：接收數(shù)據(jù)的ＩC在接收到8ｂｉｔ數(shù)據(jù)后，向發(fā)送數(shù)據(jù)的IC發(fā)出特定的低電平脈沖，表示已收到數(shù)據(jù)。CPU向受控單元發(fā)出一個(gè)信號后,等待受控單元發(fā)出一個(gè)應(yīng)答信號，ＣPU接收到應(yīng)答信號后,根據(jù)實(shí)際情況做出是否繼續(xù)傳遞信號的判斷。若未收到應(yīng)答信號，由判斷為受控單元出現(xiàn)故障。這些信號中,開始信號是必須的，結(jié)束信號和應(yīng)答信號都可以不要，IＩC總線時(shí)序圖如圖５.1。1—1所示。圖STＹLEREＦ３\ｓ5。1SEQ圖\*ARABＩＣ\s31IIＣ總線時(shí)序圖探索者STM３2F4開發(fā)板板載的EEＰROM芯片型號為24C02.該芯片的總?cè)萘繛?56字節(jié)，通過ＩＩＣ總線與外部連接。SＴM32Ｆ４開發(fā)板有硬件IIＣ，但是設(shè)計(jì)的比較復(fù)雜，而且穩(wěn)定性不好，所以我組使用GPIＯ軟件模擬ＩIC來對２４C02進(jìn)行讀寫。同時(shí)使用軟件更具有移植性，只要有IO口,將軟件移植過去就能使用模擬的IIＣ,而硬件必須MCU的支持。硬件設(shè)計(jì)實(shí)現(xiàn)模擬IIC需要用到的硬件資源有：串口（USMART）、GPIO、24Ｃ０2。圖SＴYLＥREＦ３\s5.1ＳEQ圖＼*ARABＩC\ｓ3２STM３２Ｆ4與24C０2連接圖我組通過GPＩO來模擬IIＣ，2４C２的ＳCL和SDA分別連在GＰIO_PB8和GＰIO_PB9上,連接關(guān)系如圖5。1．2-1。MPU６050工作原理MＰU60５0引腳圖STYＬEREF３\s５。2SEQ圖\*ＡRABIC\s31MPU605０結(jié)構(gòu)圖模塊外觀如圖５．２．1—2所示：圖ＳTYLEREF3\s5.2SEQ圖＼＊AＲABＩC\s32ＭＰＵ6050實(shí)物圖圖STＹＬEＲEF３＼s5。2SEＱ圖\*ＡRＡBIC\s3３MPU６0５０內(nèi)部邏輯框圖如圖5．2。1-1為MPＵ6050六軸傳感器的結(jié)構(gòu)圖，總共有２4個(gè)引腳，而圖5。2。1-2為MＰＵ6０50的內(nèi)部邏輯框圖,描述了ＭPU6050內(nèi)部的模塊結(jié)構(gòu)，以及各引腳的連接情況.表SＴYLEREF４＼s5。2。1SEＱ表\＊AＲABIC\s41ＭPU60５0引腳輸出和信號描述表5。２．1—1對每一個(gè)引腳的名稱和作用進(jìn)行了說明。在上述引腳中,SCL和ＳDA是連接MＣＵ的IIC接口，MＣU通過這個(gè)ＩIC接口來控制ＭPU6０50.另外還有一個(gè)IIＣ接口，連接的引腳為AＵＸ_CL和AUＸ_DA，這個(gè)接口可用來連接外部從設(shè)備，比如磁傳感器，這樣就可以與MPU6０５０組成一個(gè)九軸傳感器.ＶLＯGIC是IO口電壓，該引腳最低可以到1。8V，我們一般直接VDD即可.ＡD０是從IIＣ接口（接MＣＵ）的地址控制引腳,該引腳控制IIＣ地址的最低位,如果接ＧND，則MPU60５0的IIC地址是0Ｘ68；如果接VDD，則是0X69。注意：這里的地址是不包含數(shù)據(jù)傳輸?shù)淖畹臀坏模ㄗ畹臀挥脕肀硎咀x寫).在探索者STM32F４開發(fā)板上,AＤ0是接GND的,即MＰU60５0的IIC地址是0X６８（不含最低位）。硬件設(shè)計(jì)圖STYLEREF3\s5。2SEQ圖\＊ＡRＡBIC\s3４MPU6０50與STM3２F4的連接電路圖從圖5．2。２-１可以看出,MPU605０通過三根線與STM3２Ｆ4開發(fā)板連接，其中IIC總線時(shí)和２4C02以及WM89７8共用，接在PB8和PB9上面.ＭPU605０的中斷輸出,連接在STＭ３２F４的PC０腳，不過本例程我們并沒有用到中斷。另外，AD0接的GND，所以MPU６05０的器件地址是:0X68。初始化操作在使用ＳTＭ32F4讀取MＰU6050的加速度和角度傳感器數(shù)據(jù)之前,需要做以下初始化操作：初始化IIC接口MPU605０采用ＩＩC與STM３２F4通信,所以我們需要先初始化與ＭPU６05０連接的SＤA和ＳＣＬ數(shù)據(jù)線。復(fù)位MPＵ６050這一步讓MＰU60５0內(nèi)部所有寄存器恢復(fù)默認(rèn)值,通過對電源管理寄存器１（0X6B）的bit7寫1實(shí)現(xiàn).復(fù)位后,電源管理寄存器1恢復(fù)默認(rèn)值(0X40）,然后必須設(shè)置該寄存器為0X00,以喚醒MPＵ６050，進(jìn)入正常工作狀態(tài)。設(shè)置角速度傳感器（陀螺儀)和加速度傳感器的滿量程范圍這一步,我們設(shè)置兩個(gè)傳感器的滿量程范圍（FSR），分別通過陀螺儀配置寄存器（0X１Ｂ）和加速度傳感器配置寄存器(０X１C)設(shè)置。我們一般設(shè)置陀螺儀的滿量程范圍為±２００0dps，加速度傳感器的滿量程范圍為±2ｇ。設(shè)置其他參數(shù)這里,我們還需要配置的參數(shù)有：關(guān)閉中斷、關(guān)閉AUXIIＣ接口、禁止FIFO、設(shè)置陀螺儀采樣率和設(shè)置數(shù)字低通濾波器(DLPF)等。本章我們不用中斷方式讀取數(shù)據(jù),所以關(guān)閉中斷,然后也沒用到AUXＩIC接口外接其他傳感器，所以也關(guān)閉這個(gè)接口.分別通過中斷使能寄存器（0X３8)和用戶控制寄存器(0X６Ａ）控制。MPU6050可以使用FＩFＯ存儲傳感器數(shù)據(jù),不過本章我們沒有用到,所以關(guān)閉所有FIFO通道，這個(gè)通過FIFO使能寄存器(0Ｘ2３）控制，默認(rèn)都是０（即禁止FIＦO）,所以用默認(rèn)值就可以了。陀螺儀采樣率通過采樣率分頻寄存器(０X１9）控制，這個(gè)采樣率我們一般設(shè)置為50即可。數(shù)字低通濾波器（DLPF)則通過配置寄存器（0X1A）設(shè)置，一般設(shè)置ＤLＰF為帶寬的1/2即可。配置系統(tǒng)時(shí)鐘源并使能角速度傳感器和加速度傳感器系統(tǒng)時(shí)鐘源同樣是通過電源管理寄存器1（0X１B）來設(shè)置,該寄存器的最低三位用于設(shè)置系統(tǒng)時(shí)鐘源選擇,默認(rèn)值是0（內(nèi)部8ＭRC震蕩），不過我們一般設(shè)置為1,選擇ｘ軸陀螺ＰLL作為時(shí)鐘源,以獲得更高精度的時(shí)鐘。同時(shí)，使能角速度傳感器和加速度傳感器，這兩個(gè)操作通過電源管理寄存器２(0X6C）來設(shè)置，設(shè)置對應(yīng)位為0即可開啟。相關(guān)寄存器在讀取MＰＵ605０數(shù)據(jù)中主要用到了以下寄存器:ＰoｗerＭaｎagement1（電源管理寄存器１)表STＹLEＲEF4＼ｓ5．2。4ＳEQ表＼*ＡRABＩC\s4１電源管理寄存器１各位描述如表5。2.4—１,寄存器地址為0ｘ６B。ＤＥVICＥ_RＥＳET位用來控制復(fù)位，設(shè)置為1,復(fù)位MPU605０,復(fù)位結(jié)束后,MPＵ硬件自動清零該位;SLＥEＥP位用于控制ＭPU6050的工作模式，復(fù)位后，該位為１，即進(jìn)入了睡眠模式（低功耗），所以我們要清零該位，以進(jìn)入正常工作模式;ＴEMP_ＤIS用于設(shè)置是否使能溫度傳感器，設(shè)置為０,則使能;CＬKSＥL［2:0]用于選擇系統(tǒng)時(shí)鐘源，選擇關(guān)系如表5.2．４－2所示，默認(rèn)是使用內(nèi)部８ＭRC晶振的,精度不高,所以我們一般選擇X/Ｙ/Z軸陀螺作為參考的PＬL作為時(shí)鐘源，一般設(shè)置ＣLKＳEＬ=0０1即可。表ＳTYLEREF４\s5．2。4ＳＥQ表＼*ARＡBIC\s4２CＬＫSEL選擇列表GyｒoscopeConｆigｕratioｎ(陀螺儀配置寄存器)表STYLERＥF４\s5。2.4SEＱ表＼＊ARABIC\s４3表陀螺儀配置寄存器各位描述如表5.2。4-3,寄存器地址為0x１B。FS_SEL［1：０]兩個(gè)位用于設(shè)置陀螺儀的滿量程范圍：０為±2５0°/s;1為±50０°/s;２為±１００0°／s；3為±２000°/s.我們一般設(shè)置為3，即±2000°/s,而因?yàn)橥勇輧x的ADC為１６位分辨率，所以得到靈敏度為：６5536／4000=16。４ＬSB/(°/s）。AｃcｅleｒoｍetｅrConfigｕration（加速度傳感器配置寄存器)表SＴYLERＥF4\ｓ5．２．4SEQ表\*ＡＲＡBIC\s44加速度傳感器配置寄存器各位描述如表5。２．4-4，寄存器地址為０ｘ1C。AＦS_SEＬ［1：0]兩個(gè)位用于設(shè)置加速度傳感器的滿量程范圍：0為±2g；1為±4g;2為±８g;３為±１6g。我們一般設(shè)置為0，即±2ｇ，而因?yàn)榧铀俣葌鞲衅鞯腁DC也是16位分辨率，所以得到靈敏度為：6５536/4＝1６3８４LSB/g.ＦIFOEnaｂle（ＦIFO使能寄存器)表SＴYLEREF4\ｓ5。2.4SEＱ表\*ARＡBIC\s4５FIFＯ使能寄存器各位描述如表5.2.4—５，寄存器地址為０x２3.該寄存器用于控制FＩFＯ使能,在簡單讀取傳感器數(shù)據(jù)的時(shí)候,可以不用FIFO，設(shè)置對應(yīng)位為０即可禁止FＩFＯ，設(shè)置為1，則使能FＩFO。注意加速度傳感器的3個(gè)軸,全由1個(gè)位(ACＣEL_FIFO_EN)控制，只要該位置1，則加速度傳感器的三個(gè)通道都開啟ＦIFＯ了。SampleRateDividｅr(采樣率分頻寄存器)表STYＬEREF4＼ｓ５。2．４SEQ表\＊ＡRABＩＣ＼s４6陀螺儀采樣率分頻寄存器各位描述如表5。2．4—6，寄存器地址為0x１9。該寄存器用于設(shè)置MＰU6０5０的陀螺儀采樣頻率，計(jì)算公式為：采樣頻率＝陀螺儀輸出頻率/（1+SMPＬRT_DＩV）這里陀螺儀的輸出頻率,是1Khz或者8Ｋhｚ，與數(shù)字低通濾波器(DＬPF)的設(shè)置有關(guān),當(dāng)DLＰF_CFG=0／7的時(shí)候，頻率為8Ｋhz,其他情況是1Khz.而且DLPF濾波頻率一般設(shè)置為采樣率的一半。我們假定設(shè)置采樣率為5０Ｈz，那么SMPＬＲT_ＤIV=１00０/50—1=19。Cｏnｆiｇurａt(yī)ioｎ（配置寄存器）表STYＬEREF4＼ｓ5。2．４SＥQ表\*ＡＲABIC＼s47配置寄存器各位描述如表5.2。4-７,寄存器地址為０x1A。加速度計(jì)和陀螺儀是根據(jù)數(shù)字低通濾波器（DLPF)的三個(gè)設(shè)置位進(jìn)行過濾的，即DLPF_ＣＦG[2:0].DLＰＦ_CFＧ不同配置對應(yīng)的過濾情況如表5．2。4—8所示：表SＴYLEREF4\s5。2.4SEＱ表\＊ARＡBIＣ\ｓ4８DLPＦ_CＦG配置表加速度傳感器輸出速率(Fｓ)固定是1Khｚ,而角速度傳感器的輸出速率(Ｆs），則根據(jù)DLPF_CFG的配置有所不同。一般我們設(shè)置角速度傳感器的帶寬為其采樣率的一半,如前面所說的,如果設(shè)置采樣率為50Hz，那么帶寬就應(yīng)該設(shè)置為２5Hｚ，取近似值20Hz，就應(yīng)該設(shè)置ＤLPF_CＦG為10０。ＰoｗerMaｎageｍｅnt2(電源管理寄存器2）表SＴYLEREF４＼ｓ5．2。4SEQ表\*ARAＢＩC＼ｓ４9電源管理寄存器2各位描述如表5．2。4-9,寄存器地址為０x6C.該寄存器的ＬP＿WAKＥ_CTＲL用于控制低功耗時(shí)的喚醒頻率,項(xiàng)目中沒有用到.剩下的6位,分別控制加速度和陀螺儀的x/y/z軸是否進(jìn)入待機(jī)模式，由于不需要進(jìn)入待機(jī)模式，所以全部設(shè)置為０。ＧyｒｏscｏpeＭeasurements（陀螺儀數(shù)據(jù)輸出寄存器）表STYLＥＲEF4\ｓ5．2。4SEQ表＼＊ＡRABIＣ＼ｓ4１0陀螺儀數(shù)據(jù)輸出寄存器各位描述如表5。２．４—10,總共有６個(gè)寄存器，地址為0x43～0ｘ４8，通過讀取這6個(gè)寄存器,就可以讀到陀螺儀ｘ/y／z軸的值，比如x軸的數(shù)據(jù)，可以通過讀取0ｘ43(高8位)和0x４4(低8位）寄存器得到,其他軸以此類推。AｃcelerometerMeasurementｓ（加速度傳感器數(shù)據(jù)輸出寄存器）表STＹＬERＥF4\s５。2．4ＳEQ表\*ARＡBIＣ\ｓ4１1加速度傳感器數(shù)據(jù)輸出寄存器各位描述如表5．2.4-1１，總共有6個(gè)寄存器，地址為0ｘ3B~０x40，通過讀取這８個(gè)寄存器，就可以讀到加速度傳感器x/ｙ/z軸的值,比如讀ｘ軸的數(shù)據(jù)，可以通過讀?。皒3B(高8位)和0x3C(低８位）寄存器得到，其他軸以此類推.ＡTK－HＣ0５工作原理ＡＴK－HC05引腳AＴＫ—ＨC０5模塊非常小巧（1６ｍｍ*32ｍm)，模塊通過6個(gè)2。54ｍm間距的排針與外部連接，模塊外觀如圖５.３。1-1所示：圖STＹＬEREF3\s5。3SEQ圖\＊ＡRABIC＼s31ATK－HC05模塊外觀圖圖５．3.1—１中，從右到左，依次為模塊引出的PＩN1～PIN6腳，各引腳的詳細(xì)描述如表5.３.1—1所示:表STＹＬEREF4\s5.３。１SEQ表＼＊AＲABIC\s41ＡTK—HC０5模塊各引腳功能描述另外，模塊自帶了一個(gè)狀態(tài)指示燈：SＴA.該燈有3種狀態(tài),分別為:在模塊上電的同時(shí)（也可以是之前），將KEＹ設(shè)置為高電平（接ＶCＣ），此時(shí)STＡ慢閃（1秒亮１次）,模塊進(jìn)入AT狀態(tài)，且此時(shí)波特率固定為384００.在模塊上電的時(shí)候,將KEＹ懸空或接GND，此時(shí)ＳＴA快閃（1秒2次）,表示模塊進(jìn)入可配對狀態(tài).如果此時(shí)將KEY再拉高，模塊也會進(jìn)入AT狀態(tài),但是ＳＴA依舊保持快閃。模塊配對成功，此時(shí)SＴA雙閃（一次閃2下,2秒閃一次）。硬件設(shè)計(jì)圖ＳTYLEREF3＼s５.3SEＱ圖\*ＡRＡBIC\s3２ＡＴK—ＨＣ０５藍(lán)牙串口模塊原理圖模塊與單片機(jī)連接最少只需要4根線即可：ＶCC、GND、TＸD、RXD、VCC和ＧND用于給模塊供電，模塊TXD和RＸＤ則連接單片機(jī)的RXＤ和TXＤ即可。該模塊兼容5Ｖ和3。3Ｖ單片機(jī)系統(tǒng),所以可以很方便的連接到系統(tǒng)里面去。ＡＴK－HC05模塊與單片機(jī)系統(tǒng)的典型連接方式如圖５。3。2—２所示：圖STＹLERＥF3＼s5.3ＳEQ圖＼＊ＡRABIC\ｓ33AＴK-HC０５模塊與單片機(jī)系統(tǒng)連接示意圖圖中虛線連接表示可有可無，可以根據(jù)需要，選擇性的使用。ATK－HC0５與藍(lán)牙主機(jī)連接首先，開機(jī)檢測ATK－HC05藍(lán)牙模塊是否存在，如果檢測不成功,則報(bào)錯。檢測成功之后，顯示模塊的主從狀態(tài)，并顯示模塊是否處于連接狀態(tài)，ＤS0閃爍，提示程序運(yùn)行正常.按KEY0按鍵,可以開啟/關(guān)閉自動發(fā)送數(shù)據(jù)（通過藍(lán)牙模塊發(fā)送）；按KＥY_UP按鍵可以切換模塊的主從狀態(tài)。藍(lán)牙模塊接收到的數(shù)據(jù),將直接顯示在LCD上（僅支持ASCＩＩ字符顯示).同時(shí),還可以通過USMART對AＴK—ＨC0５藍(lán)牙模塊進(jìn)行AＴ指令查詢和設(shè)置。結(jié)合手機(jī)端藍(lán)牙軟件（藍(lán)牙串口助手v１．97.ａpk)，可以實(shí)現(xiàn)手機(jī)無線控制開發(fā)板（點(diǎn)亮和關(guān)閉LEＤ１）。所要用到的硬件資源如下:指示燈DS0、DＳ1KEY０／KEY_UＰ兩個(gè)按鍵串口１、串口3TFTLCD模塊AＴＫ-ＨC05-V１３藍(lán)牙串口模塊前面介紹了ATK—HC05藍(lán)牙串口模塊的接口，而AＬIEＮTEK探索者STM3２F４07開發(fā)板板載了一個(gè)ATＫ模塊接口(ATＫＭODULE），ATK—HC05藍(lán)牙模塊可直接插入該接口實(shí)現(xiàn)與探索者STM32Ｆ４開發(fā)板的連接。AＴＫＭOＤULE同開發(fā)板主芯片的連接原理圖如—１所示:圖STYLＥREF３\ｓ5。3SＥQ圖\*ARABIC\s34ATK-ＭODUＬE接口與MCU連接關(guān)系從上圖可以看出，藍(lán)牙模塊的串口最簡單的辦法是連接在開發(fā)板的串口３上面，只需要用跳線帽短接P１0的USＡRT3_ＲＸ和GＢC_TX以及ＵSＡRT3_ＴＸ和GＢC_RX即可實(shí)現(xiàn).連接好之后,探索者STＭ32Ｆ4０７開發(fā)板與ATK—HC05藍(lán)牙模塊的連接關(guān)系如表2.1所示:表STYLERＥF4＼s5.３．３SEQ表\*ARAＢIＣ\s41ATK—HＣ05藍(lán)牙模塊同探索者STM32F４0７開發(fā)板連接關(guān)系表使用時(shí)，我們只需要將ＡTK—HＣ０5藍(lán)牙模塊插入到開發(fā)板的ＡTK—MODULE接口即可,如圖５．3.3－2所示：圖STＹLEREＦ３＼s５．３SＥQ圖\*ＡＲＡBＩC\s３５A(chǔ)TＫ-HC05藍(lán)牙模塊與探索者開發(fā)板對接實(shí)物圖注意,連接好之后,記得檢查Ｐ１0的跳線帽：必須短接：ＵＳART３_RX和GBC_TX以及USART３_TX和GＢC_ＲX.另外，在實(shí)際使用的時(shí)候,如果不需要進(jìn)入AT設(shè)置和狀態(tài)指示，則連接藍(lán)牙模塊只需要4根線連接即可：ＶＣC／GND／TXD／RXD。ＡTK-HC05模塊可以與多種藍(lán)牙主機(jī)設(shè)備連接,這里僅以智能手機(jī)為例，進(jìn)行說明。首先,在手機(jī)上安裝：BTClient。apk,該應(yīng)用可以在提供的資料里面找到.安裝完應(yīng)用后，我們打開該應(yīng)用,如圖5.3。3—３所示：圖STYLEREＦ３\s5.３SEQ圖\*ＡＲＡBIC\s36ＢTClｉeｎt.ａpk運(yùn)行界面進(jìn)入搜索藍(lán)牙設(shè)備界面，如圖5.3．3—4所示:圖STYLEREF3\s5．3SEQ圖＼＊ＡＲABIC＼s3７搜索藍(lán)牙設(shè)備從上圖可以看出，手機(jī)已經(jīng)搜索到藍(lán)牙模塊了,HC-０5,點(diǎn)擊這個(gè)設(shè)備，輸入默認(rèn)密鑰１234(僅第一次連接需要設(shè)置),完成配對,如圖5。３．3-5所示：圖STＹLＥＲEF3\s5．3SＥＱ圖＼*ARABIC\s38輸入配對密碼在輸入密碼之后,等待一段時(shí)間，即可連接成功，如圖5。３。3-6所示：圖SＴYLEREF３＼s5。3SEQ圖\＊ARABＩC\ｓ39連接成功此時(shí),手機(jī)和藍(lán)牙模塊就連接上了，手機(jī)便可以接收數(shù)據(jù)并繪制三維圖形了,改變開發(fā)板的姿態(tài)，手機(jī)端繪制的三維圖形如圖5．３。3—７所示:圖ＳTYLEREＦ3\s5。3SEQ圖\＊ＡRAＢIC＼s31０手機(jī)繪制三維圖形界面這樣,我們就實(shí)現(xiàn)了AＴK—ＨC0５模塊與手機(jī)的連接。同其他藍(lán)牙主機(jī)設(shè)備的連接，方法都是類似的,比較簡單，這里我們就不再介紹了。有了STA指示燈,我們就可以很方便的判斷模塊的當(dāng)前狀態(tài),方便使用。DMP算法原理通過陀螺儀數(shù)據(jù)輸出寄存器和加速度傳感器數(shù)據(jù)輸出寄存器獲取的是ＭＰU605０加速度傳感器和陀螺儀的原始數(shù)據(jù),而實(shí)驗(yàn)需要的是姿態(tài)數(shù)據(jù),即歐拉角:航向角（Ｙaw)、橫滾角（Rｏll）和俯仰角（Piｔch）。要得到歐拉角數(shù)據(jù)，就得利用我們的原始數(shù)據(jù),進(jìn)行姿態(tài)融合解算，這個(gè)比較復(fù)雜，知識點(diǎn)比較多，而MＰＵ605０自帶了數(shù)字運(yùn)動處理器，即DMP,并且，ＩｎvenSｅnse提供了一個(gè)MPU60５０的嵌入式運(yùn)動驅(qū)動庫，結(jié)合MPU6050的DMＰ，可以將我們的原始數(shù)據(jù)，直接轉(zhuǎn)換成四元數(shù)輸出，而得到四元數(shù)之后，就可以很方便的計(jì)算出歐拉角，從而得到Y(jié)aw、Rｏll和Ｐiｔｃｈ.使用MＰU6０50的DMP輸出的四元數(shù)是q３０格式的，也就是浮點(diǎn)數(shù)放大了2的30次方倍。在換算成歐拉角之前，必須先將其轉(zhuǎn)換為浮點(diǎn)數(shù)，也就是除以2的３0次方,然后再進(jìn)行計(jì)算，計(jì)算公式如下:其中ｑuat［0］～ｑuat[3］是ＭＰＵ60５0的DMＰ解算后的四元數(shù),為q30格式，所以要除以一個(gè)2的30次方，其中q30是一個(gè)常量為１０7３7４1８24，即2的3０次方，然后帶入公式，計(jì)算出歐拉角。上述計(jì)算公式的57．3是弧度轉(zhuǎn)換為角度，即１80/π，這樣得到的結(jié)果就是以度(°）為單位的。ｑ0＝qｕat[0］/q３０;//ｑ30格式轉(zhuǎn)換為浮點(diǎn)數(shù)q1=quaｔ[1]／q30;q2＝quat［2］/q３0;q３＝quａt(yī)［3］/q30；//計(jì)算得到俯仰角/橫滾角／航向角Ｐitch=ａｓｉn(－2＊q１*q３+２*ｑ0＊ｑ２）＊57。3;//俯仰角Roｌl=ａt(yī)an2(2＊q2＊ｑ3+2*q0＊ｑ1,-2*q1＊q1—2*ｑ2*q2＋1)*57。３；／/橫滾角Yaw=atan2(2＊(q1*q２+q0＊q３)，ｑ0＊q0+q1*ｑ1－q2＊q2-q３＊q3）*57。3；//航向角AHＲSAHRS簡介ＡHRS稱為航姿參考系統(tǒng)，包括多個(gè)軸向傳感器，能夠?yàn)轱w行器提供航向，橫滾和側(cè)翻信息，這類系統(tǒng)用來為飛行器提供準(zhǔn)確可靠的姿態(tài)與航行信息。圖ＳＴYLEREF3\s５.５ＳEQ圖\＊ＡRABIC\s3１航姿參考系統(tǒng)航姿參考系統(tǒng)包括基于MEMS的陀螺儀，加速度計(jì)和磁強(qiáng)計(jì)。航姿參考系統(tǒng)與慣性測量單元IMU的區(qū)別在于，航姿參考系統(tǒng)(ＡHRS）包含了嵌入式的姿態(tài)數(shù)據(jù)解算單元與航向信息，慣性測量單元(IＭU）僅僅提供傳感器數(shù)據(jù)，并不具有提供準(zhǔn)確可靠的姿態(tài)數(shù)據(jù)。目前常用的航姿參考系統(tǒng)（AHＲＳ)內(nèi)部采用的多傳感器數(shù)據(jù)融合進(jìn)行的航姿解算單元為卡爾曼濾波器。航姿參考系統(tǒng)(AHRS）具有兩個(gè)主要特點(diǎn):高精度360度全方位位置姿態(tài)輸出，但采用歐拉角的會具有萬向鎖，不能全向轉(zhuǎn)動。高效的數(shù)據(jù)融合算法快速動態(tài)響應(yīng)與長時(shí)間穩(wěn)定性（無漂移，無積累誤差）相結(jié)合.航姿參考系統(tǒng)（ＡHRS)輸出模式為三維全姿態(tài)數(shù)據(jù)(四元數(shù)/歐拉角／旋轉(zhuǎn)矩陣）。AHRS軟件設(shè)計(jì)ＡHRS的軟件設(shè)計(jì)主要分為：傳感器初始化，包括設(shè)置傳感器的更新速率、量程.初始化卡爾曼濾波的相關(guān)矩陣，根據(jù)傳感器的特點(diǎn)設(shè)置過程激勵噪聲協(xié)方差矩陣Q,設(shè)為對角元素為０.1的四維對角方陣.若成功讀取陀螺儀數(shù)據(jù)，進(jìn)行卡爾曼濾波的時(shí)間更新。采集加速度傳感器和磁阻傳感器的數(shù)據(jù)，若讀取成功則進(jìn)行觀測更新.加速度觀測更新與磁場觀測更新算法差別在于觀測方差的Ｒ,可根據(jù)兩種傳感器的置信度沒置相應(yīng)的值,航向姿態(tài)參考系的程序流程如圖5。5.２－1所示。圖ＳTYLＥREF3\s5。５SEQ圖＼＊ＡRＡBIＣ＼s32航向姿態(tài)參考系的程序流程手機(jī)端功能原理及Anｄｒｏｉd代碼對手機(jī)端aｐk的修改主要是將兩個(gè)獨(dú)立的安卓ａpｋ進(jìn)行了源碼級的合并。藍(lán)牙幀處理藍(lán)牙模塊方面，使用了開源的藍(lán)牙串口通訊代碼。在進(jìn)行合并的過程中，主要對代碼的接收模塊進(jìn)行了修改。由于藍(lán)牙的發(fā)送和接收以及Android廣播中傳遞的均為字符串，因此,為了解析方便，我們將藍(lán)牙幀設(shè)置為如下格式： u3_prｉnｔｆ（”P：％c％。1fR:％ｃ%．１fＹ：%c％.１ｆ\ｎ"，cp，Ｐitｃh，ｃr，Roll,cｙ,Ｙaｗ)；其中，cｐ、cr、cｙ分別為Piｔch、Ｒoｌl、Ｙaw的符號，為非負(fù)數(shù)時(shí)顯示為空格符.這里注意到在進(jìn)行格式化輸出的時(shí)候,控制小數(shù)點(diǎn)后保留一位小數(shù)，但是小數(shù)點(diǎn)前的位數(shù)無法控制,猜測是由于c語言頭文件功能較為精簡造成的。參數(shù)的廣播傳遞為了實(shí)現(xiàn)參數(shù)傳遞的功能,查閱了大量Ａｎdrｏid相關(guān)的代碼和資料，最終選擇了通過Androｉｄ特有的廣播機(jī)制進(jìn)行參數(shù)的傳遞.此外，在廣播的過程中,還是用了意圖（Intｅｎt），這也是Andｒoid特有的一個(gè)機(jī)制,用來對廣播進(jìn)行定向生成和接收，保證三維制圖端收到的數(shù)據(jù)為藍(lán)牙接收到并進(jìn)行廣播的數(shù)據(jù)。另外在對藍(lán)牙數(shù)據(jù)進(jìn)行接收時(shí)，發(fā)現(xiàn)無法理想地收取到單個(gè)幀數(shù)據(jù)，大部分時(shí)候是一次性獲取到了幾十幀的數(shù)據(jù)，其中第一幀和最后一幀往往是不完整的,這就要求我們對數(shù)據(jù)進(jìn)行處理之后再進(jìn)行廣播：//接收數(shù)據(jù)線程while（ｔrｕe）{num=ｉs.read(buffｅr);//讀入數(shù)據(jù)n=0;Ｓtrings0=nｅｗStｒｉｎg（ｂｕffer，0,nuｍ);ｆmｓｇ+=ｓ0；//保存收到數(shù)據(jù)for(i=0；ｉ＜nｕm；ｉ＋＋){ｉf（（bｕffeｒ[i]＝=0x０ｄ)&&(buｆfer[i+1］==0x0a)）｛ｂuｆfer_neｗ[n]=０x0ａ；i＋+；}eｌsｅ{buffeｒ_neｗ[n］=buffer[i];}n+＋;｝Stｒings=ｎewStｒing(buffer_ｎeｗ,０，n);sｍsｇ+=s;/／寫入接收緩存toSend+=s；//寫入廣播緩存Strｉngtｍp[］=ｔoSend．spliｔ(”\n”);//判斷換行符iｆ（ｔoSeｎd．lｅngｔh（）〉＝21）｛ foｒ（intlen=0;leｎ＜tｍｐ.lｅngth；ｌen++）{iｆ（tmp［lｅn]。lengｔh（）>=20）｛?iｎtｅnt。puｔExtｒa（”ＣCSｅndflａg”，tｍp［lｅn］)； intent.sｅｔAｃｔｉｏn（"CＣSeｎｄｆｌaｇ")；//設(shè)置廣播的action//只有和這個(gè)ａction一樣的接收者才能接收廣播；sｅｎdBroadcast(intenｔ)；/／發(fā)送廣播 }?}?tｏSeｎd="";／／處理完成,清空廣播緩存｝if（is。ａｖailable(）==0)ｂreak;/／短時(shí)間沒有數(shù)據(jù)才跳出進(jìn)行顯示｝//廣播接收類ｐublicclaｓsＴeｓｔＲeceiｖeｒｅxtenｄｓBrｏaｄcastＲeceiveｒ｛privaｔeｓtaticｆｉnalSｔｒｉｎgtａg=＂flａg”;pｒivateｓｔatｉcＳｔrinｇmessage＝"nｕｌｌ＂;puｂliｃvoidｏnＲeceive（Cｏntexｔconteｘｔ,Intenｔinｔent){?Stringstrinｇ＝intent.getStｒｉngExtｒａ（＂CCSendflａg＂）;／／接收指定廣播 ｉf（string!=”＂){ ｓｅtSｔring（sｔring);?｝｝pｕbｌicvoidsｅｔStriｎg(Sｔriｎgｓtr）｛ meｓsage=stｒ； ｝publｉcＳtringｇetStrinｇ（）{?ｒｅｔurnmeｓsage; }｝視圖切換在合并過程中，由于涉及到視圖的切換,因此對Ａndｒoiｄ操作系統(tǒng)的進(jìn)程管理和線程創(chuàng)建進(jìn)行了學(xué)習(xí)，最終能夠?qū)崿F(xiàn)從藍(lán)牙接收視圖到三維繪制視圖的平滑切換.切換的時(shí)機(jī)最終選擇在藍(lán)牙連接建立后，開始接收數(shù)據(jù)的瞬間：／/打開接收線程try{is=＿socｋｅt。ｇeｔＩｎｐuｔＳｔｒeam();//得到藍(lán)牙數(shù)據(jù)輸入流｝caｔch（IＯEｘceptｉoｎe）｛Toaｓt．mａkeText(ｔhis，”接收數(shù)據(jù)失??！”，Toast。LＥNGTH_ＳHＯRT).show（）；?rｅturn;}ｉf(bThrｅａd==falｓｅ）{?ReadThｒead.stａｒt()； bThread=true；Ieｎt＝neｗＩｎtent（BTＣlｉｅnt。this，ｍainActivity。clasｓ）;?iｎteｎt。ｐutExｔｒａ(”CC0"，ｓmsg）; sｔａrｔAcｔivitｙ（ｉnｔenｔ）;}else｛ bＲun=tｒｕe；}OpenＧＬ繪圖三維繪圖方面，使用了官方提供的示例代碼,主要在每一幀繪制的過程中加入了參數(shù)傳遞的功能,從而實(shí)現(xiàn)了實(shí)時(shí)從藍(lán)牙接收數(shù)據(jù)并繪制三維圖形：TestReｃeｉｖｅｒtestReceiver；prｏteｃtedvoidinｉt(ＧL10gl){gl．glClearＣolor（０.0f,0.０f，0．0f,1。０f）；gl．ｇｌＥnaｂlｅ(GＬ10．ＧＬ_ＤEPTＨ_TEＳＴ);gｌ.glEnable(GL1０.GＬ_CUＬL_FACE)；gl.glClearDｅpｔｈf（１。0f)；ｇl．gｌDepthFｕｎc（GＬ10.ＧL_LＥQＵＡL);gl．ｇlＣlearDeｐthf(１．０f）gl．gｌＳｈａdeModｅl（GＬ10。ＧＬ_ＳＭOOＴＨ);//實(shí)例化接收類testReceｉｖｅr=newTestReceiver（）；｝ｐｕblicvoiｄoｎDrａwFramｅ(ＧL1０gl)｛/／TODOAutｏ-gｅnｅrａt(yī)edmｅthｏdstuｂｇｌ．glClｅar（GL１0。GL_CＯLＯＲ_BUFFER＿ＢIＴ｜GL１0.GL_ＤEＰTH_BＵFＦＥR_BIT）； //獲取廣播的數(shù)據(jù)StrｉngalｌＳtrｉng=testReｃeiver。ｇeｔSｔring（)；?／/對廣播數(shù)據(jù)進(jìn)行解析,從字符串轉(zhuǎn)化為fｌoat類型if（aｌlStrｉnｇ。lｅngtｈ()>=２0)｛Pitcｈ=Float.parseFloａt(yī)(aｌlStrinｇ.substriｎg(aｌlStｒｉng.iｎdｅxOf(＂P：”）+2, allStｒing。inｄexOｆ("R:＂)-1)）;Rｏlｌ=Ｆｌoat．paｒseFloａｔ(allStｒｉng．ｓubstrｉng(ａllＳｔｒing。iｎｄeｘＯf（”Ｒ：＂)+2, aｌlＳtrinｇ。iｎｄexOf（＂Ｙ：")—１))；Yaｗ=Floａt(yī).paｒseＦｌoat（allＳｔring.ｓuｂｓtring(aｌｌStriｎｇ．indeｘOf（"Y:＂）+2, ａｌlString.leｎgth（)）)；}gl。gｌＭａｔrｉｘMode（GL10.GL_MOＤＥLVIEW)；ｇl．glLoadIdeｎtity(）;ＧLU.ｇluＬookAt（gl，0，０，3，０,0，0,0,1，0）;gl。ｇlVertｅxPoinｔer（3，GL10.GL_FLOAT，0，cubeBuff）;gl。ｇｌＥnablｅＣlientＳｔａt(yī)e(GL1０.ＧL_VＥRTEX_ARRAY)；gｌ。glRotａt(yī)ef（ｘroｔ，1,0,0）;gl.gｌRｏtａｔeｆ（yｒｏｔ,０，1，0)；gｌ.glRotaｔef(zｒot，０,0,1）；ｇl.gｌCoｌoｒ４ｆ（1.0f,０，０,1.0ｆ);gl．ｇlDrawＡrrayｓ（GL10。ＧL＿TRIANGＬE_ＳTRIＰ,0,４）；gl.ｇlDｒawＡｒraｙs(GL10。GＬ＿ＴRＩANGLE＿STＲIP,4,4);gl．glCoｌｏr4f(0，1.0f,0，1.0ｆ）；ｇl.ｇlＤraｗＡrｒays(GＬ１０。ＧＬ_ＴRIＡNGＬE_STRIP，8，4）；gl。glDraｗＡrｒays（GL１０。ＧL_ＴＲIAＮGLＥ＿SＴRIP，12,4）；gl．gｌColor４f（0,0，1.0f，1.0f);gl。glDrawAｒraｙs(ＧＬ10．GL＿TRＩANＧＬE＿STRIP，16，4)；ｇｌ。glDrawArrayｓ（ＧＬ1０．ＧL_ＴRIＡNGLＥ_SＴRIP，２0，4); //將讀取到的歐拉角賦值給三維圖形的旋轉(zhuǎn)角xrｏt=Ｐiｔcｈ；yｒot＝-Roｌｌ;zrot=Yaw；}系統(tǒng)測試由于三維電子旋轉(zhuǎn)臺和加速度測量儀價(jià)格過于昂貴，因此,只是利用Maｔｌab軟件進(jìn)行了簡單的系統(tǒng)測試。AHＲS測試我組對從串口獲取的ＭPU6050數(shù)據(jù)進(jìn)行了處理,獲得到浮點(diǎn)型數(shù)據(jù)，并對使用AＨRS和不使用AＨRS進(jìn)行了測試比較，由于數(shù)據(jù)量較大，這里我們用圖像展示,結(jié)果如下：圖STＹLＥRＥF3＼s６。１SＥＱ圖\*AＲABIC\s31未進(jìn)行ＡHRS處理得到的姿態(tài)變化圖ＳＴYLＥRＥＦ3\ｓ6。1SEＱ圖\*AＲＡＢＩC＼s32AHRS處理后得到的姿態(tài)變化由圖可看出，不使用AHRＳ的數(shù)據(jù)繪出的圖像與實(shí)際姿態(tài)完全不對應(yīng)，誤差較大。而使用了AHＲS處理的數(shù)據(jù)較穩(wěn)定，且比較符合實(shí)際姿態(tài)。高通濾波算法根據(jù)我組需要實(shí)現(xiàn)的還原MＰU6０50六軸傳感器姿態(tài)變化的功能,我們需要對獲取的MPＵ6０5０加速度傳感器數(shù)據(jù)進(jìn)行處理，通過二次積分得到MPU60５０的姿態(tài)的位移。由于加速度傳感器的輸出存在固定的零點(diǎn)漂移，即當(dāng)實(shí)際加速度為０時(shí)，傳感器輸出并不一定為0，而是一個(gè)非零輸出。若設(shè)傳感器的輸出值為+，其中為實(shí)際加速度,則在對傳感器輸出值的誤差進(jìn)行二次積分后會產(chǎn)生積分累計(jì)效應(yīng),導(dǎo)致誤差更大。同時(shí)，若傳感器初始化時(shí)加速度不為０，也會導(dǎo)致二次積分后產(chǎn)生積分累計(jì)效應(yīng)。通過查閱資料,高通濾波可以解決零點(diǎn)漂移問題，我組選擇使用高通濾波處理數(shù)據(jù),而Mａt(yī)ｌab提供了高通濾波函數(shù)filtfilt（），我組調(diào)用函數(shù)處理的主要代碼如下：%％Ｈigｈ—pasｓfｉlteｒlinｅaｒvelocｉtｙtoremｏvｅdriftordｅｒ=1;filtＣutOff=0.1；［ｂ，a]=ｂuｔter(ｏrder，(２＊ｆiｌtCｕtＯff)／（1/sａmｐlePeriod),＇ｈigh＇）;linVelHP=ｆｉｌtfilt（ｂ，a,lｉｎVel);%ｌiｎＶeｌＨP=linVel；％Plｏtfｉgure（＇Nｕｍbeｒ＇,＇ofｆ’,＇Ｎａme＇,＇High—passfiｌteｒedLｉnearVeｌocｉty');ｈolｄoｎ；plot（liｎVelHＰ（：,１），’r'）；plｏt（liｎVelHＰ（：,2)，'g’);ploｔ(linVｅlHP（：，３)，’b’)；ｘlabel('ｓａmplｅ＇）；ylaｂel（’g'）;tiｔle（＇Ｈigｈ-passfiｌｔeｒedliｎearvｅｌoｃiｔy'）；lｅgenｄ（'X’,’Y'，＇Z＇）;其中samｐｌePeriod為取樣周期，取為1/２5６；bｕffer(）為分段函數(shù)，將數(shù)據(jù)分段進(jìn)行濾波;plot（）為繪圖函數(shù)。我們對使用高通濾波和不使用高通濾波進(jìn)行了測試比較，由于數(shù)據(jù)量較大，我們以圖像的形式展現(xiàn)，結(jié)果如下圖：圖STYLＥREF3\s6。2ＳEQ圖\＊ARＡＢＩＣ\s31未進(jìn)行高通濾波處理得到的速度變化圖ＳTＹLEREF3＼ｓ6.2ＳEＱ圖＼*ＡＲＡBＩC\s32高通濾波處理后得到的速度變化如圖6。2－2可看出，不使用高通濾波處理得到的數(shù)據(jù)由于零點(diǎn)漂移導(dǎo)致的二次積分誤差，趨于正向最大或負(fù)向最小;而使用高通濾波處理得到的數(shù)據(jù)趨于穩(wěn)定，且動作被放大，變得更明顯。實(shí)時(shí)姿態(tài)檢測系統(tǒng)測試當(dāng)時(shí)演示時(shí),提出要將手機(jī)固定在板上進(jìn)行測試，結(jié)果是偏差非常大,這與實(shí)際情況有很大差別。后來找到了原因，當(dāng)時(shí)代碼的編寫是將數(shù)據(jù)作為正反饋傳遞給繪圖程序的,而現(xiàn)在為了檢驗(yàn)精度，需要改為負(fù)反饋，同時(shí)，為了便于觀察，將3D圖形替換為了一條直線，代碼改變?nèi)缦?原始代碼已被注釋)：//gｌ。ｇｌDrawArraｙs(ＧL10.ＧＬ＿TRＩＡNGLE_STＲIＰ，0，４）; ｇｌ。gｌＤrawArｒays（GL1０。ＧL_LＩNE_SＴＲＩＰ，２，2)；xrot=-Ｐiｔch；?ｙroｔ=Roll；?zrot=-Ｙaw； /＊xrot=Pitch;?yrot=—Roｌl; zrｏt＝Y(jié)aｗ;*/如圖，為測試結(jié)果，可以看到誤差逐漸變大，最后無法復(fù)位:關(guān)鍵代碼說明IＩC關(guān)鍵代碼初始化ＩＩC／／初始化ＩICｖoｉdIIC_Iｎiｔ(vｏｉd){GPIO_InｉｔTypeＤefGPＩO＿IniｔSｔruｃturｅ;RCC_AHB1PeriphClockCmd（RCＣ_AHＢ1Ｐｅriph_GＰIOB,ENＡBＬE）;//使能GPIOＢ時(shí)鐘//GＰIOB8,B９初始化設(shè)置GPＩO_IniｔStructuｒｅ。GPＩO_Piｎ=GＰIO＿Pｉｎ_8｜ＧPＩO_Piｎ_９；GPIＯ＿ＩnitSｔructｕrｅ。GPIＯ_Ｍode=ＧPIO＿M(jìn)odｅ_OＵT；/／普通輸出模式ＧＰＩO_InitＳtructure。GPIO_OTｙpe=GPIO_OType_PＰ；//推挽輸出ＧＰＩＯ_InｉtStｒucture。ＧＰIO_Speeｄ=GPIO_Ｓｐｅed＿100ＭＨz;／/100MＨzＧPIO_InitＳｔruｃture。GPIO_ＰuPd＝GPIＯ_PuPd_UP;／/上拉ＧPＩO_Ｉｎiｔ(GPＩOB,＆GPIO_IｎitSｔrｕcture);//初始化IIC_SCL=1;IIC_SDA=1；｝通過IＩC_Iniｔ函數(shù)可看出，函數(shù)主要對GPIO的ＰB8、PB9引腳進(jìn)行了初始化設(shè)置。先使能ＧPIＯＢ時(shí)鐘,然后對PB8、PB9引腳的相關(guān)寄存器進(jìn)行初始化設(shè)置，設(shè)置完畢后通過GＰIO＿Init函數(shù)將初始化設(shè)置有效，最后將SCL和SＤA使能。數(shù)據(jù)處理/／ＩO方向設(shè)置#defineSDA_IN（）｛GPIＯB-＞MOＤER&=～（3<<(9＊２)）；GPＩＯB-＞MODＥＲ｜=０＜〈９＊２; //PB9輸入模式}#ｄｅfiｎeSDA＿OＵT(）｛GPIOB—＞MOＤEＲ&=～(3〈<（9＊２)）；GPＩOB－〉MODER｜=1＜〈9＊２；//PB9輸出模式}上述代碼定義了SDA_IＮ和ＳDA_OＵT兩個(gè)函數(shù)，通過設(shè)置ＰB9引腳功能來設(shè)置IIC_ＳDＡ接口是輸入模式還是輸出模式。有了這兩個(gè)函數(shù)之后,在IIC函數(shù)文件中，就能夠使用ＩＩC_Senｄ＿Bｙtｅ和IIC＿Ｒead＿Ｂyte函數(shù)來發(fā)送或讀取數(shù)據(jù)。具體代碼如下：//ＩIＣ發(fā)送一個(gè)字節(jié)//返回從機(jī)有無應(yīng)答//1，有應(yīng)答//0，無應(yīng)答vｏｉdIIＣ_Ｓend_Byte（ｕ８tｘｄ）{u8t;SDＡ＿OUT（);IIC＿SＣL=０;/／拉低時(shí)鐘開始數(shù)據(jù)傳輸for(t=0；t<8；t＋+)｛ＩIC＿SDA＝（txd&0ｘ8０）〉〉７;ｔxd<<=１；delay_us（２);／/對ＴEA5767這三個(gè)延時(shí)都是必須的IIC_ＳCL=1；delａｙ_us（２)；IIC_SCＬ=0;dｅlａy_ｕs（2)；｝｝//讀１個(gè)字節(jié)，ack=1時(shí),發(fā)送AＣK，ack=0,發(fā)送nAＣKu８IＩＣ_Ｒｅad_Ｂｙｔｅ(unsignedcｈaｒack）｛unsigｎｅdｃhaｒi，receiｖｅ=0;?ＳDA_ＩN(）；／/SDA設(shè)置為輸入for(ｉ=0;ｉ<8；i＋＋）｛ＩIＣ_SＣL=0;delaｙ＿us(2);?ＩＩC_SCL=１;ｒeｃｅｉve<＜=1;if（ＲEAD_ＳDA)ｒeceive++;delay_uｓ（1);}ｉｆ(!acｋ)IＩC_ＮＡcｋ（);／/發(fā)送nＡCKelsｅIIC_Acｋ（);//發(fā)送AＣKretuｒnreceｉｖe；}MPＵ6050關(guān)鍵代碼初始化根據(jù)5.2。3中說明的,對MPU60５０初始化，需要先初始化IＩC接口，然后復(fù)位ＭPU60５0，接著設(shè)置相關(guān)參數(shù)，最后配置系統(tǒng)時(shí)鐘源并使能加速度傳感器和陀螺儀。具體代碼如下：//初始化MPU６０５0//返回值:0，成功／/其他，錯誤代碼u8ＭＰU＿Init(void）｛ u８res；?ＩIC_Iｎit（)；//初始化ＩIＣ總線 MPＵ_Writｅ＿Byte(MＰU_ＰＷR＿M(jìn)GMＴ１＿REG,０X８0)；?／/復(fù)位ＭPＵ6050ｄelay_ｍs（１0０）； ＭPＵ＿Ｗｒite＿Ｂytｅ（ＭＰU_PＷR_MGMT1_REG,０X0０）； //喚醒ＭPＵ6050?MＰＵ_Set＿Ｇｙro＿Ｆsr（0）；//陀螺儀傳感器,３±200０dｐs?MＰU_Sｅt_Accｅl_Fｓr（0）;/／加速度傳感器，±2g MＰＵ_Set_Rate(5０）；／/設(shè)置采樣率５０Ｈｚ ＭPU＿Wriｔｅ＿Byte（MPU_ＩＮT＿EＮ_REＧ，０X00）;//關(guān)閉所有中斷?MPU＿Ｗrite_Ｂyte(MPU_USER_CTRＬ_REG,0X00）；／/I2C主模式關(guān)閉 MPU_Wrｉtｅ_Ｂyｔe(ＭPU＿FIＦO_EN_RＥG，0X00);//關(guān)閉FIFＯ MPU_Wriｔｅ_Ｂyte(MPU_INTＢＰ_CFＧ_REG,0X８0）；/／IＮT引腳低電平有效 res＝ＭPＵ_Rｅａd＿Bｙte(MPU_ＤEVＩＣＥ_ＩD_REG)； ｉf（res＝＝MPU＿ＡDDR）//器件ID正確?｛ MPU＿Ｗriｔe_Byｔe(MPＵ_PWR_MGＭＴ１＿ＲEＧ，0Ｘ01）; //設(shè)置ＣLKSEL，PLLX軸為參考 MPU_Write_Bｙte（MＰU_PWＲ＿M(jìn)GMT2_REG,０X00);?//加速度與陀螺儀都工作 MPU_Ｓet_Rate（5０）；//設(shè)置采樣率為5０Hｚ ｝elｓｅretuｒn1;?retｕrn0;｝設(shè)置寄存器在MPU６050初始化中，需要設(shè)置角速度傳感器(陀螺儀）和加速度傳感器的滿量程范圍以及采樣率,而根據(jù)5。2.4需要通過修改寄存器來設(shè)置，所以用到MＰＵ_Ｗrite_Byte函數(shù)來對寄存器進(jìn)行寫操作。具體代碼如下://設(shè)置ＭPU６０５０陀螺儀傳感器滿量程范圍//ｆsｒ:0,±２50ｄｐｓ；1，±500ｄps;2，±1000ｄps;3,±2０00ｄps//返回值：0，設(shè)置成功//其他，設(shè)置失敗u8MPU_Set_Gyro_Fsr（u８fsr）{ returnMPU_Writｅ_Bytｅ(MＰＵ_GYRO_CＦＧ_REG，fsｒ〈〈３）；//設(shè)置陀螺儀滿量程范圍}/／設(shè)置MPU6050加速度傳感器滿量程范圍//fsｒ:0,±2g；1，±4g；2,±８g;3，±１6g//返回值:0，設(shè)置成功//其他，設(shè)置失敗u8MＰU_Ｓeｔ_Accel_Ｆsr(u８fｓｒ){?ｒeturｎMＰU_Ｗriｔe_Byte(MPU_ACCＥL_CFＧ_ＲEG，fsr＜<3）；／／設(shè)置加速度傳感器滿量程范圍｝／/設(shè)置MPU6050的采樣率（假定Fs=１KＨz）//rate:4～1000（Hz）／/返回值：0，設(shè)置成功//其他，設(shè)置失敗u8MＰＵ＿Set_Raｔe(u1６rａt(yī)e）｛?u８datａ； iｆ（ratｅ>1０0０）ｒate=1000; if（raｔe〈4)raｔe=4；?data=１0００/ratｅ-1;?datａ=MPU_Ｗrite_Bｙte（ＭPU_SAMPLE_RATＥ＿REG,dａt(yī)a);?//設(shè)置數(shù)字低通濾波器 returｎMPＵ_Seｔ_LPF(rate／2)；/／自動設(shè)置ＬPF為采樣率的一半}從數(shù)據(jù)輸出寄存器讀取數(shù)據(jù)在MPＵ6050初始化完成后，需要通過加速度傳感器數(shù)據(jù)輸出寄存器和陀螺儀數(shù)據(jù)輸出寄存器來讀取加速度傳感器數(shù)據(jù)和陀螺儀數(shù)據(jù),所以用到MPU_Reａd_Len函數(shù)來對寄存器進(jìn)行讀操作。具體代碼如下：／/得到陀螺儀值（原始值)//gx，gｙ,gz：陀螺儀x，ｙ，z軸的原始讀數(shù)（帶符號）//返回值:0,成功/／其他，錯誤代碼u8MPＵ＿Get_Gyroｓｃｏpe（short*gｘ,shorｔ*gy,sｈorｔ＊gz)｛u8buf［６],rｅs; res=MPU＿Rｅaｄ_Lｅｎ（MPＵ＿ADＤＲ，ＭPU_ＧYRO＿XOＵTＨ_REG,６，buf)；?if(reｓ=＝０） {?*gx=（（ｕ１6）buf［０]〈<8)|ｂｕf［１]；?*ｇy=（（ｕ16)buf［2]〈＜8）｜buf［3]; ＊ｇz=(（u１6）ｂuｆ［4］＜<８）｜buf［5]; ｝ｒeｔｕｒnreｓ；;｝//得到加速度值（原始值)//ｇｘ，ｇy，gz：陀螺儀x,y，z軸的原始讀數(shù)（帶符號）／/返回值：0,成功//其他,錯誤代碼u8MPＵ_Get＿Acｃｅｌerometer（shｏrt*aｘ，ｓhort*ａy，shoｒt＊az）｛ｕ8buf[6］，ｒes； ｒes＝MPU_Rｅad_Lｅn(MPU_AＤＤR，ＭPＵ_AＣCEL＿XOＵTＨ＿ＲEG，6,buf)；?if（reｓ=＝0) {?＊aｘ＝（（u16）buf［0]〈＜８）|bｕf［１］； *ａy=(（u16）ｂｕf［２］<〈8）｜ｂuｆ［3]; ＊az=(（u16）buf[４］<〈8）｜bｕf[5]；?}returnrｅｓ；;}DMＰ關(guān)鍵代碼初始化mｐu＿ｄmｐ＿inｉt（)是MＰＵ6０５0DMP初始化函數(shù),該函數(shù)具體代碼如下：//mｐｕ6050,ｄmp初始化//返回值：0,正常//其他，失敗u8mpu＿dmp_init（voiｄ)｛u8rｅs=0；IIC_Inｉt（）；//初始化IIC總線if(mpu_ｉniｔ（）=＝0）｛/／初始化MPU605０//設(shè)置需要的傳感器reｓ=mｐｕ＿set＿senｓｏrs（IＮV＿XＹＺ_GＹＲO|ＩNＶ_XYＺ_ACＣＥL）；ｉf（ｒeｓ)return1；res=mpu＿cｏnfiｇurｅ＿fｉfo（INV_ＸYZ_GＹRO｜INV_ＸYZ＿ACCEＬ)；//設(shè)置ＦIFＯｉf（res）reｔurｎ2;rｅs=mpu_ｓet_sample_rate(DＥFAULＴ＿M(jìn)ＰU＿HZ）;／/設(shè)置采樣率ｉf（ｒｅｓ)reｔｕｒn３;reｓ=dmp＿loaｄ_mｏｔｉoｎ_driver_firｍwａre（)；／/加載dmｐ固件if(rｅs)retｕrｎ4；//設(shè)置陀螺儀方向res＝dmp_sｅｔ_oriｅntａｔｉon（inv_oriｅntation＿ｍａｔｒix_to＿scaｌaｒ(gyｒo＿orientaｔｉon)）；iｆ（ｒes）ｒｅturn5;//設(shè)置dmｐ功能res＝dmｐ＿eｎablｅ_ｆeatｕｒｅ(ＤＭP_ＦＥＡTURE_６Ｘ_LP_QUＡT|DMP_FＥＡTＵRE_ＴAP｜DMP_FEATUＲＥ_ＡNDＲOＩD_ORIＥNＴ｜DMＰ_FEATURE＿ＳENＤ_RＡＷ＿ACCEＬ|DMP_FEATURE_SEND_ＣＡL_GYRＯ|DMP_ＦEＡTＵRＥ_ＧYRＯ＿CAL）;ｉf（res）return6；/／設(shè)置DMP輸出速率（最大200Ｈz)rｅs=ｄmｐ_set_fifo_ratｅ(DＥＦAULＴ_MＰU_HZ)；iｆ（rｅs）return７;ｒeｓ=run_self_test（）;／/自檢if（rｅs）retｕrｎ8;res=ｍpu＿seｔ＿dｍp_state（１）；//使能DMPｉf（ｒes）return9；}rｅturｎ０；｝函數(shù)首先通過ＩIC_Inｉt（需外部提供）初始化與MPU6０５0連接的IＩＣ接口,然后調(diào)用mpｕ_ｉnｉｔ函數(shù)，初始化MＰU60５0,之后就是設(shè)置DMP所用傳感器、FIFO、采樣率和加載固件等一些列操作，在所有操作都正常之后，最后通過mｐu＿sｅt_dｍp_state(１）使能DMP功能,在使能成功以后才可以通過mｐｕ_dmp_ｇet_datａ來讀取姿態(tài)解算后的數(shù)據(jù)。讀取數(shù)據(jù)mpｕ_dmp_get_data(）函數(shù)用于得到ＤMＰ姿態(tài)解算后的俯仰角、橫滾角和航向角。具體代碼如下：//得到dｍp處理后的數(shù)據(jù)(注意,本函數(shù)需要比較多堆棧，局部變量有點(diǎn)多）/／pｉtcｈ:俯仰角精度：０。1°范圍:—９0.0°<－——>＋90．0°//rｏlｌ：橫滾角精度:０。１°范圍:－18０．0°<—-－〉+180.0°//yaｗ：航向角精度:0。１°范圍:-18０。0°＜—--〉＋18０.0°//返回值：0，正常/／其他，失敗u8mpｕ_dｍp＿ｇeｔ_ｄata(ｆｌoat＊pitch，fｌoａｔ＊roｌｌ,floａt(yī)＊yaw）{fｌoatq0=1.0f,q1＝0。0f，q2=0.0f,q３=0。０ｆ；unsiｇnedlongｓensｏr_ｔiｍｅstamp；ｓｈortgyro[3]，accｅl[3］,ｓｅnsoｒs；ｕnｓiｇneｄcharmorｅ；longquaｔ［４］;if(dmp_read＿fｉｆｏ（gyｒo，accel，qｕat,＆sｅnsｏr＿timｅsｔamp,＆sensorｓ，＆more)）retｕrn１;ｉf（sensoｒｓ&INＶ_ＷXYZ_QUAT）｛q０＝qｕat[0］／ｑ30；／／ｑ30格式轉(zhuǎn)換為浮點(diǎn)數(shù)q1=ｑuａt(yī)［１］/ｑ３0；ｑ２=quaｔ［2］/q30；q3=quat[３]/ｑ３0;//計(jì)算得到俯仰角/橫滾角/航向角*pitcｈ=ａsｉn(—２＊q1＊ｑ3+２＊q0＊q2)*57．3;＊roll＝ａt(yī)an２（2＊ｑ２*q３+2＊q0*q1,－2＊q1*ｑ1－２＊ｑ２＊ｑ2+1）*57。3;*yaw＝atan2（2*（q1＊q２+q０*q3）,q0*q0+q1＊q１—q2＊q2－ｑ3＊q3）＊57．３;}ｅlsｅｒetｕrn2;ｒeturn０；}AＨRS關(guān)鍵代碼fuｎctioｎobｊ=Updａt(yī)e(ｏbj，Gｙroscoｐe，Ａｃcｅleromeｔer,Magnetomeｔer）q=obj。Ｑuatｅｒnion;％shｏrｔnameｌoｃａｌvaｒiablｅfoｒｒeaｄabｉlitｙ%Noｒｍａliseaccelerometeｒmeａsurｅｍentif（norm（Aｃｃeｌｅrｏｍeteｒ）==0)，rｅtｕｒn;ｅnd％handleNaNAccｅlerｏmeter=Aｃcelerｏmeter／nｏrm(Ａcｃeleｒometer)；%normaｌisemagnituｄe%Ｎｏrmalisｅmａgnetoｍetermeasurｅmｅnｔiｆ(norｍ(Magneｔometer)==０），rｅｔuｒn;end％handleNaNＭagnetoｍｅｔer=Maｇｎetｏmｅtｅr／ｎｏrm(Mａgｎｅtomｅter）;％norｍalisemagｎitｕde％ＲeferｅncｅｄirectioｎｏｆＥａrtｈ'smａgneticｆeildh=qｕａt(yī)eｒnＰrod(q,quａt(yī)ernＰrod([0Maｇnｅtｏmｅteｒ］，quateｒnConj（ｑ)）)；b=[0norｍ（［h（2）h（３)]）0h（４）］;％Estimateｄｄirecｔionofgrａｖiｔｙandｍagneticfieldv=[2＊(q（2）＊ｑ（４)-q（１)*q（3））2＊(q(1）*ｑ(2）＋q(3）*q（4）)ｑ（1)＾2-q(2）^2-ｑ（３）^２+q（4)^2];w=[２*b(2）＊(0。5-q(3）^2—ｑ(4）^2）＋2＊b(4）＊（ｑ（2)*q（4）—q(１)＊q(3）)２*ｂ(2）*（q（2）*ｑ(3）-q(１）*q(4））＋2＊b(4）＊(ｑ(1）＊q(2）+q（３）＊q（4））2*b（2)*（q(1）*q（3)+q(2）＊q（4））＋2＊b（４)*（0。5—ｑ（２)＾2—q（３）＾2）]；%Errｏriｓｓuｍofｃrossproductbeｔｗｅenesｔiｍaｔeddirectｉon?％aｎdmeasureddireｃtｉoｎoffiｅlｄse＝ｃross(Aｃcelerometeｒ,v）+ｃroｓs（Mａgneｔomeｔｅr，w）；iｆ（oｂj。Ki〉0)oｂｊ。eInt＝oｂj。eInt+e*oｂj.SamｐlePeｒiod；elseobj.eInt=［0０0]；eｎd％AppｌyfeeｄbａｃktermsGyroscｏpｅ＝Ｇyroｓcope+oｂj.Ｋｐ*ｅ＋oｂj。Kｉ*obj。eＩnt;％CｏmputｅrateｏfｃｈangｅｏfｑｕaternioｎｑDｏｔ=０。5*quatｅrnＰrod（q，［０Gyrｏsｃopｅ（1)Gyroｓcｏpe(2）Gｙroscｏpe(３)］);％Integraｔetoｙieldquａｔerniｏnq=q+qＤot＊ｏbj.SamｐlｅPeriｏd；obj。Ｑｕaternｉｏn＝q/ｎorｍ(q);%ｎoｒmalｉｓequaternｉonendfuｎctiｏｎobｊ=ＵpｄａｔeIMU（obj,Gyrｏscope，Acceｌｅroｍeｔer）q=obj．Ｑuａt(yī)ｅrniｏn；％shortnａmelｏcaｌvariaｂleｆｏｒｒｅadabiｌｉty％Ｎｏｒｍaｌiseaccｅleｒｏmeｔerｍeａｓｕrementif(norｍ（Acｃelｅroｍeter)==0）,ｒｅturn;end%ｈandleNaNAccelerｏｍｅter=Accelerometer/norm(Ａcceｌeroｍeteｒ)；%normalｉｓemａgnituｄｅ％Esｔimateｄｄｉｒecｔｉonofｇｒavitｙaｎdmaｇneticfluxv＝[2*(q（２）*q(４）－ｑ（1)＊q（3）)2＊（q（１）*q（2）＋q（3）*q(4)）q(1）^2-ｑ(２)＾2—q（3)^2+q（4）^2]；%Erｒｏrisｓumofcｒoｓductbetｗeｅnesｔiｍaｔeｄｄirectiｏn ％ａｎdｍeasuredｄirectionｏｆfｉelｄe=cｒｏss(Acｃeｌｅromｅｔer,v);if（ｏbj.Ki〉0）ｏbj．eIｎt＝obｊ.eIｎt+e＊oｂj.SampｌｅPeriod;ｅlseoｂｊ.eInt＝[000］；end％AppｌyfeedbacｋtermｓＧyroscopｅ=Ｇyroscope+oｂj。Kp＊e+ｏbｊ．Kｉ＊oｂj.eIｎt；％Ｃomputeraｔeofｃhangｅofｑuａt(yī)ernｉonqDoｔ=０.5＊quａt(yī)eｒｎPrｏd（q，[０Gyｒoｓcopｅ(1）Ｇyrｏｓcope（2)Gyrｏscope（3)］）；%Integｒaｔeｔoyieｌdquａt(yī)ernionq=ｑ+qDot＊obｊ。ＳamｐｌePeriｏｄ;ｏbj。Qｕａｔernion=q/noｒm(ｑ)；％normaliｓequaｔerniｏｎend卡爾曼濾波四維擴(kuò)展卡爾曼濾波算法擴(kuò)展卡爾曼濾波算法(EｘtendｅｄＫａｌmaｎFilteｒ,EＫF）是一套由計(jì)算機(jī)實(shí)現(xiàn)的實(shí)時(shí)遞推算法，所處理的對象是隨機(jī)信號,利用系統(tǒng)噪聲和觀測噪聲的統(tǒng)計(jì)特性，以系統(tǒng)的觀測量作為濾波器的輸入，以所要求的估計(jì)值(系統(tǒng)的狀態(tài)變量)作為濾波器的輸出,濾波器的輸入和輸出由時(shí)間更新和觀測更新算法聯(lián)系在一起,根據(jù)系統(tǒng)的狀態(tài)方程和觀測方程估算出所需要處理的信號。AＨＲS擴(kuò)展卡爾曼濾波算法的狀態(tài)變量采用四維四元數(shù),與采用歐拉角相比,避免了采用歐拉角計(jì)算時(shí)涉及的大量三角函數(shù)運(yùn)算,保證了更新速率和實(shí)時(shí)性，同時(shí)不存在采用歐拉角運(yùn)算出現(xiàn)的奇異性.歐拉角與四元數(shù)的轉(zhuǎn)換關(guān)系如下:θ=-asin2qφ=atan2Ψ=atan22四元數(shù)微分方程如式（4）所示:q1四元數(shù)姿態(tài)矩陣微分方程只要解4個(gè)微分方程，比方向余弦姿態(tài)矩陣微分方程減少了大量的運(yùn)算，便于微處理器的編程實(shí)現(xiàn)。時(shí)間更新系統(tǒng)的狀態(tài)方程如式（５）所示。Xk=AXk-1其中狀態(tài)變量為四元數(shù)X=q0，q1，qA=1-狀態(tài)變量的時(shí)間更新如式(7）所示。Xk=AXk-1協(xié)方差矩陣Pk預(yù)測如式(８)所示，式中Pk=APk-1觀測更新AＨRS的觀測更新是通過本體坐標(biāo)系上的重力加速度和地磁場的參考矢量旋轉(zhuǎn)至導(dǎo)航坐標(biāo)系上，再與加速度和磁場傳感器比較,得到觀測變量的殘余。由本體系轉(zhuǎn)換至導(dǎo)航系的轉(zhuǎn)移矩陣由四元數(shù)可以表示為式(9）。Cnb三位參考向量v轉(zhuǎn)移至導(dǎo)航系中可有觀測方程式（10）表示h=Cnb當(dāng)重力加速度觀測更新時(shí)參考向量v等于重力加速度參考矢量（可設(shè)置為當(dāng)平臺靜止水平放置時(shí),加速度計(jì)測量得到的三維矢量為:v=當(dāng)磁場觀測更新時(shí)v等于磁場參考矢量（可設(shè)置為當(dāng)平臺靜止水平放置且航向指向正北時(shí)，磁阻計(jì)測量得到的三維矢量為：v=Ｈ是h對Ｘ求偏導(dǎo)的雅可比矩陣，如式(11）所示。Hi,j=?卡爾曼增益矩陣KkKk=Pk觀測更新:Xk=Xk當(dāng)重力加速度觀測更新時(shí)zkz當(dāng)磁場觀測更新時(shí)zkz協(xié)方差更新:Pk=(1-KkＡＴK-HＣ０5關(guān)鍵代碼初始化ATK-HC０5模塊//初始化ATK－HC0５模塊//返回值:0，成功；１，失敗。u8ＨＣ05＿Ｉniｔ(voｉd）{?ｕ8reｔry=10，ｔ; u8temp=1； ＧＰＩＯ＿ＩｎitTypeDｅｆＧＰIO_InitSｔrucｔure；?RＣＣ＿AＨB1PｅｒｉphClockCmd（RCＣ_AHB1Ｐｅｒiph＿GPＩOF｜RＣC_AHB1Ｐeriph＿ＧPIOC，EＮABLE)；／/使能GPIOC，GPIOF時(shí)鐘?GPIO_IｎｉtＳtructure.GＰIO_Pin=GPIO