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

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


背景技术：

2.摇臂机构是伺服电机的一种典型应用，现在广泛应用于各种机器人关节。很多关节类摇臂机构结构紧凑，没有位置安装位置传感器，因此，对于高精度摇臂机构，无位置传感器驱动控制技术尤为关键。摇臂由于受负载转矩影响，运动情况复杂，因此提高位置辨识精度具有重要意义。
3.近些年，基于凸优化的位置估计方法被用来估计电机的位置。这种方法估计电机位置无需在低速和高速情况下切换。相反，对于传统的位置估计，在低速情况下和高速情况下需要采用不同的位置估计方法。基于凸优化的位置估计法是一种新的无传感器控制策略。根据凸优化的理论，通过寻找损失函数的最小值，可以求得位置和速度。该方法可以被应用到低速和高速情况下。应该值得注意的是，对于凸优化法来说，在低速情况下观测位置，需要注入高频信号。然而该方法不需要数字解调和滤波。
4.然而，无位置传感器伺服驱动控制还存在关键问题，即低速过载情况下，位置估计精度较差，这是由于伺服电机在过载情况下d
‑
q轴电感非常接近，即凸极率较低，而低速下电机运动反电势幅值很低，不足以支持位置估计，必须以注入高频信号的方式跟随转子凸极。因此，低凸极率现象限制了伺服电机低速过载下的无位置传感器控制性能。


技术实现要素：

5.本发明的目的在于提供一种基于卡尔曼数据融合的无位置传感器摇臂伺服控制方法，以提高摇臂伺服机构位置辨识精度。
6.实现本发明目的的技术方案为：一种基于卡尔曼数据融合的无位置传感器摇臂伺服控制方法，具体步骤为：
7.步骤1，通过基于凸优化的电流环位置估计辨识摇臂位置；
8.步骤2，基于摇臂运动方程采用卡尔曼状态观测器，观测摇臂位置；
9.步骤3，动态调节噪声协方差矩阵。
10.优选地，通过基于凸优化的电流环位置估计辨识摇臂位置的具体方法为：
11.将α
‑
β轴系下电流、α
‑
β轴系下电压，以及上一时刻估计转速ω
re
输入电流环位置估计模块，电流环位置估计模块根据损失函数通过牛顿迭代法，计算出当前转子位置，转子位置通过锁相环，对噪声造成的估计波动进行滤波，获得电流环的电机转子位置估计值，根据电机转子位置估计值计算摇臂的位置。
12.优选地，转子位置的获取方法为：
13.构建永磁同步电机静止坐标系下电压方程：
[0014][0015]
其中，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轴电感；
[0016]
令基于电压方程建立损失函数：
[0017][0018]
其中，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
)是在α
‑
β轴系下的旋转运算；
[0019]
对损失函数加入惩罚项，构造：
[0020][0021]
其中，是电机转子的位置估计；
[0022]
根据凸优化理论，使用牛顿迭代法求损失函数的最小值点对应的估计量作为转子位置估计量，迭代方法为：
[0023][0024]
优选地，在α
‑
β轴系下的旋转运算具体为：
[0025][0026]
式中，δθ
re
为转子位置。
[0027]
优选地，基于摇臂运动方程采用卡尔曼状态观测器，观测摇臂位置的具体方法为：
[0028]
构建摇臂运动方程：
[0029][0030]
其中，j是折算到电机轴上的总转动惯量，θ
rm
是电机轴机械角位置，有pθ
rm
＝θ
re
，其中p是电机极对数，t
e
是电磁转矩，b
m
是摩擦系数，mgl
arm
是重力矩幅值，m是摇臂的质量，l
arm
是摇臂的长度；
[0031]
令将摇臂运动方程写成状态空间形式：
[0032][0033]
[0034]
在具体状态下对sinx1做局部线性化，并对其进行离散处理，得到摇臂运动方程的离散形式：
[0035][0036]
将记为x
k
,x
k
代表k时刻状态量，将记为x
k
‑1，x
k
‑1代表k
‑
1时刻的状态量，记a是状态转移矩阵，记b是输入矩阵，t
e
为k
‑
1时刻输入信号，记为u
k
‑1；
[0037]
将摇臂运动方程简写为x
k
＝ax
k
‑1 bu
k
‑1[0038]
摇臂运动过程中，状态变量将x1＝θ
rm
作为观测变量，得到观测方程y＝cx，y是摇臂位置的测量值，其中，c是观测矩阵，c＝[1 0]；
[0039]
将过程噪声w
k
和测量噪声v
k
分别添加到运动方程和观测方程中，由此得到：
[0040]
x
k
＝ax
k
‑1 bu
k
‑1 w
k
‑1[0041]
y
k
＝cx
k
v
k
[0042]
w
k
和v
k
分别代表过程噪声和测量噪声,y
k
是k时刻摇臂位置的测量值，x
k
是k时刻的状态变量；
[0043]
根据运动方程和观测方程，确定卡尔曼滤波观测器的预测方程和更新方程；
[0044]
预测方程：
[0045][0046][0047]
更新方程：
[0048][0049][0050][0051]
其中，q和r分别是过程噪声方差和测量噪声方差，p
k
‑1和p
k
分别为k
‑
1时刻和k时刻的状态估计误差协方差，为k时刻误差协方差预测值，k
k
是卡尔曼增益，y
k
为通过测量得到的摇臂位置，为状态变量x
k
预测估计值，和分别为k
‑
1时刻和k时刻状态变量x
k
的估计值；
[0052]
给定状态变量和误差协方差的初值将通过步骤1得到的摇臂位置作为摇臂位置的测量值y
k
传递给卡尔曼滤波观测器，不断执行预测方程和更新方程，得到各个时刻，融合之后的状态变量的估计值。
[0053]
本发明与现有技术相比，其显著优点为：
[0054]
本发明在低速重载情况下利用速度环位置估计，利用卡尔曼滤波算法，通过动态调节两种估计的融合权重，得到了全摆动范围内的最佳位置估计，算法简单，且提高了估计
精度。
附图说明
[0055]
图1是本发明的流程图。
[0056]
图2是本发明实施例的无位置传感器伺服摇臂控制原理框图。
具体实施方式
[0057]
如图1、2所示，一种基于卡尔曼数据融合的无位置传感器摇臂伺服控制方法，具体步骤为：
[0058]
步骤1，通过基于凸优化的电流环位置估计辨识摇臂位置：将α
‑
β轴系下电流、α
‑
β轴系下电压，以及上一时刻估计转速ω
re
输入电流环位置估计模块，根据损失函数通过牛顿迭代法，计算出当前转子位置，转子位置通过锁相环，对噪声造成的估计波动进行滤波，获得电流环的电机转子位置估计值，再根据电机转子角度计算摇臂的位置。
[0059]
构建永磁同步电机静止坐标系(α
‑
β)下电压方程为：
[0060][0061]
其中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
。
[0062]
为了简化，记基于电压方程建立损失函数为：
[0063][0064]
其中，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
)是在α
‑
β轴系下的旋转运算，即
[0065][0066]
考虑低速下转子位置不能变化过快，对损失函数加入惩罚项，构造：
[0067][0068]
其中，是电机转子的位置估计。
[0069]
根据凸优化理论，当损失函数取得最小值时，对应的位置估计量即为最接近真实位置的估计值，使用牛顿迭代法求损失函数的最小值点对应的估计量迭代方法为：
[0070][0071]
将通过牛顿迭代法得到的当前转子位置通过锁相环，对噪声造成的估计波动进行滤波，获得电流环的电机转子位置估计值，再根据电机转子位置计算摇臂的位置。
[0072]
步骤2，基于位置环的摇臂位置观测：基于摇臂运动方程采用卡尔曼状态观测器，观测摇臂位置；
[0073]
构建摇臂运动方程：
[0074][0075]
其中，j是折算到电机轴上的总转动惯量，θ
rm
是电机轴机械角位置，有pθ
rm
＝θ
re
，其中p是电机极对数，t
e
是电磁转矩，b
m
是摩擦系数，mgl
arm
是重力矩幅值，m是摇臂的质量，l
arm
是摇臂的长度；
[0076]
令将摇臂运动方程写成状态空间形式：
[0077][0078][0079]
在具体状态下对sinx1做局部线性化，并对其进行离散处理，得到摇臂运动方程的离散形式：
[0080][0081]
将记为x
k
,x
k
代表k时刻状态量，将记为x
k
‑1，x
k
‑1代表k
‑
1时刻的状态量，记a是状态转移矩阵，记b是输入矩阵，t
e
为k
‑
1时刻输入信号，记为u
k
‑1。
[0082]
摇臂运动方程可简写为x
k
＝ax
k
‑1 bu
k
‑1[0083]
将x1＝θ
rm
作为观测变量，得到观测方程y
k
＝cx
k
，y
k
是k时刻摇臂位置的测量值。其中，c是观测矩阵，c＝[1 0]。
[0084]
由于运动方程并不能完全描述摇臂的运动以及测量过程中存在的误差，将过程噪声w
k
和测量噪声v
k
分别添加到运动方程和观测方程中，由此得到：
[0085]
x
k
＝ax
k
‑1 bu
k
‑1 w
k
‑1[0086]
y
k
＝cx
k
v
k
w
k
和v
k
分别代表过程噪声和测量噪声。
[0087]
得到运动方程和观测方程之后，实施具体的卡尔曼滤波算法。首先给出预测方程和更新方程。
[0088]
预测方程：
[0089][0090]
[0091]
更新方程：
[0092][0093][0094][0095]
其中，q和r分别是过程噪声方差和测量噪声方差，p
k
‑1和p
k
分别为k
‑
1时刻和k时刻的状态估计误差协方差，为k时刻误差协方差预测值，k
k
是卡尔曼增益，y
k
为通过测量得到的摇臂位置，为状态变量x
k
预测估计值，和分别为k
‑
1时刻和k时刻状态变量x
k
的估计值；
[0096]
然后，给定状态变量和误差协方差的初值然后将通过步骤1得到的摇臂位置传递给卡尔曼滤波观测器，作为摇臂位置的测量值y
k
。然后，不断执行预测方程和更新方程，即可得到各个时刻，融合之后的状态变量的估计值。因此，最终获得了融合之后的摇臂位置。
[0097]
利用卡尔曼滤波观测器进行数据融合正是通过式将测量值y
k
和模型预测值相互融合，最终获得状态变量估计值即摇臂的位置。
[0098]
步骤3，动态调节噪声协方差矩阵q和r，在整个摆动范围内获得较高位置估计精度。
[0099]
在摇臂处于低速重载情况下，增大测量协方差r的幅值，减小过程噪声协方差q的幅值，让速度环估计作为主导；在摇臂其它位置采用，减小测量协方差r的幅值，增大过程噪声协方差q的幅值，让电流环估计作为主导，从而在整个摆动范围内获得较高位置估计精度。
[0100]
实施例
[0101]
下面结合附图对本发明作更进一步的说明。以一台永磁同步电机拖动摇臂负载的伺服系统为例，如图2所示，整个控制系统采用位置环、速度环和电流环组成的三环系统，电流环计算出电压参考值通过pwm调制后，以驱动器驱动永磁电机拖动摇臂。三环控制分别需要的转速和位置信息都是卡尔曼观测器的输出。特别需要指出的是，低速运行下，电流环位置估计需要辅以高频电压信号注入，以观测转子凸极位置。
[0102]
步骤1，通过基于凸优化的电流环位置估计辨识摇臂位置：将α
‑
β轴系下电流、α
‑
β轴系下电压，以及上一时刻估计转速ω
re
输入电流环位置估计模块，根据损失函数通过牛顿迭代法，计算出当前转子位置，转子位置通过锁相环，对噪声造成的估计波动进行滤波，获得电流环的电机转子位置估计值，再根据电机转子角度计算摇臂的位置。
[0103]
步骤2，基于位置环的摇臂位置观测：基于摇臂运动方程采用卡尔曼状态观测器，观测摇臂位置；
[0104]
步骤3，动态调节噪声协方差矩阵q和r，在整个摆动范围内获得较高位置估计精度。
[0105]
具体在本技术实施例中，摇臂停在θ
rm
＝90
°
时，此时转速为0，重力矩为最大值mgl
arm
，属于典型低速过载状态，由于此时d
‑
q轴电感接近，电流环估计效果差，甚至发生不收敛的情况，因此采用较小q矩阵和较大r矩阵，让速度环估计作为主导；在摇臂其它位置采
用较大q矩阵和较小r矩阵，让电流环估计作为主导，从而在整个摆动范围内获得较高位置估计精度。
[0106]
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示意性实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
[0107]
尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。