Calibração de girômetros numa IMU

Modelo matemático para girô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 girô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 representa a não ortogonalidade da tríade de girômetro; 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, tem-se que:

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

DIferentemente da calibração do acelerômetro, a ideia aqui é posicionar o sistema a ser calibrado alinhado ao eixo de referência e incidir sobre ele uma rotação constante na plataforma. No caso, eu desenvolvi um sistema de calibração que possibilita a rotação do sistema na velocidade de 60º/s. Essa, portanto, será a referência de rotação. Para deixar o sistema adaptável à qualquer velocidade de seu interesse, mas de preferência que não seja nula. Para fins de código, será adotado o parâmetro .

1. X+

Posicionando o girômetro com o eixo X com a direção para cima, é só. Nessa condição, a medida de referência para a calibração deve ser + 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 "gyro_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 "giro_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 "gyro_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 "gyro_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 "gyro_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 "gyro_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.

Após a coleta de dados do sistema, a ideia é tentar organizar os parâmetros de forma que a gente consiga buscar uma solução ótima para todos os parâmetros do sistema (ou seja, a de 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 velocidades angulares 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 12 parâmetros a serem determinados, todos eles organizandos na variável X a seguir (você pode escolher a sua própria organização):

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:

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

% Definição da variável de velocidade angular de referência

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

omega_rot = 60; % em º/s.

 

% Matriz de medidas dos girômetros.

Yg = [omega_x_x_up - 2*omega_rot;

omega_y_x_up;

omega_z_x_up;

omega_x_x_down + 2*omega_rot;

omega_y_x_down;

omega_z_x_down;

omega_x_y_up;

omega_y_y_up - 2*omega_rot;

omega_z_y_up;

omega_x_y_down;

omega_y_y_down + 2*omega_rot;

omega_z_y_down;

omega_x_z_up;

omega_y_z_up;

omega_z_z_up - 2*omega_rot;

omega_x_z_down;

omega_y_z_down;

omega_z_z_down + 2*omega_rot;];

 

% Matriz de construção para os girômetros.

A = [ 1, 0, 0, w_rot, 0, 0, 0, 0, 0, 0, 0, 0;

0, 1, 0, 0, 0, 0, 0, 0, w_rot, 0, 0, 0;

0, 0, 1, 0, 0, 0, 0, 0, 0, 0, w_rot, 0;

1, 0, 0, -w_rot, 0, 0, 0, 0, 0, 0, 0, 0;

0, 1, 0, 0, 0, 0, 0, 0, -w_rot, 0, 0, 0;

0, 0, 1, 0, 0, 0, 0, 0, 0, 0, -w_rot, 0;

1, 0, 0, 0, 0, 0, w_rot, 0, 0, 0, 0, 0;

0, 1, 0, 0, w_rot, 0, 0, 0, 0, 0, 0, 0;

0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, w_rot;

1, 0, 0, 0, 0, 0, -w_rot, 0, 0, 0, 0, 0;

0, 1, 0, 0, -w_rot, 0, 0, 0, 0, 0, 0, 0;

0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, -w_rot;

1, 0, 0, 0, 0, 0, 0, w_rot, 0, 0, 0, 0;

0, 1, 0, 0, 0, 0, 0, 0, 0, w_rot, 0, 0;

0, 0, 1, 0, 0, w_rot, 0, 0, 0, 0, 0, 0;

1, 0, 0, 0, 0, 0, 0, -w_rot, 0, 0, 0, 0;

0, 1, 0, 0, 0, 0, 0, 0, 0, -w_rot, 0, 0;

0, 0, 1, 0, 0, -w_rot, 0, 0, 0, 0, 0, 0;];

 

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

Y = inv(A'*A) * A' * Yg;

 

%%% Separação da solução.

b = [Y(1); Y(2); Y(3)] % BIAS

S = [Y(4), 0, 0;

0, Y(5), 0;

0, 0, Y(6)]

N = [ 1, Y( 7), Y( 8);

Y( 9), 1, Y(10);

Y(11), Y(12), 1]

 

%%% Cálculo do sistema (matriz inversa).

M = inv(N + S + eye(3));

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

Nesse ponto o que resta é simplesmente a obtenção dos parâmetros do giro dado que as variáveis de calibração já foram determinadas. Isso pode ser feito por meio de:

Como os parâmetros das matrizes a seguir são determinados offline, a gente pode simplesmente realizar a obtenção dos parâmetros de forma direta.