滤波融合算法的管道钢珠运动检测系统设计

(重庆城市职业学院 信息工程系，重庆 402160)

引 言

管道运动钢珠检测系统属于球杆系统的一种，它是一个多变量、强耦合、非线性、时变和自然不稳定系统，是近年来研究平衡杆及摆动控制问题的又一研究平台，是检验各种控制理论的典型模型。仿人智能控制(Human-Simulated Intelligent Control)理论从分层递阶智能控制系统的最低层(运行控制级)着手，根据被控对象变化的大小、方向及速率等动态特征，充分运用已有的智能装备技术和控制技术，模仿人脑的智能识别、智能决策，并将其编制成各种简单实用、精度高、能实时运行的控制算法即动觉智能图式[1]，直接应用于实际控制系统。本文依据动觉智能图式的仿人智能控制理论实现管道钢珠的运行检测，对管道运行角度和角速度提出了融合算法,得到了较为理想的检测效果。

1 系统总体设计

图1 总体方案框图

本系统是基于STM32F103单片机为核心的控制器，采用LDC-1000金属传感器模块和MPU-6050陀螺仪作为钢珠的识别及方向检测。STM32F103单片机以Arm Cortex-M3内核为核心的编程器, 通过LDC-1000金属传感器采集钢珠的数量信息，并经MPU-6050陀螺仪检测运行方向，采用卡尔曼滤波(Kalman filtering)融合算法获取管道角度和角速度，最终由LCD-ILI9325显示屏显示数量、角度及钢珠滚动的方向，总体方案框图如图1所示。

2 硬件设计

2.1 主控系统

本系统采用STM32F103ZET6作为核心控制芯片，具有72 MHz时钟频率，64 KB的SRAM，2个DMA控制器，支持ADC、定时器、SPI、USB、I2C和UART等外围设备。系统采用STM32F103单片机控制LDC-1000金属传感器模块、MPU-6050 9轴运动陀螺仪、LCD-ILI9325显示屏等各部分电路。

2.2 电源电路设计

本系统采用12 V锂电池供电，由于电路模块较多，每个模块供电需求存在较大的差异，因此设计多种输出电源供电。另外，为了满足各模块供电电压，本系统进行了电源转换设计，其中主控芯片STM32F103、MPU-6050陀螺仪、LCD-ILI9325显示屏需3.3 V供电，外部电源模块需采用12 V的直流电压，LDC-1000金属传感器模块需采用5 V供电。

(1)直流电压12V转换成5V

采用KA7805芯片实现12 V到5 V的转换。KA7805是常用的稳压芯片，使用方便,引脚较少，易于连接和实现，稳定性高[2]，用很简单的电路能够将大于5 V的直流电压转换成5 V的直流电压。

(2)直流电压5V转换成3.3V

本系统采用三端稳压输出AMS1117-5输出5 V电压，再利用AMS1117-3.3芯片将5 V转换成3.3 V，电路设计如图2所示。

图2 电源电路

2.3 LDC-1000 金属传感器

LDC-1000金属传感器属于电感式传感器，它利用电磁感应原理，在缠绕在LDC-1000的线圈中加一个交变电流，线圈会产生交变磁场，这时如果有金属物体进入这个磁场，则在电磁感应作用下会产生涡流。涡流电流与线圈电流相互作用，通过探测线圈电流的大小和相位，从而探知金属球。LDC-1000由三大部分组成：振荡器、开关电路及放大输出电路。振荡器产生一个交变磁场，当金属目标接近这一磁场并达到感应距离时，在金属目标内产生涡流，从而导致振荡衰减，以至停振。振荡器振荡及停振的变化被后级放大电路处理并转换成开关信号，触发驱动控制器件，从而实现非接触式检测。LDC-1000金属传感器电路如图3所示。

图3 LDC-1000金属传感器电路

2.4 MPU-6050 陀螺仪

MPU-6050陀螺仪用作检测管道的倾斜角度。MPU-6050陀螺仪主要由一个位于轴心且可旋转的转子构成，负责检测x、y、z的旋转及相应轴的转动速度，也就是检测围绕各个轴转动的速度。MPU-6050的角速度全格感测范围为±250、±500、±1000与±2000 °/sec (dps)，可准确捕获慢速与快速动作，并且可编程控制的加速器全格感测范围为±2g、±4g、±8g与±16g[3]。MPU-6050可在不同电压下工作，VDD供电电压为2.5 V±5%、3.0 V±5%或3.3 V±5%，逻辑接口VVDIO供电为1.8 V± 5%(MPU-6050仅用VDD)，本系统中MPU-6050与芯片PB10、PB11相连。MPU-6050陀螺仪电路如图4所示。

图4 MPU-6050陀螺仪电路

2.5 LCD-ILI9325显示屏

LCD-ILI9325显示屏用于显示钢珠进入管道内时的数量、管道倾斜的实时角度及钢珠滚动的方向，LCD-ILI9325显示电路如图5所示。

图5 LCD-ILI9325显示电路

3 系统软件设计

3.1 系统软件

利用IAR EWARM 5.3编写各模块程序，IAR有着极高的机器识别率，且结构清晰明朗，容易理解识读，尤其在研发大型项目时，其优点更加突出。系统软件设计包括系统初始化、LCD-1000钢珠运行方向和个数检测、MPU-6050陀螺仪数据采集、卡尔曼数据滤波及融合获取角度及角速度等，系统软件流程图如图6所示。

图6 系统流程图

3.2 Kalman滤波融合算法

卡尔曼滤波(Kalman filtering)是一种高效率的递归滤波器，通过系统输入输出观测数据，对系统状态进行最优估计。基本思想是采用信号与噪声的状态空间模型，利用前一时刻的估计值和现时刻的观测值来更新对状态变量的估计[4]，求出现在时刻的估计值，由量测值重构系统的状态向量。卡尔曼滤波的核心在于修正之前的估计值，得到当前时刻的最优估计值和最优最小均方误差[5]，它以“预测—实测—修正”的顺序递推，根据量测值来消除随机干扰[6]。算法的核心是新旧信息融合，再现系统的状态。程序段如下所示：

void Kalman_Filter(float Angle_Kal,float Gyro_Kal){

g_fAngle += (Gyro_Kal - q_bias) * dt;

//先验估计

Pdot[0] = Q_angle - P[0][1] - P[1][0];

//先验估计误差协方差的微分

Pdot[1] =- P[1][1];

Pdot[2] =- P[1][1];

Pdot[3] = Q_gyro;

P[0][0] += Pdot[0] * dt;

//先验估计误差协方差

P[0][1] += Pdot[1] * dt;

P[1][0] += Pdot[2] * dt;

P[1][1] += Pdot[3] * dt;

angle_err = Angle_Kal - g_fAngle; //先验估计

PCt_0 = C_0 * P[0][0];

PCt_1 =C_0 * P[1][0];

E = R_angle+C_0 *PCt_0;

K_0 = PCt_0 / E; //Kk

K_1 = PCt_1 / E;

t_0 = PCt_0;

t_1 = P[0][1]; //C_0 *

P[0][0] -= K_0 * t_0; //后验估计误差协方差

P[0][1] -= K_0 * t_1;

P[1][0] -= K_1 * t_0;

P[1][1] -= K_1 * t_1;

g_fAngle += K_0 * angle_err; //后验估计

q_bias += K_1 * angle_err;

//后验估计

g_fAngle_Dot = Gyro_Kal - q_bias;

//输出值即后验估计值的微分 = 角速度

}

3.3 检测及融合验证

检测系统分模块搭建单元电路并分别测试软硬件是否能正常运行，然后将分立的模块搭建在一起测试整体软硬件功能。经测试，系统单片机核心控制模块、电源输出模块、钢珠检测模块、显示模块、角度及角速度检测模块的软硬件均工作正常。

融合验证1，两个LCD-1000之间的距离任意，假设一端为M端，另一端为N端:

① 由M 端放入若干钢珠，放入的时间间隔小于2 s，显示屏能够准确显示放入钢珠的个数。

② 将管道倾斜角度设为 10°～80°之间的某一角度，由 M 端放入 1 粒钢珠，系统能够精准测量倾斜角的角度值且误差值小于3°。

融合验证2，两个LCD-1000之间的距离为 20 mm，假设一端为M端，另一端为N端:

① 将若干钢珠放入管内，摆动管道，摆动周期≤1 s，测量系统能够显示摆动的周期个数。

② 将管道放置任意角度，由高端连续倒入若干钢珠，显示屏能够精准显示倒入钢珠的个数。

③ M端高于N端或N端高于M端，倾斜角为10°～80°之间的某一角度，从高端放入1粒或多粒钢球，显示屏能够精准测量倾斜角的角度值和显示钢珠的运动方向。

结 语

吕值敏(实验师)，主要从事高职高专《嵌入式原理》的教学。