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

Вектор p, выходящий из начала системы координат плеча (x₀, y₀, z₀) и заканчивающийся на пересечении в точке пересечения осей трех последних сочленений, определяется выражением:

, (7-6)

что соответствует вектору положения матрицы :

=. (7-7)

Решение для первого сочленения

Проецируя, как показано на рис. 7.1, вектор р на плоскость x₀, y₀, получаем следующие уравнения для определения угла :

, , (7-8)

, , (7-9)

, , (7-10)

, , (7-11)

где индексы L и R означают ЛЕВУЮ и ПРАВУЮ конфигурацию манипулятора.

Рисунок 7.1. Решение для 1-го сочленения

Из уравнений (7-8) – (7-11) получаем значения функций синуса и косинуса угла для ЛЕВОЙ/ПРАВОЙ конфигурации манипулятора:

, (7-12)

, (7-13)

, (7-14)

. (7-15)

Объединив равенства (7-12) – (7-15) и используя индикатор РУКА для учета ЛЕВОЙ/ПРАВОЙ конфигурации манипулятора, получаем значения функций синуса и косинуса угла в следующем виде:

, (7-16)

. (7-17)

В этих равенствах используется положительное значение квадратного корня, а индикатор РУКА определен равенством (7-2). Для вычисления , лежащего в пределах , воспользуемся функцией арктангенса, определенной равенством (6-13). Из равенств (7-16) и (7-17) с учетом равенства (6-13) получаем следующую формулу для определения :

. (7-18)

Решение для второго сочленения

Чтобы найти , спроектируем вектор p на плоскость x₁, y_1, как показано на рис. 7.2.

Рисунок 7.2. Решение для 2-го сочленения

В соответствии с этим рисунком возможны четыре различных конфигурации манипулятора. Каждой конфигурации соответствует свое значение угла при и (табл. 7.1):

Таблица 7.1. Угол при различных конфигурациях манипулятора

Конфигурация манипулятора		РУКА	ЛОКОТЬ	РУКА· ЛОКОТЬ
ЛЕВАЯ ВЕРХНЯЯ рука		-1	+1	-1
ЛЕВАЯ НИЖНЯЯ рука		-1	-1	+1
ПРАВАЯ ВЕРХНЯЯ рука		+1	+1	+1
ПРАВАЯ НИЖНЯЯ рука		+1	-1	-1

Как следует из табл. 7.1, используя индикаторы конфигурации РУКА и ЛОКОТЬ, для можно записать единое для всех возможных конфигураций манипулятора выражение:

, (7-18)

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

, , (7-19)

, (7-20)

, (7-21)

,

(7-22)

. (7-23)

Из равенств (7-18) – (7-23) можно определить значение функций синуса и косинуса угла :

, (7-24)

. (7-25)

Равенства (7-24) и (7-25) позволяют найти значение :

. (7-26)