Математическое обеспечение программных реализаций алгоритмов кинематики манипулятора для моделей покраски поверхности тел Текст научной статьи по специальности «Математика»

УДК 621.865.8-5

МАТЕМАТИЧЕСКОЕ ОБЕСПЕЧЕНИЕ ПРОГРАММНЫХ РЕАЛИЗАЦИЙ АЛГОРИТМОВ КИНЕМАТИКИ МАНИПУЛЯТОРА ДЛЯ МОДЕЛЕЙ ПОКРАСКИ ПОВЕРХНОСТИ ТЕЛ

Шабном Мустари1, А.А. Внуков2

2Кафедра кибернетики и мехатроники Российский университет дружбы народов (РУДН) ул. Миклухо-Маклая, 6, Москва, Россия, 117198

!Кафедра кибернетики НИУ ВШЭ

Б. Трехсвятительский пер., д. 3/12, стр. 8, Москва, Россия, 109028

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

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

Позиция захвата манипулятора описывается в декартовых координатах и во внутренних обобщенных относительно положения всех соседних звеньев.

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

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

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

метры (si, a, аг) имеют постоянные значения для звеньев, угол 9г- является переменной величиной при вращении i-го звена относительно i - 1-го. Для линейных степеней свободы параметр si является переменной величиной при выдвижении i-го звена относительно i - 1-го.

Решения прямой и обратной задач кинематики манипулятора. Пусть известны геометрические параметры манипулятора (si, ai, ai) с шестью вращательными парами. Применяем матричный метод.

Прямая задача кинематики: получить позицию конечного звена Т6, зная конфигурацию (9г-, si, ai, аг), углы Qi между звеньями, измеряемые датчиками.

Знаем (9г-, s, ai, аг), обобщенные координаты: 9 = [91, 92, 93, 94, 95, 96]T. Требуется определить положение захвата в пространстве, т.е. матрицу Т6 с координатами начала координат системы координат захвата и направляющих векторов трех ее осей, совпадающих по направлению с векторами a — подхода к цели, o — ориентации последнего звена и n = (ao) — прямого произведения векторов a и o. Углы Эйлера связаны с этой системой координат, описывают ориентации захвата относительно абсолютной системы координат.

Рис. 1. Система координат захвата робота

Алгоритм решения прямой задачи состоит в последовательном преобразовании положения захвата в пространстве в глобальную систему координат: Т6 = = А1 А2 А3 А4 А5 А6, вид матриц показан на рис. 2.

л, =

cos 9, - sin 9, cos а, sin9, cos 9, cos а,

sin 9, sin а, а, cos 9, - cos 9, sinа, а, sin9,

0 sinа cos а, Sl

0 0 0 1

Л 0x ax Px' ... ai a2 a3"

ПУ 0У ay Py Ii ...... a4 a5

nz 0z 0z Pz a6

0 0 0 1

Рис. 2. Вид матриц преобразования координат А i и захвата Т6

T6 =

Шести элементов матрицы Т6 достаточно (см. рис. 2) для описания положения и ориентации захвата в пространстве: px, py, pz — координаты центра захвата; ox, ax, ay — углы Эйлера. Они обозначены a1 = ox, a2 = ax, a3 = px, a4 = ay, a5 = py, a6 = pz.

Обратная задача кинематики: по заданному или рассчитанному положению захвата в матрице Т6 надо получить значения углов 0г- между звеньями для перемещения захвата в требуемое положение в пространстве.

Знаем конфигурацию робота (0г-, а, аг), положение захвата Т6. Требуется

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

Решение обратной задачи кинематики переходит в задачу решения системы нелинейных уравнений с шестью неизвестными а0 = /¡(в1, 02, 03, 04, 05, 06), записанную в матричной форме: /(0) = а, где а1 (ох, ах, рх, ау, ру, р) являются известными элементами в матрице преобразования захвата Т6. Таких элементов в матрице Т6 у робота с 6 степенями свободы может быть от 6 выше диагонали до всех 12 элементов (см. рис. 2).

При использовании метода Ньютона необходимо найти набор из шести обобщенных координат 0 = [01, 02, 03, 04, 05, 06]Т.

Зависимость матрицы Т6 от обобщенных координат можно выразить отрезком степенного ряда Тейлора, ограниченного линейными членами, с центром разложения в точке 0( к):

В нулевом приближении обобщенные координаты имеют произвольные начальные значения 0(о), 0(2О),..., 06О).

При заданных 6 элементах матрицы Т6 соответствующие им равенства образуют систему линейных уравнений для 6 обобщенных координат:

найти обобщенные координаты: 0 = [01, 02, 03, 04, 05, 06]Т.

6 ( Л(к-1)

('«),2 -(<«)(2-1)+1 (161 К)-ец)

]-1 [ а 0 ;,2

('. )м-('.)(4-1)+¿^Г 1) (0(к'-0('-,))

} -1 V а 0 У14

(<6 )23-('. )2г+1 [-а^т1) (0(к)-0(к-1))

;-1 [ а 01 У23

(б )24 = (б )

(k-1) 24

б

+1

j=1

dT

\( k-1)

(б )34 =(t6 )3Г + X

j=1

d е

i У

dT<

24 \( k-1)

V d ei У 34

(e(k) -е(k-1)) (е(k) -е(k-1)).

Из системы линейных алгебраических уравнений (СЛАУ) определяются уточненные значения 9( ) по известным приближенным значениям е(к-1). На следующем шаге метода последовательных приближений уточненные значения е(к) обозначаются е(к-1), подставляются для вычисления в матрицы Т6(к-1) и йТ6(к-1) / йег. Процедура нахождения е( к) из СЛАУ повторяется.

Чем ближе е(к-1) к е(к), тем ближе Т6(к-1) к Т6 и меньше разность е(к) -е(к-1). При разности меньше заданной малой величины в уточнения прекращают.

Для формализации метода применяют Якобиан 1(е„) = йfi/йеj — первую производную, взятую от функции f (е), отражающую малое линейное изменение положения захвата в пространстве при малом изменении всех обобщенных координат ей.

Общий вид Якобиана для случая всех известных элементов в матрице Т6:

" йУ1/ е1 ... й^/ йе6" 1(е п) = й^ / й е, = ... ...

й^2 / йе1 ... й/12 / йе6

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

йТ6 /й% Дег, где йТ6 /йег — г'-й столбец Якобиана.

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

Дх « I де,

где де = [де1; де2, де3, де4, де5, де6]Т.

Вычислим столбец Якобиана:

dT6/de i = A1... dAt/de i ... Аб

где i = 1..б.

Для вращательной оси сустава i матрицы преобразования Ai получаем: cos ef - cos а • sin ef sin a¿ • sin ef a¡ • cos ег. sin ef cos a¿ • cos ef - sin a¿ • cos e¿ a¿ • sin e¿ 0 sin a¿ cos a¿ d¡

0 0 0 1

dA эе,.

эе,.

- 8Ш 9г- - 008 а{ ■ 008 9г-

008 0г. - 008 а{ • 8Ш 9г.

0 0

0 0

81п а{• 008 9г- -а1 • 81п 9г-

81п аг. • 81п 9г- а{ • 008 9г.

0 0

0 0

Теперь вычислим обратную матрицу Якобиана и получим решение:

Л9 » Д^Лх.

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

Учитывая, что столбец Якобиана представляет dT6 М9Ь метод Ньютона в матричном виде записывают так:

_ т(^-1)

Т = Т

+

(к) _

I (0 к)-0к-1),

>(к-1) + Г1^

Т(к-1)).

где 0 = [Л91, Л92, Л93, Л94, Л95, Л96] ; 0(к) = 0( , I (т6

Существует два способа вычисления обратной задачи методом Ньютона: 1) решение системы линейных уравнений, когда столбцы Якобиана на каждой итерации участвуют в формировании системы линейных уравнений, и 2) вычисление обратной матрицы Якоби, когда Якобиан вычисляется целиком, чтобы потом вычислить обратную ему матрицу.

Процесс решения системы линейных уравнений в данном случае как раз и заключается в отыскании обратного Якобиана.

Лх «I Л9,

где Л9 = [Л91, Л92, Л93, Л94, Л95, Л96]Т; Л9 - Г*Лх.

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

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

По ЛР надо найти приращения углов: Л9 = [[Л91, Л92, Л93, Л94, Л95, Л96]Т.

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

Позиционирование — итерационный алгоритм решения СЛАУ вида

Л91 Л92 Л93 Л94 Л95 Л9

dP 1 ох \и61 и 14 и 62 и 14 и 63 и 14 и 64 и 14 и 65 и 14 и 66 и 14

dPoy и61 и 24 и 62 и 24 и 63 и 24 и 64 и 24 и 65 и 24 и 66 и 24

dPoz и 61 и 34 и 62 и 34 и 63 и 34 и 64 и 34 и 65 и 34 и 66 и 34

0 и 61 и13 и 62 и13 и 63 и13 и 64 и13 и 65 и13 и 66 и13

0 и 61 и 23 и 62 и 23 и 63 и 23 и 64 и 23 и 65 и 23 и 66 и 23

0 _ и 61 и 22 и 62 и 22 и 63 и 22 и 64 и 22 и 65 и 22 и 66 и 22

Из этих 6 уравнений могут быть получены приращения обобщенных координат робота де при позиционировании захвата робота-манипулятора.

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

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

дх -I де,

где де = [де1; де2, де3, де4, де5, де6]Т; де - г1^

Позиционирование является численным методом простой итерации, как и ре-

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

( к-1)

к Т6.

Углы Эйлера. Пусть имеются две системы координат с общим началом в неподвижной точке О: неподвижная в пространстве («латинская») начальная система координат х, у, 2 и жестко связанная с телом и движущаяся вместе с ним («греческая») конечная система координат п, С Для определения положения в пространстве тела с неподвижной точкой, имеющего три степени свободы, надо задать три обобщенные координаты.

N линия узлов Рис. 2. Углы Эйлера

Через оси £ и п проводим плоскость Р(£Оп) до пересечения с плоскостью хОу. Линия N пересечения плоскостей Р и хОу называется линией узлов, а угол у между осью х и линией узлов N—углом прецессии.

Угол у полностью определяет положение линии узлов N в пространстве. Поворот плоскости Р относительно линии узлов N происходит без изменения

угла у. Система координат с осями £, п, С вращается без изменения угла у относительно оси £ (ось Z). Угол собственного вращения ф в плоскости Р между линией узлов и осью £ фиксирует положение в плоскости Р осей £ и п греческой подвижной системы координат. Когда углы у, ф фиксированы, у тела остается лишь одна степень свободы: не меняя углы у, ф, можно повернуть тело вокруг линии узлов. Чтобы фиксировать поворот тела вокруг линии узлов Ы, введем угол нутации 9 между осью 2 и осью £ (ось 2). Задание трех углов у, ф, 9 полностью определяет положение греческой подвижной системы относительно латинской неподвижной системы координат и полностью определяет положение тела. Эйлеровы углы у, ф, 9 можно независимо менять без изменения двух других углов и выбрать обобщенными координатами тела с неподвижной точкой О.

Углы Эйлера у, ф, 9 определяют три а, в, у поворота системы с целью привести любое положение системы к текущему положению. Повороты а, в, у некоммутативны, и конечное положение системы зависит от порядка совершения поворотов. Для углов Эйлера сначала производится поворот на угол а = у вокруг оси 2 (неподвижной в пространстве («латинской») начальной системы координат), потом поворот на угол в= 9 вокруг оси N линии узлов, и последним поворот на угол у = ф вокруг оси 2 (движущейся вместе с телом («греческой») конечной системы координат).

Углы Эйлера не единственно возможный выбор обобщенных координат.

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

Рис. 3. Математическое обеспечение траекторного движения захвата

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

свободы. Основная цель — реализация позиционирования захвата, которая в своей работе использует решения прямой и обратной задач. Программа может быть использована для моделирования движения робота-манипулятора по определенной траектории. Программа состоит из трех частей: решение прямой задачи, решение обратной задачи и позиционирование захвата робота. В качестве технологии реализации пользовательского интерфейса выбрана библиотека ASP.NET, позволяющая создавать приложения с веб интерфейсом, а также библиотека Iridium, поддерживающая математические вычисления.

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

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

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

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

Web-приложение зарегистрировано в Реестре программ для ЭВМ 14.09.2011, № 2011617167.

ЛИТЕРАТУРА

[1] Шахинпур М. Курс робототехники / Под редакцией С.Л. Зинкевича. — М.: Мир, 1990.

[Shakhinur M. Kurs robototekhniki / Pod redaktciei C.L. Zinkevicha. — M.: Mir, 1990.]

[2] Амосов А.А., Дубинский Ю.А., Копченова Н.В. Вычислительные методы для инженеров:

Учеб. пособие. — М.: Высшая школа, 1994. [Amosov A.A, Dubinskii IU.A., Kopchenova N.V.

Vychislitelnye metody dlia inzhenerov: Ucheb. posobie. — M.: Vyssh. shk., 1994.]

[3] AS язык программирования. Руководство по программированию. Kawasaki Heavy Industries, Ltd, 2002. [AS iazyk programmirovaniia. Rukovodstvo po programmirovaniiu. Kawasaki Heavy Industries, Ltd, 2002.]

[4] Фу К., Гонсалес Р., Ли К. Робототехника / Под ред. В.Г. Градецкого. — М.: Мир,1989. [Fu K., Gonsales R., Li K. Robototekhnika / Pod red. V.G. Gradetckogo. — M.: Mir, 1989.]

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

[6] Пол Р. Моделирование, планирование траекторий и управление движением робота-манипулятора. — М., 1976. [Pol R. Modelirovanie, planirovanie traektorii I upravlenie dvizheniem robota-manipuliatora. — M., 1976.]

MATHEMATICAL SOFTWARE IMPLEMENTATIONS OF ALGORITHMS OF KINEMATICS MANIPULATOR MODELS FOR PAINTING OF THE SURFACE BODIES

Shabnom Mustary1, A.A. Vnukov2

Department of Cybernetics and Mechatronics Russian University of Friendship of the People (RUFP)

Miklukho-Maclay str., 6, Moscow, Russia, 117198

'Department of Cybernetics B. Trekhsvyatitelsky lane, 3/12, p. 8, Moscow, Russia, 109028

The article discusses the mathematical model of the forward task of kinematics, inverse, positioning capture of the robot is considered. Software implementation of these tasks allowed conducting the study of convergence and accuracy solution of inverse problem selecting the initial values and ranging of angles on each iteration and getting graphs of dependencies accuracy of iterations and reducing the angle. For more accurate solution of inverse problems of kinematics is applied a solution of the task of positioning to better reflect the real situation capture what is specified by the transformation matrix capture T6. Results are used to paint on the surface of the bodies.

Key words: industrial robot, forward task of kinematics, inverse task of kinematics, capture positioning, modeling, accuracy.