Расчет шага, рыскания и крена по данным mag, acc и gyro

19

У меня есть доска Arduino с датчиком 9 степеней свободы, по которому я должен определить высоту, рыскание и крен доски.

Вот пример одного набора данных от датчика 9-DOF:

Акселерометр (м / с)

  • ТочностьИкс = -5,85
  • ТочностьY = 1,46
  • ТочностьZ = 17,98

Гироскоп (об / мин)

  • ГирИкс = 35,14
  • ГирY = -40,22
  • ГирZ = -9,86

Магнитометр (Гаусс)

  • магнитныйИкс = 0,18
  • магнитныйY = -0,04
  • магнитныйZ = -0,15

Как я могу рассчитать высоту, рыскание и крен по этим данным?

MaltDew
источник
1
Основной принцип: от обнаружения силы тяжести в вашем акселерометре вы знаете, какой путь вниз; По обнаружению магнитного поля Земли в вашем магнитометре вы узнаете, какой путь идет на север. Основываясь на этом и не предполагая других значительных ускорений или сильных магнитных полей, вы можете определить свое собственное отношение.
Вс
1
Данные гироскопа обеспечивают скорость вращения, но не абсолютную позицию. Он может быть интегрирован для оценки изменений по известному положению, но обычно он шумный и может дрейфовать, если он не используется совместно с другими датчиками.
Welf
1
также обратитесь к фильтрам Калмана, так как статические числа должны быть обработаны довольно много, чтобы дать надежные оценки шага крена и рыскания. Также обратите внимание, что положение датчика имеет важное значение (вам необходимо это учитывать).
Гюркан Четин

Ответы:

15

Наклон, крен и рыскание определяются как вращение вокруг осей X, Y и Z. Ниже в качестве рисунка для иллюстрации определения.

Ролл шаг и рыскание

В предыдущем проекте я использовал акселерометр ADXL345 от Analog Devices для расчета крена и шага. Ниже приведены уравнения, используемые для расчета крена и тангажа. Я сделал часть исходного кода доступной для публичного использования.

accelerationX = (signed int)(((signed int)rawData_X) * 3.9);
accelerationY = (signed int)(((signed int)rawData_Y) * 3.9);
accelerationZ = (signed int)(((signed int)rawData_Z) * 3.9);
pitch = 180 * atan (accelerationX/sqrt(accelerationY*accelerationY + accelerationZ*accelerationZ))/M_PI;
roll = 180 * atan (accelerationY/sqrt(accelerationX*accelerationX + accelerationZ*accelerationZ))/M_PI;

Полный исходный код можно найти здесь .

Основываясь на приведенных выше определениях

yaw = 180 * atan (accelerationZ/sqrt(accelerationX*accelerationX + accelerationZ*accelerationZ))/M_PI;

Примечание: M_PI = 3.14159265358979323846 это константа, определенная в math.h

Ниже приведены некоторые ссылки, включая базовый исходный код Arduino, который может вам помочь.


Ссылки:

Махендра Гунавардена
источник
2
Хороший ответ, стоило бы добавить, что положение и ориентация датчика в транспортном средстве будут важны, и что данные должны обрабатываться дальше, чтобы дать надежные результаты. (отфильтрованный или объединенный с более надежными низкочастотными данными, такими как GPS)
Гюркан Четин
(@Zubair) "yaw = 180 * атан (ускорение Z / sqrt (ускорениеXaccelerationX + ускорениеZaccelerationZ)) / M_PI;" что это за M_PI ??
Васаби
@Wasabi M_PI = 3.14159265358979323846. Это константа, определенная в библиотеке math.h.
Махендра Гунавардена
8

Поэтому мой более длинный ответ ниже предполагает, что доска будет ускоряться, и в течение этого времени вам все еще нужно будет измерить ваш наклон, крен и рыскание в течение короткого промежутка времени. Если доска будет стационарной для всех измерений, то ответ Махендры Гунавардены подойдет вам идеально. Если речь идет о таком устройстве, как сегвей, модельный самолет, мультиротор или что-то, что движется, вы можете продолжать читать. Этот пост о том, как использовать все три сенсора, хотя метод называется сенсорным синтезом. Слияние датчиков позволяет получить сильные стороны каждого датчика и минимизировать влияние слабых сторон каждого датчика.

Характеристики сенсора и фона

Сначала поймите, что акселерометр измеряет все силы, приложенные к нему, а не только силу тяжести. Таким образом, в идеальном мире, когда акселерометр находится в неподвижном положении без каких-либо вибраций, вы можете точно определить, какой путь идет вверх, используя некоторую базовую тригонометрию, как показано в ответе Махендры Гунавардены. Однако, так как акселерометр соберет все силы, любые вибрации приведут к шуму. Следует также отметить, что если доска ускоряется, вы не можете просто использовать простую тригонометрию, поскольку сила, сообщаемая акселерометром, представляет собой не только силу тяжести Земли, но и силу, которая заставляет вас ускоряться.

Магнитометр более прост, чем акселерометр. Движение не вызовет проблем с ним, но такие вещи, как железо и другие магниты, будут влиять на ваш результат. Если источники, вызывающие эти помехи, постоянны, с ними нетрудно бороться, но если эти источники не являются постоянными, это создаст тонны шума, которые проблематично удалить.

Из трех датчиков гироскоп является, пожалуй, самым надежным, и они обычно очень хорошо измеряют скорость вращения. На него не влияют такие вещи, как источники железа, и ускорения практически не влияют на их способность измерять скорость вращения. Они очень хорошо сообщают о скорости вращения устройства, однако, поскольку вы ищете абсолютный угол, вам нужно интегрировать скорость, чтобы получить положение. Это добавит ошибку последнего измерения к ошибке новых измерений, поскольку интеграция в основном представляет собой сумму значений в диапазоне, даже если ошибка для одного измерения составляет всего 0,01 градуса в секунду, при 100 измерениях ваша позиция можно выключить на 1 градус, на 1000 измерений, можно выключить на 10 градусов. Если вы делаете сотни измерений в секунду, Вы можете видеть, что это вызывает проблемы. Это обычно называется гироскопическим дрейфом.

Датчик слияния

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

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

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

filteredData = (1-weight)*filteredData + weight*newData

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

fusedData = (1-weight)*gyroData + weight*accelMagData

Следует отметить, что данные представляют собой вектор тангажа, крена и рыскания. Вы можете просто использовать три переменные вместо массивов, если хотите. Для этого расчета гироскоп обеспечивает положение в градусах по тангажу, крену и рысканию, магнитометр обеспечивает угол по рысканию, а акселерометр предоставляет свои собственные числа для тангажа и крена.

Если вам все еще нужна дополнительная информация, вы можете воспользоваться Google «Sensor Fusion с дополнительным фильтром», здесь есть множество статей.

Dave851
источник
3

Из данных датчика акселератора можно рассчитать только тангаж и крен. Нижеприведенный документ Freescale объясняет большим количеством информации, что вам нужно:

AN3461 - Измерение наклона с использованием трехосевого акселерометра

Основываясь на высказываниях документа,

загарφИксYZзнак равнограммпYграммпZ

загарθИксYZзнак равно-граммпИксграммпYгрехφ+граммпZсозφзнак равно-граммпИксграммпY2+граммпZ2

что соответствует:

roll = atan2(accelerationY, accelerationZ)

pitch = atan2(-accelerationX, sqrt(accelerationY*accelerationY + accelerationZ*accelerationZ))

Конечно, результат таков, только если повороты происходят в определенном порядке (Rxyz):

  1. φ
  2. θ
  3. ψ

рИксYZψ

ekalyvio
источник
1
Он, добро пожаловать на Engineering SE! Этот сайт поддерживает Latex, посмотрите, каким красивым стал ваш ответ сейчас. :-)
Петер - Восстановить Монику