張文祥,徐林森,孔令成

(1.常州大學微電子與控制工程學院,常州 213164;2.河海大學機電工程學院,常州 213022;3.中國科學院合肥物質科學研究院先進制造技術研究所,常州 213164)

0 引言

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

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

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

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

1 力傳感器信號噪聲混合濾波原理

力傳感器信號的混合濾波由自適應卡爾曼濾波和滑動平均值濾波組成,算法流程如圖1所示。接下來對所設計濾波算法作具體說明。

圖1 混合濾波流程圖

1.1 力傳感器信號噪聲的自適應卡爾曼濾波

自適應卡爾曼濾波是建立在標準卡爾曼濾波的基礎上。在卡爾曼濾波模型中,當前時刻的狀態(tài)是由前一時刻的狀態(tài)變化而來。其狀態(tài)方程如式(1)所示。

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

(1)

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

真實狀態(tài)的測量滿足:

zk=Hkxk+vk

(2)

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

卡爾曼濾波器的操作分為預測和更新兩個階段。在預測階段利用上一時刻的狀態(tài)估計做出對當前狀態(tài)的估計。預測階段方程如式(3)和式(4)所示。

(3)

(4)

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

(5)

(6)

(7)

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

(8)

(9)

1.1.1 漸消因子的計算

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

(10)

(11)

令:

(12)

從以上結論出發(fā),由式(7)可知預測估計的協(xié)方差影響卡爾曼增益,通過式(10)中的漸消因子來實時調整卡爾曼增益。為了得到調整之后最優(yōu)卡爾曼增益,令:

(13)

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

(14)

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

(15)

由于μk≥1,則μk的估計值為:

(16)

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

(17)

(18)

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

(19)

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

(20)

1.1.2 過程噪聲和測量噪聲的在線估計

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

(21)

式中:φ為噪聲方差自適應調整系數(shù)。

假設測量噪聲方差與測量噪聲方差系數(shù)無關則式(21)可化簡為式(22),再進一步化簡為式(15)。

(22)

(23)

同理假設過程噪聲的協(xié)方差與過程噪聲方差系數(shù)無關則式(21)可化簡為式(24)。

(24)

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

(25)

1.2 力傳感器信號噪聲的滑動平均值濾波

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

(26)

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

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

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

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

(27)

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

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

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

3 實驗分析

3.1 實驗流程

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

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

圖2 六軸力傳感器實驗平臺

3.2 實驗結果分析

從圖3中可以看出,自適應卡爾曼濾波和標準卡爾曼濾波對力傳感器的隨機突變噪聲有一定的抑制作用,他們在初始階段的相對誤差都較大,標準卡爾曼濾波收斂到真實值附近的速度略微快于自適應卡爾曼濾波。從局部放大圖3b中看出自適應卡爾曼濾波相對于標準卡爾曼濾波來說對隨機突變信號的抑制效果較好,信號波動性小,所以自適應卡爾曼濾波對傳感器真實值的擬合度高于標準卡爾曼濾波,能夠有效提高傳感器的測量精度。

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

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

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

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

圖5 Fz=10 N標準卡爾曼濾波和自適應卡爾曼濾波的誤差

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

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

(c) 局部放大圖

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

4 結論

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