Coleta e processamento de dados IMU

Os dois códigos a seguire serão importante quando na leitura dos dados brutos (raw data) advindos diretamente do meu sistema IMU quando em calibração. A coleta de dados para calibração ocorre de maneira remota (bluetooth) empregando um aplicativo desenvolvido por mim utilizando o App Inventor, tarefa facilitada devido a facilitação em contruir aplicativos por meio de blocos (linguagem de alto nível). A figura abaixo mostra a primeira e única página do app. No caso, faz-se necessário conectar com o telefone, via appp, ao dispositivo ESP32. Tem que se ativar o bluetooth Em caso de sucesso a aba de conexão indica o Status na cor verde. A partir desse ponto, é só selecionar a coleta de dados que se deseja fazer e voilà.

O código embarcado no ESP32 é um pouco mais complicado, roda em tempo real por meio do freeRTOS. Ele utiliza algumas classes desenvolvidas: a do sensor inercial e a do cartão SD. Existem algumas tasks que realizam a coleta e gravação dos dados. Tudo, conforme dito anteriormente, é organizado por meio do controle por bluetooth (recepção de dados, processamento e execução de atividades desejadas). Após a realização da coleta em cada uma das posições próprias para a calibração, um arquivo do tipo '*.txt' é gerado. Esse arquivo, do jeito que foi construída a sua arquitetura, encontra-se numa situação que não passível de leitura rápido e fácil. Todos os dados estão organizados em formato uint8_t. De modo a poder facilitar a gravação dos dados no cartão SD durante as posições de calibração,

// (*) Parâmetros dos sensores inerciais

typedef struct imu_base{

float ax;

float ay;

float az;

float gx;

float gy;

float gz;

float temperature;

} IMU_base_t;

 

 

// (*) Para armazenamento de dados advindos do MPU6050

// (*) Essa estrutura possui o tamanho de 32 bytes,

// desse modo, consegue ser armazenada no buffer

// de 512 bytes 16 vezes em sequência.

typedef struct imu_to_sd{

float time;

IMU_base_t sensor;

} IMU_t;

Cada rodada de leitura de dados capta 7 tipos de dados: 3 dos acelerômetros, 3 do giros e 1 de temperatura. Foi inserido o parâmetro de tempo para que outras características futuras sejam exploradas (coisa lá para frente e que será abordado oportunamente). A coleta de dados ocorre a cada 10ms. Cada dado coletado tem a dimensão de 4 bytes (composição de float). Como são 8 tipos de dados por rodada, serão 32 bytes por leitura. De modo a facilitar a gravação em blocos no cartão SD, existe um buffer embarcado de dimensão 512 bytes. A cada coleta, faz-se o preenchimento desse buffer com 32 bytes. Quando completado o preenchimento desse buffer, envia esses dados para uma queue no freeRTOS, que faz o gerenciamento para a gravação no cartão SD. Ao término da coleta de dados, gera-se um arquivo '*.txt'.

O que se faz, então, é utilizar o código a seguir para ler os arquivos para cada situação da calibração (six-positions) e transformar os dados inintelegíveis em informações úteis (unidades de engenharia). Ao término do programa, um arquivo em formato '*.mat' é gerado. Será, então, a partir da manipulação desse arquivo novo '.mat' gerado que serão realizados os demais trabalhos de engenharia. Maiores detalhes ficam para um outro post.

clear all; close all; clc;

 

% ===================================

% Seletor de arquivo

% ===================================

[fileName, pathLocation] = uigetfile('*.txt');

if (isempty(fileName))

print("Arquivo com problema");

else

ID = fopen(fileName, 'rb');

data = fread(ID);

fclose(ID);

end

 

% ===================================

% Quantidade de blocos de 512 bytes

% ===================================

nBlocks = floor(length(data)/512);

 

% ===================================

% Alocando memória

% ===================================

time = zeros(nBlocks * 16,1);

ax = zeros(nBlocks * 16,1);

ay = zeros(nBlocks * 16,1);

az = zeros(nBlocks * 16,1);

gx = zeros(nBlocks * 16,1);

gy = zeros(nBlocks * 16,1);

gz = zeros(nBlocks * 16,1);

temp = zeros(nBlocks * 16,1);

 

% ===================================

% Ler cada bloco e processa

% ===================================

cont = 0;

for i = 1:nBlocks

for j = 1:16

% Índice inicial da mensagem

K = (i-1)*512 + (j-1)*32;

 

% Extrair os 4 bytes de cada variável

time_bytes = data(K + 1 : K + 4);

ax_bytes = data(K + 5 : K + 8);

ay_bytes = data(K + 9 : K + 12);

az_bytes = data(K + 13: K + 16);

gx_bytes = data(K + 17: K + 20);

gy_bytes = data(K + 21: K + 24);

gz_bytes = data(K + 25: K + 28);

temp_bytes = data(K + 29: K + 32);

 

% Atualiza o contador

cont = cont + 1;

 

% Converte os bytes para float (single)

time(cont) = typecast(uint8(time_bytes), 'single');

ax(cont) = typecast(uint8(ax_bytes), 'single');

ay(cont) = typecast(uint8(ay_bytes), 'single');

az(cont) = typecast(uint8(az_bytes), 'single');

gx(cont) = typecast(uint8(gx_bytes), 'single');

gy(cont) = typecast(uint8(gy_bytes), 'single');

gz(cont) = typecast(uint8(gz_bytes), 'single');

temp(cont) = typecast(uint8(temp_bytes), 'single');

end

end

 

 

% ===================================

% Salva em dados de engenharia

% ===================================

saveName = split(fileName, ".");

save([saveName{1},'.mat'], 'time', 'ax', 'ay', 'az', 'gx', 'gy', 'gz');

disp('Leitura concluída!');

clear all;

Calibração de acelerômetros numa IMU -- Parte 1

Modelo matemático para acelerômetro

Esse modelo eu peguei no livro do Noureldin (Fundamentals of Inertial Navigation, Satellite-based, Positioning ant their Integration, Springer, 2013.), presente na seção 4.11.

em que:

: é o valor medido diretamente pelo acelerômetro, ou seja, o que a gente ler do hardware;

: é o valor verdadeiro a ser empregado no desenvolvimento das equações de mecanização;

: é a matriz que condensa os fatores de escala linear;

: é a matriz que condensa os fatores de escala não-linear;

: é a matriz que representa a não hortogonalidade da tríade de acelerômetros;

: é o vetor de gravidade anômalo, ou seja, os desvios em relação ao valor teórico; e

: é o vetor que representa os erros dos ruídos dos sensores.

A seguir, as matrizes e seus termos:

e

OBS: o subscrito "m" nos termos de dizem respeito à medida realizada (measurement).

Desenvolvendo a equação acima, em seus termos (modificação aqui minha), tem-se que:

Após juntar os termos semelhantes, tem-se o seguinte resultado:

Que desenvolvidas, termo a termo, fornecem os seguintes resultados para cada um dos eixos.

1. X+

Posicionando o acelerômetro com o eixo X com a direção para cima, o vetor gravidade encontra-se na direção contrária à direção positiva do eixo X. Nessa condição, a medida de referência para a calibração deve ser +g para o eixo X. Desse modo, as equações ficam representadas da seguinte forma:

Nessa posição, com o sistema fixo e sem nenhuma parturbação, coleta-se vários dados dos sensores e tira-se a média de cada um deles (basicamente para tentar eliminar os erros de medida):

A ideia agora é guardar essas equações e tentar encontrar similares para os demais eixos e orientações possíveis.

2. X-

Após a coleta de bastante dados, tira-se a média das medidas (para basicamente eliminar o parâmetro de erro):

3. Y+

De modo similar ao que fora realizado para o eixo X, tem-se que:

Após bastante dados, tira-se a média (para basicamente eliminar o parâmetro de erro):

4. Y-

Após bastante dados, tira-se a média (para basicamente eliminar o parâmetro de erro):

5. Z+

De modo similar ao que fora realizado para os eixos X e Y, tem-se que:

Após bastante dados, tira-se a média (para basicamente eliminar o parâmetro de erro):

6. Z-

Após bastante dados, tira-se a média (para basicamente eliminar o parâmetro de erro):

Com base nesses parâmetros e médias do sistema, a ideia é tentar organizar os parâmetros em matrizes e encontrar a solução que melhor se acomoda ao sistema. Para tal a gente realizará o seguinte algoritmo.

1. Coloca o IMU fixo com o eixo X apontado para cima (X up) durante um tempo considerável (>10min, por exemplo). Salva o resultado das medidas de aceleração em um arquivo do tipo "x_up.txt".
2. Coloca o IMU fixo com o eixo X apontado para baixo (X down) durante um tempo considerável (>10min, por exemplo). Salva o resultado das medidas de aceleração em um arquivo do tipo "x_down.txt".
3. Coloca o IMU fixo com o eixo Y apontado para cima (Y up) durante um tempo considerável (>10min, por exemplo). Salva o resultado das medidas de aceleração em um arquivo do tipo "y_up.txt".
4. Coloca o IMU fixo com o eixo Y apontado para baixo (Y down) durante um tempo considerável (>10min, por exemplo). Salva o resultado das medidas de aceleração em um arquivo do tipo "y_down.txt".
5. Coloca o IMU fixo com o eixo Z apontado para cima (Z up) durante um tempo considerável (>10min, por exemplo). Salva o resultado das medidas de aceleração em um arquivo do tipo "z_up.txt".
6. Coloca o IMU fixo com o eixo Z apontado para baixo (Z down) durante um tempo considerável (>10min, por exemplo). Salva o resultado das medidas de aceleração em um arquivo do tipo "z_down.txt".
7. Carrega todos os dados num software de mais alto nível (Matlab, Python) ou mesmo em C/C++ (fica a seu critério) para separação das médias dos sistemas. A organização dos parâmetros e a separação de todos os termos encontram-de disponíveis no código abaixo:

clear all; close all; clc;

 

% ===========================================================

% Adiciona o caminho de modo a possibilitar pegar os dados

% coletados em determinado dia. Todos os 6 arquivos '*.txt'

% encontram-se dentro da pasta "dados_acelerometros"

% ===========================================================

addpath("dados_acelerometros");

 

% ===========================================================

% A primeira coisa que precisamos é coletar os parâmetros

% da calibração. No caso, a gente faz a parte de tratativa

% dos dados do sistema.

% ===========================================================

%%%%%%%%%%%%%%

% X UP %

%%%%%%%%%%%%%%

data = load('x_up.txt');

fx_x_up = mean(data(:,1));

fy_x_up = mean(data(:,2));

fz_x_up = mean(data(:,3));

clear data;

 

%%%%%%%%%%%%%%

% X DOWN %

%%%%%%%%%%%%%%

data = load('x_down.txt');

fx_x_down = mean(data(:,1));

fy_x_down = mean(data(:,2));

fz_x_down = mean(data(:,3));

clear data;

 

%%%%%%%%%%%%%%

% Y UP %

%%%%%%%%%%%%%%

data = load('y_up.txt');

fx_y_up = mean(data(:,1));

fy_y_up = mean(data(:,2));

fz_y_up = mean(data(:,3));

clear data;

 

%%%%%%%%%%%%%%

% Y DOWN %

%%%%%%%%%%%%%%

data = load('y_down.txt');

fx_y_down = mean(data(:,1));

fy_y_down = mean(data(:,2));

fz_y_down = mean(data(:,3));

clear data;

 

%%%%%%%%%%%%%%

% Z UP %

%%%%%%%%%%%%%%

data = load('z_up.txt');

fx_z_up = mean(data(:,1));

fy_z_up = mean(data(:,2));

fz_z_up = mean(data(:,3));

clear data;

 

%%%%%%%%%%%%%%

% Z DOWN %

%%%%%%%%%%%%%%

data = load('z_down.txt');

fx_z_down = mean(data(:,1));

fy_z_down = mean(data(:,2));

fz_z_down = mean(data(:,3));

clear data;

Após a coleta de dados do sistema, a ideia é tentar organizar os parâmetros de forma que a gente consiga organizar os parâmetros para buscar uma solução ótima para todos os parâmetros do sistema (menor custo). A forma encontrada de fazer isso foi organizar os dados no formato matricial:

em que é a matriz que condensa as medidas de médias das acelerações encontradas para cada eixo, X representa a matriz que desejamos encontrar. Em específico, para o meu caso, fiz a seguinte organização e sequência:

1º) Reescrever todas as equações acima apresentadas. Todas as variáveis a serem determinadas estão em negrito.

São ao todo 15 parâmetros a serem determinados, todos eles organizandos na variável X a seguir (você pode escolher a sua própria):

2º) Rearranjar as variáveis acima, de modo a separar as variáveis a serem determinadas, tem-se que:

O que equivale a escrevermos em produtos de matrizes a:

%============================================================

% Para resolução do sistema Ya = A.X, precisamos identificar

% as variáveis Ya e A.

%============================================================

 

% Matriz Ya

Ya = zeros(18, 1);

Ya(1, 1) = fx_x_up - 2*g;

Ya(2, 1) = fy_x_up;

Ya(3, 1) = fz_x_up;

Ya(4, 1) = fx_x_down + 2*g;

Ya(5, 1) = fy_x_down;

Ya(6, 1) = fz_x_down;

Ya(7, 1) = fx_y_up;

Ya(8, 1) = fy_y_up - 2*g;

Ya(9, 1) = fz_y_up;

Ya(10, 1) = fx_y_down;

Ya(11, 1) = fy_y_down + 2*g;

Ya(12, 1) = fz_y_down;

Ya(13, 1) = fx_z_up;

Ya(14, 1) = fy_z_up;

Ya(15, 1) = fz_z_up - 2*g;

Ya(16, 1) = fx_z_down;

Ya(17, 1) = fy_z_down;

Ya(18, 1) = fz_z_down + 2*g;

 

% Matriz A

A = zeros(18, 15);

A(1, 1) = 1;

A(1, 4) = g;

A(1, 13) = g*g;

A(2, 2) = 1;

A(2, 9) = g;

A(3, 3) = 1;

A(3, 11) = g;

A(4, 1) = 1;

A(4, 4) = -g;

A(4, 13) = g*g;

A(5, 2) = 1;

A(5, 9) = -g;

A(6, 3) = 1;

A(6, 11) = -g;

A(7, 1) = 1;

A(7, 7) = g;

A(8, 2) = 1;

A(8, 5) = g;

A(8, 14) = g*g;

A(9, 3) = 1;

A(9, 12) = g;

A(10, 1) = 1;

A(10, 7) = -g;

A(11, 2) = 1;

A(11, 5) = -g;

A(11, 14) = g*g;

A(12, 3) = 1;

A(12, 12) = -g;

A(13, 1) = 1;

A(13, 8) = g;

A(14, 2) = 1;

A(14, 10) = g;

A(15, 3) = 1;

A(15, 6) = g;

A(15, 15) = g*g;

A(16, 1) = 1;

A(16, 8) = -g;

A(17, 2) = 1;

A(17, 10) = -g;

A(18, 3) = 1;

A(18, 6) = -g;

A(18, 15) = g*g;

 

 

%%% Solução da equação

x = inv(A'*A) * A' * Ya;

É basicamente isso que a gente faz na tarefa de calibração dos acelerômetros, com a determinação dos 15 parâmetros possíveis nessa caracterização do sistema.