代码改变世界

电子陀螺仪的角度获取测试

2013-06-07 16:11  钱吉  阅读(14063)  评论(0编辑  收藏  举报

导航中经常用到电子陀螺。为了测试陀螺仪在静止下的零漂输出和运动情况下的输出角度值,做了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