- PVSM.RU - https://www.pvsm.ru -
Много времени утекло с того момента, как я стал начинающим «ардуинщиком». После заказа «стартового набора» из поднебесной и поморгал светодиодами, и покрутил сервоприводами, и даже собрал простенькую платформу Гью-Стюарта [1] из кусков упаковочного полиэтилена, скрепок, кнопок и копеечных сервоприводов SG90.
Прошу прощения за качество фото — другого не сохранилось, как и самой платформы — дети сломали.
С этой платформы и началась, может быть даже кому-то интересная история. Управлять платформой с помощью клавиатуры или джойстиком интересно, но управлять ею датчиком положения в пространстве — интереснее вдвойне. Пробуя корректно заставить работать очень известный датчик от TDK — InvenSense MPU-9250, вспомнилась статья [2] уважаемого ObelardO [3] — «Как я делал костюм захвата движений» и затаилась идея в голове повторить этот опыт.
Что из этого получилось, можно увидеть и прочесть под катом.
Перепробовав всевозможные фильтры, свободно доступные в интернете, перерыв github и все общедоступные форумы, я не нашел практически нигде нет корректного кода для Arduino и MPU-9250. Скорее всего я не умею готовить чего-то напутал с математикой фильтров, но ни один из найденных у меня корректно не заработал. Зато после всех изысканий — я смог существенно подтянуть знания в трехмерном евклидовом пространстве, математике кватернионов и бикватернионов.
В качестве визуализации вращений датчика я использовал Blender, потому как буквенно-цифровое представление в кватернионах очень тяжело для человеческого понимания положения датчика в пространстве, уверен что в особых представлениях этот мультимедиа-комбайн на хабре не нуждается.
Моей настольной ОС уже давно (наверное с выходом 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);
Кубик на экране никак не хотел вращаться даже отдаленно напоминая реальные движения датчика в пространстве, движения были очень «шумны» и неестественны.
В конце концов я добрался до «магического» 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 протокол по нескольким для меня причинам:
Так как человеческое тело для Blender со скелетом “Game engine” я просто сгенерировал в MakeHuman и обозвал его “adam”, а для беспроводной сети я использую отдельную точку доступа с внутренней сетью 192.168.50.0/24, то общая схема датчиков имеет следующий вид:
Из этой картинки понятно — каналы для сообщений будут иметь вид “adam/head" / ”adam/thigh_r" / ”adam/hand_l" и т.п., а для подписки на нужные достаточно подписаться у брокера соединений на канал “adam/#". Думаю пояснения требует только один “root” — это вектор перемещения в пространстве одноименной кости, которая задает местоположение центра модели, но для удобства обработки на стороне ПК я перевожу этот вектор в кватернион с нулевой скалярной частью, все остальные — “настоящие” кватернионы, которыми задаются положения костей в пространстве.
Что такое кватернионы и почему лучше использовать их, а не матрицы или углы Эйлера думаю не нужно объяснять лучше меня расскажут некоторые статьи здесь на хабре — поиск в помощь.
Я же опишу некоторые моменты использованию векторов и кватернионов в своем проекте:
Общая схема соединения WeMos D1 Mini + MPU 9250 не содержит ничего необычного и уверен не требует объяснений.
Итак в новый год я заказал себе подарок на сумму аж в $125:
Где-то в середине февраля все мои посылки были доставлены, но, к сожалению, это все хозяйство пролежало на полке до начала марта. Паял наверно неделю или даже больше, выделяя свой обеденный перерыв и/или оставаясь после работы, так как дома не имею паяльной станции.
Кстати Китай — такой Китай, на одной платке D1 была плохо припаяна ESP8266, один датчик 9250 имел КЗ между ножками. Процент брака более 6%, но на мою удачу с помощью фена эти проблемы легко решались.
Фото промежуточное без MPU9250.
И вот вопреки благодаря COVID-19 за счет удаленной работы и экономии примерно 3-х часов в день на дороге туда-обратно смог более плотно приступить к финальной сборке.
При походе в строймаг были куплены:
20-ти метровая бобина черного ремня 30мм. — цена 155 грн.
Пряжки для ремня D30мм черные 2 шт. в количестве 8 уп. — цена 38,50 грн/уп.
И началось:
.
Мне почему-то кажется что это лучше один раз увидеть чем тысячу раз прочесть.
Нужно сделать несколько пояснений:
Сейчас в разработке собственная платка, где все компоненты будут расположены на площади 34х50 мм (примерный размер используемых аккумуляторов). Также планирую добавить на нее более удобную кнопку Reset и RGB-светодиод для сигнализации (работа, калибровка, передача данных и т.п.).
От ремней собираюсь отказаться, даже если купить тканевую эластичную ленту и в местах соединений сделать “липучку” думаю все равно будет неудобно — и долго одевать, и снимать. Есть идея пошить что-то наподобие компрессионного костюма для тренировок с карманами для датчиков, благо моя свояченица — профессиональный модельер. Это еще более увеличит удобство одевания и снятия, так как все датчики находятся в доступных местах их легко вытащить и вложить из/в кармана, появится возможность стирать костюм и надевать другую одежду сверху.
Думаю применить полученный опыт и наработки в VR — было бы неплохо совместить отслеживание движений и получения ускорений от тела, таким образом использовать тело как геймпад. В этом случае думаю нужна адаптация под конкретную платформу — вот кстати очень интересный концепт SilverCordVR [5] нужно написать ребятам. Я думаю возможно убрать эту ситуацию неестественного движения вперед-назад/влево-вправо как будто рикша.
Автор: Владимир Харлашкин
Источник [6]
Сайт-источник PVSM.RU: https://www.pvsm.ru
Путь до страницы источника: https://www.pvsm.ru/python/352080
Ссылки в тексте:
[1] платформу Гью-Стюарта: https://ru.wikipedia.org/wiki/%D0%9F%D0%BB%D0%B0%D1%82%D1%84%D0%BE%D1%80%D0%BC%D0%B0_%D0%93%D1%8C%D1%8E_%E2%80%94_%D0%A1%D1%82%D1%8E%D0%B0%D1%80%D1%82%D0%B0
[2] статья: https://geektimes.ru/post/273600/
[3] ObelardO: https://habr.com/ru/users/obelardo/
[4] http: https://cloud.google.com/blog/products/iot-devices/http-vs-mqtt-a-tale-of-two-iot-protocols
[5] SilverCordVR: http://silvercord-vr.com/
[6] Источник: https://habr.com/ru/post/370921/?utm_source=habrahabr&utm_medium=rss&utm_campaign=370921
Нажмите здесь для печати.