Фильтр Мажвика
Фильтр Мажвика для вычисления углов наклона
В одном из уроков мы познакомились с понятием системы стабилизации полета, которая даёт возможность летающему роботу хоть как то удерживать положение относительно горизонта. Мы разобрали альфа-бета фильтр — математический аппарат, который позволяет с помощью акселерометра и гироскопа получать достаточно точные значения тангажа и крена.
И всё бы хорошо, но простой альфа-бета фильтр не может хорошо бороться с вибрацией, вызываемой моторами мультикоптера. Вибрация вызывает ошибки вычисления углов наклона, от чего беспилотник ведет себя неустойчиво.
Есть два распространенных алгоритма, которые работают лучше альфа-бета — это фильтр Калмана и фильтр Мажвика. Оба являются достаточно сложными, чтобы разбираться с ними в рамках данного урока. Однако, мы можем использовать готовую программную реализацию фильтра в своих целях.
Фильтр Себастиана Мажвика
Данный алгоритм смешивания данных акселерометра, гироскопа и магнитометра был предложен Себастианом Мажвиком (Sebastian Madgwick) в 2009 году, в рамках научного исследования на соискание Ph.D. Статью с описанием математической модели фильтра, как и реализацию алгоритма на разных языках программирования можно найти на сайте проекта (ссылка в конце урока).
Помимо всего прочего, в статье говорится о том, что точность фильтра Мажвика достигает точности фильтра Калмана, но в отличие от последнего, требует меньше вычислительных ресурсов. Это очень полезное свойство, которое делает алгоритм пригодным для работы на слабых микроконтроллерах.
Попробуем использовать фильтр Мажвика для обработки данных с модуля MPU6050, который имеет в своем составе датчик ускорения — акселерометр и датчик скорости вращения — он же гироскоп.
Благодарю Вас !
Добрый день, спасибо Вам за статью.
Но я вот не понял о сырых данных акселерометра. Разве не нужно учитывать настройки акселерометра (2G, 4G…) и делить на соответствующий коэффициент (4096 и т.п.).
И ещё вопрос, у меня почему-то ось Х и Y гироскопа и акселерометра не совпадают, я был уверен что так и есть, но Вы негде их не меняете местами для функции Маджвика…
Спасибо.
Для данных акселерометра нет нужды делать нормировку, она уже есть внутри библиотеки. Насчет несовпадения осей, тут главное не запутаться. К примеру если мы вращаем датчик вокруг оси X, то акселерометр естественно не будет ничего показывать по этой оси. Ведь он измеряет наклон этой оси, а она неподвижна. Это еще не значит, что оси не совпадают.
Спасибо большое, а для каких настроек (±2g, ±4g, ±8g, ±16g) нормировка в библиотеке, и как ее изменить?
в коде есть такая строка:
quat[0] = q0; quat[1] = q1; quat[2] = q2; quat[3] = q3;
Откуда взялись q0, q1, q2, q3 ???????
Это переменные из файла MadgwickAHRS.h
А как быть, если используешь два модуля 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 равны нулю… Почему?
Добрый день, статья очень помогла! спасибо!
Но почему-то перестали работать кнопки перехода на
ПОДКЛЮЧЕНИЕ / ПРОГРАММА/ ПОЛЕЗНЫЕ ССЫЛКИ
Есть какие-то ограничения?
Это все свежее обновление сайта, чиним. Нет никаких ограничений!
Спасибо, заработало!!!
Спасибо, искал какими библиотеками можно реализовать фильтр Мэджвика, нашел в этой статье. К сожалению нету датчика чтобы проверить код, переписываю под аналогичный. Есть непонятный момент, в коде есть строка преобразования сырых данных гироскопа из град/сек в рад/сек, но данные преобразования уже заложены в библиотечную функцию, получается код преобразовывает данные в рад/сек, а затем когда данные попадают в функцию, преобразование делается еще раз. Разве это будет работать? Кто нибудь проверял на практике, как работает код? В любом случае спасибо за статью!
В дополнение к предыдущему комментарию, вот строки кода из библиотеки:
// Convert gyroscope degrees/sec to radians/sec
gx *= 0.0174533f;
gy *= 0.0174533f;
gz *= 0.0174533f;