Расчет смещения с помощью акселерометра и гироскопа (MPU6050)
Я студент информатики и работаю над проектом электроники, который требует вычисления рыскания, тангажа, крена и смещения X,Y,Z. Я хочу прикрепить IMU к пистолету и отследить его ориентацию и смещение. Я могу получить Yaw, Pitch and Roll, но, к сожалению, не могу понять, как рассчитать смещение или положение моего пистолета. Я использую датчик 10-DOF GY-87, который содержит MPU-6050.
Я получаю значения в формате g и m/s2. Из исследования, которое я изучал, я понял, что мне нужно получить ускорение / время2, а затем добавить все значения. Но не могу понять, какую разницу во времени я должен использовать. Refrence: Как рассчитать расстояние на основе ускорения телефона
#include "I2Cdev.h"
#include "MPU6050.h"
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;
//MPU6050 accelgyro(0x69); // <-- use for AD0 high
int16_t ax, ay, az;
float dx, dy, dz = 0;
int16_t gx, gy, gz;
#define LED_PIN 13
bool blinkState = false;
void setup() {
// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
Serial.begin(38400);
Serial.println("Initializing I2C devices...");
accelgyro.initialize();
Serial.println("Testing device connections...");
Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
Serial.println("Updating internal sensor offsets...");
accelgyro.setXGyroOffset(85);
accelgyro.setYGyroOffset(1);
accelgyro.setZGyroOffset(-4);
accelgyro.setXAccelOffset(-4269);
accelgyro.setYAccelOffset(-4836);
accelgyro.setZAccelOffset(1080);
pinMode(LED_PIN, OUTPUT);
}
void loop() {
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
dx=dx+(float)(((float)ax/(float)16384)*9.8*0.05*0.05);
dy=dy+(float)(((float)ay/(float)16384)*9.8*0.05*0.05);
dz=dz+(float)(((float)az/(float)16384)*9.8*0.05*0.05);
Serial.print(dx); Serial.print("\t");
Serial.print(dy); Serial.print("\t");
Serial.print(dz); Serial.print("\t\n");
delay(1000);
// blink LED to indicate activity
blinkState = !blinkState;
digitalWrite(LED_PIN, blinkState);
}
Я хочу отслеживать объект, как показано в следующих видео YouTube.
http://www.youtube.com/watch?v=ZYyyaJgKsDg
Я был бы признателен вам, если кто-нибудь из вас может направить меня в этом отношении. Спасибо
PS: извините за мой плохой английский и использование нетехнических терминов.
5 ответов
Я боюсь, что ответ не тот, который вы захотите услышать. Очень, очень трудно вычислить позицию из блока IMU. Это видео от Google - очень хороший пример того, почему (перейдите к 24-й минуте для подробного объяснения). По сути, вам нужно интегрировать ускорение дважды, чтобы добраться до позиции. Вам также нужно убрать гравитацию из ускорения, которое видит ваш IMU. Если это не сделано идеально, ошибки накапливаются очень быстро.
Видео, на которое вы ссылались, использовало информацию о том, что мяч катился по столу, чтобы сообщить их модели. Они могли отслеживать ориентацию своего датчика, чтобы знать, куда катится шарик. Они использовали радиус шара вместе с угловыми изменениями со своей доски, чтобы отследить мяч по x и y. Если вы уберете их со стола, это не сработает.
Если вам нужно что-то отследить, вам нужно найти датчик, который может дать вам информацию о положении вашего объекта (GPS, видеоанализ). Затем вы можете использовать фильтр Калмана, чтобы объединить это с данными IMU, чтобы получить хорошую точность позиционирования.
Удачи с вашим проектом.
Я знаю, что это старый пост, но я подумал, что опубликую некоторые исправления из экспериментов, которые я провел с MPU 6050 и arduino.
Во-первых, уравнение, которое вы используете, чтобы найти смещение, неверно, вам нужно использовать уравнение кинематики. однако уравнение Xf = 1 / 2at ^2 + Vo t + Xo также неверно, поскольку оно ТОЛЬКО для постоянных ускорений. В этом случае ускорение меняется, поэтому вы можете либо взять среднее ускорение между двумя точками набора данных и вставить его в предыдущее уравнение, либо использовать следующее уравнение:
Xf = 1/4 (Af + Ao)t ^2 + Vo t + Xo
Где Xf - это окончательное расстояние в метрах, Af - это окончательное текущее ускорение в м / с ^2, Ao - предыдущее ускорение последнего набора данных в м / с ^2, t - ИЗМЕНЕНИЕ во времени между наборами Af и Ao. данных в SECONDS, Vo - это мгновенная скорость последнего набора данных в м / с, а Xo - это конечное расстояние последнего набора данных или сумма всех предыдущих расстояний в метрах. Vo необходимо рассчитать, используя предыдущее ускорение и ускорение из двух предыдущих наборов данных назад или Ao-1, используя следующее уравнение кинематики:
Vo = 1/2 (Ao + Ao-1) * t + Vo-1
Где Vo - предыдущая мгновенная скорость в м / с, Ao - предыдущее ускорение в м / с ^2, Ao-1 - ускорение из двух наборов данных назад в м / с ^2, t - изменение времени между Ao и Наборы данных Ao-1 в SECONDS, а Vo-1 - мгновенная скорость в м / с набора данных Ao-1 или двух наборов данных назад.
Во-вторых, вам нужно использовать более надежные часы. Я рекомендую использовать функцию micros() и помните, что t - это ИЗМЕНЕНИЕ во времени между наборами данных. Я не уверен в его надежности, но это лучшее, что я могу придумать. УБЕДИТЕСЬ, чтобы преобразовать микросекунды в СЕКУНДЫ, используя указанные уравнения.
В-третьих, я рекомендую вам калибровать смещения очень часто или даже каждый раз в самом начале кода, комбинируя код с калибровочным эскизом, таким как рисунок Луиса Роденаса. Вы можете поместить его в подпрограмму setup() и использовать небольшое значение размера буфера или наборы данных, например 200 или 300, чтобы убедиться, что вы не будете слишком долго ждать между экспериментами.
В-четвертых, вы можете либо использовать среднее значение между двумя ускорениями набора данных (что мы и делали выше), либо сделать шаг вперед и использовать среднее значение для различных наборов данных, например, использовать буферный массив fifo для хранения различных значений ускорения и среднее из всех значений в буфере. Буферы Fifo требуют, чтобы в них всегда оставалось заданное количество значений, но при вводе нового значения старшее значение остается. Чем больше fifo, тем более неточными будут ваши расчеты расстояний, но буфер fifo позволит значимым значениям ускорения слишком сильно повлиять на ваши данные. Размер буфера требует, чтобы вы находили точечную точку между точностью и единичными выбросами в значениях ускорения. Если вы используете буфер fifo для значения ускорения, используйте следующие уравнения:
Xf = 1 / 2At ^2 + Vo t + Xo
Vo = Aold * t + Vo-1
Где A - новое среднее ускорение, полученное из гипотетического буфера FIFO, Aold - старое среднее ускорение от последнего среднего FIFO, а t - изменение во времени между двумя отдельными точками набора данных. Все в стандартных единицах конечно, м / с и тд.
Вы хорошо поработали, преобразовав необработанные значения ускорения в м / с ^2, разделив на 16384 и умножив на 9,8 м / с ^2. Значение 16384 зависит от стандартной настройки чувствительности +-2g, которая может измениться, если вы выберете другую настройку, например +-4g.
Наконец, даже со всеми вышеописанными изменениями было бы ЧРЕЗВЫЧАЙНО трудно получить точные показания из-за множества различных факторов, таких как температура. Важно поддерживать контролируемую среду для вашего акселерометра с помощью вентиляторов или чего-либо еще, чтобы держать ваш гироскоп / акселерометр при комнатной температуре или 25 ° C. В библиотеке Jeff Rowberg MPU6050 есть функция для получения текущей температуры, mpu.getTemperature() Я верю.
Даже со всеми этими изменениями все еще будет ОЧЕНЬ трудно получить хорошее точное чтение из-за математических причин и небольших неточностей. Вы можете попробовать установить в своем гироскопе менее чувствительный параметр, потому что я знаю, что для mpu 6050 установлено значение по умолчанию +-2g, более высокий параметр может не дать многим проблемам повлиять на ваши показания, но он станет менее чувствительным к небольшим смещениям.
Всегда есть больше способов, с помощью которых вы можете оптимизировать значения гироскопа / акселерометра. Один из способов заключается в том, что если вы увеличите частоту потока данных MPU для получения большего количества данных за определенный период времени, это может позволить вам получить более точные показания, Я считаю, что в библиотеке Джеффа Роуберга есть функция, позволяющая это сделать.
Да.. в вашем цикле () у вас есть задержка (1000);
Это означает, что ваше время дельты составляет 1 секунду, а не 50 мс.
попробуйте это в цикле ():
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
double t2 = millis() - pms;
t2 /= 1000; // convert ms to s
t2 *=t2;
// your Math and Serial here...
pms = millis();
Уже поздно, но, возможно, полезно. Вы можете искать на стендах INS инерциальной навигационной системы, чтобы найти детали. Однако вкратце: ИДУ измеряет ускорения и угловые скорости в прикрепленной к нему рамке. Например, acc в направлении x является направлением x IMU, а не направлением x системы отсчета, в котором вы хотите вычислить позиции. Поэтому вам необходимо преобразовать ускорения в систему отсчета. Это можно сделать с помощью углов Эйлера. Как я понял, MPU 6050 дает вам углы Эйлера и даже матрицу преобразования. Если это так, вы должны использовать
Acc_inert = T * Acc_body
перенести вектор акк, измеренный датчиком, в вектор сравнения в вашей системе отсчета. Затем двойная интеграция дает вам положение в системе отсчета. Более подробная информация об углах Эйлера и расчете матриц преобразования доступна в Интернете.
Я понимаю, что в конце ваш вопрос: "Но я не могу понять, какую разницу во времени я должен использовать".
Дельта-время, которое вы используете в своем коде, составляет 0,05. Имея в виду; вы предполагаете, что ваша скорость ускорения составляет 20 Гц. Если это не 20 Гц, измените его соответственно. Ваши расчеты основаны на следующих формулах:
dis = 1/2 a t^2 + vt
v = v0 + at
где t - время между двумя выборками с последовательным ускорением.
Удачи