CC BY
59
УДК 621.865.8-5
ИССЛЕДОВАНИЕ ЭФФЕКТИВНОСТИ ПРОГРАММНЫХ РЕАЛИЗАЦИЙ АЛГОРИТМОВ КИНЕМАТИКИ МАНИПУЛЯТОРА
Шабном Мустари1, А.А. Внуков2, К.О. Прохоров2
'Российский университет дружбы народов ул. Орджоникидзе, 3, Москва, Россия, 117923
Московский институт электроники и математики НИУ ВШЭ Б. Трехсвятительский пер., 3, Москва, Россия, 109028
Программные реализации математических моделей задач кинематики (прямой, обратной), позиционирования захвата робота позволили исследовать сходимость и точность решений обратной задачи от выбора начальных значений и диапазона углов на итерации, получить зависимости точности от итераций и уменьшения углов, уточнить алгоритмы. Для большего соответствия реального положения захвата, заданного матрицей преобразования захвата Т6, решения обратной задачи кинематики уточняются решением задачи позиционирования.
Ключевые слова: промышленный робот, прямая задача кинематики, обратная задача кинематики, позиционирование захвата, моделирование, управление, точность, сходимость.
Качественное нанесение рисунка и текста на поверхность тел предъявляет высокие требования к точности, скорости и сходимости вычислений [3]. Требуется исследовать и уточнить вычислительные алгоритмы кинематики.
Позиция захвата манипулятора описывается в декартовых и внутренних обобщенных координатах (9г, s) относительно положения соседних звеньев.
Введение локальных систем координат и использование их в матричном преобразовании координат c вращательными соединениями манипулятора создают проблемы решения нелинейных уравнений обратной задачи кинематики из-за тригонометрических функций неизвестных углов 9г.
Параметры (а, а) характеризуют конструкцию г-го звена — его длину и угол скручивания. Для вращательных степеней свободы параметры звеньев (s, a, аг) имеют постоянные значения, угол 9г является переменной величиной при вращении г-го звена относительно [5], для линейных — параметр st является переменной величиной выдвижении г-го звена относительно г\.
Прямая задача кинематики. Решение прямой задачи кинематики является первым этапом работы программы (рис. 1): вводятся параметры конфигурации манипулятора (s, a, а), обобщенные координаты 9г, получаем матрицу положения Т6 = А' • А2 • А3 • А4 • А5 • А6 [3—5]. Параметры конфигурации манипулятора сохраняются, что позволяет в дальнейшем не вводить их на втором этапе при решении обратной задачи. Углы между сочленениями — обобщенные координаты 9г указываются в радианах. Линейные величины измерены в миллиметрах [6].
Кинематика робота-манипулятора
Прямая задача кинематики Обратная задача кинематики Позиционирование манипулятора
РЕШЕНИЕ ПРЯМОЙ ЗАДАЧИ КИНЕМАТИКИ РОБОТА-МАНИПУЛЯТОРА
& a a d Тб
|2 | [-90 115 1|250 | 1-0,197911 Oj [0.70553243| [5.680481 Б0| |43.6240918|
|2 ||0 ||30 ||[> | |0.7355Б907| |[>.ББ575058| |-0,372Б45^| |-24,4470B2j
|2 |[о ||40 ||[> | 1-0,54789611] [[>.42Б790ББ|Р0,Б309361[||214.Б90827|
|2 |[» ||50 ||[> | |0 |[[> |[5 ||' |
|2 ||-90 |[Ь 110- |
|2 I Го 115 1[в5 I
| Рассчитать |
Сохранить конфигурацию
Рис. 1. Реализация решения прямой задачи кинематики
Алгоритм решения состоит в последовательном произведении матриц Аь поставленных в соответствие каждому звену. Матрицы Аь кроме последней А6, преобразуют локальную систему координат i к i - 1 вплоть до базовой системы. Матрица А6 описывает положение захвата манипулятора с точки зрения пятой локальной системы координат. Процесс из цепочки последовательных преобразований координат и порядок умножения матриц имеет значение. Алгоритм решения прямой задачи кинематики в блоке 2 (рис. 5) в начале вычислений присваивает Т6 = I единичную матрицу [6].
Обратная задача кинематики. Для решения обратной задачи следует ввести новую или восстановить введенную на первом этапе конфигурацию манипулятора с матрицей Т6, для которой надо найти углы 9i между сочленениями. Нужно указать точность £ и начальные обобщенные координаты выбранные таким образом, чтобы итерационный процесс решения методом Ньютона сходился (рис. 2). На практике это означает, что точка, описываемая начальными углами, должна недалеко отстоять от точной позиции, соответствующей матрице Т6 [3—5].
Алгоритм решения обратной задачи кинематики состоит из двух частей — реализации итерационного процесса и получения столбцов Якобиана как составной части первой. Каждое новое уточненное значение обобщенных координат 0/k) находится путем решения СЛАУ. Процесс формирования СЛАУ требует получения не всего Якобиана целиком, как в случае с вычислением обратной матрицы, а только очередного его столбца. Всего СЛАУ состоит из 12 элементов, но независимыми из них являются только 6, что позволяет сформировать квадратную матрицу. При решении обратной задачи можно использовать и все 12 уравнений, что улучшит точность решения системы на каждом очередном этапе. Однако библиотека Iridium позволяет решать СЛАУ в матричном виде A-X = B для квадратной матрицы A и число неизвестных соответствует числу независимых уравнений. Если решение рассматривается не с точки зрения СЛАУ, а Якобиана, то найти обратную матрицу можно только, если она является квадратной. Но и здесь можно использовать все 12 уравнений, вычислить псевдообратную матрицу, соответствующую не всем свойствам обратной, а только части из них [3; 4].
а)
б)
Рис. 2. Реализация а) для менее точного е = 10 за 8 шагов решения
и б) для более точного е = 10 решения 8, за к = 9 шагов обратной задачи кинематики
При формировании СЛАУ надо получать столбцы Якобиана (рис. 3).
От алгоритма прямой задачи кинематики он отличается только заменой матрицы Аг на матрицу дифференциального сдвига и поворота / й8ь задающей малое линейное изменение обобщенной координаты А8г звена г и малое линейное изменение йТ6 / • А8г положения захвата, где йТ6 /d8i г-й столбец Якобиана. Изменение положения J(8) • А8 захвата с учетом малых изменений А8 всех 6 звеньев определяет весь Якобиан J(8) = й/\/й8 ^ совокупностью столбцов, отвечающих за соответствующие звенья [1; 4]. В матричной форме уравнение записывается /8) = а, где набор неизвестных обобщенных координат 8 = [81, 82, 83, 84, 85, 86]т, набор известных значений а = [/12,/13,/14,/23,/24,/34]т в матрице Т6 с элементами в виде функций /<8г).
В итоге преобразований получаем матрицу ^Т6(8г)/^8г- размерности 4 х 4, из нее выбираем 6 элементов по числу 8г, соответствующих столбцу Якобиана:
• /12 /13 /14 "
/п • • /23 /24
д8п /34
Биш
=Т - т^=X X и -
,6 п( к )
г=1 ]=1
(вход)
/ те, a, t ~J
^ Готово ^
D,= dA, /dB ,=
- sinO, - cos a, cos 0, sina, cos O, -щ ■ sinO,
cos0, -cosa, sinO, sina, sinO, щ ■ cos0, 0 0 0 0 0 0 0 0
A =
cosO, - sinO, cosa, sinO, sina, a, cosO,
sinO, cosO, cosa, -cosO, sina, a, sinO,
0 sina, cosa, S,
0 0 0 1
Рис. 3. Алгоритмы получения столбцов Якобиана и итерационного процесса
При формировании системы из 12 уравнений надо выбрать 12 элементов получившейся матрицы дифференциального перемещения схвата dT6(9i■)/d9i■.
л6 n
A
6 n
по-
Критерием окончания служит соответствие матрицы Т6п = Л1' ... a6 лученной на шаге итерации п, заданной точной матрице Т6. Для проверки вычисляется модуль разности матриц |Т6 - Т6п|, а затем первые 12 элементов суммируются, эта сумма (sum) и сравнивается с числом е (sum < е) [4]:
T6minusTn = T6-T6n; sum = 0.0; for i = 1:3 forj = 1:4
sum = sum + abs(T6minusTn(i,j)); end end
При разной точности е1 = 0.1 и е2 = 0.001, но одинаковых начальных углах 9/0) = (1.5, 1.5, 1.5, 1, 1, 1) (см. рис. 2) график а) на рис. 4 соответствует и более точному, и менее точному решению на итерациях к = 1, ..., 8. Решения в табл. 1 практически совпадают с точными исходными углами 9г- = (2, 2, 2, 2, 2, 2) (см. рис. 1), заданными в решении прямой задачи.
Таблица 1
Сходимость решений обратной задачи кинематики
Итера- Точ- e1(k) e2(k) ef e;k) e5(k) С
ция k ность
0 1.5 1.5 1.5 1.0 1.0 1.0
8 0.1 1.99558132 1.99869902 2.00082434 1.99987003 2.00339982 1.99865400
9 0.001 1.99998623 2.00000716 1.99999241 2.00000168 2.00001337 1.99999819
В зависимости от подобранных начальных значений решение может не сходиться, они влияют на сходимость к разным решениям, когда их может быть несколько. При одинаковой точности в разные начальные углы могут приводить к решению за 8 итераций в табл. 1 и графику а) рис. 4. График в) рис. 4 соответствует решению, сходящемуся за 19 итераций к другим углам 8 = [81, 82, 83, 84, 85, 86]т = = [0.2213, -3.0899, -2.1496, -2.6052, 2.0615, 0.4732]т. В этом случае матрица Т6п совпадает с матрицей Т6. Значит, есть еще решение.
При решении СЛАУ получаются большие углы в радианах: 8 = [81, 82, 83, 84, 85, 86]т = [20.677282242454829, -55.709237201657174, -23.063980202317705, 72.151914198811085, -48.154640688892563, -23.248819247347353]т. Их надо уменьшать, чтобы они находились в диапазоне от -п до п. Если это не делать на каждой итерации, точность решения упадет, а в определенных случаях решение даже перестанет сходиться. Видно, что сходимость без уменьшения углов по графику б на рис. 4 хуже, чем с их корректировкой по графику в.
а) итерации б) без уменьшения углов в) с уменьшением углов
1 3 5 7 9 11 13 15 17 19 21 23 25 27 29 31 33
Рис. 4. Зависимость точности (sum) а) от итераций, б) от итераций без уменьшения углов, в) от итераций с уменьшением углов
Таблица 2
Зависимость точности (sum) от итераций
Графики на рис. 4 Точность | Т6 - Т6п | < £ Диапазон (по осям координат)
Шкала точности Количество итераций
а 0.1 и 0.001 от 300 • 10-3 до 0 от 0 до 8 и от 0 до 9
б 0.001 от 300 • 10-3 до 0 от 0 до 33
в 0.001 от 300 • 10-3 до 0 от 0 до 19
Из рис. 4 и табл. 2 видно, что вычислительные процессы сходятся за разное число итераций — 8, 9, 33 и 19 при разной и одинаковой точности.
Вероятно, это связано с тем, что хотя угол 0-® + 2пп и эквивалентен углу 0/^, их синусы и косинусы могут незначительно отличаться при вычислении на компьютере. При решении прямой задачи кинематики и вычислении Якобиана математические функции используются интенсивно, и в сумме, возможно, получается заметная погрешность [6].
Позиционирование. Позиционирование — третий, заключительный этап работы программы, зависит от результатов решения обратной задачи кинематики за п итераций 0/и) и уточняет это решение 0прибл. Вводим единственный параметр точности 8 и запускаем позиционирование для вычисления приращений обобщенных координат Д0;, чтобы ими скорректировать 0прибл решение обратной задачи кинематики. Цель — большее соответствие реального положения захвата Т6„, после позиционирования заданному матрицей Т6 [7]. Позиционирование позволяет исправить две неточности: 1) грубое решение обратной задачи для выбора большой точности; 2) отклонения, вызванные самим манипулятором, при износе узлов, что приводит к малому смещению от ожидаемой позиции.
Алгоритм решения задачи позиционирования представлен на рис. 5. При захвате цели он использует каждый раз решение прямой задачи кинематики для уточнения истинного начального положения захвата манипулятора Т6п.
В моделирующей программе начальные данные о положении захвата робота Т6п задаются отводом захвата робота от цели в исходных данных обратной задачи 0/°\ изменением точности решения, конфигурации робота или полученным решением обратной задачи кинематики за п итераций 0/и). На практике начальными данными является решение прямой задачи кинематики после отработки роботом решения обратной задачи кинематики.
Для реального манипулятора Б803К точность позиционирования в программе выбирается на основании технических характеристик точности позиционирования захвата (0.05 мм = 50 • 10-6 м), датчиков обобщенных координат, электроприводов двигателей и исполнительных устройств.
Таблица 3
Сравнительная точность обратной задачи и позиционирования
Решение обратной задачи кинематики Вычисление позиционирования
Переменная Точность £ Итерации к Переменная Точность £ Итерации к
0,<к) 0.1 9 Д0,<") 10-13 1
0<к) 0.001 8 Д0,<п) 10-13 2
(Вход)
^Готово)
(Готово)
Рис. 5. Алгоритм решения прямой задачи и позиционирования
,-3
Для более точного решения обратной задачи кинематики |Т6 - Т6п| < е = 10 за 9 итераций (см. рис. 2) результат позиционирования будет достигаться за 1 шаг при точности позиционирования |Т6 - Т6п| < е = 10-13.
При уменьшении точности вычисления обратной задачи кинематики до 0.1 за 8 итераций (см. рис. 2) робот достигает позиции, расположенной дальше от целевой, поэтому позиционирование при той же точности 10-13 осуществляется за большее число шагов, т.е. за два шага.
Сетевое приложение рег. № 2011617167-14.09.2011 [6; 7] решает с заданной точностью 8 за конечное число итераций (к, n) обратную задачу кинематики 0/к), уточняет ее Л0/п), меняет конфигурацию (0г-, s, a, аг), исследует решения по сходимости к другим углам 0/к), точности соответствия |Г6 - Г6п| < 8, числу итераций к. Затем создание траекторий манипулятора с 6 вращательными степенями свободы модели Kawasaki FS03N реализуется на языке AS [2].
ЛИТЕРАТУРА
[1] Шахинпур М. Курс робототехники / Под ред. С.Л. Зинкевича. — М.: Мир, 1990. [Shakhi-nur M. Kurs robototekhniki / Pod red. S.L. Zinkevicha. — M.: Mir, 1990.]
[2] AS язык программирования. Руководство по программированию. — Kawasaki Heavy Industries, Ltd, 2002. [AS iazyk programmirovaniia. Rukovodstvo po programmirovaniiu. — Kawasaki Heavy Industries, Ltd, 2002.]
[3] Фу К., Гонсалес Р., Ли К. Робототехника / Под ред. В.Г. Градецкого. — М.: Мир, 1989. [Fu K., Gonsales R., Li K. Robototekhnika / Pod red. V.G. Gradetckogo. — M.: Mir, 1989.]
[4] Пол Р. Моделирование, планирование траекторий и управление движением робота-манипулятора. — М., 1976. — C. 16—26. [Pol R. Modelirovanie, planirovanie traektorii i uprav-lenie dvizheniem robota-manipuliatora. — M., 1976. — S. 16—26.]
[5] Прохоров К. О., Внуков А.А. Программа для ЭВМ «Моделирование кинематики робота-манипулятора» // Официальный бюллетень Программы, зарегистрированные в реестре программ для ЭВМ Российской Федерации, RU ОБПБТ № 4(77) 20.12.2011, с. 487 Рег. номер 2011617167 (14.09.2011) Свидетельство о государственной регистрации программ для ЭВМ № 2011617167. Зарегистрировано в Реестре программ для ЭВМ 14 сентября 2011 г. [Prokhorov K. O., Vnykov A.A. Programma dlia EVM <Modelirovanie kinematiki robota-mani-puliatora> zhurnal ofitcialnyi biulleten programmy, zapegistrirovannye v reestre program dlia EVM Rossiiskoi Federatcii, RU OBPBT № 4(77) 20.12.2011, s. 487 Reg. nomer 2011617167 (14.09.2011) Svidetelstvo o gosudarstvennoi registratcii program dlia EVM № 2011617167 za-registrirovano v reestre programm dlia EVM 14 sentiabria 2011 g.]
[6] Лисенков М.А., Внуков А.А. Модель системы параллельной обработки данных для решения задачи преобразования координат в промышленных роботах Инновационные информационные технологии: Материалы международной научно-практической конференции. Т. 3 / Гл. редактор С.У. Увайсов; Отв. редактор И.А. Иванов. С. 232—237. Прага, 22—26 апреля 2013. — М.: МИЭМ НИУ ВШЭ, 2013. [Lisenkov M.A., Vnykov A.A. Model sistemy parallelnoi obrabotki dannykh dlia resheniia zadachi preobrazovaniia koordinatv promyshlennykh robotakh innovatcionnye informatcionnye tekhnologii: Materialy mezhdunarodnoi nauchno-prakticheskoi konferentcii. T. 3 / Gl. redactor S.U. Uvaisov; Otv. Redactor I.A. Ivanov. S. 232—237. Praga, 22—26 aprelia 2013. — M.: MIEM NIU VSHE, 2013.]
RESEARCH OF THE EFFICIENCY OF SOFTWARE IMPLEMENTATIONS OF THE ALGORITHMS OF KINEMATICS OF THE MANIPULATOR
Shabnom Mustary1, А.А. Vnukov2, K.O. Prokhorov2
'Peoples' Friendship University of Russia Ordzhonikidze str., 3, Moscow, Russia, 117923 2Moscow institute of electronics and mathematics National Research University Higher School of Economics B. Trehsvjetitelsky str., 3, Moscow, Russia, 109028
Software implementations of mathematical simulation of direct task of kinematics, reverse task of kinematics and positioning capture of the robot allowed to conduct the study of convergence and accurate solution of inverse problem, selecting the initial values and the range of angles in the iteration and receive accuracy of iterations and reduce the angle, refine the algorithms. To better reflect the real position that is to capture the transformation matrix the solution of the inverse task of kinematics is specified by a decision of the task of positioning.
Key word: industrial robot, direct task of kinematics, reverse task of kinematics, positioning capture, simulation, controlling, accurate, convergence.
