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

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

, , (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)