張文祥,徐林森,孔令成

(1.常州大學(xué)微電子與控制工程學(xué)院,常州 213164;2.河海大學(xué)機(jī)電工程學(xué)院,常州 213022;3.中國(guó)科學(xué)院合肥物質(zhì)科學(xué)研究院先進(jìn)制造技術(shù)研究所,常州 213164)

0 引言

六軸力傳感器能同時(shí)檢測(cè)空間三維坐標(biāo)下的力分量(Fx,Fy,Fz)以及繞這3個(gè)坐標(biāo)的力矩分量(Mx,My,Mz)[1]。它能從實(shí)際物理環(huán)境中獲取相關(guān)力的交互信息,廣泛用于機(jī)器人柔性裝配、機(jī)械臂抓取、觸覺(jué)感知、虛擬現(xiàn)實(shí)等領(lǐng)域[2]。然而力傳感器由于其敏感元件,信號(hào)采集、放大電路的熱噪聲和外界環(huán)境的電磁干擾等原因,使得傳感器的輸出信號(hào)出現(xiàn)沒(méi)有明顯規(guī)律的信號(hào)漂移,信號(hào)不穩(wěn)定,信號(hào)波動(dòng)較大等現(xiàn)象,影響傳感器的測(cè)量精度[3]。

Kalman濾波是一種高效率的自回歸濾波算法,它使用一系列隨時(shí)間觀(guān)察到的數(shù)據(jù),其中包含噪聲和其他不準(zhǔn)確的數(shù)據(jù),來(lái)更準(zhǔn)確地估計(jì)新的狀態(tài)估計(jì)值[4]。它具有實(shí)時(shí)、快速、高效、抗干擾性強(qiáng)等優(yōu)點(diǎn),因此適合用于實(shí)時(shí)信號(hào)處理[5]。然而傳統(tǒng)的卡爾曼濾波算法由于建模誤差,噪聲模型很難被確定等因素的影響,實(shí)際模型與建立的模型之間存在較大的偏差[6]。如果過(guò)分依賴(lài)有偏差的模型會(huì)導(dǎo)致結(jié)果的誤差累計(jì),其估計(jì)精度會(huì)大大降低,嚴(yán)重的時(shí)候可能會(huì)造成濾波的結(jié)果發(fā)散,出現(xiàn)較大的偏差[7]。

針對(duì)力傳感器的卡爾曼濾波問(wèn)題,孫正宇等[8]設(shè)計(jì)了基于正態(tài)分布的滑動(dòng)平均值濾波算法,雖然有效地抑制了過(guò)高或者過(guò)低的噪聲,但是無(wú)法實(shí)現(xiàn)對(duì)真實(shí)信號(hào)的高精度擬合,與真實(shí)值相比存在一定的誤差。何飛等[9]使用雙漸消Kalman濾波來(lái)補(bǔ)償噪聲模型誤差,實(shí)驗(yàn)表明能有效降低噪聲模型偏差帶來(lái)的影響,但是濾波之后的信號(hào)存在過(guò)高或者過(guò)低的值,平滑性較差。周啟帆等[10]通過(guò)自適應(yīng)調(diào)整噪聲協(xié)方差來(lái)跟蹤噪聲變化,但是忽略了新息方差帶來(lái)的影響。CHANG等[11-12]通過(guò)引入漸消因子來(lái)調(diào)整測(cè)量噪聲協(xié)方差矩陣,提升了濾波精度,但是同樣忽略了新息方差在濾波過(guò)程中的作用。

上述研究中,均未同時(shí)考慮觀(guān)測(cè)噪聲協(xié)方差和預(yù)測(cè)估計(jì)協(xié)方差對(duì)濾波效果的影響,同時(shí)還忽略了信號(hào)輸出的平滑性。針對(duì)這一系列問(wèn)題,本文設(shè)計(jì)了一種混合濾波算法,利用簡(jiǎn)化的力傳感器模型,先將原始數(shù)據(jù)經(jīng)過(guò)自適應(yīng)卡爾曼濾波算法處理,使觀(guān)測(cè)噪聲協(xié)方差和預(yù)測(cè)估計(jì)協(xié)方差隨著測(cè)量值自適應(yīng)變化,提高對(duì)真實(shí)值的擬合度,然后再使用滑動(dòng)平均值濾波對(duì)數(shù)據(jù)進(jìn)行二次處理,抑制高頻噪聲,提升數(shù)據(jù)的平滑度?；旌蠟V波算法實(shí)現(xiàn)了對(duì)力傳感器真實(shí)值的高度擬合且輸出數(shù)據(jù)更加平滑。實(shí)驗(yàn)結(jié)果表明,本文算法優(yōu)于傳統(tǒng)卡爾曼濾波、自適應(yīng)卡爾曼濾波和引言文獻(xiàn)中的方法,降噪效果明顯,波形更加平滑。

1 力傳感器信號(hào)噪聲混合濾波原理

力傳感器信號(hào)的混合濾波由自適應(yīng)卡爾曼濾波和滑動(dòng)平均值濾波組成,算法流程如圖1所示。接下來(lái)對(duì)所設(shè)計(jì)濾波算法作具體說(shuō)明。

圖1 混合濾波流程圖

1.1 力傳感器信號(hào)噪聲的自適應(yīng)卡爾曼濾波

自適應(yīng)卡爾曼濾波是建立在標(biāo)準(zhǔn)卡爾曼濾波的基礎(chǔ)上。在卡爾曼濾波模型中,當(dāng)前時(shí)刻的狀態(tài)是由前一時(shí)刻的狀態(tài)變化而來(lái)。其狀態(tài)方程如式(1)所示。

xk=Ak,k-1xk-1+Bk,k-1uk-1+wk-1

(1)

式中:xk是k時(shí)刻的系統(tǒng)狀態(tài)變量,Ak,k-1是作用在前k-1時(shí)刻狀態(tài)變換轉(zhuǎn)移矩陣,Bk,k-1是系統(tǒng)輸入關(guān)系矩陣,uk-1是系統(tǒng)輸入變量,wk-1是系統(tǒng)過(guò)程噪聲并假設(shè)wk-1符合方差為Qk,期望為0的高斯分布,即wk-1～N(0,Qk)。

真實(shí)狀態(tài)的測(cè)量滿(mǎn)足:

zk=Hkxk+vk

(2)

式中:zk是k時(shí)刻的測(cè)量值,Hk是觀(guān)測(cè)模型,vk是觀(guān)測(cè)噪聲并假設(shè)vk符合方差為Rk,期望為0的高斯分布,即vk～N(0,Rk)。

卡爾曼濾波器的操作分為預(yù)測(cè)和更新兩個(gè)階段。在預(yù)測(cè)階段利用上一時(shí)刻的狀態(tài)估計(jì)做出對(duì)當(dāng)前狀態(tài)的估計(jì)。預(yù)測(cè)階段方程如式(3)和式(4)所示。

(3)

(4)

更新前首先計(jì)算出新息,新息的協(xié)方差和最優(yōu)卡爾曼增益分別如式(5)～式(7)所示。

(5)

(6)

(7)

用式(5)～式(7)來(lái)更新?tīng)顟B(tài)變量和協(xié)方差估計(jì),如式(8)和式(9)所示,其中式(9)是在最優(yōu)卡爾曼增益的情況下得出的公式。

(8)

(9)

1.1.1 漸消因子的計(jì)算

在六軸力傳感器噪聲處理中,引發(fā)標(biāo)準(zhǔn)卡爾曼濾波發(fā)散的一個(gè)重要原因是當(dāng)系統(tǒng)模型不準(zhǔn)確時(shí),觀(guān)測(cè)值對(duì)估計(jì)值的矯正作用下降,而之前時(shí)刻觀(guān)測(cè)值在系統(tǒng)中的修正作用相對(duì)上升[13]。因此濾波的新息很容易被當(dāng)前時(shí)刻的不完整的觀(guān)測(cè)值影響,所以要想阻止濾波發(fā)散,就要提高新息在當(dāng)前時(shí)刻濾波中的比重[14]。在標(biāo)準(zhǔn)卡爾曼濾波式(4)的基礎(chǔ)上加入漸消因子λk。改進(jìn)后的預(yù)測(cè)估計(jì)協(xié)方差如式(10)所示,其中λk≥1。

(10)

(11)

令:

(12)

從以上結(jié)論出發(fā),由式(7)可知預(yù)測(cè)估計(jì)的協(xié)方差影響卡爾曼增益,通過(guò)式(10)中的漸消因子來(lái)實(shí)時(shí)調(diào)整卡爾曼增益。為了得到調(diào)整之后最優(yōu)卡爾曼增益,令:

(13)

由式(7)和式(13)得新的卡爾曼增益為:

(14)

由式(14)可知,能通過(guò)確定最優(yōu)的λk,μk來(lái)得到最優(yōu)卡爾曼增益。μk的值由式(13)來(lái)確定,其中sk的值通過(guò)標(biāo)準(zhǔn)卡爾曼濾波方程得到,使用如式(15)所示的類(lèi)似于sage濾波的開(kāi)窗法來(lái)確定區(qū)間長(zhǎng)度為N的新息序列方差的估計(jì)值。

(15)

由于μk≥1,則μk的估計(jì)值為:

(16)

將式(7)帶入式(12)得:

(17)

(18)

將式(10)帶入式(18)得:

(19)

因此通過(guò)式(19)可以得到λk,如式(20)所示。

(20)

1.1.2 過(guò)程噪聲和測(cè)量噪聲的在線(xiàn)估計(jì)

在六軸力傳感器的標(biāo)準(zhǔn)卡爾曼濾波過(guò)程中,過(guò)程噪聲和測(cè)量噪聲無(wú)法精確獲得,而根據(jù)經(jīng)驗(yàn)法選取的過(guò)程噪聲和觀(guān)測(cè)噪聲的準(zhǔn)確性會(huì)直接影響到標(biāo)準(zhǔn)卡爾曼濾波的性能,最嚴(yán)重的后果就是狀態(tài)估計(jì)發(fā)散而徹底無(wú)法工作。為了提高卡爾曼濾波的性能,減小濾波的實(shí)際誤差,提出測(cè)量噪聲協(xié)方差R和過(guò)程噪聲協(xié)方差Q的估計(jì)方法。

(21)

式中:φ為噪聲方差自適應(yīng)調(diào)整系數(shù)。

假設(shè)測(cè)量噪聲方差與測(cè)量噪聲方差系數(shù)無(wú)關(guān)則式(21)可化簡(jiǎn)為式(22),再進(jìn)一步化簡(jiǎn)為式(15)。

(22)

(23)

同理假設(shè)過(guò)程噪聲的協(xié)方差與過(guò)程噪聲方差系數(shù)無(wú)關(guān)則式(21)可化簡(jiǎn)為式(24)。

(24)

將式(24)與式(3)～式(7)聯(lián)立得到式(25)過(guò)程噪聲協(xié)方差的估計(jì):

(25)

1.2 力傳感器信號(hào)噪聲的滑動(dòng)平均值濾波

滑動(dòng)平均濾波器是一種有限脈沖響應(yīng)(FIR)濾波器,通常用于分析時(shí)域中的時(shí)間序列,經(jīng)過(guò)處理之后的數(shù)據(jù)相比于原始數(shù)據(jù)來(lái)說(shuō),平滑度較高。在濾波過(guò)程中,使用信號(hào)沿時(shí)間軸的N個(gè)樣本來(lái)計(jì)算時(shí)間統(tǒng)計(jì)數(shù)據(jù),并對(duì)時(shí)間移動(dòng)窗口中的樣本進(jìn)行平均,以產(chǎn)生不同時(shí)間點(diǎn)的輸出結(jié)果。即N個(gè)數(shù)據(jù)為一組,按照先進(jìn)先出的原則,每來(lái)一個(gè)新數(shù)據(jù)就將其放入尾部,首部數(shù)據(jù)彈出隊(duì)列,形成長(zhǎng)度始終是N的數(shù)據(jù)組,然后求這組數(shù)據(jù)的平均值?；瑒?dòng)平均值濾波表達(dá)式如式(23)所示。

(26)

式中:ο(k)是經(jīng)過(guò)滑動(dòng)平均值濾波器處理之后的輸出,x(k+i)是濾波器的輸入。

2 六軸力傳感器系統(tǒng)模型

建立六軸力傳感器的系統(tǒng)測(cè)試模型可以從單維力傳感器測(cè)試模型出發(fā),然后再推廣到六維力傳感器。單維力傳感器模型由文獻(xiàn)[17]可知,應(yīng)變體,放大電路,模擬信號(hào)調(diào)理電路能等效為一階慣性環(huán)節(jié),采樣電路能等效為零階保持器。單維力傳感器的狀態(tài)方程如式(27)所示。

xk=e-aTxk-1+(1-e-aT)uk+wk-1
zk=xk+vk

(27)

式中:uk為施加在力傳感器上的真實(shí)值,zk為力傳感器的輸出值,T為采樣間隔時(shí)間,vk為觀(guān)測(cè)器噪聲,a為傳感器固有頻率。

與式(1)和式(2)相比Ak,k-1=e-aT,Bk,k-1=1-e-aT,Hk=1。單維力傳感器的模型和濾波算法推廣到六維力傳感器需要滿(mǎn)足文獻(xiàn)[18]中的以下兩個(gè)條件:①傳感器工作在量程范圍之內(nèi),且應(yīng)變體的變化與所施加的力呈現(xiàn)線(xiàn)性關(guān)系。②在工作量程之內(nèi),傳感器的解耦合矩陣內(nèi)各個(gè)元素都為常數(shù)。

在實(shí)際應(yīng)用中,六軸力傳感器均可以滿(mǎn)足這兩個(gè)條件。因此,本文的單維力傳感器的系統(tǒng)測(cè)試模型能推廣到六軸力傳感器的系統(tǒng)測(cè)試模型。

3 實(shí)驗(yàn)分析

3.1 實(shí)驗(yàn)流程

本文采用某公司的型號(hào)為M3813D的六軸力傳感器,搭配其公司生產(chǎn)的M8128數(shù)據(jù)采集卡來(lái)驗(yàn)證混合濾波算法的可行性和降噪性能。此款六軸力傳感器經(jīng)過(guò)該公司的設(shè)計(jì)和計(jì)算,得到該傳感器的一階固有頻率為2000 Hz,其應(yīng)變體變化與所施加的力成線(xiàn)性關(guān)系,傳感器的測(cè)量力的量程范圍為0～±260 N,測(cè)量力矩范圍為0～±12 N·m。采集卡內(nèi)部集成了六軸力傳感器解耦運(yùn)算功能,通過(guò)Ethernet總線(xiàn)與主機(jī)進(jìn)行通訊,采樣率為10 Hz～20 kHz,具有24位sigma-delta ADC,同時(shí)能為力傳感器的應(yīng)變體提供激勵(lì),然后對(duì)產(chǎn)生的信號(hào)進(jìn)行放大。實(shí)驗(yàn)時(shí),設(shè)置采集卡的采樣頻率為100 Hz。

實(shí)驗(yàn)平臺(tái)如圖2所示。將傳感器放置于平穩(wěn)且無(wú)外部干擾的環(huán)境中。在傳感器Z軸正方向施加10 N的力,即Z軸的力矩和其他方向的力、力矩都為0。從采集的一系列數(shù)據(jù)中選取一組連續(xù)的數(shù)據(jù)。然后將這組數(shù)據(jù)分別使用標(biāo)準(zhǔn)卡爾曼濾波,自適應(yīng)卡爾曼濾波和混合濾波算法進(jìn)行降噪處理。將卡爾曼濾波中的過(guò)程噪聲協(xié)方差Q和測(cè)量噪聲協(xié)方差R分別設(shè)置為1,10;滑動(dòng)平均值樣本數(shù)N=10。

圖2 六軸力傳感器實(shí)驗(yàn)平臺(tái)

3.2 實(shí)驗(yàn)結(jié)果分析

從圖3中可以看出,自適應(yīng)卡爾曼濾波和標(biāo)準(zhǔn)卡爾曼濾波對(duì)力傳感器的隨機(jī)突變?cè)肼曈幸欢ǖ囊种谱饔?他們?cè)诔跏茧A段的相對(duì)誤差都較大,標(biāo)準(zhǔn)卡爾曼濾波收斂到真實(shí)值附近的速度略微快于自適應(yīng)卡爾曼濾波。從局部放大圖3b中看出自適應(yīng)卡爾曼濾波相對(duì)于標(biāo)準(zhǔn)卡爾曼濾波來(lái)說(shuō)對(duì)隨機(jī)突變信號(hào)的抑制效果較好,信號(hào)波動(dòng)性小,所以自適應(yīng)卡爾曼濾波對(duì)傳感器真實(shí)值的擬合度高于標(biāo)準(zhǔn)卡爾曼濾波,能夠有效提高傳感器的測(cè)量精度。

(a) 濾波整體對(duì)比圖 (b) 局部放大圖

再對(duì)經(jīng)過(guò)自適應(yīng)卡爾曼濾波處理的數(shù)據(jù)進(jìn)行滑動(dòng)平均值濾波處理,數(shù)據(jù)對(duì)比如圖4所示。經(jīng)過(guò)滑動(dòng)平均值濾波處理之后的波形和未處理的波形都能得到與實(shí)際值相差不大的較為準(zhǔn)確的濾波結(jié)果?；旌蠟V波算法的輸出穩(wěn)定在9.93～10.12 N,自適應(yīng)卡爾曼濾波算法穩(wěn)定在9.72～10.35 N,未經(jīng)滑動(dòng)平均值濾波處理的波形相鄰兩點(diǎn)之間的幅值波動(dòng)較大,波形不如經(jīng)過(guò)滑動(dòng)平均值濾波處理的數(shù)據(jù)平滑。因此,經(jīng)過(guò)滑動(dòng)平均值濾波可以有效提高數(shù)據(jù)的平滑性,減少峰值的出現(xiàn),提高傳感器測(cè)量的穩(wěn)定性和準(zhǔn)確性。

(a) 濾波整體對(duì)比圖 (b) 局部放大圖

標(biāo)準(zhǔn)卡爾曼濾波和自適應(yīng)卡爾曼濾波與真實(shí)值的誤差如圖5所示。從圖中可以看出標(biāo)準(zhǔn)卡爾曼濾波與施加在力傳感器上的真實(shí)值之間的誤差和波動(dòng)幅度較大。標(biāo)準(zhǔn)卡爾曼濾波的最大誤差為0.92 N,自適應(yīng)卡爾曼濾波為0.23 N,最大誤差降低為傳統(tǒng)卡爾曼濾波的0.25倍,所以自適應(yīng)卡爾曼濾波有更高的濾波精度。自適應(yīng)卡爾曼濾波由于在標(biāo)準(zhǔn)卡爾曼濾波的基礎(chǔ)上引入了漸消因子來(lái)抑制濾波發(fā)散,同時(shí)不斷在線(xiàn)估計(jì)、修正過(guò)程噪聲和測(cè)量噪聲的協(xié)方差,所以自適應(yīng)卡爾曼濾波的誤差較小且誤差幅度變化基本穩(wěn)定。

圖5 Fz=10 N標(biāo)準(zhǔn)卡爾曼濾波和自適應(yīng)卡爾曼濾波的誤差

Fz=10 N時(shí),本文使用的濾波算法與引言中所使用的Kalman濾波算法對(duì)比如圖6所示。在圖6a中經(jīng)過(guò)文獻(xiàn)[9-12]使用的濾波算法處理之后的力傳感器數(shù)據(jù)與力傳感器的真實(shí)值相比,二者之間的最大誤差分別為0.36 N、0.32 N、0.12 N,文獻(xiàn)[9]的方法出現(xiàn)的誤差最大。在圖6b的局部放大圖中,本文的混合濾波算法最接近真實(shí)值,且波動(dòng)較小。在圖6c中,文獻(xiàn)[8]的方法雖然在真實(shí)值附近波動(dòng),但是出現(xiàn)了較大的波動(dòng)點(diǎn),沒(méi)有本文混合濾波算法處理之后的數(shù)據(jù)平滑。因此,本文中的混合濾波算法相比于引言文獻(xiàn)中所使用的算法,能更好地?cái)M合力傳感器真實(shí)值,且獲得的濾波后的數(shù)據(jù)更加平滑穩(wěn)定。

(a) 濾波整體對(duì)比圖 (b) 局部放大圖

(c) 局部放大圖

實(shí)驗(yàn)結(jié)果表明:在力傳感器的信號(hào)噪聲處理中,混合濾波算法綜合了兩種濾波的優(yōu)點(diǎn),既提升了去噪能力,又提高了數(shù)據(jù)的平滑性。相比于標(biāo)準(zhǔn)卡爾曼濾波、自適應(yīng)卡爾曼濾波和引言文獻(xiàn)中的濾波方法,混合濾波算法的性能得到了顯著的提升,減小了測(cè)量誤差,提高了力傳感器測(cè)量的精度和穩(wěn)定性。

4 結(jié)論

本文針對(duì)六軸力傳感器信號(hào)噪聲的處理問(wèn)題和傳統(tǒng)卡爾曼濾波算法的局限性,提出了基于自適應(yīng)卡爾曼濾波算法和滑動(dòng)平均值濾波算法的混合濾波算法,搭建了以六軸力傳感器為數(shù)據(jù)源的實(shí)驗(yàn)平臺(tái),利用簡(jiǎn)化的力傳感器模型,通過(guò)引入漸消因子來(lái)抑制可能存在的因力傳感器建模不精確而導(dǎo)致的濾波發(fā)散問(wèn)題,同時(shí)對(duì)系統(tǒng)和觀(guān)測(cè)噪聲的協(xié)方差進(jìn)行動(dòng)態(tài)估計(jì)來(lái)提高測(cè)量精度,接下來(lái)使用滑動(dòng)平均值濾波來(lái)提升數(shù)據(jù)的平滑度,最后將算法推廣到多維力傳感器的情況下。與傳統(tǒng)卡爾曼濾波和文獻(xiàn)中的方法對(duì)比分析,結(jié)果表明本文的自適應(yīng)卡爾曼濾波性能優(yōu)于標(biāo)準(zhǔn)卡爾曼濾波,其濾波最大誤差降低為標(biāo)準(zhǔn)卡爾曼濾波的0.25倍、有效地濾除了干擾信號(hào);使用滑動(dòng)平均值濾波之后,輸出信號(hào)穩(wěn)定在9.93～10.12 N,比只使用自適應(yīng)卡爾曼濾波精度提高了0.21～0.23 N,且波動(dòng)幅度更小,平滑性更好;相比于引言文獻(xiàn)中的方法,其對(duì)真實(shí)值的擬合度和數(shù)據(jù)平滑性更佳。說(shuō)明本文算法能有效提高力傳感器的測(cè)量精度和輸出的平滑性。