1、IMU加速度到位移的變換方法 分類： 算法學(xué)習(xí) 2014-06-16 10:16 52人閱讀 評論(0) 收藏 舉報         在IMU中，利用加速度計和陀螺儀組成的6DOF系統(tǒng)進(jìn)行姿態(tài)求解方法還是很多的，主要是利用陀螺儀進(jìn)行積分，然后使用加速度計進(jìn)行約束，最后進(jìn)行求解！類似的方法很多，效果還是很不錯的，SOH大牛提出了兩種的方法，還提供了源代碼進(jìn)行測試，效果還是不錯的。        但是如何利用加速度獲取位移就比較麻煩，本人嘗試了很多方法， 效果還是很不好呀！這里先轉(zhuǎn)載一篇文章的方法，原文的地址為：最近做

2、有關(guān)加速度的數(shù)據(jù)處理，需要把加速度積分成位移，網(wǎng)上找了找相關(guān)資料，發(fā)現(xiàn)做這個并不多，把最近做的總結(jié)一下吧！   積分操作主要有兩種方法：時域積分和頻域積分，積分中常見的問題就是會產(chǎn)生二次趨勢。關(guān)于積分的方法，在國外一個論壇上有人提出了如下說法，供參考。Double integration of raw acceleration data is a pretty poor estimate for displacement. The reason is that at each integration, you are compounding t

3、he noise in the data.If you are dead set on working in the time-domain, the best results come from the followingsteps.1. Remove the mean from your sample (now have zero-mean sample)2. Integrate once to get velocity using some rule (trapezoidal, etc.)3. Remove the mean from the velocity4. I

4、ntegrate again to get displacement.5. Remove the mean. Note, if you plot this, you will see drift over time.6. To eliminate (some to most) of the drift (trend), use a least squares fit (high degree depending on data) to determine polynomial coefficients.7. Remove the least squares polynomial fu

5、nction from your data.A much better way to get displacement from acceleration data is to work in the frequency domain. To do this, follow these steps.1. Remove the mean from the accel. data2. Take the Fourier transform (FFT) of the accel. data.3. Convert the transformed accel. data to disp

6、lacement data by dividing each element by -omega2, where omega is the frequency band.4. Now take the inverse FFT to get back to the time-domain and scale your result.This will give you a much better estimate of displacement.    說到底就是頻域積分要比時域積分效果更好，實際測試也發(fā)現(xiàn)如此。原因可能是時域積分時積分一次就要去

7、趨勢，去趨勢就會降低信號的能量，所以最后得到的結(jié)果常常比真實幅值要小。下面做一些測試，對一個正弦信號的二次微分做兩次積分，正弦頻率為50Hz，采樣頻率1000Hz，恢復(fù)效果如下時域積分頻域積分可見恢復(fù)信號都很好（對于50Hz是這樣的效果）。   分析兩種方法的頻率特性曲線如下時域積分頻域積分可以看到頻域積分得到信號更好，時域積分隨著信號頻率的升高恢復(fù)的正弦幅值會降低。    對于包含兩個正弦波的信號，頻域積分正常恢復(fù)信號，時域積分恢復(fù)的高頻信息有誤差；對于有噪聲的正弦信號，噪聲會使積分結(jié)果產(chǎn)生大的趨勢項（不是簡單的二次趨勢）

8、，如下圖對此可以用濾波的方法將大的趨勢項去掉。  測試的代碼如下% 測試積分對正弦信號的作用clcclearclose all% 原始正弦信號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*

9、f1*t); % 速度% acc1 = -(2*pi*f1).2.*sin(2*pi*f1*t); % 加速度% dis = dis + dis1;% vel = vel + vel1;% acc = acc + acc1;% 結(jié)：頻域積分正常恢復(fù)信號，時域積分恢復(fù)加入的高頻信息有誤差% 加噪聲測試acc = acc + (2*pi*f).2*0.2*randn(size(acc);% 結(jié)：噪聲會使積分結(jié)果產(chǎn)生大的趨勢項figureax(1) = subplot(311);plot(t, dis), title('位移')ax(2) = subplot(312);plot(t,

10、 vel), title('速度')ax(3) = subplot(313);plot(t, acc), title('加速度')linkaxes(ax, 'x');% 由加速度信號積分算位移disint, velint = IntFcn(acc, t, ts, 2);axes(ax(2);   hold onplot(t, velint, 'r'), legend('原始信號', '恢復(fù)信號')axes(ax(1);   hold onp

11、lot(t, disint, 'r'), legend('原始信號', '恢復(fù)信號')% 測試積分算子的頻率特性n = 30;amp = zeros(n, 1);f = 5:30 40:10:480;figurefor i = 1:length(f)    fi = f(i);    acc = -(2*pi*fi).2.*sin(2*pi*fi*t); % 加速度    disint, velint = IntFcn(acc,

12、 t, ts, 2); % 積分算位移    amp(i) = sqrt(sum(disint.2)/sqrt(sum(dis.2);    plot(t, disint)    drawnow%     pauseendclosefigureplot(f, amp)title('位移積分的頻率特性曲線')xlabel('f')ylabel('單位正弦波的積分位移幅值')

13、0;    以上代碼中使用IntFcn函數(shù)實現(xiàn)積分，它是封裝之后的函數(shù)，可以實現(xiàn)時域積分和頻域積分，其代碼如下% 積分操作由加速度求位移，可選時域積分和頻域積分function disint, velint = IntFcn(acc, t, ts, flag)if flag = 1    % 時域積分    disint, velint = IntFcn_Time(t, acc);    velenergy = sqrt(sum(vel

14、int.2);    velint = detrend(velint);    velreenergy = sqrt(sum(velint.2);    velint = velint/velreenergy*velenergy;      disenergy = sqrt(sum(disint.2);    disint = detrend(disint); 

15、60;  disreenergy = sqrt(sum(disint.2);    disint = disint/disreenergy*disenergy; % 此操作是為了彌補去趨勢時能量的損失    % 去除位移中的二次項    p = polyfit(t, disint, 2);    disint = disint - polyval(p, t);else    %

16、 頻域積分    velint =  iomega(acc, ts, 3, 2);    velint = detrend(velint);    disint =  iomega(acc, ts, 3, 1);    % 去除位移中的二次項    p = polyfit(t, disint, 2);    disin

17、t = disint - polyval(p, t);endend 其中時域積分的子函數(shù)如下% 時域內(nèi)梯形積分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 頻域積分的子函數(shù)如下（此代碼是一個老外編的，在頻域內(nèi)實現(xiàn)積分和微分操作）function dataout =  iom

18、ega(datain, dt, datain_type, dataout_type)%   IOMEGA is a MATLAB script for converting displacement, velocity, or%   acceleration time-series to either displacement, velocity, or%   acceleration times-series. The script takes an array of waveform data%

19、60;  (datain), transforms into the frequency-domain in order to more easily%   convert into desired output form, and then converts back into the time%   domain resulting in output (dataout) that is converted into the desired%   form.%  

20、; Variables:%   -%   datain       =   input waveform data of type datain_type%   dataout      =   output waveform data of type dataout_type%  

21、60;dt           =   time increment (units of seconds per sample)%                    1 - Displacement%   datain_typ

22、e  =   2 - Velocity%                    3 - Acceleration%                   

23、0;1 - Displacement%   dataout_type =   2 - Velocity%                    3 - Acceleration%   Make sure that datain_type and dataout_type are either 1, 2 or

24、3if (datain_type < 1 | datain_type > 3)    error('Value for datain_type must be a 1, 2 or 3');elseif (dataout_type < 1 | dataout_type > 3)    error('Value for dataout_type must be a 1, 2 or 3');end%   Determine Number

25、 of points (next power of 2), frequency increment%   and Nyquist frequencyN = 2nextpow2(max(size(datain);df = 1/(N*dt);Nyq = 1/(2*dt);%   Save frequency arrayiomega_array = 1i*2*pi*(-Nyq : df : Nyq-df);iomega_exp = dataout_type - datain_type;%   Pad datai

26、n array with zeros (if needed)size1 = size(datain,1);size2 = size(datain,2);if (N-size1 = 0 && N-size2 = 0)    if size1 > size2        datain = vertcat(datain,zeros(N-size1,1);    else        datain = horzcat(datain,zeros(1,N-size2);    endend%   Transform datain into frequency domain via FFT and shift output (A)%   so that zero-frequency amplitude is in the middle of the array%