РЕШЕНИЕ ОБРАТНОЙ ЗАДАЧИ КИНЕМАТИКИ МАНИПУЛЯТОРА Текст научной статьи по специальности «Математика»

ВЕСТНИК ТОМСКОГО ГОСУДАРСТВЕННОГО УНИВЕРСИТЕТА

2021 Управление, вычислительная техника и информатика № 56

УПРАВЛЕНИЕ ДИНАМИЧЕСКИМИ СИСТЕМАМИ

УДК 004.021

Б01: 10.17223/19988605/56/1

С.А. Братчиков, Е.А. Абрамова, Ю.В. Федосов РЕШЕНИЕ ОБРАТНОЙ ЗАДАЧИ КИНЕМАТИКИ МАНИПУЛЯТОРА

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

Ключевые слова: манипулятор; планирование траектории; аналитическое решение; обратная задача кинематики; обобщенные координаты.

Управление любым робототехническим комплексом [1] требует четкого детерминирования траектории всех движущихся частей этого комплекса, так называемого задания траектории. Это обусловлено необходимостью контролировать положение каждого из звеньев механизма для выполнения поставленной задачи [2]. Чаще всего перед робототехнической системой в производственных условиях задача ставится в терминах пространственно-временного состояния в декартовых координатах: в заданный момент времени ^ привести конечную точку робота в точку с координатами х, у, г. Такое задание понятно человеку, который формирует задание для машины, однако кинематическая схема робота не позволяет без обработки и преобразования входного сигнала обеспечить выполнение этого задания и желаемое положение конечной точки. Должно быть осуществлено преобразование к обобщенным координатам манипулятора из входных данных, задаваемых в декартовой системе координат. Процесс такого преобразования называется решением обратной задачи кинематики (ОЗК).

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

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

1. Описание рассматриваемой кинематической схемы

Рассматриваться решение ОЗК будет на примере распространенной на современных производствах типовой антропоморфной кинематической схемы промышленного робота [9, 10] (рис. 1).

Рис. 1. Манипулятор и его обобщенные координаты Fig. 1. Manipulator and its generalized coordinates

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

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

За начальные положения при расчетах (и, следовательно, нулевые углы поворота звеньев относительно друг друга) удобно принимать конфигурацию, при которой плечо расположено строго вертикально, длинная часть предплечья - строго горизонтальна и сонаправлена с осью у, схват - строго горизонтален, ось О4 занимает положение, при котором схват поворачивается в вертикальной плоскости, а инструмент, если бы был прикреплен к схвату, занимал бы строго вертикальное положение. Дальнейшие рассуждения и расчеты строятся исходя из этих начальных условий кинематической схемы.

Предплечье

Рис. 2. Упрощенная кинематическая структура манипулятора в вертикальной плоскости Fig. 2. Simplified kinematic structure of manipulator in the vertical plane

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

2. Аналитическое решение ОЗК для рассматриваемой структуры

Решение ОЗК удобно начать с поиска углов ф1, ф2 и фз. Для этого необходимо определить координаты конечной точки предплечья манипулятора, обозначенной на рис. 2 как D. Это возможно сделать, зная координаты конечной точки схвата (хе, Уе, ze), требуемые углы азимута и возвышения схва-та (s, ф) (рис. 3), а также длину схвата 1з (отрезок DE на рис. 2). Координаты точки D находятся в соответствии с соотношениями

xD = xE -13 cos(s)sin(ф),

yD = Уе -13COS(s)cos(ф), (!)

■d

= ze -13sin(e).

Рис. 3. Вектор желаемого положения и углы, определяющие это положение Fig. 3. Desired position vector and the angles forming this position

Зная координаты этой точки, нахождение углов ф1, ф2, ф3 сводится к решению задачи расположения в вертикальной плоскости треугольника со сторонами ¡1 и ¡2 и с вершинами, находящимися в начале координат и точке D. Однако остается вопрос влияния формы предплечья на соотношения. Он решается введением корректирующего угла acor, который равен углу CDB на рис. 2. Угол CDB находится как arctg (BC / CD). Тогда за длину ¡1 принимается отрезок AB, за длину ¡2 - отрезок BD.

Также следует отметить, что возможна ситуация, когда между осями О1 и О2 существует горизонтальное расстояние (отрезок МА на рис. 2). Решение этой задачи со всеми описанными допущениями записывается через соотношения

ZABD = arceos

12 +-I

xD + yD - ma )

zD

2l1l2

(

Ф1 = arceos

yD

Ф2 = -~2+ arcsin

l2 ■ sin (ZABD )

2 ZD

D

-yD

j

(2)

Л (

+ arcsin

xd + yD -MA ) + zl j x2D + y2D -MA ) + z

zD

2 ZD

Фз =ZABD -аcor --.

После определения улов ф1, ф2, ф3 необходимо найти углы ф4, ф5, которые зададут положение схвата относительно абсолютной системы координат. Однако определенные ранее углы ф1, ф2, ф3 внесли свой вклад в поворот системы координат схвата относительно абсолютной системы координат, что должно быть включено в расчет. Это влияние отражено на рис. 3.

Предположим, что система координат (x, y, z) абсолютная (связана с основанием), и именно относительно нее задается желаемое положение вектора направления схвата R. Углы, которые задают желаемое направление схвата в абсолютной системе кординат: ф - азимут и s - возвышение. Угол азимута соответствует повороту системы координат относительно оси z, а угол возвышения соответствует повороту системы координат относительно оси x. Система координат (x1, y', z') - это система, которая соосна координатой у' с осью предплечья и является базовой для схвата. То есть положение в пространстве системы (x1, у', z') определяет начальное положение схвата, относительно которого будут рассчитываться дальнейшие требуемые углы поворота осей. Это также означает, что положение предплечья в пространстве определяет положение системы координат (x1, у', z'). Стоит отметить, что при выборе таких объектов исследования точка O (начало координат обеих систем) совпадает с точкой E, изображенной на рис. 2, и является центром крепления схвата к предплечью робота.

Из кинематической схемы (см. рис. 1) можно заметить, что показанные на рис. 3 углы а и ß связаны с углами ф1, ф2, ф3 соотношениями

' а = фх

Р = Ф2 +Ф3.

Поскольку ось у в начальном положении сонаправлена с отрезком CD предплечья, то требуемое положение схвата необходимо достичь следующим образом: сопоставить ось у' схвата с направлением желаемого единичного вектора R. Исходя из кинематической схемы манипулятора, этого можно добиться поворотами относительно осей O4 и O5. Поворот относительно оси O4 соответствует углу вращения ф4 и в условиях, показанных на рис. 3, будет соответствовать повороту системы (x1, у', z') относительно оси у', а поворот относительно оси O5 соответствует углу вращения ф5 и в условиях, показанных на рис. 3, будет соответствовать повороту системы (x1, у', z') относительно оси x'.

(3)

2

Можно заметить, что угол ф4 в представленной на рис. 3 конфигурации на самом деле представляет из себя угол между плоскостью, проходящей через векторы у' и г, и плоскостью, проходящей через векторы у' и Я . В то же время угол ф5 представляет из себя угол между векторами у' и Я в плоскости, проходящей через эти самые векторы.

Остается дать определение углу фб, представленному на рис. 1. Для этого нужно определить его физический смысл и то, как составитель траектории манипулятора может его задавать в понятном для человека формате. Физически этот угол удобно понимать как угол между вертикальной плоскостью и продольной плоскостью инструмента, который находится в схвате. Необходимо понимать, что на задание этого угла оказывает влияние поворот схвата на углы ф4 и ф5. Угол на который вносится поправка при задании углов ф4 и ф5, представляет из себя угол между плоскостью, проходящий через векторы г и Я , и плоскостью, проходящей через векторы у' и Я . Углы ф4, ф5, С наглядно представлены на рис. 4.

Рис. 4. Плоскости в системе координат и углы, соответствующие обобщенным координатам Fig. 4. Planes in coordinate system and angles corresponding to generalized coordinates

Угол между плоскостями т и п может быть определен в соответствии с уравнением

( ... „ „ „ „ ■ Л

Ф = arccos

\AmAn + BmBn + CmCn\

№ + Bl + c2J A2 + B2 + C2

(4)

где Л,, Б,, С,, (7 6 т, п) - коэффициенты 7-го уравнения плоскости, заданного в виде:

ЛгХ + Б,у + Сг + Вг = 0.

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

X _ X0i У ~ y0i Z ~ Z0i Xli ~ X0i yli ~ y0i Zli ~ Z0i X2i ~ X0i y2i ~ y0i Z2i ~ Z0i

(5)

где х, у, г - координаты декартова пространства; Хог, уог, го7 - декартовы координаты первой точки, принадлежащей рассматриваемой плоскости 7; Х17, у 17, гц - декартовы координаты второй точки, принадлежащей рассматриваемой плоскости 7; Х27, уъ7, гъ - декартовы координаты третьей точки, принадлежащей рассматриваемой плоскости 7.

Коэффициенты Л7, Б7, С7 для конкретной плоскости находятся как соответствующие коэффициенты при множителях х, у, г определителя (5). Однако следует заметить, что все плоскости, представ-

ляющие интерес в настоящем вычислении, проходят через начало координат (0, 0, 0), что дает возможность упростить определитель (5) до вида

х у г

хи У1/ % , (6)

хи уи

Тогда коэффициенты уравнения плоскости записываются в виде:

Ai - y\iz2i zliy2i,

x1iZ2i z 'x-

1ix2i

),

(7)

Ci - x1iy2i y1ix2i •

Пусть плоскость, проходящая через единичные векторы z и у' (см. рис. 4), обозначается нижним индексом т, плоскость, проходящая через единичные векторы _у' и R. обозначается нижним индексом п, а плоскость, проходящая через единичные векторы z и R, обозначается нижним индексом о. Тогда за точки, которые принадлежат этим плоскостям удобно выбрать концы указанных единичных векторов, а координаты этих точек записать в виде:

( х1т , У1т , z1m ) - ( X1o , У1о, z1o ) - ( ^ 0,1), ( X2m , У2т , z2m )-( X1n , У1И, z1n )-( COS (Р) sin (a) ,COS (р) COS (а) ,SÍn (р)), (8)

( x2n, У2и, z2n )-( X2o , У2о, Z2o )-( COS (s) Sin (ф),COS (s) COS (ф),Sin (s)).

Тогда соответствующие коэффициенты уравнения плоскостей будут иметь вид: Am --cos (р) cos (а), Bm - cos (Р) sin (а),

Cm - 0,

An - cos (s) cos (ф) sin (р) - cos (Р) cos (а) sin (s),

Bn - -cos (s) sin (ф) sin (Р) + cos (р) sin (a) sin (s), (9)

Cn - cos (s) sin (ф) cos (р) cos (a) - cos (s) cos (ф) cos (р) sin (a),

Ao --cos(s)cos(ф), Bo - COS (s) sin (ф),

Co - 0.

Для того чтобы получить угол ф4, необходимо подставить коэффициенты (9) в уравнение (4). В терминах нижних индексов плоскостей, которые были введены ранее, угол ф4 соответствует углу между плоскостями m и п.

Угол ф6 может быть определен как разность между следующими углами: углом поворота 0 продольной плоскости инструмента относительно вертикальной плоскости и углом поправки Z между плоскостями п и o, который определяется в соответствии с выражением (4) на основании коэффициентов (9) и требуемого угла. Требуемый угол 0 задается оператором и является входным параметром для алгоритма решения ОЗК.

Фб-е-?.

(10)

Угол ф5, судя из кинематики манипулятора, соответствует углу между векторами У и й, который удобно определять в системе (х1, у\ z'). В таком случае координаты вектора у' = (0, 1, 0), а вектора R = (cos(s + fS)sin((j) + a), cos(s + f))cos((j) + a), sin(s + P)). Тогда удобно найти угол между векторами, пользуясь определением скалярного произведения векторов. Поскольку вектор у' имеет только координату у, слагаемые, относящиеся к координатам x и z, в скалярном произведении обращаются в ноль, а угол между векторами определяется как

Ф5 = arccos (cos (s + P) cos (ф + а)). (11)

Таким образом, используя шесть входных параметров (xe, yE, ze, в, ф, 9), а также габаритные параметры робота, возможно определить аналитические выражения для обобщенных координат - углов поворота звеньев манипулятора относительно друг друга (ф1, ф2, фз, ф4, ф5, фб) - применяя выражения (2), (4), (10), (11). Однако такой метод требует знания кинематической структуры манипулятора и его габаритов.

Заключение

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

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

ЛИТЕРАТУРА

1. Munasinghe S.R., Nakamura M. Trajectory Planning and Control of Industrial Robot Manipulators // Industrial robotics: theory,

modelling and control / ed. by S. Cubero. Pro Literatur Verlag, 2006. P. 335-348.

2. Kabir A.M., Shah B.C., Gupta S.K. Trajectory planning for manipulators operating in confined workspaces // Proceedings IEEE

14th International Conference on Automation Science and Engineering (CASE). IEEE, 2018. P. 84-91.

3. Dantam N.T. Robust and efficient forward, differential, and inverse kinematics using dual quaternions // The International Journal

of Robotics Research. 2020. V. 4 (40). P. 1-21. DOI: 10.1177/0278364920931948

4. Chen L. et al. Solution of an inverse kinematics problem using dual quaternions // International Journal of Applied Mathematics

and Computer Science. 2020. V. 30, № 2. P. 351-361.

5. Zhao Z., Wang T., Wang D. Inverse kinematic analysis of the general 6R serial manipulators based on unit dual quaternion and

Dixon resultant // Proceedings 2017 Chinese Automation Congress (CAC). IEEE, 2017. P. 2646-2650.

6. Srisuk P., Sento A., Kitjaidure Y. Inverse kinematics solution using neural networks from forward kinematics equations //

2017 9th international conference on Knowledge and Smart Technology (KST). IEEE, 2017. P. 61-65.

7. Kelemen M. et al. A novel approach for a inverse kinematics solution of a redundant manipulator // Applied Sciences. 2018.

V. 8, № 11. P. 9-10.

8. Tian X., Xu Q., Zhan Q. An analytical inverse kinematics solution with joint limits avoidance of 7-DOF anthropomorphic manipu-

lators without offset // Journal of the Franklin Institute. 2021. V. 358, № 2. P. 1252-1272.

9. Wang M. et al. Optimal trajectory planning of free-floating space manipulator using differential evolution algorithm // Advances in

Space Research. 2018. V. 61, № 6. P. 1525-1536.

10. Sciavicco L., Siciliano B. Modelling and control of robot manipulators. 2nd ed. London : Springer Science & Business Media, 2000. 390 р.

Поступила в редакцию 15 апреля 2021 г.

Bratchikov S.A., Abramova E.A., Fedosov U.V. (2021) SOLUTION OF THE INVERSE KINEMATICS PROBLEM OF THE MANIPULATOR. Vestnik Tomskogo gosudarstvennogo universiteta. Upravlenie, vychislitelnaja tehnika i informatika [Tomsk State University Journal of Control and Computer Science]. 56. pp. 4-11

DOI: 10.17223/19988605/56/1

There are a lot of methods for inverse kinematics problem of manipulator solution. But most of them include usage of transcendental equations, numerical methods, non-linear differential equations, and recursive calculations, which are very complicated for application in embedded systems. However, in real-time systems the speed and ease of algorithm are valued the most. These qualities are inherent for analytical solutions, which unequivocally connect the input coordinates of trajectory and generalized coordinates of kinematic scheme of manipulator, A.K.A angles of relative rotation of elements of manipulator.

First, it is needed to solve the simple problem of identification of triangle angles, located in vertical plane. The sides of the triangle are known, as the dimensions of manipulator are given. After doing so, it is possible to use the principles of interplanar angle identification to reach the last generalized coordinates. Also, the method of identifying the angle between two vectors is useful in these calculations.

After calculation of the generalized coordinates according to the described method, it is needed to apply constraints on the signs of the values of these coordinates depending on the desired position since the expressions include absolute value functions and trigonometric functions, which are not unambiguous.

Keywords: manipulator; trajectory planning; analytical solution; inverse kinematics problem; generalized coordinates.

BRATCHIKOVStepan Artemievich (Post-graduate Student of the Control Systems and Robotics Faculty, ITMO University, Saint-Petersburg, Russian Federation). E-mail: Loross90@gmail.com

ABRAMOVA Evgenia Aleksandrovna (Post-graduate Student of the Software Engineering and Computer Systems Faculty, ITMO University, Saint-Petersburg, Russian Federation). E-mail: vectra4444@mail.ru

FEDOSOV Yurii Valerievich (Associate Professor, Candidate of Technical Sciences, Associate Professor of the Control Systems and Robotics Faculty, ITMO University, Saint-Petersburg, Russian Federation). E-mail: yf01@yandex.ru

REFERENCES

1. Munasinghe, S.R. & Nakamura, M. (2006) Trajectory Planning and Control of Industrial Robot Manipulators. In: Cubero, S. (ed.)

Industrial Robotics: Theory, Modelling and Control. Pro Literatur Verlag. pp. 335-348.

2. Kabir, A.M., Shah, B.C. & Gupta, S.K. (2018) Trajectory planning for manipulators operating in confined workspaces. Proceedings

IEEE 14th International Conference on Automation Science and Engineering (CASE). August. pp. 84-91.

3. Dantam, N.T. (2020) Robust and efficient forward, differential, and inverse kinematics using dual quaternions. The International

Journal of Robotics Research. 4(40). pp. 1-21. DOI: 10.1177/0278364920931948

4. Chen, L., Zielinska, T., Wang, J. & Ge, W. (2020) Solution of an inverse kinematics problem using dual quaternions. International

Journal of Applied Mathematics and Computer Science. 30(2). pp. 351-361. DOI: 10.34768/amcs-2020-0027

5. Zhao, Z., Wang, T. & Wang, D. (2017) Inverse kinematic analysis of the general 6R serial manipulators based on unit dual quaternion and Dixon resultant. Proceedings 2017 Chinese Automation Congress (CAC). pp. 2646-2650.

6. Srisuk, P., Sento, A. & Kitjaidure, Y. (2017) Inverse kinematics solution using neural networks from forward kinematics equa-

tions. Proceedings 2017 9th International Conference on Knowledge and Smart Technology (KST). pp. 61-65.

7. Kelemen, M., Virgala, I., Liptak, T., Mikova, E., Filakovsky, F. & Bulej, V. (2018) A novel approach for a inverse kinematics

solution of a redundant manipulator. Applied Sciences. 8(11). pp. 9-10. DOI: 10.3390/app8112229

8. Tian, X., Xu, Q. & Zhan, Q. (2021) An analytical inverse kinematics solution with joint limits avoidance of 7-DOF anthropo-

morphic manipulators without offset. Journal of the Franklin Institute. 358(2). pp. 1252-1272. DOI: 10.1016/j.jfranklin.2020.11.020

9. Wang, M., Luo, J., Fang, J. & Yuan, J. (2018) Optimal trajectory planning of free-floating space manipulator using differential

evolution algorithm. Advances in Space Research. 61(6). pp. 1525-1536. DOI: 10.1016/j.asr.2018.01.011

10. Sciavicco, L. & Siciliano, B. (2000) Modelling and control of robot manipulators. London: Springer Science & Business Media.