73717

Уравнения кинематики манипулятора

Лекция

Физика

Манипулятор состоит из последовательности твердых тел (или звеньев), первое из которых соединено с опорной стойкой, а последнее снабжено рабочим инструментом

Русский

2014-12-19

1011.5 KB

17 чел.

54

Лекция 6

Уравнения кинематики манипулятора

 Рисунок 6.1. Система координат схватa

Однородная матрица , определяющая положение i-й системы координат относительно базовой системы координат, представляет собой произведение последовательности однородных матриц преобразования i-1Ai и имеет вид:

0Ti= 0Ai 1Ai i-1Ai===для i=1, 2, …, n,

где  - матрица, определяющая ориентацию i-й системы координат, связанной с i-м звеном, по отношению к базовой системе координат. Это верхняя левая подматрица , имеющая размерность 3×3.

рi- вектор, соединяющий начало базовой системы координат с началом i-й системы координат. Это верхняя правая подматрица матрицы , имеющая размерность 3×1. В частности, при i=6 мы получаем матрицу , которая задает положение и ориентацию схвата манипулятора относительно базовой системы координат. Эта матрица часто используется при описании кинематики манипулятора. Ее называют «матрицей манипулятора».       

      Положим, что матрица Т имеет следующий вид:

T====,

где n – вектор нормали к схвату. В случае плоскопараллельного движения              пальцев этот вектор перпендикулярен пальцам манипулятора;

      s – касательный вектор схвата. Он лежит в плоскости движения пальцев и указывает направление движения пальцев во время открытия или закрытия схвата;

      a  - вектор подхода схвата. Он направлен по нормали к ладони схвата, (т.е. перпендикулярно плоскости крепления инструмента в схвате);  

      p  - вектор положения схвата. Этот вектор направлен из начала базовой системы координат к началу системы координат схвата, которое, как правило, расположено в точке, являющейся геометрическим центром полностью сжатых пальцев.

Если положение манипулятора в абсолютном пространстве определяется матрицей B, а в схвате манипулятора зафиксирован инструмент, положение которого в системе координат схвата определяется матрицей H, то положение рабочего узла инструмента относительно абсолютной системы координат дается произведением матриц В, 0Т0 и Н, т.е.:

                              .                                 (6-1)

При этом  H ≡ , B ≡ .

Решение прямой задачи кинематики для шестизвенного манипулятора является вычислением T=0A6 с помощью последовательного перемножения шести матриц i-1Ai. Решение этой задачи приводит к единственной матрице Т при заданных и фиксированных системах координат, где  для вращательного сочленения и  для поступательного сочленения. Ограничения определяются только физическими пределами изменения   для каждого сочленения манипулятора.

Матрица T манипулятора Пума имеет вид:

                  T = 0A11A22A33A44A55A6=,                (6-2)

где  ;

;        ; (6-3) ;

       ;

       ;                                                 (6-4)

;

;

;                                                                      (6-5)

;

;

.                               (6-6)

    Например, при      имеем

T=,

что согласуется с выбором системы координат на рис. 5.4.

      Из равенств (6-3) – (6-6) видно, что вычисление матрицы манипулятора Т требует обращения к программам вычисления 12 трансцендентных функций, выполнения 40 умножений и 20 сложений в том случае, если производится только вычисление правой подматрицы Т, имеющей размерность 3×3, а вектор n определяется как векторное произведение векторов s и a(n=s×a). Если объединить d6 с длиной рабочего инструмента, то d6=0, а длина инструмента увеличивается на d6 единиц. Это сокращает объем вычислений до 12 бращений и программ вычисления трансцендентных функций, 35 операций умножения и 16 операций сложения.

Классификация манипуляторов

      Манипулятор состоит из последовательности твердых тел (или звеньев), первое из которых соединено с опорной стойкой, а последнее снабжено рабочим инструментом. Каждое звено соединено не более чем с двумя другими так, чтобы не образовывалось замкнутых цепей. Соединение двух звеньев – сочленение – имеет только одну степень свободы. С учетом этого ограничения интерес представляет два типа сочленений: вращательное и поступательное. Вращательное сочленение допускает только вращение вокруг некоторой оси; поступательное сочленение обеспечивает поступательное движение вдоль некоторой оси при отсутствии вращения (поступательное движение с вращением имеет место в винтовых сочленениях). Звенья манипулятора участвуют в относительном движении, в результате которого достигается определенное положение и ориентация схвата или инструмента.

      Следовательно, рассматривая манипуляторы как некоторые последовательности сочленений и звеньев, их можно классифицировать по типу используемых сочленений и последовательности их расположения в направлении от опорной стойки к схвату. При таком подходе манипулятор Пума следует отнести к классу 6В, а манипулятор «Электроника» - к классу 2П-В-П-В. Здесь «В» обозначает вращательное, а «П» – поступательное сочленение.

Обратная задача кинематики

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

Для того, чтобы решение обратной задачи кинематики было получено в явном виде, необходимо, чтобы конструкция робота удовлетворяла одному из двух условий:

  1.  Оси трех смежных сочленений пересекаются в одной точке.
  2.  Оси трех смежных сочленений параллельны между собой.

Из равенства (4-2) следует вид матрицы манипулятора T:

                          T6==0A1 1A2 2A3 3A4 4A5 5A6.          (6-7)

Из равенства (4-7) видно, что матрица T является функцией синусов и косинусов углов  Приравнивая элементы матриц в левой и правой частях матричного уравнения (4-7), получаем, например, для манипулятора Пума двенадцать уравнений (4-3) – (4-6) относительно шести неизвестных (присоединенных углов). Поскольку число уравнений превышает число переменных, можно сразу сделать вывод о том, что решение обратной задачи кинематики для манипулятора Пума не единственно. Мы рассмотрим два метода решения обратной задачи кинематики: метод обратных преобразований в эйлеровых координатах и геометрический подход, выгодно отличающийся наглядностью.

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

      Задача состоит в том, чтобы, зная трехмерную матрицу поворота и учитывая равенство (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) либо не определены, либо дают низкую точность вычислений.

      Более устойчивый способ определения углов Эйлера для вычисления угла , значения которого лежат в пределах -, использует функции арктангенса 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начала вычисляется вектор, направленный от плеча к запястью. Проекции этого вектора на плоскость xi-1yi-1 используются при нахождении присоединенного угла i-го  сочленения  (i=1, 2, 3) для первых трех сочленений.

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

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

=.            (7-1)

Определение различных конфигураций манипулятора

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

Рисунок 7.1. Определение различных конфигураций манипулятора

      ПРАВАЯ  РУКА: При неподвижном 3-м сочленении увеличение угла приводит к увеличению координаты запястья по оси z0.

      ЛЕВАЯ  РУКА: При неподвижном 3-м сочленении увеличение угла  приводит к уменьшению координаты запястья по оси z0.

      ВЕРХНЯЯ (локоть выше запястья) РУКА: Положение запястья {ПРАВОЙ/ЛЕВОЙ} руки по отношению к системе координат плеча характеризуется {отрицательным/положительным} значением координаты по оси  y2.

      НИЖНЯЯ (локоть ниже запястья) РУКА: Положение запястья {ПРАВОЙ/ЛЕВОЙ} руки по отношению к системе координат плеча характеризуется {положительным/отрицательным} значением координаты по оси  y2.

      КИСТЬ ВНИЗ: Скалярное произведение единичного вектора s системы координат схвата и единичного вектора y5 системы координат (х5, у5, z5) положительно.

      КИСТЬ  ВЕРХ: Скалярное произведение единичного вектора s системы координат схвата и единичного вектора y5 системы координат (х5, у5, z5) отрицательно.

      Каждый из трех индикаторов конфигурации звеньев может быть определен следующим образом:

 РУКА=                                     (7-2)

   ЛОКОТЬ=                               (7-3)

ЗАПЯСТЬЕ=                      (7-4)

      В дополнение к этим индикаторам существует ПЕРЕКЛЮЧАТЕЛЬ:

ПЕРЕКЛЮЧАТЕЛЬ=

                                                                                                    (7-5)

      Значения индикаторов и переключателя задаются исследователем до начала решения обратной задачи кинематики.

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

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

                                   ,                          (7-6)

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

       =.                  (7-7)

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

      Проецируя, как показано на рис. 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)

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

      Чтобы найти , спроектируем вектор p на плоскость x1, y1, как показано на рис. 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)

Лекция 8

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

      Для определения  спроецируем вектор p на плоскость x2, y2  (рис.8.1).

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

Конфигурация

манипулятора

РУКА

ЛОКОТЬ

РУКА∙

ЛОКОТЬ

ЛЕВАЯ ВЕРХНЯЯ рука

 

 

-1

+1

-1

ЛЕВАЯ НИЖНЯЯ рука

 

-1

-1

+1

ПРАВАЯ ВЕРХНЯЯ рука

 

+1

+1

+1

ПРАВАЯ НИЖНЯЯ рука

 

+1

-1

-1

      В соответствии с рис. 8.1, как и в предыдущем случае, возможны четыре различные конфигурации манипулятора. Как показано в табл. 8.1, каждой конфигурации соответствует свое выражение .

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

      Параметр  представляет собой y-ю компоненту вектора, выходящего из начала системы координат (x2, y2, z2) и заканчивающегося в точке пересечения осей последних трех сочленений.

      Из рис. 8.1 получаем следующие равенства, позволяющие определить :

                                         ,                           (8-1)

          ,                         (8-2)

    ,

,    .              (8-3)

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

                   .                                     (8-4)

      Из равенства (8-4) получаем следующие выражения для функций синуса и косинуса угла .

   ,    (8-5)

  .     (8-6)

      Из равенств (8-5) и (8-6) с использованием равенств (8-1) – (8-3) находим решение для :

         .                (8-7)

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

для последних трех сочленений

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

      Для манипулятора Пума это решение можно получить, приводя сочленения в соответствие  со следующими требованиями:

      1. Сочленение 4 должно быть установлено так, чтобы вращением в сочленении 5 можно было совместить ось вращения сочленения 6 с заданным вектором подхода (вектором a матрицы T).

      2. Сочленение 5 должно быть установлено так, чтобы ось вращения сочленения 6 совпадала с вектором подхода.

      3. Сочленение 6 должно быть установлено так, чтобы ось у6 совпала с заданным касательным вектором схвата, определяющим его ориентацию.

      Перечисленные условия соответственно записываются в следующем виде:

                       при  заданном ,            (8-8)

                         при   заданном ,                      (8-9)

          при  заданных и . (8-10)

В равенстве (8-8) векторное произведение может быть как положительным, так и отрицательным. Поэтому возможны два решения для . При равенстве векторного произведения нулю (т.е. ось параллельна a) имеет место вырожденный случай. Это происходит, когда оси вращения 4-го и 6-го сочленений параллельны, и означает, что при данной конкретной конфигурации был бы достаточен пятиосный, а не шестиосный манипулятор.

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

      Обе возможные ориентации запястья (ВВЕРХ и ВНИЗ) определяются ориентацией системы координат схвата (n, s, a) относительно системы координат (x5, y5, z5). Знак векторного произведения в равенстве (8-8) должен быть определен с учетом ориентации n или s по отношению к единичным векторам  или  соответственно, которые в свою очередь ориентированы определенным образом относительно единичного вектора  в соответствии с правилами выбора систем координат.

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

                                                      (8-11)

      В соответствии с рис. 5.4  y5=z4,  и  используя равенство  (8-8) можно представить индикатор ориентации в следующем виде:

                                                 (8-12)

      Таблица 8.2 устанавливает соответствие между ориентацией запястья и различными комбинациями значений индикатора ЗАПЯСТЬЕ и индикатора ориентации, между ориентацией запястья и различными комбинациями значений индикатора ЗАПЯСТЬЕ и индикатора ориентации.

Таблица 8.2. Различные ориентации запястья

Ориентация

запястья

 

или

М-ЗАПЯСТЬЕ∙sign()

КИСТЬ ВНИЗ

 +1                      +1    

КИСТЬ ВНИЗ

 +1                       -1

КИСТЬ ВВЕРХ

  -1                       -1

КИСТЬ ВВЕРХ

  -1                      +1   

       Проецируя систему координат   (x4, y4, z4) на плоскость x3y3 (рис. 8.2) и используя таблицу 8.2, получаем следующие соотношения:

                ,    ,           (8-13) где  и  - соответственно первый и второй столбцы матрицы,                                   M=ЗАПЯСТЬЕ ∙sign(), а функция sign определяется выражением:

                                           sign (x)=                                    (8-14)

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

Таким образом, с помощью индикатора ЗАПЯСТЬЕ и индикатора ориентации решение для  может быть представлено в виде:

,

                                                                                                  (8-15)

В вырожденном случае переменной может быть присвоено любое значение, согласующееся с ориентацией запястья (КИСТЬ ВВЕРХ/ВНИЗ). Это условие всегда удовлетворяется, если положить  равным текущему значению . Кроме того, сменив значение ПЕРЕКЛЮЧАТЕЛЯ, можно получить другое решение для :   =+180

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

Для определения  принимаем, что ось шестого сочленения совпадает с заданным вектором подхода (a=z5). Проецируем систему координат (x5, y5, z5) на плоскость x4y4 (рис. 8.3). Тогда:

                                 ,    ,                         (8-16)

где  и - соответственно первый и второй столбцы матрицы ,

      a –вектор подхода.

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

Таким образом, получено решение для :

                                         =

=,

                                                  .                                            (8-17)

Если , имеет место вырожденный случай.

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

Необходимо получить такую ориентацию схвата, чтобы поднять объект манипулирования. Для этого надо так расположить схват, чтобы s=y6. Проецируя систему координат схвата (n, s, a) на плоскость x5y5, получаем (рис. 8.4):

                                         ,     ,                       (8-18)

где - второй столбцы матрицы , a n и s– соответственно нормальный и касательный векторы матрицы .

Таким образом, для имеем:

=

=,

                                                          .                                      (8-19)

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

 Итак, для шестизвенного манипулятора «Пума» существует восемь решений обратной задачи кинематики. Решения для первых трёх присоединённых углов  обеспечивают требуемое расположение руки (первых трёх звеньев), а углы  обеспечивают заданную ориентацию схвата. Для первых трёх присоединённых углов существует 4 решения: два - для манипулятора с левосторонней конфигурацией и два – с правосторонней. Для каждой  конкретной  конфигурации  манипулятора  равенства   (7-18),  (7-26),

(8-7), (8-15), (8-17), (8-19) дают решение обратной задачи кинематики, причем  также является решением этой задачи (если ПЕРЕКЛЮЧАТЕЛЬ «включен»).

 

Лекция 9

Уравнения вида конфигурации для определения индикаторов конфигурации манипулятора

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

      Для индикатора РУКА, следуя определению  ПРАВОЙ/ЛЕВОЙ руки, уравнение конфигурации можно записать в виде:  

,   

                                                                                                                           (9-1)

где - проекция вектора p (равенство (7-7)) на плоскость -третий столбец матрицы  и .

Возможны следующие варианты:

  1.  Если , реализована конфигурация ПРАВОЙ руки.
  2.  Если , реализована конфигурация ЛЕВОЙ руки.
  3.  Если , конфигурация манипулятора одновременно соответсвует определению как ПРАВОЙ, так и ЛЕВОЙ руки: манипулятор находится внутри цилиндра радиусом d2 (рис. 7.1). В этом случае принимается для определённости, что реализована конфигурация правой руки (РУКА=+1).

      Поскольку знаменатель выражения (9-1) всегда положителен, определение ЛЕВОЙ/ПРАВОЙ конфигурации сводится к определению знака числителя :

                      РУКА=,     (9-2)

где функция  sign определена равенством (8-14). Подстановкой первой и второй компонент вектора р из равенства (7-7) в равенство (9-2) получаем:

РУКА=, (9-3)

      Следовательно, из  уравнения (9-3) значение индикатора РУКА для ПРАВОЙ/ЛЕВОЙ конфигурации манипулятора устанавливается выражением:

            РУКА=      (9-4)

      При выводе уравнения конфигурации для индикатора ЛОКОТЬ используем определение ВЕРХНЕЙ/НИЖНЕЙ руки. Взяв  и индикатор РУКА из табл. 7.1, получим уравнение конфигурации для индикатора ЛОКОТЬ, использующее знак второй компоненты вектора положения матрицы  и индикатор РУКА:

(9-5)

      Для индикатора ЗАПЯСТЬЕ, следуя определению возможных конфигураций запястья (КИСТЬ ВВЕРХ/ВНИЗ), сформируем скалярное произведение единичных векторов s и у5 (или z4).

                                      (9-6)

      Если , значение индикатора ЗАПЯСТЬЕ можно определить из выражения:

                       .            (9-7)

      Объединив равенства (9-6) и (9-7), получим:

   (9-8)

      Полученные уравнения конфигурации позволяют проверить решения обратной задачи кинематики. С их помощью при решении прямой задачи кинематики вычисляются значения индикаторов конфигурации, которые затем используются для решения обратной задачи кинематики (рис. 9.1).

Машинное моделирование

      Для проверки правильности решения обратной задачи кинематики манипулятора Пума, изображенного на рис. 5.4, может быть составлена программа для ЭВМ.

Рисунок 9.1. Блок-схема модели решения обратной задачи кинематики

на ЭВМ.

      Первоначально в программе задаётся положение манипулятора в пределах допустимых значений присоединенных углов (решение прямой задачи кинематики, формирование матрицы манипулятора Т).

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

Динамика манипулятора

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

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

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

       Таким образом, уравнения динамики движения реального манипулятора могут быть получены методами Лагранжа-Эйлера или Ньютона-Эйлера. Уравнения Лагранжа-Эйлера обеспечивают строгое описание динамики манипулятора. Их можно использовать для решения прямой и обратной задачи динамики.

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

      Обратная задача динамики заключается в том, чтобы по заданным обобщённым координатам, скоростям и ускорениям определить действующие в сочленениях манипулятора силы и моменты.  

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

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

Метод Лагранжа-Эйлера

      Полное описание движения манипулятора можно получить, применяя метод Лагранжа-Эйлера для неконсервативных систем. Описав кинематику манипулятора с помощью матричного представления Денавита-Хартенберга, можно получить уравнение динамики. Такое совместное использование Д-Х-представления и метода Лагранжа приводит к компактной векторно-математической форме уравнений движения, удобной для аналитического исследования и допускающей реализацию на ЭВМ.

      Вывод уравнений динамики движения манипулятора основан на следующем:

  1.  На описании взаимного пространственного расположения систем координат i-го и (i-1)-го звеньев с помощью матрицы преобразования однородных координат . Эта матрица преобразует координаты произвольной точки относительно i-й системы координаты этой же точки относительно (i-1)-й системы координат.

2.   На использовании уравнения Лагранжа-Эйлера:

                         ;        ,            (9-9)

где L-функция Лагранжа (L=K-P);

     K-полная кинетическая энергии манипулятора;

      P-полная потенциальна энергия манипулятора

      -обобщённые координаты манипулятора;

      -первая производная по времени обобщённых координат;

       -обобщённые силы (или моменты), создаваемые в i-м сочленении для реализации заданного движения i-го звена.

      Для того, чтобы воспользоваться уравнением Лагранжа-Эйлера, необходимо выбрать систему обобщённых координат. Обобщённые координаты представляют собой набор координат, обеспечивающий, полное описание положения рассматриваемой физической системы в абсолютной системе координат. Существуют различные системы обобщенных координат, пригодные для описания простого манипулятора с вращательными и поступательными сочленениями. Однако, поскольку углы поворотов в сочленениях непосредственно доступны измерению с помощью потенциометров или других датчиков, то они составляют наиболее естественную систему обобщенных координат. В этом случае обобщённые координаты совпадают с присоединенными переменными манипулятора. В частности, если i-е сочленение вращательное, то  , если же i-е сочленение поступательное, то .

Скорость произвольной точки звена манипулятора

      Для того, чтобы воспользоваться уравнениями Лагранжа-Эйлера, необходимо знать кинетическую энергию рассматриваемой физической системы, а следовательно, и скорости всех её точек.

      Рассмотрим произвольную точку, неподвижную относительно i-го звена и заданную в системе координат i-го звена однородными координатами  (рис. 9.2):

                                             .                 (9-10)

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

Рисунок 9.2. Точка  i-го звена

      Тогда связь между  и  определяется соотношением:

                                                        ,                                          (9-11)

где   .                                                                                 (9-12)

      Если i-е сочленение – вращательное, то матрица  имеет вид:

             ,       (9-13)

      Если i-ое сочленение – поступательное, то матрица  имеет вид:

                   .            (9-14)

      В общем все ненулевые элементы матрицы  являются функциями величин  и , причём в зависимости от типа j-го сочленения  или  представляет собой присоединенную переменную этого сочленения, а остальные величины – известны (задаются конструкцией манипулятора). В выводах уравнений движения, как вращательных, так и поступательных, используется обобщённые координаты , , если i-е  сочленение – вращательное и , если i-е сочленение – поступательное).

      Скорость точки  относительно базовой системы координат (при ):

       .    (9-15)         

      Частные произведение матрицы  по переменным  легко вычисляется с помощью матрицы , которая для вращательного сочленения имеет вид:

                                             ,                                     (9-16а)

а для поступательного сочленения:

                                             .                                     (9-16б)

      Используя эту матрицу, можно написать:

                                                  .                                       (9-17)

      Например, для манипулятора с вращательными сочленениями . Используя равенство   (9-13), имеем:  

     Таким образом, для    

                  (9-18)

      По смыслу равенство (9-18) описывает изменение положения точек i-го звена, вызванное движением в j-м сочленении манипулятора. Для упрощения формул введём обозначение , с учетом которого равенство (9-18) можно представить для :

                                           (9-19)

Используя введённое обозначение, формулу для  можно записать в форме:

                                              .                                 (9-20)

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

                     (9-21)

      Например, для манипулятора вращательными сочленениями при   и  имеем:

.

Лекция 10

Кинематическая энергия манипулятора

      Зная скорость произвольной точки каждого звена манипулятора, найдём кинетическую энергию i-го звена.

      Обозначим через  кинетическую энергию i-го звена (i=1, 2, …, n). Пусть  кинетическую энергию элемента массы dm i-го звена. Тогда:

 . (10-1)

  Здесь вместо скалярного произведения используется оператор  (след матрицы ), что в дальнейшем позволит перейти к матрице инерции  i-го звена.

      Подставляя  в выражение (10-1) значение  из равенства (9-20), получим выражение для кинетической энергии элемента массой dm:

                                                                                                                         (10-2)

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

      Данная матрица одинакова для всех точек i-го звена и не зависит от распределения массы в этом звене, также как и . Таким образом:    

                 . (10-3)

Интегральный член в скобках представляет собой матрицу инерции  i-го звена:

          .     (10-4)

Преобразуя выражения, получим:

,  (10-5)

где  однородные координаты центра масс i-го звена в i-й системе координат;

      - тензор инерции, где i, j, k принимают значения xi, yi, zi (оси i-ой системы координат), а  - символ Кроникера.

 Формулу (6-26) можно также записать в виде:

. (10-6)

 Здесь  и  j, k=1, 2, 3, а - радиус вектор центра масс i-го звена в системе координат i-го звена. Таким образом, полная кинетическая энергия манипулятора равна:

. (10-7)

      Отметим, что величина Ji (i=1, 2,…, n) зависит только от распределения массы i-го звена в i-й системе координат и не зависит ни от положения, ни от скорости звеньев. Это позволяет однажды вычислив матрицу Ji, использовать полученное значение в дальнейшем для вычисления кинетической энергии манипулятора.

Потенциальная энергия манипулятора

      Обозначим полную потенциальную энергию манипулятора через  Р, а потенциальную энергию i-го звена – через . Тогда:   

                    .     (10-8)

      Суммируя потенциальные энергии всех звеньев, получаем:

                                   .                         (10-9)

 Здесь  - вектор-строка, описывающая гравитационное ускорение в базовой системе координат. В земной системе координат , а g – ускорение свободного падения на поверхности Земли (g=9,8062 м/с2).

Уравнение движения манипулятора

      Используя равенства (10-7) и (10-9), запишем выражение для функции Лагранжа:

               .   (10-10)

      Подставив это выражение в уравнение Лагранжа, получим выражение для обобщённой силы , которую должен развить силовой привод i-го сочленения, чтобы реализовать задание движение i-го звена манипулятора:

                              (10-11)

                                                   .

      Выражение (10-11) можно представить в более простой форме:

             ,  ,  (10-12)

или в матричном виде:

                 ,         (10-13)

где  - вектор (размерностью n×1) обобщённых сил, создаваемых силовыми приводами в сочленениях манипулятора:

                               ;                     (10-14)

- вектор (размерностью n×1) присоединенных переменных манипулятора:

                                 ;                      (10-15)

- вектор (размерностью n×1) обобщённых скоростей:                          

                                  ;                       (10-16)

- вектор (размерностью n×1) обобщённых ускорений:     

                                 ;                      (10-17)

 D(q) – симметричная матрица размерностью n×n, элементы которой даются выражением:

                             ,    ;             (10-18)

- вектор (размерностью n×1) кориолисовых и центробежных сил:

                                            ,

                                 ,          ,          (10-19)

                               ,  ;         (10-20)

- вектор (размерностью n×1) гравитационных сил:

                                     ,

                              .                     (10-21)

Уравнения движения манипулятора с вращательными сочленениями

      Конкретизация равенств (10-13) – (10-21) для шестизвенного манипулятора с вращательными сочленениями приводит к следующему виду членов уравнения, определяющих динамику движения манипулятора:

 Матрица .  Исходя из равенства (10-18), имеем:

                   ,                  (10-22)

где

,,,

,

,

,

,

,

,

,

,

,

,

,

,

,

,

,

,

,

,

,

.

      Вектор . Коэффициенты при обобщённых скоростях в выражениях (10-18), (10-19) для центробежных и кариолисовых сил можно сгруппировать в матрицы  вида:

         ,    .    (10-23)

      Пусть скорости изменения всех шести присоединенных переменных манипулятора характеризуются вектором :

                               .                               (10-24)

      С учетом (10-23) и (10-24) равенство (10-19) можно представить в виде следующего произведения матриц и векторов:

                                              .                                              (10-25)

      Здесь индекс i указывает номер сочленения (), в котором измеряются моменты и силы центробежного и кориолисового типа.

                                    .                                  (10-26)

 Вектор гравитационных сил . Из равенства (10-21) имеем:

                           ,                (10-27)

где

,

,

,

,

,

.

      Коэффициенты  в выражениях (10-18) – (10-21) являются функциями как присоединенных переменных, так и динамических параметров манипулятора. Их называют динамическими коэффициентами манипулятора. Физический смысл динамических коэффициентов легко понять из уравнений (10-18) – (10-21), описывающих динамику движения манипулятора.

  1.  Коэффициенты , определяемые равенством (10-21), учитывают силу тяжести, действующую на каждое из звеньев манипулятора.
  2.  Коэффициенты , определяемые равенством (10-18),  устанавливают связь действующих в сочленениях сил и моментов с ускорением присоединенных переменных. В частности, при i=k коэффициент  связывает момент , действующий в i-м сочленении, с ускорением i-й присоединенный переменной.  Если , то  определяет момент (или силу), возникающий в i-м сочленении под действием  ускорения  в k-м сочленении. Поскольку матрица инерции симметрична и  то .
  3.  Коэффициенты , определяемые равенствами (10-19) и (10-20), устанавливают связь действующих в сочленениях сил и моментов со скоростями изменения присоединенных переменных. Коэффициент  определяет связь момента, возникающего в i-м сочленении в результате движения в k-м и m-м сочленениях, со скоростями изменения k-й и m-й присоединенных переменных. В соответствии с физическим смыслом .

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

  1.  Конкретная кинематическая схема манипулятора может исключить динамическое взаимовлияние движений в некоторых парах сочленений (коэффициенты).
  2.  Некоторые из коэффициентов  присутствуют в формулах (9-20) и (10-19) чисто фиктивно, будучи нулевыми в соответствии с физическим смыслом. Например, коэффициент  всегда равен нулю, так как центробежная сила, порожденная движением в i-м сочленении,  на само i-е сочленение влияния не оказывает, хотя и влияет на другие сочленения, т.е.  при .
  3.  Некоторые из динамических коэффициентов могут принимать нулевые значения в отдельные моменты времени при реализации определённых конфигураций манипулятора

Пример: двухзвенный манипулятор

      Применение уравнений Лагранжа-Эйлера в форме (6-35) – (6-42) для описания динамики движения манипулятора рассмотрим на примере двухзвенного манипулятора с вращательными сочленениями (рис. 6.3).

      Все оси сочленений рассматриваемого манипулятора параллельны оси z, перпендикулярной плоскости рисунка. Физические характеристики, такие, как положение центра масс, масса каждого звена и выбранные системы координат, указаны ниже. Требуется получить уравнения движения рассматриваемого двухзвенного манипулятора, основываясь на равенствах (6-35) – (6-42).

Рисунок 6.3. Двухзвенный манипулятор

      Примем:

-присоединенными переменными являются  ;

-первое и второе звенья имеют массы  и

-параметры звеньев имеют значения ; ; .

Тогда для матрицы имеем:

              ,          ,

,

где     

      В соответствии с определением матрицы  для вращательного сочленения имеем:

.

      Используя выражение (6-19), получаем:

.

      Аналогично для  и  получаем:

      Полагая, что центробежные моменты инерции равны нулю, получим формулу для матрицы псевдоинерции  :

;       .

      Для определения слагаемых, описывающих центробежное и кориолисово ускорение, воспользуемся равенством (6-40).  Для  i=1 оно дает:

.

      С помощью (6-41) можно получить значения коэффициентов . Подставляя их в предыдущее выражение, имеем:

.

      Аналогично для i=2:

.

Таким образом:

.

      Слагаемые, определяющие влияние гравитационных сил :

      Таким образом, вектор, определяющий влияние силы тяжести:

.

      Окончательно имеем уравнения описывающие динамику движения двухзвенного манипулятора:

,


 

А также другие работы, которые могут Вас заинтересовать

22009. Международные отношения в средние века. Итальянские войны 116.5 KB
  Примером является империя Карла V. Таким образом накануне похода Карла VIII Италия разделилась на 2 лагеря – Милан Венеция Рим – с одной стороны и Неаполь Флоренция – с другой. Флорентийцы восстали и изгнали Медичи Флоренция встретила Карла VIII не как завоевателя а как союзника хотя и пришлось выполнить ряд тяжелых условий – выплатить 120. Задача флорентийского правительства Пьетро Каппони видело главную задачу в том чтобы скорее выпроводить Карла VIII из Флоренции и из Тосканы.
22010. Нидерландская буржуазная революция 133 KB
  Расширялось стойловое содержание скота Голландия а ломовые лошади из Голландии Фрисландии Зеландии шли даже на экспорт. они исполнялись с непреклонной жестокостью и среди уголовных приговоров суда Голландии 2030х гг. Флот одной Голландии в 60 г. Среди городов Голландии на первое место постепенно выдвигается Амстердам по объемам морского флота мореходства рыболовства он перегнал все остальные города.
22011. Османская империя в XIII-первой половине XVII вв. 94 KB
  Расцвет военно-политического могущества Османской империи. Будой была включена в состав Османской империи. Второй соперник Ирана – Египет в контроле над торговыми путями перестал существовать как самостоятельное государство его территории были включены в состав империи. Социальноэкономическое развитие Османской империи определялось прежде всего тем что в рамках этого государства все еще продолжался процесс феодализации и вплоть до XVII в.
22013. Позднее средневековье, или раннее новое время 140 KB
  К началу XVI в. Через французский и немецкий языки слово инженер проникло в Россию в XVII в. Но все же XVI век несмотря на многочисленные технические находки и нововведения еще не был отмечен подлинной технической и технологической революцией.
22014. Позднесредневековый Иран 62 KB
  Запустевшие и заброшенные земли были отданы на льготных условиях землевладельцам с обязательством заселить и обрабатывать их. Знать захватывала земли у мелких феодалов либо путем прямого захвата и насилия либо путем судебных процессов. участок обрабатываемый в течение сезона упряжкой волов мера земли разного размера для разных местностей в ср.000 федданов земли.
22015. Польские земли до XV вв. 115.5 KB
  В Польше некоторое ограничение крестьянских выходов были узаконено для всей Малой Польши Вислицким статутом Казимира III так как села пустеют то мы устанавливаем чтобы из одного села в другое вопреки желанию господина села в котором они живут могло перебраться не больше чем 12 кметя. Изданный одновременно для Великой Польши Пётрковский статут разрешал выход на рождество если за крестьянином не было недоимок. В христианизации Польши большую роль сыграла Чехия. Мешко в борьбе с Чехией овладел Силезией и частью Малой Польши.
22016. Польша в XVI-XVII вв. 89 KB
  В XVI в. Население Польши росло вплоть до середины XVII в. Судя по данным описей второй половины XVI в.
22017. Скандинавия до XV в. 127.5 KB
  Температура января – в Северной Норвегии 0 7 в Южной и Центральной Швеции – от 1 до 3. Климат морской в Норвегии Дании Исландии умеренно континентальный на большей части Швеции. Это было вызвано тем что доля территории Швеции и Норвегии это не касается Дании на которой можно вести земледельческое хозяйство невелика – в Норвегии – 3 в Швеции – 9 в Исландии – около 1 от площади страны. Полная деревня Швеции – 48 дворов.