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

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

引 言

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

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

圖1 總體方案框圖

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

2 硬件設(shè)計

2.1 主控系統(tǒng)

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

2.2 電源電路設(shè)計

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

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

采用KA7805芯片實現(xiàn)12 V到5 V的轉(zhuǎn)換。KA7805是常用的穩(wěn)壓芯片，使用方便,引腳較少，易于連接和實現(xiàn)，穩(wěn)定性高[2]，用很簡單的電路能夠?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è)計如圖2所示。

圖2 電源電路

2.3 LDC-1000 金屬傳感器

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

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

2.4 MPU-6050 陀螺儀

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

圖5 LCD-ILI9325顯示電路

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

3.1 系統(tǒng)軟件

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

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

3.2 Kalman濾波融合算法

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

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];

//先驗估計誤差協(xié)方差的微分

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

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

Pdot[3] = Q_gyro;

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

//先驗估計誤差協(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; //先驗估計

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; //后驗估計誤差協(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; //后驗估計

q_bias += K_1 * angle_err;

//后驗估計

g_fAngle_Dot = Gyro_Kal - q_bias;

//輸出值即后驗估計值的微分 = 角速度

}

3.3 檢測及融合驗證

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

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

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

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

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

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

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

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

結(jié) 語

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