如何使用2个加速度计测量相对位置

Question

我目前正在尝试使用2个加速度计（特别是adxl337）和一个Arduino来跟踪相对位置。我正在使用matlab处理加速度计值。

我已经阅读了无数类似的帖子和在线资源，但还没有找到实现“良好”结果的方法。我知道我要执行的操作不会产生最准确的结果，但是我目前获得的结果似乎比其他人使用类似方法跟踪相对位置所获得的结果要精确得多。在下面的代码中，我只是使用其中一个加速度计的值（另一个加速度计将用于跟踪横摇，俯仰和偏航），并保持加速度计在表面上平整（z内没有移动，即使经过补偿重力时，由于某种原因，当加速度计在桌子上移动时，加速度计仍具有z的加速度。我还实现了两种集成方法，一种是rk4（Runge-Kutta）方法（已注释掉），另一种是here，这是我当前正在使用的方法。将加速度值积分两次以找到位置。

clear ard
ard = arduino;
rawMin = 0;
rawMax = 3.3;
scale = 3;

acceleration = 0;
xPos = zeros(1,1000);
yPos = zeros(1,1000);
zPos = zeros(1,1000);

xVelo = zeros(1,1000);
yVelo = zeros(1,1000);
zVelo = zeros(1,1000);

xAccel = zeros(1,1000);
yAccel = zeros(1,1000);
zAccel = zeros(1,1000);

accelMatrix = zeros(1,1000);
velocityMatrix = zeros(1,1000);
positionMatrix = zeros(1,1000);
dt = 36.0832/1000; %Determined previously 

stationaryCheck = 0;
zeroACount = 0;

tot_toc = 0;

ax = 0;
ay = 0;
az = 0;

xSum = 0;
ySum = 0;
zSum = 0;

for jj = 1:500
   %Calibration loop
   %Keep Accelerometer Stationary During this time
   xC = readVoltage(ard,'A1');
   yC = readVoltage(ard,'A2');
   zC = readVoltage(ard,'A3');
   xSum = xSum + map(xC, rawMin, rawMax, -scale, scale);
   ySum = ySum + map(yC, rawMin, rawMax, -scale, scale);
   zSum = zSum + map(zC, rawMin, rawMax, -scale, scale);
end

xBias = xSum/500;
yBias = ySum/500;
zBias = zSum/500;

if xBias < 0
    xBias = abs(xBias);
else
    xBias = - xBias;
end

if yBias < 0
    yBias = abs(yBias);
else
    yBias = -yBias;
end

If zBias < 0
   zBias = abs(zBias);
else
   zBias = -zBias
end



for count = 1:1000
tic;
x1 = readVoltage(ard,'A1');
y1 = readVoltage(ard,'A2');
z1 = readVoltage(ard,'A3');
GxP = map(x1, rawMin, rawMax, -scale, scale) + xBias;
GyP = map(y1, rawMin, rawMax, -scale, scale) + yBias;
GzP = map(z1, rawMin, rawMax, -scale, scale) + zBias;


threshold = 0.09;  %A change in acceleration greater than this means the accelerometer is moving (0.09)

if (-threshold > GxP| GxP > threshold | GyP < -threshold | GyP > threshold | GzP < - threshold| GzP > threshold)
%At this point, it is determined the object is moving
    zeroACount = 0;
    ax = GxP * 9.8;
    ay = GyP * 9.8;
    az = GzP * 9.8;
    xAccel(count) = ax;
    yAccel(count) = ay;
    zAccel(count) = az;

end

if (-threshold < GxP & GxP < threshold & -threshold < GyP & GyP < threshold & -threshold < GzP & GzP < threshold)
    xAccel(count) = 0;
    yAccel(count) = 0; 
    zAccel(count) = 0;
    zeroACount = zeroACount + 1;
end 


%tot_toc = DisplayEstimatedTimeOfLoop( tot_toc+toc, i, 1000-1 );
count

if zeroACount >= 5
    stationaryCheck = 1;
end

if count == 1
    %Assuming starting from stationary
    xVelo(count) = mbedIntA(0,0,dt,xAccel(count));
    yVelo(count) = mbedIntA(0,0,dt,yAccel(count));
    zVelo(count) = mbedIntA(0,0,dt,zAccel(count));

    xPos(count) = mbedIntV(0,0,dt,xVelo(count));
    yPos(count) = mbedIntV(0,0,dt,yVelo(count));
    zPos(count) = mbedIntV(0,0,dt,zVelo(count));
end

if (count > 1)
    if (stationaryCheck)
        xVelo(count) = 0;
        yVelo(count) = 0;
        zVelo(count) = 0;
    else
    xVelo(count) = mbedIntA(xAccel(count-1),xVelo(count - 1),dt,xAccel(count));
    yVelo(count) = mbedIntA(yAccel(count-1),yVelo(count - 1),dt,yAccel(count));
    zVelo(count) = mbedIntA(zAccel(count-1),zVelo(count - 1),dt,zAccel(count));
    end

    xPos(count) = mbedIntV(xVelo(count - 1),xPos(count - 1),dt,xVelo(count));
    yPos(count) = mbedIntV(yVelo(count - 1),yPos(count - 1),dt,yVelo(count));
    zPos(count) = mbedIntV(zVelo(count - 1),zPos(count - 1),dt,zVelo(count));

%This was previously how I was integrating
    %Sets final velocity after one time period dt to be used as initial
    %velocity for next dt period
%     xVelo(count + 1) = rk4IntA(xVelo(count), xAccel(count), dt);
%     xPos(count + 1) = rk4IntV(xPos(count),rk4IntA(xVelo(count), xAccel(count), dt), dt);
%     
%     yVelo(count + 1) = rk4IntA(yVelo(count), yAccel(count), dt);
%     yPos(count + 1) = rk4IntV(yPos(count),rk4IntA(yVelo(count), yAccel(count), dt), dt);
%     
%     zVelo(count + 1) = rk4IntA(zVelo(count), zAccel(count), dt);
%     zPos(count + 1) = rk4IntV(zPos(count),rk4IntA(zVelo(count), zAccel(count), dt), dt);
%     

%     velocityMatrix(count + 1) = rk4IntA(velocityMatrix(count), accelMatrix(count), dt);
%     positionMatrix(count + 1) = rk4IntV(positionMatrix(count),rk4IntA(velocityMatrix(count), accelMatrix(count), dt), dt) ;

end

stationaryCheck = 0;

end



function mapValue = map(x, inMin, inMax, outMin, outMax)
mapValue = -((x - inMin) * (outMax - outMin)/(inMax - inMin) + outMin);
end 

function velocity2 = mbedIntA(prevA, prevV, dt, a)
velocity2 = prevV + (prevA + ((a - prevA)/2))*dt;
end

function position2 = mbedIntV(prevV, prevP, dt , v)
position2 = prevP + (prevV + ((v - prevV)/2))*dt;
end

function velocityOutput = rk4IntA(v, a, dt)
velocityX1 = v;
velocityX2 = v + a*dt*0.5;
velocityX3 = velocityX2 + a*dt*0.5;
velocityX4 = velocityX3 + a*dt;
velocityOutput = (velocityX1+2.0*velocityX2+2.0*velocityX3+velocityX4)/6;
end

function positionOutput = rk4IntV(p, v, dt)
positionX1 = p;
positionX2 = p + v*dt*0.5;
positionX3 = positionX2 + v*dt*0.5;
positionX4 = positionX3 + v*dt;
positionOutput = (positionX1+2.0*positionX2+2.0*positionX3+positionX4)/6;
end


function [ tot_toc, estimated_time_hours  ] = DisplayEstimatedTimeOfLoop( tot_toc, curr_ix, tot_iter )
    if curr_ix == tot_iter
        disp(['Total elapsed time (HH:MM:SS): ' datestr(tot_toc/(24*60*60),'HH:MM:SS')]);
    else
        avg_toc = tot_toc/curr_ix;
        estimated_time_hours = (avg_toc*(tot_iter-curr_ix))/(24*60*60);
        disp(['Estimated time to finish (HH:MM:SS): ' datestr(estimated_time_hours, 'HH:MM:SS') ' ' num2str(round(curr_ix*100/tot_iter)) '%']);
    end
end

我在将加速度计从提示卡的左下角移动时运行了此代码（3英寸乘5英寸提示卡；提示卡躺在桌子上，并且加速度计在其上方；加速度计的正x朝向提示卡的右侧，正y指向提示卡的顶部，正z朝向天花板），当我绘制ypos vs xpos时得到this plot。尽管就运动方式而言，该图看起来还可以，但是由于我将加速度计沿正x和正y移动，因此方向错误。另外，x和y值太大，因为我当然没有将其移动到x的0.7米和y的0.25米。这是我目前遇到的两个主要问题（结果方向错误，幅度错误）。任何帮助表示赞赏。