力控机器人接触力滤波与估计

力控机器人本身关节具有力传感器，可为什么还需要接触力滤波和估计呢？这是不是有些多余？显然是不是的，本篇博文总结下力控机器人接触力滤波与估计的一些原因：

1. 环境噪声和不确定性：在力控机器人与环境进行物理交互时，存在来自环境的噪声和不确定性，这些因素可能导致力传感器测量值的不稳定性和波动。接触力滤波可以帮助去除这些噪声和不确定性，得到更加准确和可靠的接触力信息。

2. 控制稳定性：力控机器人依赖于实时感知和反馈控制系统，以实现对接触力的精确控制。如果接触力信号存在快速变化或噪声，将导致控制系统的不稳定性和振荡。通过滤波和估计接触力，可以平滑力信号，提高控制系统的稳定性和性能。

3. 精确的力控操作：某些应用场景中，力控机器人需要实现对接触力的精确调节和控制，例如装配、力敏操作等。通过滤波和估计接触力，可以提供准确的力信息，帮助机器人进行精确的力控操作，使机器人能够适应不同的环境和任务需求。

4. 安全性和保护：力控机器人在与人类或脆弱物体进行交互时，需要保证接触力在安全范围内，并避免对人体或物体造成损害。通过接触力滤波和估计，可以监测和控制接触力的大小和变化，确保机器人在安全和可控的范围内操作。

综上所述，接触力滤波和估计对于力控机器人的正常运行、控制稳定性、精确操作和安全性至关重要。可以提供可靠的接触力信息，帮助机器人感知和调节与环境的物理交互。

有多种方法可用于接触力滤波和估计，以下是一些常见的方法：

1. 低通滤波器：通过设计和应用低通滤波器，可以去除接触力信号中的高频噪声，从而平滑信号并减少不稳定性。常用的低通滤波器包括滑动平均滤波器、指数加权移动平均滤波器等。

2. 卡尔曼滤波器：卡尔曼滤波器是一种基于状态估计的滤波器，可以通过融合传感器测量和系统动态模型，对接触力信号进行滤波和估计。卡尔曼滤波器可以提供更准确的估计结果，并适应信号的动态变化。

3. 尺度变换法：该方法通过对接触力信号进行尺度变换，将其映射到期望范围内。尺度变换可以根据已知的力传感器特性进行，使得接触力估计更加准确和可靠。

4. 机器学习方法：机器学习技术如神经网络、支持向量机等可以应用于接触力滤波和估计。通过对大量接触力数据进行训练和建模，可以建立接触力模型并实现对未知力信号的估计。

5. 物理模型和辨识方法：利用物理模型和系统辨识技术，可以建立机械臂与环境之间接触力的动态模型，并通过与实际测量值进行比较和校正，实现接触力的估计和滤波。

这些方法可以单独或结合使用，具体的选择取决于应用场景、系统要求和可用的传感器数据。在实际应用中，通常需要根据具体情况进行试验和调整，以获得最佳的接触力滤波和估计效果。

下面举一些简单的例子，对接触力数据进行滤波处理：

function filtered_force = frequency_filter(force, cutoff_frequency, sampling_frequency)
    % 计算滤波器参数
    normalized_cutoff = cutoff_frequency / (sampling_frequency / 2);
    [b, a] = butter(4, normalized_cutoff, 'low'); % 4阶低通巴特沃斯滤波器

    % 应用滤波器
    filtered_force = filtfilt(b, a, force);
end

% 生成模拟的接触力数据
sampling_frequency = 100; % 采样频率（Hz）
duration = 5; % 数据持续时间（秒）
t = 0:1/sampling_frequency:duration;
force = sin(2*pi*2*t) + 0.5*sin(2*pi*10*t) + 0.2*sin(2*pi*30*t); % 模拟接触力数据

% 应用频率滤波器
cutoff_frequency = 15; % 截止频率（Hz）
filtered_force = frequency_filter(force, cutoff_frequency, sampling_frequency);

% 绘制原始数据和滤波后的数据
figure;
subplot(2,1,1);
plot(t, force);
title('原始接触力数据');
xlabel('时间（秒）');
ylabel('力');
subplot(2,1,2);
plot(t, filtered_force);
title('滤波后的接触力数据');
xlabel('时间（秒）');
ylabel('力');

仿真结果如下：

具有单轴力传感器的单关节机械臂接触力估计：

接触力估计需要考虑多个因素进行补偿，以提高估计的准确性。以下是一些常见的补偿因素：

1. 重力补偿：机械臂在接触过程中会受到重力的影响，因此需要对测量到的力进行重力补偿。通过减去机械臂当前位置的重力分量，可以获得实际的接触力。

2. 惯性补偿：机械臂在运动过程中可能会产生惯性力，这些力会影响接触力的测量。通过考虑机械臂的加速度和速度信息，可以进行惯性补偿，以消除不必要的测量误差。

3. 摩擦补偿：在接触过程中，摩擦力会干扰力的测量。通过估计和补偿摩擦力，可以得到更准确的接触力信息。

4. 噪声补偿：传感器可能会受到环境噪声的影响，这些噪声会对接触力的测量结果产生干扰。使用滤波技术或噪声模型，可以对传感器数据进行滤波或去噪，以减小噪声对接触力估计的影响。

以上是一些常见的补偿因素，具体的实现方法会根据机械臂和传感器的特性而有所不同。在接触力估计的实际应用中，还需要考虑到系统的动态特性、传感器的校准和精度等因素，以获得更精确和可靠的接触力估计结果。

数据分析：对采集到的数据进行分析，以确定需要进行的补偿方案。以下是一些分析方法和指标：

• • 重力补偿：分析机械臂在不同位置和姿态下的重力分量，计算出重力对接触力的影响。可以通过测量机械臂的姿态角度和质量分布等信息来估计重力分量。

  • 惯性补偿：分析机械臂在不同加速度和速度下的惯性力，计算出惯性力对接触力的影响。可以通过测量机械臂的加速度和速度，结合机械臂的惯性参数来估计惯性力。

  • 摩擦补偿：分析机械臂在接触过程中的摩擦力特性，计算出摩擦力对接触力的影响。可以通过测量机械臂的运动阻力和摩擦系数等信息来估计摩擦力。

  • 噪声补偿：分析传感器输出的噪声特性，确定噪声对接触力估计的影响程度。可以通过统计分析、滤波技术或噪声模型来进行噪声补偿。

在只有角编码器和单轴力矩传感器的情况下，可以通过以下方式进行接触力估计的补偿：

1. 重力补偿：

   • 角编码器测量角度：使用角编码器测量机械臂关节的角度。根据角度和机械臂的几何结构，可以估计出机械臂在不同位置和姿态下的重力分量。
   • 机械臂质量估计：通过对机械臂的质量进行测量或估计，结合机械臂几何结构，可以估计出机械臂的质量分布。根据质量分布和重力加速度的方向，可以计算出不同位置和姿态下的重力对接触力的影响。

2. 惯性补偿：

   • 角编码器测量角速度：利用角编码器测量机械臂关节的角速度。通过积分角速度，可以获得机械臂的角度变化。结合机械臂的惯性参数，可以估计机械臂在不同加速度和速度下的惯性力。

3. 摩擦补偿：

   • 力矩传感器测量力矩：使用单轴力矩传感器测量机械臂关节的力矩。根据关节力矩和摩擦系数，可以估计机械臂在接触过程中的摩擦力特性。

4. 噪声补偿：

   • 滤波技术：对传感器输出的接触力数据进行滤波处理，以减小噪声的影响。可以使用数字滤波器，例如低通滤波器或卡尔曼滤波器等，来抑制高频噪声成分。
   • 校准和校正：对传感器进行校准和校正，以降低系统误差和噪声。可以使用校准算法和技术，例如零偏校准和灵敏度校准，来提高传感器的准确性。

下面举一些例子，实现对机器人接触力的数据滤波！

首先是导入数据：

clc
clear all;
close all;
X = xlsread('E:\程序\test\~\六维力数据.csv');%导入数据
A=X(:,1);B=X(:,2);C=X(:,3);D=X(:,4);E=X(:,5);F=X(:,6);%提取力数据

%% 滤波
x=A;
N=1347; %时域点数
fs = 100;  % 重采样频率
T = 1/fs;  % 周期
n = 5;  % 1Hz频率被分成n段
% N = fs*n;  % 因为1Hz频率被分成了n段，所以频谱的x轴数组有fs*n个数
f = (0: N-1)*fs/N;  % 将fs个频率细分成fs*n个（即原来是[0, 1, 2, …, fs]，现在是[0, 1/N, 2/N, …, (N-1)*fs/N]）
t = (0: N-1)*T;  % 信号所持续的时长（N个周期）
nHz = 10;  % 画的频谱的横坐标到nHz
Hz = nHz*n;  % 画的频谱的横坐标的数组个数

%% 绘制原始信号时频图
figure
subplot(211),plot(x,'k'),title('原始信号时域'),xlabel('采样点/n'),ylabel('力或力矩');  % 绘制原始信号时域

fx = abs(fft(x-mean(x)))/(N/2);  % 傅里叶变换
subplot(212),plot(f(1:Hz), fx(1:Hz),'k'),title('原始信号频谱图'),xlabel('频率/Hz'),ylabel('幅值');  % 绘制原始信号频域

可以得到如下结果：

通过傅里叶变换可以得到接触力信号频域上的内容。

进行低通滤波处理：

%% 低通滤波
fc = 20;
Wc = 2*fc/fs; 
[b,a] = butter(2,Wc,'low');  % 四阶的巴特沃斯低通滤波,保留频率低于fc的振动
fprintf('a = %6.18f\n',a);
fprintf('b = %6.18f\n',b);
x1 = filter(b,a,x);

figure
subplot(211),plot(x1,'b'),title('低通滤波信号时域图'),xlabel('采样点/n'),ylabel('力或力矩');
fx = abs(fft(x1-mean(x1)))/(N/2);  % 傅里叶变换
subplot(212),plot(f(1:Hz), fx(1:Hz),'b'),title('低通滤波信号频谱图'),xlabel('频率/Hz'),ylabel('幅值');  % 绘制原始信号频域

Butterworth digital and analog filter design：

function varargout = butter(n, Wn, varargin)
narginchk(2,4);
if coder.target('MATLAB')
   [varargout{1:nargout}] = butterImpl(n,Wn,varargin{:});
else
   allConst = coder.internal.isConst(n) && coder.internal.isConst(Wn);
   for ii = 1:length(varargin)
      allConst = allConst && coder.internal.isConst(varargin{ii});
   end
   if allConst && coder.internal.isCompiled
      [varargout{1:nargout}] = coder.const(@feval,'butter',n,Wn,varargin{:});
   else
      [varargout{1:nargout}] = butterImpl(n,Wn,varargin{:});
   end
end
end

function varargout = butterImpl(n,Wn,varargin)
inputArgs = cell(1,length(varargin));
if nargin > 2
   [inputArgs{:}] = convertStringsToChars(varargin{:});
else
   inputArgs = varargin;
end
validateattributes(n,{'numeric'},{'scalar','real','integer','positive'},'butter','N');
validateattributes(Wn,{'numeric'},{'vector','real','finite','nonempty'},'butter','Wn');

[btype,analog,~,msgobj] = iirchk(Wn,inputArgs{:});
if ~isempty(msgobj)
   coder.internal.error(msgobj.Identifier,msgobj.Arguments{:});
end
% Cast to enforce precision rules
n1 = double(n(1));
coder.internal.errorIf(n1 > 500,'signal:butter:InvalidRange')
% Cast to enforce precision rules
Wn = double(Wn);
% step 1: get analog, pre-warped frequencies
fs = 2;
if ~analog
   u = 2*fs*tan(pi*Wn/fs);
else
   u = Wn;
end

% step 2: Get N-th order Butterworth analog lowpass prototype
[zs,ps,ks] = buttap(n1);
% Transform to state-space
[a,b,c,d] = zp2ss(zs,ps,ks);
% step 3: Transform to the desired filter
if length(Wn) == 1
   % step 3a: convert to low-pass prototype estimate
   Wn1 = u(1);
   Bw = []; %#ok<NASGU>
   % step 3b: Transform to lowpass or high pass filter of desired cutoff
   % frequency
   if btype == 1           % Lowpass
      [ad,bd,cd,dd] = lp2lp(a,b,c,d,Wn1);
   else % btype == 3       % Highpass
      [ad,bd,cd,dd] = lp2hp(a,b,c,d,Wn1);
   end
else % length(Wn) is 2
   % step 3a: convert to low-pass prototype estimate
   Bw = u(2) - u(1);      % center frequency
   Wn1 = sqrt(u(1)*u(2));
   % step 3b: Transform to bandpass or bandstop filter of desired center
   % frequency and bandwidth
   if btype == 2           % Bandpass
      [ad,bd,cd,dd] = lp2bp(a,b,c,d,Wn1,Bw);
   else % btype == 4       % Bandstop
      [ad,bd,cd,dd] = lp2bs(a,b,c,d,Wn1,Bw);
   end
end
% step 4: Use Bilinear transformation to find discrete equivalent:
if ~analog
   [ad,bd,cd,dd] = bilinear(ad,bd,cd,dd,fs);
end

if nargout == 4 % Outputs are in state space form
   varargout{1} = ad;          % A
   varargout{2} = bd;          % B
   varargout{3} = cd;          % C
   varargout{4} = dd;          % D
else
   p = eig(ad);
   [z,k] = buttzeros(btype,n1,Wn1,analog,p+0i);
   if nargout == 3         % Transform to zero-pole-gain form
      varargout{1} = z;
      varargout{2} = p;
      varargout{3} = k;
   else
      den = real(poly(p));
      num = [zeros(1,length(p)-length(z),'like',den)  k*real(poly(z))];
      varargout{1} = num;
      varargout{2} = den;
   end
end
end


function [z,k] = buttzeros(btype,n,Wn,analog,p)
% This internal function computes the zeros and gain of the ZPK
% representation. Wn is scalar (sqrt(Wn(1)*Wn(2)) for bandpass/stop).
if analog
   % for lowpass and bandpass, don't include zeros at +Inf or -Inf
   switch btype
      case 1  % lowpass: H(0)=1
         z = zeros(0,1,'like',p);
         k = Wn^n;  % prod(-p) = Wn^n
      case 2  % bandpass: H(1i*Wn) = 1
         z = zeros(n,1,'like',p);
         k = real(prod(1i*Wn-p)/(1i*Wn)^n);
      case 3  % highpass: H(Inf) = 1
         z = zeros(n,1,'like',p);
         k = 1;
      case 4  % bandstop: H(0) = 1
         z = 1i*Wn*((-1).^(0:2*n-1)');
         k = 1;  % prod(p) = prod(z) = Wn^(2n)
      otherwise
         coder.internal.error('signal:iirchk:BadFilterType','high','stop','low','bandpass');
   end
else
   Wn = 2*atan2(Wn,4);
   switch btype
      case 1  % lowpass: H(1)=1
         z = -ones(n,1,'like',p);
         k = real(prod(1-p))/2^n;
      case 2  % bandpass: H(z) = 1 for z=exp(1i*sqrt(Wn(1)*Wn(2)))
         z = [ones(n,1,'like',p); -ones(n,1,'like',p)];
         zWn = exp(1i*Wn);
         k = real(prod(zWn-p)/prod(zWn-z));
      case 3  % highpass: H(-1) = 1
         z = ones(n,1,'like',p);
         k = real(prod(1+p))/2^n;
      case 4  % bandstop: H(1) = 1
         z = exp(1i*Wn*( (-1).^(0:2*n-1)' ));
         k = real(prod(1-p)/prod(1-z));
      otherwise
         coder.internal.error('signal:iirchk:BadFilterType','high','stop','low','bandpass');
   end
end
% Note: codegen complains when z set to both real and complex values above
if ~any(imag(z))
   z = real(z);
end
end

可以得到滤波后的接触力数据：

为了更详细的进行原始数据与滤波后的数据进行对比，接下来以几种不同形式的滤波方式进行对比。

原始接触力数据：

移动平均滤波
b  =  [1 1 1 1 1 1]/6;
x11 = filter(b,1,x);

很明显，去除噪声的效果较为突出！

接下来采用中值滤波：

中值滤波的效果相比于移动平均滤波有改善。

接下来进行维纳滤波：

Rxx=xcorr(x, x);              %得到混合信号的自相关函数
M=100;                                                             %维纳滤波器阶数
for i=1:M                                                           
    for j=1:M
        rxx(i,j)=Rxx(abs(j-i)+N);   %得到混合信号的自相关矩阵
    end
end
Rxy=xcorr(x,x1);       %(此处x1为中值信号滤波后效果，原信号不存在)得到混合信号和原信号的互相关函数
for i=1:M
    rxy(i)=Rxy(i+N-1);   %得到混合信号和原信号的互相关向量
end                                                                  
h = inv(rxx)*rxy';                                               %得到所要涉及的wiener滤波器系数
x1=filter(h,1, x);               %将输入信号通过维纳滤波器

但维纳滤波的效果没有前面两个滤波算法的效果好，需要进一步整定参数。

下面进行自适应滤波：

k=100;                                                  %时域抽头LMS算法滤波器阶数
u=0.001;                                             %步长因子

%设置初值
x1=zeros(1,N);                                  %output signal
x1(1:k)=x(1:k);                 %将输入信号SignalAddNoise的前k个值作为输出yn_1的前k个值
w=zeros(1,k);                                        %设置抽头加权初值
e=zeros(1,N);                                        %误差信号

%用LMS算法迭代滤波
for i=(k+1):N
        XN=x((i-k+1):(i));
        XN=XN';
        x1(i)=w*XN';
        e(i)=x11(i)-x1(i);%不存在原信号，此处换为平均滤波后的时域波形
        w=w+2*u*e(i)*XN;
end

四阶的巴特沃斯低通滤波：

Wc=2*3/fs;                                          %截止频率 3Hz
[b,a]=butter(4,Wc,'low');  % 四阶的巴特沃斯低通滤波
x1=filter(b,a,x);

二阶的巴特沃斯带通滤波：

Wc1=2*1/fs;                                          %下截止频率 1Hz
Wc2=2*6/fs;                                          %上截止频率 6Hz
[b,a]=butter(2,[Wc1, Wc2],'bandpass');  % 二阶的巴特沃斯带通滤波
x1=filter(b,a,x);

需要生成滤波器时,可以使用matlab中自带的工具。filterDesigner

利用这个工具，可以将设计的滤波器保存成一个函数。

参考链接页面：

https://ww2.mathworks.cn/help/dsp/ref/filterdesigner.html?searchHighlight=filterDesigner&s_tid=srchtitle_filterDesigner_1

参考文献：

【1】https://blog.csdn.net/qq_36758914/article/details/110367804?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522168619407216800215093423%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fall.%2522%257D&request_id=168619407216800215093423&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~first_rank_ecpm_v1~rank_v31_ecpm-1-110367804-null-null.142^v88^control_2,239^v2^insert_chatgpt&utm_term=%28%E6%AD%A4%E5%A4%84x1%E4%B8%BA%E4%B8%AD%E5%80%BC%E4%BF%A1%E5%8F%B7%E6%BB%A4%E6%B3%A2%E5%90%8E%E6%95%88%E6%9E%9C%EF%BC%8C%E5%8E%9F%E4%BF%A1%E5%8F%B7%E4%B8%8D%E5%AD%98%E5%9C%A8%29%E5%BE%97%E5%88%B0%E6%B7%B7%E5%90%88%E4%BF%A1%E5%8F%B7%E5%92%8C%E5%8E%9F%E4%BF%A1%E5%8F%B7%E7%9A%84%E4%BA%92%E7%9B%B8%E5%85%B3%E5%87%BD%E6%95%B0&spm=1018.2226.3001.4187

【2】https://blog.csdn.net/weixin_45502929/article/details/112017409?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522168619421616800182117128%2522%252C%2522scm%2522%253A%252220140713.130102334..%2522%257D&request_id=168619421616800182117128&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~top_positive~default-1-112017409-null-null.142^v88^control_2,239^v2^insert_chatgpt&utm_term=MATLAB%E6%BB%A4%E6%B3%A2&spm=1018.2226.3001.4187