如何實現(xiàn)對機(jī)器人接觸力的數(shù)據(jù)濾波

下面舉一些例子，實現(xiàn)對機(jī)器人接觸力的數(shù)據(jù)濾波！

首先是導(dǎo)入數(shù)據(jù)：

clc
clear all;
close all;
X = xlsread('E:程序test~六維力數(shù)據(jù).csv');%導(dǎo)入數(shù)據(jù)
A=X(:,1);B=X(:,2);C=X(:,3);D=X(:,4);E=X(:,5);F=X(:,6);%提取力數(shù)據(jù)

%% 濾波
x=A;
N=1347; %時域點數(shù)
fs = 100;  % 重采樣頻率
T = 1/fs;  % 周期
n = 5;  % 1Hz頻率被分成n段
% N = fs*n;  % 因為1Hz頻率被分成了n段，所以頻譜的x軸數(shù)組有fs*n個數(shù)
f = (0: N-1)*fs/N;  % 將fs個頻率細(xì)分成fs*n個（即原來是[0, 1, 2, …, fs]，現(xiàn)在是[0, 1/N, 2/N, …, (N-1)*fs/N]）
t = (0: N-1)*T;  % 信號所持續(xù)的時長（N個周期）
nHz = 10;  % 畫的頻譜的橫坐標(biāo)到nHz
Hz = nHz*n;  % 畫的頻譜的橫坐標(biāo)的數(shù)組個數(shù)

%% 繪制原始信號時頻圖
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('幅值');  % 繪制原始信號頻域

可以得到如下結(jié)果：

通過傅里葉變換可以得到接觸力信號頻域上的內(nèi)容。

進(jìn)行低通濾波處理：

%% 低通濾波
fc = 20;
Wc = 2*fc/fs; 
[b,a] = butter(2,Wc,'low');  % 四階的巴特沃斯低通濾波,保留頻率低于fc的振動
fprintf('a = %6.18fn',a);
fprintf('b = %6.18fn',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

可以得到濾波后的接觸力數(shù)據(jù)：

為了更詳細(xì)的進(jìn)行原始數(shù)據(jù)與濾波后的數(shù)據(jù)進(jìn)行對比，接下來以幾種不同形式的濾波方式進(jìn)行對比。

原始接觸力數(shù)據(jù)：

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

很明顯，去除噪聲的效果較為突出！

接下來采用中值濾波：

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

接下來進(jìn)行維納濾波：

Rxx=xcorr(x, x);              %得到混合信號的自相關(guān)函數(shù)
M=100;                                                             %維納濾波器階數(shù)
for i=1:M                                                           
    for j=1:M
        rxx(i,j)=Rxx(abs(j-i)+N);   %得到混合信號的自相關(guān)矩陣
    end
end
Rxy=xcorr(x,x1);       %(此處x1為中值信號濾波后效果，原信號不存在)得到混合信號和原信號的互相關(guān)函數(shù)
for i=1:M
    rxy(i)=Rxy(i+N-1);   %得到混合信號和原信號的互相關(guān)向量
end                                                                  
h = inv(rxx)*rxy';                                               %得到所要涉及的wiener濾波器系數(shù)
x1=filter(h,1, x);               %將輸入信號通過維納濾波器

但維納濾波的效果沒有前面兩個濾波算法的效果好，需要進(jìn)一步整定參數(shù)。

下面進(jìn)行自適應(yīng)濾波：

k=100;                                                  %時域抽頭LMS算法濾波器階數(shù)
u=0.001;                                             %步長因子


%設(shè)置初值
x1=zeros(1,N);                                  %output signal
x1(1:k)=x(1:k);                 %將輸入信號SignalAddNoise的前k個值作為輸出yn_1的前k個值
w=zeros(1,k);                                        %設(shè)置抽頭加權(quán)初值
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

利用這個工具，可以將設(shè)計的濾波器保存成一個函數(shù)。