Easyelectronics.ru

Электроника для всех
Текущее время: 10 апр 2021, 23:34

Часовой пояс: UTC + 5 часов



JLCPCB – Прототипы печатных плат за $2/5шт. два слоя. $5/5шт. четыре слоя
Крупнейший производитель печатных плат и прототипов. Более 600000 клиентов и свыше 10000 заказов в день!
Получите скидку на почтовую отправку при первом заказе в JLCPCB!

Начать новую тему Ответить на тему  [ Сообщений: 7 ] 
Автор Сообщение
 Заголовок сообщения: Инерциальная навигационная система
СообщениеДобавлено: 11 июл 2013, 05:19 
Заглядывает иногда

Зарегистрирован: 12 авг 2012, 19:04
Сообщения: 92
Имеется гироскоп и акселерометр 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, чтобы они стали пригодны для фильтрации? Как эти странные числа превратить в угловую скорость, которую потом можно будет проинтегрировать в угол?


Вернуться к началу
 Профиль  
 
 Заголовок сообщения: Re: Инерциальная навигационная система
СообщениеДобавлено: 11 июл 2013, 13:26 
Старожил
Аватара пользователя

Зарегистрирован: 18 фев 2010, 14:42
Сообщения: 1153
Откуда: Лондон
Я когда маялся баклаврской делал код для IMU, нашел замечательный проектик ArduIMU. Там много говнокода, но в коментах к коду вполне неплохо написано, что нужно зачем и как считается и перепилить в нормальную реализацию совсем не проблема.


Вернуться к началу
 Профиль  
 
 Заголовок сообщения: Re: Инерциальная навигационная система
СообщениеДобавлено: 11 июл 2013, 13:57 
Заглядывает иногда

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


Вернуться к началу
 Профиль  
 
 Заголовок сообщения: Re: Инерциальная навигационная система
СообщениеДобавлено: 11 июл 2013, 23:14 
Заглядывает иногда

Зарегистрирован: 12 авг 2012, 19:04
Сообщения: 92
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. Редко, но бывает. И это неприятно. Что не так?


Вернуться к началу
 Профиль  
 
 Заголовок сообщения: Re: Инерциальная навигационная система
СообщениеДобавлено: 12 июл 2013, 13:25 
Старожил
Аватара пользователя

Зарегистрирован: 18 фев 2010, 14:42
Сообщения: 1153
Откуда: Лондон
По мелочам: +-32767 - это крайние значения signed short.

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


Вернуться к началу
 Профиль  
 
 Заголовок сообщения: Re: Инерциальная навигационная система
СообщениеДобавлено: 12 июл 2013, 13:42 
Старожил

Зарегистрирован: 15 янв 2013, 13:24
Сообщения: 5665
kiv писал(а):
иногда на выходе бывает +-32767 (т.е. максимальное значение int). Да, это после перевода результата в градусы и приведения к int из float. Редко, но бывает. И это неприятно. Что не так?
Проверьте "сырые" данные сенсоров в этих случаях. Обычно, когда MEMS зашкаливает или ещё почему-то не может дать достоверный результат, он возвращает константу типа 0xF000 или 0xF800 и т.п. Надо просто игнорировать "сырое" значение (и конверсию для него не делать), если оно оказывается таким числом.


Вернуться к началу
 Профиль  
 
 Заголовок сообщения: Re: Инерциальная навигационная система
СообщениеДобавлено: 25 дек 2013, 15:49 
Здравствуйте!

Зарегистрирован: 25 дек 2013, 15:47
Сообщения: 1
А не поделитесь кодом как с этого датчика что-то читать, а то купил мультивии плату и хочется что-то написать а с чего начать не знаю :)


Вернуться к началу
 Профиль  
 
Показать сообщения за:  Поле сортировки  
Начать новую тему Ответить на тему  [ Сообщений: 7 ] 


Часовой пояс: UTC + 5 часов


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

Сейчас этот форум просматривают: нет зарегистрированных пользователей


Вы не можете начинать темы
Вы не можете отвечать на сообщения
Вы не можете редактировать свои сообщения
Вы не можете удалять свои сообщения
Вы не можете добавлять вложения

Найти:
Перейти:  

Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group
Русская поддержка phpBB