Всем привет! В предыдущей статье Позиционирование бионического предплечья взглядом / Хабр (habr.com) мы обсуждали алгоритм для позиционирования бионического предплечья. В этот раз мы пойдем чуть дальше (на один сустав), добавив к предплечью кисть, и рассмотрим алгоритм, с помощью которого мы будем управлять всей конструкцией.

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

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

Рассмотрим алгоритм позиционирования кисти. После фиксации предплечья мы также фиксируем плоскость взгляда. После этого мы можем наклонять голову вперед и назад и поворачивать ее влево и вправо, меняя углы поворота линии взгляда относительно осей X и Z. Мы хотим, чтобы кисть вращалась вокруг этих же осей глобальной системы координат XYZ. Из соображений удобства и эстетики мы также хотим, чтобы повороты головы были небольшими, но соответствующие движения кисти происходили в более широком диапазоне. На демонстрации в конце статьи можно наблюдать, что повороты головы в режиме управления кистью практически незаметны, тогда как сама кисть разворачивается на большие углы - это достигается зачёт отображения малых диапазонов углов поворота головы в большие диапазоны углов поворота кисти (рабочий диапазон поворота головы для каждой из осей X и Z равен от -8 до 8 градусов, что транслируется в диапазоны от -90 до 90 градусов для кисти). Таким образом из углов поворота головы мы получаем два угла - vX и vZ с диапазонами по 180 градусов. Далее по этим углам мы строим вектор, сонаправленный кисти, в системе координат xyz, связанной с запястьем, то есть окончанием предплечья. И наконец, мы находим углы α и β для сервомоторов в системе координат xyz, которые продуцируют такой вектор.

Теперь приступим к имплементации. Сначала мы рассмотрим аппаратную часть, а затем программную.

Конструкция претерпела некоторые изменения. Во-первых я избавился от драйвера l239d и ручного управления напряжениями на сервомоторе. В прошлый раз я ошибочно решил, что Arduino сломана и генерирует неправильные ШИМ-сигналы, что не позволяло управлять ими сервомотором, но проблема, как оказалось, была в недостаточной силе тока у источника питания. Теперь все три сервомотора подключены напрямую к Arduino, и у них есть отдельная линия питания через dc-dc повышающий конвертер с максимальной силой тока в 4 ампера.

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

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

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

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

Давайте теперь посмотрим на код. Тут я хочу остановиться на двух вещах, которые считаю нетривиальными для понимания, а все остальное можно посмотреть на страничке проекта на github Wosk1947/Eye_Guide_Bionic_Hand: A prototype for a bionic hand that is being controlled by sight (github.com).

Для начала рассмотрим как мы получаем из углов поворотов головы углы поворота кисти vX и vZ.

const double headPalmThreshold = 8;
const double palmThreshold = 90;

dZ = mpu.getAngleZ() - prevZ;
if (abs(dZ) < 0.08) {dZ = 0;}
prevZ = mpu.getAngleZ();
currentZ += dZ;
currentX = mpu.getAngleX() - originX;
if (currentX < -headPalmThreshold) {
    currentX = -headPalmThreshold;
}
if (currentX > headPalmThreshold) {
    currentX = headPalmThreshold;
}
if (currentZ < -headPalmThreshold || currentZ > headPalmThreshold) {
    currentZ -= dZ;
}
vX = palmThreshold * currentX / headPalmThreshold;
vZ = palmThreshold * currentZ / headPalmThreshold;

Чтобы исключить дрифт по оси Z мы отфильтровываем любые малые изменения dZ, приравнивая их к нулю. Это простое решение, которое показало себя лучше чем Low-Pass фильтр, поскольку при дрифте нежелательные скачки значения угла носят постоянный характер. Далее мы ограничиваем углы поворота головы 8 градусами в каждую сторону и масштабируем результирующий угол до диапазона от -90 до 90 градусов. Данный метод не позволяет использовать абсолютное значение угла, вместо чего мы используем относительные изменения угла, то есть центр (vZ = 0) со временем будет смещаться (но только когда голова движется), что можно легко скорректировать повернув голову в сторону смещения центра чуть за пределы рабочего диапазона угла в 16 градусов. Решение, опять же, неидеальное, но на практике практически не мешает работе с устройством.

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

void palmMotorAngles(BLA::Matrix<4,4> headRotation, BLA::Matrix<4,4> armRotation, double vX, double vZ, double* angles) {
  rVH = headRotation * eulerAnglesToMatrix(vX, 0, vZ, EEulerOrder::ORDER_ZYX);                              
  getTranslation(Inverse(armRotation) * rVH * palmForward, vPalm);
  if (vPalm[1] < 0) {
    vPalm[1] = 0;
    double lenMultiplier = 1 / sqrt(vPalm[0] * vPalm[0] + vPalm[2] * vPalm[2]);
    vPalm[0] = vPalm[0] * lenMultiplier;
    vPalm[2] = vPalm[2] * lenMultiplier;
  } 
  beta = asin(vPalm[2]); 
  if (cos(beta) != 0) {
    double s = vPalm[0] / cos(beta);
    if (s > 1) {
      s = 1;
    }
    if (s < -1) {
      s = -1;
    }
    alpha = -asin(s);
  }        
  angles[0] = beta * 180 / PI; 
  angles[1] = alpha * 180 / PI;
}

В качестве аргументов мы принимаем матрицы поворота головы headRotation и поворота предплечья armRotation, которые мы зафиксировали в момент перехода из режима позиционирования предплечья в режим позиционирования кисти, углы vX и vZ и массив angles, куда мы будем записывать результат.

Матрица rVH является матрицей поворота кисти в глобальной системе координат. Чтобы получить матрицу поворота кисти в системе координат запястья нужно умножить матрицу rVH слева на матрицу, обратную матрице armRotation. После этого домножим результирующую матрицу на единичный вектор palmForward, извлечем компоненты трансляции и получим вектор кисти vPalm в системе координат запястья.

Далее мы должны ограничить этот вектор только положительными значениями y. Связано это с тем, что у сервомоторов sg90 рабочий диапазон составляет 180 градусов, то есть наша кисть сможет двигаться в пределах полусферы, направленной вдоль оси предплечья. Если вектор выходит из этой полусферы, то мы строим новый вектор, который является проекцией оригинала на ее основание. Наконец, из вектора мы находим углы α и β его поворота в системе координат запястья. Отдельно надо уточнить, как мы решаем проблему с gimbal lock, которая возникает в случае если угол β равен +90 и -90. Решение, опять же, тривиальное - в этом случае мы просто не рассчитываем угол α, а используем предыдущее его значение. Если же gimbal lock отсутствует, то мы можем столкнуться с другой проблемой, когда синус, полученный делением двух очень малых чисел, из-за ограниченной их точности может принять значение больше 1 по модулю. В этом случае мы просто искусственно ограничиваем значение до 1.

Для тестов была написана визуализация на Python:

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

Ну и, наконец, давайте посмотрим на всю конструкцию в действии:

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

Заключение

Эта итерация заняла у меня гораздо больше времени, поскольку я столкнулся с очень многими трудностями. Организация питания, балансирование центрального сервомотора. Центральный сервомотор незадолго после испытаний таки вышел из строя - разрушились шестерни редуктора. Также при написании кода я начал упираться в ограничения по памяти у самой Arduino. Для последующих итераций нужно будет переходить уже на более серьезную элементную базу, с более мощными сервомоторами, более точными сенсорами и другим микроконтроллером (в данный момент я думаю в сторону Rapsberry Pi). Есть несколько задумок, куда можно развивать проект. Во-первых как уже было написано выше, хочется наконец добавить миодатчик. Во-вторых, в следующей итерации хочется сделать механизм захвата объектов. В общем, планов большое количество, и не терпится приступить к работе. На этом я с вами прощаюсь, всем спасибо за внимание!