基于MPU6050的INS惯性导航和实时姿态检测系统方案

1、基于MPU6O50勺INS惯性导航和实时姿态检测系统1.项目目标及功能说明1.1项目目标学习使用正点原子探索者开发板，并熟悉开发板上的MPU605六轴传感器的 工作原理和各函数的调用过程。同时学习开发板的扩展接口，尝试在开发板上扩 展蓝牙模块，并实现开发板与手机等含有蓝牙模块的电子设备通过蓝牙连接并进 行数据的传输。在完成上述内容的基础上，实现将MPU605六轴传感器的加速度 计和陀螺仪的数据传送到手机上，在手机上实现陀螺仪的变化效果展示。同时通 过串口将MPU605数据传送到电脑上，通过 Matlab编程处理数据，实现惯性导 航的简单展示。1.2系统功能说明系统最主要的功能有两个：一个是在手

2、机端能够展示开发板上 MPU6050它螺 仪的姿态变化，通过一个立方体的转动来表示陀螺仪的转动； 另一个是在电脑端 能够读取MPU6050勺数据，并通过对数据的处理还原数据中存储的 MPU6050勺姿 态变化，简单展现出惯性导航的效果。在实现系统最主要的两个功能过程中，还需要实现一些基础功能。开发板能 够通过蓝牙与手机连接并传输数据； 开发板能够通过串口将数据发送出去； 在电 脑端能够读取开发板上串口输出的数据等。专业技术资料2.需求分析惯性导航系统用于各种运动机具中，包括飞机、潜艇、航天飞机等运输工 具及导弹，然而成本及复杂性限制了其可以应用的场合。但是，存在一种情形：卫星一旦突然因故障、敌

3、方打击或干扰（如太阳风暴）等原因无法提供服务， 这对依赖GPS北斗等卫星导航系统作为唯一 PNTPosition Navigation Time） 信息来源的系统来说可能是致命的灾难。作为目前为止卫星导航系统最好的备援惯性导航系统 （INS），将于届时 发挥出巨大的作用，其精度完全可以媲美GPS等卫星导航系统。并且它不需要外 部参考就可确定当前位置、方向及速度，从而使它自然地不受外界的干扰和欺骗。定位、导航和授时服务对军队而言就像氧气对人类一样不可或缺， 因此通过 研究新机理、研制新设备、开发新算法，以摆脱人员和系统设备对 GPS的依赖, 具有极大的战略意义。姿态监测系统可广泛应用于关键资产姿

4、态变化的无线实时监控。由于目前 移动智能终端设备的数量和质量逐步提升， 因此，通过计算机上传统的上位机软 件进行姿态监测，逐渐暴露出了自身的缺点一一串口传输无法实现无线监测、计算机相比智能终端便携性极差。因此，使用无线传输（蓝牙、红外、 WIFI、GSM等）的技术，开发一款在移 动智能终端可以实时显示物体姿态的应用， 具有很高的实用价值和广泛的市场应 用前景。3.开发环境移动终端操作系统：An droid 444 KitKat计算机操作系统：Win dows 8.1 Pro x64串口开发：开发板IDE:MATLAB R2014aKeil uVisio n5An droid IDE :Ecli

5、pse Java EE IDE for Web DevelopersAn droid Developme nt Toolkit 23.0. 4.14685184.项目进展情况到目前为止，我组已实现了以下功能:1. STM32F开发板上MPU605六轴传感器的数据获取并显示在 LCD屏幕上。2. 在LCD屏幕上绘出圆形图案，且圆形图案能根据 MPU605六轴传感器的 姿态变化而运动，传感器倾斜角度越大，图案运动速度越快。3. 扩展蓝牙模块，能通过蓝牙模块与手机连接并进行数据通信。4. 根据函数提供的帧格式定义数据帧， 并通过USAR接口将数据帧传给PC 端。5. 在手机端能根据蓝牙获取的 MPU

6、605六轴传感器的陀螺仪数据绘出立方体，立方体能在可接受的时间延迟内实时展现MPU605啲姿态变化（转动方向和角度）。6. 在PC端能通过对从USART接口获取的数据帧进行解析获取 MPU605加速 度传感器和陀螺仪的数据，并根据数据帧中设置的校验位进行数据校验。7. 在PC端能根据解析出的加速度传感器和陀螺仪数据，在可接受的误差范围内还原MPU605啲姿态变化（包括位移、转动方向和角度），实现一个 简单的惯性导航系统。5.系统设计5.1 IIC总线工作原理5.1.1 总线的构成及信号类型I2C总线是由数据线SDA和时钟SCL构成的串行总线，可发送和接收数据。 在CPL与被控IC之间、IC与I

7、C之间进行双向传送，最高传送速率100kbps。各 种被控制电路均并联在这条总线上，但就像电话机一样只有拨通各自的号码才能 工作，所以每个电路和模块都有唯一的地址，在信息的传输过程中，I2C总线上并接的每一模块电路既是主控器（或被控器），又是发送器（或接收器），这取决 于它所要完成的功能。CPU发出的控制信号分为地址码和控制量两部分，地址码 用来选址，即接通需要控制的电路，确定控制的种类；控制量决定该调整的类别 （如对比度、亮度等）及需要调整的量。这样，各控制电路虽然挂在同一条总线 上，却彼此独立，互不相关。I2C总线在传送数据过程中共有三种类型信号，它们分别是：开始信号、结 束信号和应答信号

8、。开始信号：SCL为高电平时，SDA由高电平向低电平跳变，开始传送数据。 结束信号：SCL为低电平时，SDA由低电平向高电平跳变，结束传送数据。应答信号：接收数据的IC在接收到8bit数据后，向发送数据的IC发出特 定的低电平脉冲，表示已收到数据。CPU向受控单元发出一个信号后，等待受控 单元发出一个应答信号，CPU接收到应答信号后，根据实际情况做出是否继续传 递信号的判断。若未收到应答信号，由判断为受控单元出现故障。这些信号中，开始信号是必须的，结束信号和应答信号都可以不要，IIC总线时序图如图5.1.1-1所示。图5.1-1 IIC 总线时序图探索者STM32F开发板板载的EEPRO芯片型

9、号为24C02该芯片的总容量 为256字节，通过IIC总线与外部连接。STM32F卅发板有硬件IIC，但是设计 的比较复杂，而且稳定性不好，所以我组使用GPIO软件模拟IIC来对24C02进行读写。同时使用软件更具有移植性，只要有IO 口，将软件移植过去就能使用模拟的lie，而硬件必须MCU勺支持5.1.2 硬件设计iicCsdaJPB8 139PB9 14G实现模拟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,注意：这里的地址是不包 含数据传输的最低位的（最低位用来表示读写）。在探索者STM32F开发板上，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开发板连接，其中 IIC总线时和24C02以及 WM897共用，接在PB8和PB9上面。MPU6050勺中断输 出，连接在STM32F4勺PC0脚，不过本例程我们并没有用到中断。另外， AD0接 的GND所以MPU6050勺器件地址是：0X6&5.2.3 初始化操作在使用STM32F碱取MPU6050勺加速度和角度传感器数据之前，需要做以下 初始化操作：初始化IIC接口MPU6050采用IIC与STM32F4通信，所以我们需要先初始化与 MPU605连接的SDA

18、和SCL数据线。(2) 复位 MPU6050这一步让MPU605内部所有寄存器恢复默认值，通过对电源管理寄 存器1 (0X6B的bit7写1实现。复位后，电源管理寄存器1恢复默 认值(0X40)，然后必须设置该寄存器为 0X00,以唤醒MPU6050进入正 常工作状态。（3）设置角速度传感器（陀螺仪）和加速度传感器的满量程范围这一步，我们设置两个传感器的满量程范围（FSR），分别通过陀螺仪配置寄存器（0X1B和加速度传感器配置寄存器（0X1C设置。我们 一般设置陀螺仪的满量程范围为土 2000dps，加速度传感器的满量程范 围为土 2g。（4）设置其他参数这里，我们还需要配置的参数有：关闭中断

19、、关闭 AUX IIC接口、 禁止FIFO、设置陀螺仪采样率和设置数字低通滤波器（DLPF等。本章 我们不用中断方式读取数据，所以关闭中断，然后也没用到 AUXIIC接 口外接其他传感器，所以也关闭这个接口。分别通过中断使能寄存器（0X38和用户控制寄存器（0X6A 控制。MPU605可以使用FIFO存储 传感器数据，不过本章我们没有用到，所以关闭所有FIFO通道，这个通过FIFO使能寄存器（0X23）控制，默认都是0 （即禁止FIFO），所以 用默认值就可以了。陀螺仪采样率通过采样率分频寄存器（0X19）控制， 这个采样率我们一般设置为 50即可。数字低通滤波器（DLPF）则通过配 置寄存器

20、（0X1A设置，一般设置DLPF为带宽的1/2即可。（5）配置系统时钟源并使能角速度传感器和加速度传感器系统时钟源同样是通过电源管理寄存器1（ 0X1B来设置，该寄存器的最低三位用于设置系统时钟源选择，默认值是0（内部8MRC震荡）， 不过我们一般设置为1,选择x轴陀螺PLL作为时钟源，以获得更高精 度的时钟。同时，使能角速度传感器和加速度传感器，这两个操作通过 电源管理寄存器2（ 0X6C来设置，设置对应位为0即可开启。5.2.4 相关寄存器在读取MPU605数据中主要用到了以下寄存器:（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用于选择系统时钟源，选择关系如表 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关闭时钟，保持时序产生电路复位状态(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使能，在简单 读取传感器数据的时候，可以不用 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,与数字低通滤波器（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。加速度计和陀螺仪是根据数字低通滤波 器（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）， 则根据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（陀螺仪数据输出寄存器）表5.2.4-10 陀螺仪数据输出寄存器各位描述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轴的数据，可以通过读取0x43 （高8位）和0x44 （低8位）寄存器得到，其他轴以此类推。（9）Accelerometer Measurements（加速度传感器数据输出寄存器）表5.2.4-11加速度传感器数据输出寄存器各位描述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轴的数据，可以通过读 取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配对状态输出；配对成功输出高电平，未配对则输出低电平。2KEY用于进入AT状态；高电平有效（悬空默认为低电平）。3RXD模块串口接收脚（TTL电平，不能直接接 RS232电平！），可接单片机的TXD4TXD模块串口发送脚（TTL电平，不能直接接 RS232电平！），可接单片机的 RXD5GND地6VCC电源（3.3V5.0V）另外，模块自带了一个状态指示灯：STA该灯有3种状态，分别为:1. 在模块上电的同时（也可以是之前），将KEY设置为高电平（接VCC）， 此时STA慢闪（1秒亮1次

34、），模块进入AT状态，且此时波特率固 定为38400。2. 在模块上电的时候，将KEY悬空或接GND此时STA快闪（1秒2次）, 表示模块进入可配对状态。如果此时将KEY再拉高，模块也会进入AT 状态，但是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单片机系统，所以可以很方便的连接到系统里面去。ATK-HC05莫块与单片机系统的典型连接方式如图532-2所示：图5.3-3 ATK-HC05模块与单片机系统连接示意图图中虚线连接表示可有可无，可以根据需要，选择性的使用5.3.3ATK-HC05与蓝牙主机连接首先，开机检

36、测ATK-HC05蓝牙模块是否存在，如果检测不成功，则报错。 检测成功之后，显示模块的主从状态，并显示模块是否处于连接状态，DS0闪烁， 提示程序运行正常。按KEYC按键，可以开启/关闭自动发送数据（通过蓝牙模块 发送）；按KEY_U按键可以切换模块的主从状态。蓝牙模块接收到的数据，将直 接显示在LCdJ（仅支持ASCII字符显示）。同时，还可以通过USMAF对ATK-HC05 蓝牙模块进行 AT指令查询和设置。结合手机端蓝牙软件（蓝牙串口助手v1.97.apk），可以实现手机无线控制开发板（点亮和关闭LED1）。所要用到的硬件资源如下：1. 指示灯DS0 DS12. KEY0/KEY_U两个

37、按键3. 串口 1、串口 34. TFTLCD模块5. ATK-HC05-V13蓝牙串口模块前面介绍了 ATK-HC05蓝牙串口模块的接口，而ALIENTEK索者STM32F407 开发板板载了一个ATK模块接口（ ATKMODULJEATK-HC05蓝牙模块可直接插入 该接口实现与探索者STM32F卅发板的连接。ATK MODULB开发板主芯片的连接原理图如图 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连接关系从上图可以看出，蓝牙模块的串口最简单的办法是连接在开发板的串口3上面，只需要用跳线帽短接 P10的USART3_R冷口 GBC_TX以及USART3_TX和 GBC_RX!卩可实现。连接好 之后，探索者STM32F407开发板与ATK-HC05蓝牙 模块的连接关系

39、如表2.1所示：表533-1 ATK-HC05 蓝牙模块同探索者 STM32F407开发板连接关系表ATK-HC05蓝牙模块与开发板连接关系ATK-HC05蓝牙串口模块VCCGNDTXDRXDKEYLED探索者STM32F407开发板5VGNDPB11PB10PF6PC0使用时，我们只需要将ATK-HC05蓝牙模块插入到开发板的ATK-MODUL接口即可，如图5.3.3-2 所示：图5.3-5 ATK-HC05 蓝牙模块与探索者开发板对接实物图注意，连接好之后，记得检查P10的跳线帽：必须短接：USART3_R和GBC_TX 以及USART3_T和GBC_RX另外，在实际使用的时候，如果不需要

40、进入 AT设置 和状态指示，则连接蓝牙模块只需要 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 所示：从上图可以看出，手机已经搜索到蓝牙模块了，HC-05,入默认密钥1234 （仅第一次连接需要设置），完成配

41、对，如图图5.3-8输入配对密码在输入密码之后，等待一段时间，即可连接成功，如图533-6所示:图5.3-9连接成功此时，手机和蓝牙模块就连接上了，手机便可以接收数据并绘制三维图形了, 改变开发板的姿态，手机端绘制的三维图形如图533-7所示：21 43 S A图5.3-10手机绘制三维图形界面这样，我们就实现了 ATK-HC05莫块与手机的连接。同其他蓝牙主机设备的 连接，方法都是类似的，比较简单，这里我们就不再介绍了。有了 STA指示灯，我们就可以很方便的判断模块的当前状态，方便使用。5.4 DMF算法原理通过陀螺仪数据输出寄存器和加速度传感器数据输出寄存器获取的是MPU605加速度传感器

42、和陀螺仪的原始数据，而实验需要的是姿态数据，即欧拉 角：航向角（YaW、横滚角（Roll ）和俯仰角（Pitch ）。要得到欧拉角数据，就得利用我们的原始数据，进行姿态融合解算，这个比 较复杂，知识点比较多，而 MPU6050自带了数字运动处理器，即 DMP并且， InvenSense提供了一个 MPU6O50勺嵌入式运动驱动库，结合MPU605啲DMP可 以将我们的原始数据，直接转换成四元数输出，而得到四元数之后，就可以很方 便的计算出欧拉角，从而得到 Yaw Roll和Pitch。使用MPU6O50勺DMP俞出的四元数是q30格式的，也就是浮点数放大了 2 的30次方倍。在换算成欧拉角之前

43、，必须先将其转换为浮点数，也就是除以 2 的30次方，然后再进行计算，计算公式如下：其中quat0卜quat3 是MPU605啲DMP解算后的四元数，为q30格式，所以要除以一个2的30次方，其中q30是一个常量为1073741824,即2的30 次方，然后带入公式，计算出欧拉角。上述计算公式的57.3是弧度转换为角度， 即180/ n,这样得到的结果就是以度（ ）为单位的。q0=quat0 / q30; q30 q1= quat1 / q30;格式转换为浮点数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称为航姿参考系统，包括多个轴向传感器，能够为飞行器提供航向, 横滚和侧翻信息，这类系统用来为飞行器提供准确可靠的姿态与航行信息。PitchRoll图5.5-1航姿参考系统航姿参考系统包括基于 MEMS勺陀螺仪，加速度计和磁强计。航姿参考系统 与

45、惯性测量单元IMU的区别在于，航姿参考系统（AHRS包含了嵌入式的姿态数 据解算单元与航向信息，惯性测量单元（IMU）仅仅提供传感器数据，并不具有 提供准确可靠的姿态数据。目前常用的航姿参考系统（AHRS内部采用的多传感 器数据融合进行的航姿解算单元为卡尔曼滤波器。航姿参考系统（AHRS具有两个主要特点：（1）高精度360度全方位位置姿态输出，但采用欧拉角的会具有万向锁，不 能全向转动。（2）高效的数据融合算法快速动态响应与长时间稳定性（无漂移，无积累误 差）相结合。航姿参考系统（AHRS输出模式为三维全姿态数据（四元数/欧拉角/旋转矩 阵）。5.5.2 AHR漱件设计AHRS勺软件设计主要分

46、为： 传感器初始化，包括设置传感器的更新速率、量程。 初始化卡尔曼滤波的相关矩阵，根据传感器的特点设置过程激励噪声协 方差矩阵Q,设为对角元素为0.1的四维对角方阵。 若成功读取陀螺仪数据，进行卡尔曼滤波的时间更新。 采集加速度传感器和磁阻传感器的数据，若读取成功则进行观测更新。加速度观测更新与磁场观测更新算法差别在于观测方差的 R,可根据两 种传感器的置信度没置相应的值，航向姿态参考系的程序流程如图5.5.2-1 所示。图5.5-2航向姿态参考系的程序流程5.6手机端功能原理及 An droid代码对手机端apk的修改主要是将两个独立的安卓apk进行了源码级的合并5.6.1 蓝牙帧处理蓝牙模

47、块方面，使用了开源的蓝牙串口通讯代码。 在进行合并的过程中，主 要对代码的接收模块进行了修改。由于蓝牙的发送和接收以及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的符号，为非负数时显示为空格符。这里注意到在进行格式化输出的时候， 控制小数点后保留一位小数，但是 小数点前的位数无法控制，猜测是由于 c语言头文件功能较为精简造成的。562参数的广播传递为了实现参数传递的功

48、能，查阅了大量An droid相关的代码和资料，最终选 择了通过An droid特有的广播机制进行参数的传递。此外，在广播的过程中，还 是用了意图(In te nt )，这也是An droid特有的一个机制，用来对广播进行定向 生成和接收，保证三维制图端收到的数据为蓝牙接收到并进行广播的数据。另外在对蓝牙数据进行接收时，发现无法理想地收取到单个帧数据， 大部分 时候是一次性获取到了几十帧的数据，其中第一帧和最后一帧往往是不完整的， 这就要求我们对数据进行处理之后再进行广播：/接收数据线程读入数据while(true)num = is.read(bufer); / n=0;String s0 =

49、 new Strin g(buffer,0 ,nu m);fmsg+=sO;/保存收到数据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 发送广播toSend=;II处理完成，清空广播缓存if(is.available()=O)break; II短时间没有数据才跳出进行显示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操作系统的进程管 理和线程创建进行了学习，最终能够实现从蓝牙接收视图到三维绘制视图的平滑 切换。切换的时机最终选择在蓝牙连接建立后，开始接收数据的瞬间：/打开接收线程tryis = _socket.getl nputStream(); /得到蓝牙数据输入流catch(IOExcepti on e)Toast.makeText(this,接收数据失败！, 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绘图三维绘图方面，使用了官方提供的示例代码，主要在每一帧绘制的过程中加 入了参数传递的功能，从而实现了