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

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

Затем в универе показали линейную алгебру и MATLAB. Мне открылись новые возможности, и создать прототип программы удалось очень быстро.
К тому же у меня есть тяга к относительно научному познанию, и через такую работу я могу потренироваться. Текст же поможет структурировать мысли и сохранить математические выкладки.

Описание

Я в процессе разработки электронного девайса с набором датчиков, умеющих измерять ускорения, угловые скорости, магнитные поля, и давление ( 10 степеней свободы). Он обрабатывает эти данные и выдаёт набор чисел, которые отправляются на компьютер по USB. Там их считывает программа, написанная на матлабе и выдаёт картинку из заголовка.
При запуске программа определяет свой наклон при помощи акселерометра, азимут - по магнитометру, затем хитрым образом суммирует повороты по осям, компенсирует дрейф гироскопов и получает набор векторов XYZ.

Цели и задачи

Пока что план такой - написать библиотеку, функции которой будут принимать массив данных с условного датчика, измерять время между приёмами и возвращать необходимые данные:

• трёхмерный базис, векторы которого сонаправлены с датчиком ()

• скорость относительно земли ()

• ускорение относительно земли () и относительно датчика ()

• азимут ()

• высота над уровнем моря или хотя бы над местом старта (alt)

• время между измерениями акселерометра и гироскопа (), магнитометра () и барометра ()

• угловые скорости (, то есть )

• крен () и тангаж () для удобоваримости

Затем подготовлю шаблон для настройки микроконтроллера (МК) и сделаю библиотеку для работы с конкретной моделью датчика.

Электронная начинка

Для проверки алгоритмов подготовил элетронную схему с датчиками и микроконтроллером. Главное устройство тут - это модуль 10 DOF IMU sensor (B) от Waveshare, включающий в себя

• 3-x осевой акселерометр (до )

• 3-х осевой гироскоп (до )

• 3-х осевой магнитометр (до мкТл)

• барометр (, т.е м)

• термометр (даже)

Считыванием и обработкой данных занимается МК STM32F103C8T6, в народе Blue Pill. Работает на 72 МГц и не имеет ускорителя вычислений с плавающей точкой (FPU), поэтому считает относительно медленно. Он собирает по I²C данные с акселерометра и гироскопа каждые 4500 мкc, а магнитометра - 15000 мкс. С барометром я пока не работал. Результаты вычислений отправляются на ПК через USB-UART конвертер, где их ловит матлаб и выводит их на экран в виде векторов и индикаторов перегрузки.

Алгоритм работы и программная реализация

Перед описанием алгоритма расскажу как и что я назвал. Как рассказал выше, датчик выдаёт такой набор величин:

• ускорение ()

• давление (P)

• угловые скорости

• магнитная индукция

Также измеряются .

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

Частым термином будет базис (поправьте, если правильно называть иначе). Под ним я подразумеваю три ортонормированных вектора датчика, как-то ориентированных в пространстве. Например, направлен вверх, - на север. Получится единичная матрица матрица. Если же смотрит на запад, получится так:

Также часто будут использоваться матрицы поворота:
вокруг :

вокруг :

вокруг :

вокруг произвольного :

где

Математика

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

Но возникает проблема. При поступательном ускорении оно складывается с силой тяжести:

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

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

Но мне не нравится представление ориентации в виде трёх (+рысканье) углов, потому что оно понятно только оператору РУ самолётика. Да и сложно представить, что происходит с азимутом, когда нос смотрит вверх, какой угол равен при полёте брюхом кверху - тангаж или крен. И почему. А компьютеру это не объяснить и подавно. Надо бы найти универсальное решение: опуститься к естественному представлению тел в нашем трёхмерном пространстве и обратиться к линейной алгебре.

Так вот. Надо как-то складывать повороты, которые (по крайней мере, в моём представлении) некоммутативные. Однако для малых углов этим можно пренебречь. Поворачиваю я так:

Оказалось, что если перемножать в таком порядке , то базис повернётся вокруг абсолютного вектора . А если наоборот, то вокруг своего .

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

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

Третье - это данность, первое даже не обсуждается. Неверным остаётся второй элемент. Надо его исправить.

Датчик какой-то стороной наклонён к земле, значит надо наклонить его обратно, то есть повернуть вокруг какой-то оси на угол отклонения. К тому же ось должна проходить через 0 и быть нормальной к обоим векторам.
Ось вот такая:

Такой угол:

Такой поворот:

Дабы уменьшить погрешности в будущем, МК перезапоминает силу тяжести:

Чтобы получить оригинальное ускорение, надо вычесть силу тяжести:

Теперь можно вращать датчик и любоваться его отображением в матлабе. Но вот ещё одна проблема. Гироскопы сложно откалибровать, и они очень шумные. К тому же от резких поворотов, у них случаются перегрузы, и все скорости выше порога обрезаются. От этого накапливаются ошибки, и вектора постепенно уплывают. Поэтому необходимо периодически корректировать ориентацию. Делается это описанным выше способом, но чтобы было плавнее, базис доворачивается не на весь угол за раз, а на какую-то подобранную долю угла (в моём случае - 0.02), а функция выполняется при .

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

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

Прототип

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

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

Программа для ПК

Приложение принимает 10 байт, 9 из которых - это координаты трёх векторов, помноженные на 100 (векторы в длину меньше 1, а передавать single precision вчетверо дольше), а последний хранит 2 бита для индикаторов перегруза датчиков.

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

Программа для МК

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

Мне были необходимы:

• переменная, хранящая произвольный набор float чисел и информацию об их порядке

• умножение на число

• сумма двух матриц

• умножение двух матриц

• транспонирование

• определитель

• векторное произведение

• скалярное произведение

• нормирование вектора

• модуль вектора

• чтение элемента

• запись элемента

• создание матрицы

• создание единичной матрицы

• вычисление угла между векторами

• матрицы поворота

Матлабовского должно быть вот что:

• матрица выглядит как одна переменная

• функции проверяют возможность выполнения операции

• результат возвращается в виде числа, либо записывается в переменную, указанную в аргументах

• переменные для результатов не приходится готовить вручную, а функция сама распоряжается памятью для сохранения результата, записывает её параметры матрицы

Матрица у меня - это структура:

struct matrix {
    float *arr;
    uint8_t rows;
    uint8_t cols;
    uint8_t size; // rows*cols чтобы не вычислять каждый раз
};

Типичная функция выглядит так:

int matrixSum(matrix* input1, matrix* input2, matrix* output) {
    if (input1->cols != input2->cols || input1->rows != input2->rows)
        return 1; // проверяю, что размеры совпадают
    if (output->cols != input2->cols || output->rows != input2->rows) { // если размер выходной матрицы не соответствует
        float* tmp = (float*) malloc(sizeof(float) * input1->size); // то создаю новую
        for (int i = 0; i < input1->size; i++) {
            tmp[i] = input1->arr[i] + input2->arr[i];
        }
        matrixRenewArray(output, input1->rows, input1->cols, tmp);
    }
    else {
        for (int i = 0; i < input1->cols * input1->rows; i++) { // или же сразу пишу в выходную
            output->arr[i] = input1->arr[i] + input2->arr[i]; 
        }
    }
    return 0; // всё ок
}

Таким образом, можно на ходу перезаписывать матрицы, не заморачиваясь их размерами.
Я прототипировал в Visual studio на C++. Но у МК память ограничена, а избегать её переполнения я ещё не научился. Поэтому сделал всем матрицам массив по 9 элементов. На удобстве это почти не сказалось, зато могу при компиляции прикинуть количество занятой RAM.

Что касается производительности. Общение с датчиками почти не занимает процессорное время благодаря DMA и прерываниям. В основном цикле нет делэев а есть отсчёт времени между операциями и статусы выполнения. Самым долгим делом пока являются описанные вычисления - до 900 мкс, что при периоде опроса в 4500 мкс оставляет много времени на другие задачи. К тому же речь идёт про один из самых простых МК этой серии. При использовании FPU, вычисления выполняются вдвое быстрее.

Проблемы

Перечислю недостатки, которые я ожидаю.

Корявые вычисления с плавающей точкой

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

Частота измерений датчика и интерполяция

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

Калибровка

Эти микросхемы несовершенные. Датчики могут иметь различную чувствительность, и вообще располагаться не перпендикулярно. А таких датчиков 9. А производитель, видимо, не проводит выходные испытания, и не поставляет покупателю информацию для компенсации. Поэтому придётся придумывать процесс самостоятельно. Поделюсь новостями сразу как появятся результаты.

Планы на будущее

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