Гравитационная компенсация в данных IMU - PullRequest
Новая
       14

Гравитационная компенсация в данных IMU

0 голосов
/ 02 марта 2020

Я пытаюсь выполнить гравитационную компенсацию на моих данных акселерометра. Учитывая акселерометр, с 6 DOF (Акселерометр, Гироскоп) я хочу удалить / компенсировать влияние силы тяжести при чтении акселерометра (Акселерометр может вращаться свободно).

Ниже описан способ сохранения исходных значений датчика в struct с именем sample: 

uint8_t *p=data; // p is a pointer to the sensor data

    int i;
    for(i=0; i<4; i++)  // quaternion
    {
        sample.quaternion[i]=((float)get_int32(p))/(1<<29);
        len+=snprintf(s+len, sizeof(line)-len, "\t%9.6f", sample.quaternion[i]);
        p+=4;
    }

    for(i=0; i<3; i++) // euler213_degrees
    {
        sample.euler213_degrees[i]=get_int16(p);
        len+=snprintf(s+len, sizeof(line)-len, "\t%d", sample.euler213_degrees[i]);
        p+=2;
    }

    for(i=0; i<3; i++) // euler123_degrees
    {
        sample.euler123_degrees[i]=get_int16(p);
        len+=snprintf(s+len, sizeof(line)-len, "\t%d", sample.euler123_degrees[i]);
        p+=2;
    }

    for(i=0; i<3; i++) // acceleration_g
    {
        sample.acceleration_g[i]=(2.0*get_int16(p))/(1<<15);
        len+=snprintf(s+len, sizeof(line)-len, "\t%6.3f", sample.acceleration_g[i]);
        p+=2;
    }

    for(i=0; i<3; i++) // gyroscope_dps
    {
        sample.gyroscope_dps[i]=(2000.0*get_int16(p))/(1<<15);
        len+=snprintf(s+len, sizeof(line)-len, "\t%6.1f", sample.gyroscope_dps[i]);
        p+=2;
    }

Можете ли вы показать мне способ получения данных акселерометра с гравитационной компенсацией?

1 Ответ

2 голосов
/ 06 марта 2020

Кватернион, вычисляемый IMU (устройство с 6 степенями свободы), является в основном отношением устройства (3D-ориентация) по отношению к локальной системе координат Земли; поэтому его можно использовать для поворота измерения ускорения от локальной системы отсчета тела (значения, непосредственно измеренные IMU) к локальной системе отсчета Земли (система координат с плоскостью xy, касательной к земле). Вы можете использовать кватернионное умножение, чтобы сделать это

v '= q. против q *

, где q - кватернион, а v - вектор ускорения (пожалуйста, посмотрите умножение на кватернионный вектор для получения дополнительной информации *). Поскольку мы знаем, что гравитация является вектором u = ( 0,0, г ) в локальной системе координат Земли (где g ≈ 9,81 м / с ^ 2, предполагая, что IMU измерения также в м / с ^ 2), мы можем затем вычесть этот вектор из v '

v' '= v' - u

v '' - вектор с гравитационной компенсацией / удалением. Тем не менее, он не находится в локальной системе отсчета тела ИДУ (помните, что мы повернули его в локальной системе отсчета Земли) Таким образом, чтобы преобразовать его обратно, вы можете умножить его на обратный кватернион

v '' '= q-1. против q-1 *

v '' ' будет исходным измерением ускорения за вычетом силы тяжести. Чтобы убедиться в правильности результата, вы можете проводить измерения, пока устройство находится в неподвижном состоянии. Независимо от того, как устройство вращается, измерения ускорения должны быть близки к ( 0,0,0 ).

Если IMU не обеспечивал измерение кватернионов, он может были вычислены путем слияния показаний акселерометра и гироскопа (что в любом случае делает ИДУ). Если вы заинтересованы в этом, я предлагаю поискать алгоритмы Madgwick, Mahony или Extended Kalman Filter для оценки отношения.

* Полезные ресурсы

Умножение кватернионов: https://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/arithmetic/index.htm

Вращение вектора с кватернионом (особенно третий ответ, если вы хотите увидеть какой-то код): https://math.stackexchange.com/questions/40164/how-do-you-rotate-a-vector-by-a-unit-quaternion

...