Всем привет! В предыдущей статье Позиционирование бионического предплечья взглядом / Хабр (habr.com) мы обсуждали алгоритм для позиционирования бионического предплечья. В этот раз мы пойдем чуть дальше (на один сустав), добавив к предплечью кисть, и рассмотрим алгоритм, с помощью которого мы будем управлять всей конструкцией.
Часть 2 - Вы здесь
Напомню суть алгоритма из предыдущей статьи: через голову по линии глаз проходит плоскость, и дальний конец бионического предплечья стремится встать в позицию максимально близкую к данной плоскости. Если пользователь зафиксирует положение глаз относительно головы и будет менять направление линии взгляда засчет поворотов головы, то окончание предплечья будет находиться всегда в центре поля зрения по вертикали, а удаленность окончания предплечья от головы варьируется разгибанием и сгибанием плеча.
При добавлении еще одного сустава сразу возникает вопрос, как управлять им в связке с предыдущим. Моей первой идеей было перенести принцип, использованный только для предплечья на всю конструкцию: к плоскости взгляда стремится окончание всей конструкции, а позиции промежуточных суставов определяются с помощью инверсной кинематики. На практике такая рука вела бы себя подобно змее, которая извиваясь следует за линией взгляда. Эту идею я решил оставить на потом, когда в моем распоряжении будут более точные сервоприводы и сенсоры, а нынешняя реализация получилась гораздо проще. Предплечье позиционируется по старому алгоритму, но мы в любой момент имеем возможность перейти в режим позиционирования кисти, при котором положение предплечья фиксируется. То есть управление двумя суставами происходит раздельно. Это неидеальное решение, но если понаблюдать за поведением своей руки в реальной жизни, то можно обнаружить, что очень много движений имеют похожий раздельный характер, когда сначала совершается грубое позиционирование предплечья, и уже потом производятся тонкие операции кистью.
Рассмотрим алгоритм позиционирования кисти. После фиксации предплечья мы также фиксируем плоскость взгляда. После этого мы можем наклонять голову вперед и назад и поворачивать ее влево и вправо, меняя углы поворота линии взгляда относительно осей X и Z. Мы хотим, чтобы кисть вращалась вокруг этих же осей глобальной системы координат XYZ. Из соображений удобства и эстетики мы также хотим, чтобы повороты головы были небольшими, но соответствующие движения кисти происходили в более широком диапазоне. На демонстрации в конце статьи можно наблюдать, что повороты головы в режиме управления кистью практически незаметны, тогда как сама кисть разворачивается на большие углы - это достигается зачёт отображения малых диапазонов углов поворота головы в большие диапазоны углов поворота кисти (рабочий диапазон поворота головы для каждой из осей X и Z равен от -8 до 8 градусов, что транслируется в диапазоны от -90 до 90 градусов для кисти). Таким образом из углов поворота головы мы получаем два угла - vX и vZ с диапазонами по 180 градусов. Далее по этим углам мы строим вектор, сонаправленный кисти, в системе координат xyz, связанной с запястьем, то есть окончанием предплечья. И наконец, мы находим углы α и β для сервомоторов в системе координат xyz, которые продуцируют такой вектор.
Теперь приступим к имплементации. Сначала мы рассмотрим аппаратную часть, а затем программную.
Конструкция претерпела некоторые изменения. Во-первых я избавился от драйвера l239d и ручного управления напряжениями на сервомоторе. В прошлый раз я ошибочно решил, что Arduino сломана и генерирует неправильные ШИМ-сигналы, что не позволяло управлять ими сервомотором, но проблема, как оказалось, была в недостаточной силе тока у источника питания. Теперь все три сервомотора подключены напрямую к Arduino, и у них есть отдельная линия питания через dc-dc повышающий конвертер с максимальной силой тока в 4 ампера.
Центральный сервомотор, отвечающий за поворот предплечья, и само предплечье были модифицированы. Во-первых, поскольку на дальнем конце штанги теперь закреплены два сервомотора, пришлось сбалансировать штангу, закрепив на ближнем ее конце груз, чтобы сервомотор предплечья не совершал дополнительную работу против силы тяжест��. Однако это увеличило общий момент инерции, что приводит к осцилляциям мотора вокруг заданного угла (вал доходит до целевого угла, но из-за инерции перелетает через него, потом пытается скорректировать свое положение и снова перелетает через целевой угол уже в обратном направлении и т.д.). Эта проблема была решена добавлением в конструкцию сервомотора импровизированного фрикциона, состоящего из двух пластин, одна из которых закреплена на корпусе мотора, а другая - на валу, и губчатого материала, который обеспечивает трение между пластинами и гасит небольшие колебания.
Оставшиеся два сервомотора смонтированы на дальнем конце штанги последовательно, образуя сустав с двумя степенями свободы. Здесь сразу стоит отметить, что у настоящей кисти есть все три степени свободы (а на самом деле все 6, поскольку суставы имеют очень сложную геометрию и вместе с поворотами происходят и трансляции по всем трем осям, что доставляет массу проблем разработчикам экзоскелетов, но это уже тема для другого разговора), и с двумя степенями свободы мы лишь можем задать продольную ось кисти, но не можем задать поворот вокруг этой оси (попробуйте вытянуть указательный палец и указать им в определенном направлении, а потом сохраняя это направление поворачивать кисть вокруг оси пальца).
В качестве сенсоров все так же выступают два mpu6050, один закрепляется на ремне, который надевается на голову, а второй закреплен на плече. Напомню об особенности данного сенсора - замер угла поворота вокруг оси Z происходит инерционно, то есть интегрированием угловых ускорений по времени, что приводит к дрифту абсолютного значения угла из-за погрешностей в измерениях. По этой причине мы не можем просто использовать абсолютное значение угла, и далее мы посмотрим, как была решена эта проблема.
Последнее, что я хочу рассказать о конструкции - как именно происходит переключение между режимами управления предплечьем и кистью. Тут все тривиально - используется кнопка, по зажатию которой мы переходим в режим управления кистью. В дальнейшем я планирую заменить кнопку на миодатчик, который уже ко мне едет, и использовать сокращения бицепса для управления конструкцией.
Давайте теперь посмотрим на код. Тут я хочу остановиться на двух вещах, которые считаю нетривиальными для понимания, а все остальное можно посмотреть на страничке проекта на на гитхабе.
Для начала рассмотрим как мы получаем из углов поворотов головы углы поворота кисти 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). Есть несколько задумок, куда можно развивать проект. Во-первых как уже было написано выше, хочется наконец добавить миодатчик. Во-вторых, в следующей итерации хочется сделать механизм захвата объектов. В общем, планов большое количество, и не терпится приступить к работе. На этом я с вами прощаюсь, всем спасибо за внимание!
