基于MPU9250的示教航行姿态研究

彭琰举，宋文学，王晋，王鹏

（西安航空学院机械工程学院机电技术研究所陕西省泵类装备工程研究中心陕西西安710077）

随着科学技术的不断发展，基于PC系统的虚拟仪器仪表被广泛应用于工业及军事领域中。其中，可输出高精度的位置、速度、姿态、角速度、加速度和时间等信息的航姿测量仪最为常见。但这类仪器多采用惯性导航技术和组合导航技术，往往内置了价格较为昂贵的高精度光纤陀螺、石英加速度计及多模卫星导航接收机等设备[1-3]。

在教学中，若采用此类仪器作为示教仪器，将会增加教育成本。文中所研制的仪器系统采用高度集成的数字式加速度计、磁场计、陀螺仪传感器设计的AHRS航姿参考系统，既具备常见航测量仪的功能，又更大地降低了教学成本，与此同时该系统又能被扩展至各类无线设备中，具有较高的市场价值和应用前景[4-5]。

1 系统总体框架设计

系统总体框架设计方案如图1所示，本系统分为两部分，图 1（a）为服务端，图 1（b）为客户端。服务端主要由USB调试模块、DC/DC、LDO，电池模块、无线模块ESP8266、航行姿态检测模块MPU9250等部分组成。客户端主要由PC、CH340[6]、模块ESP8266组成。

图1 系统逻辑框图

其中ESP8266模块采用了Tensilica公司的Xtensa® 32-bit LX6处理器[7-9]，它在封装为QFN32的芯片上集成了主频达160 MHz的IP内核，可以同时工作在AP模式与STA模式下，具备云升级及多总线扩展功能，在物联网领域应用广泛。

该系统中的MPU9250是一个QFN封装的复合芯片（MCM），在3x3x1mm上集成了3轴16位加速度、3轴16位陀螺仪、3轴16位磁力计以及数字运动处理器（DMP），并且可以承受较大的震动冲击，该器件具备快速和慢速运动状态跟踪功能，内置的温度传感器和偏差计数器可以有效的进行器件的温度补偿，在85℃时可以保证仅有1%的时钟漂移。使用I2C方案使得系统连接大大简化，通讯速率可达400 kHz，能够直接输出9轴全部数据，通过软件滤波和归一化计算运算可以得到姿态参数。

2 系统原理

教学示教航行姿态系统，通过安装在航模上的九轴数字传感器MPU9250进行姿态测量，其内部结构如图2所示。核心控制器ESP8266通过I2C总线与传感器MPU9250通讯读取10组AD值。分别为，三轴加速度、三轴磁强计度、三轴陀螺仪、一组温度的AD值，经过归一化姿态融合后就可以得到Pitch、Roll、Yaw角，再通过建立无线WIFI路由器AP模式的TCP/UDP服务，为客户端提供测试所得的姿态数据。

在客户端使用Unity3Dengine创建UI，如图3所示。

图2 MPU9250逻辑结构图

示教时，针对性的展示姿态角，它们是：俯仰角pitch、滚转角yaw、偏航角roll。在地面上选一点Og，使Xg轴在水平面内并指向某一方向；Zg轴垂直于地面并指向地心，重力方向；Yg轴在水平面内垂直于Xg轴，其指向按左手定则确定。

图3 示教虚拟展示

3 硬件设计

3.1 USB通讯电路设计

CH340是一个USB总线的转接芯片，通过USB总线提供异步串口、打印口、并口以及常用的2线和4线等同步串行接口。该芯片提供了兼容USB相关规范和Windows操作系统的USB转串行通讯，在本系统中大大简化了硬件通讯设计。

如图4所示，USB与串行通讯的转换完全在芯片CH340内部完成。在本系统能够中实现下载与通讯调试两种角色。其中一键程序电路，DTR为数据终端准备好信号，RTS为数据终端请求信号。

图4 USB通讯电路

3.2 无线控制器电路设计

在本系统设计中如图5所示。

无线通讯部分，只需在外部加入的由L1、L2、C5组成的π型选频网络即可，在PCB制版时需要加入50欧姆的阻抗匹配即可。

由于ESP8266进入BOOT下载模式必须满足以下条件：

1）CHIP_EN引脚置高

2）GPIO0引脚置低（置高则进入从FLASH运行模式）

而实现一键下载的关键就在于，GPIO0引脚和RESET引脚电平状态，当DTR为1，RTS为0时，RESET复位引脚拉低，反之，GPIO0引脚拉低，逻辑关系如表1所示：

在使用PC端自动下载固件时，在软件端需要启动硬件流控制并适当在不同模式间加入50 ms延时才能正常使用，否则可能出现异常。

3.3 电源电路设计

本系统设计的电源如图6所示，输入电压工作范围为6～24 V，输出电压为3.30 V，采用DC/DC（BUCK）与LDO协同工作，在DC/DC中加入了旁路滤波选频器（C3、R6），在输出级加入了两级自恢复保险（F1、F2）。

其中DC/DC选用芯源半导体生产的MP1584EN，与之配合的电感选用FDV系列铁粉芯电感，电容选用TDK系列，输出级采用电压3倍的耐压值，LDO选用AMS1117。

该设计一方面具有较高的转换效率，还可适应更宽的输入电压范围，另一方面有效降低了系统电源纹波，提高整体电路输出电压工作稳定性和热稳定性，同时加入的自恢复保险避免了短路风险对核心控制器的损害。

4 软件设计

态解算一般有两种算法，一种是IMU，另一种是被各种无人机广泛使用的AHRS。

图5 ESP8266控制器子系统

表1 ESP8266启动逻辑

图6 供电源电路

4.1 姿态解算

本系统姿态检测部分由加速度计，磁场计，陀螺仪构成，能够为飞行器提供准确可靠的航向（yaw），横滚（roll）和侧翻（pitch）等姿态航行信息。传感器所测得的数据分别为3X，3Y，3Z三轴的加速度和三轴角速度原坐标系，因此常采用四元数和欧拉角表示旋转状态[10]，其公式如式（1）（2）所示。

式（1）中w、x、y、z为四元数，式（2）中φ、θ、ψ分别为全局坐标系下的角度。

在系统中采用上述方法对姿态进行解算。

4.2 滤波器设计

尽管采用了数字式的九轴传感器，但由于加速度及磁场对外界震动影响较大，需要结合陀螺仪进行归一化解算，进而得到AHRS状态，本文采用卡尔曼滤波器进行滤波，采用最小均方误差为估计准则，这种方法采用信号与噪声的状态空间模型，利用循环迭代求出当前测量估计值。这类方法非常适合微处理实时计算[11-15]。其滤波模型如式（4）所示：

对所测得的数值取不同的权值，把它们结合到一起，进行修正，如式（5）、（6）所示：

FK是作用在xk上的状态变换矩阵；

Bk是作用在控制向量uk上的输入模型；

Hk是测量矩阵，wk为噪声干扰；

Qk为协方差矩阵，Pk|k为当前状态协方差；

就那样分手了。三年的校园爱情，一年的共同生活，苍茫地结束。而夏天也就过去了，所有的阳光都好像在夏季里消耗殆尽。

|k-1为上次更新的状态（角度）；

其中（5）式为预测估计阶段，包含预测状态和协方差矩阵；式（6）是数据更新阶段包含，卡尔曼系数，更新状态的推算和协方差的推算；

其中角度解算滤波步骤如下[16]：

1）计算预估计协方差矩阵；

2）计算卡尔曼增益矩阵；

3）更新测量估计；

5）循环重复以上步骤。

4.3 解算程序

以下为嵌入式卡尔曼滤波算法实现：

4.4 实验结果

测试时沿着Z轴旋转并记录数据，如图7所示。

如图7～8所示，为示教器实际运动轨迹，测量所得的轨迹与滤波后的轨迹。经过滤波后的姿态解算，基本与原始运动轨迹一致，角度解算达到设计要求。

5 结 论

在航姿示教器的设计过程中，充分考虑了噪声对系统的干扰，在硬件使用了更为精密的电源设计，在软件上采用了卡尔曼权值滤波。同时系统采用了C/S架构无线WIFI模式，便于与计算机、手机等设备连接。通过实验室及教学验证，达到了设计要求，可以满足示教功能的姿态解算，具有较好的示教效果，具有一定的参考价值。

图7 偏航角roll测试

图8 偏航角roll姿态解算滤波