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

При решении данной задачи нам известно:
Точка p0, в которой тело гексапода соединяется с каждой ногой гексапода;
Точка p3, где нога гексапода должна соединяться с землей (рис. 18).
а)
б)
Рисунок 18 – Обозначения расчётных точек
а) Вид сверху, б) вид сбоку
cog – центр тяжести; coxia, femur, tibia – обозначение длин суставов (англоязычное наименование костей животных)
Нужно определить:
18 углов наклона между суставами робота;
12 точек (p1 и p2).
Вектор движения каждой ноги робота — это вектор от точки po к точке p1.
Зная конструкцию робота, можно определить направления шести векторов, проходящих через точки x0-5.
Ось движения каждой ноги робота проходит через углы:
[0, 45, 135, 180, 225, 315] (рис. 19).
Рисунок 19 – Векторы x0-x5
Для каждой ноги робота необходимо определить:
угол beta - угол между вектором коксии (ось x ноги) и вектором femur
угол gamma - угол между вектором tibia и вектором, перпендикулярным к вектору femur.
Решим задачу на конкретном примере.
В рамках решения обратной задачи кинематики необходимо определить углы поворота каждого привода (3 шт.) робота, чтобы переместить его в требуемую точку пространства

. Конструктивные параметры робота известны. На рис. 20 показано требуемое положение правого верхнего сустава робота.
Матрица с координатой точки, куда нужно переместиться имеет вид:
T1=100183,1010149,1001 0000 1
Матрица поворота системы координат на 45 градусов (- означает поворот против часовой стрелки):
Rz=cos⁡(-π4)-sin⁡(-π4)00-sin⁡(-π4)cos⁡(-π4)00001 0000 1
Рисунок 20 – Расчётная модель
Расположим систему координат в центре правой верхней ноги робота. Сначала необходимо повернуть систему координат на 45 градусов, что соответствует конструктивному параметру платформы, т.к. выступающая площадка расположена под углом 45 градусов. Переместить данный сустав робота необходимо в точку с координатами (183,1;149,1).
Модель, с расположенным в нужном положении суставом требуется для проверки расчётов обратной задачи кинематики.
Рисунок 21 – Верхний сустав робота
Повернутая система координат на 45 градусов показана на рис