基于STM32F767IGT6 的AGV 姿态解算系统设计

陈世强，刘明

（云南民族大学，云南昆明 650500）

近年来，随着自动导引车AGV（Automatic Guided Vehicle）在工厂、码头等高动态复杂场景中的应用普及，迫切需要能够满足室内和室外环境的低成本、高精度、小体积的导航定位系统[1]。利用磁传感器、惯性测量单元IMU（Inertial Measurement Unit）与外部信号如GPS 和红外反射组合构成的导航定位系统成为主要的解决方案。惯性测量单元（IMU）是广泛用于姿态检测中的导航器件，通过解算九轴数据可以得到载体的姿态信息。但由于惯性测量元件随温度发生漂移、交叉轴存在耦合误差，严重影响了系统的输出精度[2]，且IMU 往往成本较高，无法在低成本的AGV 车辆中使用。目前低成本解决方案中常用于姿态解算的芯片有博世(BOSCH)公司的BMX055 和InvenSense 公司的MPU6050、MPU9250、ICM-20948等传感器芯片。文献[3]运用ICM-20948 九轴传感器对风的三维姿态角进行解算，设计手持式多信息风速风向定位仪，帮助消防员进行救援工作。文献[4]使用BMX055 芯片，并结合四元数解算转欧拉角的算法实现小车的自平衡。文献[5]采用FPGA+MPU9250的组合实现四轴无人机的姿态解算，FPGA 软硬件结合提高了姿态解算效率。但是FPGA 需要使用硬件描述语言来实现，且较为复杂。而STM32F767IGT6使用最基本的C 语言就能实现，操作简单，内部集成的双精度硬件浮点计算单元（DPFPU）和DSP 指令，能够满足姿态解算对数据处理能力的要求。另外，价格方面STM32 相比FPGA 也较便宜。文献[6]采用FPGA+DSP 双处理器的方案实现对某弹载控制器的姿态解算，输出精度能达到±0.5°。但在低成本的AGV 中，使用双处理器解算姿态成本较高，且STM32F767IGT6 具有DSP 单元，能加速数字信号的处理。文献[7]中采用6 轴处理组件MPU6050 和磁阻传感器HMC5883L 进行电路设计和编程，以实现姿态解算。文献[8]中使用MPU9250 辅助控制无人机的飞行姿态，飞行试验表明，该系统可以较好地控制无人机飞行姿态。MPU9250 运用于低成本、小体积、高精度的姿态解算系统中有较好的效果。

该文采用MPU9250 来完成AGV 的传感器数据采集和姿态解算，MPU9250 整合了九轴运动组件[9]，避免了组合陀螺仪与加速度器的轴间差问题，免除了惯性测量单元进行姿态解算前的校准工作，姿态解算效率高。相较其他的组合方案，采用STM32F767 IGT6+MPU9250 具有小体积、低功耗、高精度、易于控制等特点。

1 AGV姿态描述与更新

1.1 AGV姿态描述

首先建立坐标系，取“东-北-天”地理坐标系作为导航参考坐标系，取On点为坐标系原点；载体坐标系是b系，为笛卡尔坐标系，即“右-前-上”，其坐标原点Ob与导航坐标系的On点重合[10]，载体的姿态信息用偏航角、横滚角和俯仰角描述，载体坐标系和参考坐标系如图1 所示。

图1 载体坐标系和参考坐标系

在AGV 运动之后，通过坐标转换，可以得到新坐标系下的坐标。用方向余弦矩阵表示的坐标变换为：

由式（1）结合四轮AGV 运动模型，在行驶过程中给定AGV 一个控制量u=[v ω]T，则通过MCU 给定控制量就可控制AGV 的运动。

1.2 姿态更新的四元数表示

四元数（quaternion）的概念最早于1843 年由数学家哈密顿（W.R.Hamilton）提出，它可用于描述刚体运动或姿态变换[11]。与方向余弦阵相比，四元数表示方法虽然比较抽象，但却十分简洁。四元数可以表示为：

在姿态解算系统中，方向余弦矩阵运算量大，欧拉角法存在万向节死锁的现象，而四元数法计算简洁、精度高，不存在欧拉角“万向节死锁”现象，可以完美的弥补二者不足[12]。因此，为了方便、准确地描述AGV 的运动，大多使用四元数法。四元数与方向余弦矩阵之间的关系如下：

在这里四元数q能够显示转动的全部信息，包括旋转角度、旋转方向以及旋转轴的信息。因此姿态矩阵的四元数表示关系式为：

2 系统硬件设计

该系统采用STM32F767IGT6的两个GPIO（PH4、PH5）连接MPU9250 的IIC 接口23 脚IIC-SCL、24 脚IIC-SDA。通过电路集成总线IIC，实时将九轴传感器采集到的原始数据和解算后的欧拉角信息上报给MCU，再通过串口将数据上报给上位机，可以实时地显示MPU9250 的传感器状态曲线，同时在LCD 模块上显示温度和欧拉角等信息。通过PA8、PA9、PA10、PA11、PB6、PB7、PB8、PB9 八个GPIO 输出PWM 控制信号，以控制AGV 的前进和后退。系统框图如图2所示。

图2 系统框图

2.1 主控芯片

系统主控芯片采用ST 公司的STM32F767IGT6，该芯片采用六级流水线，自带指令和数据Cache、集成JPEG 编码器、集成双精度硬件浮点计算单元（DPFPU）和DSP 指令，并具有512 kB SARM、1 024 kB Flash、四个IIC、八个串口以及140 个通用I/O 口等[13]，芯片主频率高达216 MHz 可以轻松应对系统的需求。STM32F767IGT6 最小系统如图3 所示。

图3 STM32F767IGT6最小系统

2.2 MPU9250

MPU9250 是InvenSense 公司推出的九轴运动跟踪传感器，内部集成一个MPU6050 和一个磁力计AK8963[14]。广泛运用于手势控制、体感游戏控制器、可穿戴智能设备、游戏手柄、位置查找服务、姿态解算等领域。3×3×1 mm3的超小封装使其能装在各种载体上，能承受住10 000 g 的震动冲击。九轴三个16 位分辨率的传感器以数字形式输出融合演算数据，能极大地减少MCU 处理任务的工作量。具有灵活的时钟方案，适用于各种内部时钟源同步电路，可以对信号进行调理和数模转换，片上PLL 允许各种输入的时钟信号。工作条件：在40～85 ℃时带高精度的1%的时钟漂移，嵌入温度传感器，并带可编程中断。其完美的IIC 方案，可以直接以数字的形式输出九轴融合演算数据，具有可编程数字滤波器，自带512 字节FIFO 缓冲区，支持SPI 和IIC 两种通信方式。

2.2.1 MPU9250与STM32硬件连接

MPU9250 的SCL、SDA 与STM32F767IGT6 的PH4、PH5 连接，与PCF8574T 共用IIC 总线。图4 中，AD0 引脚接GND，AD0 引脚是IIC 从机LSB 的地址，MPU9250 器件IIC 地址其实是内部集成的MPU6050的地址b01101000，即0X68。9D_INT 信号线接在PCF8574T 的P5 脚上，在读取9D_INT 时，需要先初始化PCF8574T，然后通过IIC 总线读取数据。MPU9250 通过IIC 总线进行单字节或多字节的配置，加速度值、陀螺仪值等存放在FIFO 数据缓存器中，随时等待读取。STM32F767IGT6 与MPU9250 的硬件连接电路如图4 所示。

图4 STM32F767IGT6与MPU9250的硬件连接电路

2.2.2 IIC总线运行模式

IIC 是一个双线通信方案，属于半双工同步通信方式，通过SDA 数据线和SCL 时钟线这两根线电平的变化，分别传输数据和时钟信号[15]。通常这两个接口是双向的开漏极接口，在连接设备时可以做主机或者从机。从机在通信时，通过地址即可匹配，MPU9250 通常和控制芯片连接时作为从机，SDA 和SCL 需要接阻值为1.2 kΩ的上拉电阻到VDD，其最快通信速度达400 kHz。

MPU9250 有辅助IIC 通信芯片通信的功能，这个功能有两个模式：1）IIC 主机模式：MPU9250 作为主机直接与从机通过IIC 进行通信。允许MPU9250 直接读传感器的寄存器，比如加速度计。在这个模式下，MPU9250 可以不需要主控芯片而直接读取辅助芯片的寄存器。2）通行模式：主控芯片直接通过IIC控制MPU9250 和外设芯片。该系统采用IIC 主机模式，IIC 口的SDA 和SCL 是复用口，采用主机模式MPU9250 对辅助芯片进行配置，复用功能不能再用，只能通过辅助芯片读取STM32F767IGT6的数据，中断脚必须连接系统的GPIO，从而唤醒系统。其中MPU9250 的AUX_DA 和AUX_CL 不连接，悬空。IIC时序逻辑如图5 所示。

图5 IIC时序逻辑

2.2.3 DMP使用

数字运动处理（Digital Motion Processor，DMP）内置于MPU9250 中，可以直接处理九轴传感器采集到的原始数据[16]。先将原始数据转化为四元数，再融合解算为姿态角，大大节约了姿态解算时间，使用DMP 进行数据融合演算，能有效地释放主芯片STM32F767IGT6 的工作任务，使得MCU 能处理更多的任务。MPU9252 从寄存器中就能直接读取到DMP计算好的演算数据，或者直接把算好的数据存入FIFO缓冲器中等待需要时调用。DMP 有个中断引脚，可用来唤醒主控芯片。为了保证数据融合解算的高速率和高精度，使用DMP 可以节能，也可以节约程序运行时间，对于设计AGV 是一个很好的选择。在软件设计中，设置两个函数：mpu_dmp_init 函数，用于初始化MPU9250 DMP；mpu_dmp_get_data 函数，用于得到DMP 姿态解算后的俯仰角、横滚角和航向角，以及温度等信息。

当STM32F767IGT6 读取MPU9250 的加速度和角速度传感器数据时（非中断方式），需要先初始化相关的配置参数等，初始化完成后就可以读取相关的寄存器，首先将加速度，陀螺仪、磁场值传给DMP，融合演算之后，再从寄存器中将数据传给MCU。数据读取流程图如图6 所示。

图6 数据读取流程图

3 系统软件设计

AGV 姿态解算系统软件是在硬件的基础上进行设计的，STM32F767IGT6 控制程序采用C 语言在keil 5 中编写完成，主要包括MPU9250 初始化、MPU9250 数据采集、IIC 驱动、MPL 驱动、上报数据给上位机等程序设计。

MPU_Init()函数按照图6 所示流程进行初始化，函数执行完成之后，便可读取传感器数据。陀螺仪满量程设为±2 000°/s，加速度传感器满量程设置为±2g，陀螺仪采样频率设置为50 Hz。通过电源管理寄存器1 设置系统时钟源，将CLKSEL 置为001。通过MPU_Get_Magnetometer、MPU_Get_Accelerometer、MPU_Get_Gyroscope 三个函数获取九轴（加速度、陀螺仪、磁力计）数据。Void usart1_report_imu()函数通过串口1 上报解算后的姿态数据（roll、pitch、yaw）给上位机，通过状态帧（01H）进行发送。代码编译成功下载到STM32F767IGT6 后，从LCD 屏幕可以实时查看当前的MPU9250 温度、俯仰角（pitch）、横滚角（roll）和航向角（yaw）的数值，AGV 姿态信息如图7所示。

图7 AGV 姿态信息

上位机采用ANO_TC 匿名科创地面站v4.exe 软件，MPU9250_send_data 函数用于上报加速度和陀螺仪的原始数据，通过传感器帧（02H）进行数据发送，串口COM3 进行数据接收(共6 条线:ACC_X、ACC_Y、ACC_Z、GYRO_X、GYRO_Y、GYRO_Z)，串口波特率设置为500 kbps，上位机数据校验设置为OFF，从而以图形的形式显示传感器数据。

4 结论

该文设计的AGV 姿态解算系统，综合利用了STM32F76IGT6、MPU9250 强大的控制能力和传感解算能力，传感器输出频率达到1 000 Hz，姿态解算频率达到320 Hz，姿态角精度为±0.2°，并能实时显示。在AGV 运动中实现了高精度的姿态解算，实时掌控AGV 的姿态信息。在陌生复杂的环境中，对于AGV 自身姿态的把握控制以及后续运动控制的执行都是至关重要的。该系统运用于无人车、无人机和水下控制器的控制中都能为控制中心提供高精度的载体姿态信息，给后台控制人员提供了便利。在STM32F767IGT6 中运用姿态解算结果控制AGV 运动的算法还有待提高，AGV 运动与实际预期还有一定差距，后期将从算法层面改进AGV 运动控制，优化控制效果。