Алгоритм обратной кинематики для траекторий с заданной ориентацией инструмента

Бесплатный доступ

Введение. При сварке, нанесении покрытий и выполнении сборочных и монтажных операций положение инструмента задается совместно с его ориентацией. Одной заданной позе могут соответствовать различные наборы углов в суставах манипулятора, причем решения, полученные для отдельных точек, не всегда образуют непрерывную траекторию. В таком случае переход от одной точки к другой сопровождается скачком углов, что необходимо учитывать при задании скоростей движения. Траекторию задают углами в суставах либо массивом декартовых поз инструмента. Во втором случае исходными данными служат координаты и ориентации, полученные по CAD-модели, точкам обучения или от внешнего планировщика. Для расчета по этим данным требуется согласовать решения обратной кинематики в соседних точках; в рассматриваемом алгоритме это выполняется за счет передачи найденных углов в расчет следующей точки. Материалы и методы. Объектом исследования является шестизвенный манипулятор с шестью степенями свободы. По заданным положениям и ориентациям TCP определялись углы в суставах с использованием псевдообратной матрицы численного якобиана, а ориентационная часть ошибки рассчитывалась по углам Эйлера ZYX. Начальным приближением для каждой последующей точки служило решение, полученное на предыдущей. Проверка проведена для траекторий с различным числом точек. Результаты и обсуждение. Приведены порядок формирования шестимерной ошибки, расчет якобиана и условия завершения итераций. Для траекторий из 5, 10 и 20 точек определены ошибки положения и ориентации, число итераций и время расчета; дополнительно рассмотрены траектории до 50 точек. По данным расчетов для одинаковой геометрической траектории среднее число итераций на точку составило 28, 18 и 12 соответственно. Использование предыдущего решения уменьшало число итераций, а в рассмотренных тестах позволяло сохранять выбранную ветвь решения. Выводы. Разработанный алгоритм позволяет по массиву положений и ориентаций инструмента получить последовательность углов в суставах манипулятора. В проведенных расчетах передача решения между соседними точками обеспечивала плавное изменение углов, однако сама по себе не исключает смену ветви решения при любой траектории. Область применения алгоритма ограничивается условиями сходимости численного расчета. При приближении к сингулярной конфигурации или при редком задании точек необходима дополнительная проверка числа обусловленности якобиана. Для повышения устойчивости расчета могут потребоваться более плотная дискретизация траектории и регуляризованное псевдообращение.

шестизвенный манипулятор \ обратная кинематика \ движение по траектории \ углы Эйлера \ численный якобиан \ ошибка позиционирования

Короткий адрес: https://sciup.org/142249004

IDS: 142249004   |   УДК: 621.86   |   DOI: 10.15828/2075-8545-2026-18-5-681-690

Inverse kinematics algorithm for trajectories with specified tool orientation

Introduction. During welding, coating application, and assembly or installation operations, the tool’s position is specified in conjunction with its orientation. A single specified pose may correspond to various sets of manipulator joint angles, and the solutions obtained for individual points do not always form a continuous trajectory. In such cases, the transition from one point to another involves a sudden change in joint angles, a factor that must be taken into account when setting movement speeds. The trajectory is defined either by joint angles or by an array of Cartesian tool poses. In the latter case, the input data consists of coordinates and orientations derived from a CAD model, teach points, or an external planner. Calculating the trajectory from this data requires reconciling inverse kinematics solutions at adjacent points; in the algorithm under consideration, this is achieved by passing the calculated angles to the computation for the next point. Methods and Materials. The object of study is a six-link manipulator with six degrees of freedom. Joint angles were determined based on specified Tool Center Point (TCP) positions and orientations using the pseudoinverse of the numerical Jacobian matrix, while the orientation error component was calculated using ZYX Euler angles. The solution obtained for a given point served as the initial approximation for the subsequent point. Verification was performed using trajectories comprising varying numbers of points. Results and Discussion. The procedure for formulating the six-dimensional error, the Jacobian calculation, and the iteration termination criteria are presented. Position and orientation errors, iteration counts, and computation times were determined for trajectories consisting of 5, 10, and 20 points; trajectories with up to 50 points were also examined. Calculation results for an identical geometric trajectory showed average iteration counts per point of 28, 18, and 12, respectively. Utilizing the previous solution reduced the number of iterations and, in the tests conducted, enabled the maintenance of the selected solution branch. Conclusion. The developed algorithm enables the derivation of a sequence of manipulator joint angles from a set of tool positions and orientations. In the calculations performed, the transfer of the solution between adjacent points ensured smooth angular variation; however, this mechanism alone does not guarantee the avoidance of a solution branch switch for every possible trajectory. The algorithm’s applicability is constrained by the convergence conditions of the numerical computation. When approaching a singular configuration or using sparse point spacing, it is necessary to additionally check the condition number of the Jacobian. Enhancing computational stability may require finer trajectory discretization and regularized pseudo-inversion.

Текст научной статьи Алгоритм обратной кинематики для траекторий с заданной ориентацией инструмента

Кулаков П.А., Афанасенко В.Г. Алгоритм обратной кинематики для траекторий с заданной ориентацией инструмента. Нанотехнологии в строительстве. 2026;18(5):681–690. – EDN: BDBXER.

Kulakov P.A., Afanasenko V.G. Inverse kinematics algorithm for trajectories with specified tool orientation. Nanotechnologies in Construction. 2026;18(5):681–690. – EDN: BDBXER.

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

Траекторные задачи решаются в пространстве суставов (joint space) или в декартовом пространстве (cartesian space) [2–4]. В первом случае углы задаются как функции времени, а положение и ориентация TCP определяются прямой кинематикой. Во втором случае задается массив целевых поз, после чего для каждой позы решается обратная задача. Этот способ удобен, если траектория получена по CAD-модели,

Nanotechnologies in construction

Нанотехнологии в строительстве

2026; 18 (5): 681–690

Nanob

СИСТЕМНЫЕ РЕШЕНИЯ ТЕХНОЛОГИЧЕСКИХ ПРОБЛЕМ

точкам обучения или данным внешнего планировщика. При использовании произвольного начального приближения алгоритм может перейти на другую ветвь решения. В результате в последовательности углов появляются разрывы, затрудняющие интерполяцию по времени [3, 4, 11, 12]. В работе для следующей точки используется решение, полученное на предыдущей точке. При достаточно малом шаге такой прием сохраняет выбранную ветвь решения [2–4].

Для одной позы обратная кинематика может рассчитываться методом псевдообращения якобиана, транспонированного якобиана или регуляризованного метода наименьших квадратов [5–9, 16]. Позиционная ошибка определяется разностью декартовых координат. Для ориентации могут применяться углы Эйлера, представление ось-угол, кватернионы или вектор вращения [7–10]. От выбранного представления зависят сходимость и поведение алгоритма вблизи вырожденных положений. В рассматриваемом алгоритме ориентационная ошибка задается углами Эйлера ZYX, численный якобиан рассчитывается методом конечных разностей.

Влияние шага дискретизации на непрерывность суставных координат рассматривается в работах [3, 4, 11–15]. При редком задании поз решение предыдущей точки может оказаться недостаточно близким к решению следующей, вследствие чего разность qi +1 – qi возрастает. Это ограничение особенно существенно вблизи сингулярности, при смене ветви решения и значительном изменении ориентации инструмента. Поэтому шаг между точками выбираем с учетом изменения положения и ориентации, а не только по числу узлов траектории. Исходную траекторию представляем массивом целевых поз, полученных по точкам обучения или от внешнего планировщика [3, 4, 11, 12].

Исходными данными послужила кинематическая модель проекта STP3 [17], для которой ранее были получены прямая кинематика, численный якобиан и алгоритм решения обратной задачи для одной позы. Целью работы являлась разработка и проверка алгоритма расчета траектории при заданных положениях и ориентациях TCP. На вход алгоритма поступает массив целевых поз, на выходе формируется последовательность углов в суставах. Для достижения поставленной цели были сформулированы условия непрерывности, выбран способ задания ориентации, определены критерии сходимости и проведены расчеты для 5, 10, 20 и 50 точек.

Рассматривается шестизвенный манипулятор, кинематика которого задана параметрами Денавита-Хартенберга. Модель соответствует антропоморфному манипулятору с вращательными суставами плеча, локтевого звена и запястья. Значения параметров приведены в работе [17]. Прямая кинематика зада- ет отображение q → T06(q), где T06(q) – однородная матрица размера 4×4 вида [R(q) p(q); 0 1], q ∈ R6 – вектор углов в суставах,p(q) ∈ I3 – положение TCP, R(q) ∈ SO(3) – матрица ориентации инструмента [1–5, 17]. Для заданной позы Td = (Rd, pd) необходимо определить вектор q*, при котором выполняются условия ‖ p(q*) – pd ‖ ≤ εp и ρ(R(q*), Rd) ≤ εR. Величина ρ(·,·) характеризует рассогласование ориентации.

Заданы массив положений pi ∈ I3 и массив ориентаций Ri ∈ SO (3), i = 1, … , N . Требуется определить последовательность углов qi ∈ I6, i = 1, … , N , при которой для каждого значения i матрица T 06 ( qi ) соответствует заданной позе Ti = ( Ri | pi ) с установленной точностью. Разности ‖ qi +1 – qi ‖ между соседними точками не должны приводить к превышению допустимых скоростей и ускорений суставов при интерполяции по времени [2–4, 11, 12]. Начальное приближение для точки i + 1 принимается равным решению предыдущей точки: q 0 ( i +1) = qi .

Ориентацию задаем углами Эйлера в конвенции ZYX, которые далее используем при формировании шестимерного вектора ошибки:

R = Rz ( ψ ) Ry ( θ ) Rx ( ϕ ), η = ( ϕ,θ,ψ ) T .

С учетом позиционной и ориентационной составляющих получаем вектор ошибки

Первые три компоненты характеризуют ошибку положения, последние три – ошибку ориентации [7–10]. Такое представление используется при построении численного якобиана. В области gimbal lock требуется отдельная обработка углов.

Предполагается, что целевые позы достижимы и не расположены в непосредственной близости от сингулярных конфигураций. При нарушении этого условия якобиан становится плохо обусловленным, число итераций увеличивается, а небольшое изменение целевой позы может вызвать значительное изменение углов [5–9, 16]. Поэтому вместе с решением обратной задачи необходимо контролировать устойчивость расчета и выбирать допустимый шаг траектории.

МЕТОДЫ

Расчет проводим последовательно для точек i = 1, … , N , причем для каждой из них выполняем внутренний итерационный расчет. При i = 1 задаем начальное приближение q 0(1) нулевой конфигурацией либо известной конфигурацией, близкой к первой целевой позе. Для остальных точек, то есть при i ≥ 2, используем полученное ранее решение:

Nanotechnologies in construction

Нанотехнологии в строительстве

2026; 18 (5): 681–690

Nanob

СИСТЕМНЫЕ РЕШЕНИЯ ТЕХНОЛОГИЧЕСКИХ ПРОБЛЕМ

q0(i) = q*i–1, где начальное приближение принимается равным результату предыдущей точки. На итерации k сначала определяем текущую позу T06(qk) и ошибку ek, затем находим численный якобиан J(qk) и приращение

∆ q = J +( qk ) ek .

После определения приращения уточняем углы по формуле qk+1 = qk + λ∆q.

Итерационный расчет прекращается при ‖ ek ‖ <  ε или после достижения установленного числа итераций N max [5–9, 16].

Из матрицы поворота R ∈ SO (3) определяем углы Эйлера ZYX ( ϕ , θ , ψ ), необходимые для расчета ориентационной ошибки. При записи

R = Rz ( ψ ) Ry ( θ ) Rx ( ϕ )

обратное преобразование может быть выполнено по формулам

0 = atan2 ^-r31, Jr^ + r^ ;

/ ^32   ^33 A ф = atan2 I—-,—-);

40Sp COS0 '

при условии cos θ ≠ 0 [9, 10]. В окрестности gimbal lock применяется специальная обработка, полученные углы приводятся к интервалу [– π , π ]. Эти же углы используются в шестимерном векторе ошибки и при расчете численного якобиана.

Якобиан J ( q ) ∈ R6×6 связывает приращение углов ∆ q с приращением вектора состояния «положение + ориентация»:

∆ x ≈ J ( q )∆ q .

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

В используемой реализации число обусловленности cond( J ) автоматически не контролируется. При cond( J ) > 105 обычное псевдообращение следует заменить регуляризованной схемой damped least squares

Jµ+ = (JT J + µI)–1JT, где µ > 0 – коэффициент регуляризации [7–9, 16]. Регуляризация ограничивает приращение ∆q при уменьшении сингулярных величин якобиана.

Сходимость определяется по норме ошибки ‖ e ‖. Порог ε объединяет позиционную часть в метрах и ориентационную часть в радианах. Если после N max итераций требуемая точность не достигнута, расчет можно повторить с другим начальным приближением или с регуляризованным псевдообращением [5–9]. В разработанном скрипте коэффициент λ остается постоянным. Для точки, на которой сходимость не достигнута, выводится предупреждение, а значение qi принимается равным предыдущему решению qi –1. Это исключает перенос большого приращения на следующие точки, однако увеличивает ошибку в рассматриваемом узле.

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

Величина шага определяет, какая часть найденного приращения используется для уточнения углов. Если матрица J хорошо обусловлена, при λ = 1 приращение используется полностью, однако при большом начальном рассогласовании такой

Рис. 1. Блок-схема алгоритма ОК для траектории

Nanotechnologies in construction

Нанотехнологии в строительстве

2026; 18 (5): 681–690

Nanob

СИСТЕМНЫЕ РЕШЕНИЯ ТЕХНОЛОГИЧЕСКИХ ПРОБЛЕМ

шаг может сопровождаться колебаниями решения. Для ограничения шага рассматривают значения λ ∈ [0.5, 1] либо зависимость коэффициента от текущей ошибки:

, где λ0 – базовый коэффициент для выбора шага [5–9]. В расчетах принят другой вариант: по предварительным тестам выбрано λ = 0.7, после чего это значение использовалось на всех итерациях. Приведенная зависимость показывает возможный способ изменения λ; в проверяемой реализации адаптивный выбор шага не выполнялся.

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

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

где wη ≈ 0.1 … 0.2 [7–10]. Также можно задать раздельные допуски: εp для положения и εη ≈ 10–4 рад для ориентации при wη = 1. В этом случае условие сходимости записывается какI e||< JfJ + f*. Полученная в экспериментах ошибка порядка10–8 рад позволяет применять раздельные критерии без ужесточения требований к решателю.

Для одной точки выполняется несколько итераций. На каждой итерации рассчитываются прямая кинематика, численный якобиан и псевдообратная матрица 6×6. Наибольшие затраты связаны с численным якобианом, поскольку его построение требует шести дополнительных расчетов прямой кинематики. Общее время можно оценить выражением total точек iter iter, где Niter – среднее число итераций на точку, Titer – время одной итерации [3–6, 14, 15]. Для сокращения времени могут применяться аналитический якобиан, сохранение промежуточных матриц и скомпилированные библиотеки линейной алгебры. В рассматриваемом алгоритме начальное приближение, полученное на предыдущей точке, уменьшает затраты за счет сокращения числа итераций.

На рис. 1 приведена схема прохождения данных от массива поз TCP до массива углов в суставах. Схема соответствует последовательности операций в программной реализации алгоритма.

Были рассмотрены три сценария. В первом случае траектория состояла из 5 точек, расположенных на отрезке в рабочей зоне; ориентация инструмента оставалась постоянной. Во втором случае 10 точек задавались по дуге в плоскости x – y на постоянной высоте, ориентация оставалась постоянной или изменялась по направлению движения. Третий сценарий включал 20 точек с равномерным изменением положения и поворотом ориентации вокруг вертикальной оси.

Для каждого сценария определялись число итераций, ошибка положения ‖ p ( qi ) – pi ‖, ошибка ориентации по разности углов Эйлера и общее время расчета последовательности q 1, … , qN [3–9, 11, 12]. Плавность траектории оценивалась по изменению углов между соседними точками.

Для проведения расчетов приняты порог сходимости ε = 10–6, предельное число итераций N max = 100 и коэффициент шага λ = 0,7. Значение λ = 0,7 выбрано по предварительным тестам; в дальнейшем λ оставался постоянным, то есть адаптивное изменение шага не применялось. Для первой точки начальное приближение задавалось нулевой конфигурацией q 0 = (0,0,0,0,0,0), для каждой следующей использовалось решение предыдущей точки. Дополнительные весовые коэффициенты ориентационной ошибки не вводились.

Расчеты проводились в ОС Windows 10, Python 3.8, с библиотеками NumPy, SciPy и Matplotlib. Время полного расчета траектории без визуализации измерялось функцией time.perf_counter(). По результатам строились траектория TCP, графики ошибки положения и изменения углов в суставах.

Дополнительно были проверены точки на границе рабочей зоны и малые изменения позы относительно уже полученного решения. При малом изменении позы число итераций снижалось. Для одной геометрической траектории проведены расчеты с N = 5, 10, 20, 50. С увеличением числа узлов среднее число итераций на точку уменьшалось. При N = 50 общее время составляло 1–2 с, что допускает использование алгоритма при офлайн-планировании.

Для траектории из 15 точек был задан поворот ориентации на 30° вокруг оси z . Полученная последовательность qi была непрерывной, ошибка не пре-

Nanotechnologies in construction

Нанотехнологии в строительстве

2026; 18 (5): 681–690

Nanob

СИСТЕМНЫЕ РЕШЕНИЯ ТЕХНОЛОГИЧЕСКИХ ПРОБЛЕМ

высила установленного порога. При задании того же протяженного отрезка тремя точками в одном из расчетов произошла смена ветви решения и появился скачок углов. Для траекторий из 10 и 20 точек число обусловленности cond( J ) находилось в пределах 8–12. В отдельных узлах 5-точечной траектории оно принимало большие значения. Следовательно, плотность задания поз оказывает существенное влияние на работу алгоритма [2–4, 11, 12].

РЕЗУЛЬТАТЫ

Для пяти точек условие сходимости было выполнено. Наибольшее число итераций потребовалось для первой точки, для которой использовалась нулевая конфигурация. По данным табл. 1, среднее число итераций для этой траектории также было наибольшим. Ошибка положения не превысила 10–7 м или 10–4 мм (рис. 2). Полученная траектория TCP совпадает с целевой с указанной точностью, углы в суставах изменяются плавно [5–10] (рис. 3).

При увеличении числа точек среднее число итераций уменьшилось с 28 при N = 5 до 18 при N = 10 и до 12 при N = 20 (табл. 1). Ошибки положения и ориентации не превысили заданного порога. Время расчета 20 точек допускает применение алгоритма при офлайн-планировании (рис. 4). Для работы с частотой 100–500 Гц потребуется аналитический якобиан или более быстрая реализация вычислительных операций [3–8, 14–16].

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

Было выполнено сравнение с расчетом, в котором для каждой точки задавалось нулевое начальное приближение (рис. 5). В этом случае увеличивалось число итераций и мог происходить переход на другую ветвь решения. Использование предыдущего результата уменьшило среднее число итераций в 2–3 раза

Таблица 1. Число итераций и время вычисления обратной кинематики для траектории

N точек

Среднее число итераций на точку

Макс. итераций на точку

Суммарное время (мс)

5

28

45

180

10

18

35

220

20

12

28

320

Nanotechnologies in construction

Нанотехнологии в строительстве

2026; 18 (5): 681–690

Nanob

СИСТЕМНЫЕ РЕШЕНИЯ ТЕХНОЛОГИЧЕСКИХ ПРОБЛЕМ

Рис. 3. Углы в суставах по точкам траектории

Номер точки

Рис. 4. Ошибка позиционирования по точкам траектории (мм)

Рис. 2. Целевая и полученная траектория TCP в 3D (5 точек)

и исключило скачки в рассмотренных траекториях (табл. 1). Таким образом, начальное приближение влияет на время расчета и непрерывность суставных координат [2–4, 7–9].

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

Из таблицы 1 видно, что среднее время на одну точку уменьшается с 36 мс для траектории из 5 точек до 16 мс для траектории из 20 точек. При этом полное время расчета увеличивается со 180 до 320 мс: уменьшение числа итераций на точку не компенсирует увеличение количества самих точек.

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

Nanotechnologies in construction

Нанотехнологии в строительстве

2026; 18 (5): 681–690

Nanob

СИСТЕМНЫЕ РЕШЕНИЯ ТЕХНОЛОГИЧЕСКИХ ПРОБЛЕМ

Таблица 2. Российский и зарубежный взгляд на траекторную обратную кинематику

Критерий Российская практика Зарубежная практика Основной акцент Непрерывность решения и инженерная устойчивость Warm start, branch consistency, task-space pipelines Представление ориентации Чаще углы Эйлера и матричное описание Euler, quaternion, axis-angle Основной риск Переключение ветви решения Loss of continuity near singularity Типовые источники Учебные курсы по робототехнике и инженерные руководства Craig, Siciliano, Buss, Wampler сохранить wη = 1, задать допуск εη ≈ 10–4 рад и использовать критерий II ₽ IK JfJ + f*. Аηлгоритм предназначен для подготовки офлайн-траекторий и может применяться в составе модуля планирования [3, 4, 7–9, 11–16].

При разработке алгоритма были использованы инженерный контроль непрерывности решения, представление ориентации углами Эйлера, контроль сингулярных положений и регуляризованное псевдообращение. Сочетание этих методов соответствует рассмотренным российским и зарубежным подходам [2–9, 16].

ВЫВОДЫ

Предложен алгоритм обратной кинематики для траекторий с заданной ориентацией инструмента, в котором начальным приближением служит реше- ние предыдущей точки. Для рассмотренных траекторий такой порядок расчета позволил сохранить непрерывность суставных координат и сократить число итераций. При использовании псевдообратной матрицы численного якобиана и углов Эйлера ZYX получена ошибка положения порядка 10–7 м при пороге ε = 10–6 [5–10].

При приближении к сингулярной конфигурации условия сходимости ухудшаются, а при редком задании поз возможна смена ветви решения даже с использованием предыдущего результата. Для траекторий из 10 и 20 точек число обусловленности оставалось умеренным, тогда как в отдельных узлах 5-точечной траектории наблюдалось его увеличение. Эти результаты указывают на необходимость контроля cond( J ), уточнения шага дискретизации и применения регуляризации при признаках неустойчивости [7–9, 16].

Nanotechnologies in construction

Нанотехнологии в строительстве

2026; 18 (5): 681–690

Nanob

СИСТЕМНЫЕ РЕШЕНИЯ ТЕХНОЛОГИЧЕСКИХ ПРОБЛЕМ

Алгоритм позволяет получать суставные координаты по декартовым позам, заданным оператором или CAD-системой. Это сокращает объем ручного задания траектории. Дальнейшие исследования мо- гут быть направлены на учет ограничений суставов, обход сингулярных положений, применение аналитического якобиана и проверку алгоритма на реальном манипуляторе [3, 4, 11–17].