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


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

    Траектории


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

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

    Драйвер поддерживает следующие траектории движения:

    1. XYZ_LINAR — самая простая из всех траекторий. Траектория используется при движении вперед, назад, подъеме и спуске. Все координаты меняются линейно и вычисляются следующим образом:

      x = t * (x1 - x0) / 180.0f + x0;
      y = t * (y1 - y0) / 180.0f + y0;
      z = t * (z1 - z0) / 180.0f + z0;
      

      Тут с понимаем проблем нет. Координаты задают углы параллелепипеда и они совпадают с реальными координатами. Движение происходит по диагонали параллелепипеда.

      Визуализация траектории




    2. YZ_ARC_Y_LINEAR — данная траектория позволяет реализовать движение по дуге. Траектория используется при повороте, когда нужно переместить конечность по земле. Координаты вычисляются следующим образом:

      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
      

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

      Визуализация траектории




    3. XZ_ARC_Y_SINUS — данная траектория так же позволяет реализовать движение по дуге, но более сложное, чем YZ_ARC_Y_LINEAR. Траектория используется при повороте, когда нужно переместить конечность по воздуху. Координаты вычисляются следующим образом:

      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 целевой точки задает высоту синуса.

      Визуализация траектории


    4. XZ_ELLIPTICAL_Y_SINUS — данная траектория позволяет реализовать движение по эллипсу. Траектория используется при движении вперед и назад, когда нужно переместить конечность по воздуху. Данная траектория является усложнением XZ_ARC_Y_SINUS и понадобилась только из-за визуально некрасивой походки при использовании XZ_ARC_Y_SINUS (ноги слишком сильно выпирали). Координаты вычисляются следующим образом:

      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 — содержит информацию об итерации последовательности:

    • point_list — массив точек для каждой конечности в формате XYZ;
    • path_list — массив траекторий для каждой конечности;
    • smooth_point_count — задает количество точек траектории (шаг параметра t);

    sequence_info_t — содержит информацию о всей последовательности:

    • is_sequence_looped — задает тип последовательности: цикличная или нет;
    • main_sequence_begin — задает начальный индекс основного блока в массиве iteration_list;
    • finalize_sequence_begin — задает начальный индекс блока завершения в массиве iteration_list;
    • total_iteration_count — задает количество итераций в последовательности;
    • iteration_list — массив итераций;
    • available_sequences — задает список последовательностей, доступных для перехода из текущей (допустим, мы не можем начать ходить, не встав сначала с пола);

    NOTE: Индекс блока подготовки не указывается намеренно, он всегда располагается в начале массива итераций.

    К сожалению, код определения последовательности представить тут не могу, т.к. он довольно широкий и после переносов ужасно выглядит. Я просто оставлю тут ссылку на файл с определениями.

    Схема обработки движений


    Стоит разобрать все круги ада, которые проходит последовательность во время выполнения. Схема обработки выглядит следующим образом:


    1. MOVEMENT ENGINE — занимается организацией обработки и переключением между последовательностями. Никаких вычислений там не проводится. Если упрощенно, то этот модуль подсовывает следующую точку модулю «LIBMS DRIVER» после завершения обработки текущей.
      Вход модуля: массив координат целевых точек.
      Выход модуля: целевая точка для текущей итерации последовательности.
    2. LIBMS DRIVER — самый сложный из всех модулей. Тут царит вся математика передвижения: обратная кинематика, расчеты траекторий и сглаживание движений. Этот модуль имеет строгую синхронизацию с модулем PWM. Расчеты проводятся c частотой 150Hz, соответственно управляющий импульс на приводы так же подается с частотой 150Hz.
      Вход модуля: координаты целевой точки.
      Выход модуля: углы поворота сервоприводов.
    3. SERVO DRIVER. Нечего особенного в нем нет, кроме кучи параметров для настройки и корректировки приводов.
      Вход модуля: углы поворота сервоприводов.
      Выход модуля: ширина управляющего импульса.
    4. PWM DRIVER. Драйвер программного ШИМ для управления приводами. Тут просто дергаются пины в нужные моменты времени. Переменная для синхронизации «PWM synchro» инкрементируется каждый период PWM.
      Вход модуля: ширина управляющего импульса.
      Выход модуля: импульсы на управляющих пинах.

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

    Последние новости и найденные раки проекта


    Последние новости
    1. Вышел нового тестовый вариант корпуса (архив с чертежами) и я его немного покрасил. Полная сборка гексапода с выставлением насадок приводов в центральное положение занимает 7 — 8 часов непрерывной сборки и это если учитывать, что я уже не однократно проводил эту процедуру.

      Фотки







    2. Поставил на него OLED дисплей для вывода какой-нибудь инфы, получилось довольно красиво.
      Фотки



    3. Запущена коммуникация через WIFI. Теперь он управляется с телефона (тулзу пришлось написать свою)
    4. Снижено напряжение питания с 12V до 7V из-за проблем с перегревом платы питания
    5. К выходу 5 части разработки выложу ссылку на исходники, они наконец-то приобрели состояние при котором их не стыдно показать людям


    Найденные раки
    1. HC-SR04. Я знал, что этот датчик плохой, но не думал что настолько. В общем, нужен другой дальнометр
    2. MG996R не соответствуют заявленным характеристикам. Обещали 12кг\см — по факту 5кг\см при PWM с частотой 300Гц, при 50Гц еще хуже и к тому же они оказались аналоговыми (обещали цифру). Годятся только для поворотов. Пришлось перейти на более дорогие цифровые приводы DS3218 на 20кг\см — по факту 23кг\см
    3. Составил таблицу «угол-импульс» через каждые 10 градусов и заметил, что значения ширины управляющего импульса для MG996R находятся на разном расстоянии друг от друга. Пришлось делать калибровочные таблицы для каждого привода и вычислять импульс индивидуально.

      Как видно, шаг импульса для каждого привода отличаются, это было неожиданным для меня открытием.
    4. Минимальное, максимальное и центральное значения импульса отличаются из-за насадок для приводов (как их ни крути, все равно не ровно). На рисунке показаны приводы, на которые подается импульс 1500us и видно, что одна насадка находится не по центру и соответственно приходится подгонять импульс так, чтобы все насадки находились в одном и том же положении.


    Кстати калибровку делал при помощи этого устройства:


    Ссылки на другие этапы разработки


    Часть 1 — проектирование
    Часть 2 — сборка
    Часть 3 — кинематика
    Часть 5 — электроника
    Поделиться публикацией

    Похожие публикации

    Комментарии 32

      0
      Три двигателя на одну ногу, а траектория движения считается для пятна контакта ноги с поверхностью. Мне кажется этого мало. Каждый двигатель обладает инертностью, и это нужно учитывать. При этом инертность первого двигателя, что используется для поворота всей конечности — получается максимальной. А вот последнего, что выдвигает лапу от корпуса — минимальной, и его можно существенно ускорить.
      Модуль где вся математика — наверное самое интересное. Хотелось-бы увидеть в любом варианте: в виде ссылки, спойлера, или публикации на гитхабе.
      0
      Кхм, я рассчитывал увидеть обратное преобразование, а у вас решение в лоб. Плоскость платформы гексапода — как начальная точка отчёта. В таком варианте расчёты значительно упрощаются. :)
      Насчёт преодоления сложного рельефа.
      Я заметил сохранение точки касания, после чего её высота не меняется. Для разного рельефа эта точка будет находится на разной высоте. То-есть нужен датчик удара о поверхность. Могут быть варианты: контактный, ёмкостный, или датчик ускорения (аксель). Для последнего доступно сразу несколько функций: вертикальное препятствие, высота/пустота, и режим контактного осязания — постройка грубой формы рельефа препятствия. Постройка рельефа — рискованная функция. Есть шанс что гексапод сумеет забуриться туда — откуда его трудно будет достать.
        0
        Я не слишком силен в математике, поэтому не стал усложнять всё. Да и зачем, когда есть более простое решение. Возможно по мере усложнения проекта будет усложняться и математика.

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

        На счет акселерометра можно поподробнее? Есть много MPU6050 :)
      0
      а модель доски калибровки сервы можно как-нибудь получить?
        0
        К сожалению чертеж в КОМПАС утерян. Есть только PDF, готовая для лазерной резки. Но тут нужно учитывать, что прорезь соответствует только MG996R и идентичным им. Цифровые приводы туда влезали со скрипом :)
          0
          Спасибо, сойдёт!
        0
        Много MPU6050 — это уже хорошо. Два таких датчика можно использовать на одной шине. Для старта более чем достаточно. Но придётся делать собственную ПП для датчика. Tе что продаются — имеют большие габариты и несовместимость с плоскими шлейфами.

        Суть в том что последнее звено лапы нужно сделать чуть более подвижным. Либо на саму лапу прикрепить контактную поверхность с небольшим свободным ходом. И вот уже на неё крепить датчик. Вариантов перехода много: от резиновой трубки, до демпферов с вязким наполнителем.
        Движение лапы необходимо просчитать для кординат датчика. У вас получится ещё один массив данных — но уже для датчика. При наличии препятствия — показания акселя будут многократно отличаться от расчётных.
          0
          Кажется я уловил Вашу мысль. Идея достаточно интересная и сложная — то что нужно. Попробую более подробно задуматься об этом. Спасибо за идею.
            0
            Да, да — совершенно слепой гексапод, который передвигается на ощупь.
            Такой способен неожиданно напугать, если его надолго потерять из поля зрения. Я думаю домашние будут в восторге, но немного седые.
              0
              Тестировал на своем коте — гексапод его толкает уже лапами в бок, а он просто смотрит на него и всё. Ну хотя, что от кота ожидать — он пылесоса не боится. Люди от неожиданности пугаются :)
            0
            MPU6050 лучше всего использовать для стабилизации платформы,
            а чтобы корректно работать с конечностями, нужно использовать сервы с обратной связью.
            Но это дорого
              0
              Да, я очень долго смотрел на такие сервы и облизывался — минимум проводов и полный контроль над всем, но цена даже на Ali довольно высокая, особенно учитывая их требуемое количество.
                0
                кстати, можно было бы использовать для хексапода сервы со сквозным валом — их крепить удобнее
                  0
                  Думал над этим, но такие сервы довольно толстые и плохо смотрятся на таком корпусе.
                    0
                    почему толстые? SR403P стандартного размера
                      0
                      Мне такие не попадались на глаза. В основном видел только приводы, на другой стороне которых просто пластмассовая пипка торчит.
                        0
                        у SR403P металлический сквозной вал
                          0
                          Да, я уже погуглил их :). Дорогие. Да и текущие приводы более чем устраивают
                            0
                            так они дешевле стоят, чем DS3218 — 1.160 рублей в Москве
                            но я, конечно, ни на чём не настаиваю )))
                              0
                              Я DS3218 купил за 460р за штуку (у китайца скидку выпросил)
            0
            А не проще использовать обычную обратную кинематику?
              0
              Так она там и используется. Траектории это один из этапов обработки входных данных до алгоритма обратной кинематики. Сама обратная кинематика рассматривается тут: habr.com/ru/post/436748
              +1
              Могу добавить про MG996R из личного опыта. Года 3 назад хотел собрать механическую руку, чтобы потом рисовать ею какие-то контуры. Для меня основной проблемой тогда стало то что под нагрузкой серва уже не занимала заданный мне угол а прижималась как бы к крайнему положению. Так же была проблема задания одного и того же угла когда серва поворачивается по направлению действия нагрузки и против направления действия нагрузки. Возможно в вашем случае сервы посильней, а плечо меньше и такой проблемы не возникнет, но то что серва под нагрузкой может принимать не совсем точное положение надо иметь ввиду.

              Кстати сохранилось само видео той руки:


                0
                А ради интереса и удешевления, не думали спроектировать то же самое, но с четырьмя лапками?

                На ваш взгляд, какое преимущество у дополнительной пары ног?
                  +1
                  Относительная простота реализации. Да и хотят они красивее и быстрее
                  0
                  Как видно, шаг импульса для каждого привода отличаются, это было неожиданным для меня открытием.

                  Кто имеет дело с сервоприводами — об этом знают и везде есть понятие «калибровка» сервоприводов. Указывают крайние точки на которые отклоняется выход сервомашинки и запоминают значения подаваемых импульсов, ну а дальше на основе этих данных масштабируют нужный угол в значения длины импульсов для каждой сервомашинки. Я бы добавил возможность калибровки каждого сервопривода и тогда все будет довольно точно. Вдруг далее захотите заменить сервомашинки на другие и опять таблицу будете делать :)
                    0
                    Калибровка это здорово, но от приводов нет обратной связи и нет датчиков положения конечностей. Довольно сложно (если вообще возможно) откалибровать автоматически без каких-либо данных от приводов. А так да, другие приводы — другая таблица.
                      0
                      Я говорю не про автомат. А про ручную калибровку. Например входим в режим калибровки: 1 сервопривод отклоняем в одну сторону и кнопками + и — устанавливаем отклонение на контрольную метку, потом 1 привод отклоняем в другую сторону и кнопками совмещаем вторую метку. Можно еще выставлять среднюю метку (края и середина, но обычно сервоприводы линейны в обе стороны). Таким образом запоминаем мин и макс значения импульс когда сервоприводы откланяются на заданный угол согласно меток, ну а дальше вычисляем коэффициент и смещение центра. И так все остальные приводы. Да, долго — но зато универсально.
                        0
                        Хорошая идея, нужно подумать об этом. Спасибо.
                      0
                      Буду рад, если предложите несколько идей для развития этой мысли :)

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

                    Самое читаемое