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


    Всем привет! В результате перехода на удаленную работу у меня появилось больше свободного времени на разработку гексапода (+2 часа в день за счет экономии на дороге). Я наконец-то смог сделать универсальный алгоритм для построения траектории движения в реальном времени. Новая математика позволила реализовать базовые движения путем изменения всего двух параметров. Это очередной шаг к внедрению «автопилота». В этой статье я постараюсь подробно рассказать о новой математике и как это вообще работает. Будет много картинок и gif.

    Этапы разработки:

    Часть 1 — проектирование
    Часть 2 — сборка
    Часть 3 — кинематика
    Часть 4 — математика траекторий и последовательности
    Часть 5 — электроника
    Часть 6 — переход на 3D печать
    Часть 7 — новый корпус, прикладное ПО и протоколы общения
    Часть 8 — улучшенная математика передвижения
    Часть 9 — завершение версии 1.00

    Назад во времени


    До этого момента все базовые движения (вперед, назад, поворот) задавались в виде «перемести ногу из текущей позиции в точку (X, Y, Z), используя линейное/синусоидальное/какое-нибудь перемещение». Это работает достаточно хорошо и надежно, но сильно ограничивает в движении. Например, чтобы реализовать движение по дуге с радиусом R1 нужно заранее рассчитать координаты начала и окончания движения для каждой конечности, добавить дополнительную кнопку в приложение, чтобы можно было выбрать это движение. Соответственно для добавления движения по дуге с другим радиусом R2, нужно опять рассчитать координаты и добавить еще одну кнопку. Крайне неудобно.

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

    Идея алгоритма


    Для начала стоит объяснить на пальцах как это работает. Что будет, если выбрать на окружности окно фиксированного размера и начать увеличивать её радиус? Вот что:


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

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


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


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


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

    Математика


    Входные параметры


    Переменными входными параметрами являются расстояние (distance) и кривизна траектории движения (curvature). Значение кривизны траектории должно находится в диапазонах [-1.999; -0.001] и [0.001; 1.999], в то время как максимальное значение расстояния зависит от физических характеристик гексапода. В моем случае максимальное расстояние за цикл равно 110мм, при больших значениях конечности начинают упираться в друг друга. Для примера расчета возьмем значения curvature = 1.5 и distance = 20.

    Так же для работы алгоритма необходимо знать начальные положения конечностей. Это точки, в которых находятся конечности, когда гексапод встал на ноги. Для примера будем использовать следующие точки (начало координат каждой конечности находится в месте крепления COXA):


    Note: Я работаю в плоскости XZ и на координату Y можно не обращать внимания. Ознакомиться с системой координат гексапода можно в третьей части цикла

    В итоге мы имеем следующее:

    Формулы и расчеты


    Начнем с расчета координат центра движения гексапода в зависимости от значения кривизны и расстояния:

    $R = tg(\frac{(2 - curvature) * \Pi}{4}) * distance$

    $R = tg(\frac{(2 - 1.5) * \Pi}{4}) * 20 = 8.28$

    В итоге мы получили точку [R; 0] и траекторию движения тела гексапода. Относительно нее будут рассчитываться траектории для каждой конечности.


    Далее необходимо вычислить радиусы траекторий для каждой конечности относительно центра движения (точки [R; 0]) c учетом начального положения конечности [x0; z0]. Более понятным языком — найти длину вектора, проведенного из точки [R; 0] в точку [x0; z0]:

    $R_i = \sqrt{(R - x_{0i})^2 + z_{0i}^2}$

    $R_0 = \sqrt{(8.28 - (-20))^2 + 20^2} = 34.64$

    $R_1 = \sqrt{(8.28 - (-35))^2 + 0^2} = 43.28$

    $R_2 = \sqrt{(8.28 - (-20))^2 + (-20)^2} = 34.64$

    $R_3 = \sqrt{(8.28 - 20)^2 + (-20)^2} = 23.17$

    $R_4 = \sqrt{(8.28 - 35)^2 + 0^2} = 26.71$

    $R_5 = \sqrt{(8.28 - 20)^2 + 20^2} = 23.17$


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


    Находим из полученных значений максимальное:

    $R_{max} = maximum(R_{0-5})$

    $R_{max} = 43.28$


    Дальше нужно найти угол для каждого вектора, которые мы пытали в предыдущем шаге.

    $\alpha_{0i} = atan2(z_{0i}; -(R - x_{0i}))$

    $\alpha_{00} = atan2(20; -(8.28 - (-20))) = 2.52 (144.7°)$

    $\alpha_{01} = atan2(0; -(8.28 - (-35))) = 3.14 (180°)$

    $\alpha_{02} = atan2(-20; -(8.28 - (-20))) = -2.52 (-144.7°)$

    $\alpha_{03} = atan2(-20; -(8.28 - 20)) = -1.04 (-59.6°)$

    $\alpha_{04} = atan2(0; -(8.28 - 35)) = 0 (0°)$

    $\alpha_{05} = atan2(20; -(8.28 -20)) = 1.04 (59.6°)$


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


    Угол вычисляется следующим образом:

    $arcMax = sign(R) * \frac{distance}{R_{max}}$

    $arcMax = \frac{20}{43.28} = 0.462 (26°)$


    Дальше используя этот угол мы можем вычислить начальные и конечные углы дуг для других конечностей. Как то так это должно получиться:


    Небольшое отступление. Для реализации этого алгоритма необходимо ввести понятие времени, значение которого лежит в диапазоне [0; 1]. Так же необходимо, чтобы каждая конечность при значении времени 0.5 возвращалась в свою начальную точку. Данное правило является проверкой корректности расчетов — все окружности должны проходить через начальные точки каждой конечности.

    Дальше начинается вычисление точек по полученным параметрам дуг, используя следующие формулы:

    $arcAngle_i = (time - 0.5) * \alpha_{0i} + arcMax$

    $x_i = R + R_i * cos(arcAngle_i)$

    $z_i = R_i * sin(arcAngle_i)$


    Исходный код функции


    static bool process_advanced_trajectory(float motion_time) {
    
        // Check curvature value
        float curvature = (float)g_current_trajectory_config.curvature / 1000.0f;
        if (g_current_trajectory_config.curvature == 0)    curvature = +0.001f;
        if (g_current_trajectory_config.curvature > 1999)  curvature = +1.999f;
        if (g_current_trajectory_config.curvature < -1999) curvature = -1.999f;
        
        //
        // Calculate XZ
        //
        float distance = (float)g_current_trajectory_config.distance;
    
        // Calculation radius of curvature
        float curvature_radius = tanf((2.0f - curvature) * M_PI / 4.0f) * distance;
    
        // Common calculations
        float trajectory_radius[SUPPORT_LIMBS_COUNT] = {0};
        float start_angle_rad[SUPPORT_LIMBS_COUNT] = {0};
        float max_trajectory_radius = 0;
        for (uint32_t i = 0; i < SUPPORT_LIMBS_COUNT; ++i) {
            
            float x0 = g_motion_config.start_positions[i].x;
            float z0 = g_motion_config.start_positions[i].z;
    
            // Calculation trajectory radius
            trajectory_radius[i] = sqrtf((curvature_radius - x0) * (curvature_radius - x0) + z0 * z0);
    
            // Search max trajectory radius
            if (trajectory_radius[i] > max_trajectory_radius) {
                max_trajectory_radius = trajectory_radius[i];
            }
    
            // Calculation limb start angle
            start_angle_rad[i] = atan2f(z0, -(curvature_radius - x0));
        }
        if (max_trajectory_radius == 0) {
            return false; // Avoid division by zero
        }
    
        // Calculation max angle of arc
        int32_t curvature_radius_sign = (curvature_radius >= 0) ? 1 : -1;
        float max_arc_angle = curvature_radius_sign * distance / max_trajectory_radius;
    
        // Calculation points by time
        for (uint32_t i = 0; i < SUPPORT_LIMBS_COUNT; ++i) {
            
            // Inversion motion time if need
            float relative_motion_time = motion_time;
            if (g_motion_config.time_directions[i] == TIME_DIR_REVERSE) {
                relative_motion_time = 1.0f - relative_motion_time;
            }
    
            // Calculation arc angle for current time
            float arc_angle_rad = (relative_motion_time - 0.5f) * max_arc_angle + start_angle_rad[i];
    
            // Calculation XZ points by time
            g_limbs_list[i].position.x = curvature_radius + trajectory_radius[i] * cosf(arc_angle_rad);
            g_limbs_list[i].position.z = trajectory_radius[i] * sinf(arc_angle_rad);
            
            // Calculation Y points by time
            if (g_motion_config.trajectories[i] == TRAJECTORY_XZ_ADV_Y_CONST) {
                g_limbs_list[i].position.y = g_motion_config.start_positions[i].y;
            }
            else if (g_motion_config.trajectories[i] == TRAJECTORY_XZ_ADV_Y_SINUS) {
                g_limbs_list[i].position.y = g_motion_config.start_positions[i].y;
                g_limbs_list[i].position.y += LIMB_STEP_HEIGHT * sinf(relative_motion_time * M_PI);  
            }
        }
        
        return true;
    }
    

    Тут я решил показать еще и расчет Y координаты (Calculation Y points by time). Расчет зависит от выбранной траектории, которая задана в коде жестко и необходима для реализации движения конечности по земле и по воздуху.

    Так же имеется кусок для реализации обратного направления движения (Inversion motion time if need). Он нужен, чтобы во время движения трех конечностей по земле, другие три конечности двигались в обратном направлении по воздуху.

    Результаты


    Only registered users can participate in poll. Log in, please.

    В процессе разработки проекта создалось небольшое комьюнити, в котором идет обмен идеями. Мне стало интересно, а много ли тех кто следит за развитием проекта?

    • 11.2%Слежу за развитием проекта и делаю своего гексапода11
    • 31.6%Слежу за развитием проекта и хочу начать делать своего гексапода31
    • 20.4%Просто слежу за развитием проекта20
    • 36.7%Просто читаю статьи для общего развития36
    AdBlock has stolen the banner, but banners are not teeth — they will be back

    More
    Ads

    Comments 33

      0
      А почему вы перемещаете конечности по дугам, а не по прямым линиям?
        0
        Если перемещать конечности по прямым линиям, то получится походка без поворота тела (не помню как называется). Я хотел, чтобы нос гексапода всегда был направлен в сторону движения.

        Я правильно понял Вашу мысль?
          0
          Но дугами тоже ведь не все движения корпуса можно нарисовать…
            0
            Верно. Данная математика используется только для базовых движений: вперед, назад, поворот, движение по дуге. В основном она была введена для реализации движения по дуге — очень уж хотелось этого.

            Для подъема гексапода и различных танцулек используется старый вариант: «передвинь ногу из текущей позиции в XYZ по линейной траектории».
              +1
              Так получилось, что я сейчас разрабатываю смежную тематику
              Возможно вас заинтересует управление на тензорных регуляторах.


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

              Там довольно много линейной алгебры, зато результат может быть очень красивым.
                0
                Это очень круто! Я потихоньку к этому хочу придти, но пока есть ряд других проблем и хотелок. Спасибо большое за информацию, обязательно ознакомлюсь.
                  0
                  Только имейте в виду, что сервомоторы должны управляться именно заданием скорости а следование по траектории должно поддерживаться регулятором верхнего уровня. Это та проблема на которую я напоролся при установке серводвигателей на свои станки. Я сначала пытался использовать контур позиционирования в самом двигателе, но постоянно возникало рассогласование координат, потом оставил в двигателе только контур скорости а контур позиционирования перевел выше в контроллер положения
                    0
                    А с манипуляторами по другому и не получается. Потому что если вы делаете уставку по положению, вам приходится решать обратную задачу кинематики. А обратная задача кинематики — это то, что нужно гнать тряпкой из подобных систем, потому как она нелинейна, а обратная скоростная задача линейна.

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

                    Вам не кажется странным, что в каждый момент времени движения ваш гексапод стоит на 2х ногах, а 4 перемещает в воздухе? Неустойчиво же!

                      0

                      Думаю, вы неправильно посчитали :). В воздухе всегда не более трёх ног. Там программа запрещает более трёх ног отрывать от земли. Перестановка ног разрешается не раньше, чем все 6 окажутся на земле. Но вообще тут могут быть баги, поскольку тут нет физического движка. В последней версии за подобными проблемами у меня следит openbullet. Он таких ошибок не прощает.


                      Скорее всего это из-за того, что камера вращается. Кажется, что дальняя нога перемещается.

                        0
                        Не. Тут 3 ноги на земле и 3 в воздухе, это вот прям основа всего. Ноги не станут подниматься пока все 6 ног не будут на земле (актуально для движения).
                    0
                    Если быть совсем точным, то нам нужна какая то точность перемещения по траектории, т.е всегда есть допустимое отклонение, а с учетом этого любую траекторию можно апроксимировать даже прямыми отрезками. Если есть еще и дуги то очень сильно упрощается анализ и на порядки уменьшается объем примитивов движения
                    0
                    Кстати о походках, где-то с 1:02
                  0
                  У меня так 3 кошки топчут пол. Ещё железяки для полной клиники не хватает ))
                    0
                    Может я неправ, но подумалось что при движении прямо вперед имеет смысл ходить не тройками ног, а парами, тогда расчет математики можно делать для одной стороны, а вторую просто зеркалировать. А при повороте, если точки крепления ног осесимметричны, можно вообще считать для одной ноги, и на две по 120 градусов просто поворачивать.
                      0
                      Да, можно. Такая походка будет медленнее, чем тройками (tripod gait). Приводы я использую довольно мощные, соответственно скорость поворота у них не высокая. В этом случае проект можно переименовать с hexapod в turtle :)

                      Да и ресурсы МК позволяют выбрать более сложный и универсальный путь с большим запасом.
                      0
                      проект можно переименовать с hexapod в turtle
                      да ладно, черепаха по одной ноги переставляет :)
                      Вообще, самый интересный вопрос — ходьба по пересеченной местности, когда высота пола разная для разных ног.
                        0
                        Это будет чуть позже. Для реализации этого на концах лап находятся концевики, но пока они не задействованы. Не думаю, что будет сложно. Все сводится к манипуляции с Y координатой.
                        0
                        Я делаю станки с ЧПУ, и рассматриваю такой аппарат для для резки больших листов с точным позиционированием в каждый момент, меня привлекает абсолютная маневренность
                          0

                          Вы наверное путаете с hexapod платформой. В данном случае hexapod это шестиногий робот.

                            +1
                            Нет, не путаю. Есть большие листы металла скажем 3х6 метра(есть и больше), если делать станок для раскроя такого листа, то он будет очень большой и неперемещаемый, а вот если установить на подвижный робот плазмотрон и систему позиционирования достаточной точности, то это будет и дешево и удобно
                              0
                              А есть идеи о том, как такая система позиционирования будет выглядеть?
                                0
                                Предполагается что с помощью СВЧ видимо 8 или 3 мм с частотной модуляцией. 4 синхронизированных излучателя размещенных по границам поля обработки и анализ на гексаподе с точностью по фазе не хуже 10 градусов. Можно добиться ошибки 0,1 мм
                                0

                                Чтобы обеспечить постоянную погонную энергию — нужно добиться равномерности движения.

                                  0
                                  Равномерности движения в такой системе добиться можно, но ошибки всё равно будут накапливаться. Понадобится внешняя коррекция, а вот как её делать с достаточной точностью не очень понятно.
                                    0
                                    Существует подход к крупногабаритной печати, когда печатающая головка (аэрозольный баллончик с электрическим клапаном) подвешивается на стене многоэтажного здания, где множество факторов мешает равномерному перемещению головки, но так как её фактическое положение отслеживается с высокой точностью, команда на открытие клапана всегда соответствует реальному положению.
                                    Что если попробовать такой подход? Положение робота можно измерять точно при помощи стационарных лазерных дальномеров. Также робота можно оснастить дополнительным актуатором, осуществляющим прецизионное позиционирование плазменной головки (как в приводе CD/DVD, лазер в магнитной подвеске нивелирует неидеальность позиционирования головки), чтобы не пришлось прерывать линию реза.
                                      0
                                      Можно и лазер, но с ним побольше сложностей, он конечно на порядки точнее и приемник имеет меньше габариты но с СВЧ проще обеспечить сферические волны, он не чувствителен к пыли и дыму, при работе плазменного аппарата еще и яркое излучение и т.д. Я все таки больше склоняюсь к СВЧ, хотя у него тоже большие сложности в основном с отражением от поверхности металла.
                                  0
                                  Напомнило
                                  image

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

                                      Скоро скоро, совсем немного осталось :)

                                        0

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

                                          0
                                          Я думаю проще сделать не получится. Если условиться, что гибкая конечность состоит из сегментов (как на картинке), то все равно нужно вычислять поворот для каждого сегмента. В данном случае получается манипулятор с N степенями свободы, где N явно больше 20 и отсюда выливается лютый матан. Я сложно представляю систему уравнений для такого манипулятора :)
                                            0

                                            Думаю, можно расчленить конечность на отдельные сегменты по нескольку звеньев. Каждый сегмент будем решать независимо, а поверх управления отдельными сегментами добавим систему управления конечностью в целом. Это позволит решать не все 30 уравнений разом, а иерархически, по 5 штук в 6 группах и одну в 6 уравнений для 6 сочленений.


                                            Кстати, это идея. Я внесу это мысль в свою статью по теме управления манипуляторами :).

                                Only users with full accounts can post comments. Log in, please.