基于嵌入式的姿态检测系统设计

周 恒, 王民慧

(贵州大学 电气工程学院, 贵阳550000)

0 引 言

随着可穿戴嵌入式设备、无人机、VR 等技术的快速发展,物姿信息的求取成为了运动控制的前提条件和必要手段。 单一的传感器检测的姿态信息,存在计算的累计误差或存在量测误差,而无法得到物体姿态的准确信息［1］。 因此,本文设计了基于MPU6050 多轴姿态传感器和STM32F103ZET6 单片机的姿态检测系统,通过多个传感器对姿态进行检测,借助互补滤波器及卡尔曼滤波器对多传感器数据进行融合。 实验验证,两种滤波器均可得到物体准确的姿态信息。

1 系统的总体设计

本文所设计的姿态检测系统以STM32F103ZET6单片机为核心,通过MPU6050 多轴姿态传感器对物体的运动姿态进行检测,经互补滤波器或卡尔曼滤波器处理后的数据即可在OLED 显示屏上显示,也可通过相应的通信协议,通过串口将处理前及处理后的数据发送至上位机进行观测,得到物体的运动曲线。本系统的总体设计如图1 所示。

2 软件设计

2.1 系统软件的总体设计

本姿态检测系统的主程序流程图如图2 所示,主要由系统初始化程序、传感器数据采集程序、量程转换程序、互补滤波器程序、卡尔曼滤波器程序、按键扫描程序、OLED 显示程序、通信协议程序等几部分组成。 系统启动后,首先对MPU6050 进行初始化,并判断是否初始化成功。 如果初始化失败,则OLED 显示MPU6050Init error；如果初始化成功,则读取MPU6050 传感器采集的姿态并对按键进行检测,如果key1 按键按下,则状态标志位置1,并调用互补滤波器对进行量测转换后的数据进行数据融合处理,得到物体实时姿态信息。 将得到的融合姿态信息及原始数据发送至上位机进行监测；如果key2按键按下,则状态标志位置2,调用卡尔曼滤波器对进行量测转换后数据进行数据融合处理,将得到的融合姿态信息及原始数据经通信协议打包发送至上位机进行检测。

图1 系统总体设计框图Fig. 1 Block diagram of the overall design of the system

图2 系统的主程序流程图Fig. 2 Main program flow chart of the system

2.2 互补滤波器设计

互补滤波器就是根据不同传感器特性,通过不同的高通滤波器或低通滤波器,相加得到整个频带的信号［2］。 针对MPU6050,由于加速度计对传感器运动过程中的加速度比较敏感,取加速度计的瞬时值计算的姿态信息误差较大；而陀螺仪测得的角速度经过积分后得到的姿态信息不受传感器运动过程中加速度的影响。 但随着时间的推移,通过积分所得到的姿态信息便会有一个较大的累计误差,可采用互补滤波器对两个传感器得到姿态信息进行校正滤波,进而得到一个准确的姿态信息。

本实验所设计的互补滤波器以对陀螺仪数据积分得到的姿态信息为最优值,定时对加速度计的数据采样得到的姿态信息来校正陀螺仪得到的姿态,通过加速度计滤除高频信号,陀螺仪滤除低频信号［3］,将陀螺仪和加速度计的优点融合起来,使之在高频段及低频段都可以得到一个准确的姿态信息,其算法框图如图3 所示：

图3 互补滤波器算法框图Fig. 3 Block diagram of complementary filter algorithm

其互补滤波器的姿态信息输出方程(1)：

式中,Angle 为互补滤波器输出的传感器的当前角度值,k 为对加速度计取值的权重,Y＿gyro 为陀螺仪的Y 轴输出值,X＿acc 为加速度计X 轴输出值,dt 为采样周期。

本文中互补滤波器的初始参数设置如表1 所示：

表1 互补滤波器的初始值参数Tab. 1 Initial value parameters of complementary filter

2.3 卡尔曼滤波器的设计

首先,建立系统的状态方程(2)及量测方程(3)如下：

状态方程：

式中,Xk为系统k 时刻的状态量,A 为系统从k- 1 时刻到k 时刻的状态转移矩阵,Uk为系统k 时刻的输入量,B 为与之对应的输入控制加权矩阵,Wk是系统k 时刻的过程演化噪声,Zk为系统k 时刻量测向量,H 为系统k 时刻的量测矩阵,Vk为系统k 时刻的量测噪声。

在本系统中,选择陀螺仪X 轴方向上的角度值作为一个状态向量,以加速度计传感器的值估计得到的陀螺仪的常值偏差b 作为另一个状态向量［4］,建立其量测倾角的模型(4)：

式中,θk为MPU6050 传感器真实旋转的角度,ωk为陀螺仪k 时刻的角速度,bk为陀螺仪k 时刻的常值偏差,vk为陀螺仪的噪声, dt 为采样周期。 根据式(2)～(4),建立离散化后的系统状态方程和量测方程(5)：

式中,Q＿angle 为角度估算值的协方差,Q＿gyro 为陀螺仪位偏差的协方差,其数值表示卡尔曼滤波器对传感器数据的信任程度,数值越小,表明信任程度越高。 因此,卡尔曼滤波递推公式可概括为5 个核心公式(7) ～(11)：

系统此刻状态的预估值为公式(7)：

本文中卡尔曼滤波器的初始参数设置如表2 所示。 经多次试验,该初始值设置使卡尔曼滤波器具有很好的鲁棒特性和准确性。

表2 卡尔曼滤波器的初始参数值Tab. 2 Initial parameter values ofKalman filter

2.4 通信协议设计

为了便于学生对实验数据的观察,本系统不仅可通过oled 显示屏查看MPU6050 传感器的当前姿态信息,还可通过上位机以波形的形式查看传感器的姿态变化情况,其下位机与上位机的通信协议如表3 所示：

表3 上位机与下位机之间的通信协议Tab. 3 Communication protocol between upper computer and lower computer

表中DATA 表示要发送的数据,最大数据长度不超过28 个字节,SUM 表示从帧头0x88 一直到DATA 最后一个字节的和,通过该协议帧格式,可将数据通过串口上传至上位机进行观测。

3 实验测试

通过两种滤波器均有效滤除了陀螺仪传感器产生的漂移误差,得到物体的实际姿态情况,如图4、图5 所示。

图4 互补滤波器实验测试Fig. 4 Complementary filter experiment test

图5 卡尔曼滤波器实验测试Fig. 5 Kalman filter experimental test

4 结束语

本姿态检测系统利用MPU6050 传感器和STM32F103ZET6 单片机搭建,实现了MPU6050 传感器六轴数据的读取,通过量程转换之后以本系统所设计的互补滤波器和卡尔曼滤波器对陀螺仪、加速度计传感器的值进行数据融合,提高了物体姿态检测的准确性。 该系统具有响应快、量测准确、数据显示直观、实时性好等特点,可广泛应用于本科实验教学,增强学生对互补滤波和卡尔曼滤波算法的理解以及学生对STM32 单片机的应用。