電子陀螺儀的角度獲取測試

導航中經常用到電子陀螺。為了測試陀螺儀在靜止下的零漂輸出和運動情況下的輸出角度值，做了2個實驗。用的陀螺儀型號是：L3G4200DTR

是一個三軸輸出的MEMS電子陀螺。

1 零漂測試

讓陀螺儀靜止不動，運行5分鍾，記錄串口輸出數據，並解析出三個軸的角速度值。在matlab里面進行仿真。

function main
clc;
clear;

dataFile = input('put file: ','s');
nCount = 0;
fileHandle = fopen(dataFile,'r');
while ~feof(fileHandle)
    string = fgetl(fileHandle);
    nCount = nCount + 1;    
end

nCount = nCount-2;
frewind(fileHandle);
i = 1;
while ~feof(fileHandle)
    string = str2num(fgetl(fileHandle));
    zData(i) = string(3);
    i = i + 1;
    if i > nCount
        break;
    end
end
fclose(fileHandle);

figure(1);
plot(zData,'b-');
hold on;
meanZData = mean(zData);
title('原始Z軸輸出數據');
xlabel('ms');
ylabel('dps(degree per second)');
text(3500, 1.2, ['mean=',num2str(meanZData)]);

figure(2);
newData = runge_kutta_intergration(zData);
plot(newData);
xlabel('ms');
ylabel('degree');
title('積分輸出的角度');

function newData = runge_kutta_intergration(rawData)
samplePeriod = 0.1;%%100ms
size = length(rawData);
i=1;
while (i <= size)
    %%%%%簡單的積分累加獲取角度
    
    if(i == 1)
        Data(i) = rawData(i);
    else
        Data(i) = Data(i-1) + rawData(i)*samplePeriod;
    end
    
    %%%%runge-kutta積分累加獲取角度
    %{
    if i<4
        Data(i) = rawData(i);
    else
        Data(i) = Data(i-1)+1/6*(rawData(i-3)+2*rawData(i-2)+2*rawData(i-1)+rawData(i))*samplePeriod;
    end
    %}
    i = i+1;
end
newData = Data;

輸出結果：

 

結果表明，靜止情況下的陀螺儀也存在很大的累加誤差(本身的零漂和溫度引起的漂移)，5分鍾后居然產生了160度的角度誤差。

 

2 運動情況下的輸出測試

實驗中，我把儀器逆時針旋轉90度，然后還原到原來的位置，再順時針旋轉90度，然后回到原來的位置。同樣記錄在文件中，解析文件，對解析出來的數據進行測試。因為旋轉理論上只會導致Z軸的角速率變化，所以只分析這個軸上的角度輸出。

function main
clc;
clear;

dataFile = input('put file: ','s');
nCount = 0;
fileHandle = fopen(dataFile,'r');
while ~feof(fileHandle)
    string = fgetl(fileHandle);
    nCount = nCount + 1;    
end

nCount = nCount-2;
frewind(fileHandle);
i = 1;
while ~feof(fileHandle)
    string = str2num(fgetl(fileHandle));
    zData(i) = string(3);
    i = i + 1;
    if i > nCount
        break;
    end
end
fclose(fileHandle);

figure(1);
plot(zData,'b-');
hold on;
zData = zData-mean(zData);%為了抑制零漂，采用減均值的方式
plot(zData,'r-');
title('原始Z軸輸出數據');
legend('未減均值','減均值');
xlabel('ms');
ylabel('dps(degree per second)');
hold off;


figure(2);
newData = runge_kutta_intergration(zData);
plot(newData);
title('積分輸出的角度');
xlabel('ms');
ylabel('degree');

function newData = runge_kutta_intergration(rawData)
samplePeriod = 0.1;%%100ms
size = length(rawData);
i=1;
while (i <= size)
    %%%%%簡單的積分累加獲取角度
    
    if(i == 1)
        Data(i) = rawData(i);
    else
        Data(i) = Data(i-1) + rawData(i)*samplePeriod;
    end
    
    %%%%runge-kutta積分累加獲取角度
    %{
    if i<4
        Data(i) = rawData(i);
    else
        Data(i) = Data(i-1)+1/6*(rawData(i-3)+2*rawData(i-2)+2*rawData(i-1)+rawData(i))*samplePeriod;
    end
    %}
    i = i+1;
end
newData = Data;

輸出結果：

從figure2里面，可以看到，出現了2個峰值，分別對應這逆時針和順時針的旋轉角度變化。雖然總體上的趨勢是對的，但是仍然存在誤差。

總結：

MEMS的電子陀螺存在零漂和溫度誤差，動態較零的方式很多，可以采取簡單的采集靜止條件下的一段數據取平均值來實現，也可以采取復雜一點的建模的方式，對其實施誤差補償。這是一個很復雜的過程，需要慢慢研究。

 

Refrence:

http://tom.pycke.be/mav/70/gyroscope-to-roll-pitch-and-yaw

http://www.starlino.com/imu_guide.html