基于双扩展卡尔曼滤波估计串联粘弹性作动器力矩的方法与流程

本发明属于串联粘弹性作动器技术领域，涉及一种估计串联粘弹性作动器力矩的方法，具体涉及一种基于双扩展卡尔曼滤波(dekf)估计串联粘弹性作动器力矩的方法。

背景技术：

随着工业4.0时代的到来，机器人产业得到了快速发展。各式各样的机器人不断涌现并服务于不同的行业，提升了生产的智能化程度，极大地促进了生产力发展。机器人代替人类完成许多重复和危险的工作，提高生产效率并降低工厂事故率的同时，也一定程度地解决了人口老龄化所带来的劳动力短缺问题。随着科技的进步，机器人在人类生产生活中将扮演着越来越重要的角色。工业机器人因其高速精确的定位能力，在喷漆、焊接等主要要求位置控制性能的任务中占据很大比重。但是，在一些接触式作业中，例如人机交互、抓取柔软物体、自动螺栓紧固、规避障碍物等，机器人与工件或环境难免发生接触，如果其末端执行器与工件或环境接触力太大，很可能给机器人和工件带来损伤甚至破坏。服务机器人需要频繁地与人交互，处理很多未知的接触，因此对其交互的友好性和安全性提出了更高要求。只有具备了对力的精确感知能力，作动器才能为接触后的决策和控制提供依据。因此力传感在提高接触式作业和人机交互安全性方面具有十分重要的作用。力矩传感器作为一种高精度的力传感方式，应用于许多高端的机器人领域。然而其昂贵的价格增加了机器人成本，自身的安装体积也占用了额外的空间，因此其应用范围受到一定限制。

串联弹性作动器(serialelasticactuator，sea)通常以弹性体形变与力的对应关系估计负载力/力矩，从而避免使用昂贵的力/力矩传感器。串联粘弹性作动器(serialviscoelasticactuator，sva)将弹性体替换为粘弹性体，在增强控制性能的同时，也使建立力/力矩估计模型更加困难，同时会引入各种复杂问题。

基于粘弹性体的力矩估计方法，基于zenner本构方程建立力矩模型，在短期内使用误差小于0.03n·m，然而在长期内使用存在诸多缺点，例如在温度变化、马林斯效应、自然老化等因素下，模型参数会发生不同程度的改变，致使模型误差增大甚至失效。

基于电机电流的力矩估计方法，作为一种传统且低成本的力矩估计方式，在一些力矩精度要求不高的场合得到一定程度应用，例如ur机器人的各种触停和柔性示教。然而在主流机器人中，出于关节力矩的输出要求，难以避免地引入减速器来提高力矩，而减速器力学特性复杂，导致系统摩擦力矩非线性强烈。减速器带来复杂摩擦力矩，使系统精确动力学建模很困难，使得通过电机电流估计力矩的精度大大降低。在复杂运动状态下，受各种因素影响，电流波动通常很大，也带来力矩估计困难，使得这种方式在主流机器人领域并未得到广泛应用。但是在较平稳的状态下，基于电机电流的力矩估计方法也可以达到不错的估计效果。

前述两种力矩估计方法，前者在短期内比较稳定而在长期内缺点很大，后者在长期来看相对稳定但在短期内可能存在问题。现有技术中，基于上述两种方法均不能独立有效地估计sva力矩。

技术实现要素：

为了解决现有技术中存在的问题：基于粘弹性体的力矩估计方法在温度变化、马林斯效应、自然老化等因素下估计力矩存在很大的误差；基于电机电流的力矩估计方法在零速、换向、角速度靠近stribeck速度的非线性区、角加速度较大的状态下，受摩擦力矩误差和转动惯量的影响很大，致使模型误差无法接受。本发明提供一种基于双扩展卡尔曼滤波估计串联粘弹性作动器力矩的方法，基于双扩展卡尔曼滤波融合前述两种方法，基于电机电流的力矩估计方法无时效性的优点能弥补基于粘弹性体的力矩估计方法时效性差的缺点，基于粘弹性体的力矩估计方法状态相关性弱的优点能弥补基于电机电流的力矩估计方法状态相关性强的缺点，两者融合后可以有效估计力矩，实现在任何情况下误差均小于0.03n·m，增强系统的鲁棒性。本发明的目的通过以下技术方案予以实现。

基于双扩展卡尔曼滤波估计串联粘弹性作动器力矩的方法，其特征在于，包括以下步骤：

(1)基于粘弹性体的sva力矩估计(简称粘弹性体力矩模型)；

(2)基于电机电流的sva力矩估计(简称电机电流力矩模型)；

(3)基于双扩展卡尔曼滤波的力矩融合：力矩融合过程包括两个扩展卡尔曼滤波器，一个是状态估计器ekf1，另一个是参数估计器ekf2，两个估计器级联运行；状态估计器ekf1以步骤(1)的力矩估计结果为基础估计力矩状态，参数估计器ekf2以步骤(2)的力矩估计结果为基础校正粘弹性体参数。

以下对各个步骤做详细描述。

(1)基于粘弹性体的sva力矩估计

根据所需的性能参数，选择合适的粘弹性材料用于制作粘弹性体，例如天然橡胶50a。在尽可能大的范围内，测试记录粘弹性体的输入力矩和输出扭转形变数据。输入力矩以力矩传感器检测，输出形变以角度编码器检测。观察力矩与扭转形变的滞回曲线特点，选择合适的粘弹性本构方程加以描述。例如天然橡胶线性度较好，选择基于线性粘弹性zenner本构方程建立力矩模型，公式为

其中t，δ分别为力矩和扭转角度，为对应的变化率，a为松弛系数，b为阻尼系数，c为刚度系数。为了便于用采样数据辨识出a，b，c，通常将其离散化为

tk＝-a1tk-1+b1δk-1+b2δk-2(2)

最后利用采集的输入力矩和输出形变离散数据，以最小二乘法辨识力矩模型的参数。

(2)基于电机电流的sva力矩估计

根据所设计的作动器结构特点，对各运动部件的动力学特性进行合理简化。例如将电机简化为只具有惯量的力矩源，减速器简化为具有摩擦阻尼和惯量的转动副。例如使用谐波减速器的系统中，建立系统动力学方程：

n为减速比，tm为电机力矩，tm为负载力矩，j0包括电机转子和谐波减速器波发生器的惯量，j1包括谐波减速器刚轮或柔轮和输出轴的惯量，分别为减速器输出端角加速度、角速度和角位置。阻尼d包括谐波减速器和系统其它转动副的阻尼，通常为的函数，还可能与角位置有关。

对模型的误差源进行隔离和补偿。由于惯性项测量误差很大，考虑隔离该误差源。在匀速转动或角加速度很小时，该项可忽略。阻尼项主要源于谐波减速器的摩擦力矩tf，无法进行隔离，需要对其精确建模并补偿。

经过测试发现，谐波减速器不仅包含与角速度相关的静态摩擦，还包含与角位置相关的动态摩擦。静态摩擦包括库伦摩擦、粘滞摩擦和非线性摩擦，常采用图2所示的stribeck指数摩擦模型描述。

它的数学表达式如下：

该模型的参数具有明确的物理意义。tc为库伦摩擦力矩，tms为最大静摩擦力矩，σ为粘滞摩擦因数，当足够大时σ为切线斜率，为stribeck速度，过截距tms的切线和过tc点水平线的交点横坐标即为δ是经验参数。本发明依据tustin经验模型，δ取值1。

在同一角速度下测得摩擦力矩与角位置存在正弦周期性变化关系，原因是谐波减速器或者转动联接中存在同轴度偏差问题，使得摩擦力矩在不同角位置处大小不同。以式(5)的静态摩擦加上正弦动态摩擦描述，其中a为正弦幅值。

结合式(4)和式(5)，得该sva的系统摩擦力矩模型：

再由电机力矩tm与电流im和电机力矩常数c的正比例关系tm＝cim，得出在平稳状态下隔离了加速度惯性项后，利用电流计算负载力矩的公式

tl＝ncim-tf(7)

定义一个置信函数w表示当前状态估计力矩的置信度

w取值范围为[0，1]，越接近0表示越不可信，越接近1越可信。由角速度角加速度电流变化率以及常数stribeck速度和比例因子γ计算得到，当角速度越远离表征摩擦非线性区的stribeck速度时，置信度越高；当角加速度电流变化率越大时，表示角速度越不稳定，两者的乘积越大，计算的置信度越低。γ为便于统一单位和数量级的缩放因子。

(3)基于双扩展卡尔曼滤波的力矩融合

融合两种力矩是对力矩t的状态估计问题和对粘弹性体模型中a，b，c的参数估计问题，双扩展卡尔曼滤波可用于非线性系统状态估计或参数估计，一个用于状态估计，称为状态估计器ekf1，另一个用于参数估计，称为参数估计器ekf2。

首先设计状态估计器ekf1：令状态向量x＝[t]，输入向量以粘弹性体模型为基础建立离散系统方程为

其中ts为离散系统采样时间间隔，

量测方程需要经过特殊设计和处理，唯一能当作传感器量测值的电机电流模型并非任何时候都有效，在非平稳状态下当电机电流模型无效时，显然此时不能对预测的状态向量进行更新。如果将测量方程定义成电流估计力矩tcur和预测力矩xk的损失函数

并且认为任意时刻的实际量测值都为0。那么预测的状态向量会向使损失函数趋于0的方向变化，最终收敛于真值。当sva运行在非平稳状态时，由于w＝0，无论电流估计的tcur为多少，zk＝0，与设定的实际量测值zk相等，由橡胶体预测的状态向量xk不更新；当sva运行在平稳状态时，w靠近1，zk不为0。xk将会朝向tcur变化，同时使损失函数趋于0，使状态更新。由量测方程得状态估计器ekf1的量测矩阵

设计参数估计器ekf2。参数估计器需要估计的参数为橡胶力学模型参数a，b，c。令状态向量x＝[a，b，c]^t，过程方程：

由状态向量xk计算得到预测的力矩。测量方程与ekf1类似，离散形式为

tk、tk-1分别为ekf1在k和k-1时刻的输出，δk、δk-1分别为k和k-1时刻的形变量。量测方程的设计原理同ekf1，认为量测方程的实际值为0。则状态向量会被驱使改变，改变的方向就是使损失函数的值趋于0，即参数a，b，c会被不断迭代更新，直至收敛于某一时间内的真值。当这些参数因时效性等因素再次改变时，滤波器会继续将其校正到改变后的真值。

此时的新息

则测量矩阵为

基于上述讨论的过程方程和量测方程，绘制出图1所示的dekf工作流程图，其基本流程为：先计算参数估计器ekf2一步预测的先验参数即参数再将先验参数用于状态估计器ekf1的一步预测阶段，得到ekf1一步预测的先验状态继续完成ekf1的更新阶段得到后验状态最后将后验状态即估计的力矩用于ekf2的更新阶段，得到ekf2的后验参数由此完成一次dekf算法的迭代，迭代的结果既得到了状态估计值t，又得到了参数估计值[a，b，c]。

本发明所提供的基于双扩展卡尔曼滤波估计串联粘弹性作动器力矩的方法，该方法解决了粘弹性体力矩模型短期有效而长期无效，电流估计力矩模型在平稳状态有效而非平稳状态无效的问题。电流估计力矩模型在运行平稳状态，能很好地计算力矩，能够作为一种有效的力矩观测值。且该观测值的误差与粘弹性体力矩模型无关，误差不会受粘弹性体的马林斯效应、内部温度、使用寿命等因素影响。因此作为力矩估计器的同时，还可作为对粘弹性体模型参数修正的依据。当运行在非平稳状态时，电流估计力矩存在很大误差，力矩估计失效。在这段较短的非平稳状态期间，粘弹性体参数几乎不变，粘弹性体模型又可以作为有效的力矩估计手段，填补了电流模型的真空期。电流模型无时效性的优点能弥补粘弹性体模型时效性的缺点，粘弹性体模型状态相关性弱的优点能弥补电流模型状态相关性强的缺点，两者融合时可增强系统的鲁棒性。

附图说明

图1是基于双扩展卡尔曼滤波的sva力矩估计方法流程图。

图2是stribeck指数摩擦模型

图3是串联弹性作动器结构示意图。

图4是橡胶体力矩-扭转角度曲线。

图5是摩擦力矩-转速-时间关系图

图6a-d是谐波减速器动态摩擦与角位置关系图。

图7是谐波减速器角速度与摩擦力矩关系图。

图8是理想粘弹性体参数条件下dekf估计力矩。

图9是非理想粘弹性体参数条件下dekf估计力矩。

附图标记：1-电机，2-谐波减速器，3-输入角度编码器，4-粘弹性体，5-输出角度编码器，6-动态力矩传感器。

具体实施方式

下面结合附图对本发明作进一步详细的描述。

本发明涉及的串联粘弹性作动器结构如图3所示，由电机1、谐波减速器2、输入角度编码器3、粘弹性体4、输出角度编码器5、动态力矩传感器6以及单片机和上位机组成。

单片机作为系统主控制器，控制电机1的转速和扭矩。

单片机采集输入角度编码器3和输出角度编码器5的脉冲信号，解算为角度θ1和θ2，由此得到粘弹性体的形变量θs＝θ2-θ1，减速器输出端绝对角位置θ＝θ1，减速器的输出端转速由角位置求导得粘弹性体的形变量变化率由粘弹性体的形变量θs对时间求导得到。单片机采集动态力矩传感器6输出的模拟电压信号，并转换为对应的力矩值，作为输出端的真实值，用于摩擦力矩模型参数辨识。

上位机采集电机电流im，并将电流im低通滤波以减小高频噪声干扰。上位机接收单片机上传的角度相关的数据和力矩传感器数据，并对数据进行相应的处理。

在使用dekf融合力矩之前，需要对橡胶体力矩模型和谐波减速器的摩擦力矩模型进行参数辨识以及判断电流估计力矩准确度的判断。

粘弹性体力矩模型参数辨识：

由于所使用的测试驱动电机为位置控制型，设置位置输入信号为

x＝22°sin(2πt)(15)

通过力矩传感器测量力矩，编码器测量形变量，得出图4所示粘弹性体力矩-扭转角度曲线。与线性粘弹性zenner本构方程吻合较好，因此基于该本构方程建立式(16)的离散力矩模型。

tk＝-a1tk-1+b1δk-1+b2δk-2(16)

并通过最小二乘法辨识初始的模型参数[a1，b1，b2]＝[-0.9873,147.3，-146.0]，得到最终离散力矩模型为

tk＝0.9873tk-1+147.3δk-1-146.0δk-2(17)

谐波减速器的摩擦力矩模型的参数辨识：

在空载条件(tl＝0)下，测得不同转速条件下摩擦力矩随时间变化数据，并绘制图5所示摩擦力矩、转速、时间的三维曲线，每条曲线对应一个转速下摩擦力矩变化情况。静态摩擦与角速度相关，动态摩擦与角位置相关，在角速度分别为0.65rad/s，1.18rad/s，1.70rad/s，2.23rad/s的匀速条件下，测得一段时间内的电流并换算为摩擦力矩，得到摩擦力矩随减速器输出端角位置变化曲线如图6所示，曲线随角位置呈正弦规律变化，由于角速度大小不同，正弦偏移量不同，对应于不同的静态摩擦力矩。采用正弦函数拟合得

在一定角速度下，通过采集一段较长时间电机电流的均值，计算出的摩擦力矩可以认为是该角速度下的静态摩擦。输入幅值为0.1～2.6rad/s的阶梯的角速度指令，步长为0.1rad/s，每步持续时间100s，计算得到图7所示静态摩擦力矩随角速度变化的曲线。采用stribeck静态摩擦模型拟合静态摩擦特性，并根据其参数确定方法，得出在正方向上的拟合公式

考虑角速度方向，综合动态摩擦和静态摩擦的谐波减速器摩擦力矩

反映电机运转平稳程度的置信函数w。已知stribeck速度通过调参确定比例因子γ＝0.02，此时w为：

基于双扩展卡尔曼滤波的力矩融合：

辨识了粘弹性体力矩模型初始参数和谐波减速器的摩擦力矩模型后，通过dekf进行融合力矩来弥补前两种方法的不足。设置粘弹性体力矩模型、电流力矩模型和dekf模型参数值如表1所示。

表1模型初始参数设置

dekf工作的基本流程如图1所示。先计算参数估计器ekf2一步预测的先验参数即参数再将先验参数用于状态估计器ekf1的一步预测阶段，得到ekf1一步预测的先验状态继续完成ekf1的更新阶段得到后验状态最后将后验状态即估计的力矩用于ekf2的更新阶段，得到ekf2的后验参数由此完成一次dekf算法的迭代，迭代的结果既得到了状态估计值t，又得到了参数估计值[a，b，c]。

首先验证状态估计器ekf1的效果，将粘弹性体力矩模型参数[a，b，c]设置成辨识的结果[0.7764，105.5，114.8]，由于实验间隔时间较短且环境温度相近，认为这组参数是此时的正确参数，因此可忽略ekf2的作用。实验结果如图8所示，整个过程中力矩估计误差都较小，最大不超过0.03n·m。虽然该工况下转速频繁换向，电流无法有效估计力矩，但是融合了粘弹性体力矩模型后，在这些复杂的运行状态下也能有效估计力矩，即力矩估计器ekf1是有效的。

验证参数估计器ekf2的效果，将粘弹性体力矩模型参数[a，b，c]设置成与辨识结果偏差很大的错误值[10,10,10]，同样成在线状态进行dekf数据处理，得到图9所示的估计结果。可以看出，由于粘弹性体参数偏离真实参数较大，在开始阶段，估计的力矩与真实力矩偏差较大，最大偏差接近1n·m，而随着ekf2的参数估计作用，粘弹性体的参数被逐渐估计到接近正确值，力矩误差也随之减小，减小到与图8同一水平。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明披露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求书的保护范围为准。