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

Метод прямоугольников

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

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

y(n) = y(n-1) + x(n-1)*T

где:
n — номер текущего шага;
y(n) — значение интеграла на шаге n;
y(n-1) — значение интеграла на предыдущем шаге n-1;
x(n-1) — значение подынтегральной функции на предыдущем шаге n-1;
T — приращение времени на текущем шаге.

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

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

Метод трапеций

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

Как известно, площадь трапеции со сторонами a и b равна:

S = a*b*h/2

Исходя из этого, разностное уравнение для данного метода интеграции принимает вид:

y(n) = y(n-1) + (x(n-1) + x(n))*T/2

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

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

Физический смысл интеграла

Рассмотрим применение интеграла в классической динамике. Пусть некоторое тело (большой человекоподобный робот) двигается постоянным ускорением a. Другими словами тело двигается равноускоренно (либо равнозамедленно). Следовательно, функцию ускорения от времени можно записать следующим образом:

a(t) = a0

Интегрируя данную функцию по времени t мы получим выражение для функции скорости v от времени t:

v(t) = v0 + a0*t

Еще раз интегрируя полученное выражение мы получим уже функцию расстояния s от t:

s(t) = s0 + v(t)*t = s0 + v0*t + a0*t*t/2

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

Интегрирование показаний гироскопа

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

Пусть угол поворота датчика angle измеряется в градусах (гр), а угловая скорость вращения aspeed в градусах за секунду (гр/сек). Для вычисления интеграла применим метод прямоугольников. Для этого разобьем временную ось на небольшие отрезки по delta секунд в каждом. Таким образом, каждые delta секунд мы будем вычислять площадь очередного прямоугольника и прибавлять полученное значение к уже накопленной площади:

angle = angle + aspeed * delta

Ниже представлен код соответствующей программы для Arduino.

const int gyrPin = A0;
const int INTEGR_DELAY = 20;
const int SERIAL_DELAY = 100;

// Датчик Pololu LPR550AL
const float vref = 3.3;
const float vzero = 1.23;
const float sens = 0.0005;
const float adc = 1023;

int integr_time, serial_time, real_delta;
short gyr_raw;
float angle, aspeed;

void setup() {
    Serial.begin(9600);
    angle = 0;
}

void loop() {
    // Интегрирование скорости поворота гироскопа
    time = millis();
    real_delta = time - integr_time;
    if( real_delta > INTEGR_DELAY ){
        integr_time = time;
        gyr_raw = analogRead(gyrPin);
        aspeed = ((gyr_raw * vref)/adc - vzero)/sens;
        angle = angle + aspeed * real_delta;
    }

    // Отправка угла через последовательный порт на ПК
    time = millis();
    if( time - serial_time > SERIAL_DELAY ){
        serial_time = time;
        Serial.print(angle, 4);
    }
}

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

gyr = (gyr_raw * vdd — gyr_zero) / gyr_sens

где величины vdd, gyr_zero и gyr_sens следует брать из спецификаций используемого гироскопа.

Повышение точности интегрирования

Мы знаем, что точность расчета интеграла тем больше, чем меньше отрезок дискретизации delta. В указанной выше программе, данный временной отрезок delta равен 20мс (INTEGR_DELAY), что в принципе позволяет достаточно сносно решать задачу стабилизации мультикоптера. Как вариант, для увеличения точности мы можем попробовать уменьшить delta, если конечно нам это позволит мощность микроконтроллера.

Либо, мы можем применить другой метод интегрирования, например метод трапеций. В последнем случае программа вычисления угла наклона гироскопа не слишком усложнится и примет следующий вид:

const int gyrPin = A0;
const int INTEGR_DELAY = 20;
const int SERIAL_DELAY = 100;

// Датчик Pololu LPR550AL
const float vref = 3.3;
const float vzero = 1.23;
const float sens = 0.0005;
const float adc = 1023;

int integr_time, serial_time, real_delta;
short gyr_raw;
float angle, old_aspeed, aspeed;

void setup() {
    Serial.begin(9600);
    angle = 0;
    aspeed = 0;
}

void loop() {
    // Интегрирование скорости поворота гироскопа
    time = millis();
    real_delta = time - integr_time;
    if( real_delta > INTEGR_DELAY ){
        integr_time = time;
        gyr_raw = analogRead(gyrPin);
        aspeed = ((gyr_raw * vref)/adc - vzero)/sens;
        angle = angle + (aspeed + old_aspeed) * real_delta / 2;
        old_aspeed = aspeed;
    }

    // Отправка угла через последовательный порт на ПК
    time = millis();
    if( time - serial_time > SERIAL_DELAY ){
        serial_time = time;
        Serial.print(angle, 4);
    }
}

Заключение

Итак, теперь мы умеем интегрировать показания гироскопа и получать угол наклона вокруг его осей. Следовательно, мы можем сделать простейшую систему стабилизации для квадрокоптера на основе одного лишь датчика и платформы Arduino Uno, к примеру. Такой аппарат будет сносно держаться в воздухе, но его будет всегда немного вести в стороны из-за дрейфа нуля у гироскопа. Об этом плохом эффекте и о том, как его победить, я поведаю в следующей статье под названием  «Комплементарный фильтр».