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

Системы координат

Для начала стоит определится с системами координат робота — их аж целых три:

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


Как вы уже наверно заметили, системы координат на левой и правой сторонах расположены симметрично относительно центра корпуса. Мне показалось, что так будет проще.

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

А вот так они расположены относительно конечности:


Координаты целевой точки задаются относительно MAIN COORDINATE SYSTEM. Изначально я хотел задавать координаты относительно центра корпуса, но это оказалось не очень удобно и требовало хранения ряда дополнительных параметров.

Далее по тексту обозначения X*, X** и т.д. будут заменены на X?, X??.

Решение обратной задачи кинематики

Начнем с самого сложного и интересного для меня момента — кинематики. Алгоритм я придумывал сам, иногда поглядывая на свойства треугольника и тригонометрические функции. С механикой у меня тяжко, а с тригонометрией еще хуже, так что возможно я где-то что-то мог не учесть, но этот алгоритм работает (видео в конце).

1. Постановка задачи

Допустим нам нужно, чтобы передняя правая конечность дотянулась до точки А с координатами (150; -20; 100). Так же известно, что конечность повернута относительно корпуса на 45 градусов (параметр coxa_zero_rotate):


Сама конечность имеет следующие параметры:


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


Всех представленных выше параметров достаточно для решения поставленной задачи.

2. Решение задачи

2.1 Нахождение угла поворота COXA

Данный этап является самым простым. Для начала необходимо пересчитать координаты точки А относительно LIMB COORDINATE SYSTEM. Очевидно, что нужно выполнить поворот на угол coxa_zero_rotate, сделать это можно используя следующие формулы:

$\$\$display\$\$x?\; =\; x\; ?\; cos(?)\; +\; z\; ?\; sin(?)\; =\; 150\; ?\; cos(45)\; +\; 100\; ?\; sin(45)\; =\; 176.78\$\$display\$\$$

$$

$\$\$display\$\$z?\; =\; -x\; ?\; sin(?)\; +\; z\; ?\; cos(?)\; =\; -150\; ?\; sin(45)\; +\; 100\; ?\; cos(45)\; =\; -35.36\$\$display\$\$$

Таким образом мы получили координаты целевой точки А (176.78; -20; -35.36) относительно LIMB COORDINATE SYSTEM.

Теперь можно найти угол COXA используя функцию atan2:

$$

И так, мы получили угол, на который нужно повернуть сервопривод COXA, чтобы точка А находилась в плоскости X?Y?. Давайте теперь проверим наши расчеты в КОМПАС 3D:


Все верно.

2.2 Нахождение угла поворота FEMUR и TIBIA
Для нахождения этих углов необходимо перейти к плоскости X?Y?. Для перехода к плоскости нужно выполнить поворот точки на угол COXA, который мы уже рассчитали ранее.

$\$\$display\$\$x?\; =\; x?\; ?\; cos(?)\; +\; y?\; ?\; sin(?)\; =\; 176.78\; ?\; cos(-11)\; +\; -20\; ?\; sin(-11)\; =\; 180.28\$\$display\$\$$

$$

Координата y? не меняется, так как мы выполняем поворот по оси Y?.

Дальше нужно убрать из расчета длину COXA, т.е. перейдем к плоскости X??Y??, для этого выполним сдвиг координаты x? точки на длину COXA:

$$

$$

После всех этих манипуляций дальнейшее решение задачи сводится к нахождению углов a и b треугольника:


Перед нахождением углов необходимо найти третью сторону C этого треугольника. Это расстояние ни что иное как длина вектора и рассчитывается по формуле:

$\$\$display\$\$C\; =\; \backslash sqrt\{x??^2\; +\; y??^2\}\; =\; \backslash sqrt\{140.28^2\; +\; (-20)^2\}\; =\; 141.70\$\$display\$\$$

Теперь необходимо выполнить проверку, сможет ли конечность дотянуться до этой точки. Если С больше чем сумма длин FEMUR и TIBIA, то точка является не достижимой. В нашем случае 141.70 < 141 + 85 — точка достижима.

Теперь нам известны все стороны треугольника и можно найти нужные нам углы a и b используя теорему косинусов:

$$

$$

Полученные углы не применимы для скармливания их сервоприводам, так как тут не учитывается начальное положение и угол наклона прямой С к оси X. Если начальные углы FEMUR и TIBIA нам известны (135° и 45°), то угол наклона C к оси X нам не известен. Найти его можно используя функцию atan2(y??, x??):

$\$\$display\$\$?\; =\; atan2(y??,\; x??)\; =\; atan2(-20,\; 140.28)\; =\; -8.11°\$\$display\$\$$

Наконец-то можно рассчитать угол поворота сервоприводов FEMUR и TIBIA:

$$

$$

Давайте проверим наши расчеты:


Вроде похоже на правду.

Итог

Рассчитанные углы COXA, FEMUR и TIBIA пригодны для скармливания их сервоприводам. Можно заметить, что угол COXA отрицательный и соответственно возникает вопрос: «А как повернуть привод на -11.3 градуса?». Хитрость состоит в том, что я использую нейтральное положение сервопривода COXA в качестве логического нуля, это позволяет вращать привод как на положительные углы, так и на отрицательные. Это конечно очевидно, но я думаю будет не лишним об этом упомянуть. Более подробно расскажу об этом в следующих статьях, когда буду рассказывать о реализации всего выше упомянутого.

Исходники

Алгоритм в действии

Тут случайно получилась последовательность, чем-то напоминающая танец

P.S.

Буду рад если кто-нибудь сможет упростить этот алгоритм. Я делал его таким, чтобы понять его спустя, допустим, пол года.