CN113630055A - 基于卡尔曼数据融合的无位置传感器摇臂伺服控制方法 - Google Patents

Abstract

本发明公开了一种基于卡尔曼数据融合的无位置传感器摇臂伺服控制方法。一方面，通过基于电流环凸优化的无位置传感器算法，得到摇臂的一个位置估计。另一方面，基于摇臂运动方程，在速度环利用卡尔曼滤波器辨识出摇臂的位置和速度，进而得到摇臂的另一个位置估计，利用卡尔曼滤波器实现数据融合，将两种估计结果相融合，通过动态调节两种估计的融合权重，得到了全摆动范围内的最佳位置估计。

Description

基于卡尔曼数据融合的无位置传感器摇臂伺服控制方法

技术领域

本发明属于伺服控制技术，具体为一种基于卡尔曼数据融合的无位置传感器摇臂伺服控制方法。

背景技术

摇臂机构是伺服电机的一种典型应用，现在广泛应用于各种机器人关节。很多关节类摇臂机构结构紧凑，没有位置安装位置传感器，因此，对于高精度摇臂机构，无位置传感器驱动控制技术尤为关键。摇臂由于受负载转矩影响，运动情况复杂，因此提高位置辨识精度具有重要意义。

近些年，基于凸优化的位置估计方法被用来估计电机的位置。这种方法估计电机位置无需在低速和高速情况下切换。相反，对于传统的位置估计，在低速情况下和高速情况下需要采用不同的位置估计方法。基于凸优化的位置估计法是一种新的无传感器控制策略。根据凸优化的理论，通过寻找损失函数的最小值，可以求得位置和速度。该方法可以被应用到低速和高速情况下。应该值得注意的是，对于凸优化法来说，在低速情况下观测位置，需要注入高频信号。然而该方法不需要数字解调和滤波。

然而，无位置传感器伺服驱动控制还存在关键问题，即低速过载情况下，位置估计精度较差，这是由于伺服电机在过载情况下d-q轴电感非常接近，即凸极率较低，而低速下电机运动反电势幅值很低，不足以支持位置估计，必须以注入高频信号的方式跟随转子凸极。因此，低凸极率现象限制了伺服电机低速过载下的无位置传感器控制性能。

发明内容

本发明的目的在于提供一种基于卡尔曼数据融合的无位置传感器摇臂伺服控制方法，以提高摇臂伺服机构位置辨识精度。

实现本发明目的的技术方案为：一种基于卡尔曼数据融合的无位置传感器摇臂伺服控制方法，具体步骤为：

步骤1，通过基于凸优化的电流环位置估计辨识摇臂位置；

步骤2，基于摇臂运动方程采用卡尔曼状态观测器，观测摇臂位置；

步骤3，动态调节噪声协方差矩阵。

优选地，通过基于凸优化的电流环位置估计辨识摇臂位置的具体方法为：

将α-β轴系下电流、α-β轴系下电压，以及上一时刻估计转速ω_re输入电流环位置估计模块，电流环位置估计模块根据损失函数通过牛顿迭代法，计算出当前转子位置，转子位置通过锁相环，对噪声造成的估计波动进行滤波，获得电流环的电机转子位置估计值，根据电机转子位置估计值计算摇臂的位置。

优选地，转子位置的获取方法为：

构建永磁同步电机静止坐标系下电压方程：

其中，v_α是α轴电压，v_β是β轴电压，R是绕组相电阻，p是微分算子，i_α和i_β是α-β轴电流，ω_re是转子速度，L_α(θ_re)、L_β(θ_re)、L_αβ(θ_re)是电感值在α-β坐标系下的中间变量，θ_re是转子位置，L_α＝L_∑+L_Δcos2θ_re，L_β＝L_∑-L_Δcos2θ_re，L_αβ＝L_Δsin2θ_re，其中

L_d为d轴电感，L_q为q轴电感；

令

基于电压方程建立损失函数：

其中，T_s是采样时间，i_α(k)和i_β(k)分别是第k次α轴和β轴电流，i_α(k-1)和i_β(k-1)分别是第k-1次α轴和β轴电流，ω_re(k-1)是第k-1次转子电气角速度，L_a(θ_re(k))、L_β(θ_re(k))是L_α(θ_re)、L_β(θ_re)的离散形式，T_pk(Δθ_re)是在α-β轴系下的旋转运算；

对损失函数加入惩罚项，构造：

其中，

是电机转子的位置估计；

根据凸优化理论，使用牛顿迭代法求损失函数的最小值点对应的估计量

作为转子位置估计量，迭代方法为：

优选地，在α-β轴系下的旋转运算具体为：

式中，Δθ_re为转子位置。

优选地，基于摇臂运动方程采用卡尔曼状态观测器，观测摇臂位置的具体方法为：

构建摇臂运动方程：

其中，J是折算到电机轴上的总转动惯量，θ_rm是电机轴机械角位置，有Pθ_rm＝θ_re，其中P是电机极对数，T_e是电磁转矩，B_m是摩擦系数，mgL_arm是重力矩幅值，m是摇臂的质量，L_arm是摇臂的长度；

令

将摇臂运动方程写成状态空间形式：

在具体状态下对sinx₁做局部线性化，并对其进行离散处理，得到摇臂运动方程的离散形式：

将

记为x_k,x_k代表k时刻状态量，将

记为x_k-1，x_k-1代表k-1时刻的状态量，记

A是状态转移矩阵，记

B是输入矩阵，T_e为k-1时刻输入信号，记为u_k-1；

将摇臂运动方程简写为x_k＝Ax_k-1+Bu_k-1

摇臂运动过程中，状态变量

将x₁＝θ_rm作为观测变量，得到观测方程y＝Cx，y是摇臂位置的测量值，其中，C是观测矩阵，C＝[1 0]；

将过程噪声w_k和测量噪声v_k分别添加到运动方程和观测方程中，由此得到：

x_k＝Ax_k-1+Bu_k-1+w_k-1

y_k＝Cx_k+v_k

w_k和v_k分别代表过程噪声和测量噪声,y_k是k时刻摇臂位置的测量值，x_k是k时刻的状态变量；

根据运动方程和观测方程，确定卡尔曼滤波观测器的预测方程和更新方程；

预测方程：

更新方程：

其中，Q和R分别是过程噪声方差和测量噪声方差，P_k-1和P_k分别为k-1时刻和k时刻的状态估计误差协方差，

为k时刻误差协方差预测值，K_k是卡尔曼增益，y_k为通过测量得到的摇臂位置，

为状态变量x_k预测估计值，

和

分别为k-1时刻和k时刻状态变量x_k的估计值；

给定状态变量和误差协方差的初值

将通过步骤1得到的摇臂位置作为摇臂位置的测量值y_k传递给卡尔曼滤波观测器，不断执行预测方程和更新方程，得到各个时刻，融合之后的状态变量的估计值。

本发明与现有技术相比，其显著优点为：

本发明在低速重载情况下利用速度环位置估计，利用卡尔曼滤波算法，通过动态调节两种估计的融合权重，得到了全摆动范围内的最佳位置估计，算法简单，且提高了估计精度。

附图说明

图1是本发明的流程图。

图2是本发明实施例的无位置传感器伺服摇臂控制原理框图。

具体实施方式

如图1、2所示，一种基于卡尔曼数据融合的无位置传感器摇臂伺服控制方法，具体步骤为：

步骤1，通过基于凸优化的电流环位置估计辨识摇臂位置：将α-β轴系下电流、α-β轴系下电压，以及上一时刻估计转速ω_re输入电流环位置估计模块，根据损失函数通过牛顿迭代法，计算出当前转子位置，转子位置通过锁相环，对噪声造成的估计波动进行滤波，获得电流环的电机转子位置估计值，再根据电机转子角度计算摇臂的位置。

构建永磁同步电机静止坐标系(α-β)下电压方程为：

其中v_α是α轴电压，v_β是β轴电压，R是绕组相电阻，p是微分算子，i_α和i_β是α-β轴电流。L_α(θ_re)、L_β(θ_re)、L_αβ(θ_re)是电感值在α-β坐标系下的中间变量，随转子位置θ_re发生变化，L_α＝L_∑+L_Δcos2θ_re，L_β＝L_∑-L_Δcos2θ_re，L_αβ＝L_Δsin2θ_re。

为了简化，记

基于电压方程建立损失函数为：

其中，T_s是采样时间，i_α(k)和i_β(k)分别是第k次α轴和β轴电流，i_α(k-1)和i_β(k-1)分别是第k-1次α轴和β轴电流，ω_re(k-1)是第k-1次转子电气角速度，L_a(θ_re(k))、L_β(θ_re(k))是L_α(θ_re)、L_β(θ_re)的离散形式。T_pk(Δθ_re)是在α-β轴系下的旋转运算，即

考虑低速下转子位置不能变化过快，对损失函数加入惩罚项，构造：

其中，

是电机转子的位置估计。

根据凸优化理论，当损失函数取得最小值时，对应的位置估计量

即为最接近真实位置的估计值，使用牛顿迭代法求损失函数的最小值点对应的估计量

迭代方法为：

将通过牛顿迭代法得到的当前转子位置

通过锁相环，对噪声造成的估计波动进行滤波，获得电流环的电机转子位置估计值，再根据电机转子位置计算摇臂的位置。

步骤2，基于位置环的摇臂位置观测：基于摇臂运动方程采用卡尔曼状态观测器，观测摇臂位置；

构建摇臂运动方程：

其中，J是折算到电机轴上的总转动惯量，θ_rm是电机轴机械角位置，有Pθ_rm＝θ_re，其中P是电机极对数，T_e是电磁转矩，B_m是摩擦系数，mgL_arm是重力矩幅值，m是摇臂的质量，L_arm是摇臂的长度；

令

将摇臂运动方程写成状态空间形式：

在具体状态下对sinx₁做局部线性化，并对其进行离散处理，得到摇臂运动方程的离散形式：

将

记为x_k,x_k代表k时刻状态量，将

记为x_k-1，x_k-1代表k-1时刻的状态量，记

A是状态转移矩阵，记

B是输入矩阵，T_e为k-1时刻输入信号，记为u_k-1。

摇臂运动方程可简写为x_k＝Ax_k-1+Bu_k-1

将x₁＝θ_rm作为观测变量，得到观测方程y_k＝Cx_k，y_k是k时刻摇臂位置的测量值。其中，C是观测矩阵，C＝[1 0]。

由于运动方程并不能完全描述摇臂的运动以及测量过程中存在的误差，将过程噪声w_k和测量噪声v_k分别添加到运动方程和观测方程中，由此得到：

x_k＝Ax_k-1+Bu_k-1+w_k-1

y_k＝Cx_k+v_kw_k和v_k分别代表过程噪声和测量噪声。

得到运动方程和观测方程之后，实施具体的卡尔曼滤波算法。首先给出预测方程和更新方程。

预测方程：

更新方程：

其中，Q和R分别是过程噪声方差和测量噪声方差，P_k-1和P_k分别为k-1时刻和k时刻的状态估计误差协方差，

为k时刻误差协方差预测值，K_k是卡尔曼增益，y_k为通过测量得到的摇臂位置，

为状态变量x_k预测估计值，

和

分别为k-1时刻和k时刻状态变量x_k的估计值；

然后，给定状态变量和误差协方差的初值

然后将通过步骤1得到的摇臂位置传递给卡尔曼滤波观测器，作为摇臂位置的测量值y_k。然后，不断执行预测方程和更新方程，即可得到各个时刻，融合之后的状态变量的估计值。因此，最终获得了融合之后的摇臂位置。

利用卡尔曼滤波观测器进行数据融合正是通过式

将测量值y_k和模型预测值

相互融合，最终获得状态变量估计值

即摇臂的位置。

步骤3，动态调节噪声协方差矩阵Q和R，在整个摆动范围内获得较高位置估计精度。

在摇臂处于低速重载情况下，增大测量协方差R的幅值，减小过程噪声协方差Q的幅值，让速度环估计作为主导；在摇臂其它位置采用，减小测量协方差R的幅值，增大过程噪声协方差Q的幅值，让电流环估计作为主导，从而在整个摆动范围内获得较高位置估计精度。

实施例

下面结合附图对本发明作更进一步的说明。以一台永磁同步电机拖动摇臂负载的伺服系统为例，如图2所示，整个控制系统采用位置环、速度环和电流环组成的三环系统，电流环计算出电压参考值

通过PWM调制后，以驱动器驱动永磁电机拖动摇臂。三环控制分别需要的转速和位置信息都是卡尔曼观测器的输出。特别需要指出的是，低速运行下，电流环位置估计需要辅以高频电压信号注入，以观测转子凸极位置。

步骤1，通过基于凸优化的电流环位置估计辨识摇臂位置：将α-β轴系下电流、α-β轴系下电压，以及上一时刻估计转速ω_re输入电流环位置估计模块，根据损失函数通过牛顿迭代法，计算出当前转子位置，转子位置通过锁相环，对噪声造成的估计波动进行滤波，获得电流环的电机转子位置估计值，再根据电机转子角度计算摇臂的位置。

步骤2，基于位置环的摇臂位置观测：基于摇臂运动方程采用卡尔曼状态观测器，观测摇臂位置；

步骤3，动态调节噪声协方差矩阵Q和R，在整个摆动范围内获得较高位置估计精度。

具体在本申请实施例中，摇臂停在θ_rm＝90°时，此时转速为0，重力矩为最大值mgL_arm，属于典型低速过载状态，由于此时d-q轴电感接近，电流环估计效果差，甚至发生不收敛的情况，因此采用较小Q矩阵和较大R矩阵，让速度环估计作为主导；在摇臂其它位置采用较大Q矩阵和较小R矩阵，让电流环估计作为主导，从而在整个摆动范围内获得较高位置估计精度。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示意性实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。

Claims (5)

1.一种基于卡尔曼数据融合的无位置传感器摇臂伺服控制方法，其特征在于，具体步骤为：

步骤1，通过基于凸优化的电流环位置估计辨识摇臂位置；

步骤2，基于摇臂运动方程采用卡尔曼状态观测器，观测摇臂位置；

步骤3，动态调节噪声协方差矩阵。

2.根据权利要求1所述的基于卡尔曼数据融合的无位置传感器摇臂伺服控制方法，其特征在于，通过基于凸优化的电流环位置估计辨识摇臂位置的具体方法为：

将α-β轴系下电流、α-β轴系下电压，以及上一时刻估计转速ω_re输入电流环位置估计模块，电流环位置估计模块根据损失函数通过牛顿迭代法，计算出当前转子位置，转子位置通过锁相环，对噪声造成的估计波动进行滤波，获得电流环的电机转子位置估计值，根据电机转子位置估计值计算摇臂的位置。

3.根据权利要求2所述的基于卡尔曼数据融合的无位置传感器摇臂伺服控制方法，其特征在于，转子位置的获取方法为：

构建永磁同步电机静止坐标系下电压方程：

其中，v_a是α轴电压，v_β是β轴电压，R是绕组相电阻，p是微分算子，i_α和i_β是α-β轴电流，ω_re是转子速度，L_α(θ_re)、L_β(θ_re)、L_αβ(θ_re)是电感值在α-β坐标系下的中间变量，θ_re是转子位置，i_α＝L_∑+L_Δcos2θ_re，L_β＝L_∑-L_Δcos2θ_re，L_αβ＝L_Δsin2θ_re，其中

L_d为d轴电感，L_q为q轴电感；

令

基于电压方程建立损失函数：

其中，T_s是采样时间，i_α(k)和i_β(k)分别是第k次α轴和β轴电流，i_α(k-1)和i_β(k-1)分别是第k-1次α轴和β轴电流，ω_re(k-1)是第k-1次转子电气角速度，L_a(θ_re(k))、L_β(θ_re(k))是L_α(θ_re)、L_β(θ_re)的离散形式，T_pk(Δθ_re)是在α-β轴系下的旋转运算；

对损失函数加入惩罚项，构造：

其中，

是电机转子的位置估计；

根据凸优化理论，使用牛顿迭代法求损失函数的最小值点对应的估计量

作为转子位置估计量，迭代方法为：

4.根据权利要求3所述的基于卡尔曼数据融合的无位置传感器摇臂伺服控制方法，其特征在于，在α-β轴系下的旋转运算具体为：

式中，Δθ_re为转子位置。

5.根据权利要求1所述的基于卡尔曼数据融合的无位置传感器摇臂伺服控制方法，其特征在于，基于摇臂运动方程采用卡尔曼状态观测器，观测摇臂位置的具体方法为：

构建摇臂运动方程：

其中，J是折算到电机轴上的总转动惯量，θ_rm是电机轴机械角位置，有Pθ_rm＝θ_re，其中P是电机极对数，T_e是电磁转矩，B_m是摩擦系数，mgL_arm是重力矩幅值，m是摇臂的质量，L_arm是摇臂的长度；

令

将摇臂运动方程写成状态空间形式：

在具体状态下对sinx₁做局部线性化，并对其进行离散处理，得到摇臂运动方程的离散形式：

将

记为x_k，x_k代表k时刻状态量，将

记为x_k-1，x_k-1代表k-1时刻的状态量，记

A是状态转移矩阵，记

B是输入矩阵，T_e为k-1时刻输入信号，记为u_k-1；

将摇臂运动方程简写为x_k＝Ax_k-1+Bu_k-1

摇臂运动过程中，状态变量

将x₁＝θ_rm作为观测变量，得到观测方程y＝Cx，y是摇臂位置的测量值，其中，C是观测矩阵，C＝[1 0]；

将过程噪声w_k和测量噪声v_k分别添加到运动方程和观测方程中，由此得到：

x_k＝Ax_k-1+Bu_k-1+w_k-1

y_k＝Cx_k+v_k

w_k和v_k分别代表过程噪声和测量噪声，y_k是k时刻摇臂位置的测量值，x_k是k时刻的状态变量；

根据运动方程和观测方程，确定卡尔曼滤波观测器的预测方程和更新方程；

预测方程：

更新方程：

其中，Q和R分别是过程噪声方差和测量噪声方差，P_k-1和P_k分别为k-1时刻和k时刻的状态估计误差协方差，

为k时刻误差协方差预测值，K_k是卡尔曼增益，y_k为通过测量得到的摇臂位置，

为状态变量x_k预测估计值，

和

分别为k-1时刻和k时刻状态变量x_k的估计值；

给定状态变量和误差协方差的初值

将通过步骤1得到的摇臂位置作为摇臂位置的测量值y_k传递给卡尔曼滤波观测器，不断执行预测方程和更新方程，得到各个时刻，融合之后的状态变量的估计值。

Images