- PVSM.RU - https://www.pvsm.ru -
Всем привет! Разработка гексапода продвинулась на еще один шаг. На этот раз реализованы и протестированы траектории движения конечности — очередная часть математики передвижения. В этой статье я расскажу об этих траекториях и базовых последовательностях для движения. Надеюсь будет интересно.
Суть данного механизма состоит в том, что при задании двух точек можно выбрать траекторию движения конечности. При движении из одной точки в другую координаты будут меняться по определенным параметрическим уравнениям. Механизм получился достаточно мощным и позволяет получать интересные кривые для движения. Так же при помощи него реализуется сглаживание движений за счет изменения шага параметра t — чем меньше шаг, тем больше будет промежуточных точек, соответственно ниже скорость и выше плавность движения.
Процедура задания параметров траектории местами немного непонятна и можно запутаться в ней. Сложность заключается в том, что при задании координат начальной и конечной точек не всегда задаются координаты реальных точек в пространстве, т.е. некоторые координаты задают параметры траектории. Пришлось написать программу, которая отображает заданную траекторию и заодно проверяет достижимость каждой точки траектории.
Драйвер поддерживает следующие траектории движения:
x = t * (x1 - x0) / 180.0f + x0;
y = t * (y1 - y0) / 180.0f + y0;
z = t * (z1 - z0) / 180.0f + z0;
Тут с понимаем проблем нет. Координаты задают углы параллелепипеда и они совпадают с реальными координатами. Движение происходит по диагонали параллелепипеда.
float R = sqrt(x0 * x0 + z0 * z0);
float atan0 = RAD_TO_DEG(atan2(x0, z0));
float atan1 = RAD_TO_DEG(atan2(x1, z1));
float t_mapped_rad = DEG_TO_RAD(t * (atan1 - atan0) / 180.0f + atan0);
x = R * sin(t_mapped_rad); // Circle Y
y = t * (y1 - y0) / 180.0f + y0;
z = R * cos(t_mapped_rad); // Circle X
Вот тут начинается веселье. Координаты задают направления лучей для ограничения дуги и они могут не совпадать с реальными координатами. Лучи находятся в одной плоскости, при этом радиус окружности равен длине вектора до начальной точки.
float R = sqrt(x0 * x0 + z0 * z0);
float atan0 = RAD_TO_DEG(atan2(x0, z0));
float atan1 = RAD_TO_DEG(atan2(x1, z1));
float t_mapped_rad = DEG_TO_RAD(t * (atan1 - atan0) / 180.0f + atan0);
x = R * sin(t_mapped_rad); // circle Y
y = (y1 - y0) * sin(DEG_TO_RAD(t)) + y0;
z = R * cos(t_mapped_rad); // circle X
Веселье продолжается. Координаты так же задают направления лучей для ограничения дуги, но они НЕ совпадают с реальными координатами. Координата Y целевой точки задает высоту синуса.
float a = (z1 - z0) / 2.0f;
float b = (x1 - x0);
float c = (y1 - y0);
x = b * sin(DEG_TO_RAD(180.0f - t)) + x0; // circle Y
y = c * sin(DEG_TO_RAD(t)) + y0;
z = a * cos(DEG_TO_RAD(180.0f - t)) + z0 + a;
Координаты задают углы параллелепипеда и они НЕ совпадают с реальными координатами. Движение происходит из нижних углов параллелепипеда, находящихся в одной плоскости с касанием верней его части. Тут лучше посмотреть картинку в спойлере, я не знаю как это кратко описать словами.
На этом базовая математика передвижения гексапода заканчивается. В моем проекте это необходимый минимум для реализации почти любых движений.
Последовательности — это элементарные действия из которых состоит походка. Делятся они на цикличные и не цикличные.
Каждая последовательность имеет три блока итераций: блок подготовки, основной блок, блок завершения.
На рисунке ниже показана последовательность для движения вперед.
Координаты точек выбираются исходя из конфигурации корпуса. Я выбирал точки как можно ближе к корпусу для уменьшения длины рычага. За один цикл последовательности гексапод перемещается на 18см (за 1 цикл делается 2 шага — по 1 шагу на 3 конечности). Если сделать расстояние больше, то конечности начнут цеплять друг друга. Данный параметр ограничивается только конфигурацией корпуса.
Последовательность задается двумя точками (1, 2) для каждой конечности и используются две траектории: XYZ_LINEAR (синие линии) и XZ_ELLIPTICAL_Y_SINUS (черные линии). Точка 1 используется траекторией XZ_ELLIPTICAL_Y_SINUS для установки высоты синуса и соответственно высоты, на которую поднимется нога. Точка 2 и 3 являются реальными точками, которых достигает конечность при движении.
Расположение точек зависит только от вашей фантазии и возможностей гексапода. Возможно это все получилось немного сложно и есть более простой вариант, но видимо я пока до него не дошел.
Теперь немного разберем реализацию всего этого счастья. Структуры с параметрами последовательности выглядит следующим образом:
typedef struct {
point_3d_t point_list[SUPPORT_LIMB_COUNT];
path_type_t path_list[SUPPORT_LIMB_COUNT];
uint32_t smooth_point_count;
} sequence_iteration_t;
typedef struct {
bool is_sequence_looped;
uint32_t main_sequence_begin;
uint32_t finalize_sequence_begin;
uint32_t total_iteration_count;
sequence_iteration_t iteration_list[15];
sequence_id_t available_sequences[SUPPORT_SEQUENCE_COUNT];
} sequence_info_t;
sequence_iteration_t — содержит информацию об итерации последовательности:
sequence_info_t — содержит информацию о всей последовательности:
NOTE: Индекс блока подготовки не указывается намеренно, он всегда располагается в начале массива итераций.
К сожалению, код определения последовательности представить тут не могу, т.к. он довольно широкий и после переносов ужасно выглядит. Я просто оставлю тут ссылку на файл с определениями [1].
Стоит разобрать все круги ада, которые проходит последовательность во время выполнения. Схема обработки выглядит следующим образом:
Я попытался сделать модули независимыми друг от друга и у меня это получилось. Это позволяет вставить в эту схему любой промежуточный модуль (допустим модуль адаптации к ланшафту) и нечего при этом не сломается, при этом внедрение произойдет с минимальными изменениями в коде.
Как видно шаг импульса для каждого привода отличаются, это было неожиданным для меня открытием.
Кстати калибровку делал при помощи этого устройства:
Автор: Neoprog
Источник [3]
Сайт-источник PVSM.RU: https://www.pvsm.ru
Путь до страницы источника: https://www.pvsm.ru/diy/312200
Ссылки в тексте:
[1] файл с определениями: https://drive.google.com/open?id=18lD5Q1zU598KF0lwWydi-jJ0lY71ZKWT
[2] (архив с чертежами): https://drive.google.com/open?id=1sUhSzTh5mMTA5tXVT_DEQzkcIgs3o5V0
[3] Источник: https://habr.com/ru/post/444070/?utm_campaign=444070
Нажмите здесь для печати.