Учебное пособие по акселерометру и гироскопу: 3 шага

2024 Автор: John Day | [email protected]. Последнее изменение: 2024-01-30 11:55

Вступление

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

Мы рассмотрим:

• Что измеряет акселерометр?
• Что измеряет гироскоп (он же гироскоп)?
• Как преобразовать аналого-цифровые (АЦП) показания, которые вы получаете от этих датчиков, в физические единицы (это будет g для акселерометра, град / с для гироскопа)
• Как совместить показания акселерометра и гироскопа, чтобы получить точную информацию о наклоне вашего устройства относительно плоскости земли

На протяжении всей статьи я постараюсь свести математику к минимуму. Если вы знаете, что такое синус / косинус / тангенс, тогда вы сможете понять и использовать эти идеи в своем проекте независимо от того, какую платформу вы используете: Arduino, Propeller, Basic Stamp, чипы Atmel, Microchip PIC и т. Д.

Есть люди, которые считают, что вам нужна сложная математика, чтобы использовать блок IMU (сложные FIR или IIR фильтры, такие как фильтры Калмана, фильтры Паркс-Макклеллана и т. Д.). Вы можете исследовать все это и получить прекрасные, но сложные результаты. Мой способ объяснения требует простой математики. Я верю в простоту. Я думаю, что простую систему легче контролировать и контролировать, к тому же у многих встроенных устройств нет мощности и ресурсов для реализации сложных алгоритмов, требующих матричных вычислений.

Я буду использовать в качестве примера новый блок IMU, акселерометр Acc_Gyro + Gyro IMU. Мы будем использовать параметры этого устройства в наших примерах ниже. Это устройство - хорошее устройство для начала, потому что оно состоит из 2 устройств:

- LIS331AL (технический паспорт) - трехосный акселерометр 2G - LPR550AL (технический паспорт) - двухкоординатный гироскоп по тангажу и крену, 500 град / с

Вместе они представляют собой инерциальный измерительный блок с 5 степенями свободы. Вот это причудливое имя! Тем не менее, за причудливым названием скрывается очень полезное комбинированное устройство, которое мы рассмотрим и подробно объясним в этом руководстве.

Шаг 1: акселерометр

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

Если мы возьмем этот ящик в месте без гравитационных полей или, если на то пошло, без других полей, которые могли бы повлиять на положение мяча, мяч просто будет плавать в середине ящика. Вы можете представить, что ящик находится в космическом пространстве, вдали от любых космических тел, или, если такое место трудно найти, представьте хотя бы космический корабль, вращающийся вокруг планеты, где все находится в невесомом состоянии. На картинке выше вы можете видеть, что мы назначаем каждой оси пару стен (мы удалили стену Y +, чтобы мы могли заглянуть внутрь коробки). Представьте, что каждая стена чувствительна к давлению. Если мы внезапно переместим коробку влево (мы разгоняем ее с ускорением 1g = 9,8 м / с ^ 2), мяч ударится о стену X-. Затем мы измеряем силу давления, которую мяч прикладывает к стене, и выводим значение -1g по оси X.

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

Если мы возьмем нашу модель и поместим ее на Землю, мяч упадет на Z-стенку и приложит силу 1g к нижней стенке, как показано на рисунке ниже:

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

Хотя эта модель не совсем соответствует конструкции датчика MEMS, она часто бывает полезной при решении проблем, связанных с акселерометром. На самом деле существуют аналогичные датчики с металлическими шариками внутри, они называются переключателями наклона, однако они более примитивны и обычно могут только определить, наклонено ли устройство в пределах некоторого диапазона или нет, но не степень наклона.

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

Значения 0,71 не произвольны, они фактически являются приближением для SQRT (1/2). Это станет более ясным, когда мы представим нашу следующую модель акселерометра.

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

Пожалуйста, взгляните на модель выше, я сохранил цвета осей, чтобы вы могли мысленно перейти от предыдущей модели к новой. Только представьте, что каждая ось в новой модели перпендикулярна соответствующим граням коробки в предыдущей модели. Вектор R - это вектор силы, который измеряет акселерометр (это может быть либо сила гравитации, либо сила инерции из приведенных выше примеров, либо их комбинация). Rx, Ry, Rz - проекции вектора R на оси X, Y, Z. Обратите внимание на следующее соотношение:

R ^ 2 = Rx ^ 2 + Ry ^ 2 + Rz ^ 2 (уравнение 1)

что в основном эквивалентно теореме Пифагора в 3D.

Помните, что чуть ранее я говорил вам, что значения SQRT (1/2) ~ 0,71 не случайны. Если вы подставите их в формулу выше, вспомнив, что наша сила гравитации составляла 1 г, мы можем убедиться, что:

1 ^ 2 = (-КОРМАТ (1/2)) ^ 2 + 0 ^ 2 + (-КОРМАТ (1/2)) ^ 2

просто подставив R = 1, Rx = -SQRT (1/2), Ry = 0, Rz = -SQRT (1/2) в уравнение 1

После долгой теоретической преамбулы мы приближаемся к реальным акселерометрам. Значения Rx, Ry, Rz фактически линейно связаны со значениями, которые выводит ваш реальный акселерометр и которые вы можете использовать для выполнения различных вычислений.

Прежде чем мы перейдем к делу, давайте немного поговорим о том, как акселерометры доставляют нам эту информацию. Большинство акселерометров делятся на две категории: цифровые и аналоговые. Цифровые акселерометры предоставят вам информацию с использованием последовательного протокола, такого как I2C, SPI или USART, в то время как аналоговые акселерометры будут выводить уровень напряжения в заранее определенном диапазоне, который вы должны преобразовать в цифровое значение с помощью модуля АЦП (аналого-цифрового преобразователя). Я не буду вдаваться в подробности того, как работает ADC, отчасти потому, что это такая обширная тема, а отчасти потому, что она отличается от одной платформы к другой. Некоторые микроконтроллеры будут иметь встроенные модули АЦП, некоторым из них потребуются внешние компоненты для выполнения преобразований АЦП. Независимо от того, какой тип модуля АЦП вы используете, вы получите значение в определенном диапазоне. Например, 10-битный модуль АЦП выдаст значение в диапазоне 0..1023, обратите внимание, что 1023 = 2 ^ 10 -1. 12-битный модуль АЦП выдаст значение в диапазоне 0..4095, обратите внимание, что 4095 = 2 ^ 12-1.

Давайте перейдем к рассмотрению простого примера, предположим, что наш 10-битный модуль АЦП дал нам следующие значения для трех каналов (осей) акселерометра:

AdcRx = 586 AdcRy = 630 AdcRz = 561

Каждый модуль АЦП будет иметь опорное напряжение, допустим, в нашем примере оно составляет 3,3 В. Чтобы преобразовать 10-битное значение АЦП в напряжение, мы используем следующую формулу:

VoltsRx = AdcRx * Vref / 1023

Небольшое примечание: для 8-битного АЦП последний делитель будет 255 = 2 ^ 8 -1, а для 12-битного АЦП последний делитель будет 4095 = 2 ^ 12 -1.

Применяя эту формулу ко всем 3 каналам, получаем:

VoltsRx = 586 * 3,3 В / 1023 = ~ 1,89 В (мы округляем все результаты до 2 десятичных знаков) VoltsRy = 630 * 3,3 В / 1023 = ~ 2,03 В Volts Rz = 561 * 3,3 В / 1023 = ~ 1,81 В

Каждый акселерометр имеет нулевой уровень напряжения, вы можете найти его в спецификациях, это напряжение, которое соответствует 0g. Чтобы получить значение напряжения со знаком, нам нужно рассчитать сдвиг от этого уровня. Допустим, наш уровень напряжения 0g равен VzeroG = 1,65 В. Мы рассчитываем сдвиги напряжения от нулевого напряжения следующим образом:

DeltaVoltsRx = 1,89 В - 1,65 В = 0,24 В DeltaVoltsRy = 2,03 В - 1,65 В = 0,38 В DeltaVolts Rz = 1,81 В - 1,65 В = 0,16 В

Теперь у нас есть показания акселерометра в вольтах, но они все еще не в g (9,8 м / с ^ 2), для окончательного преобразования мы применяем чувствительность акселерометра, обычно выражаемую в мВ / г. Допустим, наша чувствительность = 478,5 мВ / г = 0,4785 В / г. Значения чувствительности можно найти в технических характеристиках акселерометра. Чтобы получить окончательные значения силы, выраженные в g, мы используем следующую формулу:

Rx = DeltaVoltsRx / Чувствительность

Rx = 0,24 В / 0,4785 В / г = ~ 0,5 г Ry = 0,38 В / 0,4785 В / г = ~ 0,79 г Rz = 0,16 В / 0,4785 В / г = ~ 0,33 г.

Конечно, мы могли бы объединить все шаги в одну формулу, но я прошел все шаги, чтобы прояснить, как вы переходите от показаний АЦП к компоненту вектора силы, выраженному в g.

Rx = (AdcRx * Vref / 1023 - VzeroG) / Чувствительность (Eq.2) Ry = (AdcRy * Vref / 1023 - VzeroG) / Чувствительность Rz = (AdcRz * Vref / 1023 - VzeroG) / Чувствительность

Теперь у нас есть все 3 компонента, которые определяют вектор нашей силы инерции. Если на устройство не действуют другие силы, кроме гравитации, мы можем предположить, что это направление вектора нашей силы гравитации. Если вы хотите рассчитать наклон устройства относительно земли, вы можете рассчитать угол между этим вектором и осью Z. Если вас также интересует направление наклона для каждой оси, вы можете разделить этот результат на 2 компонента: наклон по осям X и Y, который можно рассчитать как угол между вектором гравитации и осями X / Y. Вычислить эти углы проще, чем вы думаете, теперь, когда мы рассчитали значения для Rx, Ry и Rz. Вернемся к нашей последней модели акселерометра и сделаем несколько дополнительных обозначений:

Нас интересуют углы между осями X, Y, Z и вектором силы R. Мы определим эти углы как Axr, Ayr, Azr. Вы можете заметить из прямоугольного треугольника, образованного R и Rx, что:

cos (Axr) = Rx / R, и аналогично: cos (Ayr) = Ry / R cos (Azr) = Rz / R

Из уравнения 1 можно вывести, что R = SQRT (Rx ^ 2 + Ry ^ 2 + Rz ^ 2).

Теперь мы можем найти наши углы, используя функцию arccos () (обратная функция cos ()):

Axr = arccos (Rx / R) Ayr = arccos (Ry / R) Azr = arccos (Rz / R)

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

Но прежде чем мы это сделаем, давайте сделаем еще несколько полезных обозначений:

cosX = cos (Axr) = Rx / R cosy = cos (Ayr) = Ry / R cosZ = cos (Azr) = Rz / R

Этот триплет часто называют косинусом направления, и он в основном представляет собой единичный вектор (вектор с длиной 1), который имеет то же направление, что и наш вектор R. Вы легко можете убедиться, что:

КОРЕНЬ (cosX ^ 2 + cosy ^ 2 + cosZ ^ 2) = 1

Это хорошее свойство, так как оно освобождает нас от контроля модуля (длины) вектора R. Часто, если нас просто интересует направление нашего инерционного вектора, имеет смысл нормализовать его модуль, чтобы упростить другие вычисления.

Шаг 2: гироскоп

Мы не собираемся вводить какую-либо эквивалентную коробчатую модель для гироскопа, как мы это сделали для акселерометра, вместо этого мы собираемся сразу перейти ко второй модели акселерометра и показать, что гироскоп измеряет в соответствии с этой моделью.

Каждый канал гироскопа измеряет вращение вокруг одной из осей. Например, 2-осевой гироскоп будет измерять вращение вокруг (или некоторые могут сказать «около») осей X и Y. Чтобы выразить это вращение в числах, сделаем несколько обозначений. Сначала давайте определимся:

Rxz - проекция вектора силы инерции R на плоскость XZ. Ryz - проекция вектора силы инерции R на плоскость YZ.

Из прямоугольного треугольника, образованного Rxz и Rz, используя теорему Пифагора, получаем:

Rxz ^ 2 = Rx ^ 2 + Rz ^ 2, и аналогично: Ryz ^ 2 = Ry ^ 2 + Rz ^ 2

также обратите внимание, что:

R ^ 2 = Rxz ^ 2 + Ry ^ 2, это может быть получено из уравнения 1 и выше, или это может быть получено из прямоугольного треугольника, образованного R и Ryz R ^ 2 = Ryz ^ 2 + Rx ^ 2

Мы не собираемся использовать эти формулы в этой статье, но полезно отметить взаимосвязь между всеми значениями в нашей модели.

Вместо этого мы собираемся определить угол между осью Z и векторами Rxz, Ryz следующим образом:

Axz - это угол между Rxz (проекция R на плоскость XZ) и осью Z Ayz - это угол между Ryz (проекция R на плоскость YZ) и осью Z

Теперь мы приближаемся к тому, что измеряет гироскоп. Гироскоп измеряет скорость изменения углов, определенных выше. Другими словами, он выведет значение, которое линейно связано со скоростью изменения этих углов. Чтобы объяснить это, давайте предположим, что мы измерили угол поворота вокруг оси Y (это был бы угол Axz) в момент времени t0, и мы определяем его как Axz0, затем мы измерили этот угол в более позднее время t1, и это был Axz1. Скорость изменения будет рассчитана следующим образом:

RateAxz = (Axz1 - Axz0) / (t1 - t0).

Если выразить Axz в градусах, а время в секундах, то это значение будет выражено в градусах / с. Это то, что измеряет гироскоп.

На практике гироскоп (если это не специальный цифровой гироскоп) редко дает вам значение, выраженное в градусах / с. Как и для акселерометра, вы получите значение АЦП, которое вам нужно будет преобразовать в град / с, используя формулу, аналогичную уравнению. 2, который мы определили для акселерометра. Давайте представим формулу преобразования АЦП в градусы / с для гироскопа (мы предполагаем, что мы используем 10-битный модуль АЦП, для 8-битного АЦП замените 1023 на 255, для 12-битного АЦП замените 1023 на 4095).

RateAxz = (AdcGyroXZ * Vref / 1023 - VzeroRate) / Чувствительность уравнение 3 RateAyz = (AdcGyroYZ * Vref / 1023 - VzeroRate) / Чувствительность

AdcGyroXZ, AdcGyroYZ - получаются из нашего модуля adc, и они представляют каналы, которые измеряют вращение проекции вектора R в XZ соответственно в плоскостях YZ, что эквивалентно тому, что вращение было выполнено вокруг осей Y и X соответственно.

Vref - это опорное напряжение АЦП, которое мы будем использовать в приведенном ниже примере. 3,3 В. VzeroRate - это напряжение с нулевой скоростью, другими словами, напряжение, которое выдает гироскоп, когда он не подвержен никакому вращению, для платы Acc_Gyro это например 1,23 В (вы можете найти эти значения в спецификациях) Чувствительность - это чувствительность вашего гироскопа, она выражается в мВ / (град / с), часто записывается как мВ / град / с, в основном это говорит вам, сколько мВ будет выход гироскопа увеличится, если вы увеличите скорость вращения на один град / с. Чувствительность платы Acc_Gyro составляет, например, 2 мВ / град / с или 0,002 В / град / с.

Давайте возьмем пример, предположим, что наш модуль ADC вернул следующие значения:

AdcGyroXZ = 571 AdcGyroXZ = 323

Используя приведенную выше формулу и используя параметры спецификации платы Acc_Gyro, мы получим:

RateAxz = (571 * 3,3 В / 1023 - 1,23 В) / (0,002 В / град / с) = ~ 306 град / с RateAyz = (323 * 3,3 В / 1023 - 1,23 В) / (0,002 В / град / с) = ~ -94 град / с

Другими словами, устройство вращается вокруг оси Y (или мы можем сказать, что оно вращается в плоскости XZ) со скоростью 306 град / с и вокруг оси X (или мы можем сказать, что оно вращается в плоскости YZ) со скоростью - 94 град / с. Обратите внимание, что отрицательный знак означает, что устройство вращается в противоположном направлении от обычного положительного направления. Условно одно направление вращения положительное. Хороший лист спецификаций гироскопа покажет вам, какое направление является положительным, в противном случае вам придется найти его, поэкспериментировав с устройством и отметив, какое направление вращения приводит к увеличению напряжения на выходном контакте. Лучше всего это делать с помощью осциллографа, поскольку как только вы остановите вращение, напряжение снова упадет до нулевого уровня. Если вы используете мультиметр, вам нужно поддерживать постоянную скорость вращения в течение как минимум нескольких секунд и отмечать напряжение во время этого вращения, а затем сравнивать его с напряжением с нулевой скоростью. Если оно больше нулевого напряжения, это означает, что направление вращения положительное.

Шаг 3: объединение акселерометра и гироскопа

Собираем все вместе - объединяем данные акселерометра и гироскопа

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

Первым шагом в использовании комбинированного устройства IMU, которое объединяет акселерометр и гироскоп, является выравнивание их систем координат. Самый простой способ сделать это - выбрать систему координат акселерометра в качестве опорной системы координат. В большинстве таблиц данных акселерометра отображается направление осей X, Y, Z относительно изображения физического чипа или устройства. Например, вот направления осей X, Y, Z, как показано в спецификациях для платы Acc_Gyro:

Следующие шаги:

Определите выходы гироскопа, которые соответствуют значениям RateAxz, RateAyz, обсужденным выше. Определите, нужно ли эти выходы инвертировать из-за физического положения гироскопа относительно акселерометра

Не думайте, что если гироскоп имеет выход, помеченный X или Y, он будет соответствовать любой оси в системе координат акселерометра, даже если этот выход является частью блока IMU. Лучше всего это проверить. Предположим, вы зафиксировали положение гироскопа относительно акселерометра. Предполагается, что границы гироскопа и акселерометра параллельны друг другу, то есть вы размещаете гироскоп под углом, кратным 90 градусам, относительно микросхемы акселерометра. Если вы приобрели правление IMU, скорее всего, они уже настроены таким образом. Мы не собираемся обсуждать в этой статье модели, в которых гироскоп расположен под неправильным углом относительно акселерометра (скажем, 45 или 30 градусов), хотя это может быть полезно в некоторых приложениях.

Вот пример последовательности для определения того, какой выходной сигнал гироскопа соответствует значению RateAxz, описанному выше.

- начните с размещения устройства в горизонтальном положении. Оба выхода акселерометра X и Y будут выдавать нулевое напряжение (например, для платы Acc_Gyro это 1,65 В).

- затем начните вращать устройство вокруг оси Y, иначе говоря, вы поворачиваете устройство в плоскости XZ, так что выходы акселерометров X и Z изменяются, а выход Y остается постоянным. - при вращении устройства с постоянной скоростью обратите внимание на то, какой выход гироскопа изменяется, другие выходы гироскопа должны оставаться постоянными - выход гироскопа, который изменился во время вращения вокруг оси Y (вращение в плоскости XZ), предоставит входное значение для AdcGyroXZ, из которого мы вычисляем RateAxz - последний шаг - убедиться, что направление вращения соответствует нашей модели, в некоторых случаях вам может потребоваться инвертировать значение RateAxz из-за физического положения гироскопа относительно акселерометра - повторите вышеуказанный тест, вращая устройство вокруг ось Y, на этот раз отслеживайте выходное значение X акселерометра (AdcRx в нашей модели). Если AdcRx растет (первые 90 градусов поворота от горизонтального положения), то AdcGyroXZ также должен расти. В противном случае вам нужно инвертировать RateAxz, вы можете добиться этого, введя знаковый фактор в уравнение 3, как показано ниже:

RateAxz = InvertAxz * (AdcGyroXZ * Vref / 1023 - VzeroRate) / Sensitivity, где InvertAxz равно 1 или -1

То же самое испытание можно провести для RateAyz, вращая устройство вокруг оси X, и вы можете определить, какой выход гироскопа соответствует RateAyz, и нужно ли его инвертировать. Получив значение InvertAyz, вы должны использовать следующую формулу для расчета RateAyz:

RateAyz = InvertAyz * (AdcGyroYZ * Vref / 1023 - VzeroRate) / Чувствительность

Если вы проведете эти тесты на плате Acc_Gyro, вы получите следующие результаты:

- выходной контакт для RateAxz - GX4 и InvertAxz = -1. - выходной контакт для RateAyz - GY4 и InvertAyz = -1

С этого момента мы будем считать, что вы настроили свой IMU таким образом, чтобы вы могли рассчитывать правильные значения для Axr, Ayr, Azr (как определено в Части 1. Акселерометр) и RateAxz, RateAyz (как определено в Части 2. Гироскоп.). Далее мы проанализируем отношения между этими значениями, которые окажутся полезными для получения более точной оценки наклона устройства относительно плоскости заземления.

К этому моменту вы можете спросить себя, если модель акселерометра уже дала нам углы наклона Axr, Ayr, Azr, зачем нам беспокоиться о данных гироскопа? Ответ прост: данным акселерометра не всегда можно доверять на 100%. Есть несколько причин, помните, что акселерометр измеряет силу инерции, такая сила может быть вызвана гравитацией (а в идеале только гравитацией), но она также может быть вызвана ускорением (движением) устройства. В результате, даже если акселерометр находится в относительно стабильном состоянии, он по-прежнему очень чувствителен к вибрации и механическому шуму в целом. Это основная причина, по которой большинство систем IMU используют гироскоп для сглаживания любых ошибок акселерометра. Но как это сделать? И свободен ли гироскоп от шума?

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

На следующих этапах я представлю алгоритм, вдохновленный некоторыми идеями, использованными в фильтре Калмана, однако он намного проще и легче реализовать на встроенных устройствах. Перед этим давайте сначала посмотрим, что мы хотим, чтобы наш алгоритм вычислял. Ну, это направление вектора силы гравитации R = [Rx, Ry, Rz], из которого мы можем получить другие значения, такие как Axr, Ayr, Azr или cosX, cosy, cosZ, которые дадут нам представление о наклоне нашего устройства. относительно плоскости земли мы обсуждаем взаимосвязь между этими значениями в Части 1. Можно сказать - разве у нас уже нет этих значений Rx, Ry, Rz из уравнения 2 в Части 1? Ну да, но помните, что эти значения получены только из данных акселерометра, поэтому, если вы собираетесь использовать их непосредственно в своем приложении, вы можете получить больше шума, чем может выдержать ваше приложение. Чтобы избежать дальнейшей путаницы, давайте переопределим измерения акселерометра следующим образом:

Racc - вектор инерционной силы, измеренный акселерометром, который состоит из следующих составляющих (проекций на оси X, Y, Z):

RxAcc = (AdcRx * Vref / 1023 - VzeroG) / Чувствительность RyAcc = (AdcRy * Vref / 1023 - VzeroG) / Чувствительность RzAcc = (AdcRz * Vref / 1023 - VzeroG) / Чувствительность

Пока у нас есть набор измеренных значений, которые мы можем получить исключительно из значений АЦП акселерометра. Мы назовем этот набор данных «вектором» и будем использовать следующие обозначения.

Racc = [RxAcc, RyAcc, RzAcc]

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

Обратите внимание: поскольку Racc измеряет силу гравитации, вы будете правы, если предположите, что длина этого вектора, определенного следующим образом, равна или близка к 1g.

| Racc | = КОРЕНЬ (RxAcc ^ 2 + RyAcc ^ 2 + RzAcc ^ 2), Однако для уверенности имеет смысл обновить этот вектор следующим образом:

Racc (нормализованный) = [RxAcc / | Racc |, RyAcc / | Racc |, RzAcc / | Racc |].

Это гарантирует, что длина вашего нормализованного вектора Racc всегда равна 1.

Далее мы представим новый вектор и назовем его

Отдых = [RxEst, RyEst, RzEst]

Это будет выход нашего алгоритма, это скорректированные значения, основанные на данных гироскопа и на основе прошлых оценочных данных.

Вот что будет делать наш алгоритм: - акселерометр сообщает нам: «Вы сейчас находитесь в позиции Racc» - мы говорим «Спасибо, но позвольте мне проверить», - затем исправьте эту информацию с помощью данных гироскопа, а также прошлых данных отдыха и выводим новый оценочный вектор Rest. - мы считаем Rest нашим "лучшим выбором" относительно текущего положения устройства.

Посмотрим, как заставить его работать.

Мы начнем нашу последовательность, доверив нашему акселерометру и назначив:

Отдых (0) = Racc (0)

Кстати, помните, что Rest и Racc - векторы, поэтому приведенное выше уравнение - это просто простой способ написать 3 набора уравнений и избежать повторения:

RxEst (0) = RxAcc (0) RyEst (0) = RyAcc (0) RzEst (0) = RzAcc (0)

Затем мы будем проводить регулярные измерения с равными интервалами времени T секунд, и мы получим новые измерения, которые мы определим как Racc (1), Racc (2), Racc (3) и так далее. Мы также будем выпускать новые оценки через каждый временной интервал Отдых (1), Отдых (2), Отдых (3) и так далее.

Предположим, мы на шаге n. У нас есть два известных набора значений, которые мы хотели бы использовать:

Rest (n-1) - наша предыдущая оценка, с Rest (0) = Racc (0) Racc (n) - наше текущее измерение акселерометра.

Прежде чем мы сможем вычислить Rest (n), давайте введем новое измеренное значение, которое мы можем получить из нашего гироскопа и предыдущей оценки.

Мы назовем его Rgyro, и это тоже вектор, состоящий из 3-х компонентов:

Rgyro = [RxGyro, RyGyro, RzGyro]

Мы будем вычислять этот вектор по одному компоненту за раз. Начнем с RxGyro.

Давайте начнем с наблюдения следующего соотношения в нашей модели гироскопа, из прямоугольного треугольника, образованного Rz и Rxz, мы можем вывести это:

tan (Axz) = Rx / Rz => Axz = atan2 (Rx, Rz)

Atan2 может быть функцией, которую вы никогда раньше не использовали, она похожа на atan, за исключением того, что возвращает значения в диапазоне (-PI, PI) в отличие от (-PI / 2, PI / 2), возвращаемых atan, и требует 2 аргумента вместо одного. Это позволяет нам преобразовывать два значения Rx, Rz в углы в полном диапазоне 360 градусов (-PI в PI). Вы можете узнать больше об atan2 здесь.

Итак, зная RxEst (n-1) и RzEst (n-1), мы можем найти:

Axz (n-1) = atan2 (RxEst (n-1), RzEst (n-1)).

Помните, что гироскоп измеряет скорость изменения угла Axz. Таким образом, мы можем оценить новый угол Axz (n) следующим образом:

Axz (n) = Axz (n-1) + RateAxz (n) * T

Помните, что RateAxz можно получить из показаний АЦП гироскопа. Более точная формула может использовать среднюю скорость вращения, рассчитанную следующим образом:

RateAxzAvg = (RateAxz (n) + RateAxz (n-1)) / 2 Axz (n) = Axz (n-1) + RateAxzAvg * T

Таким же образом мы можем найти:

Ayz (n) = Ayz (n-1) + RateAyz (n) * T

Итак, теперь у нас есть Axz (n) и Ayz (n). Куда мы пойдем дальше, чтобы вычесть RxGyro / RyGyro? Из уравнения. 1 мы можем записать длину вектора Rgyro следующим образом:

| Ргиро | = КОРЕНЬ (RxGyro ^ 2 + RyGyro ^ 2 + RzGyro ^ 2)

Кроме того, поскольку мы нормализовали наш вектор Racc, мы можем предположить, что его длина равна 1, и он не изменился после вращения, поэтому относительно безопасно писать:

| Ргиро | = 1

Примем временные более короткие обозначения для расчетов ниже:

х = RxGyro, y = RyGyro, z = RzGyro

Используя приведенные выше отношения, мы можем написать:

х = х / 1 = х / КОРЕНЬ (х ^ 2 + у ^ 2 + z ^ 2)

Разделим числитель и знаменатель дроби на КОРЕНЬ (x ^ 2 + z ^ 2)

x = (x / КОРЕНЬ (x ^ 2 + z ^ 2)) / КОРЕНЬ ((x ^ 2 + y ^ 2 + z ^ 2) / (x ^ 2 + z ^ 2))

Обратите внимание, что x / SQRT (x ^ 2 + z ^ 2) = sin (Axz), поэтому:

х = грех (Axz) / КОРЕНЬ (1 + у ^ 2 / (х ^ 2 + z ^ 2))

Теперь умножьте числитель и знаменатель дроби внутри КОРЕНЬ на z ^ 2.

х = грех (Axz) / КОРЕНЬ (1 + у ^ 2 * z ^ 2 / (z ^ 2 * (x ^ 2 + z ^ 2)))

Обратите внимание, что z / SQRT (x ^ 2 + z ^ 2) = cos (Axz) и y / z = tan (Ayz), поэтому, наконец:

x = sin (Axz) / КОРЕНЬ (1 + cos (Axz) ^ 2 * tan (Ayz) ^ 2)

Возвращаясь к нашим обозначениям, получаем:

RxGyro = sin (Axz (n)) / SQRT (1 + cos (Axz (n)) ^ 2 * tan (Ayz (n)) ^ 2)

таким же образом мы находим, что

RyGyro = sin (Ayz (n)) / SQRT (1 + cos (Ayz (n)) ^ 2 * tan (Axz (n)) ^ 2)

Теперь, наконец, мы можем найти:

RzGyro = Знак (RzGyro) * SQRT (1 - RxGyro ^ 2 - RyGyro ^ 2).

Где Sign (RzGyro) = 1, когда RzGyro> = 0, и Sign (RzGyro) = -1, когда RzGyro <0.

Один простой способ оценить это - взять:

Знак (RzGyro) = Знак (RzEst (n-1))

На практике будьте осторожны, когда RzEst (n-1) близко к 0. В этом случае вы можете полностью пропустить фазу гироскопа и назначить: Rgyro = Rest (n-1). Rz используется в качестве эталона для расчета углов Axz и Ayz, и когда он близок к 0, значения могут переполняться и приводить к плохим результатам. Вы будете работать с большими числами с плавающей запятой, где реализация функций tan () / atan () может быть недостаточно точной.

Итак, давайте подведем итоги того, что у нас есть до сих пор, мы находимся на шаге n нашего алгоритма и вычислили следующие значения:

Racc - текущие показания нашего акселерометра Rgyro - полученные от Rest (n-1) и текущие показания гироскопа

Какие значения мы используем для расчета обновленной оценки Rest (n)? Вы, наверное, догадались, что мы будем использовать оба. Мы будем использовать средневзвешенное значение, чтобы:

Отдых (n) = (Racc * w1 + Rgyro * w2) / (w1 + w2)

Мы можем упростить эту формулу, разделив числитель и знаменатель дроби на w1.

Отдых (n) = (Racc * w1 / w1 + Rgyro * w2 / w1) / (w1 / w1 + w2 / w1)

и после подстановки w2 / w1 = wGyro получаем:

Отдых (n) = (Racc + Rgyro * wGyro) / (1 + wGyro)

В приведенном выше форуме wGyro говорит нам, насколько мы доверяем нашему гироскопу по сравнению с нашим акселерометром. Это значение может быть выбрано экспериментально. Обычно значения от 5 до 20 дают хорошие результаты.

Основное отличие этого алгоритма от фильтра Калмана состоит в том, что этот вес относительно фиксирован, тогда как в фильтре Калмана веса постоянно обновляются на основе измеренного шума показаний акселерометра. Фильтр Калмана нацелен на предоставление вам «лучших» теоретических результатов, тогда как этот алгоритм может дать вам результаты, «достаточно хорошие» для вашего практического применения. Вы можете реализовать алгоритм, который регулирует wGyro в зависимости от некоторых факторов шума, которые вы измеряете, но фиксированные значения будут хорошо работать для большинства приложений.

Мы находимся в одном шаге от получения обновленных оценочных значений:

RxEst (n) = (RxAcc + RxGyro * wGyro) / (1 + wGyro) RyEst (n) = (RyAcc + RyGyro * wGyro) / (1 + wGyro) RzEst (n) = (RzAcc + RzGyro * wGyro) / (1 + wGyro)

Теперь давайте снова нормализуем этот вектор:

R = КОРЕНЬ (RxEst (n) ^ 2 + RyEst (n) ^ 2 + RzEst (n) ^ 2)

RxEst (n) = RxEst (n) / R RyEst (n) = RyEst (n) / R RzEst (n) = RzEst (n) / R

И мы готовы повторить наш цикл снова.

Изначально это руководство появилось на starlino.com, я внес несколько незначительных изменений и повторно разместил его с разрешения. Спасибо Старлино!