卡爾曼濾波在電動(dòng)滑臺(tái)位置實(shí)時(shí)補(bǔ)償中的應(yīng)用

吳鵬飛，單 奇，羅新河，饒 鑫，蔡正凱

(西南交通大學(xué)機(jī)械工程學(xué)院，四川成都 610031)

0 引言

電動(dòng)滑臺(tái)作為主要的直線運(yùn)動(dòng)執(zhí)行機(jī)構(gòu)之一，在精密儀器、加工制造等領(lǐng)域應(yīng)用廣泛。其軌跡跟蹤精度決定了作業(yè)任務(wù)的完成質(zhì)量，而軌跡跟蹤精度主要取決于控制器的計(jì)算精度、系統(tǒng)的響應(yīng)時(shí)間、傳感器的測量精度、零件的制造裝配精度以及減速機(jī)背隙等[1-3]。為了提高電動(dòng)滑臺(tái)的軌跡跟蹤精度，有學(xué)者做了相關(guān)研究。林劍等[4]通過測量滑臺(tái)基座的直線度，選擇較好的一側(cè)安裝主導(dǎo)軌，以自為基準(zhǔn)的原則安裝副導(dǎo)軌，獲得了μm級的直線度，但是未對控制系統(tǒng)做相關(guān)研究。李偉等[5]建立機(jī)床滑臺(tái)位置模糊PID控制模型，改善了系統(tǒng)的響應(yīng)速度和穩(wěn)定性，但對噪聲干擾等未加處理。趙帥等[6]通過改進(jìn)卡爾曼濾波算法對電機(jī)速度進(jìn)行處理，有效地抑制了測量噪聲，但對電機(jī)速度的控制沒有進(jìn)行改善。

本文針對電動(dòng)滑臺(tái)控制系統(tǒng)的軌跡跟蹤誤差以及測量裝置存在的噪聲，導(dǎo)致位置跟蹤誤差累積的問題，提出采用卡爾曼濾波算法對電動(dòng)滑臺(tái)的位置進(jìn)行最優(yōu)估計(jì)，并在此基礎(chǔ)上實(shí)現(xiàn)實(shí)時(shí)補(bǔ)償，使得電動(dòng)滑臺(tái)按照期望的軌跡運(yùn)動(dòng)。

1 卡爾曼濾波

卡爾曼濾波是Rudolf Emil Kalman提出的一種線性最小方差估計(jì)算法[7]，其主要思路是：以最小均方誤差為最佳估計(jì)原則，認(rèn)為系統(tǒng)和觀測都存在噪聲干擾，且屬于高斯白噪聲，建立系統(tǒng)的狀態(tài)空間模型，利用高斯混合模型進(jìn)行迭代求得每個(gè)采樣時(shí)刻的最優(yōu)估計(jì)值。此迭代算法的核心思路是：利用前一時(shí)刻的最優(yōu)估計(jì)值與當(dāng)前時(shí)刻的觀測值來對當(dāng)前時(shí)刻的狀態(tài)變量做出最優(yōu)估計(jì)[8]。采用迭代方法的優(yōu)點(diǎn)是不需要大量的存儲(chǔ)空間，非常適合計(jì)算機(jī)處理[9]。

線性離散系統(tǒng)由系統(tǒng)狀態(tài)方程和系統(tǒng)測量方程表示：

系統(tǒng)狀態(tài)方程：

Xk=AXk-1+BUk+Wk

(1)

系統(tǒng)量測方程：

Zk=HXk+Vk

(2)

式中：Xk為當(dāng)前時(shí)刻系統(tǒng)的n維狀態(tài)向量；A為系統(tǒng)的n×n階狀態(tài)轉(zhuǎn)移矩陣；Xk-1為上一時(shí)刻系統(tǒng)的n維狀態(tài)向量；B為系統(tǒng)控制向量的n×r階增益矩陣;Uk為當(dāng)前時(shí)刻系統(tǒng)的r維控制向量；Wk為當(dāng)前時(shí)刻系統(tǒng)的n維過程噪聲向量，服從高斯分布，協(xié)方差矩陣為Q;Zk為當(dāng)前時(shí)刻系統(tǒng)的m維量測向量；H為m×n階量測矩陣；Vk為當(dāng)前時(shí)刻系統(tǒng)的m維量測噪聲，服從高斯分布，協(xié)方差矩陣為R。

卡爾曼濾波的算法流程[10]如圖1所示。

圖1 卡爾曼濾波算法流程圖

(1)預(yù)測k時(shí)刻的狀態(tài)量，公式如下：

(3)

(2)預(yù)測k時(shí)刻的誤差協(xié)方差矩陣，公式如下：

Pk|k-1=APk-1AT+Q

(4)

式中：Pk|k-1為k時(shí)刻預(yù)測值的誤差協(xié)方差矩陣；Pk-1為k-1時(shí)刻系統(tǒng)狀態(tài)向量的誤差協(xié)方差矩陣；Q為過程噪聲的協(xié)方差矩陣。

(3)計(jì)算k時(shí)刻的卡爾曼增益，公式如下：

Kk=Pk|k-1HT(HPk|k-1HT+R)-1

(5)

式中：Kk為k時(shí)刻的卡爾曼增益；R為量測噪聲的協(xié)方差矩陣。

(4)更新k時(shí)刻的系統(tǒng)狀態(tài)量，公式如下：

(6)

(5)更新k時(shí)刻的系統(tǒng)誤差協(xié)方差矩陣，公式如下：

Pk=(I-KkH)Pk|k-1

(7)

式中I為n階單位矩陣。

2 電動(dòng)滑臺(tái)位置實(shí)時(shí)補(bǔ)償

在實(shí)際作業(yè)中，要求電動(dòng)滑臺(tái)根據(jù)工件位置移動(dòng)一定的距離，為了使滑臺(tái)在盡可能短的時(shí)間內(nèi)穩(wěn)定而準(zhǔn)確地移動(dòng)到目標(biāo)位置，需要對其運(yùn)動(dòng)控制律進(jìn)行設(shè)計(jì)。由于傳感器測量誤差以及外界環(huán)境的干擾，難以得到滑臺(tái)每一時(shí)刻的真實(shí)位置，為此建立系統(tǒng)的狀態(tài)空間模型，使用卡爾曼濾波算法對預(yù)測值以及傳感器的測量值進(jìn)行處理，得到滑臺(tái)位置的最優(yōu)估計(jì)值。并以此為基準(zhǔn)求出到達(dá)下一時(shí)刻期望位置所需的位移量，實(shí)現(xiàn)對當(dāng)前時(shí)刻位置誤差的補(bǔ)償。使得滑臺(tái)的真實(shí)位置更加逼近期望位置，從而提高滑臺(tái)的軌跡跟蹤精度。

2.1 電動(dòng)滑臺(tái)的運(yùn)動(dòng)曲線

圖2 電動(dòng)滑臺(tái)的速度曲線

其數(shù)學(xué)模型為

(8)

通過速度v對時(shí)間t的積分可得電動(dòng)滑臺(tái)位移x的數(shù)學(xué)模型為

(9)

電動(dòng)滑臺(tái)的位移曲線如圖3所示。

圖3 電動(dòng)滑臺(tái)的位移曲線

當(dāng)距離給定時(shí)，為了得到滑臺(tái)相對應(yīng)的位移曲線，需要確定3個(gè)時(shí)間段的具體數(shù)值，設(shè)它們之間的關(guān)系為

t1=t3-t2=kT

(10)

式中：T為運(yùn)動(dòng)周期，即t3=T；k值由式(11)確定。

王平生提出，“鄉(xiāng)村振興戰(zhàn)略的核心是解決我國農(nóng)業(yè)農(nóng)村農(nóng)民目前的不發(fā)達(dá)、不興旺、不富裕、不協(xié)調(diào)、不環(huán)保的‘三農(nóng)’問題，達(dá)到生產(chǎn)、生活、生態(tài)的‘三生’融合、協(xié)調(diào)發(fā)展，真正實(shí)現(xiàn)農(nóng)業(yè)發(fā)展、農(nóng)村變樣、農(nóng)民受惠，最終建成小康社會(huì)的美麗鄉(xiāng)村。”鄉(xiāng)村振興戰(zhàn)略要堅(jiān)持農(nóng)業(yè)農(nóng)村優(yōu)先發(fā)展，加快推進(jìn)農(nóng)業(yè)農(nóng)村現(xiàn)代化。其重點(diǎn)是深化農(nóng)村土地制度改革，深化農(nóng)村集體產(chǎn)權(quán)制度改革，完善農(nóng)業(yè)支持保護(hù)制度，發(fā)展多種形式適度規(guī)模經(jīng)營，培育新型農(nóng)業(yè)經(jīng)營主體。

(11)

式中：m為負(fù)常數(shù)；k0為k的初始值。

設(shè)電動(dòng)滑臺(tái)運(yùn)動(dòng)的總位移量為X，則X與T之間的關(guān)系滿足式(12)。

(12)

綜上所述，當(dāng)總位移確定時(shí)，式(9)中的時(shí)間參數(shù)t1、t2和t3的值便可通過式(10)～式(12)確定，得到電動(dòng)滑臺(tái)具體的位移曲線。整個(gè)算法流程圖如圖4所示。

圖4 電動(dòng)滑臺(tái)運(yùn)動(dòng)規(guī)律流程圖

2.2 建立狀態(tài)方程

設(shè)電動(dòng)滑臺(tái)的位移為x，速度為v，令

(13)

式中X(t)為狀態(tài)變量。

設(shè)S為滾珠絲杠的導(dǎo)程，可知滾珠絲杠轉(zhuǎn)角與電動(dòng)滑臺(tái)位移的傳動(dòng)關(guān)系為

(14)

則系統(tǒng)的狀態(tài)方程為

(15)

量測方程為

Z(t)=HX(t)+V(t)

(16)

轉(zhuǎn)移矩陣F、控制矩陣G和量測矩陣H分別為

設(shè)定采樣周期為Δt，對連續(xù)系統(tǒng)的狀態(tài)方程和量測方程進(jìn)行離散化處理，得：

(17)

可以通過式(3)～式(7)迭代得到每一時(shí)刻電動(dòng)滑臺(tái)位移的最優(yōu)估計(jì)值。

2.3 位置補(bǔ)償系統(tǒng)設(shè)計(jì)

為了減少系統(tǒng)的累積誤差，計(jì)算當(dāng)前時(shí)刻位置的最優(yōu)估計(jì)值與下一時(shí)刻位置的期望值的差值，并將其作為下一時(shí)刻運(yùn)動(dòng)控制的位移增量，以補(bǔ)償每一時(shí)刻的位置誤差。設(shè)計(jì)的補(bǔ)償系統(tǒng)框圖如圖5所示。

圖5 基于卡爾曼濾波的位置補(bǔ)償控制系統(tǒng)

控制系統(tǒng)工作流程如下：

(1)卡爾曼濾波器將得到的位置編碼器數(shù)據(jù)轉(zhuǎn)換為電機(jī)角度值θ；

(3)計(jì)算k時(shí)刻位置的真實(shí)值與k+1時(shí)刻位置的期望值差值Δx，并將其作為相鄰兩時(shí)刻的位移增量，以此補(bǔ)償k時(shí)刻的位移誤差；

(4)根據(jù)電動(dòng)滑臺(tái)機(jī)械結(jié)構(gòu)之間的傳動(dòng)關(guān)系確定與位移增量Δx相對應(yīng)的電機(jī)轉(zhuǎn)角增量Δθ，再結(jié)合電機(jī)的分辨率得到控制器需要發(fā)送的脈沖數(shù)量。

(5)驅(qū)動(dòng)器按照接收到的脈沖數(shù)量和頻率驅(qū)動(dòng)電機(jī)轉(zhuǎn)動(dòng)。

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

本實(shí)驗(yàn)的主控制板采用S5P4418嵌入式開發(fā)板，電機(jī)驅(qū)動(dòng)采用HB808C驅(qū)動(dòng)器，電機(jī)采用帶編碼器的42步進(jìn)電機(jī)，電動(dòng)滑臺(tái)導(dǎo)程為1 mm。設(shè)定采樣周期為1 ms，最大速度Vmax=10 mm/s，位移量為130 mm。

3.1 基于卡爾曼濾波但無位置補(bǔ)償?shù)碾妱?dòng)滑臺(tái)運(yùn)動(dòng)控制實(shí)驗(yàn)

將運(yùn)動(dòng)軌跡的期望值、測量值以及卡爾曼濾波器處理后的最優(yōu)估計(jì)值繪制在一幅圖中，實(shí)驗(yàn)結(jié)果如圖6、圖7所示。

圖6 基于卡爾曼濾波但無位置補(bǔ)償時(shí)電動(dòng)滑臺(tái)的位移曲線

圖7 M區(qū)域放大圖

由圖7可以看出，卡爾曼濾波算法可以有效抑制噪聲干擾。根據(jù)數(shù)據(jù)結(jié)果顯示：經(jīng)21步迭代后位置數(shù)據(jù)的誤差數(shù)量級降到了10-4，經(jīng)過113步迭代后位置數(shù)據(jù)的誤差數(shù)量級降到了10-5，說明經(jīng)卡爾曼濾波算法處理后的數(shù)據(jù)能夠快速收斂并且十分接近于真實(shí)值。但從圖6中可以看出最優(yōu)估計(jì)值與期望值之間的偏差越來越大，數(shù)據(jù)顯示在最終時(shí)刻位置的誤差為3.37 mm，其主要原因在于系統(tǒng)沒有進(jìn)行位置補(bǔ)償從而造成位置誤差不斷累積，使得電動(dòng)滑臺(tái)的軌跡跟蹤精度不理想。

3.2 基于卡爾曼濾波且有位置補(bǔ)償?shù)碾妱?dòng)滑臺(tái)運(yùn)動(dòng)控制實(shí)驗(yàn)

在3.1小節(jié)的基礎(chǔ)上給控制系統(tǒng)增加位置補(bǔ)償功能之后，實(shí)驗(yàn)結(jié)果如圖8、圖9所示。

圖8 基于卡爾曼濾波且有位置補(bǔ)償時(shí)電動(dòng)滑臺(tái)的位移曲線

圖9 N區(qū)域放大圖

滑臺(tái)位置的測量值和最優(yōu)估計(jì)值都更加接近期望值，數(shù)據(jù)結(jié)果顯示期望值與最優(yōu)估計(jì)值之差一直穩(wěn)定在0.1 mm左右，系統(tǒng)的軌跡跟蹤精度得到顯著提高。

4 結(jié)束語

本文首先介紹了卡爾曼濾波的原理和算法流程；然后根據(jù)期望位移量設(shè)計(jì)電動(dòng)滑臺(tái)的運(yùn)動(dòng)控制律，此運(yùn)動(dòng)控制律能使電動(dòng)滑臺(tái)以零速度以及零加速度狀態(tài)到達(dá)目標(biāo)位置；最后使用卡爾曼濾波算法得到系統(tǒng)狀態(tài)的最優(yōu)估計(jì)值，并將其作為控制系統(tǒng)的反饋值以代替?zhèn)鞲衅鳒y量值用于電動(dòng)滑臺(tái)的位置補(bǔ)償。實(shí)驗(yàn)結(jié)果表明，卡爾曼濾波算法得到的電動(dòng)滑臺(tái)位置最優(yōu)估計(jì)值能夠快速逼近其真實(shí)值，有效地抑制了噪聲干擾；基于最優(yōu)估計(jì)值進(jìn)行位置補(bǔ)償后，電動(dòng)滑臺(tái)的位置控制精度得到明顯改善。