Расчет смещения с помощью акселерометра и гироскопа (MPU6050)

Боюсь, что ответ не тот, который вам захочется услышать. Очень, очень сложно рассчитать позицию по блоку IMU. Это видео от Google — очень хороший пример того, почему (подробное объяснение). По сути, вам нужно дважды интегрировать ускорение, чтобы попасть в нужное положение. Вам также необходимо удалить гравитацию из ускорения, наблюдаемого вашим IMU. Если это не сделано идеально, ошибки накапливаются очень быстро.

Видео, на которое вы ссылаетесь, использовало информацию о том, что мяч катится по столу, чтобы сообщить своей модели. Они могли отслеживать ориентацию своего датчика, чтобы знать, в какую сторону катится мяч. Они использовали радиус мяча вместе с угловыми изменениями их доски, чтобы отслеживать мяч по осям x и y. Если бы вы взяли их мяч со стола, это бы вообще не сработало.

Если вам нужно что-то отслеживать, вам следует поискать какой-нибудь датчик, который может дать вам информацию о положении вашего объекта (GPS, анализ видео). Затем вы можете использовать фильтр Калмана, чтобы объединить это с данными IMU, чтобы получить хорошую точность позиционирования.

Удачи с вашим проектом.

Я знаю, что это старый пост, но я решил опубликовать некоторые исправления из экспериментов, которые я провел с MPU 6050 и Arduino.

Во-первых, уравнение, которое вы используете для определения смещения, неверно. Вам нужно использовать уравнение кинематики. однако уравнение Xf = 1/2at^2 + Vot + Xo также неверно, поскольку оно ТОЛЬКО для ПОСТОЯННЫХ ускорений. В этом случае ускорение ИЗМЕНЯЕТСЯ, поэтому вы можете либо взять среднее ускорение между двумя точками набора данных и подставить его в предыдущее уравнение, либо использовать следующее уравнение:

Xf = 1/4(Af + Ao)t^2 + Vot + Xo

Где Xf — конечное расстояние в метрах, Af — конечное ТЕКУЩЕЕ ускорение в м/с^2, Ao — предыдущее ускорение последнего набора данных в м/с^2, t — ИЗМЕНЕНИЕ во времени МЕЖДУ наборами Af и Ao данных в СЕКУНДАХ, Vo — мгновенная скорость последнего набора данных в м/с, а Xo — конечное расстояние последнего набора данных или сумма всех предыдущих расстояний в метрах. Vo необходимо рассчитать, используя предыдущее ускорение и ускорение из двух предыдущих наборов данных назад или Ao-1, используя следующее уравнение кинематики:

Vo = 1/2(Ao + Ao-1)*t + Vo-1

Где Vo — предыдущая мгновенная скорость в м/с, Ao — предыдущее ускорение в м/с^2, Ao-1 — ускорение из двух наборов данных назад в м/с^2, t — изменение во времени между Ao и Наборы данных Ao-1 в СЕКУНДАХ, а Vo-1 — это мгновенная скорость в м/с набора данных Ao-1 или двух наборов данных назад.

Во-вторых, вам нужно использовать более надежные часы. Я рекомендую использовать функцию micros() и помнить, что t — это ИЗМЕНЕНИЕ во времени между наборами данных. Я не уверен в его надежности, но это лучшее, что я могу придумать. ОБЯЗАТЕЛЬНО конвертируйте микросекунды в СЕКУНДЫ при использовании указанных уравнений.

В-третьих, я рекомендую вам калибровать свои смещения время от времени или даже каждый раз в самом начале вашего кода, комбинируя ваш код с калибровочным эскизом, таким как у Луиса Роденаса. Вы можете поместить его в подпрограмму setup() и можете использовать небольшое значение размера буфера или наборы данных, такие как 200 или 300, чтобы не ждать слишком долго между экспериментами.

В-четвертых, вы можете либо работать, используя среднее между двумя ускорениями наборов данных (что мы и делаем выше), либо сделать еще один шаг и использовать среднее значение различных наборов данных, например, используя буферный массив fifo для хранения различных значений ускорения и принимать среднее значение всех значений в буфере. Буферы FIFO требуют, чтобы в них всегда оставалось определенное количество значений, но по мере поступления нового значения более старое удаляется. Чем больше fifo, тем более неточными будут ваши расчеты расстояния, но буфер fifo позволит слишком сильно влиять на ваши данные. Размер вашего буфера требует, чтобы вы нашли золотую середину между точностью и одиночными выбросами в значениях ускорения. Если вы используете буфер FIFO для значения ускорения, используйте следующие уравнения:

Xf = 1/2At^2 + Vot + Xo

Vo = Старый *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 по инерциальной навигационной системе, чтобы найти детали. Однако вкратце: IMU измеряет ускорения и угловые скорости в прикрепленной к нему рамке. Например, acc в направлении x — это направление x IMU, а не направление x системы отсчета, в которой вы хотите вычислить позиции. Поэтому вам нужно преобразовать ускорения в систему отсчета. Это можно сделать с помощью углов Эйлера. Насколько я понял, MPU 6050 дает вам углы Эйлера и даже матрицу преобразования. Если это так, вы должны использовать

Acc_inert = T * Acc_body 

для перевода вектора Acc, измеренного датчиком, в вектор Acc в вашей системе отсчета. Затем двойное интегрирование дает вам положение в системе отсчета. Более подробная информация об углах Эйлера и вычислении матриц преобразования доступна в Интернете.

Я понимаю, что в конце ваш вопрос звучит так: «Но я не могу понять, какую разницу во времени мне следует использовать».

Дельта-время, которое вы используете в своем коде, составляет 0,05. Значение; вы предполагаете, что ваша скорость передачи данных ускорения составляет 20 Гц. Если это не 20 Гц, измените его соответствующим образом. Ваши расчеты основаны на следующих формулах:

dis = 1/2 a t^2 + vt

v = v0 + at

где t — время между двумя последовательными выборками ускорения.

Удачи