机器人关节力矩传感器数据滤波算法实现

在实验中,我们经常遇到噪声或其他因素影响导致力矩传感器读取的数据存在较大的高频数据波动,这些数据是我们所不期望的,因此,我们需要进行机器人关节力矩传感器的数据滤波。

机器人关节

机器人关节

机器人关节

低通滤波器是一种滤波器,它通过频率低于所选截止频率的信号,衰减频率高于截止频率的信号,滤波器的确切频率响应取决于滤波器的设计。

滤波器设计者通常使用低通形式作为原型滤波器。也就是说,一个具有统一带宽和阻抗的滤波器。通过缩放到所需的带宽和阻抗,并转换为所需的带型(即低通、高通、带通或带阻),从原型中获得所需的滤波器。

例如:对于如下的数据进行滤波

很明显,上图中存在较大的噪声干扰,因此,需要采用滤波算法对高频噪声进行滤波。

平均滤波算法:

算法的语法如下:

y = medfilt1(x)
y = medfilt1(x,n)
y = medfilt1(x,n,blksz,dim)
y = medfilt1(x,n,[],dim)
y = medfilt1(___,nanflag,padding)

具体函数如下:

function y = medfilt1(x,varargin)

narginchk(1,6);

if nargin > 1
    [varargin{:}] = convertStringsToChars(varargin{:});
end

% ensure X is valid
validateattributes(x, ...
  {'single','double'},{'real','nonsparse'}, mfilename, 'X', 1);

% get filter size
n = 3;
if ~isempty(varargin) && ~isempty(varargin{1}) && isnumeric(varargin{1})
  n = varargin{1};
  validateattributes(n, {'numeric'}, ...
    {'scalar','integer','nonnegative'},mfilename,'N',2);
end

% get dimension argument
dim = [];
if numel(varargin)>2 && isnumeric(varargin{3})
  dim = varargin{3};
  validateattributes(dim,{'numeric'},{'integer','scalar','positive'});
end

% process options
[padding, varargin] = getmutexclopt({'zeropad','truncate'},'zeropad',varargin);
[missing, varargin] = getmutexclopt({'includenan','omitnan'},'includenan',varargin);
chkunusedopt(varargin);

% compute central moving median with specified options
y = mvmedian(x,n,dim,'central',missing,padding);

下面我们需要进行生成具有高频噪声的信号:

例子如下:

fs = 100;     %频率
t = 0:1/fs:1;  %时间
x = sin(3*pi*t*3)+0.35*sin(2*pi*t*30);% 产生的信号

y = medfilt1(x,10); %10为阶数

plot(t,x,t,y)
legend('Original','Filtered')
legend('boxoff')

5阶的结果如下:

很明显,滤波的光滑性与阶数有关。

20阶的滤波结果如下:

但过大了则与原数据相差太大,因此,根据实际情况具体来进行合适的滤波!

当然,如果考虑到频域范围内的话,我们需要进行傅里叶变换:

fx = abs(fft(x-mean(x)))/(N/2);  % 傅里叶变换

其中,x为我们所需要进行滤波的数据。

fft - 快速傅里叶变换
    此 MATLAB 函数 用快速傅里叶变换 (fft) 算法计算 X 的离散傅里叶变换 (DFT)。 如果 X  是向量,则 fft(X) 返回该向量的傅里叶变换。 如果 X 是矩阵,则 fft(X) 将 X 的各列视为向量,并返回每列的傅里叶变换。 如果 X 是一个多维数组,则 fft(X) 将沿大小不等于 1 的第一个数组维度的值视为向量,并返回每个向量的傅里叶变换。

中通滤波方法如下:

y = medfilt1(x)
y = medfilt1(x,n)

y = medfilt1(x,n,blksz,dim)

 y = medfilt1(x,n,[],dim)

 y = medfilt1(___,nanflag,padding)

维纳滤波如下:

xcorr - 互相关
    此 MATLAB 函数 返回两个离散时间序列的互相关。互相关测量向量 x 和移位(滞后)副本向量 y 的之间的相似性,形式为滞后的函数。如果 x 和 y 的长度不同,函数会在较短向量的末尾添加零,使其长度与另一个向量相同。

语法如下:

r = xcorr(x,y)  %返回两个离散时间序列的互相关。互相关测量向量 x 和移位(滞后)副本向量 y的之间的相似性,形式为滞后的函数。如果x和 y的长度不同,函数会在较短向量的末尾添加零,使其长度与另一个向量相同。

 r = xcorr(x)  %返回 x 的自相关序列。如果 x 是矩阵,则 r 也是矩阵,其中包含 x 的所有列组合的自相关和互相关序列。

 r = xcorr(___,maxlag) % 将上述任一语法中的滞后范围限制为从 -maxlag 到 maxlag
 r = xcorr(___,scaleopt)% 还为互相关或自相关指定归一化选项。除 'none'(默认值)以外的任何选项都要求 x 和 y 具有相同的长度。

[r,lags] = xcorr(___)  %还返回用于计算相关性的滞后。

Butterworth滤波方法如下:

语法如下:

[b,a] = butter(n,Wn)

[b,a] = butter(n,Wn,ftype)

[z,p,k] = butter(___)
 [A,B,C,D] = butter(___)

 [___] = butter(___,'s')

其中,该函数具体在Matlab中表现为:

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

再次总结下:

低通滤波器函数用于需要传输较低频率的信号和阻挡较高频率的信号的地方。所需的低频波段(从直流开始)称为通波段,高频波段称为阻带。通带和阻带相遇的频率称为截止频率。通带有时也被称为滤波器的带宽。其他滤波方法以后继续总结。