姜 濤，王建中，施家棟

（北京理工大學(xué) 爆炸科學(xué)與技術(shù)國(guó)家重點(diǎn)實(shí)驗(yàn)室，北京 100081）

微機(jī)電陀螺信號(hào)盲均衡迭代反卷積算法

姜 濤，王建中，施家棟

（北京理工大學(xué) 爆炸科學(xué)與技術(shù)國(guó)家重點(diǎn)實(shí)驗(yàn)室，北京 100081）

小型移動(dòng)機(jī)器人在未知環(huán)境下運(yùn)行，陀螺所受噪聲干擾無(wú)法建立有效的數(shù)學(xué)模型，需要僅從觀測(cè)信號(hào)中把噪聲去除，并估計(jì)出原始信號(hào)，根據(jù)該特點(diǎn)提出一種微機(jī)電陀螺信號(hào)盲均衡迭代反卷積算法。該算法利用橫向?yàn)V波器對(duì)陀螺信號(hào)進(jìn)行反卷積運(yùn)算，使用貝葉斯方法對(duì)信號(hào)進(jìn)行估計(jì)，建立了誤差函數(shù)并與 LMS算法組合，實(shí)現(xiàn)了均衡器參數(shù)的自動(dòng)調(diào)整，在小型移動(dòng)機(jī)器人上進(jìn)行了算法實(shí)驗(yàn)驗(yàn)證。實(shí)驗(yàn)結(jié)果表明，該算法可以有效分離角速度信號(hào)與噪聲信號(hào)，其噪聲信號(hào)幅值減小約 10倍，移動(dòng)機(jī)器人運(yùn)行275.41 s抵達(dá)終點(diǎn)的偏航角誤差從13°下降到1.46°。

移動(dòng)機(jī)器人；MEMS陀螺；盲均衡；反卷積；濾波器

微小型移動(dòng)機(jī)器人的智能化水平逐步提高，將廣泛的應(yīng)用于城市作戰(zhàn)中。但是由于其體積小續(xù)航能力較差，通常是由士兵背負(fù)到作戰(zhàn)區(qū)域，再實(shí)施相關(guān)戰(zhàn)術(shù)任務(wù)。所以對(duì)移動(dòng)機(jī)器人的體積和重量有嚴(yán)格要求，從而導(dǎo)致此類(lèi)機(jī)器人無(wú)法使用體積與重量較大的光纖陀螺等傳感器進(jìn)行導(dǎo)航定位。雖然微機(jī)電陀螺的體積與重量均滿(mǎn)足要求，但其傳感器加工精度與抗干擾能力較差，需對(duì)其輸出信號(hào)進(jìn)行復(fù)雜的數(shù)據(jù)處理。

FIR濾波和IIR濾波或其二者的組合所構(gòu)成的低通濾波器與陷波器[1-2]，可以消除噪聲干擾，但此類(lèi)濾波器缺點(diǎn)是需要較多的系數(shù)去近似期望響應(yīng)，會(huì)造成較大滯后。采用基于AR模型的Kalman濾波算法可以有效的抑制噪聲[3-4]，其狀態(tài)的每次更新都是由前一次估計(jì)和新的輸入數(shù)據(jù)計(jì)算得到，具有較高的實(shí)時(shí)性。小波變換與多分辨率分析原理方法[5-6]，可以有效地消除陀螺的隨機(jī)游走。

本文提出一種盲均衡迭代反卷積算法對(duì)微機(jī)電陀螺信號(hào)進(jìn)行處理，該算法可有效分離陀螺信號(hào)與噪聲信號(hào)，提高了陀螺數(shù)據(jù)的輸出精度，減小了噪聲對(duì)信號(hào)的影響。

1 盲均衡迭代反卷積算法

微機(jī)電陀螺在測(cè)量數(shù)據(jù)時(shí)，由于傳感器自身的精度誤差與外界干擾的共同作用下，會(huì)產(chǎn)生零偏和隨機(jī)游走。為了便于對(duì)角速率信號(hào)進(jìn)行分析，將微機(jī)電陀螺及其內(nèi)部信號(hào)進(jìn)行建模，陀螺信號(hào)盲均衡模型如圖1所示，x(n)為待檢測(cè)平臺(tái)角速率真值，該真值通過(guò)微機(jī)電結(jié)構(gòu)轉(zhuǎn)換成電信號(hào)，但在轉(zhuǎn)換過(guò)程中信號(hào)會(huì)產(chǎn)生失真體現(xiàn)為零偏，該信號(hào)失真可認(rèn)為是微機(jī)電結(jié)構(gòu)誤差與角速率真值進(jìn)行卷積運(yùn)算所產(chǎn)生的。之后與噪聲信號(hào)v(n)相加體現(xiàn)為隨機(jī)游走，最終產(chǎn)生陀螺傳感器的輸出信號(hào)u(n)。盲均衡器是對(duì)輸入信號(hào)u(n)進(jìn)行迭代反卷積運(yùn)算與非線性估計(jì)[7-8]，使盲均衡器的輸出x?(n)盡量接近輸入數(shù)據(jù)x(n)。盲均衡器內(nèi)部結(jié)構(gòu)如圖2所示，e(n)為迭代反卷積的估計(jì)誤差[9-10]；w?(n)為橫向?yàn)V波器的權(quán)值系數(shù)，根據(jù) LMS算法與迭代反卷積的估計(jì)誤差，對(duì)橫向?yàn)V波器的權(quán)值系數(shù)進(jìn)行實(shí)時(shí)更新，實(shí)現(xiàn)了自適應(yīng)調(diào)整；g(·)為非線性估計(jì)器。

圖1 陀螺信號(hào)盲均衡模型Fig.1 Blind equalization for gyroscope signal

圖2 盲均衡器結(jié)構(gòu)Fig.2 Structure of blind equalization

1.1 迭代反卷積

反卷積是指通過(guò)測(cè)量輸出和已知輸入，重構(gòu)未知輸入的過(guò)程。利用已知輸入自身的先驗(yàn)信息可知相關(guān)干擾特性，使均衡器的輸出盡量接近輸入，在解卷積前需要對(duì)輸入信號(hào)進(jìn)行辨識(shí)。

橫向?yàn)V波器中輸出信號(hào)與期望信號(hào)之間采用信號(hào)最小均方差準(zhǔn)則為最優(yōu)，基于該準(zhǔn)則的橫向?yàn)V波器可以得到線性的解析解。通過(guò)最小均方誤差函數(shù)可以得到橫向?yàn)V波器權(quán)值系數(shù)。

令式(2)等于零，則有權(quán)值系數(shù):

在解卷積時(shí)，需要記錄當(dāng)前采樣點(diǎn)之前的采樣點(diǎn)的個(gè)數(shù)P，有作為橫向?yàn)V波器的輸入序列，通過(guò)對(duì)輸入信號(hào)的辨識(shí)得到初始化權(quán)值系數(shù)，為。用迭代法進(jìn)行解卷積運(yùn)算，在進(jìn)行n次迭代后得到反卷積序列的輸出

1.2 貝葉斯估計(jì)

由圖 2所示輸入信號(hào)進(jìn)行反卷積后，輸出信號(hào)y(n)輸入給非線性估計(jì)器。該觀測(cè)值y(n)由x(n)決定，并產(chǎn)生x(n)的估計(jì)為觀測(cè)值y(n)的函數(shù)。有

建立代價(jià)函數(shù)的均方誤差函數(shù)為：

將式(8)帶入式(6)中，可得：

由貝葉斯公式有：

式中，fX(x|y)為已知y時(shí)x的條件概率密度函數(shù)，fY(y)為y的邊緣概率密度函數(shù)，式(10)帶入式(9)中，

fY(y)為非負(fù)，則可通過(guò)求式(11)中的內(nèi)積最小化來(lái)得到風(fēng)險(xiǎn)函數(shù)?最小，令L表示式(11)的內(nèi)積，并求導(dǎo)，得

該式的解為唯一最小值。根據(jù)貝葉斯估計(jì)有

式中，fY(y|x)為給定x時(shí)y的條件概率密度函數(shù)，fX(x)為x的邊緣概率密度函數(shù)，則有：

根據(jù)圖1與圖2中所建立的模型，觀測(cè)值y(n)為橫向?yàn)V波器輸出端的反卷積序列，由估計(jì)數(shù)據(jù)序列x(n)與噪聲v(n)組成，且x(n)均值為零、方差為1，v(n)為均值為零的白高斯噪聲，與x(n)統(tǒng)計(jì)獨(dú)立。可表示為

式中，c0為比例因子略小于1，從而使。則有

將式(17)帶入式(15)中，得：

1.3 誤差函數(shù)與LMS算法

在n次迭代后得到估計(jì)值，為了使后續(xù)估計(jì)值有較高的精度，采用自適應(yīng)參數(shù)估計(jì)結(jié)構(gòu)，根據(jù)估計(jì)值的誤差自動(dòng)調(diào)整濾波器參數(shù)。將非線性估計(jì)當(dāng)作期望響應(yīng)，則迭代反卷積過(guò)程的估計(jì)誤差為：

LMS算法采用瞬時(shí)平方誤差的梯度，其自適應(yīng)方程為：

式中，μ為步長(zhǎng)參數(shù)，為權(quán)值系數(shù)的估計(jì)，利用該值估計(jì)出，對(duì)橫向?yàn)V波器的權(quán)值系數(shù)進(jìn)行更新，實(shí)現(xiàn)了自適應(yīng)調(diào)整，提高了估計(jì)精度。

2 實(shí)驗(yàn)驗(yàn)證

2.1 實(shí)驗(yàn)平臺(tái)及傳感器參數(shù)

小型移動(dòng)機(jī)器人平臺(tái)如圖3所示，利用該平臺(tái)進(jìn)行算法驗(yàn)證。選擇的微機(jī)電陀螺參數(shù)如表1所示，陀螺重量為23 g，適合安裝在微小型移動(dòng)機(jī)器人內(nèi)。

實(shí)驗(yàn)驗(yàn)證地圖及移動(dòng)機(jī)器人運(yùn)動(dòng)軌跡如圖 4所示，該實(shí)驗(yàn)環(huán)境為北京理工大學(xué)出版樓6樓。首先，移動(dòng)機(jī)器人放置起點(diǎn)處，延點(diǎn)劃線運(yùn)動(dòng)至折返點(diǎn)，在折返點(diǎn)做180°轉(zhuǎn)向后，按照原路徑返回至起點(diǎn)。實(shí)驗(yàn)全程采集微機(jī)電陀螺數(shù)據(jù)，并使用所設(shè)計(jì)的算法進(jìn)行處理，通過(guò)該算法得到平臺(tái)自身的定位信息。平臺(tái)全程運(yùn)行時(shí)間為4.6 min（275.41 s)。

圖3 小型移動(dòng)機(jī)器人Fig.3 Mini-mobile robot

表1 微機(jī)電陀螺參數(shù)Tab.1 Performance specification for MEMS gyroscope

圖4 建筑物內(nèi)部地圖及移動(dòng)機(jī)器人運(yùn)行軌跡Fig.4 Building interior map and mobile robot trajectory

2.2 結(jié)果分析

根據(jù)圖3中小型移動(dòng)機(jī)器人建立載體坐標(biāo)系，平臺(tái)前進(jìn)方向?yàn)閄軸（滾轉(zhuǎn)角)正向，Y軸（俯仰角)為與X軸垂直并處同一水平面，Z軸（偏航角)為垂直水平面，向上為正向。

實(shí)驗(yàn)記錄了未對(duì)陀螺信號(hào)進(jìn)行盲均衡反卷積算法處理時(shí)，所得三軸角速度數(shù)據(jù)，如圖5所示?？梢钥闯鲆苿?dòng)機(jī)器人在水平地面運(yùn)動(dòng)，滾轉(zhuǎn)角與俯仰角有較大噪聲干擾，是由于機(jī)器人自身機(jī)械結(jié)構(gòu)與地面相互作用所產(chǎn)生的。移動(dòng)機(jī)器人在運(yùn)動(dòng)至樓道各個(gè)轉(zhuǎn)彎處時(shí)，偏航角有明顯數(shù)據(jù)波動(dòng)與之對(duì)應(yīng)，也可見(jiàn)較大噪聲干擾存在。

圖6所示為對(duì)陀螺信號(hào)進(jìn)行盲均衡反卷積處理后，所得到的數(shù)據(jù)結(jié)果。從中可以看出相對(duì)于未處理時(shí)，滾轉(zhuǎn)角和偏航角幅值下降約10倍，有效地抑制了干擾。偏航角相對(duì)未處理時(shí)幅值沒(méi)有下降，但噪聲信號(hào)有明顯削弱。由此可知，該盲均衡迭代反卷積算法可以很好的區(qū)分角速度信號(hào)與噪聲信號(hào)。

圖7所示為陀螺原始信號(hào)幅值譜，該圖可以反映信號(hào)在各頻率下的能量成分。從圖中可以看出，滾轉(zhuǎn)角X與俯仰角Y方向上以40 Hz為中心有較多高頻干擾，這是由于電機(jī)與機(jī)器人的行走機(jī)構(gòu)在地面運(yùn)行時(shí)產(chǎn)生的振動(dòng)。在偏航角Z方向上也有噪聲干擾存在，但幅值相對(duì)較小。

圖8為盲均衡信號(hào)的幅值譜，如圖所示陀螺信號(hào)經(jīng)過(guò)盲均衡器的處理后，滾轉(zhuǎn)角X與俯仰角Y方向上的高頻干擾全部消失，低頻干擾的幅值也有大幅減小。在偏航角Z方向上大部分干擾被消除，而有用信號(hào)的幅值并沒(méi)有減小，說(shuō)明該算法可以有效消除系統(tǒng)噪聲干擾，并保留有效信號(hào)。

在實(shí)驗(yàn)驗(yàn)證過(guò)程中，移動(dòng)機(jī)器人沿著預(yù)定路線行走，經(jīng)過(guò)折返點(diǎn)返回至起點(diǎn)。理論上陀螺所測(cè)滾轉(zhuǎn)角X與俯仰角Y均為0°，偏航角Z由于是返回方向應(yīng)為+180°或-180°。表2所示為完成預(yù)定行走路徑后，陀螺所測(cè)量機(jī)器人在終點(diǎn)時(shí)的角度。為了對(duì)比算法效果，同時(shí)列出未經(jīng)信號(hào)處理時(shí)的移動(dòng)機(jī)器人終點(diǎn)定位角度。從表2可以得出本文所提出的盲均衡迭代反卷積算法在處理微機(jī)電陀螺信號(hào)時(shí)，可以有效去除噪聲干擾，使偏航角誤差從13°下降到1.46°，提高了移動(dòng)機(jī)器人定位精度。

圖5 陀螺輸出三軸原始信號(hào)Fig.5 3-axis original signal for gyroscope

圖6 陀螺信號(hào)盲均衡反卷積結(jié)果Fig.6 Blind equalization result for gyroscope

圖7 三軸原始信號(hào)幅值譜Fig.7 Amplitude spectrum of 3-axis original signal

圖8 盲均衡信號(hào)的幅值譜Fig.8 Amplitude spectrum of blind equalization signal

表2 移動(dòng)機(jī)器人運(yùn)動(dòng)陀螺所測(cè)量的角度Tab.2 Angle of measuring for robot movement

3 結(jié) 論

本文提出一種微機(jī)電陀螺信號(hào)盲均衡迭代反卷積算法，通過(guò)對(duì)微機(jī)電陀螺信號(hào)產(chǎn)生原理的分析，利用橫向?yàn)V波器對(duì)陀螺信號(hào)進(jìn)行反卷積運(yùn)算，使用貝葉斯估計(jì)對(duì)角速度信號(hào)進(jìn)行非線性估計(jì)，并建立了誤差函數(shù)與LMS算法組合，實(shí)現(xiàn)了均衡器參數(shù)的動(dòng)態(tài)調(diào)整。在移動(dòng)機(jī)器人上進(jìn)行了算法驗(yàn)證，實(shí)驗(yàn)結(jié)果表明，該算法可以有效地分離角速度信號(hào)與噪聲信號(hào)，提高了移動(dòng)機(jī)器人的定位精度，其算法可為相關(guān)領(lǐng)域應(yīng)用提供借鑒。

（References):

[1]Sun F,SUN W.Mooring alignment for marine SINS using the digital filter[J].Measurement,2010,43(10):1489-1494.

[2]Beylkin G,Lewis R D,Monzon L.On the design of highly accurate and efficient IIR and FIR filter[J].IEEE Transactions on Signal Processing,2012,60(8): 4045-4054.

[3]Sabatelli S,Galgani M,Fanucci L,et al.A double-stage kalman filter for orientation tracking with an integrated processor in 9-D IMU[J].IEEE Transactions on Instrumentation and Measurement,2013,62(3): 590-598.

[4]Bekkeng J K.Calibration of a novel MEMS inertial reference unit[J].IEEE Transactions on Instrumentation and Measurement,2009,58(6): 1967-1974.

[5]Peesapati R,Sabat S L,Anumandla K K,et al.Design and implementation of a realtime co-processor for denoising fiber optic gyroscope[J].Digital Signal Processing,2013,23(5): 1813-1825.

[6]Broquetas A,Cimeron A,Gelonch A,et al.Track detection in railway sidings based on MEMS gyroscope sensors[J].Sensors,2012,12(12): 16228-16249.

[7]Chesneau C,Fadili J.Wavelet-based density estimation in a heteroscedastic convolution model[J].Communications in Statistics-Theory and Methods,2013,42(17): 3085-3099.

[8]趙永平，許鵬，楊碩.用窗口卷積實(shí)現(xiàn)反卷積的算法研究[J].電子學(xué)報(bào)，2005，33(8)：1533-1536.ZHAO Yong-ping,XU Peng,YANG Shuo.Study on an algorithm of deconvolution via window convolution [J].Chinese Journal of Electronics,2005,33(8):1533-1536.

[9]毛玉良，陳家斌，宋春雷，等.捷聯(lián)慣導(dǎo)姿態(tài)誤差模型分析[J].中國(guó)慣性技術(shù)學(xué)報(bào)，2013，21(2)：182-185.MAO Yu-liang,CHEN Jia-bin,SONG Chun-lei,et al.Analysis of attitude error models of strapdown inertial navigation system[J].Journal of Chinese Inertial Technology,2013,21(2): 182-185.

[10]Bhatt D,Aggarwal P,Bhattacharya P,et al.An enhanced MEMS error modeling approach based on nu-support vector regression[J].Sensors,2012,12(7): 9448-9466.

Iterative deconvolution algorithm of blind equalization for MEMS gyroscope signal

JIANG Tao,WANG Jian-zhong,SHI Jia-dong
(State Key Laboratory of Explosion Science and Technology,Beijing Institute of Technology,Beijing 100081,China)

The mathematical model of a mobile mini-robot running in unknown environment cannot be effectively built due to gyroscope’s noise interference,and needs to remove the noise only from the observing signal and estimate the original signal.In this paper,an iterative deconvolution algorithm of blind equalization for MEMS gyroscope signal is presented.The transversal filter for the gyroscope signal to implement deconvolution calculation is employed,and the signal is estimated by Bayesian methods.The error function is established and combined with LMS algorithm to achieve the automatic adjustment of equalization parameter.The verification of the algorithm is carried out on the mini-mobile robot.The experiment results show that the angular velocity and noise signals can be effectively extracted from the mixed signals,and the amplitude of noise signal is decreased to about 1/10 of the original.After the mobile robot has run 275.41 s,the error of its final yaw angle for mobile robot is reduced from 13° to 1.46°.

mobile robot; MEMS gyroscope; blind equalization; deconvolution; filter

U666.12

：A

1005-6734(2014)02-0237-05

10.13695/j.cnki.12-1222/o3.2014.02.018

2013-11-20；

：2014-03-17

國(guó)防基礎(chǔ)科研計(jì)劃資助項(xiàng)目（B2220110013）；總裝預(yù)研項(xiàng)目（104060202）

姜濤（1984—），男，博士研究生，從事微小型無(wú)人作戰(zhàn)平臺(tái)導(dǎo)航研究。E-mail：eli_jiang@126.com

聯(lián) 系 人：王建中（1963—），男，教授，博士生導(dǎo)師。E-mail：cwjzwang@bit.edu.cn