Рисунок 3.5. Структура матрицы Т₆ и единичные векторы, определяющие ориентацию схвати.

Иногда вводят ещё более глобальную систему координат, чем та, что у стойки. Если матрицей преобразования между базовой и глобальной системами является матрица R, то позиция схвата относительно глобальной системы координат (R0) описывается следующим образом:

Т6=А1·А2·А3·А4·А5·А6

R0=R·T6 R6

R = А0·T6 R6

R0=R·T6 (3.4)

где R-1 - координаты точки в глобальной, удаленной от робота системе координат.

3.3 Обратная задача кинематики робота-манипулятора

Обратная задача кинематики. Способы решения

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

Матрица Т6 выглядит следующим образом:

(3.5)

Получается, что элементы матрицы Т₆ как произведения шести матриц А также содержат синусы и косинусы углов (неизвестных углов ?_i). Каждому элементу матрицы 4х4, некоторые элементы которой представляют собой сумму произведений синусов и косинусов от неизвестных обобщённых координат соответствует другая матрица 4х4 элементы которой представляют собой числа.

Поэлементное сопоставление двух матриц в результате даёт систему шести нелинейных уравнений, так как независимыми в матрице Т₆ являются шесть элементов (к примеру над диагональных):

(3.6)

Выбор шести элементов из двенадцати с аналитической точки зрения можно обосновать как выбор независимых уравнений. Если рассматривать матрицу Т₆ не как источник уравнений, а как способ описания положения схвата в пространстве, то этих шести элементов достаточно для описания позиции схвата (а₃, а₅, а₆ - координаты центра схвата; а₁, а₂, а₄ описывают углы Эйлера, то есть ориентацию схвата в пространстве). Поэтому такой способ описания позиции может применяться также как и описание в обобщённых координатах.

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

(3.7)

Или в матричной форме:

f(?)=a (3.8)

Существует два типа решений: аналитическое в закрытой форме и численное. Кроме того есть также и геометрический способ для манипуляторов с определённой конструкцией.

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

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

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

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

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

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

*Углы Эйлера

Углы Эйлера определяют три поворота системы, которые позволяют привести любое положение системы к текущему. Обозначим начальную систему координат как (x,y,z), конечную как (x,y,z). Пересечение координатных плоскостей (xy) и (XY) называется линией узлов N.



Рис. Углы Эйлера

· Угол ? между осью (x) и линией узлов.

· Угол ? между осями (z) и (Z).

· Угол ? между осью (X) и линией узлов.

Повороты системы на эти углы называются прецессия, нутация и поворот на собственный угол (вращение). Такие повороты некоммутативны и конечное положение системы зависит от порядка, в котором совершаются повороты. В случае углов Эйлера это последовательность 3,1,3 (Z,X,Z), т.е. производится сначала поворот на угол ? вокруг оси (z) , потом поворот на угол (?) вокруг оси (N), и последним поворот на угол ? вокруг оси Z.

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

Якобиан - первая производная, взятая от функции f (?). Якобиан правильнее было бы назвать матрицей Якоби, так как, строго говоря, Якобиан это определитель матрицы Якоби, однако в робототехнике эту матрицу принято называть Якобианом. Выглядит он следующим образом:

 (3.9)

где n - число степеней свободы.

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

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

Рис. 3.7. Линейное изменение позиции схвата в пространстве при изменении одной обобщённой координаты.

При изменении i-й обобщённой координаты на ??_i изменение положения схвата можно приблизительно вычислить так:

?x???_i (3.10)

где - i-й столбец Якобиана. Этот столбец состоит из 12 элементов.

Приближённое изменение положения схвата в пространстве при изменении всех обобщённых координат:

?x ? J ??,где ??=[??₁, ??₂, ??₃, ??₄, ??₅, ??₆]^T (3.11)

В более общем случае Якобиан должен иметь размер 12х6 (3.12), но так как независимых уравнений в матрице Т₆ шесть (и их достаточно для определения дифференциального сдвига и поворота), то в данном случае он может иметь меньшую размерность - 6х6.

(3.12)

Найдём столбец Якобиана:

,гдеi=1..6 (3.13)

Если ось сустава является вращательной осью, то тогда получаем для матрицы следующее:

 (3.14)

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

3.4 Решение методом Ньютона

Численный метод решения обратной задачи кинематики основан на методе Ньютона (упрощённом методе Ньютона), который является эффективным методом решения нелинейных задач.

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

При использовании метода Ньютона необходимо найти набор из шести обобщённых координат . Заданными считаем шесть над диагональных элементов матрицы T₆. Значения ?_i определим путём последовательных приближений. Необычность данного решения состоит в том, что метод Ньютона применяется к функции от шести неизвестных, несмотря на это процедура абсолютно та же, как и в случае с одной.

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

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

(3.15)

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

Одному этому матричному равенству соответствуют 16 скалярных равенств (по числу элементов матрицы ). Так как шесть над диагональных элементов матрицы заданы, то соответствующие им равенства образуют систему линейных уравнений для шести обобщенных координат:

(3.16)

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

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

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

С учётом того, что является столбцом Якобиана, то формулу метода Ньютона можно в матричном виде переписать следующим образом:

, где =[??₁, ??₂, ??₃, ??₄, ??₅, ??₆]^T (3.17)

(3.18)

Получается, что существуют две трактовки формулы, соответственно есть два способа вычисления методом Ньютона, первый при помощи решения системы линейных уравнений, а второй при помощи вычисления обратной матрицы Якоби (jacobian inverse). В первом случае столбцы Якобиана на каждой итерации участвуют в формировании системы линейных уравнений, а во втором Якобиан вычисляется целиком, чтобы потом подсчитать обратную ему матрицу. Эти две точки зрения, по сути, являются одной, так как процесс решения системы линейных уравнений в данном случае как раз и заключается в отыскании обратного Якобиана.

3.5 Позиционирование захвата робота

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

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

(3.19)

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

Уравнение (3.19) можно представить в матричной форме, и оно будет иметь вид:

(3.20)

Сначала возьмем вектор , для него имеет место:

(3.21)

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

 (3.22)

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

Для вектора подхода a в общей матрице :

(3.23)

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

(3.24)

Из общей матрицы возьмем вектор нормали:

 (3.25)

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

(3.26)

Итоговая система уравнений расширяется следующим образом: 2 уравнения берутся из системы (3.24) и одно из системы (3.26) посредством этого достигается постоянство ориентации захвата робота при позиционировании.

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

(3.27)

Из этих 6 уравнений могут быть получены приращения координат робота при позиционировании схвата робота-манипулятора (рисунок 3.8). Расстояние получаем как разность между матрицей точного положения схвата и той, что была вычислена при решении обратной задачи кинематики. Кроме этого при предположении постоянности положения точки захвата робота, ориентация захвата будет изменена, при этом могут быть получены соответствующие значения для из системы линейных алгебраических уравнений. В зависимости от требуемой точности данная процедура может повторяться, при этом первоначальная позиция, полученная при решении обратной задачи кинематики корректируется на каждом шаге при помощи полученных приращений обобщённых координат.

Рисунок 3.8. Позиционирование схвата робота.

Задача позиционирования по аналогии с обратной задачей кинематики может быть решена с точки зрения Якобиана:

?x ? J ??, где ?? = [??₁, ??₂, ??₃, ??₄, ??₅, ??₆]^T (3.28)

?? ? J^-1?x (3.29)

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

3.6 Построение траектории

Траектория движения манипулятора - последовательность позиций, через которые проходит захват (TCP), эти узловые точки, в принципе, бывают двух типов:

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

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

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

Б) Промежуточные точки, вводимые специально для коррекции движения манипулятора с целью максимального соответствия выбранной траектории движения.

Рисунок 3.9. Точка подхода и ухода.

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

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

Kawasaki может двигаться как прямолинейно, так и с использованием углового интерполированного движения. Прямолинейные траектории, вероятно, следовало бы избегать, так как создают нежелательные нагрузки на двигательные узлы манипулятора, обрывочные прямолинейные движения постепенно изнашивают манипулятор. Однако в модели FS03N инструкциям движения неявно соответствуют разные значения скоростей и ускорений по умолчанию, поэтому даже прямолинейные траектории с остановкой в каждой позиции не создают нагрузок на приводы. Кроме того существует CP режим (режим непрерывного движения), при установке которого робот будет плавно переходить от одного сегмента движения к другому без остановок (сегмент движения - движение, совершённое одной инструкцией).

В качестве траектории в данной работе выбирается прямолинейная траектория, проходящая через основные позиции (рисунок 3.11). Кроме того будут сгенерированы промежуточные позиции подхода и ухода. Какие либо другие промежуточные точки применяться не будут. Точность робота, а также CP режим, автоматически сглаживающий переходы от одного сегмента движения к другому позволяют применить простую линейную траекторию вместо использования сложной, кривой линии.

Рисунок 3.11. Траектория движения манипулятора.

Рисунок 3.10. Основные точки движения.

В данном случае описанием траектории движения манипулятора является семь позиций (рисунок 3.10.), направление вектора подхода во всех из них кроме последней должно быть перпендикулярно поверхности. Позиции 3, 4, 5, 6 отличаются только координатой центра схвата, а углы Эйлера у них одинаковые. Аналогично позиции 1 и 2 отличаются только координатами центра. Позиция 7 вообще может иметь любую ориентацию в пространстве, положение схвата выбирается произвольно, главное, чтобы оно выходило за определённую черту, за которую можно выбросить готовый продукт, после чего он упадёт в пул готовых продуктов. Позиции 1, 3 и 5 могут служить точками позиционирования, где манипулятор корректирует положение схвата.

Вместе с роботами фирмы Kawasaki поставляется программа PC-ROSET, позволяющая моделировать движения манипуляторов. С помощью этой программы генерируются реальные позиции, которые могут быть вставлены в управляющую программу на языке AS. Выбрав соответствующие значения декартовых или обобщённых координат, можно добиться нужной позиции, изменение положения захвата при этом визуализируется (рисунок 3.12.):

Рисунок 3.12. Позиция захвата объекта.

В соответствии с построенной траекторией при помощи программы PC-ROSET формируется набор позиций в декартовых координатах (таблица 3.1).

Таблица 3.1. Позиции манипулятора, выраженные через декартовы координаты и углы Эйлера.

	X	Y	Z	O	A	T
1	-149	500	-312	-62	178	-89
2	-147	505	-312	76	178	-120
3	-108	531	-135	89	89	-115
4	-108	657	-6	89	89	-115
5	-28	533	-6	89	89	-115
6	67	652	-6	89	89	-155
7	154	498	-304	80	179	54

4. Разработка программной реализации алгоритмов

4.1 Прямая задача кинематики

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

Рисунок 4.1. Реализация решения прямой задачи кинематики.

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

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



Рисунок 4.2. Алгоритм решения прямой задачи кинематики.

I - единичная матрица

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

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



Рисунок 4.3. Реализация решения обратной задачи кинематики.

На практике это означает, что точка, описываемая начальными углами должна недалеко отстоять от точной позиции, которая соответствует матрице Т₆.

Алгоритм решения обратной задачи кинематики можно разделить на две части: во-первых, сам алгоритм, реализующий итерационный процесс, а во вторых алгоритм получения столбцов Якобиана, который является составной частью первого. В данном случае каждое новое уточнённое значение обобщённых координат находится путём решения системы линейных уравнений, процесс формирования этой системы требует получения не всего Якобиана целиком (как в случае с вычислением обратной матрицы), а только очередного его столбца. Всего система линейных уравнений состоит из двенадцати элементов, однако независимыми из них являются только шесть, что позволяет сформировать квадратную матрицу. При решении обратной задачи можно использовать и все 12 уравнений, что улучшит точность решения системы на каждом очередном этапе, однако библиотека Iridium позволяет решать системы линейных уравнений, которые можно описать в матричном виде, как A*X=B, только в том случае, если матрица A является квадратной, то есть когда число неизвестных соответствует числу независимых уравнений. Если бы решение рассматривалось не с точки зрения СЛАУ, а Якобиана, то найти обратную матрицу можно было бы только если она является квадратной, однако и здесь можно было бы использовать все 12 уравнений, так как тогда вычислялась бы псевдообратная матрица, которая соответствует не всем свойствам обратной, а только части из них.

При формировании СЛАУ необходимо получать столбцы Якобиана (рисунок 4.4):

Рис. 4.4. Алгоритм получения столбцов Якобиана.

Как видно от прямой задачи кинематики он отличается только заменой соответствующей i-й матрицы на другую - матрицу дифференциального сдвига и поворота, которая определяет малое линейное изменение локальной системы координат i-го звена и как следствие малое изменение положения схвата манипулятора. Изменение положения схвата с учётом изменения всех звеньев определяет весь Якобиан как совокупность столбцов, отвечающих за соответствующие звенья.

В итоге получаем матрицу 4х4 из которой потом выбираем шесть элементов, соответствующих столбцу Якобиана:

 (4.1)

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

Итерационный алгоритм методом Ньютона можно описать следующим образом (рисунок 4.5):

Рис. 4.5. Итерационный численный алгоритм методом Ньютона.

Критерием окончания служит соответствие матрицы Т_n, полученной на определённом шаге, точной матрице Т₆. Для проверки вычисляется модуль разности матриц, а затем первые 12 элементов суммируются, эта сумма (sum) и сравнивается с числом ?:

T6minusTn = T6-T6n;

sum = 0.0;

for i = 1:3

for j = 1:4

sum = sum + abs(T6minusTn(i,j));

end

end

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

1.9956

1.9987

2.0008

1.9999

2.0034

1.9987

Это решение совпадает с точными углами. А график на рисунке 4.8 соответствует решению:

0.2213

-3.0899

-2.1496

-2.6052

2.0615

0.4732

При этом матрица Т_n в данном случае совпадает с матрицей Т₆.

Рисунок 4.6. График зависимости точности (sum) от итераций.

Кроме того при решении системы линейных уравнений получаются большие углы в радианах:

20.677282242454829

-55.709237201657174

-23.063980202317705

72.151914198811085

-48.154640688892563

-23.248819247347353

Их приходится уменьшать до тех пор, пока они не станут находиться в диапазоне от - до . Если это не делать на каждой итерации - точность решения упадёт, а в определённых случаях решение даже перестанет сходиться. Сходимость без уменьшения углов (рисунок 4.7) хуже, чем с их корректировкой (рисунок 4.8).

Рисунок 4.8. График зависимости точности (sum) от итераций с уменьшением углов.

Рисунок 4.7. График зависимости точности (sum) от итераций без уменьшения ? углов.

Вероятно, это связано с тем, что хотя угол ?+2?n и эквивалентен углу ?, тем не менее, их синусы и косинусы могут незначительно отличаться при вычислении на компьютере, а так как при решении прямой задачи кинематики и Якобиана соответствующие математические функции используются интенсивно, то в сумме получается заметная погрешность.

4.3 Позиционирование

Позиционирование - третий, заключительный этап работы программы, который напрямую зависит от результатов решения обратной задачи кинематики, так как, по сути, уточняет это решение (рисунок 4.9). Единственный параметр, который необходимо ввести - точность, после чего можно запустить позиционирование. Конечным результатом является вычисление приращений обобщённых координат, которыми необходимо скорректировать решение обратной задачи кинематики, конечная цель - ещё большее соответствие реального положения схвата тому, что задано матрицей Т₆.

Рисунок 4.9. Реализация решения задачи позиционирования.

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

Как и решение обратной задачи, позиционирование можно рассматривать либо с точки зрения решения СЛАУ либо вычисления обратной матрицы, что в данном случае одно и то же.

Алгоритм решения задачи позиционирования такой (рисунок 4.10):



Рисунок 4.10. Алгоритм решения задачи позиционирования.

Для более точного решения обратной задачи кинематики на рисунке 4.3. результат позиционирования следующий (рисунок 4.11):



Рисунок 4.11. Реализация решения задачи позиционирования для более точного решения обратной задачи кинематики.

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

Рисунок 4.12. Реализация менее точного решения обратной задачи кинематики.

Рисунок 4.13. Реализация решения задачи позиционирования для менее точного решения обратной задачи кинематики.

Заключение

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

В данном дипломе c учётом реальных возможностей модели Kawasaki FS03N построена траектория движения манипулятора, которую он должен воспроизводить при покраске объектов, также разработан набор алгоритмов и их программная реализация, позволяющая моделировать конкретный шестизвенный манипулятор с шестью вращательными степенями свободы. С помощью этих алгоритмов, во-первых по конкретным координатам точек траектории может быть сгенерирован набор позиций, который должен быть вставлен в программу управления на языке AS, а во-вторых их можно использовать в самой системе управления роботом с целью позиционирования. Все эти средства позволяют организовать процесс покраски объектов при помощи робота-манипулятора.

Библиографический список

1. Пол Р. Моделирование, планирование траекторий и управление движением робота-манипулятора. - М., 1976, C.16 -26.

2. М. Шахинпур. Курс робототехники. Под редакцией С.Л. Зинкевича. - Москва “Мир”,1990. -- 380с.

3. Bill Goodwine. Inverse Kinematics. University of Notre Dame, 2005. -- 30c.

4. Niels Joubert. Numerical Methods for Inverse Kinematics. UC Berkeley, 2008. -- 8c.

5. AS язык программирования. Руководство по программированию. Kawasaki Heavy Industries, Ltd, 2002. -- 335с.

6. К. Фу, Р. Гонсалес, К. Ли. Робототехника. Под редакцией В.Г. Градецкого. - Москва “Мир”,1989. -- 620с.

7. Bill Baxter. Презентация «Fast Numerical Methods for Inverse Kinematics». University of North Carolina at Chapel Hill, 2000.

8. Амосов А.А, Дубинский Ю.А, Копченова Н.В. Вычислительные методы для инженеров: Учеб. пособие. - М.: Высш. Шк., 1994, С.105-113, 201-207.

9. Jorge Angeles. Fundamentals of robotic mechanical systems: theory, methods, and algorithms. Springer-Velag New York, 2003. -- 545с.

10. Craig, J.J. Introduction to Robotics Mechanics and Control. Addison-Wesley, Reading, MA, 1989. -- 463c.

11. Marcello Restelli. Презентация «Robot Kinematics». Politecnico di Milano, 2008.

12. Sami Haddadin. Презентация «Robotics: Inverse kinematics». Institute of Robotics and Mechatronics, German Aerospace Center (DLR), Germany, 2009.

13. Мэтью Мак-Дональд, Марио Шпушта. Microsoft ASP.NET 3.5 с примерами на C# 2008 для профессионалов. -М., «И. Д. Вильямс», 2008. -- 1424 с.

14. Vaclav Hlavac. Презентация «Robot Kinematics». Czech Technical University, Faculty of Electrical Engineering, Department of Cybernetics.

15. Гильман А.М., Первин Ю.А. Dynamic Organisation of Files in Management Control Systems (Динамическая организация файлов в АСУП). - В кн.: Материалы Международного конгресса ИФИП, Любляна, Югославия, 1971.

16. Myke Predko, `123 Robotics experiments'.

17. Первин Ю.А. О простых и иерархических системах корректировки информационных массивов в АСУП. - В кн.: Математические модели в автоматизированных системах управления производством, М., 1971.

18. Летюшова О.И., Первин Ю.А., Шевякова Т.К. Параллельная корректировка семейства структуры изделия. - В кн.: Труды Всесоюзной конференции по математическому обеспечению АСУП, Новосибирск, 1972.

19. Первин Ю.А. Списковые структуры в информационных подсистемах АСУП с последовательным доступом. - В кн.: Материалы 4-го Всесоюзного семинара по экспериментальным задачам управления, Ленинград-Бологое, 1972.

20. Матэр Е.А., Первин Ю.А. Динамическая структура информационных массивов в АСУП. - В кн.: Прикладная математика и кибернетика. Наука, Москва-Горький, 1973.

21. Гильман А.М., Корионова Г.П., Матэр Е.А., Первин Ю.А. Математические вопросы организации и построения на ЭВМ базисной подсистемы АСУП. - В сб: Информационные материалы Научного Совета по комплексной проблеме «Оптимальное планирование и управление народным хозяйством», М., 1969.

22. Искусственный интеллект. Современный подход, С. Рассел, П. Норвиг, 2-е изд, ИД Вильямс, 2007 г. 1408 стр., с ил.; ISBN 978-5-8459-0887

23. Интеллектуальные роботы, И.А. Каляев, В.М. Лохин, И.М. Макаров, под общ. ред. Е.И. Юревича, Машиностроение, 2007 г., 360 стр.,

24. Компьютерное зрение. Современный подход, Д. Форсайт, Д. Понс , ИД Вильямс, 2004 г., 928 стр.,

25. Основы управления манипуляционными роботами, С.Л. Зенкевич, А.С. Ющенко, МГТУ им. Н.Э. Баумана, 2005 г., 480 стр.,

26. Основы робототехники, Е.И. Юревич, БХВ-Петербург, 2005 г., 408 стр.

кинематика манипуляционный робот алгоритм

Приложение

Реализация алгоритмов прямой, обратной задач кинематики, а также позиционирования в среде MATLAB.

function Main()

clc;

Theta = [2.0; 2.0; 2.0; 2.0; 2.0; 2.0];

Alpha = [-90; 0; 0; 90; -90; 0];

Apar = [0; 30; 40; 50; 0; 0];

Dpar = [250; 0; 0; 0; 0; 80];

T6 = SolveForvardKinematics(Theta, Alpha, Apar, Dpar);

Theta0 = [1.5; 1.5; 1.6; 1.6; 1.7; 1.7];

display(Theta);

display('----------------------- Positioning ---------------------------');

Angles = SolveReverseKinematics(Theta0, Alpha, Apar, Dpar, T6, 0.0000001);

display(Angles);

Tpos = SolveForvardKinematics(Angles, Alpha, Apar, Dpar);

display(T6); display(Tpos);

dT6 = [T6(1,4)-Tpos(1,4);T6(2,4)-Tpos(2,4);T6(3,4)-Tpos(3,4)];

dTheta = GetPositioningAngles(dT6, T6, Angles, Alpha, Apar, Dpar, 0.0001);

display(SolveForvardKinematics(Angles+dTheta, Alpha, Apar, Dpar));

display(Angles+dTheta);

end

%----------------------------------------------------------------

function Res = SolveForvardKinematics(Theta, Alpha, Apar, Dpar)

Res = eye(4,4);

for i = 1:6

Th = Theta(i); Al = Alpha(i); A = Apar(i); D = Dpar(i);

Buf = [cos(Th), -cos(Al)*sin(Th), sin(Al)*sin(Th), A*cos(Th);

sin(Th), cos(Al)*cos(Th), -sin(Al)*cos(Th), A*sin(Th);

0.0, sin(Al), cos(Al), D;

0.0, 0.0, 0.0, 1.0];

Res = Res * Buf;

end

end

%----------------------------------------------------------------

function Res = SolveReverseKinematics(Theta0, Alpha, Apar, Dpar, T6, eps)

ThetaN = Theta0;

sum = 0.0;

for counter = 0:50000

A = zeros(6);

B = zeros(6, 1);

T6n = SolveForvardKinematics(ThetaN, Alpha, Apar, Dpar);

TminusT = zeros(6, 1);

TminusT(1,1) = T6(1, 2) - T6n(1, 2);

TminusT(2,1) = T6(1, 3) - T6n(1, 3);

TminusT(3,1) = T6(1, 4) - T6n(1, 4);

TminusT(4,1) = T6(2, 3) - T6n(2, 3);

TminusT(5,1) = T6(2, 4) - T6n(2, 4);

TminusT(6,1) = T6(3, 4) - T6n(3, 4);

B(1,1) = TminusT(1,1);

B(2,1) = TminusT(2,1);

B(3,1) = TminusT(3,1);

B(4,1) = TminusT(4,1);

B(5,1) = TminusT(5,1);

B(6,1) = TminusT(6,1);

for i = 1:6

delta = GetDerive(i, ThetaN, Alpha, Apar, Dpar);

A(1, i) = delta(1, 2);

A(2, i) = delta(1, 3);

A(3, i) = delta(1, 4);

A(4, i) = delta(2, 3);

A(5, i) = delta(2, 4);

A(6, i) = delta(3, 4);

B(1,1) = B(1,1) + delta(1, 2)*ThetaN(i);

B(2,1) = B(2,1) + delta(1, 3)*ThetaN(i);

B(3,1) = B(3,1) + delta(1, 4)*ThetaN(i);

B(4,1) = B(4,1) + delta(2, 3)*ThetaN(i);

B(5,1) = B(5,1) + delta(2, 4)*ThetaN(i);

B(6,1) = B(6,1) + delta(3, 4)*ThetaN(i);

end %i

ThetaNew = linsolve(A, B);

for i = 1:6

while(abs(ThetaNew(i))>(pi))

ThetaNew(i) = ThetaNew(i) - (abs(ThetaNew(i))/ThetaNew(i))*2*pi;

end

end

T6n = SolveForvardKinematics(ThetaNew, Alpha, Apar, Dpar);

T6minusTn = T6-T6n;

sum = 0.0;

for i = 1:3

for j = 1:4

sum = sum + T6minusTn(i,j)*T6minusTn(i,j);

end

end

%Graph(counter+1)=sum;

if (sum < eps)

%plot(Graph);

Res = ThetaN;

return;

end

ThetaN = ThetaNew;

end %counter

Res = ThetaN;

end

%----------------------------------------------------------------

function Res = GetDerive(ThetaIndex, Theta, Alpha, Apar, Dpar)

Res = eye(4,4);

for i = 1:6

Th = Theta(i); Al = Alpha(i); A = Apar(i); D = Dpar(i);

if i == ThetaIndex

Buf = [-sin(Th), -cos(Al)*cos(Th), sin(Al)*cos(Th), -A*sin(Th);

cos(Th), -cos(Al)*sin(Th), sin(Al)*sin(Th), A*cos(Th);

0.0, 0.0, 0.0, 0.0;

0.0, 0.0, 0.0, 0.0];

else

Buf = [cos(Th), -cos(Al)*sin(Th), sin(Al)*sin(Th), A*cos(Th);

sin(Th), cos(Al)*cos(Th), -sin(Al)*cos(Th), A*sin(Th);

0.0, sin(Al), cos(Al), D;

0.0, 0.0, 0.0, 1.0];

end;

Res = Res * Buf;

end

end

%----------------------------------------------------------------

function Res = GetPositioningAngles(dT6, T6, Angles, Alpha, Apar, Dpar, eps)

sum = 0.0;

for counter = 0:50000

A = zeros(6); B = zeros(6, 1);

B(1, 1) = dT6(1);

B(2, 1) = dT6(2);

B(3, 1) = dT6(3);

for i=1:6

delta = GetDerive(i, Angles, Alpha, Apar, Dpar);

A(1, i) = delta(1, 4);

A(2, i) = delta(2, 4);

A(3, i) = delta(3, 4);

A(4, i) = delta(1, 3);

A(5, i) = delta(2, 3);

A(6, i) = delta(1, 2);

end

dTheta = linsolve(A, B);

Angles = Angles+dTheta;

Tpos = SolveForvardKinematics(Angles, Alpha, Apar, Dpar);

dT6 = [T6(1,4)-Tpos(1,4);T6(2,4)-Tpos(2,4);T6(3,4)-Tpos(3,4)];

sum = 0.0;

T6minusTn = T6 - Tpos;

for i = 1:3

sum = sum + T6minusTn(i,4)*T6minusTn(i,4);

end

Graph(counter+1)=sum;

if (sum < eps)

Res = dTheta;

plot(Graph);

display(counter);

return;

end

end

Res = dTheta;

end

As

>point st

X[mm] Y[mm] Z[mm] O[deg] A[deg] T[deg]

19.189 500.493 -163.541 16.458 175.912 -172.144

Change? (If not, Press RETURN only.)

>point a

X[mm] Y[mm] Z[mm] O[deg] A[deg] T[deg]

-149.324 500.170 -312.694 -62.625 178.689 89.959

Change? (If not, Press RETURN only.)

>point b

X[mm] Y[mm] Z[mm] O[deg] A[deg] T[deg]

-147.307 505.258 -135.254 76.795 178.645 -120.480

Change? (If not, Press RETURN only.)

>point c

X[mm] Y[mm] Z[mm] O[deg] A[deg] T[deg]

-108.108 531.104 -6.018 89.992 89.843 -115.431

Change? (If not, Press RETURN only.)

>point d

X[mm] Y[mm] Z[mm] O[deg] A[deg] T[deg]

-108.140 657.350 -6.038 89.995 89.846 -115.428

Change? (If not, Press RETURN only.)

>point f

X[mm] Y[mm] Z[mm] O[deg] A[deg] T[deg]

-28.820 533.110 -6.042 89.996 89.847 -115.434

Change? (If not, Press RETURN only.)

>point e

X[mm] Y[mm] Z[mm] O[deg] A[deg] T[deg]

67.332 652.843 -6.057 89.994 89.849 -115.432

Change? (If not, Press RETURN only.)

>point g

X[mm] Y[mm] Z[mm] O[deg] A[deg] T[deg]

67.269 540.402 -6.043 90.003 89.849 -115.429

Change? (If not, Press RETURN only.)

>point h

X[mm] Y[mm] Z[mm] O[deg] A[deg] T[deg]

154.224 498.795 -304.789 80.486 179.687 54.000

Change? (If not, Press RETURN only.)

>edit printer

.Program printer()

18?s

1 LMOVE st

1?p

1 LMOVE st

2 LAPPRO a,100

3 LMOVE a,1

4 LMOVE b

5 LMOVE c

6 LMOVE d

7 TWAIT 5

8 LMOVE f

9 LMOVE e

10 TWAIT 5

11 LMOVE g

12 DRIVE 4,190

13 ALIGN

14 DRIVE 6,-15

15 LAPPRO h,100

16 LMOVE h,-1

17 LMOVE st

18?e

Размещено на Allbest.ru