如何实现对机器人接触力的数据滤波
下面举一些例子，实现对机器人接触力的数据滤波！
首先是导入数据：
clcclear 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; % 画的频谱的横坐标到nhzhz = nhz*n; % 画的频谱的横坐标的数组个数%% 绘制原始信号时频图figuresubplot(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.18fn',a);fprintf('b = %6.18fn',b);x1 = filter(b,a,x);figuresubplot(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{:}); endendendfunction varargout = butterimpl(n,wn,varargin)inputargs = cell(1,length(varargin));if nargin > 2 [inputargs{:}] = convertstringstochars(varargin{:});else inputargs = varargin;endvalidateattributes(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 rulesn1 = double(n(1));coder.internal.errorif(n1 > 500,'signal:butter:invalidrange')% cast to enforce precision ruleswn = double(wn);% step 1: get analog, pre-warped frequenciesfs = 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 filterif length(wn) == 1 % step 3a: convert to low-pass prototype estimate wn1 = u(1); bw = []; %#ok % 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); endelse % 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); endend% step 4: use bilinear transformation to find discrete equivalent:if ~analog [ad,bd,cd,dd] = bilinear(ad,bd,cd,dd,fs);endif nargout == 4 % outputs are in state space form varargout{1} = ad; % a varargout{2} = bd; % b varargout{3} = cd; % c varargout{4} = dd; % delse 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; endendendfunction [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'); endelse 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'); endend% note: codegen complains when z set to both real and complex values aboveif ~any(imag(z)) z = real(z);endend可以得到滤波后的接触力数据：
为了更详细的进行原始数据与滤波后的数据进行对比，接下来以几种不同形式的滤波方式进行对比。
原始接触力数据：
移动平均滤波
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); %得到混合信号的自相关矩阵 endendrxy=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 signalx1(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
利用这个工具，可以将设计的滤波器保存成一个函数。
AD7534: 微处理器兼容型14位CMOS DAC
带屏智能音箱的市场发展将迎来重大变革
天拓四方工厂级物联平台帮助企业降低经营成本，提高核心竞争力
华为云 x 一粒云，让企业云盘不一样
电感式传感器产品特性探讨
如何实现对机器人接触力的数据滤波
超1700亿资金进入隔膜赛道
如何用import导入一个包
三星S9/S9+评测 努力维系着机皇该有的一切
人工智能技术助力航司精准把握复工复产出行需求
鼠标按键的“灵动”用法
格力在2017年年报中首次提出要发展集成电路
德仪LMG5200 GaN半桥功率级
python类接口和抽象超类分析
CPU是如何调度任务的？
西部数据： “敏捷开发”对汽车制造商至关重要
Parker直线驱动器OSP系列七种驱动方式
微软确认20H1/Version 2004正式名称为May 2020功能更新 计划在未来几周时间内继续打磨
基于MSP430F149单片机和步进电机实现三自由度模拟实验台的设计
送“敬业福”了！ 支付宝有破绽， 这方法轻易集齐
首先是导入数据：
clcclear 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; % 画的频谱的横坐标到nhzhz = nhz*n; % 画的频谱的横坐标的数组个数%% 绘制原始信号时频图figuresubplot(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.18fn',a);fprintf('b = %6.18fn',b);x1 = filter(b,a,x);figuresubplot(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{:}); endendendfunction varargout = butterimpl(n,wn,varargin)inputargs = cell(1,length(varargin));if nargin > 2 [inputargs{:}] = convertstringstochars(varargin{:});else inputargs = varargin;endvalidateattributes(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 rulesn1 = double(n(1));coder.internal.errorif(n1 > 500,'signal:butter:invalidrange')% cast to enforce precision ruleswn = double(wn);% step 1: get analog, pre-warped frequenciesfs = 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 filterif length(wn) == 1 % step 3a: convert to low-pass prototype estimate wn1 = u(1); bw = []; %#ok % 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); endelse % 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); endend% step 4: use bilinear transformation to find discrete equivalent:if ~analog [ad,bd,cd,dd] = bilinear(ad,bd,cd,dd,fs);endif nargout == 4 % outputs are in state space form varargout{1} = ad; % a varargout{2} = bd; % b varargout{3} = cd; % c varargout{4} = dd; % delse 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; endendendfunction [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'); endelse 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'); endend% note: codegen complains when z set to both real and complex values aboveif ~any(imag(z)) z = real(z);endend可以得到滤波后的接触力数据：
为了更详细的进行原始数据与滤波后的数据进行对比，接下来以几种不同形式的滤波方式进行对比。
原始接触力数据：
移动平均滤波
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); %得到混合信号的自相关矩阵 endendrxy=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 signalx1(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
利用这个工具，可以将设计的滤波器保存成一个函数。
AD7534: 微处理器兼容型14位CMOS DAC
带屏智能音箱的市场发展将迎来重大变革
天拓四方工厂级物联平台帮助企业降低经营成本，提高核心竞争力
华为云 x 一粒云，让企业云盘不一样
电感式传感器产品特性探讨
如何实现对机器人接触力的数据滤波
超1700亿资金进入隔膜赛道
如何用import导入一个包
三星S9/S9+评测 努力维系着机皇该有的一切
人工智能技术助力航司精准把握复工复产出行需求
鼠标按键的“灵动”用法
格力在2017年年报中首次提出要发展集成电路
德仪LMG5200 GaN半桥功率级
python类接口和抽象超类分析
CPU是如何调度任务的？
西部数据： “敏捷开发”对汽车制造商至关重要
Parker直线驱动器OSP系列七种驱动方式
微软确认20H1/Version 2004正式名称为May 2020功能更新 计划在未来几周时间内继续打磨
基于MSP430F149单片机和步进电机实现三自由度模拟实验台的设计
送“敬业福”了！ 支付宝有破绽， 这方法轻易集齐