Инерциальная навигационная система

kiv

Имеется гироскоп и акселерометр MPU-6050, а также контроллер AVR. Хочу собрать на них БИНС.

Читал эти статьи:
http://we.easyelectronics.ru/quadro_and ... govor.html
http://we.easyelectronics.ru/quadro_and ... filtr.html

Вроде всё просто, но мало соотносится с сырыми данными от датчиков. А выглядят они при неподвижном датчике (лежит на столе) как-то так:

Код:

AccelX: -739, AccelY: 792, AccelZ: 4086
GyroX: 477, GyroY: -179, GyroZ: 25

То есть ничего даже близкого к нулям. И попытка тупо в лоб интегрировать по "методу прямоугольников" из первой статьи ничем хорошим не закончится (ибо даже неподвижная система будет при таких данных якобы вращаться и довольно быстро). А значит и до комлиментарного фильтра дело не дойдёт.

Итак, вопрос: что нужно сделать с сырыми данными от MPU-6050, чтобы они стали пригодны для фильтрации? Как эти странные числа превратить в угловую скорость, которую потом можно будет проинтегрировать в угол?

cahbtexhuk

Я когда маялся баклаврской делал код для IMU, нашел замечательный проектик ArduIMU. Там много говнокода, но в коментах к коду вполне неплохо написано, что нужно зачем и как считается и перепилить в нормальную реализацию совсем не проблема.

kiv

Хм... нагуглил, что у MPU-6050 есть что-то, называемое Digital Motion Processor, который умеет выдавать данные в виде матриц, выполнять фильтрацию и т. д. Что можете об этом сказать? Что будет качественнее? Ручная обработка данных на AVR или использование Digital Motion Processor и получение уже практически готовых углов?

kiv

DMP не документирован совершенно, так что решил всё же сделать самостоятельно обработку данных. Написал вот такую программу:

Код:

void sensor_read_data(bool first_read) { // Вызывается таймером 44 раза в секунду
   temperature = mpu6050_read_data(MPU6050_TEMP_OUT_H / 340.0) + 36.53;
   
   float gyro_x = (float)mpu6050_read_data(MPU6050_GYRO_XOUT_H) / 65.5;
   float gyro_y = (float)mpu6050_read_data(MPU6050_GYRO_YOUT_H) / 65.5;
   float gyro_z = (float)mpu6050_read_data(MPU6050_GYRO_ZOUT_H) / 65.5;
   float accel_x = (float)mpu6050_read_data(MPU6050_ACCEL_XOUT_H) / 8192.0;
   float accel_y = (float)mpu6050_read_data(MPU6050_ACCEL_YOUT_H) / 8192.0;
   float accel_z = (float)mpu6050_read_data(MPU6050_ACCEL_ZOUT_H) / 8192.0;
   
   accel_rx = atan(accel_x / sqrt(accel_y * accel_y + accel_z * accel_z));
   accel_ry = atan(accel_y / sqrt(accel_x * accel_x + accel_z * accel_z));
   accel_rz = atan(accel_z / sqrt(accel_x * accel_x + accel_y * accel_y));
   
   if (first_read) {
      rx = accel_rx;
      ry = accel_ry;
      rz = 0.0; // Судить о вращении по оси Z по акселерометру не имеет особого смысла, потом буду использовать магнитометр
   } else {
      rx += gyro_x / 44;
      ry += gyro_y / 44;
      rz += gyro_z / 44;
                // На тот случай, если угол превысил +/-360 градусов
      while (rx > 2 * M_PI) rx -= 2 * M_PI;
      while (ry > 2 * M_PI) ry -= 2 * M_PI;
      while (rz > 2 * M_PI) rz -= 2 * M_PI;
      while (rx < -2 * M_PI) rx += 2 * M_PI;
      while (ry < -2 * M_PI) ry += 2 * M_PI;
      while (rz < -2 * M_PI) rz += 2 * M_PI;
   }
   
   rx = 0.9 * rx + 0.1 * accel_rx;
   ry = 0.9 * ry + 0.1 * accel_ry;
   //rz = 0.9 * rz + 0.1 * accel_rz;
}

Если смотреть на углы по данным от акселерометра, то в ситуации неподвижного датчика данные выглядят достаточно хорошо. Точность около +-5 градусов (может, чуть больше). А вот когда использую данные гироскопа получается бредятина. Где я ошибся?

UPD: Разобрался - я забыл переводить угловую скорость из градусов в радианы. Исправил и теперь всё работает. Но иногда на выходе бывает +-32767 (т.е. максимальное значение int). Да, это после перевода результата в градусы и приведения к int из float. Редко, но бывает. И это неприятно. Что не так?

cahbtexhuk

По мелочам: +-32767 - это крайние значения signed short.

DMP - вообще-то отдельная платная фишка и поэтому не документирована. Типа да, есть проц, который сам за тебя все делает и на выходе ты просто получаешь нужные углы, умеет распозновать жесты и кучу простых вещей. Но, за то, чтобы им начать пользоваться надо еще заплатить. Типа за ноу-хау.

AlexPM

kiv писал(а):

иногда на выходе бывает +-32767 (т.е. максимальное значение int). Да, это после перевода результата в градусы и приведения к int из float. Редко, но бывает. И это неприятно. Что не так?

Проверьте "сырые" данные сенсоров в этих случаях. Обычно, когда MEMS зашкаливает или ещё почему-то не может дать достоверный результат, он возвращает константу типа 0xF000 или 0xF800 и т.п. Надо просто игнорировать "сырое" значение (и конверсию для него не делать), если оно оказывается таким числом.

Gandalv

А не поделитесь кодом как с этого датчика что-то читать, а то купил мультивии плату и хочется что-то написать а с чего начать не знаю :)

Easyelectronics.ru

Инерциальная навигационная система

Кто сейчас на конференции