CN110303521A - 机器人关节力矩信号采集系统及方法 - Google Patents

Abstract

本发明涉及一种机器人关节力矩信号采集系统及方法，属于机器人技术领域。其将测量干扰噪声划分为谐波机械传动噪声，工频电磁干扰噪声，以及传感器测量误差。设计自适应振荡器对工频干扰噪声和谐波传动噪声进行数学建模，建立其建状态转移方程和观测方程，对系统固有噪声进行估计和分离。同时，针对基于动力学模型的力矩预测方法的不足，采用基于加权二次型回归的力矩状态预测算法，提升了状态预测的准确性与快速性。利用卡尔曼滤波器对系统转移方程和测量方程存在的不确定性噪声予以滤除，并将测量力矩数据与预测数据进行融合，对系统固有的干扰噪声幅值、相位和频率以及真实的力矩输出值进行无偏最优估计，实现力矩信号的去噪滤波。

Description

机器人关节力矩信号采集系统及方法

技术领域

本发明涉及机器人技术领域，特别涉及机器人控制技术领域，具体是指一种机器人关节力矩信号采集系统及方法。

背景技术

机器人末端力矩传感器用于检测机械臂与所处环境的接触力/力矩的大小和方向，并反馈给力控制系统，是机器人智能化的重要标志。机械臂末端力矩传感器设计增强了人机交互能力，是可穿戴式康复/负重机器人、协作机器人以及柔性机械臂必不可少的测量元器件，既保障了机器人自身工作的安全性，同时也实现了人机之间的柔性接触，确保了人机交互的安全性与舒适性。准确快速的输出力矩反馈是实现机器人柔顺控制，力控制必不可少的环节，同时也用以补偿谐波传动的高度非线性特性，将机器人关节等效为力矩源，提升控制系统的准确度与稳定性。机器人关节末端力传感器设计扩展了机器人的适用范围，近年来，在康复医疗机器人、服务机器人、空间机器人以及特种机器人等得到了广泛的应用。

当前关节力矩输出采集系统可分为基于电流检测和基于力矩传感器两种框架。基于电机电流检测方法通过机器人关节的输入电流和电机模型估算出关节的力矩输出，由于电机直接力矩输出需要经过谐波减速机传动到负载端，谐波减速机传动装置摩擦模型未知且高度非线性，并且电机数学模型存在误差，导致无法准确感知末端的力矩输出，并且存在力矩感知延迟大，灵敏度低等缺点；基于力矩传感器的感知方法通过采集关节末端的力矩传感器数值，并且进行一系列的滤波去噪处理来测量关节的力矩输出，改善了力矩测量的快速性，然而由于力矩传感器信号存在大量的测量噪声、干扰噪声以及电路系统引入的电磁干扰噪声，传统采用非模型低通滤波去噪的方法，会造成力矩测量信号精度降低、延迟增大的现象。

发明内容

本发明的目的是克服了上述现有技术中的缺点，提供一种机器人关节力矩信号采集系统是基于力矩传感器感知的方法，其依据噪声特性对其进行数学建模，通过对噪声的估计从而将其力矩信号中消除，这种基于噪声模型的去噪滤波方法极大提升了滤波效率，明显提升了力矩测量信号的准确性和快速性。本文中的方法设计自适应振荡器对工频干扰噪声和谐波传动噪声进行建模，研究基于加权二次型回归的力矩状态预测算法，提升力矩输出状态预测的准确性与快速性，并以此建立系统的状态转移方程和观测方程，利用卡尔曼滤波器对转移方程和测量方程中存在的不确定性噪声予以滤除，并将测量力矩数据与预测数据进行融合，实现对关节力矩输出、机械干扰以及电磁干扰噪声的最优无偏估计。

为了实现上述的目的，本发明的机器人关节力矩信号采集系统具有如下构成：

力矩传感器，设置于机器人关节输出末端，用于测量关节的输出力矩，产生力矩传感器测量信号；

下位机控制器，连接所述的力矩传感器，用于获取所述的力矩传感器测量信号；

通信板卡，包括中断控制器，连接所述的下位机控制器，用以控制所述的下位机控制器执行信号采集中断函数以获取所述的力矩传感器测量信号，并从所述的下位机控制器获取所述的力矩传感器测量信号；

上位机，根据从所述的通信板卡获取的力矩测量信息产生力矩输出值。

该机器人关节力矩信号采集系统中，所述的上位机包括卡尔曼滤波器，用以消除所述的力矩测量信息中的噪声。

该机器人关节力矩信号采集系统中，所述的力矩传感器测量信号τ_sensor如下式所示：

τ_sensor＝τ_out+n_me+n_el+n_sys。

其中，τ_out为真实的关节输出力矩、n_me为机械传动噪声、n_el为电磁干扰噪声，为n_sys系统随机噪声，

所述的卡尔曼滤波器具体包括：

机械噪音模型振荡器，用以消除所述的机械传动噪声n_me；

电磁噪音模型振荡器，用以消除所述的电磁干扰噪声n_el；

力矩拟合预测模型，用以消除所述的系统随机噪声n_sys。

该机器人关节力矩信号采集系统中，所述的消除机械传动噪声n_me和消除电磁干扰噪声n_el具体为：

对机械传动噪声n_me和电磁干扰噪声n_el进行数学建模，获取系统固有干扰噪声的幅值、相位和频率，对所述的力矩传感器测量信号τ_sensor中的机械传动噪声n_me和电磁干扰噪声n_el成分进行分离。

该机器人关节力矩信号采集系统中，对机械传动噪声n_me和电磁干扰噪声n_el进行数学建模，获取系统固有干扰噪声的幅值、相位和频率，具体为：

频率为ω₁,ω₂的正弦噪声在k时刻的状态转移方程：

A₁sin(ω₁(k+1)ΔT+θ₁)＝A₁sin(ω₁kΔT+θ₁)cos(ω₁ΔT)+A₁cos(ω₁kΔT+θ₁)sin(ω₁ΔT)

A₂sin(ω₂(k+1)ΔT+θ₂)＝A₂sin(ω₂kΔT+θ₂)cos(ω₂ΔT)+A₂cos(ω₂kΔT+θ₂)sin(ω₂ΔT)

正弦噪声的状态转移方程包含了同样幅值、相位和频率的余弦函数。

该机器人关节力矩信号采集系统中，所述的消除系统随机噪声n_sys具体为：

力矩拟合预测模型基于加权二次型回归力矩状态预测算法对力矩状态进行修正，消除所述的系统随机噪声n_sys。

该机器人关节力矩信号采集系统中，所述的加权二次型回归力矩状态预测算法包括以下步骤：

(1)系统初始化，初始化系统的初始状态X、协方差矩阵P、以及过程噪声Q和测量噪声R；

(2)状态一步预测，更新长度为L的历史力矩测量数据τ_k-L,τ_k-L+1,…,τ_k，通过加权二次线性回归算法计算拟合曲线的参数和构建系统转移矩阵A和控制矩阵B，实现系统状态变量的一步预测，如下式所示：

(3)协方差一步预测，系统状态变量误差协方差矩阵∑随着状态变量的转移进行同步更新，根据转移方程自身的高斯白噪声对状态变量的影响，系统协方差矩阵更新公式如下：

(4)计算卡尔曼增益，最小化系统变量误差的协方差矩阵∑，对系统误差关于卡尔曼增益K进行求导，并令其导数为0，可得如下的卡尔曼增益计算公式：

并利用当前状态变量自身协方差矩阵∑和观测方程的协方差矩阵R，计算增益矩阵K，用于融合预测值和当前测量值，实现系统状态的更新；

(5)系统状态更新，利用当前的力矩测量值τ_k+1和所述的步骤(4)计算的卡尔曼增益K，对系统状态变量进行数据融合和更新，公式如下：

(6)协方差更新，

根据所述的步骤(4)计算的卡尔曼增益和系统状态变量的协方差一步预测值进行协方差矩阵的更新，至此k+1时刻单次迭代的卡尔曼滤波步骤完成，k+2时刻，返回步骤(2)，继续进行卡尔曼滤波的循环迭代过程，直至滤波过程结束。

本发明还提供一种利用上述的机器人关节力矩信号采集系统实现的机器人关节力矩信号采集方法。

采用了该发明的机器人关节力矩信号采集系统和方法，其依据噪声特性对其进行数学建模，通过对噪声的估计从而将其力矩信号中消除，这种基于噪声模型的去噪滤波方法极大提升了滤波效率，明显提升了力矩测量信号的准确性和快速性。本文中的方法设计自适应振荡器对工频干扰噪声和谐波传动噪声进行建模，研究基于加权二次型回归的力矩状态预测算法，提升力矩输出状态预测的准确性与快速性，并以此建立系统的状态转移方程和观测方程，利用卡尔曼滤波器对转移方程和测量方程中存在的不确定性噪声予以滤除，并将测量力矩数据与预测数据进行融合，从而实现对关节力矩输出、机械干扰以及电磁干扰噪声的最优无偏估计。

附图说明

图1为柔性机器人关节机械结构示意图。

图2为柔性机器人关节功能模块结构示意图。

图3为力矩采集系统结构图。

图4为由力矩信号变换进行频域和时频域分析获得的频域和时频域特征。

图5为本发明所采用的滤波去噪方法的框图。

图6为基于高斯核函数的权值函数权值分布示意图。

图7为力矩信号通过线性回归与加权二次型回归方法进行回归拟合的对比图。

图8为本发明所采用的卡尔曼滤波算法流程示意图。

图9为原始力矩测量信号和滤波之后的信号对比图。

图10为滤滤波计得到的机械干扰噪声和电磁干扰噪声波形，噪声分布符合实际情况示意图。

具体实施方式

为了能够更清楚地理解本发明的技术内容，特举以下实施例详细说明。

在一种实施方式中，本发明的机器人关节力矩信号采集系统包括：

力矩传感器，设置于机器人关节输出末端，用于测量关节的输出力矩，产生力矩传感器测量信号；

下位机控制器，连接所述的力矩传感器，用于获取所述的力矩传感器测量信号；

通信板卡，包括中断控制器，连接所述的下位机控制器，用以控制所述的下位机控制器执行信号采集中断函数以获取所述的力矩传感器测量信号，并从所述的下位机控制器获取所述的力矩传感器测量信号；

上位机，根据从所述的通信板卡获取的力矩测量信息产生力矩输出值。

在较优选的实施方式中，所述的上位机包括卡尔曼滤波器，用以消除所述的力矩测量信息中的噪声。

所述的力矩传感器测量信号τ_sensor如下式所示：

τ_sensor＝τ_out+n_me+n_el+n_sys。

其中，τ_out为真实的关节输出力矩、n_me为机械传动噪声、n_el为电磁干扰噪声，为n_sys系统随机噪声，

所述的卡尔曼滤波器具体包括：

机械噪音模型振荡器，用以消除所述的机械传动噪声n_me；

电磁噪音模型振荡器，用以消除所述的电磁干扰噪声n_el；

力矩拟合预测模型，用以消除所述的系统随机噪声n_sys。

在进一步优选的实施方式中，所述的消除机械传动噪声n_me和消除电磁干扰噪声n_el具体为：

对机械传动噪声n_me和电磁干扰噪声n_el进行数学建模，获取系统固有干扰噪声的幅值、相位和频率，对所述的力矩传感器测量信号τ_sensor中的机械传动噪声n_me和电磁干扰噪声n_el成分进行分离。

对机械传动噪声n_me和电磁干扰噪声n_el进行数学建模，获取系统固有干扰噪声的幅值、相位和频率，具体为：

频率为ω₁,ω₂的正弦噪声在k时刻的状态转移方程：

A₁sin(ω₁(k+1)ΔT+θ₁)＝A₁sin(ω₁kΔT+θ₁)cos(ω₁ΔT)+A₁cos(ω₁kΔT+θ₁)sin(ω₁ΔT)

A₂sin(ω₂(k+1)ΔT+θ₂)＝A₂sin(ω₂kΔT+θ₂)cos(ω₂ΔT)+A₂cos(ω₂kΔT+θ₂)sin(ω₂ΔT)

正弦噪声的状态转移方程包含了同样幅值、相位和频率的余弦函数。

所述的消除系统随机噪声n_sys具体为：

力矩拟合预测模型基于加权二次型回归力矩状态预测算法对力矩状态进行修正，消除所述的系统随机噪声n_sys。

在更优选的实施方式中，如图8所示，所述的加权二次型回归力矩状态预测算法包括以下步骤：

(1)系统初始化，初始化系统的初始状态X、协方差矩阵P、以及过程噪声Q和测量噪声R；

(2)状态一步预测，更新长度为L的历史力矩测量数据τ_k-L,τ_k-L+1,…,τ_k，通过加权二次线性回归算法计算拟合曲线的参数和构建系统转移矩阵A和控制矩阵B，实现系统状态变量的一步预测，如下式所示：

(3)协方差一步预测，系统状态变量误差协方差矩阵∑随着状态变量的转移进行同步更新，根据转移方程自身的高斯白噪声对状态变量的影响，系统协方差矩阵更新公式如下：

(4)计算卡尔曼增益，最小化系统变量误差的协方差矩阵∑，对系统误差关于卡尔曼增益K进行求导，并令其导数为0，可得如下的卡尔曼增益计算公式：

并利用当前状态变量自身协方差矩阵∑和观测方程的协方差矩阵R，计算增益矩阵K，用于融合预测值和当前测量值，实现系统状态的更新；

(5)系统状态更新，利用当前的力矩测量值τ_k+1和所述的步骤(4)计算的卡尔曼增益K，对系统状态变量进行数据融合和更新，公式如下：

(6)协方差更新，

根据所述的步骤(4)计算的卡尔曼增益和系统状态变量的协方差一步预测值进行协方差矩阵的更新，至此k+1时刻单次迭代的卡尔曼滤波步骤完成，k+2时刻，返回步骤(2)，继续进行卡尔曼滤波的循环迭代过程，直至滤波过程结束。

本发明还提供一种利用上述的机器人关节力矩信号采集系统实现的机器人关节力矩信号采集方法。

在实际应用中，柔性机器人关节如图1及图2所示，由三相直流无刷电机、光电编码器、谐波减速机、伺服控制器和位于关节输出末端力矩传感器组成，四者的机械连接结构如图1所示。其功能模块结构如图2所示。

电机作为驱动关节的力量源，提供转动的力矩驱动系统运行，编码器用于测量关节输出轴的角位置、角速度和角加速度等信息，为控制系统提供位置反馈信息，力矩传感器则用于测量驱动关节的输出力矩，实现关节力控制和柔性控制。

输出力矩信息采集系统由力矩传感器、ADS1256模数转换模块、Arduino下位机控制器、EtherCAT通信板卡、PC控制器(ROS：Robot Operating System)以及电源模块组成。Arduino下位机控制器通过SPI通信总线与ADS1256数据采集模块和EtherCAT通信板卡进行通信；EtherCAT通信板卡利用高速EtherCAT现场总线与上位机中的ROS节点进行数据交换，并且由其内部的中断控制器触发Arduino下位机执行信号采集中断函数；在中断函数中，Arduino控制器通过SPI通信总线发送AD模数转换命令触发ADS1256的力矩信号采集，并且将采集结果返回到控制器中；Arduino控制器通过SPI通信总线将力矩测量信息发送到EtherCAT通信板卡，结束中断函数；EtherCAT通信板卡将信息发布到EtherCAT系统控制总线，实现1kHz的高频率的力矩信号采样与传输；电源模块则为上述所述模块提供能源；力矩采集系统结构图如图3所示。

谐波减速机摩擦力模型未知且高度非线性，传统基于电流检测的方法无法准确测量和感知关节末端的力矩输出，给机器人控制系统的设计带来了极大挑战。关节末端力矩传感器设计则有效地克服了这一缺陷，其在关节输出末端和电机输出之间形成一个闭环，能够借助于末端力矩测量信息对谐波减速机的摩擦力和电机自身建模误差进行补偿，将关节近似为一个力矩源，实现机器人精准的柔性控制与力控制等，因此准确稳定的力矩反馈显得至关重要。

然而关节力矩采集系统中的测量信号夹杂着大量的噪声，给控制系统的稳定性带来了极大挑战，严重影响了控制精度和系统稳定裕度。经过对机械结构研究以及测量信号的分析，将噪声的来源分为三部分：

由谐波减速机的机械传动装置导致的固有噪声，由钢轮与柔轮啮合的齿顺次移动引发的噪声，波发生器旋转1圈，钢轮/输出轴移动6个齿位，因此由谐波机械传动引发的噪声频率为电机转速的6倍。

由力传感器中应变片特性和AD模数转换芯片精度而导致的误差，该误差通常表现为随机噪声，可以近似使用白噪声来代替。

力矩传感器采集系统通常处于大量的电气设备和电磁环境中，使得电磁干扰尤其是电力系统的50Hz工频干扰噪声引入到检测信号中。

因此力矩传感器测量信号τ_sensor由真实力矩输出、机械传动噪声、电磁干扰噪声和系统随机噪声四部分组成，如下式所示：

τ_sensor＝τ_out+n_me+n_el+n_sys

通过对采集的力矩信号进行傅里叶变换(FFT)和短时傅里叶(SFT)变换进行频域和时频域分析，得到信号的频域和时频域特征，如下图所示：

在实验过程中，力矩变化过程比较缓慢，其频率在10Hz以下，所以力矩测量信号频谱中高于10Hz频率成分可以认定为噪声成分。从力矩测量信号的频谱中可以看出当中存在着大量的50Hz工频成分，其为工频电磁干扰噪声；另一方面，力矩信号的时频图中某些高频噪声成分随着时间发生变化，并且和电机转速成倍数关系，因此此部分有理由认为是谐波机械传动导致的噪声；力矩信号频谱在其他频率上也存在少量分布，但幅值都比较小，可认为是系统的随机噪声，此部分使用高斯白噪声进行近似和简化。

本发明的优点在于：

1、通过对谐波机械传动结构研究和末端力矩传感器信号的时域、频域与时频域分析，将测量干扰噪声划分为谐波机械传动噪声，工频电磁干扰噪声，以及传感器测量误差三类，并且对噪声的结构和特性进行了辨识。

2、设计自适应振荡器对工频干扰噪声和谐波传动噪声进行数学建模，建立其建状态转移方程和观测方程，对系统固有噪声进行估计和分离。

3、针对基于动力学模型的力矩预测方法的不足，研究基于加权二次型回归的力矩状态预测算法，实验验证表明上述方法提升了状态预测的准确性与快速性。

4、利用卡尔曼滤波器对系统转移方程和测量方程存在的不确定性噪声予以滤除,并将测量力矩数据与预测数据进行融合，对系统固有的干扰噪声幅值、相位和频率以及真实的力矩输出值进行无偏最优估计，实现力矩信号的去噪滤波。

由谐波减速机机械振动造成的干扰在力矩测量信号中表现为高频的正弦振荡，且振荡频率为转速的6倍，另一方面工频电磁干扰的频率表现为50Hz的正弦振荡，在这种情况下噪声结构和部分信息已知，则能够对噪声信号进行建模。为此设计两个自适应振荡器对机械和电磁噪声进行建模，建立噪声的状态转移和观测模型，并且通过加权二次型回归对真实的力矩输出值进行预测。将真实的力矩输出、机械噪声和电磁干扰噪声作为系统的状态变量，以力矩传感器测量值作为系统的观测变量，通过卡尔曼滤波器将系统的状态转移噪声和观测噪声予以滤除，并且对力矩输出和机械电磁干扰的噪声模型幅值和相位进行最优估计。这种基于噪声模型的滤波方法比传统的低通滤波效率更高，同时也能够显著降低由低通滤波所带来的延时影响。本发明所采用的滤波去噪方法的框图如图5所示。

本发明采用基于加权二次型回归的状态预测。从夹杂有噪声的力矩传感器信号中还原出真实的力矩输出，需要建立力矩输出的状态转移方程，从当前的系统状态预测出下一时刻的力矩输出。Hamid D.Taghirad等人通过建立负载端的动力学模型的方法，利用当前机械臂位置、速度以及加速度信息和动力学方程对力矩输出值进行估计和预测，机械臂动力学方程如下式所示：

其中τ_out为关节末端的力矩输出，τ_ext是机械臂受到的外界力矩，而两者之和为机械臂承受的总力矩；M(Θ)为操作臂的n×n矩阵，是n×1的离心力和哥氏力矢量，G(Θ)是n×1重力矢量。

从上式可知，当机械臂在自由空间中运动，不存在外力干扰的情况(τ_ext＝0)，能够通过方程右边的动力学方程对关节力矩输出τ_out进行估计和预测。但在实际运行环境下，机械臂会与外界环境发生接触碰撞，穿戴式机器人要与佩戴者进行人机协作，即τ_ext≠0，这种外界的力交互作用将会对这种基于动力学模型的力矩预测方法产生极大估计误差。

本发明提出一种基于二次曲线拟合的方式对力矩输出进行一步预测，利用过去一段时间的力矩测量信息进行加权曲线回归，通过曲线函数的外延实现下一时刻的力矩输出的预测。为了能够在力矩信号的拐点突变处取得良好的拟合效果，在此选用二次型曲线进行非线性拟合，并且使用高斯核函数对过去时刻的拟合误差进行加权处理，使得回归趋势侧重于当前时刻变化趋势，减小预测误差。

基于高斯核函数的权值函数如下所示：

式中，w_i表示i时刻拟合误差的权值大小，σ²为高斯分布的方差，u是高斯分布的期望。当高斯分布参数u＝200,σ²＝50时，时间点i＝0,1,2,…,200拟合误差的权值分布如图6所示。

令系统的采样周期为ΔT，拟合区间的长度为L，二次曲线拟合模型如下所示：

y＝a·t²+b·t+c

使用最小二乘法定义曲线的拟合误差：

式中，τ_i是i时刻的力矩测量值，t_i为i时刻的时间值，w_i代表在i时刻拟合曲线输出值与真实测量值之间误差的权重。如此便将曲线拟合问题转化为如下的优化问题：

将上式改写为矩阵的形式，

式中，是由力矩测量值组成的观测矩阵，H为时间变量组成的数据矩阵，W代表权值矩阵，是需要求解的参数向量。

上述优化问题是关于变量的二次优化问题，其本质是一个凸优化问题，只存在一个全局最优解，并且最优解在导数等于0处取得，为此对目标函数进行求导，如下：

令导数为0，可得：

此时曲线参数的估计值为：

在k时刻系统状态信息的基础上，通过回归二次曲线函数的外延，对k+1时刻，力矩输出状态的一步预测值如下：

对力矩测量信号中的一段变化剧烈的数据分别通过线性回归和加权二次型回归方法进行回归拟合，拟合效果如图7所示。

由图7可知，线性拟合不能够较好地表示力矩传感器数据在拐点处的变化趋势，基于此种方式的输出力矩一步预测也将存在较大误差；加权二次型回归曲线则可以完整清晰地表达力矩数据的变化趋势，并且能够贴合当前最近一段时间的力矩变化趋势，能够实现准确的力矩一步预测。

通过对谐波机械传动噪声、工频电磁干扰噪声和力矩输出进行数学建模，获取系统固有干扰噪声的幅值、相位和频率，对测量力矩信号中的固有噪声成分进行分离，通过卡尔曼滤波对系统随机噪声进行过滤，从而实现关节力矩输出值准确无偏估计。因此，将频率ω₁的正弦噪声(机械传动噪声)，频率ω₂＝50Hz的正弦噪声(电磁干扰噪声)和关节力矩的输出值作为系统的状态变量。

频率为ω₁,ω₂的正弦噪声在k时刻的状态转移方程：

A₁sin(ω₁(k+1)ΔT+θ₁)＝A₁sin(ω₁kΔT+θ₁)cos(ω₁ΔT)+A₁cos(ω₁kΔT+θ₁)sin(ω₁ΔT)

A₂sin(ω₂(k+1)ΔT+θ₂)＝A₂sin(ω₂kΔT+θ₂)cos(ω₂ΔT)+A₂cos(ω₂kΔT+θ₂)sin(ω₂ΔT)

以上正弦噪声的状态转移方程包含了同样幅值、相位和频率的余弦函数，不妨将以上余弦函数加入系统的状态变量，则预选噪声的状态转移方程为：

A₁cos(ω₁(k+1)ΔT+θ₁)＝A₁cos(ω₁kΔT+θ₁)cos(ω₁ΔT)-A₁sin(ω₁kΔT+θ₁)sin(ω₁ΔT)

A₂cos(ω₂(k+1)ΔT+θ₂)＝A₂cos(ω₂kΔT+θ₂)cos(ω₂ΔT)-A₂sin(ω₂kΔT+θ₂)sin(ω₂ΔT)

根据以上加权二次型回归的状态预测模型，关节力矩输出状态信息的一步预测为：

综合以上信息，最终将A₁sin(ω₁(k+1)ΔT+θ₁)，A₂sin(ω₂(k+1)ΔT+θ₂)，A₁cos(ω₁(k+1)ΔT+θ₁)，A₂cos(ω₂(k+1)ΔT+θ₂)和τ_out作为系统的状态变量，则系统的转移方程为：

由于系统状态可能受到外界随机干扰，并在存在一定的建模误差，其引入的状态不确定性可使用高斯白噪声进行近似替代，式中

力矩传感器的输出由机械传动噪声、电磁干扰噪声、真实力矩输出值以及测量系统本身的误差组成，受AD转换精度影响，测量系统自身存在一些不确定性因素，其造成的误差同样也可使用高斯白噪声v进行替代，则观测方程为：

其中

C＝[1,0,1,0,1,0]

卡尔曼滤波融合更新，从上述建立的状态转移和观测方程表示为如下形式：

可知，该系统为高斯线性系统，而对于带有高斯白噪声的线性系统，可以证明卡尔曼滤波是一种最优的贝叶斯估计的实现方式。卡尔曼滤波是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法，其满足估计的均方误差最小。并且由于其便于计算机编程实现，并能够对采集的数据进行实时处理和更新，是目前应用最为广泛的滤波方法，在通信、导航、制导与控制等多领域获得了广泛的应用。

考虑建模误差以及测量精度对状态转移方程和观测方程影响，使用高斯白噪声对这部分不确定性误差进行近似，上式中w和v分别过程噪声和测量噪声。则过程噪声协方差矩阵Q和观测噪声协方差矩阵R为如下形式：

卡尔曼滤波算法的具体实施步骤如下

(1)系统初始化

初始化系统的初始状态X、协方差矩阵P、以及过程噪声Q和测量噪声R。注意协方差矩阵和过程噪声测量噪声矩阵不要设置成零矩阵，否则将会影响矩阵的秩，使得卡尔曼滤波器出现发散的现象。

(2)状态一步预测

更新长度为L的历史力矩测量数据τ_k-L,τ_k-L+1,…,τ_k，通过上述加权二次线性回归算法计算拟合曲线的参数和构建系统转移矩阵A和控制矩阵B，实现系统状态变量的一步预测。

(3)协方差一步预测

系统状态变量误差协方差矩阵∑随着状态变量的转移进行同步更新，并且考虑转移方程自身的高斯白噪声对状态变量的影响，系统协方差矩阵更新公式如下：

(4)计算卡尔曼增益

最小化系统变量误差的协方差矩阵∑，对系统误差关于卡尔曼增益K进行求导，并令其导数为0，可得如下的卡尔曼增益计算公式：

利用当前状态变量自身协方差矩阵∑和观测方程的协方差矩阵R，计算增益矩阵K，用于融合预测值和当前测量值，实现系统状态的更新。

(5)系统状态更新

利用当前的力矩测量值τ_k+1和步骤(4)计算的卡尔曼增益K，对系统状态变量进行数据融合和更新，公式如下

(6)协方差更新

根据步骤(4)计算的卡尔曼增益和系统状态变量的协方差一步预测值进行协方差矩阵的更新，至此k+1时刻单次迭代的卡尔曼滤波步骤完成。k+2时刻，返回步骤(2)，继续进行卡尔曼滤波的循环迭代过程，直至滤波过程结束。算法实施框图如图8所示。

设计算法对比实验，令机械臂分别在自由空间和存在外力干扰的工况下进行运动，如下图所示前5s工作在自由空间状态下，后7秒工作在外力干扰工况下。采集机器人关节末端力矩传感器的原始数据，采样频率为1kHz，采集时间12s，分别应用传统的低通滤波器和本发明中提出的滤波去噪算法对原始测量数据进行滤波和估计。

表1滤波算法参数列表

使用以下评价标准对滤波效果进行对比分析，RMS代表滤波后的力矩值和测量力矩值之间的误差大小，反映了滤波去噪后的信号与原始测量信号的趋势贴合程度；T_delay为滤波后信号与原始测量信号互相关函数取最大值时的时间间隔，代表由滤波造成的时间延时，计算公式如下所示：

表2滤波算法指标参数对比

由表2可知，本文所述算法的滤波精度明显优于传统的低通滤波方法，并且其所导致的时间延时也明显小于低通滤波算法，由此说明了本文提出的滤波去噪算法的准确性和有效性。原始力矩测量信号和滤波之后的信号对比图如图9所示。

由图9可知，传统的低通滤波器和本文所述算法都能够有效滤除力矩信号中的高频噪声，但是传统低通滤波器造成了相位的延时，严重影响控制系统力矩环的控制精度和稳定裕度，甚至可能造成系统发散。

图10为滤波估计得到的机械干扰噪声和电磁干扰噪声波形，噪声分布符合实际情况，说明本文提出的算法能够有效地将建模的噪声信息分离出来，从另一方面也印证了算法的准确性和有效性。

采用了该发明的机器人关节力矩信号采集系统和方法，其依据噪声特性对其进行数学建模，通过对噪声的估计从而将其力矩信号中消除，这种基于噪声模型的去噪滤波方法极大提升了滤波效率，明显提升了力矩测量信号的准确性和快速性。本文中的方法设计自适应振荡器对工频干扰噪声和谐波传动噪声进行建模，研究基于加权二次型回归的力矩状态预测算法，提升力矩输出状态预测的准确性与快速性，并以此建立系统的状态转移方程和观测方程，利用卡尔曼滤波器对转移方程和测量方程中存在的不确定性噪声予以滤除，并将测量力矩数据与预测数据进行融合，从而实现对关节力矩输出、机械干扰以及电磁干扰噪声的最优无偏估计。

在此说明书中，本发明已参照其特定的实施例作了描述。但是，很显然仍可以作出各种修改和变换而不背离本发明的精神和范围。因此，说明书和附图应被认为是说明性的而非限制性的。

Claims (8)

1.一种机器人关节力矩信号采集系统，其特征在于，该系统包括：

力矩传感器，设置于机器人关节输出末端，用于测量关节的输出力矩，产生力矩传感器测量信号；

下位机控制器，连接所述的力矩传感器，用于获取所述的力矩传感器测量信号；

通信板卡，包括中断控制器，连接所述的下位机控制器，用以控制所述的下位机控制器执行信号采集中断函数以获取所述的力矩传感器测量信号，并从所述的下位机控制器获取所述的力矩传感器测量信号；

上位机，根据从所述的通信板卡获取的力矩测量信息产生力矩输出值。

2.根据权利要求1所述的机器人关节力矩信号采集系统，其特征在于，所述的上位机包括卡尔曼滤波器，用以消除所述的力矩测量信息中的噪声。

3.根据权利要求2所述的机器人关节力矩信号采集系统，其特征在于，所述的力矩传感器测量信号τ_sensor如下式所示：

τ_sensor＝τ_out+n_me+n_el+n_sys。

其中，τ_out为真实的关节输出力矩、n_me为机械传动噪声、n_el为电磁干扰噪声，为n_sys系统随机噪声，

所述的卡尔曼滤波器具体包括：

机械噪音模型振荡器，用以消除所述的机械传动噪声n_me；

电磁噪音模型振荡器，用以消除所述的电磁干扰噪声n_el；

力矩拟合预测模型，用以消除所述的系统随机噪声n_sys。

4.根据权利要求3所述的机器人关节力矩信号采集系统，其特征在于，所述的消除机械传动噪声n_me和消除电磁干扰噪声n_el具体为：

对机械传动噪声n_me和电磁干扰噪声n_el进行数学建模，获取系统固有干扰噪声的幅值、相位和频率，对所述的力矩传感器测量信号τ_sensor中的机械传动噪声n_me和电磁干扰噪声n_el成分进行分离。

5.根据权利要求4所述的机器人关节力矩信号采集系统，其特征在于，对机械传动噪声n_me和电磁干扰噪声n_el进行数学建模，获取系统固有干扰噪声的幅值、相位和频率，具体为：

频率为ω₁,ω₂的正弦噪声在k时刻的状态转移方程：

A₁sin(ω₁(k+1)ΔT+θ₁)＝A₁sin(ω₁kΔT+θ₁)cos(ω₁ΔT)+A₁cos(ω₁kΔT+θ₁)sin(ω₁ΔT)

A₂sin(ω₂(k+1)ΔT+θ₂)＝A₂sin(ω₂kΔT+θ₂)cos(ω₂ΔT)+A₂cos(ω₂kΔT+θ₂)sin(ω₂ΔT)

正弦噪声的状态转移方程包含了同样幅值、相位和频率的余弦函数。

6.根据权利要求3所述的机器人关节力矩信号采集系统，其特征在于，所述的消除系统随机噪声n_sys具体为：

力矩拟合预测模型基于加权二次型回归力矩状态预测算法对力矩状态进行修正，消除所述的系统随机噪声n_sys。

7.根据权利要求6所述的机器人关节力矩信号采集系统，其特征在于，所述的加权二次型回归力矩状态预测算法包括以下步骤：

(1)系统初始化，初始化系统的初始状态X、协方差矩阵P、以及过程噪声Q和测量噪声R；

(2)状态一步预测，更新长度为L的历史力矩测量数据τ_k-L,τ_k-L+1,…,τ_k，通过加权二次线性回归算法计算拟合曲线的参数和构建系统转移矩阵A和控制矩阵B，实现系统状态变量的一步预测，如下式所示：

(3)协方差一步预测，系统状态变量误差协方差矩阵∑随着状态变量的转移进行同步更新，根据转移方程自身的高斯白噪声对状态变量的影响，系统协方差矩阵更新公式如下：

(4)计算卡尔曼增益，最小化系统变量误差的协方差矩阵∑，对系统误差关于卡尔曼增益K进行求导，并令其导数为0，可得如下的卡尔曼增益计算公式：

并利用当前状态变量自身协方差矩阵∑和观测方程的协方差矩阵R，计算增益矩阵K，用于融合预测值和当前测量值，实现系统状态的更新；

(5)系统状态更新，利用当前的力矩测量值τ_k+1和所述的步骤(4)计算的卡尔曼增益K，对系统状态变量进行数据融合和更新，公式如下：

(6)协方差更新，

根据所述的步骤(4)计算的卡尔曼增益和系统状态变量的协方差一步预测值进行协方差矩阵的更新，至此k+1时刻单次迭代的卡尔曼滤波步骤完成，k+2时刻，返回步骤(2)，继续进行卡尔曼滤波的循环迭代过程，直至滤波过程结束。

8.一种利用权利要求1至7中任一项所述的机器人关节力矩信号采集系统实现的机器人关节力矩信号采集方法。