[发明专利]一种基于卡尔曼滤波器的摇臂伺服控制方法

说明书

本发明公开了一种基于抗扰卡尔曼滤波的摇臂伺服控制方法。基于摇臂的运动方程，利用卡尔曼滤波器辨识出摇臂的位置和速度，卡尔曼滤波器得到的速度信号比锁相环计算得出的速度信号拥有更高的响应速度，采用卡尔曼观测器的观测值作为速度环速度反馈，根据速度环速度反馈与参考转速计算未来N个采样周期参考转速轨迹，通过模型预测算法预测未来N个采样周期的转速预测值，并建立基于规划转速与预测转速差值的损失函数，通过搜索损失函数极值计算电流参考值。本发明利用卡尔曼滤波器代替锁相环，通过位置信号这个表征量，得到了更高精度的摇臂伺服控制方法。

技术领域

本发明属于伺服控制技术，具体为一种基于卡尔曼滤波器的摇臂伺服控制方法。

背景技术

摇臂机构是伺服电机的一种典型应用，现在广泛应用于各种机器人关节。通常其到位精度由位置环、速度环、电流环决定，其中速度环的反馈一般由数字锁相环计算得出，由于经过数字锁相环计算得出的速度信号噪声大，运用低通滤波器将速度信号滤波，但是却降低了速度环的响应速度，影响了摇臂到位精度。因此，对于高精度摇臂机构，速度环的反馈尤为关键。摇臂由于受负载转矩影响，运动情况复杂，降低速度环的误差具有重要意义。

发明内容

本发明的目的在于提供一种基于卡尔曼滤波器的摇臂伺服控制方法。

实现本发明目的的技术方案为：一种基于卡尔曼滤波器摇臂伺服控制方法，，包括如下步骤：

步骤1，基于卡尔曼状态观测器的位置和速度观测：基于摇臂运动方程采用卡尔曼状态观测器，观测摇臂位置和速度，卡尔曼状态观测器包括预测方程和更新方程两部分，具体为：

预测方程：

更新方程：

其中，y_k为传感器测量值，为模型预测值，K_k是卡尔曼增益矩阵，A是运动方程的状态矩阵，B是n×1的输入矩阵，P是协方差矩阵，C是观测矩阵，Q和R分别是过程噪声协方差矩阵与测量噪声协方差矩阵，n为自然数，u_k-1为修正量，I为单位矩阵，为观测值；

步骤2，速度环模型预测控制器：采用卡尔曼观测器的观测值作为速度环速度反馈，根据速度环速度反馈与参考转速计算未来N个采样周期参考转速轨迹，通过模型预测算法预测未来N个采样周期的转速预测值，并建立基于规划转速与预测转速差值的损失函数，通过搜索损失函数极值计算电流参考值i_q^*。

优选地，步骤1中，修正量具体为：

式中，J是系统机械惯量，，K_t是转矩系数，i_q是q轴电流。

优选地，步骤2中，未来N个采样周期参考转速轨迹的计算方法为：

其中，ω_ref(i)为未来N个采样周期参考转速轨迹，T_s是转速环采样时间，T_r是速度环动态响应时间,是当前速度环速度反馈，为前的参考转速。

优选地，步骤2中，未来N个采样周期的转速预测值计算方法为：

其中，ω_p(i)为未来N个采样周期的转速预测值，K_t是电流转矩系数，T_L是负载转矩，J是系统机械惯量，ω_p(0)是当前时刻的转速预测，T_s是采样周期，i^*_MPC是电流参考值。