基于卡爾曼濾波的低速伺服系統(tǒng)速度信號(hào)估計(jì)

符玉襄， 孫德新， 劉銀年

(1. 中國(guó)科學(xué)院 上海技術(shù)物理研究所，上海 200083；2. 中國(guó)科學(xué)院 紅外探測(cè)與成像技術(shù)重點(diǎn)實(shí)驗(yàn)室，上海 200083)

基于卡爾曼濾波的低速伺服系統(tǒng)速度信號(hào)估計(jì)

符玉襄1，2， 孫德新1，2， 劉銀年1，2

(1. 中國(guó)科學(xué)院 上海技術(shù)物理研究所，上海 200083；2. 中國(guó)科學(xué)院 紅外探測(cè)與成像技術(shù)重點(diǎn)實(shí)驗(yàn)室，上海 200083)

給出了基于卡爾曼濾波的測(cè)速方法，并利用電流、角速度等參數(shù)估算出加速度，作為卡爾曼濾波的控制輸入。仿真了該方法的穩(wěn)態(tài)和動(dòng)態(tài)性能，并與其他濾波方法做了對(duì)比；將該方法應(yīng)用于某伺服系統(tǒng)，測(cè)試并分析了系統(tǒng)的0.1°/s階躍響應(yīng)、頻域特性及參數(shù)攝動(dòng)對(duì)濾波性能的影響。結(jié)果表明，該方法減小了低速時(shí)的測(cè)速誤差和相位延時(shí)，擴(kuò)展了速度環(huán)帶寬，并且對(duì)參數(shù)變化不敏感，具有較強(qiáng)的魯棒性。

卡爾曼濾波； 低速； 速度信號(hào)估計(jì)； 位置差分； 絕對(duì)式編碼器

0 引 言

高精度的伺服系統(tǒng)中，經(jīng)常采用位置、速度和電流三環(huán)控制的策略。當(dāng)使用絕對(duì)式光電編碼器作為位置傳感器時(shí)，速度一般通過(guò)相鄰兩個(gè)位置采樣點(diǎn)差分的方式得到。當(dāng)電機(jī)工作在低速(例如0.1°/s)時(shí)，由于編碼器的分辨率有限，以及測(cè)量噪聲的存在，使得差分測(cè)速的誤差迅速增大[1-5]。

通過(guò)延長(zhǎng)采樣周期或增加低通濾波環(huán)節(jié)可以減小穩(wěn)態(tài)噪聲，但會(huì)增加測(cè)速延時(shí)，降低閉環(huán)帶寬，導(dǎo)致動(dòng)態(tài)性能變差[3-4]。基于狀態(tài)觀測(cè)器[2-4,6-8]和卡爾曼濾波[1,4-5,9]的測(cè)速方法，可以得到較好的穩(wěn)態(tài)和動(dòng)態(tài)性能。目前這些方法有一些共同特點(diǎn)： (1) 大多基于增量式編碼器，采用M/T法測(cè)速[1,4,6-11]，較少涉及絕對(duì)式編碼器(位置差分測(cè)速)；(2) 速度較高(大于1°/s)[1-4,6-11]，對(duì)低于1°/s的情況研究較少；(3) 常用的伺服系統(tǒng)的狀態(tài)空間模型較多依賴于被控對(duì)象的類(lèi)型和參數(shù)[1-2,6-7,9]，缺乏一般性,且較少分析參數(shù)變化對(duì)濾波器性能的影響。

因此，針對(duì)使用絕對(duì)式編碼器和低速運(yùn)行的伺服系統(tǒng)，有必要提出一種噪聲小、延時(shí)短、性能穩(wěn)健且較為通用的測(cè)速方法。基于運(yùn)動(dòng)學(xué)的卡爾曼濾波算法可以較好地解決這些問(wèn)題。本文介紹了差分測(cè)速和低通濾波的原理；建立了基于勻加速運(yùn)動(dòng)方程的伺服系統(tǒng)狀態(tài)模型，給出了基于此模型的卡爾曼濾波測(cè)速的遞推算法；并利用電機(jī)電流、角速度等參數(shù)估算出加速度，作為濾波的控制量；仿真了該方法的穩(wěn)態(tài)誤差和動(dòng)態(tài)性能，并與傳統(tǒng)濾波方法做了對(duì)比；搭建了基于DSP和永磁同步電機(jī)的試驗(yàn)平臺(tái)，測(cè)試并分析了系統(tǒng)的0.1°/s階躍響應(yīng)、穩(wěn)態(tài)精度、頻域特性、相位延時(shí)及參數(shù)攝動(dòng)對(duì)濾波性能的影響。

1 差分測(cè)速和低通濾波

后向直接差分測(cè)速原理:

(1)

θk、θk-1——當(dāng)前時(shí)刻與上一時(shí)刻的位置采樣值。

T——采樣周期；

當(dāng)位置采樣噪聲標(biāo)準(zhǔn)差為Δ，且相鄰兩個(gè)采樣點(diǎn)不相關(guān)時(shí)，速度估計(jì)的相對(duì)誤差為

(2)

式中：ω——角速度。

采樣頻率越高，速度越慢，速度估計(jì)的相對(duì)誤差就越大。

對(duì)于本系統(tǒng)而言，編碼器噪聲Δ≈0.1″，速度環(huán)控制周期T=1ms，當(dāng)轉(zhuǎn)速ω=0.1°/s時(shí)，速度估計(jì)的相對(duì)誤差e=39.3%，遠(yuǎn)不能滿足高精度控制的需要。因此有必要尋找一種合適的濾波算法。

常見(jiàn)的低通濾波器如一階RC低通濾波、巴特沃茲低通濾波等，都能夠在一定程度上減少噪聲，但不可避免地帶來(lái)了測(cè)量延時(shí)。

此外，移動(dòng)差分通過(guò)延長(zhǎng)差分時(shí)間間隔，也可以減小噪聲。其原理如下：

(3)

式中： θk、θk-N——當(dāng)前時(shí)刻與前N個(gè)時(shí)刻的位置采樣值。

N越大，抑制噪聲能力越強(qiáng)，但延時(shí)也越長(zhǎng)。

2 卡爾曼測(cè)速原理

2.1 基于運(yùn)動(dòng)學(xué)的卡爾曼濾波

伺服系統(tǒng)的勻加速運(yùn)動(dòng)方程為

(4)

基于式(4)的卡爾曼濾波的過(guò)程及測(cè)量矩陣如下：

(5)

θk、ωk——位置、速度；

ak——加速度，是控制量；

zk——角度測(cè)量值；

wk、vk——過(guò)程噪聲、測(cè)量噪聲。

卡爾曼濾波需要5步迭代運(yùn)算[1,4-5,9]：

(6)

2.2 加速度的獲取

式(5)所描述的狀態(tài)空間模型與電機(jī)類(lèi)型、負(fù)載慣量J、粘滯摩擦因數(shù)B、轉(zhuǎn)矩系數(shù)Ke和最大靜摩擦力矩Fm等無(wú)直接聯(lián)系，適用于不同類(lèi)型的伺服系統(tǒng)。

對(duì)于沒(méi)有加速度傳感器的伺服系統(tǒng)，無(wú)法直接獲取卡爾曼濾波所需的加速度ak(控制量)，此時(shí)可以直接把加速度當(dāng)作零來(lái)處理，但這樣會(huì)導(dǎo)致濾波性能變差。

本文利用電機(jī)驅(qū)動(dòng)電流Ik、角速度ωk和伺服系統(tǒng)參數(shù)估算出加速度ak：

(7)

其中，ε是一個(gè)接近零的正數(shù)，仿真和試驗(yàn)時(shí)取0.005°/s。當(dāng)速度過(guò)零時(shí)，摩擦力仍然存在，但其大小和方向難以確定，此時(shí)的估計(jì)誤差較大。試驗(yàn)時(shí)發(fā)現(xiàn)，濾波性能不會(huì)因?yàn)閍k估計(jì)不準(zhǔn)而明顯下降，即不需要精確知道伺服系統(tǒng)的相關(guān)參數(shù)。

3 仿真分析

3.1 模型搭建

采用直流電機(jī)模型，輸入電壓到輸出角度的傳遞函數(shù)：

(8)

式中：θ(s)——輸出位置；

U(s)——輸入電壓；

L、R——繞組電感、電阻；

J、B——轉(zhuǎn)動(dòng)慣量、粘滯摩擦因數(shù)。

由于速度較低，可以忽略反電動(dòng)勢(shì)的影響。永磁同步電機(jī)通過(guò)坐標(biāo)變換和id≡0的矢量控制，也可以等效為直流電機(jī)[12]。

圖1是Simulink仿真框圖。電流環(huán)采樣頻率10kHz，PWM逆變器(pwm)和電流采樣模塊(i_sample)等效為延遲一個(gè)采樣周期的慣性環(huán)節(jié)；速度估計(jì)模塊(Velocity_est)和速度環(huán)控制器(Velocity_PID)用離散型S-Function實(shí)現(xiàn)，采樣頻率1kHz；位置測(cè)量值在進(jìn)入速度估計(jì)模塊之前加了標(biāo)準(zhǔn)差為0.1"的高斯白噪聲，用以模擬真實(shí)編碼器的輸出。

圖1 MATLAB/Simulink搭建的電機(jī)控制模型

3.2 階躍響應(yīng)仿真

圖2是0.1°/s階躍響應(yīng)仿真波形，圖2(a)～圖2(d)分別對(duì)應(yīng)直接差分、5點(diǎn)移動(dòng)差分、二階巴特沃茲濾波和卡爾曼濾波。巴特沃茲濾波數(shù)字帶寬取0.2，對(duì)應(yīng)模擬帶寬100Hz；卡爾曼濾波過(guò)程噪聲Q=1.5e-9，測(cè)量噪聲R=1.022e-7。為了便于評(píng)估各種方法的性能差異，仿真是在相同的速度環(huán)和電流環(huán)PID參數(shù)下進(jìn)行的。

圖2 0.1°/s階躍響應(yīng)仿真波形

表1為相關(guān)指標(biāo)，依次為80%上升時(shí)間tr，超調(diào)量σ，穩(wěn)態(tài)速度誤差均方根值eRMS和穩(wěn)態(tài)速度波動(dòng)峰值|emax|/ωref，其中ωref=0.1°/s。穩(wěn)態(tài)從250ms算起。

表1 0.1°/s階躍響應(yīng)仿真數(shù)據(jù)

仿真結(jié)果表明，直接差分受噪聲影響很大，卡爾曼濾波相對(duì)移動(dòng)差分和巴特沃茲濾波，在tr和σ差別不大的情況下，穩(wěn)態(tài)誤差有較好的改善。階躍響應(yīng)的超調(diào)主要由測(cè)速延時(shí)和積分飽和引起。

4 試驗(yàn)驗(yàn)證

用TMS320F2812實(shí)現(xiàn)卡爾曼濾波算法；永磁同步電機(jī)型號(hào)為Kollmorgen公司的RBE02110-B；負(fù)載為轉(zhuǎn)鏡，慣量約0.015kg·m2；絕對(duì)式光電編碼器采用Heidenhain公司RCN228系列，有效分辨率24bit(0.078")。PWM波周期100us，電流環(huán)采樣頻率10kHz，速度環(huán)采樣頻率1kHz。

通過(guò)調(diào)節(jié)過(guò)程噪聲wk和測(cè)量噪聲vk的方差可以改變卡爾曼濾波的增益。加速度作為濾波的控制量，用式(7)計(jì)算得到的ak進(jìn)行濾波時(shí)效果不是最佳，這是因?yàn)橄到y(tǒng)的轉(zhuǎn)矩系數(shù)、最大靜摩擦力、粘滯摩擦因數(shù)和轉(zhuǎn)動(dòng)慣量等參數(shù)難以精確獲得。因此，在試驗(yàn)中要反復(fù)整定Ke、Fm、B、J以及Q、R，使濾波的效果達(dá)到最優(yōu)或次優(yōu)。

4.1 階躍響應(yīng)測(cè)試

參考輸入0.1°/s的速度階躍，在PID參數(shù)固定的情況下，分別用4種濾波方法作為速度反饋，測(cè)得波形如圖3所示，相關(guān)指標(biāo)如表2所示。穩(wěn)態(tài)指標(biāo)由500～900ms的400個(gè)點(diǎn)統(tǒng)計(jì)得到。

結(jié)果表明： 卡爾曼濾波有效抑制了振蕩，超調(diào)量減小到4.3%，上升時(shí)間縮短到6.2ms；穩(wěn)態(tài)誤差峰峰值和均方根值都顯著減小；同時(shí)，卡爾曼濾波穩(wěn)定時(shí)間短，第300ms后就達(dá)到穩(wěn)態(tài)，而移動(dòng)差分和巴特沃茲濾波第400ms后才逐漸趨于穩(wěn)態(tài)。

圖3 0.1°/s階躍響應(yīng)實(shí)測(cè)波形

指標(biāo)直接差分移動(dòng)差分巴特沃茲濾波卡爾曼濾波tr/ms1514206.2σ/(%)5020224.3eRMS/(°)·s-117.2e-33.19e-32.83e-30.451e-3(|emax|/ωref)/%50.29.98.21.3

4.2 頻域測(cè)試

通過(guò)測(cè)量多個(gè)頻率點(diǎn)，得到四種濾波方法的頻率特性曲線如圖4所示。圖4(a)是幅頻曲線，圖4(b)是相頻曲線。可以看出，卡爾曼濾波在通帶內(nèi)增益比較平坦，3dB截止頻率在100Hz以上，其他濾波方法通帶內(nèi)增益起伏，截止頻率約30Hz。同時(shí)，卡爾曼濾波在100Hz處相位延遲只有70°，而其他濾波方法在20Hz處就有約100°的延遲。

圖4 四種速度估計(jì)方法的閉環(huán)小信號(hào)頻率特性曲線

從頻域分析幾種濾波方法的帶寬和相位延時(shí)。參考速度信號(hào)：

ωref(t)=1·sin(2πft)+1

圖5是f=10Hz時(shí)卡爾曼濾波的測(cè)試結(jié)果，虛線是給定速度，實(shí)線是濾波得到的速度。速度過(guò)零時(shí)存在畸變，這是由于過(guò)零時(shí)摩擦力方向突變，依據(jù)式(7)得到的加速度誤差較大造成的。

圖5 卡爾曼濾波的正弦波響應(yīng)

4.3 變參數(shù)測(cè)試

通過(guò)改變Ke、Fm、B及Q、R的取值，可以驗(yàn)證算法的穩(wěn)健性以及對(duì)控制對(duì)象參數(shù)的依賴程度。測(cè)試結(jié)果如表3所示。表3中第2列是標(biāo)準(zhǔn)參數(shù)的0.1°/s階躍響應(yīng)，之后各列是某個(gè)參數(shù)偏離標(biāo)準(zhǔn)值時(shí)的0.1°/s階躍響應(yīng)。

當(dāng)轉(zhuǎn)矩系數(shù)Ke是原來(lái)的2倍時(shí)，超調(diào)量最大(17.5%)，說(shuō)明加速度估算的準(zhǔn)確度對(duì)動(dòng)態(tài)性能影響很大，但此時(shí)的σ仍低于移動(dòng)差分濾波(20%)和巴特沃茲低通濾波(22%)。

當(dāng)過(guò)程噪聲Q是原來(lái)的10倍時(shí)，穩(wěn)態(tài)速度波動(dòng)最大(4.4%)，這是因?yàn)镼增大時(shí)，卡爾曼濾波收斂后的帶寬變大，抑制噪聲能力就變差[4]。但此時(shí)的速度波動(dòng)仍小于移動(dòng)差分濾波(9.9%)和巴特沃茲低通濾波(8.2%)。

據(jù)此可以認(rèn)為基于運(yùn)動(dòng)學(xué)的卡爾曼測(cè)速算法對(duì)系統(tǒng)參數(shù)變化不敏感，具有較強(qiáng)的魯棒性。

表3 參數(shù)變化對(duì)卡爾曼濾波效果的影響

5 結(jié) 語(yǔ)

針對(duì)使用絕對(duì)式編碼器的伺服系統(tǒng)極低速(0.1°/s)時(shí)速度測(cè)量噪聲大的問(wèn)題，設(shè)計(jì)了一種基于運(yùn)動(dòng)學(xué)方程的卡爾曼濾波測(cè)速算法，根據(jù)仿真分析和實(shí)際測(cè)試，得出以下結(jié)論：

(1) 基于運(yùn)動(dòng)學(xué)方程的伺服系統(tǒng)狀態(tài)空間模型，不需要精確知道被控對(duì)象的參數(shù)，通用性較強(qiáng)；

(2) 相對(duì)于傳統(tǒng)方法，該方法減小了測(cè)速的穩(wěn)態(tài)誤差和延時(shí)，改善了系統(tǒng)動(dòng)態(tài)性能；

(3) 算法性能穩(wěn)健，對(duì)參數(shù)變化不敏感。

[1] LABBATE M, PETRELLA R, TURSINI M. Fixed point implementation of Kalman filtering for AC drives: a case study using TMS320F24x DSP[C]∥Proc of the 3rd European DSP Education and Research Conference, 2000: 20-21.

[2] SONG Y, GAO H, ZHANG S, et al. The analysis and design of low velocity estimation based on observer[C]∥Automation and Logistics, ICAL′09, IEEE International Conference on, IEEE, 2009: 766-771.

[3] 吳忠,呂緒明.基于磁編碼器的伺服電機(jī)速度及位置觀測(cè)器設(shè)計(jì)[J].中國(guó)電機(jī)工程學(xué)報(bào),2011,31(9): 82-87.

[4] PETRELLA R, TURSINI M, PERETTI L, et al. Speed measurement algorithms for low-resolution incremental encoder equipped drives: a comparative analysis[C]∥Electrical Machines and Power Elec-tronics, ACEMP′07， International Aegean Con-ference on, IEEE, 2007: 780-787.

[5] 李文軍,陳濤.基于卡爾曼濾波器的等效復(fù)合控制技術(shù)研究[J].光學(xué)精密工程,2006,14(2): 279-284.

[6] LI H, XIE H, YI X, et al. Research on low speed control of permanent magnet synchronous motor based on state observer[C]∥Biomedical Engineering and Informatics (BMEI), 2011 4th International Conference on, IEEE, 2011(4): 2018-2022.

[7] WANG G, XU D, YU Y, et al. Low speed control of permanent magnet synchronous motor based on instantaneous speed estimation[C]∥Intelligent Control and Automation, WCICA 2006, The Sixth World Congress on, IEEE, 2006(2): 8033-8036.

[8] WANG M S, KUNG Y S, THI H, et al. Superior low-speed control of a permanent magnet synchronous motor with digital encoder[C]∥Proceedings of the Institution of Mechanical Engineers, Part I: Journal of Systems and Control Engineering, 2011,225(2): 281-291.

[9] 丁信忠,張承瑞,李虎修,等.基于自適應(yīng)擴(kuò)展卡爾曼濾波器的永磁同步電機(jī)超低速控制[J].電機(jī)與控制應(yīng)用,2012,39(9): 24-29.

[10] 魯進(jìn)軍,梅志千,劉向紅,等.電動(dòng)機(jī)的高精度寬范圍轉(zhuǎn)速測(cè)量方法[J].中國(guó)電機(jī)工程學(xué)報(bào),2011,31(24): 118-123.

[11] 楊松濤,和麗清,安成斌.DSP在高精度數(shù)字式電機(jī)測(cè)速中的應(yīng)用[J].紅外與激光工程,2006,10(35): 543-548.

[12] 王宏佳,楊明,牛里,等.永磁交流伺服系統(tǒng)速度控制器優(yōu)化設(shè)計(jì)方法[J].電機(jī)與控制學(xué)報(bào),2012,16(2): 25-31.

[期刊訂閱]

在郵局漏訂的讀者，可直接從郵局匯款至我雜志社發(fā)行部補(bǔ)訂

地址：上海市武寧路509號(hào)電科大廈17樓《電機(jī)與控制應(yīng)用》發(fā)行部

郵編： 200063 電話： 021-62574990-745 傳真： 021-62576377

國(guó)內(nèi)郵發(fā)代號(hào)： 4-199 每?jī)?cè)定價(jià)： 12.00元 全年定價(jià)： 144.00元

中文核心期刊 中國(guó)科技核心期刊 中國(guó)學(xué)術(shù)期刊(光盤(pán)版)

全國(guó)優(yōu)秀科技期刊 華東優(yōu)秀科技期刊

中國(guó)科學(xué)引文數(shù)據(jù)庫(kù)來(lái)源期刊 中國(guó)學(xué)術(shù)期刊綜合評(píng)價(jià)數(shù)據(jù)庫(kù)來(lái)源期刊

Low Velocity Estimation of Servo System Based on Kalman Filter

FUYuxiang1,2，SUNDexin1,2，LIUYinnian1,2

(1. Shanghai Institute of Technical Physics, CAS, Shanghai 200083， China；2. Key Laboratory of Infrared System Detection and Imaging Technique, CAS, Shanghai 200083， China)

A velocity estimation method using kalman filter was given. Acceleration was considered as control input of kalman filter, and was estimated by motor current and velocity. The steady and dynamic performance had been simulated, the results of this method was compared with other filtering methods.The method was applied to a servo system. Some key performance indicators had been measured and analyzed, such as 0.1 °/ s step response, frequency domain characteristics and the influence of parameter perturbation. Results showed that kalman filter based on the kinematics could reduce velocity estimation error and phase lag, expanded speed loop bandwidth, and was not sensitive to parameters perturbation, which has strong robustness.

kalman filter; low speed; velocity estimation; position difference; absolute encoder

劉銀年

TM 921.54+1

A

1673-6540(2015)05-0017-06

2014-10-28