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

УДК 531.133.3

МЕТОД ИТЕРАТИВНОГО РЕШЕНИЯ ОЗК С ИСПОЛЬЗОВАНИЕМ

АЛГЕБРЫ КВАТЕРНИОНОВ

Р.Н. Яковлев, И.В. Ватаманюк, О.Ю. Сивченко

В работе представлен численный метод решения ОЗК, а также концепция описания РС в контексте кинематики. В соответствии с предложенной концепцией для позиционирования каждого отдельного узла РС используются его глобальные координаты, а также некоторая система отсчета, отвечающая за ориентацию. Основная часть работы посвящена разработанному методу решения ОЗК, который является пригодным для применения на робототехнических системах с узлами, неспособными к полному пространственному движению. Предлагаемое решение ОЗК основано на модернизации эвристического алгоритма FABRIK (Forward and Backward Reaching Inverse Kinematics) и принимает во внимание особенности разработанной концепции представления робототехнических систем.

Ключевые слова: обратная задача кинематики (ОЗК), эвристический алгоритм FABRIK, кинематическая схема, аналитическое решение ОЗК, численные методы решения ОЗК.

Введение. Существующие аналитические и численные методы решения обратной задачи кинематики (ОЗК) зачастую не в полной мере отвечают требованиям современной робототехники. К примеру, нахождение точного решения в виде аналитических зависимостей обобщенных координат от конструктивных параметров и заданного целевого положения представляется возможным не для всех робототехнических систем (РС) [1, 2]. Например, оси части смежных сочленений РС должны пересекаться в одной точке или должны быть параллельны либо перпендикулярны друг другу [1]. Аналитическое решение, таким образом, возможно только для определенных конструкций РС. Существует ряд методов, предназначенных для упрощенного получения аналитических выражений [1, 3]. В случае, если решение обратной задачи в виде аналитических выражений невозможно, используются численные методы [4-6]. Как правило, таким методам требуются значительные вычислительные мощности, что затрудняет их использование в режиме реального времени.

Рассмотрим современные методы решения ОЗК. В работе [7] предлагается специализированное модифицированное итеративное решение ОЗК с использованием матриц Якоби: сперва определяется некоторое смещение, затем положение РС корректируется на величину этого смещения и процесс повторяется до тех пор, пока не будет достигнуто целевое положение. К преимуществам данного решения следует отнести его высокую степень совместимости с реальными РС, но в тоже время стоит отметить, что сходимость итерационного процесса существенно замедляется по мере приближения к цели, таким образом время, затрачиваемое на получение итогового решения, слишком велико для использования методики в реальном времени, что является основным недостатком такого подхода.

419

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

В работе [9] представлен итеративный алгоритм NIKA для РС, аналитическое решение ОЗК для которых не представляется возможным. Предлагаемый метод использует особый итерационный алгоритм, минимизирующий ошибку позиционирования конечного узла РС, а сама РС в данном случае представляется в виде нескольких связанных систем отсчета, которые жестко совмещены с положениями узлов системы. К достоинствам данной методики следует отнести простоту практического применения метода благодаря способу представления РС, но использование данного метода на реальных системах предполагает проведение предварительного анализа РС с целью привязки систем отчета к узлам данной системы и формализацию взаимосвязей между полученными системами отсчета, что делает применяемый метод не универсальным. Скорость работы предложенного решения также недостаточно высока для его использования в режиме реального времени ввиду большего количества матричных вычислений и итерационной природы алгоритма.

В работе [10] описан эвристический метод решения ОЗК FABRIK (Forward and Backward Reaching Inverse Kinematics). Данный метод «прямого и обратного следования» избегает использования углов поворота или матричных представлений, а вместо этого определяет обновленную позицию каждого узла РС посредством вычисления точки на прямой, которая соединяет текущее положение i-го узла системы с обновленным положением i+1-го узла. Алгоритм имеет весьма высокую скорость сходимости, поиск решения занимает всего несколько итераций при достаточно высоких требованиях к точности результата. FABRIK имеет низкую вычислительную сложность и при этом полностью универсален. Однако, алгоритм не учитывает данные об ориентации узлов и предполагает возможность полноценного пространственного движения для каждого отдельно взятого узла РС, что имеет место лишь для крайне узкого круга реальных систем.

В данной работе для решения ОЗК предлагается собственная концепция описания конструкции РС, которая берет за основу классическое представление РС. Алгоритмом, на основе которого предполагается сформировать метод поиска по пространству всевозможных решений, является эвристический алгоритм FABRIK [10].

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

420

произвольной РС. Данная концепция не только описывает положение узлов РС в пространственных координатах, но и задает ориентацию каждого из них. Основные различия в сравнении с классической концепцией Дена-вита-Хартенберга [11] заключаются в описании отдельных узлов РС - при использовании предлагаемой концепции требуемые данные для описания системы являются сравнительно простыми. Так же следует отметить, что подобное представление специально оптимизировано под преобразование ориентации звеньев с помощью алгебры кватернионов [12], которая обладает рядом преимуществ в сравнении с матричными преобразованиями и преобразованиями с помощью углов Эйлера, в первую очередь, за счет меньшей вычислительной сложности. На рис. 1 а приведен пример позиционирования некоторого произвольного узла РС.

а

б

MA(D)

OR(D) OR(A) V

МА(А)

в

г

Рис. 1. Пример позиуионировани: а - позиционирование некоторого узла A задается положением и ориентацией в пространстве; б - общая система описания взаимосвязей произвольного узла A; в - часть РС, представленная в концепции твердого тела; г - результат поворота системы (в) вокруг вектора rot на 45° против часовой стрелки

В рамках предлагаемой концепции задаются: глобальная система координат XYZ, относительно которой задается пространственное положение узла /: Position/ = (p/, OR/ MA/}. Вектор p/ обозначает пространственные координаты узла /. Для описания ориентации используются следующие компоненты: вектор OR/ показывает ось, вокруг которой осуществляется вращение всех последующих узлов системы; вектор MA/ служит для определения допустимых изменений положения /+1-го узла, причем (MA/, OR/) = 0. Кроме того, MA/ указывает направление, соответствующее

421

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

Для представления связей между соседними узлами РС в дополнение к позиционированию последних необходимо описать их взаимное расположение и отслеживать отклонения, для чего была сформирована система полного описания узла. На рис. 1 б в общем виде изображена система из трех соседних узлов В, А, В, которая служит для описания взаимосвязи узлов В и В с узлом А. Компоненты, введенные для позиционирования узла А: МАЛ и ОЯЛ, по правилу правой руки задают локальную систему координат узла. Обозначим оставшийся вектор заданной координатной системы как ТУЛ.

Для описания взаимосвязей, свойственных узлу Л, в первую очередь, определим плоскость МАТБЛ, которая описывается уравнением:

х - Рах МАах ту 1 1 ' Ах

у - Рау МАау ТУау = О,

2 - Раг МАа, ТУаг _

а также обозначим проекции векторов АВматба и АОматэа на эту плоскость как Врго и Брго соответственно. Теперь введем параметры, с помощью которых можно будет полностью описать все взаимосвязи, свойственные данному узлу системы. Данными параметрами являются углы а, у, в, а и в. Здесь а - максимально допустимое угловое смешение Врго относительно МАЛ в плоскости МАТБЛ в соответствии с конструкционными ограничениями РС;

- у такой, что

д

АВ' = дАВд-АВ х Б рщ = О

АВ х оя

АВ х оя

У У

Л б1П —СОБ —

2 2

в такой, что

-1

АВ' = дАВд

АВ х в рщ = О

д =

АВ х оя

АВ х оя

. в в

81П — СОБ — 22

а такой, что <

б'

Б рщ х ма л = О

д

ОЯ л |оя.

. а а

■БШ — СОБ — 2 2

ß такой, что

ma 'А = qma Aq 1 ma 'a = в pro]

|ma '

в .

pro]

ORa ß ß

q = !-—. sin — cos—

ORa 2 2

ße [-a;a]

Таким образом осуществляется позиционирование узла A, а также полностью описываются его взаимосвязи с соседними узлами системы. Введенные углы необходимы для полного описания взаимосвязей узлов РС по аналогии с представлением Денавита-Хартенберга [11]. Их введение, во-первых, сокращает количество вычислительных операций (данные углы рассчитываются единожды; значения у, в, а являются фиксированными и постоянными), а во-вторых - значительно упрощает использование кватернионных преобразований. Теперь рассмотрим разработанную методику решения ОЗК.

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

Часть РС, представленную двумя смежными узлами, можно интерпретировать как некоторое твердое тело до тех пор, пока любое вращение или иное изменение данной системы представляет собой равное эквивалентное преобразование всех ее компонентов. Чтобы сохранить систему из двух узлов корректной при совершении некоторого произвольного вращения, необходимо соблюсти два ключевых условия: ось вращения не должна изменяться независимо от того, с какими элементами системы осуществляется работа, а также все характеристики элементов должны быть выражены относительно некоторой точки приложения вращения. На рис. 1, в) точкой приложения является узел D, соответственно векторы ORD и MAD изначально выражены корректно. Аналогичное состояние у вектора AD, но в случае узла A компоненты ORA и MAA предварительно должны быть переопределены. Соблюдая эти условия, можно осуществлять произвольные преобразования над произвольной системой узлов.

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

а

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

Рис. 2. Алгоритм репозиционироеания PC

Предложенный алгоритм итеративен, каждая следующая итерация принимает на вход результат, полученный после предыдущего запуска алгоритма. Входными данными для алгоритма являются начальное положение узлов n-звенной PC {pi, ORi, MAi, Cn}, а также целевое положение конечного узла {ptg, ORtg, Ctg}. В процессе работы алгоритма последовательно выполняются подциклы прямого (от конечного звена к начальному) и обратного (от начального - к конечному) обхода PC до тех пор, пока не будет соблюдено условие сходимости (расстояние между рассчитанными координатами конечного узла р'п и целевыми координатами меньше заданной погрешности; аналогично для нормализованных векторов ORn и Сп) или же система не перестанет изменяться. В случае недостижимости целевой точки, полученное данным алгоритмом решение будет приближено к цели настолько, насколько это возможно. На рис. 3 представлен алгоритм, осуществляющий репозиционирование отдельных узлов и обработку их связей при прямом (от конца к началу) обходе PC.

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

В первую очередь происходит обработка входных данных, а именно задается исходная конфигурация узлов системы и целевая позиция обрабатываемого узла (рис. 4.1 - узлы Д А, В и целевая позиция В'), затем проводится параллельный перенос части системы с целью восстановления требуемых величин длины звеньев (рис. 4.II - происходит смещение узлов А и В для достижения равенства отрезков В А' и ВА).

424

Начало |

РобШоп, = {р,, 0Р„ МЛ,} > = 1, ..., п-1 робшопп = {рп, 0^, МЛП, Сп}

роэ№оп1д = {рд О^д, Сд}

Рис. 3. Алгоритм репозиционирования узла при прямом (от конечного узла к начальному) обходе РС

Далее происходит формирование кватерниона, осуществляющего вращение части системы с целью совмещения координат, текущего и целевого положений обрабатываемого узла (рис. 4.111 - посредством кватерниона совмещаются узлы В и В', в соответствии с логикой алгоритма ось кватерниона проходит через предыдущий узел А', также воздействие оказывается на параметры узлов В и А'). На рис 4.1У приведен результат применения полученного кватерниона: наблюдается успешное совмещение координат, описанных выше положений обрабатываемого узла, но компоненты, описывающие ориентацию данных положений, тем не менее, отличаются.

По аналогии с предыдущим участком формируется кватернион, который осуществляет вращение системы таким образом, что происходит совмещение ОК-компонент описания ориентации у целевого и текущего положений обрабатываемого узла, ось вращения кватерниона при этом проходит через сам обрабатываемый узел. Результат приведен на рис. 4.У (совмещаются компоненты ОЯВ и ОЯВ', кватернион воздействует на узлы В и А'), следует отметить, что для целевого положения переопределяется МА-компонент ориентации, он заимствуется у текущего положения обрабатываемого узла.

На заключительном этапе определяется актуальное значение компоненты в обрабатываемого узла. Следует подчеркнуть, что при обработке любого узла, чтобы сохранить взаимосвязи, присутствующие в РС, необходимо модифицировать не только выбранный узел, но и предыдущий. Результаты произведенных преобразований представлены на рис. 4.'VI. На рис. 4.VII можно увидеть конфигурацию РС после завершения обработки одного из узлов (обработан узел В).

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

VII).

МА(О)

О^О) "ч МА<А>

\ '^Ч

\ ОЯ(А)/" \ ^А(А')

/ А

/у в С' /у

МА(В')

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

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

Результаты работы алгоритма

№ Количество звеньев РС Номер итерации Координаты конечного узла РС, см Координаты цели, см

X Y Z

1 4 0 3.75 3.75 0.0 X = 5.625 Y = -2.5 Z = 2.5

1 6,154 -2,718 2,195

2 5,903 -2,628 2,366

4 5,686 -2,531 2,493

8 5,628 -2,503 2,524

Потребовалось итераций 8 Погрешность результата, см 0.024

2 5 0 0.0 0.375 -6.25 X = -1.125 Y = 2.25 Z = -5.625

1 -0,451 2,133 -5,638

2 -1,728 2,039 -5,471

5 -1,184 2,201 -5,601

16 -1,146 2,246 -5,621

Потребовалось итераций 16 Погрешность результата, см 0.022

3 6 0 1.0 2.5 -3.125 X = 0.125 Y = 3.5 Z = -0.5

1 0,736 3,39 -1,3

2 0,795 2,671 -2,659

5 -0,458 3,744 -1,525

20 0,144 3,491 -0,505

Потребовалось итераций 20 Погрешность результата, см 0.022

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

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

С другой стороны, за счет использования предложенной концепции описания узлов РС, представленный подход во многих аспектах лишен недостатков эвристического алгоритма FABRIK [9]. Особенно важно отметить возможность применения метода к реальным РС. Так же за счет включения в описание отдельного узла информации о его ориентации стало возможным инвариантное указание целевой позиции для конечного узла, что раньше в рамках данного эвристического алгоритма не представлялось возможным.

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

427

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

Список литературы

1. Peiper D.L. The kinematics of manipulators under computer control // Stanford univ ca dept of computer science. 1968. №. CS-116.

2. Craig J. J. Introduction to robotics: mechanics and control, 3/E. -Pearson Education India, 2009.

3. Paul R.P., Shimano B. Kinematic control equations for simple manipulators //1978 IEEE Conference on Decision and Control including the 17th Symposium on Adaptive Processes. IEEE. 1979. P. 1398-1406.

4. Tsai L.W., Morgan A.P. Solving the kinematics of the most general six-and five-degree-of-freedom manipulators by continuation methods // Journal of Mechanisms, Transmissions, and Automation in Design. 1985. Vol. 107. №. 2. P. 189-200.

5. Nakamura Y., Hanafusa H. Inverse kinematic solutions with singularity robustness for robot manipulator control // Journal of dynamic systems, measurement, and control. 1986. Vol. 108. №. 3. P. 163-171.

6. Angeles J. On the numerical solution of the inverse kinematic problem // The International Journal of Robotics Research. 1985. Vol. 4. №. 2. P. 21-37.

7. Colome A., Torras C. Redundant inverse kinematics: Experimental comparative review and two enhancements // Intelligent Robots and Systems (IROS), 2012. P. 5333-5340.

8. Duka A.V. Neural network based inverse kinematics solution for trajectory tracking of a robotic arm // Procedia Technology. 2014. Vol. 12. P. 2027.

9. Kucuk S., Bingul Z. Inverse kinematics solutions for industrial robot manipulators with offset wrists // Applied Mathematical Modelling. 2014. Vol. 38. №. 7. P. 1983-1999.

10. Aristidou A., Lasenby J. FABRIK: A fast, iterative solver for the Inverse Kinematics problem // Graphical Models. 2011. Vol. 73. №. 5. P. 243260.

11. Hartenberg R. S., Denavit J. A kinematic notation for lower pair mechanisms based on matrices // Journal of applied mechanics. 1955. Vol. 77. №. 2. P. 215-221.

12. Голубев Ю.Ф. Алгебра кватернионов в кинематике твердого тела // Препринты ИПМ им. М.В.Келдыша. 2013. № 39. P. 23.

Яковлев Роман Никитич, младший научный сотрудник лаборатории автономных робототехнических систем, iakovlev. ramail. ru, Россия, Санкт-Петербург, Санкт-Петербургский институт информатики и автоматизации Российской академии наук,

Ватаманюк Ирина Валерьевна, младший научный сотрудник лаборатории автономных робототехнических систем, vatamaniuk. i. vagmail. com, Россия, Санкт-Петербург, Санкт-Петербургский институт информатики и автоматизации Российской академии наук,

Сивченко Олег Юрьевич, программист лаборатории автономных робототех-нических систем, adrelianagmail. com, Россия, Санкт-Петербург, Санкт-Петербургский институт информатики и автоматизации Российской академии наук

A METHOD FOR SOLVING INVERSE KINEMATICS PROBLEM FOR MULTI-JOINT ROBOTIC SYSTEMS BASING ON A NEW CONCEPT OF THE KINEMA TIC SCHEME

DESCRIPTION

R.N. Iakovlev, I. V. Vatamaniuk, O. Yu. Sivchenko

In this paper a numerical method of inverse kinematics problem (IKP) solution is presented, as well a concept of robotic system description in terms of kinematics. According to the proposed concept for positioning of every single joint of robotic system its global coordinates are used, as well some coordinate system, defining the orientation of the system. This work primarily details the IKP solution method, developed here and applicable to the robotic joints with limited spatial freedom. The proposed IKP solution builds upon optimization of the heuristic FABRIK (Forward and Backward Reaching Inverse Kinematics) algorithm and it respects the peculiarities of the developed robotic system presentation approach.

Key words: inverse kinematics, heuristic algorithm FABRIK, kinematical scheme, an inverse kinematics analytical solution, inverse kinematics numerical solution methods.

Iakovlev Roman Nikitich, junior researcher of Laboratory of Autonomous Robotic Systems, iakovlev. ra mail. ru, Russia, St. Petersburg, St. Petersburg Institute for Informatics and Automation of the Russian Academy of Sciences,

Vatamaniuk Irina Valer 'evna, junior researcher of Laboratory of Autonomous Robotic, vatamaniuk. i. vagmail. com, Russia, St. Petersburg, St. Petersburg Institute for Informatics and Automation of the Russian Academy of Sciences,

Sivchenko Oleg Yur'evich, software Engineer of Laboratory of Autonomous Robotic Systems, adrelianagmail. com, Russia, St. Petersburg, St. Petersburg Institute for Informatics and Automation of the Russian Academy of Sciences