基于MPU6050的INS惯性导航和实时姿态检测系统.docx
《基于MPU6050的INS惯性导航和实时姿态检测系统.docx》由会员分享，可在线阅读，更多相关《基于MPU6050的INS惯性导航和实时姿态检测系统.docx（54页珍藏版）》请在冰豆网上搜索。
基于MPU6050的INS惯性导航和实时姿态检测系统
基于MPU6050的INS惯性导航和实时姿态检测系统
1.项目目标及功能说明
1.1项目目标
学习使用正点原子探索者开发板，并熟悉开发板上的MPU6050六轴传感器的工作原理和各函数的调用过程。
同时学习开发板的扩展接口，尝试在开发板上扩展蓝牙模块，并实现开发板与手机等含有蓝牙模块的电子设备通过蓝牙连接并进行数据的传输。
在完成上述内容的基础上，实现将MPU6050六轴传感器的加速度计和陀螺仪的数据传送到手机上，在手机上实现陀螺仪的变化效果展示。
同时通过串口将MPU6050数据传送到电脑上，通过Matlab编程处理数据，实现惯性导航的简单展示。
1.2系统功能说明
系统最主要的功能有两个：
一个是在手机端能够展示开发板上MPU6050陀螺仪的姿态变化，通过一个立方体的转动来表示陀螺仪的转动；另一个是在电脑端能够读取MPU6050的数据，并通过对数据的处理还原数据中存储的MPU6050的姿态变化，简单展现出惯性导航的效果。
在实现系统最主要的两个功能过程中，还需要实现一些基础功能。
开发板能够通过蓝牙与手机连接并传输数据；开发板能够通过串口将数据发送出去；在电脑端能够读取开发板上串口输出的数据等。
2.需求分析
●惯性导航系统用于各种运动机具中，包括飞机、潜艇、航天飞机等运输工具及导弹，然而成本及复杂性限制了其可以应用的场合。
但是，存在一种情形：
卫星一旦突然因故障、敌方打击或干扰（如太阳风暴）等原因无法提供服务，这对依赖GPS、北斗等卫星导航系统作为唯一PNT（Position、Navigation、Time）信息来源的系统来说可能是致命的灾难。
作为目前为止卫星导航系统最好的备援——惯性导航系统（INS），将于届时发挥出巨大的作用，其精度完全可以媲美GPS等卫星导航系统。
并且它不需要外部参考就可确定当前位置、方向及速度，从而使它自然地不受外界的干扰和欺骗。
定位、导航和授时服务对军队而言就像氧气对人类一样不可或缺，因此通过研究新机理、研制新设备、开发新算法，以摆脱人员和系统设备对GPS的依赖，具有极大的战略意义。
●姿态监测系统可广泛应用于关键资产姿态变化的无线实时监控。
由于目前移动智能终端设备的数量和质量逐步提升，因此，通过计算机上传统的上位机软件进行姿态监测，逐渐暴露出了自身的缺点——串口传输无法实现无线监测、计算机相比智能终端便携性极差。
因此，使用无线传输（蓝牙、红外、WIFI、GSM等）的技术，开发一款在移动智能终端可以实时显示物体姿态的应用，具有很高的实用价值和广泛的市场应用前景。
3.开发环境
移动终端操作系统：
Android4.4.4KitKat
计算机操作系统：
Windows8.1Prox64
串口开发：
MATLABR2014a
开发板IDE：
KeiluVision5
AndroidIDE：
EclipseJavaEEIDEforWebDevelopers
AndroidDevelopmentToolkit23.0.4.1468518
4.项目进展情况
到目前为止，我组已实现了以下功能：
1.STM32F4开发板上MPU6050六轴传感器的数据获取并显示在LCD屏幕上。
2.在LCD屏幕上绘出圆形图案，且圆形图案能根据MPU6050六轴传感器的姿态变化而运动，传感器倾斜角度越大，图案运动速度越快。
3.扩展蓝牙模块，能通过蓝牙模块与手机连接并进行数据通信。
4.根据函数提供的帧格式定义数据帧，并通过USART接口将数据帧传给PC端。
5.在手机端能根据蓝牙获取的MPU6050六轴传感器的陀螺仪数据绘出立方体，立方体能在可接受的时间延迟内实时展现MPU6050的姿态变化（转动方向和角度）。
6.在PC端能通过对从USART接口获取的数据帧进行解析获取MPU6050加速度传感器和陀螺仪的数据，并根据数据帧中设置的校验位进行数据校验。
7.在PC端能根据解析出的加速度传感器和陀螺仪数据，在可接受的误差范围内还原MPU6050的姿态变化（包括位移、转动方向和角度），实现一个简单的惯性导航系统。
5.系统设计
5.1IIC总线工作原理
1
2
3
4
5
5.1
5.1.1总线的构成及信号类型
I2C总线是由数据线SDA和时钟SCL构成的串行总线，可发送和接收数据。
在CPU与被控IC之间、IC与IC之间进行双向传送，最高传送速率100kbps。
各种被控制电路均并联在这条总线上，但就像电话机一样只有拨通各自的号码才能工作，所以每个电路和模块都有唯一的地址，在信息的传输过程中，I2C总线上并接的每一模块电路既是主控器（或被控器），又是发送器（或接收器），这取决于它所要完成的功能。
CPU发出的控制信号分为地址码和控制量两部分，地址码用来选址，即接通需要控制的电路，确定控制的种类；控制量决定该调整的类别（如对比度、亮度等）及需要调整的量。
这样，各控制电路虽然挂在同一条总线上，却彼此独立，互不相关。
I2C总线在传送数据过程中共有三种类型信号，它们分别是：
开始信号、结束信号和应答信号。
开始信号：
SCL为高电平时，SDA由高电平向低电平跳变，开始传送数据。
结束信号：
SCL为低电平时，SDA由低电平向高电平跳变，结束传送数据。
应答信号：
接收数据的IC在接收到8bit数据后，向发送数据的IC发出特定的低电平脉冲，表示已收到数据。
CPU向受控单元发出一个信号后，等待受控单元发出一个应答信号，CPU接收到应答信号后，根据实际情况做出是否继续传递信号的判断。
若未收到应答信号，由判断为受控单元出现故障。
这些信号中，开始信号是必须的，结束信号和应答信号都可以不要，IIC总线时序图如图5.1.1-1所示。
图5.11IIC总线时序图
探索者STM32F4开发板板载的EEPROM芯片型号为24C02。
该芯片的总容量为256字节，通过IIC总线与外部连接。
STM32F4开发板有硬件IIC，但是设计的比较复杂，而且稳定性不好，所以我组使用GPIO软件模拟IIC来对24C02进行读写。
同时使用软件更具有移植性，只要有IO口，将软件移植过去就能使用模拟的IIC，而硬件必须MCU的支持。
5.1.2硬件设计
实现模拟IIC需要用到的硬件资源有：
串口（USMART）、GPIO、24C02。
图5.12STM32F4与24C02连接图
我组通过GPIO来模拟IIC，24C2的SCL和SDA分别连在GPIO_PB8和GPIO_PB9上，连接关系如图5.1.2-1。
5.2MPU6050工作原理
5.2
5.2.1MPU6050引脚
图5.21MPU6050结构图
模块外观如图5.2.1-2所示：
图5.22MPU6050实物图
图5.23MPU6050内部逻辑框图
如图5.2.1-1为MPU6050六轴传感器的结构图，总共有24个引脚，而图5.2.1-2为MPU6050的内部逻辑框图，描述了MPU6050内部的模块结构，以及各引脚的连接情况。
表5.2.11MPU6050引脚输出和信号描述
PinNumber
MPU-6050
PinName
PinDescription
1
Y
CLKIN
Optionalexternalreferenceclockinput.ConnecttoGNDifunused.
6
Y
AUX_DA
I2Cmasterserialdata,forconnectingtoexternalsensors
7
Y
AUX_CL
I2CMasterserialclock,forconnectingtoexternalsensors
8
/CS
SPIchipselect（0=SPImode）
8
Y
VLOGIC
DigitalI/Osupplyvoltage
9
AD0/SDO
I2CSlaveAddressLSB（AD0）;SPIserialdataoutput（SDO）
9
Y
AD0
I2CSlaveAddressLSB（AD0）
10
Y
REGOUT
Regulatorfiltercapacitorconnection
11
Y
FSYNC
Framesynchronizationdigitalinput.ConnecttoGNDifunused.
12
Y
INT
Interruptdigitaloutput（totempoleoropen-drain）
13
Y
VDD
PowersupplyvoltageandDigitalI/Osupplyvoltage
18
Y
GND
Powersupplyground
19,21
Y
RESV
Reserved.Donotconnect.
20
Y
CPOUT
Chargepumpcapacitorconnection
22
Y
RESV
Reserved.Donotconnect.
23
SCL/SCLK
I2Cserialclock（SCL）;SPIserialclock（SCLK）
23
Y
SCL
I2Cserialclock（SCL）
24
SDA/SDI
I2Cserialdata（SDA）;SPIserialdatainput（SDI）
24
Y
SDA
I2Cserialdata（SDA）
2,3,4,5,14,
15,16,17
Y
NC
Notinternallyconnected.MaybeusedforPCBtracerouting.
表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地址是0X68；如果接VDD，则是0X69。
注意：
这里的地址是不包含数据传输的最低位的（最低位用来表示读写）。
在探索者STM32F4开发板上，AD0是接GND的，即MPU6050的IIC地址是0X68（不含最低位）。
5.2.2硬件设计
图5.24MPU6050与STM32F4的连接电路图
从图5.2.2-1可以看出，MPU6050通过三根线与STM32F4开发板连接，其中IIC总线时和24C02以及WM8978共用，接在PB8和PB9上面。
MPU6050的中断输出，连接在STM32F4的PC0脚，不过本例程我们并没有用到中断。
另外，AD0接的GND，所以MPU6050的器件地址是：
0X68。
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）设置角速度传感器（陀螺仪）和加速度传感器的满量程范围
这一步，我们设置两个传感器的满量程范围（FSR），分别通过陀螺仪配置寄存器（0X1B）和加速度传感器配置寄存器（0X1C）设置。
我们一般设置陀螺仪的满量程范围为±2000dps，加速度传感器的满量程范围为±2g。
（4）设置其他参数
这里，我们还需要配置的参数有：
关闭中断、关闭AUXIIC接口、禁止FIFO、设置陀螺仪采样率和设置数字低通滤波器（DLPF）等。
本章我们不用中断方式读取数据，所以关闭中断，然后也没用到AUXIIC接口外接其他传感器，所以也关闭这个接口。
分别通过中断使能寄存器（0X38）和用户控制寄存器（0X6A）控制。
MPU6050可以使用FIFO存储传感器数据，不过本章我们没有用到，所以关闭所有FIFO通道，这个通过FIFO使能寄存器（0X23）控制，默认都是0（即禁止FIFO），所以用默认值就可以了。
陀螺仪采样率通过采样率分频寄存器（0X19）控制，这个采样率我们一般设置为50即可。
数字低通滤波器（DLPF）则通过配置寄存器（0X1A）设置，一般设置DLPF为带宽的1/2即可。
（5）配置系统时钟源并使能角速度传感器和加速度传感器
系统时钟源同样是通过电源管理寄存器1（0X1B）来设置，该寄存器的最低三位用于设置系统时钟源选择，默认值是0（内部8MRC震荡），不过我们一般设置为1，选择x轴陀螺PLL作为时钟源，以获得更高精度的时钟。
同时，使能角速度传感器和加速度传感器，这两个操作通过电源管理寄存器2（0X6C）来设置，设置对应位为0即可开启。
5.2.4相关寄存器
在读取MPU6050数据中主要用到了以下寄存器：
（1）PowerManagement1（电源管理寄存器1）
表5.2.41电源管理寄存器1各位描述
Register（Hex）
Register（Decimal）
Bit7
Bit6
Bit5
Bit4
Bit3
Bit2
Bit1
Bit0
6B
107
DEVICE_RESET
SLEEP
CYCLE
-
TEMP_DIS
CLKSEL[2:
0]
如表5.2.4-1，寄存器地址为0x6B。
DEVICE_RESET位用来控制复位，设置为1，复位MPU6050，复位结束后，MPU硬件自动清零该位；SLEEEP位用于控制MPU6050的工作模式，复位后，该位为1，即进入了睡眠模式（低功耗），所以我们要清零该位，以进入正常工作模式；TEMP_DIS用于设置是否使能温度传感器，设置为0，则使能；CLKSEL[2:
0]用于选择系统时钟源，选择关系如表5.2.4-2所示，默认是使用内部8MRC晶振的，精度不高，所以我们一般选择X/Y/Z轴陀螺作为参考的PLL作为时钟源，一般设置CLKSEL=001即可。
表5.2.42CLKSEL选择列表
CLKSEL[2:
0]
时钟源
000
内部8MRC晶振
001
PLL，使用X轴陀螺作为参考
010
PLL，使用Y轴陀螺作为参考
011
PLL，使用Z轴陀螺作为参考
100
PLL，使用外部32.768Khz作为参考
101
PLL，使用外部19.2Mhz作为参考
110
保留
111
关闭时钟，保持时序产生电路复位状态
（2）GyroscopeConfiguration（陀螺仪配置寄存器）
表5.2.43表陀螺仪配置寄存器各位描述
Register
（Hex）
Register（Decimal）
Bit7
Bit6
Bit5
Bit4
Bit3
Bit2
Bit1
Bit0
1B
27
XG_ST
YG_ST
ZG_ST
FS_SEL[1:
0]
-
-
如表5.2.4-3，寄存器地址为0x1B。
FS_SEL[1:
0]两个位用于设置陀螺仪的满量程范围：
0为±250°/s；1为±500°/s；2为±1000°/s；3为±2000°/s。
我们一般设置为3，即±2000°/s，而因为陀螺仪的ADC为16位分辨率，所以得到灵敏度为：
65536/4000=16.4LSB/（°/s）。
（3）AccelerometerConfiguration（加速度传感器配置寄存器）
表5.2.44加速度传感器配置寄存器各位描述
Register
（Hex）
Register
（Decimal）
Bit7
Bit6
Bit5
Bit4
Bit3
Bit2
Bit1
Bit0
1C
28
XA_ST
YA_ST
ZA_ST
AFS_SEL[1:
0]
-
如表5.2.4-4，寄存器地址为0x1C。
AFS_SEL[1:
0]两个位用于设置加速度传感器的满量程范围：
0为±2g；1为±4g；2为±8g；3为±16g。
我们一般设置为0，即±2g，而因为加速度传感器的ADC也是16位分辨率，所以得到灵敏度为：
65536/4=16384LSB/g。
（4）FIFOEnable（FIFO使能寄存器）
表5.2.45FIFO使能寄存器各位描述
Register
（Hex）
Register
（Decimal）
Bit7
Bit6
Bit5
Bit4
Bit3
Bit2
Bit1
Bit0
23
35
TEMP_FIFO_EN
XG_FIFO_EN
YG_FIFO_EN
ZG_FIFO_EN
ACCEL
_FIFO_EN
SLV2
_FIFO_EN
SLV1
_FIFO_EN
SLV0
_FIFO_EN
如表5.2.4-5，寄存器地址为0x23。
该寄存器用于控制FIFO使能，在简单读取传感器数据的时候，可以不用FIFO，设置对应位为0即可禁止FIFO，设置为1，则使能FIFO。
注意加速度传感器的3个轴，全由1个位（ACCEL_FIFO_EN）控制，只要该位置1，则加速度传感器的三个通道都开启FIFO了。
（5）SampleRateDivider（采样率分频寄存器）
表5.2.46陀螺仪采样率分频寄存器各位描述
Register
（Hex）
Register
（Decimal）
Bit7
Bit6
Bit5
Bit4
Bit3
Bit2
Bit1
Bit0
19
25
SMPLRT_DIV[7:
0]
如表5.2.4-6，寄存器地址为0x19。
该寄存器用于设置MPU6050的陀螺仪采样频率，计算公式为：
采样频率=陀螺仪输出频率/（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）
Bit7
Bit6
Bit5
Bit4
Bit3
Bit2
Bit1
Bit0
1A
26
-
-
EXT_SYNC_SET[2:
0]
DLPF_CFG[2:
0]
如表5.2.4-7，寄存器地址为0x1A。
加速度计和陀螺仪是根据数字低通滤波器（DLPF）的三个设置位进行过滤的，即DLPF_CFG[2:
0]。
DLPF_CFG不同配置对应的过滤情况如表5.2.4-8所示：
表5.2.48DLPF_CFG配置表
DLPF_CFG[2:
0]
加速度传感器
Fs=1Khz
角速度传感器
（陀螺仪）
带宽（Hz）
延迟（ms）
带宽（Hz）
延迟（ms）
Fs（Khz）
.98
8
001
184
2.0
188
1.9
1
010
94
3.0
98
2.8
1
011
44
4.9
42
4.8
1
100
21
8.5
20
8.3
1
101
10
13.8
10
13.4
1
110
5
19.0
5
18.6
1
111
保留
保留
8
加速度传感器输出速率（Fs）固定是1Khz，而角速度传感器的输出速率（Fs），则根据DLPF_CFG的配置有所不同。
一般我们设置角速度传感器的带宽为其采样率的一半，如前面所说的，如果设置采样率为50Hz，那么带宽就应该设置为25Hz，取近似值20Hz，就应该设置DLPF_CFG为100。
（7）PowerManagement2（电源管理寄存器2）
表5.2.49电源管理寄存器2各位描述
Register
（Hex）
Register
（Decimal）
Bit7
Bit6
Bit5
Bit4
Bit3
Bit2
Bit1
Bit0
6C
108
LP_WAKE_CTRL[1:
0]
STBY
_XA
STBY
_YA
STBY
_ZA
STBY
_XG
STBY
_YG
STBY
_ZG
如表5.2.4-9，寄存器地址为0x6C。
该寄存器的LP_WAKE_CTRL用于控制低功耗时的唤醒频率，项目中没有用到。
剩下的6位，分别控制加速度和陀螺仪的x/y/z轴是否进入待机模式，由于不需要进入待机模式，所以全部设置为0。
（8）GyroscopeMeasurements（陀螺仪数据输出寄存器）
表5.2.410陀螺仪数据输出寄存器各位描述
Register（Hex）
Register
（Decimal）
Bit7
Bit6
Bit5
Bit4
Bit3
Bit2
Bit1
Bit0
43
67
GYRO_XOUT[15:
8]
44
68
GYRO_XOUT[7:
0]
45
69
GYRO_YOUT[15:
8]
46
70
GYRO_YOUT[7:
0]
47
71
GYRO_ZOUT[15:
8]
48
72
GYRO_ZOUT[7:
0]
如表5.2.4-10，总共有6个寄存器，地址为0x43~0x48，通过读取这6个寄存器，就可以读到陀螺仪x/y/z轴的值，比如x轴的数据，可以通过读取0x43（高8位）和0x44（低8位）寄存器得到，其他轴以此类推。
（9）AccelerometerMeasurements（加速度传感器数据输出寄存器）
表5.2.411加速度传感器数据输出寄存器各位描述
Register
（Hex）
Register
（Decimal）
Bit7
Bit6
Bit5
Bit4
Bit3
Bit2
Bit1
Bit0
3B
59
ACCEL_XOUT[15:
8]
3C
60
ACCEL_XOUT[7:
0]
3D
61
ACCEL_YOUT[15:
8]
3E
62
ACCEL_YOUT[7:
0]
3F
63
ACCEL_ZOUT[15:
8]
40
64
A
升级会员 