IMU加速度到位移的变换方法.docx
《IMU加速度到位移的变换方法.docx》由会员分享，可在线阅读，更多相关《IMU加速度到位移的变换方法.docx（9页珍藏版）》请在冰豆网上搜索。
IMU加速度到位移的变换方法
IMU加速度到位移的变换方法
分类：
算法学习2014-06-1610:
1652人阅读评论（0）收藏举报
在IMU中，利用加速度计和陀螺仪组成的6DOF系统进行姿态求解方法还是很多的，主要是利用陀螺仪进行积分，然后使用加速度计进行约束，最后进行求解！
类似的方法很多，效果还是很不错的，SOH大牛提出了两种的方法，还提供了源代码进行测试，效果还是不错的。
但是如何利用加速度获取位移就比较麻烦，本人尝试了很多方法，效果还是很不好呀！
！
！
！
！
这里先转载一篇文章的方法，原文的地址为：
最近做有关加速度的数据处理，需要把加速度积分成位移，网上找了找相关资料，发现做这个并不多，把最近做的总结一下吧！
积分操作主要有两种方法：
时域积分和频域积分，积分中常见的问题就是会产生二次趋势。
关于积分的方法，在国外一个论坛上有人提出了如下说法，供参考。
Doubleintegrationofrawacceleration dataisaprettypoorestimatefordisplacement.Thereasonisthatateachintegration,youare compoundingthenoiseinthedata.
Ifyouaredeadsetonworkinginthe time-domain,thebestresultscomefromthefollowingsteps.
1. Removethemeanfromyoursample（nowhavezero-meansample）
2.Integrateoncetogetvelocityusingsomerule（trapezoidal,etc.）
3.Removethemeanfromthevelocity
4.Integrateagaintogetdisplacement.
5.Removethemean.Note,ifyouplotthis,youwillseedriftovertime.
6. Toeliminate（sometomost）ofthedrift（trend）,usealeastsquaresfit（highdegreedependingondata）todeterminepolynomialcoefficients.
7.Removetheleastsquarespolynomialfunctionfromyourdata.
Amuchbetterwaytogetdisplacement fromaccelerationdataistoworkinthefrequencydomain.Todothis,followthesesteps...
1.Removethemeanfromtheaccel.data
2.TaketheFouriertransform（FFT）oftheaccel.data.
3.Convertthetransformed accel.datatodisplacementdatabydividingeachelementby-omega^2,whereomegaisthefrequency band.
4.NowtaketheinverseFFTtogetbacktothetime-domainandscaleyourresult.
Thiswillgiveyouamuchbetterestimateofdisplacement.
说到底就是频域积分要比时域积分效果更好，实际测试也发现如此。
原因可能是时域积分时积分一次就要去趋势，去趋势就会降低信号的能量，所以最后得到的结果常常比真实幅值要小。
下面做一些测试，对一个正弦信号的二次微分做两次积分，正弦频率为50Hz，采样频率1000Hz，恢复效果如下
时域积分
频域积分
可见恢复信号都很好（对于50Hz是这样的效果）。
分析两种方法的频率特性曲线如下
时域积分
频域积分
可以看到频域积分得到信号更好，时域积分随着信号频率的升高恢复的正弦幅值会降低。
对于包含两个正弦波的信号，频域积分正常恢复信号，时域积分恢复的高频信息有误差；对于有噪声的正弦信号，噪声会使积分结果产生大的趋势项（不是简单的二次趋势），如下图
对此可以用滤波的方法将大的趋势项去掉。
测试的代码如下
%测试积分对正弦信号的作用
clc
clear
closeall
%%原始正弦信号
ts=0.001;
fs=1/ts;
t=0:
ts:
1000*ts;
f=50;
dis=sin（2*pi*f*t）;%位移
vel=2*pi*f.*cos（2*pi*f*t）;%速度
acc=-（2*pi*f）.^2.*sin（2*pi*f*t）;%加速度
%多个正弦波的测试
%f1=400;
%dis1=sin（2*pi*f1*t）;%位移
%vel1=2*pi*f1.*cos（2*pi*f1*t）;%速度
%acc1=-（2*pi*f1）.^2.*sin（2*pi*f1*t）;%加速度
%dis=dis+dis1;
%vel=vel+vel1;
%acc=acc+acc1;
%结：
频域积分正常恢复信号，时域积分恢复加入的高频信息有误差
%加噪声测试
acc=acc+（2*pi*f）.^2*0.2*randn（size（acc））;
%结：
噪声会使积分结果产生大的趋势项
figure
ax
（1）=subplot（311）;
plot（t,dis）,title（'位移'）
ax
（2）=subplot（312）;
plot（t,vel）,title（'速度'）
ax（3）=subplot（313）;
plot（t,acc）,title（'加速度'）
linkaxes（ax,'x'）;
%由加速度信号积分算位移
[disint,velint]=IntFcn（acc,t,ts,2）;
axes（ax
（2））; holdon
plot（t,velint,'r'）,legend（{'原始信号','恢复信号'}）
axes（ax
（1））; holdon
plot（t,disint,'r'）,legend（{'原始信号','恢复信号'}）
%%测试积分算子的频率特性
n=30;
amp=zeros（n,1）;
f=[5:
3040:
10:
480];
figure
fori=1:
length（f）
fi=f（i）;
acc=-（2*pi*fi）.^2.*sin（2*pi*fi*t）;%加速度
[disint,velint]=IntFcn（acc,t,ts,2）;%积分算位移
amp（i）=sqrt（sum（disint.^2））/sqrt（sum（dis.^2））;
plot（t,disint）
drawnow
% pause
end
close
figure
plot（f,amp）
title（'位移积分的频率特性曲线'）
xlabel（'f'）
ylabel（'单位正弦波的积分位移幅值'）
以上代码中使用IntFcn函数实现积分，它是封装之后的函数，可以实现时域积分和频域积分，其代码如下
%积分操作由加速度求位移，可选时域积分和频域积分
function[disint,velint]=IntFcn（acc,t,ts,flag）
ifflag==1
%时域积分
[disint,velint]=IntFcn_Time（t,acc）;
velenergy=sqrt（sum（velint.^2））;
velint=detrend（velint）;
velreenergy=sqrt（sum（velint.^2））;
velint=velint/velreenergy*velenergy;
disenergy=sqrt（sum（disint.^2））;
disint=detrend（disint）;
disreenergy=sqrt（sum（disint.^2））;
disint=disint/disreenergy*disenergy;%此操作是为了弥补去趋势时能量的损失
%去除位移中的二次项
p=polyfit（t,disint,2）;
disint=disint-polyval（p,t）;
else
%频域积分
velint= iomega（acc,ts,3,2）;
velint=detrend（velint）;
disint= iomega（acc,ts,3,1）;
%去除位移中的二次项
p=polyfit（t,disint,2）;
disint=disint-polyval（p,t）;
end
end
其中时域积分的子函数如下
%时域内梯形积分
function[xn,vn]=IntFcn_Time（t,an）
vn=cumtrapz（t,an）;
vn=vn-repmat（mean（vn）,size（vn,1）,1）;
xn=cumtrapz（t,vn）;
xn=xn-repmat（mean（xn）,size（xn,1）,1）;
end
频域积分的子函数如下（此代码是一个老外编的，在频域内实现积分和微分操作）
functiondataout= iomega（datain,dt,datain_type,dataout_type）
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% IOMEGAisaMATLABscriptforconvertingdisplacement,velocity,or
% accelerationtime-seriestoeitherdisplacement,velocity,or
% accelerationtimes-series.Thescripttakesanarrayofwaveformdata
% （datain）,transformsintothefrequency-domaininordertomoreeasily
% convertintodesiredoutputform,andthenconvertsbackintothetime
% domainresultinginoutput（dataout）thatisconvertedintothedesired
% form.
%
% Variables:
% ----------
%
% datain = inputwaveformdataoftypedatain_type
%
% dataout = outputwaveformdataoftypedataout_type
%
% dt = timeincrement（unitsofsecondspersample）
%
% 1-Displacement
% datain_type = 2-Velocity
% 3-Acceleration
%
% 1-Displacement
% dataout_type= 2-Velocity
% 3-Acceleration
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Makesurethatdatain_typeanddataout_typeareeither1,2or3
if（datain_type<1||datain_type>3）
error（'Valuefordatain_typemustbea1,2or3'）;
elseif（dataout_type<1||dataout_type>3）
error（'Valuefordataout_typemustbea1,2or3'）;
end
% DetermineNumberofpoints（nextpowerof2）,frequencyincrement
% andNyquistfrequency
N=2^nextpow2（max（size（datain）））;
df=1/（N*dt）;
Nyq=1/（2*dt）;
% Savefrequencyarray
iomega_array=1i*2*pi*（-Nyq:
df:
Nyq-df）;
iomega_exp=dataout_type-datain_type;
% Paddatainarraywithzeros（ifneeded）
size1=size（datain,1）;
size2=size（datain,2）;
if（N-size1~=0&&N-size2~=0）
ifsize1>size2
datain=vertcat（datain,zeros（N-size1,1））;
else
datain=horzcat（datain,zeros（1,N-size2））;
end
end
% TransformdatainintofrequencydomainviaFFTandshiftoutput（A）
% sothatzero-frequencyamplitudeisinthemiddleofthearray
% （insteadofthebeginning）
A=fft（datain）;
A=fftshift（A）;
% Convertdatainoftypedatain_typetotypedataout_type
forj=1:
N
ifiomega_array（j）~=0
A（j）=A（j）*（iomega_array（j）^iomega_exp）;
else
A（j）=complex（0.0,0.0）;
end
end
% Shiftnewfrequency-amplitudearraybacktoMATLABformatand
% transformbackintothetimedomainviatheinverseFFT.
A=ifftshift（A）;
datain=ifft（A）;
% Removezerosthatwereaddedtodataininordertopadtonext
% biggerstpowerof2andreturndataout.
ifsize1>size2
dataout=real（datain（1:
size1,size2））;
else
dataout=real（datain（size1,1:
size2））;
end
return
升级会员 