Solution of the inverse kinematics problem of the manipulator.pdf Управление любым робототехническим комплексом [1] требует четкого детерминирования траектории всех движущихся частей этого комплекса, так называемого задания траектории. Это обусловлено необходимостью контролировать положение каждого из звеньев механизма для выполнения поставленной задачи [2]. Чаще всего перед робототехнической системой в производственных условиях задача ставится в терминах пространственно-временного состояния в декартовых координатах: в заданный момент времени t привести конечную точку робота в точку с координатами x, y, z. Такое задание понятно человеку, который формирует задание для машины, однако кинематическая схема робота не позволяет без обработки и преобразования входного сигнала обеспечить выполнение этого задания и желаемое положение конечной точки. Должно быть осуществлено преобразование к обобщенным координатам манипулятора из входных данных, задаваемых в декартовой системе координат. Процесс такого преобразования называется решением обратной задачи кинематики (ОЗК). Существует несколько методов решения ОЗК: использование бикватернионов [3-5], матриц переходов, использование метода Ньютона для решения систем уравнений, нейронных сетей [6]. Однако большинство этих методов и их вариаций связано с решением систем трансцендентных уравнений либо с решением нелинейных дифференциальных уравнений [7], что существенно ограничивает их применение в системах реального времени при управлении манипуляторами, поскольку формирование таким образом задания для обобщенных координат требует значительных вычислительных ресурсов, а также обладает погрешностью, как и любой численный метод. В то же время к достоинствам подобных методов можно отнести универсальность по отношению к разным кинематическим схемам манипулятора. В задачах же прикладного характера, в условиях задания траектории манипулятора в реальном времени, правильнее использовать аналитический метод решения ОЗК [8] с целью получить значение каждой обобщенной координаты робота в виде функции от только входных параметров, заданных в декартовой системе координат. Такой подход жестко привязан к кинематической схеме робота, обладает универсальностью только в масштабах схожей кинематической структуры, однако позволяет вывести точное аналитическое соотношение между обобщенными координатами и входными параметрами. Таким образом, отпадает необходимость в трудоемких решениях транс-4 Решение обратной задачи кинематики манипулятора цендентных уравнений, а сложность вычислений ограничивается условными операторами, вычислением тригонометрических функций и выполнением арифметических операций, с чем современные встраиваемые вычислительные комплексы могут справляться довольно быстро. Поэтому нахождение аналитического решения для ОЗК является актуальной задачей. 1. Описание рассматриваемой кинематической схемы Рассматриваться решение ОЗК будет на примере распространенной на современных производствах типовой антропоморфной кинематической схемы промышленного робота [9, 10] (рис. 1). Рис. 1. Манипулятор и его обобщенные координаты Fig. 1. Manipulator and its generalized coordinates На рис. 1 обозначены наименования каждого из звеньев и показаны углы (обобщенные координаты), соотношения для которых необходимо определить. Стоит отметить, что каждый из этих углов в действительности соответствует углу поворота следующего звена относительно предыдущего. Упрощенно кинематическая схема манипулятора может быть изображена в профиль в произвольной вертикальной плоскости так, как это показано на рис. 2, где в том числе дополнительно обозначены оси и опорные точки, на которые удобно ориентироваться при расчетах. За начальные положения при расчетах (и, следовательно, нулевые углы поворота звеньев относительно друг друга) удобно принимать конфигурацию, при которой плечо расположено строго вертикально, длинная часть предплечья - строго горизонтальна и сонаправлена с осью у, схват - строго горизонтален, ось О4 занимает положение, при котором схват поворачивается в вертикальной плоскости, а инструмент, если бы был прикреплен к схвату, занимал бы строго вертикальное положение. Дальнейшие рассуждения и расчеты строятся исходя из этих начальных условий кинематической схемы. 5 С.А. Братчиков, Е.А. Абрамова, Ю.В. Федосов Рис. 2. Упрощенная кинематическая структура манипулятора в вертикальной плоскости Fig. 2. Simplified kinematic structure of manipulator in the vertical plane Стоит отметить, что кинематическая схема на рис. 2 несколько отличается от представленной на рис. 1 тем, что у изображенной на рис. 2 схемы чуть более сложная Г-образная форма предплечья, а также существует горизонтальное расстояние между осями вращения робота относительно основания и плеча относительно основания. Эти геометрические особенности, обусловленные конструктивными необходимостями и часто фигурирующие в серийных образцах, необходимо учитывать и брать соответствующие поправки в расчетах, что отражено в методе решения поставленной задачи. 2. Аналитическое решение ОЗК для рассматриваемой структуры Решение ОЗК удобно начать с поиска углов ф1, ф2 и ф3. Для этого необходимо определить координаты конечной точки предплечья манипулятора, обозначенной на рис. 2 как D. Это возможно сделать, зная координаты конечной точки схвата (хе, уж, ze), требуемые углы азимута и возвышения схва-та (е, ф) (рис. 3), а также длину схвата Із (отрезок DE на рис. 2). Координаты точки D находятся в соответствии с соотношениями xD = xE -13 cos ( s ) sin ( ф ), < Уп = Уе -13COS(s)cos(ф), (!) ^ ZD = ZE - l3Sin (s)- Рис. 3. Вектор желаемого положения и углы, определяющие это положение Fig. 3. Desired position vector and the angles forming this position 6 Решение обратной задачи кинематики манипулятора Зная координаты этой точки, нахождение углов фі, ф2, фз сводится к решению задачи расположения в вертикальной плоскости треугольника со сторонами Іі и І2 и с вершинами, находящимися в начале координат и точке D. Однако остается вопрос влияния формы предплечья на соотношения. Он решается введением корректирующего угла аСОг, который равен углу CDB на рис. 2. Угол CDB находится как arctg ( BC / CD ) . Тогда за длину Іі принимается отрезок AB, за длину І2 - отрезок BD. Также следует отметить, что возможна ситуация, когда между осями Оі и О2 существует горизонтальное расстояние (отрезок МА на рис. 2). Решение этой задачи со всеми описанными допущениями записывается через соотношения % Ф2 = ZABD = arccos -- + arcsin 2 А2 +122-I xD + yD - MA )■ 2l1l2 zD ( Ф1 = arccos l2 • sin ( ZABD ) 4 yD xD + yD \\ ( + arcsin 2 zD (2) У J( XD + yD MA ) + ZD J у J( XD + yD MA ) + ZD ^ Фз =ZABD -a cor --. После определения улов фі, ф2, фз необходимо найти углы ф4, ф5, которые зададут положение схвата относительно абсолютной системы координат. Однако определенные ранее углы фі, ф2, фз внесли свой вклад в поворот системы координат схвата относительно абсолютной системы координат, что должно быть включено в расчет. Это влияние отражено на рис. 3. Предположим, что система координат (х, y, z) абсолютная (связана с основанием), и именно относительно нее задается желаемое положение вектора направления схвата R. Углы, которые задают желаемое направление схвата в абсолютной системе кординат: ф - азимут и s - возвышение. Угол азимута соответствует повороту системы координат относительно оси z, а угол возвышения соответствует повороту системы координат относительно оси х. Система координат (x1, y', z') - это система, которая соосна координатой у' с осью предплечья и является базовой для схвата. То есть положение в пространстве системы (х1, у', z') определяет начальное положение схвата, относительно которого будут рассчитываться дальнейшие требуемые углы поворота осей. Это также означает, что положение предплечья в пространстве определяет положение системы координат (х1, у', z'). Стоит отметить, что при выборе таких объектов исследования точка O (начало координат обеих систем) совпадает с точкой E, изображенной на рис. 2, и является центром крепления схвата к предплечью робота. Из кинематической схемы (см. рис. і) можно заметить, что показанные на рис. 3 углы а и в связаны с углами фі, ф2, фз соотношениями (3) а = фі, Р = Ф2 + Фз. Поскольку ось у' в начальном положении сонаправлена с отрезком CD предплечья, то требуемое положение схвата необходимо достичь следующим образом: сопоставить ось у' схвата с направлением желаемого единичного вектора R. Исходя из кинематической схемы манипулятора, этого можно добиться поворотами относительно осей O4 и O5. Поворот относительно оси O4 соответствует углу вращения ф4 и в условиях, показанных на рис. з, будет соответствовать повороту системы (х1, у', z') относительно оси у', а поворот относительно оси O5 соответствует углу вращения ф5 и в условиях, показанных на рис. 3, будет соответствовать повороту системы (х1, у', z') относительно оси х'. 7 С.А. Братчиков, Е.А. Абрамова, Ю.В. Федосов Можно заметить, что угол ф4 в представленной на рис. 3 конфигурации на самом деле представляет из себя угол между плоскостью, проходящей через векторы у' иг, и плоскостью, проходящей через векторы у' и R . В то же время угол ф5 представляет из себя угол между векторами у' и R в плоскости, проходящей через эти самые векторы. Остается дать определение углу фб, представленному на рис. 1. Для этого нужно определить его физический смысл и то, как составитель траектории манипулятора может его задавать в понятном для человека формате. Физически этот угол удобно понимать как угол между вертикальной плоскостью и продольной плоскостью инструмента, который находится в схвате. Необходимо понимать, что на задание этого угла оказывает влияние поворот схвата на углы ф4 и ф5. Угол Z, на который вносится поправка при задании углов ф4 и ф5, представляет из себя угол между плоскостью, проходящий через векторы z и R, и плоскостью, проходящей через векторы у' и R . Углы ф4, ф5, С, наглядно представлены на рис. 4. Рис. 4. Плоскости в системе координат и углы, соответствующие обобщенным координатам Fig. 4. Planes in coordinate system and angles corresponding to generalized coordinates Угол между плоскостями m и n может быть определен в соответствии с уравнением Ф = arccos I A mAn + BmBn + CmCn Pm,+ві+c2j а2п+вп+c (4) где Ai, Bi, Ci, (i e m, n) коэффициенты i-го уравнения плоскости, заданного в виде: AiX + Biy + Ciz + Di = 0. Известно, что любая плоскость, проходящая через три точки, не лежащие на одной прямой, может быть этими тремя точками детерминирована. А значит, и уравнение плоскости может быть получено из координат трех точек, которые ей принадлежат. В контексте данного рассуждения уравнение интересующей плоскости может быть получено как определитель матрицы * - ■% У - Уоі z - zo, (5) X1i - X0i y1i - y0i z1i - z0i , X2i - X0i y2i - y0i z2i - z0i где x, y, z - координаты декартова пространства; X0i, y0i, z0i - декартовы координаты первой точки, принадлежащей рассматриваемой плоскости i; хщ y1i, Z1i - декартовы координаты второй точки, принадлежащей рассматриваемой плоскости i; X2i, y2i, Z2i - декартовы координаты третьей точки, принадлежащей рассматриваемой плоскости i. Коэффициенты Ai, Bi, Ci для конкретной плоскости находятся как соответствующие коэффициенты при множителях х, y, z определителя (5). Однако следует заметить, что все плоскости, представ-8 Решение обратной задачи кинематики манипулятора ляющие интерес в настоящем вычислении, проходят через начало координат (0, 0, 0), что дает возможность упростить определитель (5) до вида х у Z (6) Х1і y1i zli , X2i y2i z2i Тогда коэффициенты уравнения плоскости записываются в виде: Ai = yliz2i - zliy2i, ' В, = -( X1iZ2i - Z1iX2i ), (7) , C, = Х1іУ2i - уliX2i. Пусть плоскость, проходящая через единичные векторы z иу' (см. рис. 4), обозначается нижним индексом т, плоскость, проходящая через единичные векторы у' и R. обозначается нижним индексом п, а плоскость, проходящая через единичные векторы z и R, обозначается нижним индексом о. Тогда за точки, которые принадлежат этим плоскостям удобно выбрать концы указанных единичных векторов, а координаты этих точек записать в виде: ( X1m , Уіт , Z1m ) = ( X1o , Уіо, Z1o ) = ( 0,0,1), ( X2m , У 2m , Z2m ) = ( X1n , Уіп , Z1n ) = ( COS (P) Sin (a) ,cOS (P) COS (a) ,sin (p)), (8) ( X2n, У2п, Z2n ) = ( X2o , У20, Z2o ) = ( COS (S) Sin (Ф),COS (S) COS (Ф),Sin (S))- Тогда соответствующие коэффициенты уравнения плоскостей будут иметь вид: Am =- COS (p) COS (a), < Bm = COS (P) Sin (a), ^ Cm = 0, An = cos (s) cos (ф) Sin (P) - cos (p) cos (a) Sin (s), < Bn = - cos (s) sin (ф) sin (P) + cos (p) sin (a) sin (s), (9) Cn = cos (s) sin (ф) cos (p) cos (a) - cos (s) cos (ф) cos (p) sin (a), Ao =-cos (s) cos (ф), •j Bo = cos (s) sin (ф), Co = 0. Для того чтобы получить угол ф4, необходимо подставить коэффициенты (9) в уравнение (4). В терминах нижних индексов плоскостей, которые были введены ранее, угол ф4 соответствует углу между плоскостями m и п. Угол фб может быть определен как разность между следующими углами: углом поворота Ѳ продольной плоскости инструмента относительно вертикальной плоскости и углом поправки Z между плоскостями п и о, который определяется в соответствии с выражением (4) на основании коэффициентов (9) и требуемого угла. Требуемый угол Ѳ задается оператором и является входным параметром для алгоритма решения ОЗК. Фе =°-С • (і°) Угол ф5, судя из кинематики манипулятора, соответствует углу между векторами у' и R. который удобно определять в системе (х1, у', z'). В таком случае координаты вектора у' = (0, 1, 0), а вектора R = (cos(s + fS)sin((j) + a), cos(s + fS)cos((j) + a), sin(s + P)). Тогда удобно найти угол между векторами, пользуясь определением скалярного произведения векторов. Поскольку вектор у' имеет только координату у, слагаемые, относящиеся к координатам хи z, в скалярном произведении обращаются в ноль, а угол между векторами определяется как Ф5 = arccos ( cos (s + p) cos (ф + a)). (11) 9 С.А. Братчиков, Е.А. Абрамова, Ю.В. Федосов Таким образом, используя шесть входных параметров (хе, Уе, ze, в, ф, Ѳ), а также габаритные параметры робота, возможно определить аналитические выражения для обобщенных координат - углов поворота звеньев манипулятора относительно друг друга (фі, ф2, фз, ф4, ф5, фб) - применяя выражения (2), (4), (10), (11). Однако такой метод требует знания кинематической структуры манипулятора и его габаритов. Заключение Полученные соотношения позволяют соотнести входные координаты в декартовом пространстве и обобщенные координаты манипулятора, тем самым однозначно обеспечивая перевод робота в желаемое положение, задаваемое в понятном человеку виде. Плюсом описанного подхода является отсутствие численных методов при решении дифференциальных и трансцендентных уравнений, что положительно отражается на скорости вычислений. Описанный подход к нахождению аналитического соотношения между входными и обобщенными координатами может быть применен и к отличным от рассмотренной структурам манипуляторов, если применить принцип определения углов между плоскостями, а также разделить процесс поиска обобщенных координат для частей манипулятора подобно тому, как было описано. После этого потребуется наложить на полученные значения ограничения по знаку в зависимости от желаемого положения, поскольку используемые функции модуля могут определить величину значения, но не знак. Однако это предмет дальнейших исследований и дополнения представленного метода.

Скачать электронную версию публикации