Разработка hexapod с нуля (часть 12) — новое ядро передвижения

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

Также в гексаподе появилась стабилизация тела относительно горизонта на базе MPU6050. Прошивка сама компенсирует углы наклона во время движения — в будущем это очень пригодится, когда я буду реализовывать адаптацию к неровностям. В этом направлении уже ведутся разработки (датчики касания на базе тензорезисторов), настало время для следующего шага.

В этой статье расскажу, насколько простая может быть математика ядра передвижения гексапода и какие красивые движения можно выполнять с помощью неё. Разработка продолжается, и я переписал около 80% математики. Это позволило выкинуть явное указание координат точек назначения во время движения — траектории теперь строятся в реальном времени. Все технические подробности в статье. Как всегда, вас ждёт фото и видео.

Github AIWM Hexapod

▍ Введение

А почему ядро вообще нужно было переписывать? Просто посмотрите на это:

{0}, // Destination points is not use for TRAJECTORY_XZ_ADV_Y_CONST
{ TRAJECTORY_XZ_ADV_Y_CONST, TRAJECTORY_XZ_ADV_Y_SINUS, TRAJECTORY_XZ_ADV_Y_CONST, TRAJECTORY_XZ_ADV_Y_SINUS, TRAJECTORY_XZ_ADV_Y_CONST, TRAJECTORY_XZ_ADV_Y_SINUS},
{ TIME_DIR_DIRECT, TIME_DIR_REVERSE, TIME_DIR_DIRECT, TIME_DIR_REVERSE, TIME_DIR_DIRECT, TIME_DIR_REVERSE },
{{-115, LIMB_DOWN_Y, 70}, {-135, LIMB_DOWN_Y, 0}, {-115, LIMB_DOWN_Y, -70}, {115, LIMB_DOWN_Y, 70}, {135, LIMB_DOWN_Y, 0}, {115, LIMB_DOWN_Y, -70}},
.motion_time = MTIME_MID_VALUE, .time_stop = MTIME_MAX_VALUE, .time_update = MTIME_NO_UPDATE, .speed = 0

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

Сам алгоритм передвижения остался без изменений. Я его описывал в 8 части. Давайте напомню его параметры:

• curvature — степень кривизны траектории [-1000; 1000];
• distance — длина шага. Границы определяются конфигурацией корпуса. В моём случае это [-115; 115], т.е. максимальный шаг — это 11.5 см;
• speed — скорость передвижения [0; 100].

В конце мы посмотрим во что в итоге это раздуется.

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

▍ Pre-build

Начнём с некоторых вводных данных. У нас есть «базовые точки» и их координаты нам известны. Это единственные координаты, которые жёстко вписаны в прошивке. Именно в эти точки гексапод ставит свои ноги после подачи питания. Координаты зависят от физических параметров гексапода (размеры ног, расстояние от центра и прочее). В моём случае это:

static const v3d_t limbs_base_pos[] = {
    {-115, 0, 70}, {-135, 0, 0}, {-115, 0, -70}, // Left side
    { 115, 0, 70}, { 135, 0, 0}, { 115, 0, -70}  // Right side
};

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

Напомню, почему это должно быть так. Алгоритм отталкивается от центра траектории, серые векторы как раз и указывают на базовые точки. Стоит изменить базовую точку и всё рухнет.

▍ С чего всё началось

Хм, описать движение гексапода относительно земли довольно сложно, но что если зайти с другой стороны? Давайте попробуем описать движение земли относительно гексапода.

Есть земля\пол\асфальт\неважно (пусть будет земля) и есть гексапод. Гексапод встал на ноги и своим весом продавил ямки на земле, т.е. мы просто опускаем проекцию базовых точек на землю. Убираем гексапода и что остаётся?


Мы получаем шесть точек и наше первое уравнение — уравнение плоскости:

В этом уравнении фигурируют 2 сущности:

• (x0,y0,z0) — точка, которая лежит на этой плоскости;
• (nx,ny,nz) — вектор, перпендикулярный плоскости (нормаль).

Это и будут наши новые рычаги воздействия. Для простоты примем, что точка и начало вектора находятся в одном месте. Опустим пока точку и обратим внимание на нормаль. Что будет, если её повернуть по одной из осей? Разумеется, плоскость будет поворачиваться за вектором, они ведь должны быть перпендикулярны.


Хорошо. Теперь добавим условного гексапода в виде конечностей и центра корпуса:


И повернём плоскость на несколько градусов:


Поскольку изначальные координаты X и Z базовых точек нам известны, то используя уравнение плоскости, можно без проблем вычислить их смещение по оси Y. Используя новое значение Y, мы перемещаем все ноги разом в нужную позицию.

Тело гексапода при этом зафиксировано в пространстве, координаты базовых точек также не меняются — мы просто двигаем землю и вычисляем высоту проекции базовых точек на неё относительно начального положения. Это можно представить в виде какого-нибудь предмета (пусть будет ложка), фонарика и листа бумаги. Если взять ложку и посветить на неё фонариком сверху, то на листе будет её тень — проекция ложки на плоскость. Если мы будем наклонять лист бумаги, то высота каждой точки проекции будет меняться относительно их начального положения (когда лист лежал ровно), но ложка не двигалась. Понимаете меня, мистер Андерсон?

Уже на данном этапе мы можем описать движение ниже. Наклоняем плоскость по оси Х на 15 градусов и вращаем её вокруг оси Y. Это просто великолепно! Визуально выглядит сложно, но на самом деле мы просто покрутили нормаль. На видео также можно увидеть, что с помощью этого можно реализовать наклоны корпуса.


Теперь пощупаем точку (x0,y0,z0) из уравнения плоскости. Эта точка, через которую проходит плоскость. Если наклоном вектора мы можем управлять наклоном корпуса, то с помощью точки мы сможем управлять его высотой — двигаем землю вверх/вниз.


Это позволяет гексаподу вставать на ноги и изменять высоту тела в процессе движения.


С Y координатой разобрались, но как быть с X и Z? А давайте немного изменим свойство точки, пусть она будет не просто точкой, а центром плоскости. Если вернуться к ложке, то в этом случае направление света фонарика привязано к центру плоскости и перемещая плоскость, мы перемещаем и проекции базовых точек. Давайте я покажу это нагляднее


В нашем случае никаких расчётов не понадобится, т.к. достаточно просто прибавить смещение к базовым точкам. После этого нам открывается возможность перемещать плоскость по осям X и Z — ух, мы опять двигаем землю. И вот какие движения можно получить обычным сдвигом центра плоскости (видео ниже).

Кстати, на одном из видео можно увидеть поведение гексапода, когда движение упёрлось в физические ограничения — теперь он не уходит в ошибку и мигает светодиодами. Это как бы говорит «Парень, что ты творишь со мной?»

А вот что можно получить, комбинируя наклон и сдвиг — гекс теперь умеет описывать параболу. Выглядит здорово.


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

▍ Как это реализовано

Описанный выше инструмент не только простой, но и достаточно мощный. Внутри плоскость не одна, на данный момент их 2: внутренняя и пользовательская. Внутренняя плоскость формируется блоком ориентации (MPU6050) и ядром передвижения (высота корпуса), пользовательская формируется пользователем при помощи приложения для управления гексаподом. Дальше в ядре эти плоскости объединяются в одну путём объединения координат проекций базовых точек на каждую плоскость. Например, если внутренняя плоскость наклонена на 30 градусов, а пользовательская на -30, то гексапод в итоге будет стоять на месте без уклона.


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

А теперь давайте взглянем на реализацию нового ядра передвижения и ответим на вопрос «почему гексапод плавно выполняет свои движения?».

В качестве варианта реализации я взял метод последовательного приближения. У нас есть текущие параметры плоскости (точка и нормаль) и целевые, которые задаёт пользователь. Мы не можем просто взять и переместиться в конечное положение за 1 цикл — это будет очень резко. Давайте перемещать плоскость в нужное положение по шагам каждый цикл.

/// ***************************************************************************
/// @brief  Move surface on step
/// @param  src_p: source surface point pos
/// @param  dst_p: destination surface point pos
/// @param  src_r: source surface rotation
/// @param  dst_r: destination surface rotation
/// @param  max_step: max step for move
/// @return true - surface already reached destination pos, false - otherwise
/// ***************************************************************************
bool mm_move_surface(p3d_t* src_p, const p3d_t* dst_p, r3d_t* src_r, const r3d_t* dst_r, float max_step) {
    float diff[6] = { 0 };
    diff[0] = dst_p->x - src_p->x;
    diff[1] = dst_p->y - src_p->y;
    diff[2] = dst_p->z - src_p->z;
    diff[3] = dst_r->x - src_r->x;
    diff[4] = dst_r->y - src_r->y;
    diff[5] = dst_r->z - src_r->z;

    // Search max diff
    float max_diff_abs = fabs(diff[0]);
    for (int i = 1; i < sizeof(diff) / sizeof(diff[0]); ++i) {
        if (isless(max_diff_abs, fabs(diff[i]))) {
            max_diff_abs = fabs(diff[i]);
        }
    }

    // Move completed?
    if (max_diff_abs < FLT_EPSILON) {
        return true;
    }

    // Constrain step for add remainder
    if (isless(max_diff_abs, max_step)) {
        max_step = max_diff_abs;
    }

    // Add step
    src_p->x += max_step * (diff[0] / max_diff_abs);
    src_p->y += max_step * (diff[1] / max_diff_abs);
    src_p->z += max_step * (diff[2] / max_diff_abs);
    src_r->x += max_step * (diff[3] / max_diff_abs);
    src_r->y += max_step * (diff[4] / max_diff_abs);
    src_r->z += max_step * (diff[5] / max_diff_abs);
    
    // Constrain surface rotation angles value
    if (isgreater(fabs(src_r->x), 360.0f)) {
        src_r->x += -360.0f;
    }
    if (isgreater(fabs(src_r->y), 360.0f)) {
        src_r->y += -360.0f;
    }
    if (isgreater(fabs(src_r->z), 360.0f)) {
        src_r->z += -360.0f;
    }
    return false;
}

Алгоритм достаточно простой. Мы не просто сдвигаем координаты и углы на определённый шаг, а делаем это пропорционально разнице между исходным и конечным положениями с учётом шага. В итоге на последней итерации и углы наклона и координаты плоскости придут в свой пункт назначения одновременно, это важно. Без этого свойства некоторые движения будут не такими, какими должны быть.

Аналогичные алгоритмы есть для обычного вектора и для точки. Приводить смысла нет, принцип там один и тот же.

Ранее я написал о том, что алгоритм передвижения отвязан от наклонов и сдвигов плоскости. Может возникнуть вопрос «А как же они в итоге связываются?». Так как наклоны и сдвиги плоскости в итоге преобразуются в координаты (код ниже), то мы просто будем хранить их в отдельной структуре и добавлять к координатам конечностей после работы алгоритма передвижения.

bool mm_surface_calculate_offsets(limb_t* limbs, const p3d_t* surface_point, const r3d_t* surface_rotate) {
    v3d_t n = {0, 1, 0};
    float x = 0;
    float y = 0;
    float z = 0;

    // Rotate normal by axis X
    float surface_x_rotate_rad = DEG_TO_RAD(surface_rotate->x);
    y = n.y * cosf(surface_x_rotate_rad) + n.z * sinf(surface_x_rotate_rad);
    z = n.y * sinf(surface_x_rotate_rad) - n.z * cosf(surface_x_rotate_rad);
    n.y = y;
    n.z = z;

    // Rotate normal by axis Z
    float surface_z_rotate_rad = DEG_TO_RAD(surface_rotate->z);
    x = n.x * cosf(surface_z_rotate_rad) - n.y * sinf(surface_z_rotate_rad);
    y = n.x * sinf(surface_z_rotate_rad) + n.y * cosf(surface_z_rotate_rad);
    n.x = x;
    n.y = y;

    // Rotate normal by axis Y
    float surface_y_rotate_rad = DEG_TO_RAD(surface_rotate->y);
    x =  n.x * cosf(surface_y_rotate_rad) + n.z * sinf(surface_y_rotate_rad);
    z = -n.x * sinf(surface_y_rotate_rad) + n.z * cosf(surface_y_rotate_rad);
    n.x = x;
    n.z = z;

    // For avoid divide by zero
    if (fabs(n.y) < FLT_EPSILON) {
        return false;
    }

    // Nx(x - x0) + Ny(y - y0) + Nz(z - 0z) = 0
    // y = (-Nx(x - x0) - Nz(z - z0)) / Ny + y0
    for (int32_t i = 0; i < SUPPORT_LIMBS_COUNT; ++i) {
        limbs[i].surface_offsets.x = surface_point->x;
        limbs[i].surface_offsets.z = surface_point->z;
        limbs[i].surface_offsets.y = -(n.x * (limbs[i].pos.x - surface_point->x) + n.z * (limbs[i].pos.z - surface_point->z)) / n.y + surface_point->y;
    }
    return true;
}

Добавление смещений.

        
float x = limbs[i].pos.x + limbs[i].surface_offsets.x;
float y = limbs[i].pos.y + limbs[i].surface_offsets.y;
float z = limbs[i].pos.z + limbs[i].surface_offsets.z;
//
// Дальше идут расчёты кинематики
//

Алгоритм последовательного приближения позволяет не только прервать движение в любой момент, но и любой момент изменить его.

Ну вот мы и подошли к машине состояний (должно быть кликабельно):

Можно увидеть страшное слово «script». Но как так? Мы же хотим отказаться от скриптов. Их без проблем можно убрать, но тогда красиво крутить телом придётся при помощи джойстика в приложении на телефоне. Хотим мы этого? Думаю нет.

Вот пример скрипта вращения тела по двум осям из первого видео:

static void xy_rotate_init(motion_t* motion) {
    common_init(motion);
    motion->surface_rotate.x = 15;
}
static void xy_rotate_exec(motion_t* motion) {
    motion->surface_rotate.y = 361;
    motion->cfg.speed = 60;
}

Всё честно — никаких координат и заранее заданных траекторий/уравнений. Тут просто применяется хак ядра в виде указания 361 градуса, которые никогда не будут достигнуты. Таким образом, получается вечный цикл от 0 — 360 градусов. Такое без проблем можно сделать руками.

Ещё пример — наклоны в углы. Такое тоже можно сделать руками при помощи приложения. Просто нажать на кнопку удобнее, чем вращать виртуальный джойстик :)

static void square_exec(motion_t* motion) {
    static uint8_t loop = 0;
    switch (loop++) {
    case 0:
        motion->surface_rotate.x = 15;
        motion->surface_rotate.z = 15;
        break;
    case 1:
        motion->surface_rotate.x = -15;
        motion->surface_rotate.z = -15;
        break;
    case 2:
        motion->surface_rotate.x = -15;
        motion->surface_rotate.z = 15;
        break;
    case 3:
        motion->surface_rotate.x = 15;
        motion->surface_rotate.z = -15;
        loop = 0;
        break;
    }
}

С полным исходным кодом вы можете ознакомиться на GitHub (ссылка в начале статьи), ветка step_detection. Ядро передвижения находится в директории firmware/ControlBoard/MainMCU/src/motion-core/*.

▍ Что насчёт адаптации к ландшафту?

С новым ядром будет всё просто. Мы будем опускать каждую ногу до момента срабатывания датчика касания. Но более подробно я об этом подумаю после производства новой версии платы управления.

Кстати, прошлые датчики на базе тензорезисторов оказались не очень. Дело в том, что их параметры имеют просто огромный разброс в зависимости от партии. Соответственно найти 6 тензорезисторов с более-менее похожими характеристиками крайне сложно. Пришлось отказаться от них в пользу обычных кнопок (чуть позже расскажу, как это работает).

▍ Изменения в железе

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

Убрал второй МК и заменил его расширителем портов ввода-вывода PCA9555PW. Крутая штука, конфигурация портов может меняться (вход/выход) и есть прерывание по изменению уровня на входах. Производитель знает, как доставить мне удовольствие (в хорошем смысле).

Ещё в результате дефицита чипов (ну или фазы луны) из магазинов внезапно пропал мой МК STM32F373RCT6, надеюсь, это временно, т.к. проц-то хороший. Не хочется портировать прошивку под новое железо, хорошо есть запас. Да и в принципе 373 серия подорожала в 4 раза, просто нет слов.

Вот так выглядит новая плата (сверху) в сравнении со старой (снизу):


Возможно те, кто следит за проектом, заметили, что гекс теперь белый. Корпус был перепечатан в результате приобретения принтера со столом 400х400, что позволило сделать его монолитным.

▍ Ну и что в итоге

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

А что с параметрами? Давайте посмотрим, что получилось в итоге:

• curvature — степень кривизны траектории [-1000; 1000];
• distance — длина шага. Границы определяются конфигурацией корпуса. В моём случае это [-115; 115], т.е. максимальный шаг — это 11.5 см;
• speed — скорость передвижения [0; 100];
• (x0,y0,z0) — координаты центра плоскости;
• (nx,ny,nz) — углы, на которые поворачивается нормаль (0; 1; 0).

Всё достаточно лаконично и никаких координат, траекторий, управления временем и прочего хлама.

В следующей статье гексапод будет уже ходить по неровным поверхностям. Всем спасибо! Надеюсь, было интересно.