Calculando pitch, yaw e roll a partir de dados mag, acc e giroscópio

19

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,85AccX
  • = 1,46AccY
  • = 17,98AccZ

Giroscópio (RPM)

  • = 35,14GyrX
  • = -40,22GyrY
  • = -9,86GyrZ

Magnetômetro (Gauss)

  • = 0,18MagX
  • = -0,04MagY
  • = -0,15MagZ

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

MaltDew
fonte
11
Princípio básico: a partir da detecção da gravidade em seu acelerômetro, você sabe para que lado está; da detecção do campo magnético da Terra em seu magnetômetro, você sabe para que lado é o norte. Com base nisso e assumindo que não há outras acelerações significativas ou campos magnéticos fortes, você pode determinar sua própria atitude.
welf 27/06
11
Os dados do giroscópio fornecem uma taxa de rotação, mas não uma posição absoluta. Ele pode ser integrado para estimar a mudança a partir de uma atitude conhecida, mas isso geralmente é barulhento e propenso a deriva, se não for usado em conjunto com outros sensores.
welf 27/06
11
também se referem aos filtros kalman, pois os números estáticos precisam ser processados ​​bastante, para fornecer estimativas confiáveis ​​de inclinação e guinada. Observe também que a posição do sensor é importante (é preciso levar em consideração).
Gürkan

Respostas:

15

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.

Passo do rolo e guinada

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:

Mahendra Gunawardena
fonte
2
Boa resposta, vale a pena acrescentar que a posição e a orientação do sensor no veículo seriam importantes e que os dados devem ser processados ​​ainda mais, para obter resultados confiáveis. (filtrado ou fundido com dados de baixa frequência mais confiáveis, tal como o GPS)
Gürkan Çetin
(@Zubair) "guinada = 180 * atan (aceleraçãoZ / sqrt (accelerationXaccelerationX + accelerationZaccelerationZ)) / M_PI;" o que é esse 'M_PI' ??
Wasabi
@Wasabi M_PI = 3.14159265358979323846. É constante definido na biblioteca math.h.
Mahendra Gunawardena
8

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.

Dave851
fonte
3

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,

tanϕxyz=GpyGpz

tanθxyz=GpxGpysinϕ+Gpzcosϕ=GpxGpy2+Gpz2

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. ϕ
  2. θ
  3. ψ

Rxyzψ

ekalyvio
fonte
11
Ele, bem-vindo ao Engineering SE! Este site suporta Látex, veja como sua resposta ficou bonita agora. :-)
peterh - Restabelece Monica