Моя версия костюма отслеживания движений

Предыстория

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

Прошу прощения за качество фото — другого не сохранилось, как и самой платформы — дети сломали.
С этой платформы и началась, может быть даже кому-то интересная история. Управлять платформой с помощью клавиатуры или джойстиком интересно, но управлять ею датчиком положения в пространстве — интереснее вдвойне. Пробуя корректно заставить работать очень известный датчик от TDK — InvenSense MPU-9250, вспомнилась статья уважаемого ObelardO — «Как я делал костюм захвата движений» и затаилась идея в голове повторить этот опыт.

Что из этого получилось, можно увидеть и прочесть под катом.

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

Подключаем Blender к Arduino

Моей настольной ОС уже давно (наверное с выходом Windows 10) является Ubuntu — на тот момент 14.04, сейчас 18.04. Далее все манипуляции буду приводить для этой ОС.
Установка Blender тривиальна и не требует никакого описания, я использую версию 2.79b, так как там есть модуль Blender Game Engine.
Для установки необходимых сторонних библиотек:
pip3 install “нужная библиотека”
После установки pySerial, мы можем читать из порта данные о кватернионе напрямую в Blender и крутить кубик, подключив его следующим образом:

import mathutils
import serial
 
qw = 0.0
qx = 0.0
qy = 0.0
qz = 0.0
 
arduino = serial.Serial()
arduino.baudrate = 115200
arduino.port = '/dev/ttyUSB0'
 
def Port():
    global qw, qx, qy, qz
    global arduino
    data = arduino.readline().strip()
    quat = data.decode('ascii').split(',')
    qw, qx, qy, qz = [float(s) for s in quat]
    print(qw)
    print(qx)
    print(qy)
    print(qz)
 
def Cube():
    global qw, qx, qy, qz
    scene = bge.logic.getCurrentScene()
    cont = bge.logic.getCurrentController()
    obj = cont.owner
    obj['qw'] = qw
    obj['qx'] = qx
    obj['qy'] = qy
    obj['qz'] = qz
    quat_cube = mathutils.Quaternion((obj['qw'], obj['qx'], obj['qy'], obj['qz'])) 
    quat_cube.normalize()
    obj.localOrientation = quat_cube()
 
def Close():
    global arduino
    arduino.close()
 
def Start():
    global arduino
    arduino.open()

Arduino отдает нам кватернион в текстовом виде:
Serial.print(q.w, 4);
Serial.print(",");
Serial.print(q.x, 4);
Serial.print(",");
Serial.print(q.y, 4);
Serial.print(",");
Serial.println(q.z, 4);
Кубик на экране никак не хотел вращаться даже отдаленно напоминая реальные движения датчика в пространстве, движения были очень «шумны» и неестественны.

InvenSense Digital Motion Processor (DMP)

В конце концов я добрался до «магического» Digital Motion Processor (DMP). Плохо искал Оказалось, не существует корректного кода MPU-9250 с DMP для младших моделей Arduino (в моем случае я перепробовал Uno/Mini/Рro Mini с процессором 328p), даже нигде в документации на датчик не указано, что он умеет 9-ти осевые кватернионы (кватернион рассчитывается на показаниях акселерометра, гироскопа и магнитометра). Можно сказать — я немного лукавлю, и кое-где это написано, но кроме фраз «да, умеет» никаких более инструкций и т.п. просто нет. Покупать плату разработчика или более мощную Arduino Zero мне очень не хотелось и я засел за инструкции по датчику, очень много гуглил и искал код. Одним днем удача мне улыбнулась — я нашел на одном корейском сайте реализацию прошивки для процессора 328p и датчика MPU-9250. Если честно, в дальнейшем я не стал разбираться чем именно эта прошивка отличается от других, лежащих в репозиториях на github, но она работала и выдавала 6-ти осевой кватернион из IMU (6-ти осевой — значит, кватернион рассчитывается на показаниях акселерометра и гироскопа).
Попробовав получить данные магнитометра — я получил `NaN`.
То есть цифровой компас не отдавал свои значения в DMP, соответственно этот сопроцессор на датчике его не обрабатывал, но движения были как «небо и земля», по сравнению с другими реализациями.
Ну где наша не пропадала? Изучив на тот момент всю доступную документацию по датчику, множество вариантов всевозможного кода для него и продолжая изучать проблему сразу бросилось в глаза — корейский код DMP написан для датчика MPU-9150, который снят с производства. Откорректировав некоторые значения (магнитометр в датчике другой и имеет другой адрес), я был очень удивлен и поражен, что все заработало. Более того, я не только смог получать данные о магнитном поле, но и сам кватернион стал более стабильным — в режиме покоя на столе практически отсутствует дрифт вокруг оси Z (максимальная проверка проводилась около 10 часов). Некоторое смещение происходило только когда я за шнурок отчаянно крутил датчик, мне кажется — это связано с ограничением в 2g или глюками в работе магнитометра. При работе DMP, когда ускорение датчика превышает некоторый порог и данные магнитометра перестают коррелировать с остальными параметрами, DMP перестает учитывать магнитометр при вычислении кватерниона и он становится из 9-ти осевого, 6-ти осевым. Косвенно это упоминание есть в документе «Motion Driver 6.0 — Features User Guide» от InvenSense, где MPL library каждые 5 сек. проверяет корректность данных от магнитометра и в зависимости от этого использует эти данные в расчетах или нет.

Схема

За основу расположения датчиков на человеке мною была принята следующая картинка:

«Биомеханика» Автор: Владимир Иванович Дубровский
Где светлыми квадратами и цифрами обозначены центры масс частей тела человека, а буквой «S» и черным квадратом центр массы тела человека -итого 15 датчиков — для светлых квадратов и центра массы. Для такого количества датчиков очень напрашивается использование двух I2C 8-ми канальных мультиплексоров (подобное было в костюме упоминаемом в самом начале). Ну а что — дешево и сердито. После некоторых экспериментов я решил использовать связку WeMos D1 mini + MPU-9250 + Battery Shield + MQTT протокол по нескольким для меня причинам:

• Исключение проводного подключения — много соединительных проводов, неизбежные путаница, сложность “одевания и снятия” и возможные электромагнитные наводки.
• Перенос части вычислений с ПК — с одной стороны, ресурсов у любого ПК по сравнению с Arduino более чем достаточно, с другой стороны обработка одного кватерниона одним ESP8266 не сильно ресурсоемкая задача.
• При использовании мультиплексоров я не смог бы перенести математику на микроконтроллер — время считывания данных с датчика около 2 мс, при 15 датчиках это бы заняло 30 мс. Я же использую скорость опроса 50 Гц, т.е. хочу получать новые данные каждые 20 мс, т.е. у меня просто не оставалось бы времени на обработку и корректировку полученных данных.
• Использование протокола MQTT позволит сэкономить полосу пропускания (передаем только необходимые значения когда нужно), к тому же данный протокол быстрее стандартного http.
• Древовидная структура каналов, и правила подписки и публикации сообщений, как по мне более подходит для реализации.
• Возможность передачи дополнительных данных по запросу, кроме кватернионов. Т.е. организация двусторонней связи через сообщения (Start — Stop — Calibration — DeepSleep).
• Универсальность для одного модуля — взаимозаменяемость без перепайки.
• Использование обычной беспроводной сети, обновление «по воздуху» прошивок.

Так как человеческое тело для Blender со скелетом “Game engine” я просто сгенерировал в MakeHuman и обозвал его “adam”, а для беспроводной сети я использую отдельную точку доступа с внутренней сетью 192.168.50.0/24, то общая схема датчиков имеет следующий вид:

Из этой картинки понятно — каналы для сообщений будут иметь вид “adam/head" / ”adam/thigh_r" / ”adam/hand_l" и т.п., а для подписки на нужные достаточно подписаться у брокера соединений на канал “adam/#". Думаю пояснения требует только один “root” — это вектор перемещения в пространстве одноименной кости, которая задает местоположение центра модели, но для удобства обработки на стороне ПК я перевожу этот вектор в кватернион с нулевой скалярной частью, все остальные — “настоящие” кватернионы, которыми задаются положения костей в пространстве.

Немного математики

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

• Процесс калибровки запускаемый с ПК — для этого человек должен принять позу “Т”, с ПК запускается команда для всех датчиков, а каждый датчик запоминает свой сопряженный кватернион. При умножении которого на получаемый мы получаем единичный. Делается это потому что датчики калибруются от 20 до 60 секунд, при этом идет девиация по оси Z.
• Отдельно нужно наверно описать вектор перемещения. В процессе калибровки я беру “идеальный” вектор силы тяжести, поворачиваю его на настоящий кватернион, отнимаю вектор акселерометра датчика от полученного “идеального” в координатах настоящего кватерниона. И создаю вектор корректировки суммой “идеального” и разницы.
• При каком либо перемещении в пространстве, я поворачиваю корректировочный вектор на обратный настоящему, нахожу разницу между этим вектором и вектором акселерометра и поворачиваю на настоящий кватернион. Из полученного вектора и рассчитываю новое положение для кости root.
• Проверка разности кватернионов. Эту функцию используем для понимания есть ли изменения в сравнении с предыдущим отправленным значением или нет. Я не нашел плохо искал нужную функцию, поэтому использовал следующую из английской Википедии — норма разности это расстояние между двумя кватернионами. Т.е. если норма разности менее какого-то значения = кватернионы практически равны.
• Трансляция кватернионов в систему координат кости. Тут все относительно просто — берем векторную часть кватерниона и создаем новый кватернион, где перемещаем векторные части как в Blender и/или если необходимо делаем часть с обратным знаком. Для этого нужно понимать как именно будет расположен датчик на теле и эта часть должна быть “твердым телом” с костью в Blender.Например для “adam/head" такая трансляцию будет (w, y, z, x) (расположение кости z — вперед, x — влево, y — вверх, а датчик расположен x, y, z соответственно и на лбу).
• Для того, чтобы вращения вычисляемые от датчика правильно применялись, нужно знать кватернион кости. Я его поместил сразу в прошивку, но ничего не мешает нам передать его в контроллер при отпарвки команды калибровки (как пример). Чтобы применить вращение от датчика на кости нужно обратный кватернион кости умножить на нужный и затем умножить на прямой.

Сборка

Общая схема соединения WeMos D1 Mini + MPU 9250 не содержит ничего необычного и уверен не требует объяснений.

Итак в новый год я заказал себе подарок на сумму аж в $125:

• WeMos D1 Mini — 16 шт.
• WeMos Battery Shield — 16 шт.
• WeMos Prototype Board — 16 шт.
• GY 9250 — 16 шт.
• Lithium Polymer LiPo Rechargeable Battery JST 2.5mm 1200mAh 3.7V — 15 шт. размерами около 50x34x6мм.

Где-то в середине февраля все мои посылки были доставлены, но, к сожалению, это все хозяйство пролежало на полке до начала марта. Паял наверно неделю или даже больше, выделяя свой обеденный перерыв и/или оставаясь после работы, так как дома не имею паяльной станции.
Кстати Китай — такой Китай, на одной платке D1 была плохо припаяна ESP8266, один датчик 9250 имел КЗ между ножками. Процент брака более 6%, но на мою удачу с помощью фена эти проблемы легко решались.

Фото промежуточное без MPU9250.
И вот вопреки благодаря COVID-19 за счет удаленной работы и экономии примерно 3-х часов в день на дороге туда-обратно смог более плотно приступить к финальной сборке.
При походе в строймаг были куплены:
20-ти метровая бобина черного ремня 30мм. — цена 155 грн.
Пряжки для ремня D30мм черные 2 шт. в количестве 8 уп. — цена 38,50 грн/уп.
И началось:

• отмерить и нарезать ремни.
• установить пряжки на ремни.
• примерить и определить удобство затягивания ремней и соответственно места где будут датчики.
• примотать датчики “синей изолентой”
• каждый подписать, прошить нужной прошивкой и зарядить

.

Итог

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

Нужно сделать несколько пояснений:

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

Послесловие

Сейчас в разработке собственная платка, где все компоненты будут расположены на площади 34х50 мм (примерный размер используемых аккумуляторов). Также планирую добавить на нее более удобную кнопку Reset и RGB-светодиод для сигнализации (работа, калибровка, передача данных и т.п.).
От ремней собираюсь отказаться, даже если купить тканевую эластичную ленту и в местах соединений сделать “липучку” думаю все равно будет неудобно — и долго одевать, и снимать. Есть идея пошить что-то наподобие компрессионного костюма для тренировок с карманами для датчиков, благо моя свояченица — профессиональный модельер. Это еще более увеличит удобство одевания и снятия, так как все датчики находятся в доступных местах их легко вытащить и вложить из/в кармана, появится возможность стирать костюм и надевать другую одежду сверху.
Думаю применить полученный опыт и наработки в VR — было бы неплохо совместить отслеживание движений и получения ускорений от тела, таким образом использовать тело как геймпад. В этом случае думаю нужна адаптация под конкретную платформу — вот кстати очень интересный концепт SilverCordVR нужно написать ребятам. Я думаю возможно убрать эту ситуацию неестественного движения вперед-назад/влево-вправо как будто рикша.

Автор: Владимир Харлашкин

Источник