基于卡尔曼滤波的低速伺服系统速度信号估计

符玉襄， 孙德新， 刘银年

(1. 中国科学院 上海技术物理研究所，上海 200083；2. 中国科学院 红外探测与成像技术重点实验室，上海 200083)

基于卡尔曼滤波的低速伺服系统速度信号估计

符玉襄1，2， 孙德新1，2， 刘银年1，2

(1. 中国科学院 上海技术物理研究所，上海 200083；2. 中国科学院 红外探测与成像技术重点实验室，上海 200083)

给出了基于卡尔曼滤波的测速方法，并利用电流、角速度等参数估算出加速度，作为卡尔曼滤波的控制输入。仿真了该方法的稳态和动态性能，并与其他滤波方法做了对比；将该方法应用于某伺服系统，测试并分析了系统的0.1°/s阶跃响应、频域特性及参数摄动对滤波性能的影响。结果表明，该方法减小了低速时的测速误差和相位延时，扩展了速度环带宽，并且对参数变化不敏感，具有较强的鲁棒性。

卡尔曼滤波； 低速； 速度信号估计； 位置差分； 绝对式编码器

0 引 言

高精度的伺服系统中，经常采用位置、速度和电流三环控制的策略。当使用绝对式光电编码器作为位置传感器时，速度一般通过相邻两个位置采样点差分的方式得到。当电机工作在低速(例如0.1°/s)时，由于编码器的分辨率有限，以及测量噪声的存在，使得差分测速的误差迅速增大[1-5]。

通过延长采样周期或增加低通滤波环节可以减小稳态噪声，但会增加测速延时，降低闭环带宽，导致动态性能变差[3-4]。基于状态观测器[2-4,6-8]和卡尔曼滤波[1,4-5,9]的测速方法，可以得到较好的稳态和动态性能。目前这些方法有一些共同特点： (1) 大多基于增量式编码器，采用M/T法测速[1,4,6-11]，较少涉及绝对式编码器(位置差分测速)；(2) 速度较高(大于1°/s)[1-4,6-11]，对低于1°/s的情况研究较少；(3) 常用的伺服系统的状态空间模型较多依赖于被控对象的类型和参数[1-2,6-7,9]，缺乏一般性,且较少分析参数变化对滤波器性能的影响。

因此，针对使用绝对式编码器和低速运行的伺服系统，有必要提出一种噪声小、延时短、性能稳健且较为通用的测速方法。基于运动学的卡尔曼滤波算法可以较好地解决这些问题。本文介绍了差分测速和低通滤波的原理；建立了基于匀加速运动方程的伺服系统状态模型，给出了基于此模型的卡尔曼滤波测速的递推算法；并利用电机电流、角速度等参数估算出加速度，作为滤波的控制量；仿真了该方法的稳态误差和动态性能，并与传统滤波方法做了对比；搭建了基于DSP和永磁同步电机的试验平台，测试并分析了系统的0.1°/s阶跃响应、稳态精度、频域特性、相位延时及参数摄动对滤波性能的影响。

1 差分测速和低通滤波

后向直接差分测速原理:

(1)

θk、θk-1——当前时刻与上一时刻的位置采样值。

T——采样周期；

当位置采样噪声标准差为Δ，且相邻两个采样点不相关时，速度估计的相对误差为

(2)

式中：ω——角速度。

采样频率越高，速度越慢，速度估计的相对误差就越大。

对于本系统而言，编码器噪声Δ≈0.1″，速度环控制周期T=1ms，当转速ω=0.1°/s时，速度估计的相对误差e=39.3%，远不能满足高精度控制的需要。因此有必要寻找一种合适的滤波算法。

常见的低通滤波器如一阶RC低通滤波、巴特沃兹低通滤波等，都能够在一定程度上减少噪声，但不可避免地带来了测量延时。

此外，移动差分通过延长差分时间间隔，也可以减小噪声。其原理如下：

(3)

式中： θk、θk-N——当前时刻与前N个时刻的位置采样值。

N越大，抑制噪声能力越强，但延时也越长。

2 卡尔曼测速原理

2.1 基于运动学的卡尔曼滤波

伺服系统的匀加速运动方程为

(4)

基于式(4)的卡尔曼滤波的过程及测量矩阵如下：

(5)

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

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

zk——角度测量值；

wk、vk——过程噪声、测量噪声。

卡尔曼滤波需要5步迭代运算[1,4-5,9]：

(6)

2.2 加速度的获取

式(5)所描述的状态空间模型与电机类型、负载惯量J、粘滞摩擦因数B、转矩系数Ke和最大静摩擦力矩Fm等无直接联系，适用于不同类型的伺服系统。

对于没有加速度传感器的伺服系统，无法直接获取卡尔曼滤波所需的加速度ak(控制量)，此时可以直接把加速度当作零来处理，但这样会导致滤波性能变差。

本文利用电机驱动电流Ik、角速度ωk和伺服系统参数估算出加速度ak：

(7)

其中，ε是一个接近零的正数，仿真和试验时取0.005°/s。当速度过零时，摩擦力仍然存在，但其大小和方向难以确定，此时的估计误差较大。试验时发现，滤波性能不会因为ak估计不准而明显下降，即不需要精确知道伺服系统的相关参数。

3 仿真分析

3.1 模型搭建

采用直流电机模型，输入电压到输出角度的传递函数：

(8)

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

U(s)——输入电压；

L、R——绕组电感、电阻；

J、B——转动惯量、粘滞摩擦因数。

由于速度较低，可以忽略反电动势的影响。永磁同步电机通过坐标变换和id≡0的矢量控制，也可以等效为直流电机[12]。

图1是Simulink仿真框图。电流环采样频率10kHz，PWM逆变器(pwm)和电流采样模块(i_sample)等效为延迟一个采样周期的惯性环节；速度估计模块(Velocity_est)和速度环控制器(Velocity_PID)用离散型S-Function实现，采样频率1kHz；位置测量值在进入速度估计模块之前加了标准差为0.1"的高斯白噪声，用以模拟真实编码器的输出。

图1 MATLAB/Simulink搭建的电机控制模型

3.2 阶跃响应仿真

图2是0.1°/s阶跃响应仿真波形，图2(a)～图2(d)分别对应直接差分、5点移动差分、二阶巴特沃兹滤波和卡尔曼滤波。巴特沃兹滤波数字带宽取0.2，对应模拟带宽100Hz；卡尔曼滤波过程噪声Q=1.5e-9，测量噪声R=1.022e-7。为了便于评估各种方法的性能差异，仿真是在相同的速度环和电流环PID参数下进行的。

图2 0.1°/s阶跃响应仿真波形

表1为相关指标，依次为80%上升时间tr，超调量σ，稳态速度误差均方根值eRMS和稳态速度波动峰值|emax|/ωref，其中ωref=0.1°/s。稳态从250ms算起。

表1 0.1°/s阶跃响应仿真数据

仿真结果表明，直接差分受噪声影响很大，卡尔曼滤波相对移动差分和巴特沃兹滤波，在tr和σ差别不大的情况下，稳态误差有较好的改善。阶跃响应的超调主要由测速延时和积分饱和引起。

4 试验验证

用TMS320F2812实现卡尔曼滤波算法；永磁同步电机型号为Kollmorgen公司的RBE02110-B；负载为转镜，惯量约0.015kg·m2；绝对式光电编码器采用Heidenhain公司RCN228系列，有效分辨率24bit(0.078")。PWM波周期100us，电流环采样频率10kHz，速度环采样频率1kHz。

通过调节过程噪声wk和测量噪声vk的方差可以改变卡尔曼滤波的增益。加速度作为滤波的控制量，用式(7)计算得到的ak进行滤波时效果不是最佳，这是因为系统的转矩系数、最大静摩擦力、粘滞摩擦因数和转动惯量等参数难以精确获得。因此，在试验中要反复整定Ke、Fm、B、J以及Q、R，使滤波的效果达到最优或次优。

4.1 阶跃响应测试

参考输入0.1°/s的速度阶跃，在PID参数固定的情况下，分别用4种滤波方法作为速度反馈，测得波形如图3所示，相关指标如表2所示。稳态指标由500～900ms的400个点统计得到。

结果表明： 卡尔曼滤波有效抑制了振荡，超调量减小到4.3%，上升时间缩短到6.2ms；稳态误差峰峰值和均方根值都显著减小；同时，卡尔曼滤波稳定时间短，第300ms后就达到稳态，而移动差分和巴特沃兹滤波第400ms后才逐渐趋于稳态。

图3 0.1°/s阶跃响应实测波形

指标直接差分移动差分巴特沃兹滤波卡尔曼滤波tr/ms1514206.2σ/(%)5020224.3eRMS/(°)·s-117.2e-33.19e-32.83e-30.451e-3(|emax|/ωref)/%50.29.98.21.3

4.2 频域测试

通过测量多个频率点，得到四种滤波方法的频率特性曲线如图4所示。图4(a)是幅频曲线，图4(b)是相频曲线。可以看出，卡尔曼滤波在通带内增益比较平坦，3dB截止频率在100Hz以上，其他滤波方法通带内增益起伏，截止频率约30Hz。同时，卡尔曼滤波在100Hz处相位延迟只有70°，而其他滤波方法在20Hz处就有约100°的延迟。

图4 四种速度估计方法的闭环小信号频率特性曲线

从频域分析几种滤波方法的带宽和相位延时。参考速度信号：

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

图5是f=10Hz时卡尔曼滤波的测试结果，虚线是给定速度，实线是滤波得到的速度。速度过零时存在畸变，这是由于过零时摩擦力方向突变，依据式(7)得到的加速度误差较大造成的。

图5 卡尔曼滤波的正弦波响应

4.3 变参数测试

通过改变Ke、Fm、B及Q、R的取值，可以验证算法的稳健性以及对控制对象参数的依赖程度。测试结果如表3所示。表3中第2列是标准参数的0.1°/s阶跃响应，之后各列是某个参数偏离标准值时的0.1°/s阶跃响应。

当转矩系数Ke是原来的2倍时，超调量最大(17.5%)，说明加速度估算的准确度对动态性能影响很大，但此时的σ仍低于移动差分滤波(20%)和巴特沃兹低通滤波(22%)。

当过程噪声Q是原来的10倍时，稳态速度波动最大(4.4%)，这是因为Q增大时，卡尔曼滤波收敛后的带宽变大，抑制噪声能力就变差[4]。但此时的速度波动仍小于移动差分滤波(9.9%)和巴特沃兹低通滤波(8.2%)。

据此可以认为基于运动学的卡尔曼测速算法对系统参数变化不敏感，具有较强的鲁棒性。

表3 参数变化对卡尔曼滤波效果的影响

5 结 语

针对使用绝对式编码器的伺服系统极低速(0.1°/s)时速度测量噪声大的问题，设计了一种基于运动学方程的卡尔曼滤波测速算法，根据仿真分析和实际测试，得出以下结论：

(1) 基于运动学方程的伺服系统状态空间模型，不需要精确知道被控对象的参数，通用性较强；

(2) 相对于传统方法，该方法减小了测速的稳态误差和延时，改善了系统动态性能；

(3) 算法性能稳健，对参数变化不敏感。

[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].中国电机工程学报,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] 李文军,陈涛.基于卡尔曼滤波器的等效复合控制技术研究[J].光学精密工程,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] 丁信忠,张承瑞,李虎修,等.基于自适应扩展卡尔曼滤波器的永磁同步电机超低速控制[J].电机与控制应用,2012,39(9): 24-29.

[10] 鲁进军,梅志千,刘向红,等.电动机的高精度宽范围转速测量方法[J].中国电机工程学报,2011,31(24): 118-123.

[11] 杨松涛,和丽清,安成斌.DSP在高精度数字式电机测速中的应用[J].红外与激光工程,2006,10(35): 543-548.

[12] 王宏佳,杨明,牛里,等.永磁交流伺服系统速度控制器优化设计方法[J].电机与控制学报,2012,16(2): 25-31.

[期刊订阅]

在邮局漏订的读者，可直接从邮局汇款至我杂志社发行部补订

地址：上海市武宁路509号电科大厦17楼《电机与控制应用》发行部

邮编： 200063 电话： 021-62574990-745 传真： 021-62576377

国内邮发代号： 4-199 每册定价： 12.00元 全年定价： 144.00元

中文核心期刊 中国科技核心期刊 中国学术期刊(光盘版)

全国优秀科技期刊 华东优秀科技期刊

中国科学引文数据库来源期刊 中国学术期刊综合评价数据库来源期刊

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