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

Введение

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

Имплементация

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

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

В процессе реализации алгоритм претерпел некоторые изменения. Я ошибся при заказе гироскопов и вместо mpu9250 заказал mpu6050. Отличие между ними в том, что у mpu9250 на борту есть магнитометр, то есть поворот вокруг оси Z вычисляется относительно внешнего магнитного поля, тогда как у mpu6050 вычисление поворота вокруг оси Z является инерционным и быстро приводит к дрифту, что сразу убирает из изначального алгоритма возможность точного вычисления линии взгляда. Поэтому было принято решение изменить алгоритм – вместо линии взгляда мы будем оперировать плоскостью, в которой находятся глаза пользователя и линия взгляда, а манипулятор стремится к ближайшей в данный момент времени точке на данной плоскости. Это позволит выставить манипулятор на правильную высоту, а движение по горизонтали уже будет обеспечиваться вращением плеча и корпуса вокруг оси Z.

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

На схеме выше представлена геометрическая модель устройства. Плечо длиной a₁ соединено с корпусом и предплечьем длиной a₂. Угол поворота предплечья относительно плеча равен θ. Плоскость S (A, B, C, D) с вектором нормали N (A, B, C) проходит через глаза, повторяя наклон головы. В этой схеме у нас есть одно неизвестное, которое нам надо найти – угол θ. Для этого нам надо решить обратную задачу кинематики. Поворот плеча относительно корпуса описывается матрицей поворота R₁, трансляция локтевого сустава относительно плечевого сустава описывается матрицей трансляции T₁, поворот предплечья относительно плеча описывается матрицей поворота R₂, трансляция дальней оконечности предплечья относительно локтевого сустава описывается матрицей T₂. Перемножая эти матрицы и взяв из итоговой матрицы первые три компоненты из крайнего правого столбца мы получаем радиус-вектор дальней оконечности предплечья, который зависит от угла θ.

Запишем формулу для расстояния между данной точкой и плоскостью S

Нам нужно рассмотреть два случая.

Первый: d = 0, то есть точка находится строго на плоскости.

Второй: d ≠ 0, то есть точка ни при каком угле θ не может находиться на плоскости.

Рассмотрим первый случай, d = 0

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

Осуществим универсальную тригонометрическую подстановку. Здесь нам нужно также рассмотреть два случая. В первом случае, если угол не равен нечетному количеству π, мы упрощаем выражение, сводим его к квадратному уравнению, находим его корни и получаем выражение для угла θ (2). Во втором случае, если угол равен нечетному количеству π, мы находим равенство (3), при выполнении которого θ будет равен π, 3π, 5π …, то есть, исходя из геометрии задачи, где угол θ может принимать значения от 0 до π (от полностью прямой руки до полностью согнутой), просто π.

Осталось рассмотреть случай, где d ≠ 0, то есть точка ни при каком угле θ не может находиться на плоскости. В таком случае мы хотим найти такой угол θ, при котором расстояние до плоскости будет минимальным. Для этого мы возьмем производную от выражения (1) по θ, приравняем производную к нулю и получим выражение (4) для угла θ

Таким образом мы нашли все необходимые выражения для угла θ при различных условиях. Перейдем к их имплементации.

Устройство построено на основе платы Arduino Nano. К Arduino через драйвер l239d подключен сервопривод с встроенным потенциометром, замеряющим угол поворота. Привод осуществляет поворот штанги, имитирующей предплечье, вокруг сервопривода смонтированы стопоры, ограничивающие движение. Также к Arduino подключены два гироскопа mpu6050 – один смонтирован на ремне, надевающимся на голову, и измеряющий ее углы поворота, второй расположен на самом устройстве и измеряет углы поворота плеча. Вся конструкция смонтирована на основе, которая присоединяется к плечу ремнями. В качестве источника питания для сервопривода используются 4 АА батареи, подключенных последовательно, и выдающих 6V, для питания платы и гироскопов используется внешний источник 5V.

При сборке устройства возникли трудности с сервоприводом, о которых хотелось бы упомянуть. Сервопривод SG90 управляется ШИМ-сигналами, то есть последовательностями коротких импульсов. В ходе исследования вопроса мне так и не удалось понять, умеет ли Arduino без сторонних микроконтроллеров генерировать эти сигналы или нет, и если умеет, то может ли быть так, что ШИМ, генерируемый ею слишком шумный для управления. Надеюсь, с этими вопросами мне помогут в комментариях. По итогу мне не удалось управлять SG90 напрямую с Arduino, поэтому было принято решение удалить из него его родной микроконтроллер, вывести наружу провода от мотора и потенциометра и управлять мотором через драйвер l239d. Была произведена калибровка потенциометра замером углов поворота вала и снятием значения сигнала на потенциометре:

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

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

Демонстрация

Для демонстрации работы с устройством я записал короткий ролик. В нем представлены три сценария, в первом нужно быстро дотянуться до нескольких объектов, расположенных на разных высотах, во втором – перехватить объект, находящийся в свободном падении, в третьем – дотянуться до объектов, сидя за столом. Последний сценарий примечателен тем, что протез может находиться в двух режимах – в состоянии покоя, когда предплечье полностью разогнуто и не движется в соответствии с алгоритмом, и в состоянии “наведения”, когда предплечье активно движется. Переход между двумя режимами осуществляется при определенном угле между плечом и вертикальной осью.

Заключение

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

Всем спасибо за внимание!