趙 帥，肖金壯，郭 一

(河北大學(xué)，保定 071000)

0 引 言

伺服系統(tǒng)中電機(jī)轉(zhuǎn)速是閉環(huán)控制必不可少的反饋量，其速度檢測傳感器多為光電編碼器，通過編碼器輸出的兩路正交脈沖來確定速度大小及方向。目前主流的測速方法為M法、T法、M/T法[1-3]。M法通過測量單位時間編碼器脈沖數(shù)確定速度大小，適合高速測量；T法通過測量編碼器脈沖脈寬確定速度大小，適合低速測量；M/T法測量單位時間編碼器脈沖數(shù)，同時測量脈寬。相對M法或T法，M/T法測量范圍更寬，準(zhǔn)確度更高，但算法相對復(fù)雜。以上方法在實(shí)際應(yīng)用中均存在噪聲，其中T法的噪聲較大。文獻(xiàn)[4]采用閾值法切換M法與T法，即高速時利用M法，低速時利用T法，降低算法復(fù)雜度，未實(shí)現(xiàn)對噪聲的濾波。文獻(xiàn)[5]使用FPGA在M/T法的基礎(chǔ)上動態(tài)估計(jì)電機(jī)速度，提高了動態(tài)特性，但只采用簡單硬件濾波。文獻(xiàn)[6]使用FPGA在M/T法的基礎(chǔ)上引入滑動窗思想，提高算法動態(tài)性能，沒進(jìn)行噪聲濾波。為抑制測量噪聲，同時提高算法動態(tài)性能，本文提出改進(jìn)型卡爾曼濾波測速法。

1 改進(jìn)型卡爾曼濾波電機(jī)測速法

1.1 卡爾曼濾波

卡爾曼濾波[7-8]是20世紀(jì)60年代發(fā)展起來的一種現(xiàn)代理論，多數(shù)據(jù)融合的重要工具，通過遞歸更新狀態(tài)，每次更新狀態(tài)只通過上一次的狀態(tài)最優(yōu)估計(jì)值和最新的觀測值融合計(jì)算得出，其中狀態(tài)最優(yōu)估計(jì)值維數(shù)小于等于觀測值維數(shù)。使用卡爾曼濾波器對電機(jī)速度濾波時采用其離散形式。其離散化的狀態(tài)方程由以下差分方程式(1)～式(6)描述。

(1)

式中：Fk為狀態(tài)轉(zhuǎn)移矩陣；Bk為控制輸入矩陣；wk為過程噪聲向量。

(2)

式中：Qk為過程激勵噪聲協(xié)方差矩陣。

當(dāng)前時刻的測量值zk：

zk=Hkxk+vk

(3)

式中：Hk為轉(zhuǎn)換矩陣；vk為測量噪聲向量。

(4)

式中：Kk為卡爾曼系數(shù)，可由式(6)得到。

由式(2)得出的估計(jì)協(xié)方差計(jì)算當(dāng)前時刻協(xié)方差矩陣Pk|k：

(5)

計(jì)算卡爾曼系數(shù)：

(6)

式中：Rk為觀測噪聲協(xié)方差矩陣。

1.2 改進(jìn)型卡爾曼濾波電機(jī)測速原理

本文提出一種改進(jìn)型卡爾曼濾波算法對電機(jī)速度進(jìn)行濾波，其狀態(tài)最優(yōu)估計(jì)值與觀測值均為一維，即速度。計(jì)算流程如圖1所示。其中，vM是M法測得速度值，vT是T法測得速度值。

圖1 計(jì)算流程圖

算法過程具體描述如下：

同時計(jì)算當(dāng)前時刻M法和T法的相對誤差[4]εMk和εTk：

(7)

(8)

式中：Qpk為當(dāng)前時刻檢測時間ΔT內(nèi)測量的編碼器脈沖數(shù)；Qck為當(dāng)前時刻一個編碼器脈沖內(nèi)系統(tǒng)時鐘脈沖數(shù)。

比較2個當(dāng)前時刻相對誤差，誤差小的測速法測出的速度值vεs作為當(dāng)前時刻的觀測值，誤差大的測速法測出的速度值vεb與上一時刻得到的最優(yōu)估計(jì)速度值進(jìn)行平均值濾波，所得數(shù)據(jù)作為當(dāng)前時刻估計(jì)值。因?yàn)V波時無輸入，即uk=0，同時令wk=0，忽略過程噪聲。當(dāng)前時刻估計(jì)值由下式計(jì)算:

(9)

(10)

為減少算法復(fù)雜度，令過程激勵噪聲協(xié)方差為常數(shù)q。

忽略測量噪聲，當(dāng)前時刻的測量值vk：

vk=vεs

(11)

(12)

式中：Kk為卡爾曼系數(shù)，可由以下式(14)得到。

由式(10)得出的估計(jì)協(xié)方差計(jì)算當(dāng)前時刻協(xié)方差pk|k：

(13)

卡爾曼系數(shù)：

(14)

為減少算法復(fù)雜度，令觀測噪聲協(xié)方差r為常值。

通過以上遞歸過程可保證良好動態(tài)性能，同時對電機(jī)速度數(shù)據(jù)進(jìn)行噪聲抑制。

2 仿真對比實(shí)驗(yàn)

本文利用MATLAB在離線情況下對采集的伺服電機(jī)速度數(shù)據(jù)進(jìn)行仿真，將本文的算法測速結(jié)果與傳統(tǒng)卡爾曼濾波算法測速結(jié)果對比，實(shí)驗(yàn)數(shù)據(jù)采集及結(jié)果分析如下所述。

2.1 電機(jī)速度數(shù)據(jù)采集

本實(shí)驗(yàn)平臺為伺服系統(tǒng)，如圖2所示，該系統(tǒng)由Panasonic的MSMA042A1G型電機(jī)和MSDA043A1A型伺服單元組成，電機(jī)尾部裝有2500線的光電編碼器，并通過工控機(jī)控制，其控制與采集流程如圖3所示。

圖2 系統(tǒng)平臺

圖3 原始速度數(shù)據(jù)

工控機(jī)內(nèi)裝有實(shí)時操作系統(tǒng)，可通過Simulink編程對伺服系統(tǒng)實(shí)時控制，在速度模式下，編碼器將速度反饋到工控機(jī)，在工控機(jī)內(nèi)執(zhí)行PD控制，達(dá)到本文所需的各種電機(jī)速度狀態(tài)。使用STM32F103C8T6采集速度數(shù)據(jù)，經(jīng)伺服單元產(chǎn)生編碼器差分信號，該信號經(jīng)AM26LS32芯片變?yōu)閱味诵盘?，此信號可直接輸入到STM32F103C8T6中。M法測速時配置定時器為外部脈沖模式，令ΔT=5ms，T法測速時配置為捕獲模式，計(jì)數(shù)器時鐘為18 MHz，分別測量M法和T法低速300r/min和高速3 000 r/min的數(shù)據(jù)，M法采集ΔT內(nèi)測量的編碼器脈沖數(shù)，T法采集一個編碼器脈沖內(nèi)系統(tǒng)時鐘脈沖數(shù)。每組采集300個數(shù)據(jù)，共4組數(shù)據(jù)。將M法測量的2組數(shù)據(jù)合并為1組，以模擬速度突變時的信號。該組前1/2部分模擬300 r/min的數(shù)據(jù)，后1/2部分模擬3 000 r/min的數(shù)據(jù)。同樣，將T法測量數(shù)據(jù)按照相同規(guī)則合并為1組，使2種方法測量數(shù)據(jù)對稱。

2.2 仿真結(jié)果

使用傳統(tǒng)卡爾曼濾波算法與改進(jìn)的卡爾曼濾波算法分別對數(shù)據(jù)處理。令2種算法的q=0.000 05，r=0.08，其仿真圖如圖4所示。其中，實(shí)線是本文算法所得濾波結(jié)果曲線，虛線是傳統(tǒng)卡爾曼濾波算法濾波結(jié)果曲線。通過對比可見，在電機(jī)轉(zhuǎn)速出現(xiàn)階躍時，2種算法均可實(shí)現(xiàn)噪聲濾波，傳統(tǒng)卡爾曼濾波算法有明顯滯后。

圖4 同參數(shù)仿真圖

為了減小滯后，令傳統(tǒng)算法的參數(shù)q=0.00005，r=0.001，本文的算法參數(shù)不變。其濾波結(jié)果曲線如圖5所示。同樣，實(shí)線為本文算法速度曲線，虛線為傳統(tǒng)算法速度曲線?？梢?，傳統(tǒng)算法滯后減小，但噪聲明顯變大，而本文的算法滯后較小，濾波效果優(yōu)于傳統(tǒng)算法。

圖5 不同參數(shù)仿真圖

3 結(jié) 語

本文采用改進(jìn)型卡爾曼濾波算法對電機(jī)速度數(shù)據(jù)進(jìn)行處理。首先介紹了卡爾曼濾波算法原理，然后通過比較M法與T法的相對誤差，確定估計(jì)值與觀測值，并推導(dǎo)出改進(jìn)型卡爾曼濾波算法，最后通過MATLAB在離線狀態(tài)下，利用本文算法與傳統(tǒng)算法對采集到的電機(jī)速度數(shù)據(jù)進(jìn)行處理。結(jié)果表明，相對傳統(tǒng)卡爾曼濾波算法，本文算法可保證較好的動態(tài)特性，同時具有較好噪聲抑制效果。