基于MPU的INS惯性导航和实时姿态检测系统(完整资料)

基于MPU的INS惯性导航和实时姿态检测系统(完整资料）(可以直接使用，可编辑优秀版资料，欢迎下载）

基于MPU６050的INS惯性导航和实时姿态检测系统基于MPU的INS惯性导航和实时姿态检测系统(完整资料）(可以直接使用，可编辑优秀版资料，欢迎下载）项目目标及功能说明项目目标学习使用正点原子探索者开发板,并熟悉开发板上的MPU60５0六轴传感器的工作原理和各函数的调用过程.同时学习开发板的扩展接口，尝试在开发板上扩展蓝牙模块，并实现开发板与手机等含有蓝牙模块的电子设备通过蓝牙连接并进行数据的传输。在完成上述内容的基础上，实现将MＰU605０六轴传感器的加速度计和陀螺仪的数据传送到手机上，在手机上实现陀螺仪的变化效果展示。同时通过串口将MＰU６050数据传送到电脑上，通过Mａtｌab编程处理数据，实现惯性导航的简单展示。系统功能说明系统最主要的功能有两个：一个是在手机端能够展示开发板上MPU6050陀螺仪的姿态变化，通过一个立方体的转动来表示陀螺仪的转动；另一个是在电脑端能够读取ＭＰＵ60５0的数据,并通过对数据的处理还原数据中存储的MPＵ6050的姿态变化，简单展现出惯性导航的效果.在实现系统最主要的两个功能过程中,还需要实现一些基础功能.开发板能够通过蓝牙与手机连接并传输数据;开发板能够通过串口将数据发送出去;在电脑端能够读取开发板上串口输出的数据等。需求分析惯性导航系统用于各种运动机具中,包括飞机、潜艇、航天飞机等运输工具及导弹,然而成本及复杂性限制了其可以应用的场合.但是，存在一种情形:卫星一旦突然因故障、敌方打击或干扰（如太阳风暴)等原因无法提供服务，这对依赖GPS、北斗等卫星导航系统作为唯一PNT（Positioｎ、Naｖigatｉon、Ｔime)信息来源的系统来说可能是致命的灾难。作为目前为止卫星导航系统最好的备援—-惯性导航系统（INS），将于届时发挥出巨大的作用,其精度完全可以媲美ＧPS等卫星导航系统。并且它不需要外部参考就可确定当前位置、方向及速度,从而使它自然地不受外界的干扰和欺骗。定位、导航和授时服务对军队而言就像氧气对人类一样不可或缺，因此通过研究新机理、研制新设备、开发新算法,以摆脱人员和系统设备对GPS的依赖,具有极大的战略意义。姿态监测系统可广泛应用于关键资产姿态变化的无线实时监控。由于目前移动智能终端设备的数量和质量逐步提升，因此,通过计算机上传统的上位机软件进行姿态监测,逐渐暴露出了自身的缺点——串口传输无法实现无线监测、计算机相比智能终端便携性极差.因此，使用无线传输（蓝牙、红外、WＩFI、GSM等)的技术，开发一款在移动智能终端可以实时显示物体姿态的应用,具有很高的实用价值和广泛的市场应用前景。开发环境移动终端操作系统：ﻩAndroｉd4.4．４KｉtKaｔ计算机操作系统：Ｗiｎｄows８．1Proｘ６4串口开发：ﻩMＡTLABＲ2014a开发板IDE: KｅilｕVｉsioｎ5AndrｏidIDＥ：EcliｐseJａvaEEIDEforWebDevｅlopｅrsAｎdroiｄDｅvｅlopｍentTｏolkiｔ2３。0.4。14６8518项目进展情况到目前为止，我组已实现了以下功能：STM32F4开发板上MPU6050六轴传感器的数据获取并显示在ＬCD屏幕上。在LCD屏幕上绘出圆形图案,且圆形图案能根据MＰＵ6０50六轴传感器的姿态变化而运动，传感器倾斜角度越大，图案运动速度越快。扩展蓝牙模块，能通过蓝牙模块与手机连接并进行数据通信。根据函数提供的帧格式定义数据帧,并通过ＵSＡRＴ接口将数据帧传给PＣ端。在手机端能根据蓝牙获取的MPＵ60５0六轴传感器的陀螺仪数据绘出立方体，立方体能在可接受的时间延迟内实时展现MPU６0５0的姿态变化(转动方向和角度）。在PC端能通过对从USAＲT接口获取的数据帧进行解析获取ＭＰＵ6050加速度传感器和陀螺仪的数据，并根据数据帧中设置的校验位进行数据校验。在ＰC端能根据解析出的加速度传感器和陀螺仪数据,在可接受的误差范围内还原ＭPＵ６０５0的姿态变化(包括位移、转动方向和角度）,实现一个简单的惯性导航系统.系统设计IIC总线工作原理总线的构成及信号类型Ｉ2C总线是由数据线ＳＤA和时钟ＳCＬ构成的串行总线，可发送和接收数据。在ＣＰＵ与被控ＩC之间、IC与ＩＣ之间进行双向传送,最高传送速率100kbｐs。各种被控制电路均并联在这条总线上，但就像电话机一样只有拨通各自的号码才能工作，所以每个电路和模块都有唯一的地址，在信息的传输过程中,I2Ｃ总线上并接的每一模块电路既是主控器（或被控器），又是发送器(或接收器),这取决于它所要完成的功能。CPU发出的控制信号分为地址码和控制量两部分，地址码用来选址,即接通需要控制的电路,确定控制的种类;控制量决定该调整的类别(如对比度、亮度等）及需要调整的量。这样,各控制电路虽然挂在同一条总线上，却彼此独立，互不相关。I2Ｃ总线在传送数据过程中共有三种类型信号，它们分别是：开始信号、结束信号和应答信号。开始信号:ＳCL为高电平时,SDA由高电平向低电平跳变，开始传送数据。结束信号：ＳCＬ为低电平时,SDA由低电平向高电平跳变，结束传送数据。应答信号：接收数据的ＩC在接收到8ｂｉｔ数据后，向发送数据的IC发出特定的低电平脉冲，表示已收到数据。CPU向受控单元发出一个信号后,等待受控单元发出一个应答信号，ＣPU接收到应答信号后,根据实际情况做出是否继续传递信号的判断。若未收到应答信号，由判断为受控单元出现故障。这些信号中,开始信号是必须的，结束信号和应答信号都可以不要，IＩC总线时序图如图５.1。1—1所示。图STＹLEREＦ３\ｓ5。1SEQ图\*ARABＩＣ\s31IIＣ总线时序图探索者STM３2F4开发板板载的EEＰROM芯片型号为24C02.该芯片的总容量为256字节，通过ＩＩＣ总线与外部连接。SＴM32Ｆ４开发板有硬件IIＣ，但是设计的比较复杂，而且稳定性不好，所以我组使用GPIＯ软件模拟ＩIC来对２４C02进行读写。同时使用软件更具有移植性，只要有IO口,将软件移植过去就能使用模拟的IIＣ,而硬件必须MCU的支持。硬件设计实现模拟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上,连接关系如图5。1．2-1。MPU６050工作原理MＰU60５0引脚图STYＬEREF３\s５。2SEQ图\*ＡRABIC\s31MPU605０结构图模块外观如图５．２．1—2所示：图ＳTYLEREF3\s5.2SEQ图＼＊AＲABＩC\s32ＭＰＵ6050实物图图STＹＬEＲEF３＼s5。2SEＱ图\*ＡRＡBIC\s3３MPU６0５０内部逻辑框图如图5．2。1-1为MPＵ6050六轴传感器的结构图，总共有２4个引脚，而图5。2。1-2为MＰＵ6０50的内部逻辑框图,描述了ＭPU6050内部的模块结构，以及各引脚的连接情况.表SＴYLEREF４＼s5。2。1SEＱ表\＊AＲABIC\s41ＭPU60５0引脚输出和信号描述表5。２．1—1对每一个引脚的名称和作用进行了说明。在上述引脚中,SCL和ＳDA是连接MＣＵ的IIC接口，MＣU通过这个ＩIC接口来控制ＭPU6０50.另外还有一个IIＣ接口，连接的引脚为AＵＸ_CL和AUＸ_DA，这个接口可用来连接外部从设备，比如磁传感器，这样就可以与MPU6０５０组成一个九轴传感器.ＶLＯGIC是IO口电压，该引脚最低可以到1。8V，我们一般直接VDD即可.ＡD０是从IIＣ接口（接MＣＵ）的地址控制引脚,该引脚控制IIＣ地址的最低位,如果接ＧND，则MPU60５0的IIC地址是0Ｘ68；如果接VDD，则是0X69。注意：这里的地址是不包含数据传输的最低位的（最低位用来表示读写).在探索者STM32F４开发板上,AＤ0是接GND的,即MＰU60５0的IIC地址是0X６８（不含最低位）。硬件设计图STYLEREF3\s5。2SEQ图\＊ＡRＡBIC\s3４MPU6０50与STM3２F4的连接电路图从图5．2。２-１可以看出,MPU605０通过三根线与STM3２Ｆ4开发板连接，其中IIC总线时和２4C02以及WM89７8共用，接在PB8和PB9上面.ＭPU605０的中断输出,连接在STＭ３２F４的PC０脚，不过本例程我们并没有用到中断。另外，AD0接的GND，所以MPU６05０的器件地址是:0X68。初始化操作在使用ＳTＭ32F4读取MＰU6050的加速度和角度传感器数据之前,需要做以下初始化操作：初始化IIC接口MPU605０采用ＩＩC与STM３２F4通信,所以我们需要先初始化与ＭPU６05０连接的SＤA和ＳＣＬ数据线。复位MPＵ６050这一步让MＰU60５0内部所有寄存器恢复默认值,通过对电源管理寄存器１（0X6B）的bit7写1实现.复位后,电源管理寄存器1恢复默认值(0X40）,然后必须设置该寄存器为0X00,以唤醒MPＵ６050，进入正常工作状态。设置角速度传感器（陀螺仪)和加速度传感器的满量程范围这一步,我们设置两个传感器的满量程范围（FSR），分别通过陀螺仪配置寄存器（0X１Ｂ）和加速度传感器配置寄存器(０X１C)设置。我们一般设置陀螺仪的满量程范围为±２００0dps，加速度传感器的满量程范围为±2ｇ。设置其他参数这里,我们还需要配置的参数有：关闭中断、关闭AUXIIＣ接口、禁止FIFO、设置陀螺仪采样率和设置数字低通滤波器(DLPF)等。本章我们不用中断方式读取数据,所以关闭中断,然后也没用到AUXＩIC接口外接其他传感器，所以也关闭这个接口.分别通过中断使能寄存器（0X３8)和用户控制寄存器(0X６Ａ）控制。MPU6050可以使用FＩFＯ存储传感器数据,不过本章我们没有用到,所以关闭所有FIFO通道，这个通过FIFO使能寄存器(0Ｘ2３）控制，默认都是０（即禁止FIＦO）,所以用默认值就可以了。陀螺仪采样率通过采样率分频寄存器(０X１9）控制，这个采样率我们一般设置为50即可。数字低通滤波器（DLPF)则通过配置寄存器（0X1A）设置，一般设置ＤLＰF为带宽的1/2即可。配置系统时钟源并使能角速度传感器和加速度传感器系统时钟源同样是通过电源管理寄存器1（0X１B）来设置,该寄存器的最低三位用于设置系统时钟源选择,默认值是0（内部8ＭRC震荡），不过我们一般设置为1,选择ｘ轴陀螺ＰLL作为时钟源,以获得更高精度的时钟。同时，使能角速度传感器和加速度传感器，这两个操作通过电源管理寄存器２(0X6C）来设置，设置对应位为0即可开启。相关寄存器在读取MＰＵ605０数据中主要用到了以下寄存器:ＰoｗerＭaｎagement1（电源管理寄存器１)表STＹLEＲEF4＼ｓ5．2。4ＳEQ表＼*ＡRABＩC\s4１电源管理寄存器１各位描述如表5。2.4—１,寄存器地址为0ｘ６B。ＤＥVICＥ_RＥＳET位用来控制复位，设置为1,复位MPU605０,复位结束后,MPＵ硬件自动清零该位;SLＥEＥP位用于控制ＭPU6050的工作模式，复位后，该位为１，即进入了睡眠模式（低功耗），所以我们要清零该位，以进入正常工作模式;ＴEMP_ＤIS用于设置是否使能温度传感器，设置为０,则使能;CＬKSＥL［2:0]用于选择系统时钟源，选择关系如表5.2．４－2所示，默认是使用内部８ＭRC晶振的,精度不高,所以我们一般选择X/Ｙ/Z轴陀螺作为参考的PＬL作为时钟源，一般设置Ｃ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：０]两个位用于设置陀螺仪的满量程范围：０为±2５0°/s;1为±50０°/s;２为±１００0°／s；3为±２000°/s.我们一般设置为3，即±2000°/s,而因为陀螺仪的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]两个位用于设置加速度传感器的满量程范围：0为±2g；1为±4g;2为±８g;３为±１6g。我们一般设置为0，即±2ｇ，而因为加速度传感器的ADC也是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Ｏ使能,在简单读取传感器数据的时候,可以不用FIFO，设置对应位为０即可禁止FＩFＯ，设置为1，则使能FＩFO。注意加速度传感器的3个轴,全由1个位(ACＣEL_FIFO_EN)控制，只要该位置1，则加速度传感器的三个通道都开启ＦIFＯ了。SampleRateDividｅr(采样率分频寄存器)表STYＬEREF4＼ｓ５。2．４SEQ表\＊ＡRABＩＣ＼s４6陀螺仪采样率分频寄存器各位描述如表5。2．4—6，寄存器地址为0x１9。该寄存器用于设置MＰU6０5０的陀螺仪采样频率，计算公式为：采样频率＝陀螺仪输出频率/（1+SMPＬRT_DＩV）这里陀螺仪的输出频率,是1Khz或者8Ｋhｚ，与数字低通滤波器(DＬPF)的设置有关,当DLＰF_CFG=0／7的时候，频率为8Ｋhz,其他情况是1Khz.而且DLPF滤波频率一般设置为采样率的一半。我们假定设置采样率为5０Ｈz，那么SMPＬＲT_ＤIV=１00０/50—1=19。Cｏnｆiｇurａtioｎ（配置寄存器）表STYＬEREF4＼ｓ5。2．４SＥQ表\*ＡＲABIC＼s47配置寄存器各位描述如表5.2。4-７,寄存器地址为０x1A。加速度计和陀螺仪是根据数字低通滤波器（DLPF)的三个设置位进行过滤的，即DLPF_ＣＦG[2:0].DLＰＦ_CFＧ不同配置对应的过滤情况如表5．2。4—8所示：表SＴYLEREF4\s5。2.4SEＱ表\＊ARＡBIＣ\ｓ4８DLPＦ_CＦG配置表加速度传感器输出速率(Fｓ)固定是1Khｚ,而角速度传感器的输出速率(Ｆs），则根据DLPF_CFG的配置有所不同。一般我们设置角速度传感器的带宽为其采样率的一半,如前面所说的,如果设置采样率为50Hz，那么带宽就应该设置为２5Hｚ，取近似值20Hz，就应该设置Ｄ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用于控制低功耗时的唤醒频率,项目中没有用到.剩下的6位,分别控制加速度和陀螺仪的x/y/z轴是否进入待机模式，由于不需要进入待机模式，所以全部设置为０。ＧyｒｏscｏpeＭeasurements（陀螺仪数据输出寄存器）表STYLＥＲEF4\ｓ5．2。4SEQ表＼＊ＡRABIＣ＼ｓ4１0陀螺仪数据输出寄存器各位描述如表5。２．４—10,总共有６个寄存器，地址为0x43～0ｘ４8，通过读取这6个寄存器,就可以读到陀螺仪ｘ/y／z轴的值，比如x轴的数据，可以通过读取0ｘ43(高8位)和0x４4(低8位）寄存器得到,其他轴以此类推。AｃcelerometerMeasurementｓ（加速度传感器数据输出寄存器）表STＹＬERＥF4\s５。2．4ＳEQ表\*ARＡBIＣ\ｓ4１1加速度传感器数据输出寄存器各位描述如表5．2.4-1１，总共有6个寄存器，地址为0ｘ3B~０x40，通过读取这８个寄存器，就可以读到加速度传感器x/ｙ/z轴的值,比如读ｘ轴的数据，可以通过读取０x3B(高8位)和0x3C(低８位）寄存器得到，其他轴以此类推.ＡTK－HＣ0５工作原理ＡＴK－HC05引脚AＴＫ—ＨC０5模块非常小巧（1６ｍｍ*32ｍm)，模块通过6个2。54ｍm间距的排针与外部连接，模块外观如图５.３。1-1所示：图STＹＬEREF3\s5。3SEQ图\＊ＡRABIC＼s31ATK－HC05模块外观图图５．3.1—１中，从右到左，依次为模块引出的PＩN1～PIN6脚，各引脚的详细描述如表5.３.1—1所示:表STＹＬEREF4\s5.３。１SEQ表＼＊AＲABIC\s41ＡTK—HC０5模块各引脚功能描述另外，模块自带了一个状态指示灯：SＴA.该灯有3种状态,分别为:在模块上电的同时（也可以是之前），将KEＹ设置为高电平（接ＶCＣ），此时STＡ慢闪（1秒亮１次）,模块进入AT状态，且此时波特率固定为384００.在模块上电的时候,将KEＹ悬空或接GND，此时ＳＴA快闪（1秒2次）,表示模块进入可配对状态.如果此时将KEY再拉高，模块也会进入AT状态,但是ＳＴA依旧保持快闪。模块配对成功，此时SＴA双闪（一次闪2下,2秒闪一次）。硬件设计图ＳTYLEREF3＼s５.3SEＱ图\*ＡRＡBIC\s3２ＡＴK—ＨＣ０５蓝牙串口模块原理图模块与单片机连接最少只需要4根线即可：ＶCC、GND、TＸD、RXD、VCC和ＧND用于给模块供电，模块TXD和RＸＤ则连接单片机的RXＤ和TXＤ即可。该模块兼容5Ｖ和3。3Ｖ单片机系统,所以可以很方便的连接到系统里面去。ＡＴK－HC05模块与单片机系统的典型连接方式如图５。3。2—２所示：图STＹLERＥF3＼s5.3ＳEQ图＼＊ＡRABIC\ｓ33AＴK-HC０５模块与单片机系统连接示意图图中虚线连接表示可有可无，可以根据需要，选择性的使用。ATK－HC0５与蓝牙主机连接首先，开机检测ATK－HC05蓝牙模块是否存在，如果检测不成功,则报错。检测成功之后，显示模块的主从状态，并显示模块是否处于连接状态，ＤS0闪烁，提示程序运行正常.按KEY0按键,可以开启/关闭自动发送数据（通过蓝牙模块发送）；按KＥY_UP按键可以切换模块的主从状态。蓝牙模块接收到的数据,将直接显示在LCD上（仅支持ASCＩＩ字符显示).同时,还可以通过USMART对AＴK—ＨC0５蓝牙模块进行AＴ指令查询和设置。结合手机端蓝牙软件（蓝牙串口助手v１．97.ａpk)，可以实现手机无线控制开发板（点亮和关闭LEＤ１）。所要用到的硬件资源如下:指示灯DS0、DＳ1KEY０／KEY_UＰ两个按键串口１、串口3TFTLCD模块AＴＫ-ＨC05-V１３蓝牙串口模块前面介绍了ATK—HC05蓝牙串口模块的接口，而AＬIEＮTEK探索者STM3２F４07开发板板载了一个ATＫ模块接口(ATＫＭODULE），ATK—HC05蓝牙模块可直接插入该接口实现与探索者STM32Ｆ４开发板的连接。AＴＫＭOＤULE同开发板主芯片的连接原理图如—１所示:图STYLＥREF３\ｓ5。3SＥQ图\*ARABIC\s34ATK-ＭODUＬE接口与MCU连接关系从上图可以看出，蓝牙模块的串口最简单的办法是连接在开发板的串口３上面，只需要用跳线帽短接P１0的USＡRT3_ＲＸ和GＢC_TX以及ＵSＡRT3_ＴＸ和GＢC_RX即可实现.连接好之后,探索者STＭ32Ｆ4０７开发板与ATK—HC05蓝牙模块的连接关系如表2.1所示:表STYLERＥF4＼s5.３．３SEQ表\*ARAＢIＣ\s41ATK—HＣ05蓝牙模块同探索者STM32F４0７开发板连接关系表使用时，我们只需要将ＡTK—HＣ０5蓝牙模块插入到开发板的ＡTK—MODULE接口即可,如图５．3.3－2所示：图STＹLEREＦ３＼s５．３SＥQ图\*ＡＲＡBＩC\s３５ATＫ-HC05蓝牙模块与探索者开发板对接实物图注意,连接好之后,记得检查Ｐ１0的跳线帽：必须短接：ＵＳART３_RX和GBC_TX以及USART３_TX和GＢC_ＲX.另外，在实际使用的时候,如果不需要进入AT设置和状态指示，则连接蓝牙模块只需要4根线连接即可：ＶＣC／GND／TXD／RXD。ＡTK-HC05模块可以与多种蓝牙主机设备连接,这里仅以智能手机为例，进行说明。首先,在手机上安装：BTClient。apk,该应用可以在提供的资料里面找到.安装完应用后，我们打开该应用,如图5.3。3—３所示：图STYLEREＦ３\s5.３SEQ图\*ＡＲＡBIC\s36ＢTClｉeｎt.ａpk运行界面进入搜索蓝牙设备界面，如图5.3．3—4所示:图STYLEREF3\s5．3SEQ图＼＊ＡＲABIC＼s3７搜索蓝牙设备从上图可以看出，手机已经搜索到蓝牙模块了,HC-０5,点击这个设备，输入默认密钥１234(仅第一次连接需要设置),完成配对,如图5。３．3-5所示：图STＹLＥＲEF3\s5．3SＥＱ图＼*ARABIC\s38输入配对密码在输入密码之后,等待一段时间，即可连接成功，如图5。３。3-6所示：图SＴYLEREF３＼s5。3SEQ图\＊ARABＩC\ｓ39连接成功此时,手机和蓝牙模块就连接上了，手机便可以接收数据并绘制三维图形了,改变开发板的姿态，手机端绘制的三维图形如图5．３。3—７所示:图ＳTYLEREＦ3\s5。3SEQ图\＊ＡRAＢIC＼s31０手机绘制三维图形界面这样,我们就实现了AＴK—ＨC0５模块与手机的连接。同其他蓝牙主机设备的连接，方法都是类似的,比较简单，这里我们就不再介绍了。有了STA指示灯,我们就可以很方便的判断模块的当前状态,方便使用。DMP算法原理通过陀螺仪数据输出寄存器和加速度传感器数据输出寄存器获取的是ＭＰU605０加速度传感器和陀螺仪的原始数据,而实验需要的是姿态数据,即欧拉角:航向角（Ｙaw)、横滚角（Rｏll）和俯仰角（Piｔch）。要得到欧拉角数据，就得利用我们的原始数据,进行姿态融合解算，这个比较复杂，知识点比较多，而MＰＵ605０自带了数字运动处理器，即DMP,并且，ＩｎvenSｅnse提供了一个MPU60５０的嵌入式运动驱动库，结合MPU6050的DMＰ，可以将我们的原始数据，直接转换成四元数输出，而得到四元数之后，就可以很方便的计算出欧拉角，从而得到Yaw、Rｏll和Ｐiｔｃｈ.使用MＰU6０50的DMP输出的四元数是q３０格式的，也就是浮点数放大了2的30次方倍。在换算成欧拉角之前，必须先将其转换为浮点数，也就是除以2的３0次方,然后再进行计算，计算公式如下:其中ｑuat［0］～ｑuat[3］是ＭＰＵ60５0的DMＰ解算后的四元数,为q30格式，所以要除以一个2的30次方，其中q30是一个常量为１０7３7４1８24，即2的3０次方，然后带入公式，计算出欧拉角。上述计算公式的57．3是弧度转换为角度，即１80/π，这样得到的结果就是以度(°）为单位的。ｑ0＝qｕat[0］/q３０;//ｑ30格式转换为浮点数q1=quaｔ[1]／q30;q2＝quat［2］/q３0;q３＝quａt［3］/q30；//计算得到俯仰角/横滚角／航向角Ｐitch=ａｓｉn(－2＊q１*q３+２*ｑ0＊ｑ２）＊57。3;//俯仰角Roｌl=ａtan2(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称为航姿参考系统，包括多个轴向传感器，能够为飞行器提供航向，横滚和侧翻信息，这类系统用来为飞行器提供准确可靠的姿态与航行信息。图ＳＴYLEREF3\s５.５ＳEQ图\＊ＡRABIC\s3１航姿参考系统航姿参考系统包括基于MEMS的陀螺仪，加速度计和磁强计。航姿参考系统与惯性测量单元IMU的区别在于，航姿参考系统(ＡHRS）包含了嵌入式的姿态数据解算单元与航向信息，惯性测量单元(IＭU）仅仅提供传感器数据，并不具有提供准确可靠的姿态数据。目前常用的航姿参考系统（AHＲＳ)内部采用的多传感器数据融合进行的航姿解算单元为卡尔曼滤波器。航姿参考系统(AHRS）具有两个主要特点:高精度360度全方位位置姿态输出，但采用欧拉角的会具有万向锁，不能全向转动。高效的数据融合算法快速动态响应与长时间稳定性（无漂移，无积累误差）相结合.航姿参考系统（ＡHRS)输出模式为三维全姿态数据(四元数/欧拉角／旋转矩阵）。AHRS软件设计ＡHRS的软件设计主要分为：传感器初始化，包括设置传感器的更新速率、量程.初始化卡尔曼滤波的相关矩阵，根据传感器的特点设置过程激励噪声协方差矩阵Q,设为对角元素为０.1的四维对角方阵.若成功读取陀螺仪数据，进行卡尔曼滤波的时间更新。采集加速度传感器和磁阻传感器的数据，若读取成功则进行观测更新.加速度观测更新与磁场观测更新算法差别在于观测方差的Ｒ,可根据两种传感器的置信度没置相应的值,航向姿态参考系的程序流程如图5。5.２－1所示。图ＳTYLＥREF3\s5。５SEQ图＼＊ＡRＡBIＣ＼s32航向姿态参考系的程序流程手机端功能原理及Anｄｒｏｉd代码对手机端aｐk的修改主要是将两个独立的安卓ａpｋ进行了源码级的合并。蓝牙帧处理蓝牙模块方面，使用了开源的蓝牙串口通讯代码。在进行合并的过程中，主要对代码的接收模块进行了修改。由于蓝牙的发送和接收以及Android广播中传递的均为字符串，因此,为了解析方便，我们将蓝牙帧设置为如下格式： u3_prｉnｔｆ（”P：％c％。1fR:％ｃ%．１fＹ：%c％.１ｆ\ｎ"，cp，Ｐitｃh，ｃr，Roll,cｙ,Ｙaｗ)；其中，cｐ、cr、cｙ分别为Piｔch、Ｒoｌl、Ｙaw的符号，为非负数时显示为空格符.这里注意到在进行格式化输出的时候,控制小数点后保留一位小数，但是小数点前的位数无法控制,猜测是由于c语言头文件功能较为精简造成的。参数的广播传递为了实现参数传递的功能,查阅了大量Ａｎdrｏid相关的代码和资料，最终选择了通过Androｉｄ特有的广播机制进行参数的传递.此外，在广播的过程中,还是用了意图（Intｅｎt），这也是Andｒoid特有的一个机制,用来对广播进行定向生成和接收，保证三维制图端收到的数据为蓝牙接收到并进行广播的数据。另外在对蓝牙数据进行接收时，发现无法理想地收取到单个帧数据，大部分时候是一次性获取到了几十帧的数据，其中第一帧和最后一帧往往是不完整的,这就要求我们对数据进行处理之后再进行广播：//接收数据线程while（ｔrｕe）{num=ｉs.read(buffｅr);//读入数据n=0;Ｓtrings0=nｅｗStｒｉｎg（ｂｕffer，0,nuｍ);ｆmｓｇ+=ｓ0；//保存收到数据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ｇ")；//设置广播的action//只有和这个ａction一样的接收者才能接收广播；sｅｎdBroadcast(intenｔ)；/／发送广播 }ﻩ}ﻩtｏSeｎd="";／／处理完成,清空广播缓存｝if（is。ａｖailable(）==0)ｂreak;/／短时间没有数据才跳出进行显示｝//广播接收类ｐ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ｄ操作系统的进程管理和线程创建进行了学习，最终能够实现从蓝牙接收视图到三维绘制视图的平滑切换.切换的时机最终选择在蓝牙连接建立后，开始接收数据的瞬间：／/打开接收线程try{is=＿socｋｅt。ｇeｔＩｎｐuｔＳｔｒeam();//得到蓝牙数据输入流｝caｔch（IＯEｘceptｉoｎe）｛Toaｓt．mａkeText(ｔhis，”接收数据失败！”，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ＧＬ绘图三维绘图方面，使用了官方提供的示例代码,主要在每一帧绘制的过程中加入了参数传递的功能,从而实现了实时从蓝牙接收数据并绘制三维图形：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ＴＨ);//实例化接收类testReceｉｖｅr=newTestReceiver（）；｝ｐｕblicvoiｄoｎDrａwFramｅ(ＧL1０gl)｛/／TODOAutｏ-gｅnｅrａtedmｅthｏdstuｂｇｌ．glClｅar（GL１0。GL_CＯLＯＲ_BUFFER＿ＢIＴ｜GL１0.GL_ＤEＰTH_BＵFＦＥR_BIT）； //获取广播的数据StrｉngalｌＳtrｉng=testReｃeiver。ｇeｔSｔring（)；ﻩ／/对广播数据进行解析,从字符串转化为fｌoat类型if（aｌlStrｉnｇ。lｅngtｈ()>=２0)｛Pitcｈ=Float.parseFloａt(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.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Ｓｔａte(GL1０.ＧL_VＥRTEX_ARRAY)；gｌ。glRotａtef（ｘ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); //将读取到的欧拉角赋值给三维图形的旋转角xrｏt=Ｐiｔcｈ；yｒot＝-Roｌｌ;zrot=Yaw；}系统测试由于三维电子旋转台和加速度测量仪价格过于昂贵，因此,只是利用Maｔｌab软件进行了简单的系统测试。AHＲS测试我组对从串口获取的ＭPU6050数据进行了处理,获得到浮点型数据，并对使用AＨRS和不使用AＨRS进行了测试比较，由于数据量较大，这里我们用图像展示,结果如下：图STＹLＥRＥF3＼s６。１SＥＱ图\*AＲABIC\s31未进行ＡHRS处理得到的姿态变化图ＳＴYLＥRＥＦ3\ｓ6。1SEＱ图\*AＲＡＢＩC＼s32AHRS处理后得到的姿态变化由图可看出，不使用AHRＳ的数据绘出的图像与实际姿态完全不对应，误差较大。而使用了AHＲS处理的数据较稳定，且比较符合实际姿态。高通滤波算法根据我组需要实现的还原MＰU6０50六轴传感器姿态变化的功能,我们需要对获取的MPＵ6０5０加速度传感器数据进行处理，通过二次积分得到MPU60５０的姿态的位移。由于加速度传感器的输出存在固定的零点漂移，即当实际加速度为０时，传感器输出并不一定为0，而是一个非零输出。若设传感器的输出值为+，其中为实际加速度,则在对传感器输出值的误差进行二次积分后会产生积分累计效应,导致误差更大。同时，若传感器初始化时加速度不为０，也会导致二次积分后产生积分累计效应。通过查阅资料,高通滤波可以解决零点漂移问题，我组选择使用高通滤波处理数据,而Mａtｌab提供了高通滤波函数filtfilt（），我组调用函数处理的主要代码如下：%％Ｈ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(）为分段函数，将数据分段进行滤波;plot（）为绘图函数。我们对使用高通滤波和不使用高通滤波进行了测试比较，由于数据量较大，我们以图像的形式展现，结果如下图：图STYLＥREF3\s6。2ＳEQ图\＊ARＡＢＩＣ\s31未进行高通滤波处理得到的速度变化图ＳTＹLEREF3＼ｓ6.2ＳEＱ图＼*ＡＲＡBＩC\s32高通滤波处理后得到的速度变化如图6。2－2可看出，不使用高通滤波处理得到的数据由于零点漂移导致的二次积分误差，趋于正向最大或负向最小;而使用高通滤波处理得到的数据趋于稳定，且动作被放大，变得更明显。实时姿态检测系统测试当时演示时,提出要将手机固定在板上进行测试，结果是偏差非常大,这与实际情况有很大差别。后来找到了原因，当时代码的编写是将数据作为正反馈传递给绘图程序的,而现在为了检验精度，需要改为负反馈，同时，为了便于观察，将3D图形替换为了一条直线，代码改变如下(原始代码已被注释)：//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＝Yaｗ;*/如图，为测试结果，可以看到误差逐渐变大，最后无法复位:关键代码说明IＩC关键代码初始化ＩＩ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Ｂ时钟//GＰIOB8,B９初始化设置GPＩO_IniｔStructuｒｅ。GPＩO_Piｎ=GＰIO＿Pｉｎ_8｜ＧPＩO_Piｎ_９；GPIＯ＿ＩnitSｔructｕrｅ。GPIＯ_Ｍode=ＧPIO＿Modｅ_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ｔ函数可看出，函数主要对GPIO的ＰB8、PB9引脚进行了初始化设置。先使能ＧPIＯＢ时钟,然后对PB8、PB9引脚的相关寄存器进行初始化设置，设置完毕后通过GＰIO＿Init函数将初始化设置有效，最后将SCL和SＤA使能。数据处理/／ＩO方向设置#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两个函数，通过设置ＰB9引脚功能来设置IIC_ＳDＡ接口是输入模式还是输出模式。有了这两个函数之后,在IIC函数文件中，就能够使用ＩＩC_Senｄ＿Bｙtｅ和IIC＿Ｒead＿Ｂyte函数来发送或读取数据。具体代码如下：//ＩIＣ发送一个字节//返回从机有无应答//1，有应答//0，无应答vｏｉdIIＣ_Ｓend_Byte（ｕ８tｘｄ）{u8t;SDＡ＿OUT（);IIC＿SＣL=０;/／拉低时钟开始数据传输for(t=0；t<8；t＋+)｛ＩIC＿SDA＝（txd&0ｘ8０）〉〉７;ｔxd<<=１；delay_us（２);／/对ＴEA5767这三个延时都是必须的IIC_ＳCL=1；delａｙ_us（２)；IIC_SCＬ=0;dｅlａy_ｕs（2)；｝｝//读１个字节，ack=1时,发送AＣK，ack=0,发送nAＣKu８IＩＣ_Ｒｅad_Ｂｙｔｅ(unsignedcｈaｒack）｛unsigｎｅdｃhaｒi，receiｖｅ=0;ﻩＳDA_ＩN(）；／/SDA设置为输入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ｋ（);／/发送nＡCKelsｅIIC_Acｋ（);//发送AＣKretuｒnreceｉｖe；}MPＵ6050关键代码初始化根据5.2。3中说明的,对MPU60５０初始化，需要先初始化IＩC接口，然后复位ＭPU60５0，接着设置相关参数，最后配置系统时钟源并使能加速度传感器和陀螺仪。具体代码如下：//初始化MPU６０５0//返回值:0，成功／/其他，错误代码u8ＭＰU＿Init(void）｛ u８res；ﻩＩIC_Iｎit（)；//初始化ＩIＣ总线 MPＵ_Writｅ＿Byte(MＰU_ＰＷR＿MGMＴ１＿REG,０X８0)；ﻩ／/复位Ｍ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０）；／/设置采样率５０Ｈｚ ＭPU＿Wriｔｅ＿Byte（MPU_ＩＮT＿EＮ_REＧ，０X00）;//关闭所有中断ﻩMPU＿Ｗrite_Ｂyte(MPU_USER_CTRＬ_REG,0X00）；／/I2C主模式关闭 MPU_Wrｉtｅ_Ｂyｔe(ＭPU＿FIＦO_EN_RＥG，0X00);//关闭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）; //设置ＣLKSEL，PLLX轴为参考 MPU_Write_Bｙte（MＰU_PWＲ＿MGMT2_REG,０X00);ﻩ//加速度与陀螺仪都工作 MPU_Ｓet_Rate（5０）；//设置采样率为5０Hｚ ｝elｓｅretuｒn1;ﻩretｕrn0;｝设置寄存器在MPU６050初始化中，需要设置角速度传感器(陀螺仪）和加速度传感器的满量程范围以及采样率,而根据5。2.4需要通过修改寄存器来设置，所以用到MＰＵ_Ｗrite_Byte函数来对寄存器进行写操作。具体代码如下://设置ＭPU６０５０陀螺仪传感器满量程范围//ｆsｒ:0,±２50ｄｐｓ；1，±500ｄps;2，±1000ｄps;3,±2０00ｄps//返回值：0，设置成功//其他，设置失败u8MPU_Set_Gyro_Fsr（u８fsr）{ returnMPU_Writｅ_Bytｅ(MＰＵ_GYRO_CＦＧ_REG，fsｒ〈〈３）；//设置陀螺仪满量程范围}/／设置MPU6050加速度传感器满量程范围//fsｒ:0,±2g；1，±4g；2,±８g;3，±１6g//返回值:0，设置成功//其他，设置失败u8MＰU_Ｓeｔ_Accel_Ｆsr(u８fｓｒ){ﻩｒeturｎMＰU_Ｗriｔe_Byte(MPU_ACCＥL_CFＧ_ＲEG，fsr＜<3）；／／设置加速度传感器满量程范围｝／/设置MPU6050的采样率（假定Fs=１KＨz）//rate:4～1000（Hz）／/返回值：0，设置成功//其他，设置失败u8MＰＵ＿Set_Raｔe(u1６rａte）｛ﻩ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ａta);ﻩ//设置数字低通滤波器 returｎMPＵ_Seｔ_LPF(rate／2)；/／自动设置ＬPF为采样率的一半}从数据输出寄存器读取数据在MPＵ6050初始化完成后，需要通过加速度传感器数据输出寄存器和陀螺仪数据输出寄存器来读取加速度传感器数据和陀螺仪数据,所以用到MPU_Reａd_Len函数来对寄存器进行读操作。具体代码如下：／/得到陀螺仪值（原始值)//gx，gｙ,gz：陀螺仪x，ｙ，z轴的原始读数（带符号）//返回值: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轴的原始读数（带符号）／/返回值：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Ｐ关键代码初始化mｐu＿ｄmｐ＿inｉt（)是MＰＵ6０５0DMP初始化函数,该函数具体代码如下：//mｐｕ6050,ｄmp初始化//返回值：0,正常//其他，失败u8mpu＿dmp_init（voiｄ)｛u8rｅs=0；IIC_Inｉt（）；//初始化IIC总线if(mpu_ｉniｔ（）=＝0）｛/／初始化MPU605０//设置需要的传感器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Ｌ)；//设置ＦIFＯｉf（res）reｔurｎ2;rｅs=mpu_ｓet_sample_rate(DＥFAULＴ＿MＰU＿HZ）;／/设置采样率ｉf（ｒｅｓ)reｔｕｒn３;reｓ=dmp＿loaｄ_mｏｔｉoｎ_driver_firｍwａre（)；／/加载dmｐ固件if(rｅs)retｕrｎ4；//设置陀螺仪方向res＝dmp_sｅｔ_oriｅntａｔｉon（inv_oriｅntation＿ｍａｔｒix_to＿scaｌaｒ(gyｒo＿orientaｔｉon)）；iｆ（ｒes）ｒｅturn5;//设置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；/／设置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ｎ０；｝函数首先通过ＩIC_Inｉt（需外部提供）初始化与MPU6０５0连接的IＩＣ接口,然后调用mpｕ_ｉnｉｔ函数，初始化MＰU60５0,之后就是设置DMP所用传感器、FIFO、采样率和加载固件等一些列操作，在所有操作都正常之后，最后通过mｐu＿sｅt_dｍp_state(１）使能DMP功能,在使能成功以后才可以通过mｐｕ_dmp_ｇet_datａ来读取姿态解算后的数据。读取数据mpｕ_dmp_get_data(）函数用于得到ＤMＰ姿态解算后的俯仰角、横滚角和航向角。具体代码如下：//得到dｍp处理后的数据(注意,本函数需要比较多堆栈，局部变量有点多）/／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＊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格式转换为浮点数q1=ｑuａt［１］/ｑ３0；ｑ２=quaｔ［2］/q30；q3=quat[３]/ｑ３0;//计算得到俯仰角/横滚角/航向角*pitcｈ=ａsｉn(—２＊q1＊ｑ3+２＊q0＊q2)*57．3;＊roll＝ａtan２（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关键代码fuｎctioｎobｊ=Updａte(ｏ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ｕａteｒnＰrod(q,quａternＰ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ｅ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ａternｉonqDoｔ=０.5＊quａteｒｎPrｏd（q，[０Gyｒoｓcopｅ(1）Ｇyrｏｓcope（2)Gyrｏscope（3)］）；%Integｒaｔeｔoyieｌdquａternionq=ｑ+qDot＊obｊ。ＳamｐｌePeriｏｄ;ｏbj。Qｕａｔernion=q/noｒm(ｑ)；％normaliｓequaｔerniｏｎend卡尔曼滤波四维扩展卡尔曼滤波算法扩展卡尔曼滤波算法(EｘtendｅｄＫａｌmaｎFilteｒ,EＫF）是一套由计算机实现的实时递推算法，所处理的对象是随机信号,利用系统噪声和观测噪声的统计特性，以系统的观测量作为滤波器的输入，以所要求的估计值(系统的状态变量)作为滤波器的输出,滤波器的输入和输出由时间更新和观测更新算法联系在一起,根据系统的状态方程和观测方程估算出所需要处理的信号。AＨＲS扩展卡尔曼滤波算法的状态变量采用四维四元数,与采用欧拉角相比,避免了采用欧拉角计算时涉及的大量三角函数运算,保证了更新速率和实时性，同时不存在采用欧拉角运算出现的奇异性.欧拉角与四元数的转换关系如下:θ=-asin2qφ=atan2Ψ=atan22四元数微分方程如式（4）所示:q1四元数姿态矩阵微分方程只要解4个微分方程，比方向余弦姿态矩阵微分方程减少了大量的运算，便于微处理器的编程实现。时间更新系统的状态方程如式（５）所示。Xk=AXk-1其中状态变量为四元数X=q0，q1，qA=1-状态变量的时间更新如式(7）所示。Xk=AXk-1协方差矩阵Pk预测如式(８)所示，式中Pk=APk-1观测更新AＨRS的观测更新是通过本体坐标系上的重力加速度和地磁场的参考矢量旋转至导航坐标系上，再与加速度和磁场传感器比较,得到观测变量的残余。由本体系转换至导航系的转移矩阵由四元数可以表示为式(9）。Cnb三位参考向量v转移至导航系中可有观测方程式（10）表示h=Cnb当重力加速度观测更新时参考向量v等于重力加速度参考矢量（可设置为当平台静止水平放置时,加速度计测量得到的三维矢量为:v=当磁场观测更新时v等于磁场参考矢量（可设置为当平台静止水平放置且航向指向正北时，磁阻计测量得到的三维矢量为：v=Ｈ是h对Ｘ求偏导的雅可比矩阵，如式(11）所示。Hi,j=∂卡尔曼增益矩阵KkKk=Pk观测更新:Xk=Xk当重力加速度观测更新时zkz当磁场观测更新时zkz协方差更新:Pk=(1-KkＡＴK-HＣ０5关键代码初始化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时钟ﻩGPIO_IｎｉtＳtructure.GＰIO_Pin=GPIO