基于MPU6050的INS慣性導航和實時姿態(tài)檢測系統(tǒng)方案

1、基于MPU6O50勺INS慣性導航和實時姿態(tài)檢測系統(tǒng)1.項目目標及功能說明1.1項目目標學習使用正點原子探索者開發(fā)板，并熟悉開發(fā)板上的MPU605六軸傳感器的 工作原理和各函數(shù)的調用過程。同時學習開發(fā)板的擴展接口，嘗試在開發(fā)板上擴 展藍牙模塊，并實現(xiàn)開發(fā)板與手機等含有藍牙模塊的電子設備通過藍牙連接并進 行數(shù)據(jù)的傳輸。在完成上述內容的基礎上，實現(xiàn)將MPU605六軸傳感器的加速度 計和陀螺儀的數(shù)據(jù)傳送到手機上，在手機上實現(xiàn)陀螺儀的變化效果展示。同時通 過串口將MPU605數(shù)據(jù)傳送到電腦上，通過 Matlab編程處理數(shù)據(jù)，實現(xiàn)慣性導 航的簡單展示。1.2系統(tǒng)功能說明系統(tǒng)最主要的功能有兩個：一個是在手

2、機端能夠展示開發(fā)板上 MPU6050它螺 儀的姿態(tài)變化，通過一個立方體的轉動來表示陀螺儀的轉動； 另一個是在電腦端 能夠讀取MPU6050勺數(shù)據(jù)，并通過對數(shù)據(jù)的處理還原數(shù)據(jù)中存儲的 MPU6050勺姿 態(tài)變化，簡單展現(xiàn)出慣性導航的效果。在實現(xiàn)系統(tǒng)最主要的兩個功能過程中，還需要實現(xiàn)一些基礎功能。開發(fā)板能 夠通過藍牙與手機連接并傳輸數(shù)據(jù)； 開發(fā)板能夠通過串口將數(shù)據(jù)發(fā)送出去； 在電 腦端能夠讀取開發(fā)板上串口輸出的數(shù)據(jù)等。專業(yè)技術資料2.需求分析慣性導航系統(tǒng)用于各種運動機具中，包括飛機、潛艇、航天飛機等運輸工 具及導彈，然而成本及復雜性限制了其可以應用的場合。但是，存在一種情形：衛(wèi)星一旦突然因故障、敵

3、方打擊或干擾（如太陽風暴）等原因無法提供服務， 這對依賴GPS北斗等衛(wèi)星導航系統(tǒng)作為唯一 PNTPosition Navigation Time） 信息來源的系統(tǒng)來說可能是致命的災難。作為目前為止衛(wèi)星導航系統(tǒng)最好的備援慣性導航系統(tǒng) （INS），將于屆時 發(fā)揮出巨大的作用，其精度完全可以媲美GPS等衛(wèi)星導航系統(tǒng)。并且它不需要外 部參考就可確定當前位置、方向及速度，從而使它自然地不受外界的干擾和欺騙。定位、導航和授時服務對軍隊而言就像氧氣對人類一樣不可或缺， 因此通過 研究新機理、研制新設備、開發(fā)新算法，以擺脫人員和系統(tǒng)設備對 GPS的依賴, 具有極大的戰(zhàn)略意義。姿態(tài)監(jiān)測系統(tǒng)可廣泛應用于關鍵資產(chǎn)姿

4、態(tài)變化的無線實時監(jiān)控。由于目前 移動智能終端設備的數(shù)量和質量逐步提升， 因此，通過計算機上傳統(tǒng)的上位機軟 件進行姿態(tài)監(jiān)測，逐漸暴露出了自身的缺點一一串口傳輸無法實現(xiàn)無線監(jiān)測、計算機相比智能終端便攜性極差。因此，使用無線傳輸（藍牙、紅外、 WIFI、GSM等）的技術，開發(fā)一款在移 動智能終端可以實時顯示物體姿態(tài)的應用， 具有很高的實用價值和廣泛的市場應 用前景。3.開發(fā)環(huán)境移動終端操作系統(tǒng)：An droid 444 KitKat計算機操作系統(tǒng)：Win dows 8.1 Pro x64串口開發(fā)：開發(fā)板IDE:MATLAB R2014aKeil uVisio n5An droid IDE :Ecli

5、pse Java EE IDE for Web DevelopersAn droid Developme nt Toolkit 23.0. 4.14685184.項目進展情況到目前為止，我組已實現(xiàn)了以下功能:1. STM32F開發(fā)板上MPU605六軸傳感器的數(shù)據(jù)獲取并顯示在 LCD屏幕上。2. 在LCD屏幕上繪出圓形圖案，且圓形圖案能根據(jù) MPU605六軸傳感器的 姿態(tài)變化而運動，傳感器傾斜角度越大，圖案運動速度越快。3. 擴展藍牙模塊，能通過藍牙模塊與手機連接并進行數(shù)據(jù)通信。4. 根據(jù)函數(shù)提供的幀格式定義數(shù)據(jù)幀， 并通過USAR接口將數(shù)據(jù)幀傳給PC 端。5. 在手機端能根據(jù)藍牙獲取的 MPU

6、605六軸傳感器的陀螺儀數(shù)據(jù)繪出立方體，立方體能在可接受的時間延遲內實時展現(xiàn)MPU605啲姿態(tài)變化（轉動方向和角度）。6. 在PC端能通過對從USART接口獲取的數(shù)據(jù)幀進行解析獲取 MPU605加速 度傳感器和陀螺儀的數(shù)據(jù)，并根據(jù)數(shù)據(jù)幀中設置的校驗位進行數(shù)據(jù)校驗。7. 在PC端能根據(jù)解析出的加速度傳感器和陀螺儀數(shù)據(jù)，在可接受的誤差范圍內還原MPU605啲姿態(tài)變化（包括位移、轉動方向和角度），實現(xiàn)一個 簡單的慣性導航系統(tǒng)。5.系統(tǒng)設計5.1 IIC總線工作原理5.1.1 總線的構成及信號類型I2C總線是由數(shù)據(jù)線SDA和時鐘SCL構成的串行總線，可發(fā)送和接收數(shù)據(jù)。 在CPL與被控IC之間、IC與I

7、C之間進行雙向傳送，最高傳送速率100kbps。各 種被控制電路均并聯(lián)在這條總線上，但就像電話機一樣只有撥通各自的號碼才能 工作，所以每個電路和模塊都有唯一的地址，在信息的傳輸過程中，I2C總線上并接的每一模塊電路既是主控器（或被控器），又是發(fā)送器（或接收器），這取決 于它所要完成的功能。CPU發(fā)出的控制信號分為地址碼和控制量兩部分，地址碼 用來選址，即接通需要控制的電路，確定控制的種類；控制量決定該調整的類別 （如對比度、亮度等）及需要調整的量。這樣，各控制電路雖然掛在同一條總線 上，卻彼此獨立，互不相關。I2C總線在傳送數(shù)據(jù)過程中共有三種類型信號，它們分別是：開始信號、結 束信號和應答信號

8、。開始信號：SCL為高電平時，SDA由高電平向低電平跳變，開始傳送數(shù)據(jù)。 結束信號：SCL為低電平時，SDA由低電平向高電平跳變，結束傳送數(shù)據(jù)。應答信號：接收數(shù)據(jù)的IC在接收到8bit數(shù)據(jù)后，向發(fā)送數(shù)據(jù)的IC發(fā)出特 定的低電平脈沖，表示已收到數(shù)據(jù)。CPU向受控單元發(fā)出一個信號后，等待受控 單元發(fā)出一個應答信號，CPU接收到應答信號后，根據(jù)實際情況做出是否繼續(xù)傳 遞信號的判斷。若未收到應答信號，由判斷為受控單元出現(xiàn)故障。這些信號中，開始信號是必須的，結束信號和應答信號都可以不要，IIC總線時序圖如圖5.1.1-1所示。圖5.1-1 IIC 總線時序圖探索者STM32F開發(fā)板板載的EEPRO芯片型

9、號為24C02該芯片的總容量 為256字節(jié)，通過IIC總線與外部連接。STM32F卅發(fā)板有硬件IIC，但是設計 的比較復雜，而且穩(wěn)定性不好，所以我組使用GPIO軟件模擬IIC來對24C02進行讀寫。同時使用軟件更具有移植性，只要有IO 口，將軟件移植過去就能使用模擬的lie，而硬件必須MCU勺支持5.1.2 硬件設計iicCsdaJPB8 139PB9 14G實現(xiàn)模擬lie需要用到的硬件資源有：串口（ USMART GPIO 24C02PB8. TIM4_CH3/I2C1_SC1 /DCn_D6PB9/TUM-1_CHL12C 匚 EDAZDCMf D7I2S2_WSGND93L41A0VCC

10、1A1A2SCLGNDSEAU1424C026nc scl51JC SDA1艮62 lOHVCC33圖 5.1-2 STM32F4 與 24C02連接圖我組通過 GPIO來模擬 IIC ,24C2的 SCL和 SDA分別連在 GPIO_PB和 GPIO_PB9 上，連接關系如圖5.1.2-1。5.2 MPU605工作原理5.2.1 MPU605引腳g0 n 嚴232220G1KIN百NC17N.C1GMPU-05QN-C416NC514ALIXJMl6,137gi.1112Top ViewGMPNCNCIMGMCTID圖5.2-1 MPU6050 結構圖模塊外觀如圖521-2 所示：圖5.2-

11、2 MPU6050實物圖如圖521-1 為MPU605C六軸傳感器的結構圖，總共有 24個引腳，而圖5.2.1-2為MPU6050勺內部邏輯框圖，描述了 MPU605內部的模塊結構，以及各 引腳的連接情況。表5.2.1-1MPU6050引腳輸出和信號描述Pin NumberMPU-605CPin NamePin Description1YCLKINOptional external reference clock input. Connect to GND if6YAUX_DAMl HUOOX-4.I2C master serial data, for connecting to extern

12、al sensors7YAUX_CLI2C Master serial clock, for connecting to external sensors8/CSSPI chip select (0=SPI mode)8YVLOGICDigital I/O supply voltage9AD0/SDOI2C Slave Address LSB (AD0); SPI serial data output (SDO)9YAD0I2C Slave Address LSB (AD0)10YREGOUTRegulator filter capacitor connection11YFSYNCFrame

13、synchronization digital input. Connect to GND if unused.12YINTInterrupt digital output (totem pole or open-drain)13YVDDPower supply voltage and Digital I/O supply voltage18YGNDPower supply ground19, 21YRESVReserved. Do not connect.20YCPOUTCharge pump capacitor connection22YRESVReserved. Do not conne

14、ct.23SCL/SCLKI2C serial clock (SCL); SPI serial clock (SCLK)23YSCLI2C serial clock (SCL)24SDA / SDII2C serial data (SDA); SPI serial data input (SDI)24YSDAI2C serial data (SDA)2, 3, 4, 5, 14,15, 16, 17YNCNot internally connected. May be used for PCB trace routing.表521-1 對每一個引腳的名稱和作用進行了說明。在上述引腳中，SCL和

15、SDA是連接MCUI勺IIC 接口，MCUS過這個IIC接口來控制 MPU6050另外還 有一個IIC接口，連接的引腳為AUX_C和AUX_DA這個接口可用來連接外部從 設備，比如磁傳感器，這樣就可以與 MPU6050l成一個九軸傳感器。VLOGIC是 10 口電壓，該引腳最低可以到1.8V，我們一般直接VDD即可。AD0是從IIC接 口（接MCU的地址控制引腳，該引腳控制IIC地址的最低位，如果接 GND貝U MPU6050勺IIC地址是0X68;如果接VDD則是0X69,注意：這里的地址是不包 含數(shù)據(jù)傳輸?shù)淖畹臀坏模ㄗ畹臀挥脕肀硎咀x寫）。在探索者STM32F開發(fā)板上，AD0是接GND勺，即

16、卩MPU6050勺IIC地址是0X68 （不含最低位）。522硬件設計PB8 139PC0 26PBS/TIM4_CH3/I2C：_SCL/DCM1_I5PB9TIM4_CH442C：SDA.T)CMrFT，站三 WS -PCO/OTG_HS_L1_PI_STP/ADC123_TN10nc SDAnc SCL V 23l3D_INT)1 R45SCLSDA rsnRESV RESV RTSVVCC3.3R4910R3卜4乜z is|1(X1F|1O4AUX CLAUX_DACLKIN FSYNC ADO2? us2LTpTj1675VLOGICVDD REUOUI gnd cpoirrXTPU

17、605010圖5.2-4 MPU6050 與STM32F4勺連接電路圖從圖5.2.2-1可以看出，MPU605通過三根線與STM32F開發(fā)板連接，其中 IIC總線時和24C02以及 WM897共用，接在PB8和PB9上面。MPU6050勺中斷輸 出，連接在STM32F4勺PC0腳，不過本例程我們并沒有用到中斷。另外， AD0接 的GND所以MPU6050勺器件地址是：0X6&5.2.3 初始化操作在使用STM32F堿取MPU6050勺加速度和角度傳感器數(shù)據(jù)之前，需要做以下 初始化操作：初始化IIC接口MPU6050采用IIC與STM32F4通信，所以我們需要先初始化與 MPU605連接的SDA

18、和SCL數(shù)據(jù)線。(2) 復位 MPU6050這一步讓MPU605內部所有寄存器恢復默認值，通過對電源管理寄 存器1 (0X6B的bit7寫1實現(xiàn)。復位后，電源管理寄存器1恢復默 認值(0X40)，然后必須設置該寄存器為 0X00,以喚醒MPU6050進入正 常工作狀態(tài)。（3）設置角速度傳感器（陀螺儀）和加速度傳感器的滿量程范圍這一步，我們設置兩個傳感器的滿量程范圍（FSR），分別通過陀螺儀配置寄存器（0X1B和加速度傳感器配置寄存器（0X1C設置。我們 一般設置陀螺儀的滿量程范圍為土 2000dps，加速度傳感器的滿量程范 圍為土 2g。（4）設置其他參數(shù)這里，我們還需要配置的參數(shù)有：關閉中斷

19、、關閉 AUX IIC接口、 禁止FIFO、設置陀螺儀采樣率和設置數(shù)字低通濾波器（DLPF等。本章 我們不用中斷方式讀取數(shù)據(jù)，所以關閉中斷，然后也沒用到 AUXIIC接 口外接其他傳感器，所以也關閉這個接口。分別通過中斷使能寄存器（0X38和用戶控制寄存器（0X6A 控制。MPU605可以使用FIFO存儲 傳感器數(shù)據(jù)，不過本章我們沒有用到，所以關閉所有FIFO通道，這個通過FIFO使能寄存器（0X23）控制，默認都是0 （即禁止FIFO），所以 用默認值就可以了。陀螺儀采樣率通過采樣率分頻寄存器（0X19）控制， 這個采樣率我們一般設置為 50即可。數(shù)字低通濾波器（DLPF）則通過配 置寄存器

20、（0X1A設置，一般設置DLPF為帶寬的1/2即可。（5）配置系統(tǒng)時鐘源并使能角速度傳感器和加速度傳感器系統(tǒng)時鐘源同樣是通過電源管理寄存器1（ 0X1B來設置，該寄存器的最低三位用于設置系統(tǒng)時鐘源選擇，默認值是0（內部8MRC震蕩）， 不過我們一般設置為1,選擇x軸陀螺PLL作為時鐘源，以獲得更高精 度的時鐘。同時，使能角速度傳感器和加速度傳感器，這兩個操作通過 電源管理寄存器2（ 0X6C來設置，設置對應位為0即可開啟。5.2.4 相關寄存器在讀取MPU605數(shù)據(jù)中主要用到了以下寄存器:（1）Power Management 1（電源管理寄存器 1）表5.2.4-1電源管理寄存器1各位描述R

21、egister(Hex)RegisterBit7Bit6Bit5Bit4Bit3Bit2Bit1Bit06B107DEVICE_RESETSLEEPCYCLE-TEMP_DISCLKSEL2:0如表5.2.4-1 ，寄存器地址為0x6B。DEVICE_RESE位用來控制復位，設置 為1,復位MPU6050復位結束后，MPU硬件自動清零該位；SLEEEP位用于控制 MPU6050勺工作模式，復位后，該位為1,即進入了睡眠模式（低功耗），所以我 們要清零該位，以進入正常工作模式；TEMPDISI于設置是否使能溫度傳感器， 設置為0,則使能；CLKSEL2:0用于選擇系統(tǒng)時鐘源，選擇關系如表 5.2

22、.4-2 所示，默認是使用內部8MRC晶振的，精度不高，所以我們一般選擇 X/Y/Z軸陀 螺作為參考的PLL作為時鐘源，一般設置CLKSEL=00即可。表524-2 CLKSEL 選擇列表CLKSEL2:0時鐘源000內部8M RC晶振001PLL，使用X軸陀螺作為參考010PLL,使用Y軸陀螺作為參考011PLL,使用Z軸陀螺作為參考100PLL,使用外部32.768Khz作為參考101PLL,使用外部19.2Mhz作為參考110保留111關閉時鐘，保持時序產(chǎn)生電路復位狀態(tài)(2) Gyroscope Configuration(陀螺儀配置寄存器)表524-3表陀螺儀配置寄存器各位描述Regi

23、ster(Hex)Register (Decimal)Bit7Bit6Bit5Bit4Bit3Bit2Bit1Bit01B27XG_STYG_STZG_STFS_SEL1:0-如表5.2.4-3，寄存器地址為0x1B。FS_SEL1:0兩個位用于設置陀螺儀的 滿量程范圍：0 為土 250 /s ;1 為土 500 /s ;2 為土 1000 /s ;3 為土 2000 /s。 我們一般設置為3,即土 2000 /s，而因為陀螺儀的ADC為16位分辨率，所以 得到靈敏度為：65536/4000=16.4LSB/( /s)。(3) Accelerometer Configuration(加速度傳感

24、器配置寄存器)表524-4加速度傳感器配置寄存器各位描述Register (Hex)RegisterBit7Bit6Bit5Bit4Bit3Bit2Bit1Bit01C28XA_STYA_STZA_STAFS_SEL1:0-如表5.2.4-4，寄存器地址為0x1Co AFS_SEL1:0兩個位用于設置加速度 傳感器的滿量程范圍：0為土 2g； 1為土 4g； 2為土 8g； 3為土 16g。我們一般設 置為0,即土 2g，而因為加速度傳感器的 ADC也是16位分辨率，所以得到靈敏 度為：65536/4=16384LSB/go(4) FIFO Enable(FIFO 使能寄存器)表524-5 F

25、IFO使能寄存器各位描述Register(Hex)Register (Decimal)Bit7Bit6Bit5Bit4Bit3Bit2Bit1Bit02335TEMP_ FIFO_ENXG_ FIFO_ENYG_ FIFO_ENZG_ FIFO_ENACCELFIFO ENSLV2FIFO ENSLV1FIFO ENSLV0FIFO EN如表5.2.4-5，寄存器地址為0x23o該寄存器用于控制FIFO使能，在簡單 讀取傳感器數(shù)據(jù)的時候，可以不用 FIFO,設置對應位為0即可禁止FIFO,設置 為1,則使能FIFO。注意加速度傳感器的3個軸，全由1個位(ACCEL_FIFO_EN 控制，只要該

26、位置1,則加速度傳感器的三個通道都開啟 FIFO 了。(5) Sample Rate Divider( 采樣率分頻寄存器)表5.2.4-6陀螺儀采樣率分頻寄存器各位描述Register (Hex)Register (Decimal)Bit7Bit6Bit5Bit4Bit3Bit2Bit1Bit01925SMPLRT_DIV7:0如表5.2.4-6，寄存器地址為0x19。該寄存器用于設置MPU6050勺陀螺儀 米樣頻率，計算公式為：采樣頻率=陀螺儀輸出頻率/ (1+SMPLRT_DIV)這里陀螺儀的輸出頻率，是1Khz或者8Khz,與數(shù)字低通濾波器（DLPF的 設置有關，當DLPF_CFG=0/

27、7勺時候，頻率為8Khz,其他情況是1Khz。而且DLPF 濾波頻率一般設置為采樣率的一半。我們假定設置采樣率為50Hz,那么SMPLRT_DIV=1000/50-1=1。（6）Configuration（配置寄存器）表524-7配置寄存器各位描述Register (Hex)Register (Decimal)Bit7Bit6Bit5Bit4Bit3Bit2Bit1Bit01A26-EXT_SYNC_SET2:0DLPF_CFG2:0如表5.2.4-7 ,寄存器地址為0x1A。加速度計和陀螺儀是根據(jù)數(shù)字低通濾波 器（DLPF的三個設置位進行過濾的，即 DLPF_CFG2:0。DLPF_CF不同

28、配置對 應的過濾情況如表5.2.4-8所示：表 524-8 DLPF_CFG 配置表DLPF_CFG2:0加速度傳感器Fs=1Khz角速度傳感器（陀螺儀）帶寬（Hz）延遲（ms）帶寬（Hz）延遲（ms）Fs(Khz)00026002560.9880011842.01881.91010943.0982.81011444.9424.81100218.5208.311011013.81013.41110519.0518.61111保留保留8加速度傳感器輸出速率（Fs）固定是1Khz，而角速度傳感器的輸出速率（Fs）， 則根據(jù)DLPF_CF的配置有所不同。一般我們設置角速度傳感器的帶寬為其采樣 率的一

29、半，如前面所說的，如果設置采樣率為50Hz,那么帶寬就應該設置為25Hz, 取近似值20Hz,就應該設置DLPF_CF為100。（7）Power Management 2（電源管理寄存器 2）表524-9電源管理寄存器2各位描述Register (Hex)Register (Decimal)Bit7Bit6Bit 5Bit 4Bit 3Bit2Bit 1Bit 06C108LP_WAKE_CTRL1:0STBY_XASTBY_YASTBY _ ZASTBY_ XGSTBY _YGSTBY _ZG如表5.2.4-9，寄存器地址為0x6C。該寄存器的LP_WAKE_CTRL于控制低 功耗時的喚醒頻

30、率，項目中沒有用到。剩下的6位，分別控制加速度和陀螺儀的 x/y/z軸是否進入待機模式，由于不需要進入待機模式，所以全部設置為0。（8）Gyroscope Measurements（陀螺儀數(shù)據(jù)輸出寄存器）表5.2.4-10 陀螺儀數(shù)據(jù)輸出寄存器各位描述Register(Hex)Register (Decimal)Bit7Bit6Bit5Bit4Bit3Bit2Bit1Bit04367GYRO_XOUT15:84468GYRO_XOUT7:O4569GYRO_YOUT15:84670GYRO_YOUT7:04771GYRO_ZOUT15:84872GYRO_ZOUT7:O如表524-10，總共有

31、6個寄存器，地址為0x430x48,通過讀取這6個寄 存器，就可以讀到陀螺儀x/y/z軸的值，比如x軸的數(shù)據(jù)，可以通過讀取0x43 （高8位）和0x44 （低8位）寄存器得到，其他軸以此類推。（9）Accelerometer Measurements（加速度傳感器數(shù)據(jù)輸出寄存器）表5.2.4-11加速度傳感器數(shù)據(jù)輸出寄存器各位描述Register(Hex)Register (Decimal)Bit7Bit6Bit5Bit4Bit3Bit2Bit1Bit03B59ACCEL_XOUT15:83C60ACCEL_XOUT7:03D61ACCEL_YOUT15:83E62ACCEL_YOUT7:03

32、F63ACCEL_ZOUT15:84064ACCEL_ZOUT7:0如表5.2.4-11，總共有6個寄存器，地址為0x3B0x40,通過讀取這8個寄 存器，就可以讀到加速度傳感器x/y/z軸的值，比如讀x軸的數(shù)據(jù)，可以通過讀 取0x3B （高8位）和0x3C（低8位）寄存器得到，其他軸以此類推。5.3 ATK-HC05工作原理5.3.1 ATK-HC05引腳ATK-HC05模塊非常小巧（16mm*32mm模塊通過6個2.54mm間距的排針與外部連接，模塊外觀如圖5.3.1-1所示：圖5.3-1 ATK-HC05 模塊外觀圖圖5.3.1-1中，從右到左，依次為模塊引出的PIN1PIN6腳，各引腳

33、的詳細描述如表5.3.1-1所示：表5.3.1-1 ATK-HC05 模塊各引腳功能描述序號名稱說明1LED配對狀態(tài)輸出；配對成功輸出高電平，未配對則輸出低電平。2KEY用于進入AT狀態(tài)；高電平有效（懸空默認為低電平）。3RXD模塊串口接收腳（TTL電平，不能直接接 RS232電平?。山訂纹瑱C的TXD4TXD模塊串口發(fā)送腳（TTL電平，不能直接接 RS232電平?。?，可接單片機的 RXD5GND地6VCC電源（3.3V5.0V）另外，模塊自帶了一個狀態(tài)指示燈：STA該燈有3種狀態(tài)，分別為:1. 在模塊上電的同時（也可以是之前），將KEY設置為高電平（接VCC）， 此時STA慢閃（1秒亮1次

34、），模塊進入AT狀態(tài)，且此時波特率固 定為38400。2. 在模塊上電的時候，將KEY懸空或接GND此時STA快閃（1秒2次）, 表示模塊進入可配對狀態(tài)。如果此時將KEY再拉高，模塊也會進入AT 狀態(tài)，但是STA依舊保 持快閃。3. 模塊配對成功，此時STA雙閃（一次閃2下，2秒閃一次）。5.3.2 硬件設計vr ven ；Rl 5K17D KL TU仁TJAET.TXDmrr PT01&VL K 丸 MU.AHT CTSP1MUAttT-JtihFJUSKM CLKPKTPCM OtTFKpct inP105PlOiA2ODPjAIO1PK?BESETPJOlP100jNDGMD*油p nH

35、C 05二V.T TXD 1 1；血 TT g 3 LH5 KEYI-GND3JTGND圖5.3-2 ATK-HC05 藍牙串口模塊原理圖模塊與單片機連接最少只需要 4根線即可：VCC GND TXD RXD VCC和GND 用于給模塊供電，模塊TXD和 RXD則連接單片機的RXDffi TXD即可。該模塊兼容 5V和3.3V單片機系統(tǒng)，所以可以很方便的連接到系統(tǒng)里面去。ATK-HC05莫塊與單片機系統(tǒng)的典型連接方式如圖532-2所示：圖5.3-3 ATK-HC05模塊與單片機系統(tǒng)連接示意圖圖中虛線連接表示可有可無，可以根據(jù)需要，選擇性的使用5.3.3ATK-HC05與藍牙主機連接首先，開機檢

36、測ATK-HC05藍牙模塊是否存在，如果檢測不成功，則報錯。 檢測成功之后，顯示模塊的主從狀態(tài)，并顯示模塊是否處于連接狀態(tài)，DS0閃爍， 提示程序運行正常。按KEYC按鍵，可以開啟/關閉自動發(fā)送數(shù)據(jù)（通過藍牙模塊 發(fā)送）；按KEY_U按鍵可以切換模塊的主從狀態(tài)。藍牙模塊接收到的數(shù)據(jù)，將直 接顯示在LCdJ（僅支持ASCII字符顯示）。同時，還可以通過USMAF對ATK-HC05 藍牙模塊進行 AT指令查詢和設置。結合手機端藍牙軟件（藍牙串口助手v1.97.apk），可以實現(xiàn)手機無線控制開發(fā)板（點亮和關閉LED1）。所要用到的硬件資源如下：1. 指示燈DS0 DS12. KEY0/KEY_U兩個

37、按鍵3. 串口 1、串口 34. TFTLCD模塊5. ATK-HC05-V13藍牙串口模塊前面介紹了 ATK-HC05藍牙串口模塊的接口，而ALIENTEK索者STM32F407 開發(fā)板板載了一個ATK模塊接口（ ATKMODULJEATK-HC05藍牙模塊可直接插入 該接口實現(xiàn)與探索者STM32F卅發(fā)板的連接。ATK MODULB開發(fā)板主芯片的連接原理圖如圖 5.3.3-1 所示：PB10/SPI2 SCK/TIM2_CHiI；3 TX.TPHI !；TlM2_CH4yU3_RX/12C2 SDMPCO18 PF 6 &BC 肚丫U7(USART3 伐PB10 旳USART3 空PB11

38、幣 也BC 血可3D INT PCQ 26lJF6/riM10 CH1/FSN1C NlORDWC.3 IN4ATK MODULEVCC5PIO6l?SART； TX 廠GBC RX 1IU3 TXGBC TXRS232BTCOM &GPS寸r 0、-A空工口工2PdI*C69圖5.3-4 ATK-MODULE 接口與 MCU連接關系從上圖可以看出，藍牙模塊的串口最簡單的辦法是連接在開發(fā)板的串口3上面，只需要用跳線帽短接 P10的USART3_R冷口 GBC_TX以及USART3_TX和 GBC_RX!卩可實現(xiàn)。連接好 之后，探索者STM32F407開發(fā)板與ATK-HC05藍牙 模塊的連接關系

39、如表2.1所示：表533-1 ATK-HC05 藍牙模塊同探索者 STM32F407開發(fā)板連接關系表ATK-HC05藍牙模塊與開發(fā)板連接關系ATK-HC05藍牙串口模塊VCCGNDTXDRXDKEYLED探索者STM32F407開發(fā)板5VGNDPB11PB10PF6PC0使用時，我們只需要將ATK-HC05藍牙模塊插入到開發(fā)板的ATK-MODUL接口即可，如圖5.3.3-2 所示：圖5.3-5 ATK-HC05 藍牙模塊與探索者開發(fā)板對接實物圖注意，連接好之后，記得檢查P10的跳線帽：必須短接：USART3_R和GBC_TX 以及USART3_T和GBC_RX另外，在實際使用的時候，如果不需要

40、進入 AT設置 和狀態(tài)指示，則連接藍牙模塊只需要 4根線連接即可：VCC/GND/TXD/RXDATK-HC05模塊可以與多種藍牙主機設備連接，這里僅以智能手機為例，進 行說明。首先，在手機上安裝：BTClient.apk，該應用可以在提供的資料里面找到。安裝完應用后，我們打開該應用，如圖5.3.3-3所示: 3B辛回人BTCIientHC圖 5.3-6 BTClie nt.apk運行界面進入搜索藍牙設備界面，如圖533-4 所示：圖5.3-7搜索藍牙設備點擊這個設備，輸533-5 所示：從上圖可以看出，手機已經(jīng)搜索到藍牙模塊了，HC-05,入默認密鑰1234 （僅第一次連接需要設置），完成配

41、對，如圖圖5.3-8輸入配對密碼在輸入密碼之后，等待一段時間，即可連接成功，如圖533-6所示:圖5.3-9連接成功此時，手機和藍牙模塊就連接上了，手機便可以接收數(shù)據(jù)并繪制三維圖形了, 改變開發(fā)板的姿態(tài)，手機端繪制的三維圖形如圖533-7所示：21 43 S A圖5.3-10手機繪制三維圖形界面這樣，我們就實現(xiàn)了 ATK-HC05莫塊與手機的連接。同其他藍牙主機設備的 連接，方法都是類似的，比較簡單，這里我們就不再介紹了。有了 STA指示燈，我們就可以很方便的判斷模塊的當前狀態(tài)，方便使用。5.4 DMF算法原理通過陀螺儀數(shù)據(jù)輸出寄存器和加速度傳感器數(shù)據(jù)輸出寄存器獲取的是MPU605加速度傳感器

42、和陀螺儀的原始數(shù)據(jù)，而實驗需要的是姿態(tài)數(shù)據(jù)，即歐拉 角：航向角（YaW、橫滾角（Roll ）和俯仰角（Pitch ）。要得到歐拉角數(shù)據(jù)，就得利用我們的原始數(shù)據(jù)，進行姿態(tài)融合解算，這個比 較復雜，知識點比較多，而 MPU6050自帶了數(shù)字運動處理器，即 DMP并且， InvenSense提供了一個 MPU6O50勺嵌入式運動驅動庫，結合MPU605啲DMP可 以將我們的原始數(shù)據(jù)，直接轉換成四元數(shù)輸出，而得到四元數(shù)之后，就可以很方 便的計算出歐拉角，從而得到 Yaw Roll和Pitch。使用MPU6O50勺DMP俞出的四元數(shù)是q30格式的，也就是浮點數(shù)放大了 2 的30次方倍。在換算成歐拉角之前

43、，必須先將其轉換為浮點數(shù)，也就是除以 2 的30次方，然后再進行計算，計算公式如下：其中quat0卜quat3 是MPU605啲DMP解算后的四元數(shù)，為q30格式，所以要除以一個2的30次方，其中q30是一個常量為1073741824,即2的30 次方，然后帶入公式，計算出歐拉角。上述計算公式的57.3是弧度轉換為角度， 即180/ n,這樣得到的結果就是以度（ ）為單位的。q0=quat0 / q30; q30 q1= quat1 / q30;格式轉換為浮點數(shù)q2=quat2 / q30;q3=quat3 / q30;/計算得到俯仰角/橫滾角/航向角Pitch=asi n(-2*q1*q3

44、+ 2*q0*q2)*57.3;/俯仰角Roll=atan2(2*q2*q3 + 2*q0*q1, -2*q1*q1 - 2*q2*q2 + 1)*57.3;/ 橫滾角Yaw=atan2(2*(q1*q2 + q0*q3), q0*q0 + q1*q1 - q2*q2 - q3*q3)*57.3;/ 航向角5.5 AHRS5.5.1 AHRSS介AHRS稱為航姿參考系統(tǒng)，包括多個軸向傳感器，能夠為飛行器提供航向, 橫滾和側翻信息，這類系統(tǒng)用來為飛行器提供準確可靠的姿態(tài)與航行信息。PitchRoll圖5.5-1航姿參考系統(tǒng)航姿參考系統(tǒng)包括基于 MEMS勺陀螺儀，加速度計和磁強計。航姿參考系統(tǒng) 與

45、慣性測量單元IMU的區(qū)別在于，航姿參考系統(tǒng)（AHRS包含了嵌入式的姿態(tài)數(shù) 據(jù)解算單元與航向信息，慣性測量單元（IMU）僅僅提供傳感器數(shù)據(jù)，并不具有 提供準確可靠的姿態(tài)數(shù)據(jù)。目前常用的航姿參考系統(tǒng)（AHRS內部采用的多傳感 器數(shù)據(jù)融合進行的航姿解算單元為卡爾曼濾波器。航姿參考系統(tǒng)（AHRS具有兩個主要特點：（1）高精度360度全方位位置姿態(tài)輸出，但采用歐拉角的會具有萬向鎖，不 能全向轉動。（2）高效的數(shù)據(jù)融合算法快速動態(tài)響應與長時間穩(wěn)定性（無漂移，無積累誤 差）相結合。航姿參考系統(tǒng)（AHRS輸出模式為三維全姿態(tài)數(shù)據(jù)（四元數(shù)/歐拉角/旋轉矩 陣）。5.5.2 AHR漱件設計AHRS勺軟件設計主要分

46、為： 傳感器初始化，包括設置傳感器的更新速率、量程。 初始化卡爾曼濾波的相關矩陣，根據(jù)傳感器的特點設置過程激勵噪聲協(xié) 方差矩陣Q,設為對角元素為0.1的四維對角方陣。 若成功讀取陀螺儀數(shù)據(jù)，進行卡爾曼濾波的時間更新。 采集加速度傳感器和磁阻傳感器的數(shù)據(jù)，若讀取成功則進行觀測更新。加速度觀測更新與磁場觀測更新算法差別在于觀測方差的 R,可根據(jù)兩 種傳感器的置信度沒置相應的值，航向姿態(tài)參考系的程序流程如圖5.5.2-1 所示。圖5.5-2航向姿態(tài)參考系的程序流程5.6手機端功能原理及 An droid代碼對手機端apk的修改主要是將兩個獨立的安卓apk進行了源碼級的合并5.6.1 藍牙幀處理藍牙模

47、塊方面，使用了開源的藍牙串口通訊代碼。 在進行合并的過程中，主 要對代碼的接收模塊進行了修改。由于藍牙的發(fā)送和接收以及An droid廣播中傳 遞的均為字符串，因此，為了解析方便，我們將藍牙幀設置為如下格式：u3 pri ntf(P:%c%.1f R:%c%.1f Y:%c%.1fn,cp,Pitch,cr,Roll,cy,Yaw);其中，cp、cr、cy分別為Pitch、Roll、Yaw的符號，為非負數(shù)時顯示為空格符。這里注意到在進行格式化輸出的時候， 控制小數(shù)點后保留一位小數(shù)，但是 小數(shù)點前的位數(shù)無法控制，猜測是由于 c語言頭文件功能較為精簡造成的。562參數(shù)的廣播傳遞為了實現(xiàn)參數(shù)傳遞的功

48、能，查閱了大量An droid相關的代碼和資料，最終選 擇了通過An droid特有的廣播機制進行參數(shù)的傳遞。此外，在廣播的過程中，還 是用了意圖(In te nt )，這也是An droid特有的一個機制，用來對廣播進行定向 生成和接收，保證三維制圖端收到的數(shù)據(jù)為藍牙接收到并進行廣播的數(shù)據(jù)。另外在對藍牙數(shù)據(jù)進行接收時，發(fā)現(xiàn)無法理想地收取到單個幀數(shù)據(jù)， 大部分 時候是一次性獲取到了幾十幀的數(shù)據(jù)，其中第一幀和最后一幀往往是不完整的， 這就要求我們對數(shù)據(jù)進行處理之后再進行廣播：/接收數(shù)據(jù)線程讀入數(shù)據(jù)while(true)num = is.read(bufer); / n=0;String s0 =

49、 new Strin g(buffer,0 ,nu m);fmsg+=sO;/保存收到數(shù)據(jù)for(i=0;i=21)for(i nt len=0;le n=20) intent.putExtra(CCSendflag, tmplen);in te nt.setActio n(CCSe ndflag);II設置廣播的actionII只有和這個action 樣的接收者才能接收廣播 sendBroadcast(intent);II 發(fā)送廣播toSend=;II處理完成，清空廣播緩存if(is.available()=O)break; II短時間沒有數(shù)據(jù)才跳出進行顯示II廣播接收類public cla

50、ss TestReceiver exte nds BroadcastReceiverprivate static final String tag = flag;private static String message = null;public void on Receive(C on text con text,I ntent inten t)String string = intent.getStringExtra(CCSendflag);/ 接收指定廣播if(stri ng != ) setStri ng(stri ng);public void setString(String s

51、tr) message = str;public String getString() retur n message;5.6.3 視圖切換在合并過程中，由于涉及到視圖的切換，因此對An droid操作系統(tǒng)的進程管 理和線程創(chuàng)建進行了學習，最終能夠實現(xiàn)從藍牙接收視圖到三維繪制視圖的平滑 切換。切換的時機最終選擇在藍牙連接建立后，開始接收數(shù)據(jù)的瞬間：/打開接收線程tryis = _socket.getl nputStream(); /得到藍牙數(shù)據(jù)輸入流catch(IOExcepti on e)Toast.makeText(this,接收數(shù)據(jù)失敗！, Toast.LENGTH_SHORT).show();return;if(bThread=false)ReadThread.start();bThread=true;Intent intent = new Intent(BTClient.this, mainActivity.class);in ten t.putExtra(CC0, smsg);startActivity(i nte nt);elsebRun = true;564Ope nGl繪圖三維繪圖方面，使用了官方提供的示例代碼，主要在每一幀繪制的過程中加 入了參數(shù)傳遞的功能，從而實現(xiàn)了