Eu tenho uma placa Arduino com um sensor de 9 graus de liberdade, da qual devo determinar o tom, a guinada e o rolo da placa.

Aqui está um exemplo de um conjunto de dados do sensor 9-DOF:

Acelerômetro (m / s)

• = -5,85$\text{Acc}_{X}$
• = 1,46$\text{Acc}_{Y}$
• = 17,98$\text{Acc}_{Z}$

Giroscópio (RPM)

• = 35,14$\text{Gyr}_{X}$
• = -40,22$\text{Gyr}_{Y}$
• = -9,86$\text{Gyr}_{Z}$

Magnetômetro (Gauss)

• = 0,18$\text{Mag}_{X}$
• = -0,04$\text{Mag}_{Y}$
• = -0,15$\text{Mag}_{Z}$

Como posso calcular pitch, yaw e roll a partir desses dados?

Inclinação, rotação e guinada são definidos como a rotação em torno dos eixos X, Y e Z. Abaixo como uma figura para ilustrar a definição.

Em um projeto anterior, usei um acelerômetro ADXL345 da Analog Devices para calcular a rotação e a inclinação. Abaixo estão as equações usadas para o cálculo de rotação e inclinação. Eu disponibilizei parte do código fonte para uso público.

accelerationX = (signed int)(((signed int)rawData_X) * 3.9);
accelerationY = (signed int)(((signed int)rawData_Y) * 3.9);
accelerationZ = (signed int)(((signed int)rawData_Z) * 3.9);
pitch = 180 * atan (accelerationX/sqrt(accelerationY*accelerationY + accelerationZ*accelerationZ))/M_PI;
roll = 180 * atan (accelerationY/sqrt(accelerationX*accelerationX + accelerationZ*accelerationZ))/M_PI;

O código fonte completo pode ser encontrado aqui .

Baseie-se nas definições acima

yaw = 180 * atan (accelerationZ/sqrt(accelerationX*accelerationX + accelerationZ*accelerationZ))/M_PI;

Nota: M_PI = 3.14159265358979323846, é constante definida em math.h

Abaixo estão algumas referências, incluindo o código-fonte básico do Arduino, que podem ajudá-lo.

Referências:

• Dinâmica de voo
• Código fonte ADXL345 preto Beaglebone
• Demonstração de QT incorporado Adxl345 Beaglebone black
• Arduino IMU: Pitch & Roll a partir de um acelerômetro

Portanto, minha resposta mais longa abaixo pressupõe que a prancha sofrerá aceleração e, durante esse período, você ainda precisará medir sua afinação, rotação e guinada dentro de um curto período de tempo. Se o quadro estiver estacionário para todas as medições, a resposta de Mahendra Gunawardena funcionará perfeitamente para você. Se isso ocorrer em um dispositivo como um segway ou modelo de avião ou multirotor ou qualquer coisa que se mova, você pode continuar lendo. Este post é sobre como usar os três sensores através de um método chamado fusão de sensores. A fusão de sensores permite obter os pontos fortes de cada sensor e minimizar os efeitos das fraquezas de cada sensor.

Características e histórico do sensor

Primeiro, entenda que um acelerômetro mede todas as forças que estão sendo aplicadas a ele, não apenas a força da gravidade. Portanto, em um mundo perfeito, com o acelerômetro em posição estacionária, sem vibrações, você poderia determinar perfeitamente qual o caminho a seguir, usando alguma trigonometria básica, como mostra a resposta de Mahendra Gunawardena. No entanto, como um acelerômetro capta todas as forças, qualquer vibração resultará em ruído. Deve-se notar também que, se a prancha está acelerando, você não pode usar apenas trigonometria simples, pois a força que o acelerômetro está relatando não é apenas a força de gravidade da Terra, mas também a força que está causando a aceleração.

Um magnetômetro é mais direto que um acelerômetro. O movimento não causará problemas, mas coisas como ferro e outros ímãs irão afetar sua produção. Se as fontes que causam essa interferência são constantes, não é difícil lidar com elas, mas, se não forem constantes, criará toneladas de ruído problemático para remover.

Dos três sensores, o giroscópio é discutível como o mais confiável e normalmente é muito bom em medir a velocidade de rotação. Não é afetado por coisas como fontes de ferro e acelerações basicamente não têm impacto em sua capacidade de medir a velocidade de rotação. Eles fazem um trabalho muito bom em relatar a velocidade na qual o dispositivo está girando, no entanto, como você procura um ângulo absoluto, precisa integrar a velocidade para obter a posição. Isso adicionará o erro da última medição ao erro das novas medições, uma vez que a integração é basicamente uma soma de valores em uma faixa, mesmo que o erro de uma medição esteja a apenas 0,01 graus por segundo, em 100 medições, sua posição pode ser desativado em 1 grau, em 1000 medições, e em 10 graus. Se você estiver fazendo centenas de medições por segundo, você pode ver que isso causa problemas. Isso é comumente chamado de desvio do giroscópio.

Fusão do sensor

Agora, a beleza de ter todos esses sensores trabalhando juntos é que você pode usar as informações do acelerômetro e magnetômetro para cancelar a deriva do giroscópio. Isso acaba permitindo que você dê a precisão e a velocidade do giroscópio sem a falha fatal do desvio do giroscópio.

A combinação dos dados desses três sensores pode ser feita de mais de uma maneira: falarei sobre o uso de um filtro complementar, porque é muito mais simples que o filtro kalman e o filtro kalman consomem muito mais recursos em sistemas embarcados. Muitas vezes, um filtro complementar é bom o suficiente, mais simples de implementar (supondo que você não esteja usando uma biblioteca pré-criada) e permite processar os dados mais rapidamente.

Agora para o processo. Os primeiros passos que você precisa fazer é integrar a saída do giroscópio para converter a velocidade angular em posição angular. Você provavelmente também precisará aplicar um filtro passa-baixo no acelerômetro e no magnetômetro para lidar com o ruído na saída. Um filtro FIR simples como o mostrado abaixo funciona aqui. Com alguma trigonometria, você pode encontrar o pitch and roll com o acelerômetro e a guinada com o magnetômetro.

filteredData = (1-weight)*filteredData + weight*newData

O peso é apenas uma constante que pode ser ajustada dependendo da quantidade de ruído com que você lida, quanto maior o ruído, menor o valor do peso. Agora, a combinação dos dados dos sensores pode ser feita pela seguinte linha de código.

fusedData = (1-weight)*gyroData + weight*accelMagData

Note-se que os dados são um vetor do pitch, roll e yaw. Você também pode usar três variáveis ​​para fazer isso, em vez de matrizes, se desejar. Para esse cálculo, o giroscópio fornece uma posição em graus em inclinação, rotação e guinada, o magnetômetro fornece um ângulo para guinada, enquanto o acelerômetro fornece seus próprios números para a inclinação e rotação.

Se você ainda deseja obter mais informações, pode pesquisar no Google "fusão de sensores com filtro complementar", há muitos artigos sobre isso.

A partir dos dados do sensor do acelerador, você só pode calcular o pitch and roll. O documento abaixo da Freescale explica com bastante informação o que você precisa:

AN3461 - Sensor de Inclinação Usando um Acelerômetro de Três Eixos

Com base nos dizeres do documento,

que equivale a:

roll = atan2(accelerationY, accelerationZ)

pitch = atan2(-accelerationX, sqrt(accelerationY*accelerationY + accelerationZ*accelerationZ))

Obviamente, o resultado é esse somente quando as rotações estão ocorrendo em uma ordem específica (Rxyz):

1. $\phi$
2. $\theta$
3. $\psi$

$R_{xyz}$$\psi$