Полезные ссылки

Фильтр Мажвика для вычисления углов наклона

Сайт с материалами по фильтру Мажвика:

http://x-io.co.uk/open-source-imu-and-ahrs-algorithms

Библиотека для Ардуино:

https://github.com/robotclass/RobotClass-MadgwickAHRS

Изображение на титуле страницы — торпедный гироскоп конца 19 века. Источник: http://www.muzej-rijeka.hr


Изменено:

Фильтр Мажвика для вычисления углов наклона: 12 комментариев

  1. Добрый день, спасибо Вам за статью.
    Но я вот не понял о сырых данных акселерометра. Разве не нужно учитывать настройки акселерометра (2G, 4G…) и делить на соответствующий коэффициент (4096 и т.п.).
    И ещё вопрос, у меня почему-то ось Х и Y гироскопа и акселерометра не совпадают, я был уверен что так и есть, но Вы негде их не меняете местами для функции Маджвика…
    Спасибо.

    • Для данных акселерометра нет нужды делать нормировку, она уже есть внутри библиотеки. Насчет несовпадения осей, тут главное не запутаться. К примеру если мы вращаем датчик вокруг оси X, то акселерометр естественно не будет ничего показывать по этой оси. Ведь он измеряет наклон этой оси, а она неподвижна. Это еще не значит, что оси не совпадают.

      • Спасибо большое, а для каких настроек (±2g, ±4g, ±8g, ±16g) нормировка в библиотеке, и как ее изменить?

  2. в коде есть такая строка:
    quat[0] = q0; quat[1] = q1; quat[2] = q2; quat[3] = q3;

    Откуда взялись q0, q1, q2, q3 ???????

  3. А как быть, если используешь два модуля MPU6050?
    Я попробовал так:

    #include «I2Cdev.h»
    #include «MPU6050.h»
    #include «MadgwickAHRS.h»

    #define TO_RAD 0.01745329252f

    MPU6050 accelgyro(0x68); // ADO low
    MPU6050 accelgyro2(0x69); // ADO high

    unsigned long tm, imu_t, prn_t;
    const unsigned int imu_to = 20;
    const unsigned int prn_to = 100;

    float tdelta;
    int16_t ax, ay, az, ax2, ay2, az2;
    int16_t gx_raw, gy_raw, gz_raw, gx_raw2, gy_raw2, gz_raw2;

    float imu[3], imu2[3];
    float quat[4], quat2[4];
    float e[3], e2[3];

    void setup() {
    Serial.begin(57600);
    Serial.println(«Initializing I2C devices…»);
    accelgyro.initialize(); // инициализация датчика accelgyro(0x68)
    accelgyro2.initialize(); // инициализация датчика accelgyro2(0x69)
    Serial.println(«Testing device connections…»);
    Serial.println(accelgyro.testConnection() ? «MPU6050(0x68) connection successful» : «MPU6050(0x68) connection failed»);
    Serial.println(accelgyro2.testConnection() ? «MPU6050(0x69) connection successful» : «MPU6050(0x69) connection failed»);
    }

    void loop() {
    accelgyro.getMotion6(&ax, &ay, &az, &gx_raw, &gy_raw, &gz_raw);
    accelgyro2.getMotion6(&ax2, &ay2, &az2, &gx_raw2, &gy_raw2, &gz_raw2);

    // преобразуем сырые данные гироскопа в рад/с
    float gx = gx_raw * TO_RAD / 131.0;
    float gy = gy_raw * TO_RAD / 131.0;
    float gz = gz_raw * TO_RAD / 131.0;
    float gx2 = gx_raw2 * TO_RAD / 131.0;
    float gy2 = gy_raw2 * TO_RAD / 131.0;
    float gz2 = gz_raw2 * TO_RAD / 131.0;

    // каждые 20мс вычисляем углы наклона
    tm = millis();
    if (imu_t + imu_to < tm) {
    tdelta = tm — imu_t; // вычисляем дельту времени в миллисекундах
    imu_t = tm;
    // вызываем алгоритм фильтра и передаем туда:
    // — дельту времени в секундах
    // — данные гироскопа в рад/с
    // — сырые данные акселерометра
    MadgwickAHRSupdateIMU(tdelta/1000.0, gx, gy, gz, (float)ax, (float)ay, (float)az);
    MadgwickAHRSupdateIMU(tdelta/1000.0, gx2, gy2, gz2, (float)ax2, (float)ay2, (float)az2);
    quat[0] = q0; quat[1] = q1; quat[2] = q2; quat[3] = q3;
    quat2Euler(&quat[0], &imu[0]); // преобразуем кватернион в углы Эйлера
    quat2Euler(&quat2[0], &imu2[0]); // преобразуем кватернион в углы Эйлера
    }

    // каждые 100мс добавляем новую точку графика
    tm = millis();
    if (prn_t + prn_to < tm) {
    prn_t = tm;
    Serial.print("Yaw "); Serial.print(imu[2]/TO_RAD); Serial.print("\t");
    Serial.print("Pitch "); Serial.print(imu[1]/TO_RAD); Serial.print("\t");
    Serial.print("Roll "); Serial.println(imu[0]/TO_RAD);
    Serial.println();
    Serial.print("Yaw_2 "); Serial.print(imu2[2]/TO_RAD); Serial.print("\t");
    Serial.print("Pitch_2 "); Serial.print(imu2[1]/TO_RAD); Serial.print("\t");
    Serial.print("Roll_2 "); Serial.println(imu2[0]/TO_RAD);
    Serial.println();
    }
    }

    Данные со второго MPU6050 равны нулю… Почему?

  4. Добрый день, статья очень помогла! спасибо!
    Но почему-то перестали работать кнопки перехода на
    ПОДКЛЮЧЕНИЕ / ПРОГРАММА/ ПОЛЕЗНЫЕ ССЫЛКИ
    Есть какие-то ограничения?

  5. Спасибо, искал какими библиотеками можно реализовать фильтр Мэджвика, нашел в этой статье. К сожалению нету датчика чтобы проверить код, переписываю под аналогичный. Есть непонятный момент, в коде есть строка преобразования сырых данных гироскопа из град/сек в рад/сек, но данные преобразования уже заложены в библиотечную функцию, получается код преобразовывает данные в рад/сек, а затем когда данные попадают в функцию, преобразование делается еще раз. Разве это будет работать? Кто нибудь проверял на практике, как работает код? В любом случае спасибо за статью!

  6. В дополнение к предыдущему комментарию, вот строки кода из библиотеки:
    // Convert gyroscope degrees/sec to radians/sec
    gx *= 0.0174533f;
    gy *= 0.0174533f;
    gz *= 0.0174533f;

Добавить комментарий

Ваш адрес email не будет опубликован. Обязательные поля помечены *

Этот сайт использует Akismet для борьбы со спамом. Узнайте, как обрабатываются ваши данные комментариев.