Как измерить относительное положение с помощью 2 акселерометров

Question

В настоящее время я пытаюсь отследить относительное положение, используя 2 акселерометра (в частности, adxl337) и приставку Arduino. Я использую Matlab для обработки значений акселерометра.

Я прочитал бесчисленное количество похожих постов и онлайн-ресурсов, но мне еще предстоит найти способ добиться «хороших» результатов. Я знаю, что то, что я пытаюсь сделать, не даст наиболее точных результатов, но результаты, которые я сейчас получаю, кажутся гораздо менее точными, чем результаты, достигнутые другими, использующими аналогичный метод для отслеживания относительного положения. В следующем коде я просто использую значения одного из акселерометров (другой акселерометр будет использоваться для отслеживания крена, тангажа и рыскания) и держу акселерометр на ровной поверхности (без движения по z, хотя даже после компенсируя силу тяжести, акселерометр все еще имеет ускорение по оси z, когда я по какой-то причине перемещаю его по столу). Я также реализовал два метода интеграции: один - это метод интеграции RK4 (Runge-Kutta) (который закомментирован), а другой был найден здесь , который я сейчас использую. Значения ускорения интегрируются дважды, чтобы найти положение.

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 дюймов; кий-карта лежала на столе, а акселерометр был сверху; положительный х акселерометра) был направо к карте-метке, положительный у был к вершине кью-карты, а положительный z был к потолку), и я получил этот график , когда я нанес на график ypos против xpos. Хотя график выглядит нормально с точки зрения характера движения, он не в том направлении, потому что я двигал акселерометр в положительном х и положительном у. Кроме того, значения x и y слишком велики, поскольку я, конечно, не сдвинул их на 0,7 метра в x и 0,25 метра в y. Это две основные проблемы, с которыми я сталкиваюсь в данный момент (результаты в неправильном направлении и неправильной величине). Любая помощь приветствуется.