卡尔曼滤波在电动滑台位置实时补偿中的应用

吴鹏飞，单 奇，罗新河，饶 鑫，蔡正凯

(西南交通大学机械工程学院，四川成都 610031)

0 引言

电动滑台作为主要的直线运动执行机构之一，在精密仪器、加工制造等领域应用广泛。其轨迹跟踪精度决定了作业任务的完成质量，而轨迹跟踪精度主要取决于控制器的计算精度、系统的响应时间、传感器的测量精度、零件的制造装配精度以及减速机背隙等[1-3]。为了提高电动滑台的轨迹跟踪精度，有学者做了相关研究。林剑等[4]通过测量滑台基座的直线度，选择较好的一侧安装主导轨，以自为基准的原则安装副导轨，获得了μm级的直线度，但是未对控制系统做相关研究。李伟等[5]建立机床滑台位置模糊PID控制模型，改善了系统的响应速度和稳定性，但对噪声干扰等未加处理。赵帅等[6]通过改进卡尔曼滤波算法对电机速度进行处理，有效地抑制了测量噪声，但对电机速度的控制没有进行改善。

本文针对电动滑台控制系统的轨迹跟踪误差以及测量装置存在的噪声，导致位置跟踪误差累积的问题，提出采用卡尔曼滤波算法对电动滑台的位置进行最优估计，并在此基础上实现实时补偿，使得电动滑台按照期望的轨迹运动。

1 卡尔曼滤波

卡尔曼滤波是Rudolf Emil Kalman提出的一种线性最小方差估计算法[7]，其主要思路是：以最小均方误差为最佳估计原则，认为系统和观测都存在噪声干扰，且属于高斯白噪声，建立系统的状态空间模型，利用高斯混合模型进行迭代求得每个采样时刻的最优估计值。此迭代算法的核心思路是：利用前一时刻的最优估计值与当前时刻的观测值来对当前时刻的状态变量做出最优估计[8]。采用迭代方法的优点是不需要大量的存储空间，非常适合计算机处理[9]。

线性离散系统由系统状态方程和系统测量方程表示：

系统状态方程：

Xk=AXk-1+BUk+Wk

(1)

系统量测方程：

Zk=HXk+Vk

(2)

式中：Xk为当前时刻系统的n维状态向量；A为系统的n×n阶状态转移矩阵；Xk-1为上一时刻系统的n维状态向量；B为系统控制向量的n×r阶增益矩阵;Uk为当前时刻系统的r维控制向量；Wk为当前时刻系统的n维过程噪声向量，服从高斯分布，协方差矩阵为Q;Zk为当前时刻系统的m维量测向量；H为m×n阶量测矩阵；Vk为当前时刻系统的m维量测噪声，服从高斯分布，协方差矩阵为R。

卡尔曼滤波的算法流程[10]如图1所示。

图1 卡尔曼滤波算法流程图

(1)预测k时刻的状态量，公式如下：

(3)

(2)预测k时刻的误差协方差矩阵，公式如下：

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

(4)

式中：Pk|k-1为k时刻预测值的误差协方差矩阵；Pk-1为k-1时刻系统状态向量的误差协方差矩阵；Q为过程噪声的协方差矩阵。

(3)计算k时刻的卡尔曼增益，公式如下：

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

(5)

式中：Kk为k时刻的卡尔曼增益；R为量测噪声的协方差矩阵。

(4)更新k时刻的系统状态量，公式如下：

(6)

(5)更新k时刻的系统误差协方差矩阵，公式如下：

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

(7)

式中I为n阶单位矩阵。

2 电动滑台位置实时补偿

在实际作业中，要求电动滑台根据工件位置移动一定的距离，为了使滑台在尽可能短的时间内稳定而准确地移动到目标位置，需要对其运动控制律进行设计。由于传感器测量误差以及外界环境的干扰，难以得到滑台每一时刻的真实位置，为此建立系统的状态空间模型，使用卡尔曼滤波算法对预测值以及传感器的测量值进行处理，得到滑台位置的最优估计值。并以此为基准求出到达下一时刻期望位置所需的位移量，实现对当前时刻位置误差的补偿。使得滑台的真实位置更加逼近期望位置，从而提高滑台的轨迹跟踪精度。

2.1 电动滑台的运动曲线

图2 电动滑台的速度曲线

其数学模型为

(8)

通过速度v对时间t的积分可得电动滑台位移x的数学模型为

(9)

电动滑台的位移曲线如图3所示。

图3 电动滑台的位移曲线

当距离给定时，为了得到滑台相对应的位移曲线，需要确定3个时间段的具体数值，设它们之间的关系为

t1=t3-t2=kT

(10)

式中：T为运动周期，即t3=T；k值由式(11)确定。

王平生提出，“乡村振兴战略的核心是解决我国农业农村农民目前的不发达、不兴旺、不富裕、不协调、不环保的‘三农’问题，达到生产、生活、生态的‘三生’融合、协调发展，真正实现农业发展、农村变样、农民受惠，最终建成小康社会的美丽乡村。”乡村振兴战略要坚持农业农村优先发展，加快推进农业农村现代化。其重点是深化农村土地制度改革，深化农村集体产权制度改革，完善农业支持保护制度，发展多种形式适度规模经营，培育新型农业经营主体。

(11)

式中：m为负常数；k0为k的初始值。

设电动滑台运动的总位移量为X，则X与T之间的关系满足式(12)。

(12)

综上所述，当总位移确定时，式(9)中的时间参数t1、t2和t3的值便可通过式(10)～式(12)确定，得到电动滑台具体的位移曲线。整个算法流程图如图4所示。

图4 电动滑台运动规律流程图

2.2 建立状态方程

设电动滑台的位移为x，速度为v，令

(13)

式中X(t)为状态变量。

设S为滚珠丝杠的导程，可知滚珠丝杠转角与电动滑台位移的传动关系为

(14)

则系统的状态方程为

(15)

量测方程为

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

(16)

转移矩阵F、控制矩阵G和量测矩阵H分别为

设定采样周期为Δt，对连续系统的状态方程和量测方程进行离散化处理，得：

(17)

可以通过式(3)～式(7)迭代得到每一时刻电动滑台位移的最优估计值。

2.3 位置补偿系统设计

为了减少系统的累积误差，计算当前时刻位置的最优估计值与下一时刻位置的期望值的差值，并将其作为下一时刻运动控制的位移增量，以补偿每一时刻的位置误差。设计的补偿系统框图如图5所示。

图5 基于卡尔曼滤波的位置补偿控制系统

控制系统工作流程如下：

(1)卡尔曼滤波器将得到的位置编码器数据转换为电机角度值θ；

(3)计算k时刻位置的真实值与k+1时刻位置的期望值差值Δx，并将其作为相邻两时刻的位移增量，以此补偿k时刻的位移误差；

(4)根据电动滑台机械结构之间的传动关系确定与位移增量Δx相对应的电机转角增量Δθ，再结合电机的分辨率得到控制器需要发送的脉冲数量。

(5)驱动器按照接收到的脉冲数量和频率驱动电机转动。

3 实验结果与分析

本实验的主控制板采用S5P4418嵌入式开发板，电机驱动采用HB808C驱动器，电机采用带编码器的42步进电机，电动滑台导程为1 mm。设定采样周期为1 ms，最大速度Vmax=10 mm/s，位移量为130 mm。

3.1 基于卡尔曼滤波但无位置补偿的电动滑台运动控制实验

将运动轨迹的期望值、测量值以及卡尔曼滤波器处理后的最优估计值绘制在一幅图中，实验结果如图6、图7所示。

图6 基于卡尔曼滤波但无位置补偿时电动滑台的位移曲线

图7 M区域放大图

由图7可以看出，卡尔曼滤波算法可以有效抑制噪声干扰。根据数据结果显示：经21步迭代后位置数据的误差数量级降到了10-4，经过113步迭代后位置数据的误差数量级降到了10-5，说明经卡尔曼滤波算法处理后的数据能够快速收敛并且十分接近于真实值。但从图6中可以看出最优估计值与期望值之间的偏差越来越大，数据显示在最终时刻位置的误差为3.37 mm，其主要原因在于系统没有进行位置补偿从而造成位置误差不断累积，使得电动滑台的轨迹跟踪精度不理想。

3.2 基于卡尔曼滤波且有位置补偿的电动滑台运动控制实验

在3.1小节的基础上给控制系统增加位置补偿功能之后，实验结果如图8、图9所示。

图8 基于卡尔曼滤波且有位置补偿时电动滑台的位移曲线

图9 N区域放大图

滑台位置的测量值和最优估计值都更加接近期望值，数据结果显示期望值与最优估计值之差一直稳定在0.1 mm左右，系统的轨迹跟踪精度得到显著提高。

4 结束语

本文首先介绍了卡尔曼滤波的原理和算法流程；然后根据期望位移量设计电动滑台的运动控制律，此运动控制律能使电动滑台以零速度以及零加速度状态到达目标位置；最后使用卡尔曼滤波算法得到系统状态的最优估计值，并将其作为控制系统的反馈值以代替传感器测量值用于电动滑台的位置补偿。实验结果表明，卡尔曼滤波算法得到的电动滑台位置最优估计值能够快速逼近其真实值，有效地抑制了噪声干扰；基于最优估计值进行位置补偿后，电动滑台的位置控制精度得到明显改善。