(重慶城市職業(yè)學(xué)院 信息工程系,重慶 402160)
管道運(yùn)動鋼珠檢測系統(tǒng)屬于球桿系統(tǒng)的一種,它是一個(gè)多變量、強(qiáng)耦合、非線性、時(shí)變和自然不穩(wěn)定系統(tǒng),是近年來研究平衡桿及擺動控制問題的又一研究平臺,是檢驗(yàn)各種控制理論的典型模型。仿人智能控制(Human-Simulated Intelligent Control)理論從分層遞階智能控制系統(tǒng)的最低層(運(yùn)行控制級)著手,根據(jù)被控對象變化的大小、方向及速率等動態(tài)特征,充分運(yùn)用已有的智能裝備技術(shù)和控制技術(shù),模仿人腦的智能識別、智能決策,并將其編制成各種簡單實(shí)用、精度高、能實(shí)時(shí)運(yùn)行的控制算法即動覺智能圖式[1],直接應(yīng)用于實(shí)際控制系統(tǒng)。本文依據(jù)動覺智能圖式的仿人智能控制理論實(shí)現(xiàn)管道鋼珠的運(yùn)行檢測,對管道運(yùn)行角度和角速度提出了融合算法,得到了較為理想的檢測效果。
圖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所示。
本系統(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)動陀螺儀、LCD-ILI9325顯示屏等各部分電路。
本系統(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],用很簡單的電路能夠?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 電源電路
LDC-1000金屬傳感器屬于電感式傳感器,它利用電磁感應(yīng)原理,在纏繞在LDC-1000的線圈中加一個(gè)交變電流,線圈會產(chǎn)生交變磁場,這時(shí)如果有金屬物體進(jìn)入這個(gè)磁場,則在電磁感應(yīng)作用下會產(chǎn)生渦流。渦流電流與線圈電流相互作用,通過探測線圈電流的大小和相位,從而探知金屬球。LDC-1000由三大部分組成:振蕩器、開關(guān)電路及放大輸出電路。振蕩器產(chǎn)生一個(gè)交變磁場,當(dāng)金屬目標(biāo)接近這一磁場并達(dá)到感應(yīng)距離時(shí),在金屬目標(biāo)內(nèi)產(chǎn)生渦流,從而導(dǎo)致振蕩衰減,以至停振。振蕩器振蕩及停振的變化被后級放大電路處理并轉(zhuǎn)換成開關(guān)信號,觸發(fā)驅(qū)動控制器件,從而實(shí)現(xiàn)非接觸式檢測。LDC-1000金屬傳感器電路如圖3所示。
圖3 LDC-1000金屬傳感器電路
MPU-6050陀螺儀用作檢測管道的傾斜角度。MPU-6050陀螺儀主要由一個(gè)位于軸心且可旋轉(zhuǎn)的轉(zhuǎn)子構(gòu)成,負(fù)責(zé)檢測x、y、z的旋轉(zhuǎn)及相應(yīng)軸的轉(zhuǎn)動速度,也就是檢測圍繞各個(gè)軸轉(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陀螺儀電路
LCD-ILI9325顯示屏用于顯示鋼珠進(jìn)入管道內(nèi)時(shí)的數(shù)量、管道傾斜的實(shí)時(shí)角度及鋼珠滾動的方向,LCD-ILI9325顯示電路如圖5所示。
圖5 LCD-ILI9325顯示電路
利用IAR EWARM 5.3編寫各模塊程序,IAR有著極高的機(jī)器識別率,且結(jié)構(gòu)清晰明朗,容易理解識讀,尤其在研發(fā)大型項(xiàng)目時(shí),其優(yōu)點(diǎn)更加突出。系統(tǒng)軟件設(shè)計(jì)包括系統(tǒng)初始化、LCD-1000鋼珠運(yùn)行方向和個(gè)數(shù)檢測、MPU-6050陀螺儀數(shù)據(jù)采集、卡爾曼數(shù)據(jù)濾波及融合獲取角度及角速度等,系統(tǒng)軟件流程圖如圖6所示。
圖6 系統(tǒng)流程圖
卡爾曼濾波(Kalman filtering)是一種高效率的遞歸濾波器,通過系統(tǒng)輸入輸出觀測數(shù)據(jù),對系統(tǒng)狀態(tài)進(jìn)行最優(yōu)估計(jì)?;舅枷胧遣捎眯盘柵c噪聲的狀態(tài)空間模型,利用前一時(shí)刻的估計(jì)值和現(xiàn)時(shí)刻的觀測值來更新對狀態(tài)變量的估計(jì)[4],求出現(xiàn)在時(shí)刻的估計(jì)值,由量測值重構(gòu)系統(tǒng)的狀態(tài)向量??柭鼮V波的核心在于修正之前的估計(jì)值,得到當(dāng)前時(shí)刻的最優(yōu)估計(jì)值和最優(yōu)最小均方誤差[5],它以“預(yù)測—實(shí)測—修正”的順序遞推,根據(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;
//先驗(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ì)值的微分 = 角速度
}
檢測系統(tǒng)分模塊搭建單元電路并分別測試軟硬件是否能正常運(yùn)行,然后將分立的模塊搭建在一起測試整體軟硬件功能。經(jīng)測試,系統(tǒng)單片機(jī)核心控制模塊、電源輸出模塊、鋼珠檢測模塊、顯示模塊、角度及角速度檢測模塊的軟硬件均工作正常。
融合驗(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)測量傾斜角的角度值且誤差值小于3°。
融合驗(yàn)證2,兩個(gè)LCD-1000之間的距離為 20 mm,假設(shè)一端為M端,另一端為N端:
① 將若干鋼珠放入管內(nèi),擺動管道,擺動周期≤1 s,測量系統(tǒng)能夠顯示擺動的周期個(gè)數(shù)。
② 將管道放置任意角度,由高端連續(xù)倒入若干鋼珠,顯示屏能夠精準(zhǔn)顯示倒入鋼珠的個(gè)數(shù)。
③ M端高于N端或N端高于M端,傾斜角為10°~80°之間的某一角度,從高端放入1?;蚨嗔d撉?,顯示屏能夠精準(zhǔn)測量傾斜角的角度值和顯示鋼珠的運(yùn)動方向。
呂值敏(實(shí)驗(yàn)師),主要從事高職高?!肚度胧皆怼返慕虒W(xué)。