CN112847365B - 一种力矩估计方法 - Google Patents

Abstract

本发明公开了一种力矩估计方法，包括a和b两个步骤：a.建立谐波传动模型，包括谐波传动运动学模型、运动误差模型和谐波传动柔顺模型；b.构建冗余自适应鲁棒卡尔曼滤波器；加入自适应转换模块，同时将电机电流的函数作为冗余因子。该力矩估计方法，基于谐波传动模型和冗余自适应鲁棒卡尔曼滤波器，通过自调整滤波增益和自适应切换滤波模式，使转矩估计滤波对负载变化具有最优性和鲁棒性，同时将电机电流的函数作为RARKF的冗余因子，以容忍建模误差和随负载变化的滤波模式切换，并兼顾了力矩估计算法中滤波的最优性和鲁棒性，解决了传统算法只具有最优性或者只具有鲁棒性而不能同时具有最优性和鲁棒性的矛盾。

Description

一种力矩估计方法

技术领域

本发明涉及协作机器人控制技术领域，具体为一种力矩估计方法。

背景技术

关节力矩反馈可以改善机器人的控制性能，通常用于机器人操作手的运动控制，以抑制负载力矩的影响，并且可以用于机器人的动态控制，而不需要计算机器人的逆动力学，关节扭矩感知或估计对于控制机器人的力和柔顺性以及碰撞检测也是必不可少的。

在传统的机器人的力控制中使用关节力矩传感器或多轴力/力矩(F/T)传感器。当使用机器人手腕上的F/T传感器估计关节力矩时，需要进行大量的计算，并且结果受到计算延迟和模型误差的影响，同时在现有技术中存在以下缺陷：

1.测量精度与传感器刚度成反比，为了获得较高的测量精度，需要较低的传感器刚度，而导致关节动力学复杂；

2.使用低通滤波器来过滤谐波传动模型的输出,虽然提供了一种简单方便的方法来抵抗高频噪声，但转矩估计的响应速度受到滤波器带宽的限制；

3.标准卡尔曼滤波算法中的滤波增益是最优的，不能自调整，外部环境只能通过预先设定的测量噪声方差来反映，很难适应实际外界干扰的变化并及时响应；

4.状态模型和量测模型中的非线性建模误差可能会导致AREKF判断机构的切换功能失效。

为此，本发明研发了一种力矩估计方法。

发明内容

(一)解决的技术问题

针对现有技术中关节动力学复杂、转矩估计的响应速度受限的不足、不能自调整以及导致AREKF判断机构的切换功能失效的不足，本发明提供了一种力矩估计方法，具备良好的容错能力，根据负载的变化动态平衡最优性和鲁棒性等优点，解决了上述背景技术中所提出的问题。

(二)技术方案

为实现上述目的，本发明提供如下技术方案：一种力矩估计方法，包括a和b两个步骤：

a.建立谐波传动模型，包括谐波传动运动学模型、运动误差模型和谐波传动柔顺模型；

b.构建冗余自适应鲁棒卡尔曼滤波器；

加入自适应转换模块，同时将电机电流的函数作为冗余因子。

优选的，根据链路侧绝对编码器读数、电机侧编码器读数和电机电流对谐波传动柔顺性进行建模。

优选的，谐波传动的总扭转变形通过链节侧和电机侧编码器测量。

优选的，在线调整滤波器的容错能力，根据负载变化在稳健模式和最佳模式之间切换滤波模式。

优选的，根据负载的变化动态平衡最优性和鲁棒性。

优选的，总谐波传动扭转变形包括柔轮和波发生器的变形。

(三)有益效果

与现有技术相比，本发明提供了一种力矩估计方法，具备以下有益效果：

该力矩估计方法，将RARKF的冗余因子设计为电机电流的函数，以处理建模误差和扰动，可根据关节负载自适应地在最优滤波模式和鲁棒滤波模式之间切换，有效地平衡了估计精度和响应速度，兼顾了力矩估计算法中滤波的最优性和鲁棒性，进而提升了力矩估计的精度，解决了传统算法只具有最优性或者只具有鲁棒性而不能同时具有最优性和鲁棒性的矛盾，并且对建模误差具有良好的容错能力。

附图说明

图1为本发明方法的总体结构示意图；

图2为本发明中柔轮和波发生器的柔性传动示意图；

图3为本发明中谐波传动的典型刚度和迟滞曲线示意图；

图4为本发明中冗余自适应鲁棒卡尔曼滤波器切换模块基本原理示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参阅图1-4，一种力矩估计方法，包括a和b两个步骤：

a.建立谐波传动模型，包括谐波传动运动学模型、运动误差模型和谐波传动柔顺模型；

谐波传动运动学模型，用于描述谐波减速器的输入/输出运动学关系；

运动误差模型，用于描述链接侧编码器测量时的运动误差；

谐波传动柔顺模型，用于描述谐波减速器中的柔轮和波发生器的变形，并估算输出扭矩。

b.构建冗余自适应鲁棒卡尔曼滤波器；

加入自适应转换模块，同时将电机电流的函数作为冗余因子，以容忍建模误差和随负载变化的滤波模式切换。

优选的，根据链路侧绝对编码器读数、电机侧编码器读数和电机电流对谐波传动柔顺性进行建模。

优选的，谐波传动的总扭转变形通过链节侧和电机侧编码器测量。

优选的，在线调整滤波器的容错能力，根据负载变化在稳健模式和最佳模式之间切换滤波模式。

优选的，根据负载的变化动态平衡最优性和鲁棒性。

优选的，总谐波传动扭转变形包括柔轮和波发生器的变形。

该力矩估计方法，实施方式如下：

建立谐波传动模型，在建立模型时可以把该模型分为三部分，即谐波传动运动学模型、运动误差模型、谐波传动柔顺模型。

首先建立谐波传动运动学模型，假设理想的输入/输出运动学关系等于谐波传动组件处的角位置：

θ_w＝Nθ_f (1)

θ_w是波发生器的位置，θ_f是柔轮输出位置，N是传动比。

静力平衡可以描述为：

τ_w是波发生器的扭矩，τ_f是柔轮输出扭矩；

公式(1)和(2)表示谐波传动的理想线性输入/输出关系，其中谐波传动被视为完全刚性齿轮减速机构；然而，在引用的文献中提供的对输入/输出关系的经验测量表明，输出与输入不是线性相关的。

造成这种非线性的原因是谐波传动部件的扭转柔度、非线性摩擦力以及由齿轮啮合和加工误差引起的运动误差。给定描述谐波传动中存在的运动和力约束的理想运动学关系，其它的影响可以通过建模柔度、摩擦力和运动学误差来合并。

谐波传动的扭转柔度是由于柔轮和波发生器的柔顺性造成的。通过考虑波发生器的扭转柔度，可以捕捉谐波传动的迟滞行为，而不需要单独的迟滞模型，考虑了柔轮和波发生器的柔顺性。

柔轮的扭转角可以定义为：

Δθ_f＝θ_fo-θ_fi (3)

θ_fi表示柔轮齿轮周长的角位置，θ_fo表示使用链接侧编码器测量的负载侧的柔轮角度位置。

波发生器的扭转角定义为：

Δθ_w＝θ_wo-θ_wi (4)

θ_wo和θ_wi分别表示波发生器外部和中心部分的位置；

只有θ_fo和θ_wi分别由链接侧编码器和电机侧编码器测量，θ_fi和θ_wo不可用。

运动误差定义为测得的柔性样条输出减去预期的柔性样条输出。

因此，运动学误差

可以表示为：

谐波传动的总扭转角写为：

将公式(3)-(5)代入公式(6)，得到：

考虑到谐波传动摩擦，公式(2)表示为：

是相对于变速器输出侧看到的谐波传动摩擦转矩。

为了实用和简单起见，已证明Stribeck摩擦模型在机器人手臂上用于关节摩擦补偿时是有用的并经过验证的，该模型用于谐波传动扭矩估计并写为：

v是相对速度，F_s是静摩擦，F_c表示库仑摩擦力最小值，

和F_v是润滑剂和负载参数，δ是附加的经验参数。

下面建立谐波传动模型中的第二部分——运动误差模型。

仅当补偿运动误差的影响时，才能实现有效的扭矩估算，可以使用高分辨率链接侧绝对编码器测量完整一圈输出的运动误差。为了确定运动学误差

将测试接头顺时针和逆时针旋转一整圈输出而没有有效载荷。在此过程中测量总扭转变形Δθ_cw和Δθ_ccw。

和

分别是波发生器在顺时针方向和逆时针方向上的扭转变形，它们是通过在没有载荷的情况下在两个完整方向上旋转的方式使用链接侧和电动机侧编码器测量得出的。

由于输出扭矩等于零，因此挠性样条曲线的扭转变形也等于零，Δθ_f＝0。假设波发生器的扭转变形是对称的

运动误差可以通过以下公式确定：

下面建立谐波传动模型的第三部分——谐波传动柔顺模型。典型的谐波传动刚度曲线的特征是随着位移和迟滞行为而增加刚度。总谐波传动扭转变形包括柔轮和波发生器的变形。谐波传动扭转变形很大程度上是由柔轮扭转柔度引起的。Δθ_f由输出转矩的分段线性函数近似。

K₁,K₂,K₃,τ₁和τ₂由制造商给出，其曲线由刚度为K₁,K₂,K₃的三个直线段近似。刚度K₁适用于柔轮扭矩0到τ₁；刚度K₂适用于柔轮扭矩τ₁到τ₂；刚度K₃适用于柔轮扭矩大于τ₂的情况。

零扭矩输出时，谐波减速器的扭转变形可从

到

为了复制该刚度曲线的迟滞形状，波发生器的局部弹性系数近似为

K_w和C_w是需要被确定的常数。波形发生器的扭转角可使用以下公式计算：

把公式(15)代入公式(16)，可以得到

谐波传动的总扭转变形通过链节侧和电机侧编码器测量。波发生器转矩τ_w由电动机转矩指令近似。

Δθ_f和τ_f之间的关系表示在等式(14)中，其中Δθ_f是通过公式(17)与公式(7)组合计算得出的，显示为：

使用等式(14)的逆解，可估算的挠性样条输出扭矩τ_f可以表示为：

其中

上述模型中用到的变量符号见下表。

模型部分完成后，建立总体结构中的第二部分，也就是冗余自适应鲁棒卡尔曼滤波算法部分，由于不可避免的噪声，根据转矩公式计算出的估计结果不能直接用于关节的控制律，因此必须使用滤波算法，由于关节扭矩可能会迅速变化，因此需要强大的过滤功能以实现快速响应。

考虑到在鲁棒滤波中或多或少地牺牲了估计精度，采用自适应鲁棒滤波通过在最佳和鲁棒性之间引入滤波模式的自切换来抑制过多的牺牲。对于本文所涉及的关节扭矩估计，希望滤波模式在负载增加时从最佳状态切换到鲁棒性，而在负载减少时从鲁棒性切换到最佳性，但是存在未建模的非线性误差，该误差会随负载而变化，并可能导致滤波模式切换错误。

如果负载大小远大于不确定性，则滤波器将保持鲁棒模式，而不会出现故障切换；如果负载大小小于不确定性大小，则滤波器最好保持最佳模式，从而提供更准确的扭矩估算。

为了抑制误差的影响，引入了冗余自适应鲁棒卡尔曼滤波算法，用于关节扭矩估计。该方法具有冗余因素，可以帮助减少建模误差的负面影响。由于建模误差取决于关节负载，并且关节负载由电动机电流反映，因此冗余系数被设计为电动机电流的函数。在线调整滤波器的容错能力，根据负载变化在稳健模式和最佳模式之间切换滤波模式。滤波部分的输入包括等式(19)中的τ_f和测得的电流I_M。

设计过滤模型为

X(k)＝f(k,X(k-1))+w(k) (20)

Y(k)＝X(k)+v(k) (21)

X(k)是***在时间步长k上的状态，Y(k)是时间步长k上的测量值，k∈N。

***函数f(k,X(k-1))展示了在X(k)状态下的***的动力学，w和v是过程和测量噪声，分别假设不相关的零均值高斯白噪声和协方差矩阵W和V。

对于关节扭矩估计，f(k,X(k-1))＝X(k-1)，状态变量X是公式(19)中τ_f的滤波结果，测量值Y是来自等式(19)的计算结果，n和m是空间尺寸等于1。对应于转矩模型，w表示从时间步长k到k-1的转矩的未知变化，v描述了转矩计算结果中包含的随机噪声。在时间步长k，

Y(k)＝τ_f(k) (22)

冗余自适应鲁棒卡尔曼滤波算法形式如下：

1.状态预测：当前状态预测及其协方差矩阵为

P(k|k-1)＝P(k-1|k-1)+W(k-1) (24)

2.增益调整：预测误差的协方差矩阵通过下式调整

∑(k|k-1)＝S(P(k|k-1)) (25)

S是增益调度算子。

3.测量值更新：测量值的更新及其协方差矩阵

P_Y(k)＝∑(k|k-1)+V(k) (27)

4.估计更新：滤波增益和状态估计及其协方差矩阵为

P(k|k)＝(∑(k|k-1)^-1+V^-1(k))^-1 (30)

当S是单位运算符时，该算法等效于标准卡尔曼滤波算法。

典型的线性H_∞鲁棒滤波算法的设计目标是在存在建模误差的情况下确保滤波器的稳定性。将建模误差表示为E，如果满足以下条件：

对于命名为衰减因子的最小值γ，增益调度算子可以确定为：

∑(k|k-1)＝(P^-1(k|k-1)-γ^-2L^TL(k))^-1 (32)

L(k)∈R¹是用户定义的矩阵，用于调整预测协方差矩阵。

公式(31)表示γ被定义为估计误差和所有其他误差之和之间的最小传递函数。在本文中，建模误差指的是关节力矩和其他未建模的不确定性。

显然，小γ意味着从噪声和建模误差到估计误差的传递效率低，即***获得鲁棒性。鲁棒性和最优性的平衡可以通过调节衰减因子来调节。滤波鲁棒性随着衰减因子的增加而降低。当γ＝∞时，滤波器与卡尔曼滤波器相同。

衰减系数是用户定义的，有一个缺点。因为一旦给定γ，算法将工作在鲁棒滤波模式，可能达不到滤波最优性。为此，自适应鲁棒卡尔曼滤波算法提供了一种自适应切换机制，使滤波器具有自适应性。在自适应鲁棒卡尔曼滤波算法中，等式(25)中的增益调度算子由P(k|k-1)确定

α是一个将在后面的等式(37)中定义的因子，

定义如下：

ρ是一个遗忘因子。如果测量误差方差P_Y(k)小于

的值，算法将处于鲁棒滤波模式。为了满足滤波稳定性条件，正如公式(17)中

L(k)中的补偿因子满足

用公式(23)-(30)和(33)-(36)来定义自适应鲁棒卡尔曼滤波算法。

自适应鲁棒卡尔曼滤波算法的目标是通过使用基于更新频率的滤波模式的自适应实时切换来平衡滤波最优性和鲁棒性。然而，如果存在使不等式

始终成立的建模误差，公式(33)将等同于公式(32)。

当建模误差超过负载量级时，不需要触发鲁棒滤波状态。因此，需要可调的开关阀值来恢复等式(33)的转换函数，即α>1。

因此，考虑到公式(19)给出的转矩模型存在建模误差，设计了自适应鲁棒卡尔曼滤波中的冗余因子和补偿函数，形成冗余自适应鲁棒卡尔曼滤波算法，并应用于转矩滤波估计。建模误差随着关节负载的增加而增加，因此需要增加冗余因子α，以避免滤波最优性的过度损失。

考虑到关节载荷由时间步长k上测量的电机电流I_M(k)来表示,将冗余因子设计为

补偿函数项为：

其中补偿因子是：

在冗余因子α≠1的情况下，可以保证滤波器的稳定性。

用于扭矩估计的冗余自适应鲁棒卡尔曼滤波算法可以完全由公式(23)-(30)表示。

阈值被设置为建模误差的上限，并且如果测量的和预测的关节载荷之间的差超过预设阈值，则滤波器切换到鲁棒滤波模式。这有助于减少不必要的优化损失。因此，转矩估计在最优性和鲁棒性之间的切换变得更加有效，使得估计精度和快速跟踪能力之间的合理平衡。

本发明构建了协作机器人关节的谐波传动模型并设计了用于滤波的冗余自适应鲁棒卡尔曼滤波算法，通过自调整滤波增益和自适应切换滤波模式，使力矩估计滤波对负载变化具有最优性和鲁棒性，从而提升力矩估计的精度。

需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

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

Claims (3)

1.一种力矩估计方法，其特征在于，包括a和b两个步骤：

a.建立谐波传动模型，包括谐波传动运动学模型、运动误差模型和谐波传动柔顺模型；谐波传动运动学模型是基于假设理想的输入/输出运动学关系等于谐波传动组件处的角位置，并考虑谐波传动部件的扭转柔度、非线性摩擦力以及由齿轮啮合和加工误差引起的运动误差后建立的模型；运动误差模型是为实现有效的扭矩估算，在补偿运动误差的影响时，使用高分辨率链接侧绝对编码器测量完整一圈输出的运动误差，其中输出扭矩等于零；谐波传动柔顺模型是包括柔轮和波发生器的变形的模型，谐波传动扭转变形通过输出转矩的分段线性函数表示，波发生器转矩由电动机转矩指令近似，由此建立估算的挠性样条输出扭矩；

根据链路侧绝对编码器读数、电机侧编码器读数和电机电流对谐波传动柔顺性进行建模；总谐波传动扭转变形包括柔轮和波发生器的变形；

b.构建冗余自适应鲁棒卡尔曼滤波器；

冗余自适应鲁棒卡尔曼滤波器采用冗余自适应鲁棒卡尔曼滤波算法，包括（1）状态预测，预测当前状态及提供协方差矩阵；（2）卡尔曼增益系数更新，预测误差的协方差矩阵通过增益系数调整；（3）测量值更新，更新测量值和协方差矩阵；（4）估计更新：估计滤波增益和状态及提供协方差矩阵；

算法中加入自适应转换模块，设计冗余因子和补偿函数，在线调整滤波器的容错能力，根据负载变化在鲁棒模式和最佳模式之间切换滤波模式；冗余系数被设计为电动机电流的线性函数，补偿函数项为：

；其中

是当前状态下协方差矩阵的逆，

是补偿因子的倒数，

表示补偿函数，k是时间步长；

补偿因子由下式获得，

，其中，

是测量值的协方差矩阵，

是冗余因子，k是时间步长。

2.根据权利要求1所述的一种力矩估计方法，其特征在于：谐波传动的总扭转变形通过链节侧和电机侧编码器测量。

3.根据权利要求1所述的一种力矩估计方法，其特征在于：根据负载的变化动态平衡最优性和鲁棒性。

Images