Метод обратных преобразований

Задача состоит в том, чтобы, зная трехмерную матрицу поворота и учитывая равенство (2-2), представляющее собой выражение этой матрицы через углы Эйлера:

=

, (6-8)

где и ,

определить соответствующие значения углов Записывая это матричное уравнение в форме уравнений для отдельных элементов, получим:

; (6-9а)

; (6-9б)

; (6-9в)

; (6-9г)

; (6-9д)

; (6-9е)

; (6-9ж)

; (6-9з)

. (6-9и)

Из уравнений (6-9и), (6-9е) и (6-9з) получаем, что решение всей системы уравнений (6-9а) – (6-9и) имеет следующий вид:

, (6-10)

, (6-11)

. (6-12)

Полученное решение неустойчиво и плохо обусловлено по следующим причинам:

1. Функция arccos неудобна тем, что точность вычисления ее значения зависит от этого значения.

2. В точках, где sin ( ) принимает близкие к нулю значения, т.е. при »0° или при »180°, равенства (6-11) и (6-12) либо не определены, либо дают низкую точность вычислений.

Более устойчивый способ определения углов Эйлера для вычисления угла , значения которого лежат в пределах -p£ £p, использует функции арктангенса ATAN2(y,x), вычисляющий значение arctg(y/x) с учетом принадлежности аргумента соответствующему квадранту:

(6-13)

Применяя такую обратную тригонометрическую функцию двух аргументов, рассмотрим общее решение.

Элементы матрицы в левой части матричного уравнения (6-8) заданы, а элементы матриц, стоящих в правой части этого уравнения, неизвестны и зависят от Умножая слева матричное уравнение (6-8) на , переносим неизвестную в левую часть, оставляя в правой неизвестные и , и тем самым получаем:

,

или

.

(6-14)

Из равенства элементов (1, 3) (элементов, находящихся на пересечении 1-й строки и 3-го столбца матрицы) в правой и левой частях уравнения (6-14) имеем:

, (6-15)

что в свою очередь дает

. (6-16)

Из равенства элементов (1, 1), (1, 2) в правой и левой частях следует:

, (6-17а)

, (6-17б)

что позволяет найти :

(6-18)

Приравнивая элементы (2, 3), (3, 3) матриц в левой и правой частях уравнения, получаем:

,

, (6-19)

что позволяет найти :

. (6-20)

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

Лекция 7

Геометрический подход

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

По аналогии с геометрией человеческой руки и в соответствии с расположением систем координат звеньев различные конфигурации манипулятора Пума определяются с помощью трех индикаторов конфигурации (РУКА, ЛОКОТЬ, ЗАПЯСТЬЕ). Два индикатора характеризуют взаимное расположение первых трех сочленений, а третий – расположение последних трех. Для шестиосных манипуляторов типа Пума существуют четыре различных решения обратной задачи кинематики первых трех сочленений и каждому из этих четырех решений соответствует по два допустимых решения для последних трех сочленений.

Решение производится в два этапа.

I этап. Cначала вычисляется вектор, направленный от плеча к запястью. Проекции этого вектора на плоскость x_i-1y_i-1 используются при нахождении присоединенного угла i-го сочленения (i=1, 2, 3) для первых трех сочленений.

II этап. Использование предыдущего решения для решения последних трех сочленений, подматрицы поворота матриц ⁰Ти^i-1A_i (i=4, 5, 6) и проекции систем координат звеньев на плоскость x_i-1y_i-1.

Если задана матрица , то, умножив эту матрицу слева и справа на и соответственно, можно вычислить и затем, воспользовавшись указанным способом, получить:

= . (7-1)