Поэтому мой более длинный ответ ниже предполагает, что доска будет ускоряться, и в течение этого времени вам все еще нужно будет измерить ваш наклон, крен и рыскание в течение короткого промежутка времени. Если доска будет стационарной для всех измерений, то ответ Махендры Гунавардены подойдет вам идеально. Если речь идет о таком устройстве, как сегвей, модельный самолет, мультиротор или что-то, что движется, вы можете продолжать читать. Этот пост о том, как использовать все три сенсора, хотя метод называется сенсорным синтезом. Слияние датчиков позволяет получить сильные стороны каждого датчика и минимизировать влияние слабых сторон каждого датчика.
Характеристики сенсора и фона
Сначала поймите, что акселерометр измеряет все силы, приложенные к нему, а не только силу тяжести. Таким образом, в идеальном мире, когда акселерометр находится в неподвижном положении без каких-либо вибраций, вы можете точно определить, какой путь идет вверх, используя некоторую базовую тригонометрию, как показано в ответе Махендры Гунавардены. Однако, так как акселерометр соберет все силы, любые вибрации приведут к шуму. Следует также отметить, что если доска ускоряется, вы не можете просто использовать простую тригонометрию, поскольку сила, сообщаемая акселерометром, представляет собой не только силу тяжести Земли, но и силу, которая заставляет вас ускоряться.
Магнитометр более прост, чем акселерометр. Движение не вызовет проблем с ним, но такие вещи, как железо и другие магниты, будут влиять на ваш результат. Если источники, вызывающие эти помехи, постоянны, с ними нетрудно бороться, но если эти источники не являются постоянными, это создаст тонны шума, которые проблематично удалить.
Из трех датчиков гироскоп является, пожалуй, самым надежным, и они обычно очень хорошо измеряют скорость вращения. На него не влияют такие вещи, как источники железа, и ускорения практически не влияют на их способность измерять скорость вращения. Они очень хорошо сообщают о скорости вращения устройства, однако, поскольку вы ищете абсолютный угол, вам нужно интегрировать скорость, чтобы получить положение. Это добавит ошибку последнего измерения к ошибке новых измерений, поскольку интеграция в основном представляет собой сумму значений в диапазоне, даже если ошибка для одного измерения составляет всего 0,01 градуса в секунду, при 100 измерениях ваша позиция можно выключить на 1 градус, на 1000 измерений, можно выключить на 10 градусов. Если вы делаете сотни измерений в секунду, Вы можете видеть, что это вызывает проблемы. Это обычно называется гироскопическим дрейфом.
Датчик слияния
Теперь прелесть совместной работы всех этих датчиков состоит в том, что вы можете использовать информацию от акселерометра и магнитометра для устранения дрейфа гироскопа. Это в конечном итоге позволяет вам дать вам точность и скорость гироскопа без фатального недостатка дрейфа гироскопа.
Объединение данных с этих трех датчиков может быть выполнено более чем одним способом, я расскажу об использовании дополнительного фильтра, потому что он намного проще, чем фильтр Калмана и фильтры Калмана, которые потребляют гораздо больше ресурсов во встроенных системах. Часто дополнительный фильтр достаточно хорош, проще в реализации (при условии, что вы не используете предварительно созданную библиотеку) и позволяет быстрее обрабатывать данные.
Теперь о процессе. Первые шаги, которые вам нужно сделать, - это интегрировать выход гироскопа для преобразования угловой скорости в угловое положение. Скорее всего, вам придется применить фильтр низких частот на акселерометре и магнитометре, чтобы справиться с шумом на выходе. Здесь работает простой FIR-фильтр, подобный показанному ниже. С помощью некоторой тригонометрии вы можете найти высоту и крен с помощью акселерометра и рыскание с помощью магнитометра.
filteredData = (1-weight)*filteredData + weight*newData
Вес - это просто постоянная величина, которую можно регулировать в зависимости от того, с каким количеством шума вам приходится иметь дело, чем выше уровень шума, тем меньше будет значение веса. Теперь объединение данных с датчиков можно выполнить с помощью следующей строки кода.
fusedData = (1-weight)*gyroData + weight*accelMagData
Следует отметить, что данные представляют собой вектор тангажа, крена и рыскания. Вы можете просто использовать три переменные вместо массивов, если хотите. Для этого расчета гироскоп обеспечивает положение в градусах по тангажу, крену и рысканию, магнитометр обеспечивает угол по рысканию, а акселерометр предоставляет свои собственные числа для тангажа и крена.
Если вам все еще нужна дополнительная информация, вы можете воспользоваться Google «Sensor Fusion с дополнительным фильтром», здесь есть множество статей.
