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

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

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

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

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

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

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

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

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

9、M芯片型号为24C02。该芯片的总容量为256字节，通过IIC总线与外部连接。STM32F4开发板有硬件IIC，但是设计的比较复杂，而且稳定性不好，所以我组使用GPIO软件模拟IIC来对24C02进行读写。同时使用软件更具有移植性，只要有IO口，将软件移植过去就能使用模拟的IIC，而硬件必须MCU的支持。5.1.2 硬件设计实现模拟IIC需要用到的硬件资源有：串口（USMART）、GPIO、24C02。图 5.12 STM32F4与24C02连接图我组通过GPIO来模拟IIC，24C2的SCL和SDA分别连在GPIO_PB8和GPIO_PB9上，连接关系如图 5.1.2-1。5.2 MPU60

10、50工作原理5.2 5.2.1 MPU6050引脚图 5.21 MPU6050结构图模块外观如图 5.2.1-2所示：图 5.22 MPU6050实物图图 5.23 MPU6050内部逻辑框图如图 5.2.1-1为MPU6050六轴传感器的结构图，总共有24个引脚，而图 5.2.1-2为MPU6050的内部逻辑框图，描述了MPU6050内部的模块结构，以及各引脚的连接情况。表 5.2.11MPU6050引脚输出和信号描述Pin NumberMPU-6050Pin NamePin Description1YCLKINOptional external reference clock input.

11、 Connect to GND if unused.6YAUX_DAI2C master serial data, for connecting to external 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

12、 Slave Address LSB (AD0)10YREGOUTRegulator filter capacitor connection11YFSYNCFrame 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.

13、Do not connect.20YCPOUTCharge pump capacitor connection22YRESVReserved. Do not connect.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 intern

14、ally connected. May be used for PCB trace routing.表 5.2.1-1对每一个引脚的名称和作用进行了说明。在上述引脚中，SCL和SDA是连接MCU的IIC接口，MCU通过这个IIC接口来控制MPU6050。另外还有一个IIC接口，连接的引脚为AUX_CL和AUX_DA，这个接口可用来连接外部从设备，比如磁传感器，这样就可以与MPU6050组成一个九轴传感器。VLOGIC是IO口电压，该引脚最低可以到1.8V，我们一般直接VDD即可。AD0是从IIC接口（接MCU）的地址控制引脚，该引脚控制IIC地址的最低位，如果接GND，则MPU6050的IIC

15、地址是0X68；如果接VDD，则是0X69。注意：这里的地址是不包含数据传输的最低位的（最低位用来表示读写）。在探索者STM32F4开发板上，AD0是接GND的，即MPU6050的IIC地址是0X68（不含最低位）。5.2.2 硬件设计图 5.24 MPU6050与STM32F4的连接电路图从图 5.2.2-1可以看出，MPU6050通过三根线与STM32F4开发板连接，其中IIC总线时和24C02以及WM8978共用，接在PB8和PB9上面。MPU6050的中断输出，连接在STM32F4的PC0脚，不过本例程我们并没有用到中断。另外，AD0接的GND，所以MPU6050的器件地址是：0X68

16、。5.2.3 初始化操作在使用STM32F4读取MPU6050的加速度和角度传感器数据之前，需要做以下初始化操作：(1) 初始化IIC接口MPU6050采用IIC与STM32F4通信，所以我们需要先初始化与MPU6050连接的SDA和SCL数据线。(2) 复位MPU6050这一步让MPU6050内部所有寄存器恢复默认值，通过对电源管理寄存器1（0X6B）的bit7写1实现。 复位后，电源管理寄存器1恢复默认值(0X40)，然后必须设置该寄存器为0X00，以唤醒MPU6050，进入正常工作状态。(3) 设置角速度传感器（陀螺仪）和加速度传感器的满量程范围这一步，我们设置两个传感器的满量程范围(F

17、SR)，分别通过陀螺仪配置寄存器（0X1B）和加速度传感器配置寄存器（0X1C）设置。我们一般设置陀螺仪的满量程范围为2000dps，加速度传感器的满量程范围为2g。(4) 设置其他参数这里，我们还需要配置的参数有：关闭中断、关闭AUX IIC接口、禁止FIFO、设置陀螺仪采样率和设置数字低通滤波器（DLPF）等。本章我们不用中断方式读取数据，所以关闭中断，然后也没用到AUX IIC接口外接其他传感器，所以也关闭这个接口。分别通过中断使能寄存器（0X38）和用户控制寄存器（0X6A）控制。MPU6050可以使用FIFO存储传感器数据，不过本章我们没有用到，所以关闭所有FIFO通道，这个通过FI

18、FO使能寄存器（0X23）控制，默认都是0（即禁止FIFO），所以用默认值就可以了。陀螺仪采样率通过采样率分频寄存器（0X19）控制，这个采样率我们一般设置为50即可。数字低通滤波器(DLPF)则通过配置寄存器（0X1A）设置，一般设置DLPF为带宽的1/2即可。(5) 配置系统时钟源并使能角速度传感器和加速度传感器系统时钟源同样是通过电源管理寄存器1（0X1B）来设置，该寄存器的最低三位用于设置系统时钟源选择，默认值是0（内部8M RC震荡），不过我们一般设置为1，选择x轴陀螺PLL作为时钟源，以获得更高精度的时钟。同时，使能角速度传感器和加速度传感器，这两个操作通过电源管理寄存器2（0X6

19、C）来设置，设置对应位为0即可开启。5.2.4 相关寄存器在读取MPU6050数据中主要用到了以下寄存器：(1) Power Management 1(电源管理寄存器1)表 5.2.41电源管理寄存器1各位描述Register (Hex)Register (Decimal)Bit7Bit6Bit5Bit4Bit3Bit2Bit1Bit06B107DEVICE_RESETSLEEPCYCLE-TEMP_DISCLKSEL2:0如表 5.2.4-1，寄存器地址为0x6B。DEVICE_RESET位用来控制复位，设置为1，复位MPU6050，复位结束后，MPU硬件自动清零该位；SLEEEP位用于控制

20、MPU6050的工作模式，复位后，该位为1，即进入了睡眠模式（低功耗），所以我们要清零该位，以进入正常工作模式；TEMP_DIS用于设置是否使能温度传感器，设置为0，则使能；CLKSEL2:0用于选择系统时钟源，选择关系如表 5.2.4-2所示，默认是使用内部8M RC晶振的，精度不高，所以我们一般选择X/Y/Z轴陀螺作为参考的PLL作为时钟源，一般设置CLKSEL=001即可。表 5.2.42 CLKSEL选择列表CLKSEL2:0时钟源000内部8M RC晶振001PLL，使用X轴陀螺作为参考010PLL，使用Y轴陀螺作为参考011PLL，使用Z轴陀螺作为参考100PLL，使用外部32.7

21、68Khz作为参考101PLL，使用外部19.2Mhz作为参考110保留111关闭时钟，保持时序产生电路复位状态(2) Gyroscope Configuration(陀螺仪配置寄存器)表 5.2.43表陀螺仪配置寄存器各位描述Register(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，即2

22、000/s，而因为陀螺仪的ADC为16位分辨率，所以得到灵敏度为：65536/4000=16.4LSB/(/s)。(3) Accelerometer Configuration(加速度传感器配置寄存器)表 5.2.44加速度传感器配置寄存器各位描述Register (Hex)Register(Decimal)Bit7Bit6Bit5Bit4Bit3Bit2Bit1Bit01C28XA_STYA_STZA_STAFS_SEL1:0-如表 5.2.4-4，寄存器地址为0x1C。AFS_SEL1:0两个位用于设置加速度传感器的满量程范围：0为2g；1为4g；2为8g；3为16g。我们一般设置为0，即

23、2g，而因为加速度传感器的ADC也是16位分辨率，所以得到灵敏度为：65536/4=16384LSB/g。(4) FIFO Enable(FIFO使能寄存器)表 5.2.45 FIFO使能寄存器各位描述Register(Hex)Register(Decimal)Bit7Bit6Bit5Bit4Bit3Bit2Bit1Bit02335TEMP_ FIFO_ENXG_ FIFO_ENYG_ FIFO_ENZG_ FIFO_ENACCEL_FIFO_ENSLV2_FIFO_ENSLV1_FIFO_ENSLV0_FIFO_EN如表 5.2.4-5，寄存器地址为0x23。该寄存器用于控制FIFO使能，在

24、简单读取传感器数据的时候，可以不用FIFO，设置对应位为0即可禁止FIFO，设置为1，则使能FIFO。注意加速度传感器的3个轴，全由1个位（ACCEL_FIFO_EN）控制，只要该位置1，则加速度传感器的三个通道都开启FIFO了。(5) Sample Rate Divider(采样率分频寄存器)表 5.2.46陀螺仪采样率分频寄存器各位描述Register (Hex)Register (Decimal)Bit7Bit6Bit5Bit4Bit3Bit2Bit1Bit01925SMPLRT_DIV7:0如表 5.2.4-6，寄存器地址为0x19。该寄存器用于设置MPU6050的陀螺仪采样频率，计算

25、公式为：采样频率 = 陀螺仪输出频率 / (1+SMPLRT_DIV)这里陀螺仪的输出频率，是1Khz或者8Khz，与数字低通滤波器（DLPF）的设置有关，当DLPF_CFG=0/7的时候，频率为8Khz，其他情况是1Khz。而且DLPF滤波频率一般设置为采样率的一半。我们假定设置采样率为50Hz，那么SMPLRT_DIV=1000/50-1=19。(6) Configuration(配置寄存器)表 5.2.47配置寄存器各位描述Register (Hex) Register (Decimal)Bit7Bit6Bit5Bit4Bit3Bit2Bit1Bit01A26-EXT_SYNC_SET2

26、:0DLPF_CFG2:0如表5.2.4-7，寄存器地址为0x1A。加速度计和陀螺仪是根据数字低通滤波器（DLPF）的三个设置位进行过滤的，即DLPF_CFG2:0。DLPF_CFG不同配置对应的过滤情况如表 5.2.4-8所示：表 5.2.48 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.051

27、8.61111保留保留8加速度传感器输出速率（Fs）固定是1Khz，而角速度传感器的输出速率（Fs），则根据DLPF_CFG的配置有所不同。一般我们设置角速度传感器的带宽为其采样率的一半，如前面所说的，如果设置采样率为50Hz，那么带宽就应该设置为25Hz，取近似值20Hz，就应该设置DLPF_CFG为100。(7) Power Management 2(电源管理寄存器2)表 5.2.49电源管理寄存器2各位描述Register(Hex)Register(Decimal)Bit7Bit6 Bit 5 Bit 4Bit 3Bit2Bit 1Bit 06C108LP_WAKE_CTRL1:0STB

28、Y_XASTBY_YASTBY_ ZA STBY_ XGSTBY_YGSTBY_ZG如表5.2.4-9，寄存器地址为0x6C。该寄存器的LP_WAKE_CTRL用于控制低功耗时的唤醒频率，项目中没有用到。剩下的6位，分别控制加速度和陀螺仪的x/y/z轴是否进入待机模式，由于不需要进入待机模式，所以全部设置为0。(8) Gyroscope Measurements(陀螺仪数据输出寄存器)表 5.2.410陀螺仪数据输出寄存器各位描述Register (Hex)Register (Decimal)Bit7Bit6Bit5Bit4Bit3Bit2Bit1Bit04367GYRO_XOUT15:844

29、68GYRO_XOUT7:04569GYRO_YOUT15:84670GYRO_YOUT7:04771GYRO_ZOUT15:84872GYRO_ZOUT7:0如表5.2.4-10，总共有6个寄存器，地址为0x430x48，通过读取这6个寄存器，就可以读到陀螺仪x/y/z轴的值，比如x轴的数据，可以通过读取0x43（高8位）和0x44（低8位）寄存器得到，其他轴以此类推。(9) Accelerometer Measurements(加速度传感器数据输出寄存器)表 5.2.411加速度传感器数据输出寄存器各位描述Register(Hex)Register(Decimal)Bit7Bit6Bit5Bit4Bit3Bit2Bit1Bit03B59ACCEL_XOUT15:83C60ACCEL_XOUT7:03D61ACCE