基于卡尔曼滤波的新型变“M/T”编码器测速方法

徐张旗，陶家园，王克逸，简慧杰

(中国科学技术大学 精密机械与精密仪器系，安徽 合肥 230026)

增量式光电编码器因其体积小、抗干扰性强、分辨率高、输出稳和成本低等特点广泛应用于伺服控制系统。其工作原理是将角度位移信息转换成周期性脉冲信号，通过测量增量式编码器输出脉冲信号的频率或周期即可得到电动机的转速。而测速的精度正是伺服控制系统精度得以保证的关键。

1 常见的测速方法

目前常见的测速方法[1-3]包括如下3种。

1)定时测角法(“M”法)。该方法通过测量给定时间间隔内的脉冲数来计算转速，以高频时钟脉冲时间为基准，采样受编码器输出脉冲信号量化误差较大。而低速阶段中输出脉冲周期较大，对高频时钟固定时间间隔要求更大，不利于转速的实时测量，因此“M”法在实际应用中不适用于低速阶段测量。定时测角法原理与误差来源图如图1所示。

图1 定时测角法原理与误差来源图

2)定角测时法(“T”法)。该方法通过测量2个相邻脉冲之间的时间间隔来计算转速，以编码器输出脉冲周期为基准，采样受高频时钟脉冲信号量化误差影响较大。而中高速阶段编码器输出脉冲周期较小，此时要想达到较高的精度，必须采用更高的采样频率，因此“T”法在实际应用中不适用于中高速阶段测量。定角测时法原理与误差来源图如图2所示。

图2 定角测时法原理与误差来源图

3)“M/T”法。该方法是将“M”法与“T”法结合，同时测量一定数量的脉冲和产生这些脉冲所用的时间，可有效抑制采样时间间隔量化误差的影响，得到在整个速度范围内精度较高的测量结果；但在低速运行时，编码器输出的脉冲间隔较大，即采样周期较大，使得检测时间过长，从而导致后级的伺服控制系统无法及时得到最新的编码器状态信息。

卡尔曼滤波是一种利用线性系统状态方程，采用信号与噪声的状态空间模型，利用前一时刻的估计值和现时刻的观测值来更新对状态变量的估计，求出现时刻的估计值，对系统状态进行最优估计的方法。本文在“M”法和“T”法的基础上，结合卡尔曼滤波，提出一种新型的变“M/T”高精度实时动态测速方法。该方法能够根据速度范围，在“M”法和“T”法中择优选用，能够在整个速度范围内有效地抑制采样量化误差，输出相对实时、准确的速度值。

2 新型变“M/T”动态测速方法原理

(1)

式中，N为编码器输出分辨率。故可以将测速过程描述为一个离散时间的随机过程。对测量过程做如下建模，假设线性离散系统的方程如下。

系统方程：

Xk=Ak·Xk-1+Bk·Uk+Wk

(2)

观测方程：

Yk=Hk·Xk+Vk

(3)

图3 新型变“M/T”算法原理图

卡尔曼滤波算法具体计算流程[8]如下。

(4)

式中，Xk-1是上一状态最优的结果或给定的初值。

(5)

式中，Pk-1是上一状态最优估计值Xk-1的误差协方差阵。

3)结合预测值和测量值，得到k状态的最优估计值Xk为：

(6)

4)更新Xk对应的估计误差协方差阵Pk为：

(7)

当系统进入k+1状态，Pk就是式5中Pk-1。可见，只要给定初始值X1和P1，算法可以自回归运算下去，结合时刻测量值Yk，就可递推得到k时刻最优转速估计值Xk，从而实现整个速度段的精确测量。

3 试验与结果分析

3.1 硬件设计

试验采用CKD-AX4009T高速转台，编码器输出分辨率N为33 792脉冲/圈，试验时转台的速度通过信号发生器与CKD驱动器来精确控制。驱动器与测试转台如图4所示。

图4 CKD驱动器与测试转台

转台所带的增量式编码器的输出为2路正交的差分方波信号，这里采用德州仪器的DS26C32差分信号接收器，将差分信号转换为普通数字电路可以接受的单端信号。新型变“M/T”高精度实时动态测速算法在STM32F303RET6数字信号控制器中实现。该型号数字信号控制器包含了主频高达72 MHz的ARM Cortex-M4内核和浮点协处理器(FPU)，满足新型变“M/T”测速方法对硬件的要求。测速系统的硬件实物图如图5所示，测速系统的硬件框图如图6所示。

图5 测速系统硬件实物图

图6 测速系统硬件框图

单片机的定时器4配置成正交编码器输入模式，同时开启输入管脚的中断触发功能，触发方式为上升沿与下降沿均触发。单片机的定时器3每隔一段时间产生定时中断，在输入管脚的外部中断例程里计算定时器3产生中断的时间间隔。根据判断的速度段，选择相应的计数中断(即“T”法)或定时中断(即“M”法)。CPU在中断中读取定时器4的计数值与定时器3对应中断的时间间隔来测量角速度；然后由卡尔曼滤波算法计算出角速度最优估计值；最后通过USB2.0 FullSpeed接口，将新型的变“M/T”法的计算结果上传至计算机。

3.2 试验结果及分析

选择CKD转台在脉冲串输入模式下，分别做低速段、高速段和匀变速段等3组试验。

编码器给定理论输出转速ω=6.725°/s，在低速段对比“T”法与新型变“M/T”法，结果如图7所示。由图7可知，“T”法曲线受高频时钟脉冲信号量化误差影响跳动较大，新型变“M/T”法稳定阶段测量误差在0.015°/s以内，相对误差在0.223%以内，起到了很好的滤波作用，有效地抑制了量化噪声。

图7 低速段“T”法与卡尔曼算法对比图

编码器给定理论输出转速ω=399.103 3°/s，在高速段对比“M”法与新型变“M/T”法，结果如图8所示。由图8可知，“M”法曲线受编码器输出脉冲信号量化误差影响跳动较大，新型变“M/T”法稳定阶段测量误差在0.3°/s以内，相对误差在0.751%以内，同样起到了很好的滤波和抑制量化噪声的作用。

图8 高速段“M”法与卡尔曼算法对比图

信号发生器选择扫频脉冲输入，即给定编码器角加速度，1 s内由静止匀加速至ω=266.335°/s，循环往复，在匀加速段对比“M/T”法与新型变“M/T”法，结果如图9和图10所示。由图9和图10可知，新型变“M/T”法既能在整个速度段精确、实时地对速度进行测量，也能很好地抑制噪声，减少量化误差[9-10]。

图9 匀加速段“M/T”法与卡尔曼算法对比图

图10 “M/T”法与卡尔曼算法对比区间放大图

4 结语

针对增量式编码器测速问题，提出了一种基于卡尔曼滤波的新型变“M/T”高精度动态测速算法，根据速度预判，在“M”法、“T”法中进行择优选用，既保证了测量的实时性，也提高了测量精度，扩大了测速范围，并在数字信号控制器上实现了这一算法。试验结果表明，采用该测速方法实现了在整个速度段的实时、准确地对转速进行测量，能够同时抑制和消除采样时间间隔量化误差的影响，对提高整个伺服控制系统的性能具有一定的应用价值。