Разработка hexapod с нуля (часть 12) |
您所在的位置:网站首页 › С×疌崳С › Разработка hexapod с нуля (часть 12) |
![]() Также в гексаподе появилась стабилизация тела относительно горизонта на базе MPU6050. Прошивка сама компенсирует углы наклона во время движения — в будущем это очень пригодится, когда я буду реализовывать адаптацию к неровностям. В этом направлении уже ведутся разработки (датчики касания на базе тензорезисторов), настало время для следующего шага. В этой статье расскажу, насколько простая может быть математика ядра передвижения гексапода и какие красивые движения можно выполнять с помощью неё. Разработка продолжается, и я переписал около 80% математики. Это позволило выкинуть явное указание координат точек назначения во время движения — траектории теперь строятся в реальном времени. Все технические подробности в статье. Как всегда, вас ждёт фото и видео. Github AIWM Hexapod Этапы разработки Часть 1 — проектирование Часть 2 — сборка Часть 3 — кинематика Часть 4 — математика траекторий и последовательности Часть 5 — электроника Часть 6 — переход на 3D печать Часть 7 — новый корпус, прикладное ПО и протоколы общения Часть 8 — улучшенная математика передвижения Часть 9 — завершение версии 1.00 Часть 10 — датчики касания Часть 11 — стабилизация Часть 12 — новое ядро передвижения ▍ Введение А почему ядро вообще нужно было переписывать? Просто посмотрите на это: {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 у всех точек равна нулю и это не просто так. Давайте я попробую объяснить. Моя идея заключается в том, чтобы отвязать всякие наклоны, сдвиги и вообще любые манипуляции от алгоритма передвижения. В результате алгоритм никогда не увидит изменения координат базовых точек. То есть алгоритм будет думать, что выполняет движение по абсолютно ровной поверхности, положение которой никогда не меняется. Напомню, почему это должно быть так. Алгоритм отталкивается от центра траектории, серые векторы как раз и указывают на базовые точки. Стоит изменить базовую точку и всё рухнет.
![]() Есть земля\пол\асфальт\неважно (пусть будет земля) и есть гексапод. Гексапод встал на ноги и своим весом продавил ямки на земле, т.е. мы просто опускаем проекцию базовых точек на землю. Убираем гексапода и что остаётся?
![]()
(x0,y0,z0) — точка, которая лежит на этой плоскости; (nx,ny,nz) — вектор, перпендикулярный плоскости (нормаль). Это и будут наши новые рычаги воздействия. Для простоты примем, что точка и начало вектора находятся в одном месте. Опустим пока точку и обратим внимание на нормаль. Что будет, если её повернуть по одной из осей? Разумеется, плоскость будет поворачиваться за вектором, они ведь должны быть перпендикулярны.
![]()
![]()
![]()
Уже на данном этапе мы можем описать движение ниже. Наклоняем плоскость по оси Х на 15 градусов и вращаем её вокруг оси Y. Это просто великолепно! Визуально выглядит сложно, но на самом деле мы просто покрутили нормаль. На видео также можно увидеть, что с помощью этого можно реализовать наклоны корпуса. Видео х2 Теперь пощупаем точку (x0,y0,z0) из уравнения плоскости. Эта точка, через которую проходит плоскость. Если наклоном вектора мы можем управлять наклоном корпуса, то с помощью точки мы сможем управлять его высотой — двигаем землю вверх/вниз.
![]() Видео С Y координатой разобрались, но как быть с 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; // // Дальше идут расчёты кинематики // Алгоритм последовательного приближения позволяет не только прервать движение в любой момент, но и любой момент изменить его. Ну вот мы и подошли к машине состояний (должно быть кликабельно):
Вот пример скрипта вращения тела по двум осям из первого видео: 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 раза, просто нет слов. Вот так выглядит новая плата (сверху) в сравнении со старой (снизу):
![]() Фото (трафик) ![]() ![]() ![]() ![]() ![]() ![]() ![]() ![]() А что с параметрами? Давайте посмотрим, что получилось в итоге: curvature — степень кривизны траектории [-1000; 1000]; distance — длина шага. Границы определяются конфигурацией корпуса. В моём случае это [-115; 115], т.е. максимальный шаг — это 11.5 см; speed — скорость передвижения [0; 100]; (x0,y0,z0) — координаты центра плоскости; (nx,ny,nz) — углы, на которые поворачивается нормаль (0; 1; 0). Всё достаточно лаконично и никаких координат, траекторий, управления временем и прочего хлама. В следующей статье гексапод будет уже ходить по неровным поверхностям. Всем спасибо! Надеюсь, было интересно.
|
CopyRight 2018-2019 办公设备维修网 版权所有 豫ICP备15022753号-3 |