濾波融合算法的管道鋼珠運(yùn)動(dòng)檢測(cè)系統(tǒng)設(shè)計(jì)

(重慶城市職業(yè)學(xué)院 信息工程系，重慶 402160)

引 言

管道運(yùn)動(dòng)鋼珠檢測(cè)系統(tǒng)屬于球桿系統(tǒng)的一種，它是一個(gè)多變量、強(qiáng)耦合、非線性、時(shí)變和自然不穩(wěn)定系統(tǒng)，是近年來研究平衡桿及擺動(dòng)控制問題的又一研究平臺(tái)，是檢驗(yàn)各種控制理論的典型模型。仿人智能控制(Human-Simulated Intelligent Control)理論從分層遞階智能控制系統(tǒng)的最低層(運(yùn)行控制級(jí))著手，根據(jù)被控對(duì)象變化的大小、方向及速率等動(dòng)態(tài)特征，充分運(yùn)用已有的智能裝備技術(shù)和控制技術(shù)，模仿人腦的智能識(shí)別、智能決策，并將其編制成各種簡(jiǎn)單實(shí)用、精度高、能實(shí)時(shí)運(yùn)行的控制算法即動(dòng)覺智能圖式[1]，直接應(yīng)用于實(shí)際控制系統(tǒng)。本文依據(jù)動(dòng)覺智能圖式的仿人智能控制理論實(shí)現(xiàn)管道鋼珠的運(yùn)行檢測(cè)，對(duì)管道運(yùn)行角度和角速度提出了融合算法,得到了較為理想的檢測(cè)效果。

1 系統(tǒng)總體設(shè)計(jì)

圖1 總體方案框圖

本系統(tǒng)是基于STM32F103單片機(jī)為核心的控制器，采用LDC-1000金屬傳感器模塊和MPU-6050陀螺儀作為鋼珠的識(shí)別及方向檢測(cè)。STM32F103單片機(jī)以Arm Cortex-M3內(nèi)核為核心的編程器, 通過LDC-1000金屬傳感器采集鋼珠的數(shù)量信息，并經(jīng)MPU-6050陀螺儀檢測(cè)運(yùn)行方向，采用卡爾曼濾波(Kalman filtering)融合算法獲取管道角度和角速度，最終由LCD-ILI9325顯示屏顯示數(shù)量、角度及鋼珠滾動(dòng)的方向，總體方案框圖如圖1所示。

2 硬件設(shè)計(jì)

2.1 主控系統(tǒng)

本系統(tǒng)采用STM32F103ZET6作為核心控制芯片，具有72 MHz時(shí)鐘頻率，64 KB的SRAM，2個(gè)DMA控制器，支持ADC、定時(shí)器、SPI、USB、I2C和UART等外圍設(shè)備。系統(tǒng)采用STM32F103單片機(jī)控制LDC-1000金屬傳感器模塊、MPU-6050 9軸運(yùn)動(dòng)陀螺儀、LCD-ILI9325顯示屏等各部分電路。

2.2 電源電路設(shè)計(jì)

本系統(tǒng)采用12 V鋰電池供電，由于電路模塊較多，每個(gè)模塊供電需求存在較大的差異，因此設(shè)計(jì)多種輸出電源供電。另外，為了滿足各模塊供電電壓，本系統(tǒng)進(jìn)行了電源轉(zhuǎn)換設(shè)計(jì)，其中主控芯片STM32F103、MPU-6050陀螺儀、LCD-ILI9325顯示屏需3.3 V供電，外部電源模塊需采用12 V的直流電壓，LDC-1000金屬傳感器模塊需采用5 V供電。

(1)直流電壓12V轉(zhuǎn)換成5V

采用KA7805芯片實(shí)現(xiàn)12 V到5 V的轉(zhuǎn)換。KA7805是常用的穩(wěn)壓芯片，使用方便,引腳較少，易于連接和實(shí)現(xiàn)，穩(wěn)定性高[2]，用很簡(jiǎn)單的電路能夠?qū)⒋笥? V的直流電壓轉(zhuǎn)換成5 V的直流電壓。

(2)直流電壓5V轉(zhuǎn)換成3.3V

本系統(tǒng)采用三端穩(wěn)壓輸出AMS1117-5輸出5 V電壓，再利用AMS1117-3.3芯片將5 V轉(zhuǎn)換成3.3 V，電路設(shè)計(jì)如圖2所示。

圖2 電源電路

2.3 LDC-1000 金屬傳感器

LDC-1000金屬傳感器屬于電感式傳感器，它利用電磁感應(yīng)原理，在纏繞在LDC-1000的線圈中加一個(gè)交變電流，線圈會(huì)產(chǎn)生交變磁場(chǎng)，這時(shí)如果有金屬物體進(jìn)入這個(gè)磁場(chǎng)，則在電磁感應(yīng)作用下會(huì)產(chǎn)生渦流。渦流電流與線圈電流相互作用，通過探測(cè)線圈電流的大小和相位，從而探知金屬球。LDC-1000由三大部分組成：振蕩器、開關(guān)電路及放大輸出電路。振蕩器產(chǎn)生一個(gè)交變磁場(chǎng)，當(dāng)金屬目標(biāo)接近這一磁場(chǎng)并達(dá)到感應(yīng)距離時(shí)，在金屬目標(biāo)內(nèi)產(chǎn)生渦流，從而導(dǎo)致振蕩衰減，以至停振。振蕩器振蕩及停振的變化被后級(jí)放大電路處理并轉(zhuǎn)換成開關(guān)信號(hào)，觸發(fā)驅(qū)動(dòng)控制器件，從而實(shí)現(xiàn)非接觸式檢測(cè)。LDC-1000金屬傳感器電路如圖3所示。

圖3 LDC-1000金屬傳感器電路

2.4 MPU-6050 陀螺儀

MPU-6050陀螺儀用作檢測(cè)管道的傾斜角度。MPU-6050陀螺儀主要由一個(gè)位于軸心且可旋轉(zhuǎn)的轉(zhuǎn)子構(gòu)成，負(fù)責(zé)檢測(cè)x、y、z的旋轉(zhuǎn)及相應(yīng)軸的轉(zhuǎn)動(dòng)速度，也就是檢測(cè)圍繞各個(gè)軸轉(zhuǎn)動(dòng)的速度。MPU-6050的角速度全格感測(cè)范圍為±250、±500、±1000與±2000 °/sec (dps)，可準(zhǔn)確捕獲慢速與快速動(dòng)作，并且可編程控制的加速器全格感測(cè)范圍為±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)，本系統(tǒng)中MPU-6050與芯片PB10、PB11相連。MPU-6050陀螺儀電路如圖4所示。

圖4 MPU-6050陀螺儀電路

2.5 LCD-ILI9325顯示屏

LCD-ILI9325顯示屏用于顯示鋼珠進(jìn)入管道內(nèi)時(shí)的數(shù)量、管道傾斜的實(shí)時(shí)角度及鋼珠滾動(dòng)的方向，LCD-ILI9325顯示電路如圖5所示。

圖5 LCD-ILI9325顯示電路

3 系統(tǒng)軟件設(shè)計(jì)

3.1 系統(tǒng)軟件

利用IAR EWARM 5.3編寫各模塊程序，IAR有著極高的機(jī)器識(shí)別率，且結(jié)構(gòu)清晰明朗，容易理解識(shí)讀，尤其在研發(fā)大型項(xiàng)目時(shí)，其優(yōu)點(diǎn)更加突出。系統(tǒng)軟件設(shè)計(jì)包括系統(tǒng)初始化、LCD-1000鋼珠運(yùn)行方向和個(gè)數(shù)檢測(cè)、MPU-6050陀螺儀數(shù)據(jù)采集、卡爾曼數(shù)據(jù)濾波及融合獲取角度及角速度等，系統(tǒng)軟件流程圖如圖6所示。

圖6 系統(tǒng)流程圖

3.2 Kalman濾波融合算法

卡爾曼濾波(Kalman filtering)是一種高效率的遞歸濾波器，通過系統(tǒng)輸入輸出觀測(cè)數(shù)據(jù)，對(duì)系統(tǒng)狀態(tài)進(jìn)行最優(yōu)估計(jì)。基本思想是采用信號(hào)與噪聲的狀態(tài)空間模型，利用前一時(shí)刻的估計(jì)值和現(xiàn)時(shí)刻的觀測(cè)值來更新對(duì)狀態(tài)變量的估計(jì)[4]，求出現(xiàn)在時(shí)刻的估計(jì)值，由量測(cè)值重構(gòu)系統(tǒng)的狀態(tài)向量。卡爾曼濾波的核心在于修正之前的估計(jì)值，得到當(dāng)前時(shí)刻的最優(yōu)估計(jì)值和最優(yōu)最小均方誤差[5]，它以“預(yù)測(cè)—實(shí)測(cè)—修正”的順序遞推，根據(jù)量測(cè)值來消除隨機(jī)干擾[6]。算法的核心是新舊信息融合，再現(xiàn)系統(tǒng)的狀態(tài)。程序段如下所示：

void Kalman_Filter(float Angle_Kal,float Gyro_Kal){

g_fAngle += (Gyro_Kal - q_bias) * dt;

//先驗(yàn)估計(jì)

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

//先驗(yàn)估計(jì)誤差協(xié)方差的微分

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

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

Pdot[3] = Q_gyro;

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

//先驗(yàn)估計(jì)誤差協(xié)方差

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

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

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

angle_err = Angle_Kal - g_fAngle; //先驗(yàn)估計(jì)

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; //后驗(yàn)估計(jì)誤差協(xié)方差

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; //后驗(yàn)估計(jì)

q_bias += K_1 * angle_err;

//后驗(yàn)估計(jì)

g_fAngle_Dot = Gyro_Kal - q_bias;

//輸出值即后驗(yàn)估計(jì)值的微分 = 角速度

}

3.3 檢測(cè)及融合驗(yàn)證

檢測(cè)系統(tǒng)分模塊搭建單元電路并分別測(cè)試軟硬件是否能正常運(yùn)行，然后將分立的模塊搭建在一起測(cè)試整體軟硬件功能。經(jīng)測(cè)試，系統(tǒng)單片機(jī)核心控制模塊、電源輸出模塊、鋼珠檢測(cè)模塊、顯示模塊、角度及角速度檢測(cè)模塊的軟硬件均工作正常。

融合驗(yàn)證1，兩個(gè)LCD-1000之間的距離任意，假設(shè)一端為M端，另一端為N端:

① 由M 端放入若干鋼珠，放入的時(shí)間間隔小于2 s，顯示屏能夠準(zhǔn)確顯示放入鋼珠的個(gè)數(shù)。

② 將管道傾斜角度設(shè)為 10°～80°之間的某一角度，由 M 端放入 1 粒鋼珠，系統(tǒng)能夠精準(zhǔn)測(cè)量?jī)A斜角的角度值且誤差值小于3°。

融合驗(yàn)證2，兩個(gè)LCD-1000之間的距離為 20 mm，假設(shè)一端為M端，另一端為N端:

① 將若干鋼珠放入管內(nèi)，擺動(dòng)管道，擺動(dòng)周期≤1 s，測(cè)量系統(tǒng)能夠顯示擺動(dòng)的周期個(gè)數(shù)。

② 將管道放置任意角度，由高端連續(xù)倒入若干鋼珠，顯示屏能夠精準(zhǔn)顯示倒入鋼珠的個(gè)數(shù)。

③ M端高于N端或N端高于M端，傾斜角為10°～80°之間的某一角度，從高端放入1粒或多粒鋼球，顯示屏能夠精準(zhǔn)測(cè)量?jī)A斜角的角度值和顯示鋼珠的運(yùn)動(dòng)方向。

結(jié) 語

呂值敏(實(shí)驗(yàn)師)，主要從事高職高專《嵌入式原理》的教學(xué)。