Преобразование координат при калибровке роботов (Coordinate transformation when calibrating robots)

Tags: Калибровка робот манипулятор TCP

Введение
Преобразования ЛСК относительно осей ГСК
Вращение ЛСК относительно собственных осей
Тестирование алгоритмов вращения относительно осей ГСК и ЛСК
Задача совмещения 2-х ЛСК
Тестирование алгоритма совмещения 2-х ЛСК
Полезные ссылки

Введение

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

Схема калибровки может быть быть представлена двумя системами координат (СК):

Глобальная (ее еще называют мировой) система координат (ГСК или МСК) – это СК, относительно которой определяется положение элементов робота. Обычно она неподвижна (или считается таковой по отношению к другим элементам).
Локальная система координат (ЛСК) — это СК, в которой наиболее удобным способом описываются характерные точки подвижного элемента робота. Перемещение ЛСК относительно ГСК определяется через соответствующие геометрические преобразования.

Оси ГСК будем обозначать XYZ, оси ЛСК — xyz. Цвет осей на рисунках красный, зеленый и синий — соответственно.

Конечное положение ЛСК зависит от 3-х факторов:

Числового значения параметров (величин углов и перемещений)
Последовательности выполнения элементарных преобразований
От выбора систем координат (ГСК или ЛСК) и осей (X, Y, Z или x,y,z), относительно которых выполняются элементарные преобразования.

Каждая из позиций робота характеризуются положением одной или нескольких пар точек — TCP (Tool Center Point). TCP в ГСК определяет позицию соответствующей ей TCP точки из ЛСК. Простыми словами, точка на манипуляторе должна «прийти» в точку с указанной позицией в пространстве.

Чтобы откалибровать положение робота с 6 степенями свободы, необходимо использовать как минимум 3 пары калибровочных точек.

По умолчанию начало ЛСК находится в центре фланца робота, но ее положение может быть перенастроено. Например, ЛСК может совпадать с острием инструмента, установленного на манипуляторе робота.

Калибровку можно также проводить по схеме, в которой применяется видеофиксация.

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

Преобразования ЛСК относительно осей ГСК

Перемещение ЛСК к заданному в ГСК положению может определяться последовательностью из 6 элементарных преобразований — 3 вращения и 3 перемещения относительно осей ГСК . Ниже рассмотрены преобразования относительно осей ГСК

Элементарными считаем преобразования, которые выполняются относительно осей СК. На рисунке выше показана последовательность из 3-х поворотов и 3-х перемещений относительно осей ГСК до совмещения с 3-мя соответствующими ТСР. Математически их можно описать последовательностью перемножений матриц базовых преобразований.

Поворот на угол γ вокруг оси Z
Поворот на угол α вокруг оси Y
Поворот на угол β вокруг оси X
Перемещение на Px вдоль оси X
Перемещение на (-Py) вдоль оси Y
Перемещение на Pz вдоль оси Z

Для определения положения ЛСК относительно ГСК необходимо задать 3 точки.

С 3-мя точками можно составить 9 уравнений. Достаточно 6 уравнений, поскольку необходимо определить только 6 независимых параметров — 3 угла и 3 перемещения:

Согласно теории параметризации положения одной СК относительно другой определяется одной фиксированной точкой (3 уравнения), текущей на одной из осей (2 уравнения) и текущей в плоскости (1 уравнение).

Вращение ЛСК относительно собственных осей

Выше были рассмотрены преобразования, которые выполнялись относительно осей ГСК. Однако, в роботах обычно преобразования вращения выполняются относительно осей ЛСК, т.е., каждый очередной поворот выполняется относительно одной из осей в текущем положении ЛСК.

Возникает проблема, как математически описать взаимное положение ГСК и ЛСК, если каждое очередное преобразование по сути выполняется в новой СК. Использовать композицию элементарных преобразований, как для ГСК (рассмотрено выше), не представляется возможным, поскольку математически композиция преобразований не может быть описана в единой ГСК.

Как решить проблему с описанием преобразований относительно ЛСК в единой ГСК отражено на рисунке (подробнее описано по ссылке).

Композиция матриц в этом случае представляет последовательность преобразований в обратном порядке:

Тестирование алгоритмов вращения относительно осей ГСК и ЛСК

Для тестирования выше изложенных подходов к описанию преобразований было разработано WinApi C++ приложение, которое обеспечивает преобразование ЛСК с визуализацией взаимного положения осей ГСК и ЛСК. Оси ГСК остаются неподвижными

Приложение можно скачать с моего Google-диска по ссылке tcp0_tcp1!.zip.

При запуске приложения оси ГСК и ЛСК совпадают. При нажатии на клавиши происходит соответствующий поворот ЛСК относительно собственных осей или осей ГСК:

W — переключение режима преобразований относительно осей ГСК в ЛСК и наоборот
1, 2, 3 — преобразования относительно осей z(gamma), y(alpha), x(beta) или Z,Y,X
4, 5, 6 — обратное преобразование относительно x(-beta), y(-alpha), z(-gamma) или X,Y,Z
7 — композиция 3-х поворотов относительно осей ЛСК в последовательности z(gamma), y(alpha), x(beta).

Тест1. Последовательность вращений относительно осей ГСК: Z(30), Y (45), X(60)

Тест2. Последовательность вращений относительно осей ЛСК: z(30), y (45), x(60)

Тест3. Композиция 3-х поворотов на те же углы относительно осей ЛСК. Здесь должна быть активна ГСК, поскольку тестируется алгоритм описания преобразований относительно осей ЛСК в общей ГСК.

Выводы. Запуски программ показывают, что результаты соответствуют ожидаемым — как при вращении относительно осей ГСК, так и при вращении относительно осей ЛСК. Главное, что показали результаты — алгоритм преобразований относительно осей ЛСК в единой ГСК корректный.

Задача совмещения 2-х ЛСК

Ниже рассматривается базовая задача, решаемая при калибровке роботов.

Постановка задачи: Задано положение 2-х ЛСК относительно ГСК (6 параметрами — 3 угла и 3 перемещения):

Какие параметры нужны, чтобы из нового положения переместить ЛСК0 в положение ЛСК1?

Ниже рассматривается решение только для 3-х углов (без перемещений).

Алгоритм решения задачи.

Выше были выведены уравнения, которые обеспечивают перерасчет координат точек из ЛСК в ГСК по заданным 3 параметрам.

Эти уравнения можно использовать для определения взаимного положения одной ЛСК относительно другой. Для этого достаточно предположить, что одна из ЛСК — это ГСК. Пускай ЛСК1 — это ГСК

На вход рассматриваемого алгоритма подаются координаты точек ЛСК0 и ЛСК1 в общей ГСК. На выходе получаем углы gamma (вращение относительно z), alpha(y), beta (x).

В программе при нажатии на клавишу 8 предусмотрен вывод в файл значений координат точек на осях ЛСК0: Px(1,0,0); Py(0,1,0); Pz(0,0,1). Координаты выводятся преобразованными (искаженными) в ЛСК1, согласно выше приведенной системе уравнений.

После того, как параметры положения ЛСК0 относительно ЛСК1 определены, для совмещения обеих СК выполняется последовательность вращений ЛСК0 с этими углами.

Тестирование алгоритма совмещения 2-х ЛСК

Идем от простого к сложному. Вначале пусть ЛСК0 остается на месте, а ЛСК1 повернута на 2 угла.

Тест 1

ЛСК0 g=0, a=0, b=0

XPx=1 YPx=0 ZPx=0
XPy=0 YPy=1 ZPy=0
XPz=0 YPz=0 ZPz=1

ЛСК1 g=30, a=0, b=30

XPx=0.866 YPx=0.5 ZPx=0
XPy=-0.433 YPy=0.75 ZPy=0.5
XPz=0.25 YPz= — 0.433 ZPz=0.866

Для 3-х точек Px(1,0,0); Py(0,1,0); Pz(0,0,1) можно составить 9 уравнений.

Подставляем значения единичных отрезков на осях координат

Для решения системы уравнений относительно 3-х углов достаточно взять 3 уравнения, причем, по крайней мере для 2-х разных точек. Например, можно взять 2 уравнения для точки Px и одно уравнение для точки Py.

Из решения уравнений находим углы (g=30, a=0, b=30), которые определяют положение ЛСК1 относительно ЛСК0 (и одновременно ГСК).

При совмещении систем координат углы вводятся в последовательности g=30, a=0, b=30 когда включен режим ЛСК (W= false) или в обратной последовательности, когда включен режим ГСК (W= true). Это соответствует теоретическим предпосылкам, изложенным в разделе Вращение ЛСК относительно собственных осей.

Тест №1 пройден успешно!

Тест 2

В предыдущем тесте ЛСК0 совпадало с ГСК. Немного усложним задачу — ЛСК0 повернута относительно ГСК на угол g=30.

ЛСК0 g=30, a=0, b=0

XPx=0.866 YPx=0.5 ZPx=0
XPy=-0.5 YPy=0.866 ZPy=0
XPz=0 YPz=0 ZPz=1

ЛСК1 g=0, a=0, b=30

XPx=1 YPx=0 ZPx=0
XPy=0 YPy=0.866 ZPy=0.5
XPz=0 YPz=-0.5 ZPz=0.866

Составляем уравнения для точки Px

Явно выразить углы из такой системы уравнений практически невозможно – в отличие от предыдущего теста, где ЛСК0 была совмещена с ГСК.

Для решения задачи поступим следующим образом. Поскольку нас интересуют углы, которые определяют относительное положение обеих СК, то эту задачу можно свести к предыдущей. Для этого синхронно повернем обе СК в одном направлении, чтобы ЛСК0 совместилась с ГСК:

поворачиваем СК0 (b =0, a= 0, g= -30) до совмещения с ГСК — выполнятся обратное преобразование (поменялись знаки углов и последовательность преобразований). Сказать проще, оставляем ЛСК0 совмещенной с ГСК (g=0, a=0, b=0)
Выполняем повороты СК1 в 2 этапа: синхронные повороты с ЛСК0 (b =0, a= 0, g= -30) и собственные (g=0, a=0, b=30).

В итоге подошли к такой же постановке задачи, как и в тесте №1.

ЛСК0 g=0, a=0, b=0

XPx=1 YPx=0 ZPx=0
XPy=0 YPy=1 ZPy=0
XPz=0 YPz=0 ZPz=1

ЛСК1 принимает вот такое положение:

XPx=0.866 YPx= -0.5 ZPx=0
XPy=0.433 YPy= 0.75 ZPy=0.5
XPz=-0.25 YPz= -0.433 ZPz=0.866

По аналогии с тестом №1 определяем углы (g=-30 a=0 b=30), которые используем для совмещения ЛСК0 и ЛСК1.

Тест №2 пройден успешно!

Тест №3

Максимально усложним задачу. В предыдущем тесте ЛСК0 и ЛСК1 были повернуты относительно ГСК только на один угол — ЛСК0 (g=30), ЛСК1 (b=30). В этом тесте ЛСК0 и ЛСК1 повернуты относительно ГСК на все 3 угла. Напомню, что согласно поставленной задачи необходимо определить 3 угла, на которые нужно повернуть ЛСК0, чтобы совместить ее с ЛСК1.

ЛСК0 g=20, a=30, b=40

XPx=0.814 YPx=0.296 ZPx=-0.5
XPy=0.040 YPy=0.830 ZPy=0.557
XPz=0.580 YPz=-0.473 ZPz=0.663

ЛСК1 g=50, a=60, b=70

XPx=0.321 YPx=0.383 ZPx=-0.866
XPy=0.261 YPy=0.843 ZPy=0.470
XPz=0.910 YPz=-0.377 ZPz=0.171

Меняем положение СК также, как и в тесте 2:

поворачиваем СК0 (b =-40, a= -30, g= -20) до совмещения с ГСК
Выполняем повороты СК1 в 2 этапа: синхронные повороты с СК0 (b =-40, a= -30, g= -20) и собственные (g=50, a=60, b=70).

ЛСК0 совмещена с ГСК (g=0, a=0, b=0)

XPx=1 YPx=0 ZPx=0
XPy=0 YPy=1 ZPy=0
XPz=0 YPz=0 ZPz=1

ЛСК1 принимает вот такое положение:

XPx=0.808 YPx=-0.151 ZPx=-0.569
XPy=0.228 YPy=0.972 ZPy=0.064
XPz=0.544 YPz=-0.181 ZPz=0.820

Были получены не одно, а 2 решения уравнений вследствие того, что система уравнений была приведена к квадратному уравнению . Первое решение дало положительный результат — ЛСК0 совместилась с ЛСК1:

g= -9.85 a=34.68 b=4.47

Второе решение дало неверный результат — совмещения не произошло:

g= 25.53 a=34.68 b=4.47

Тест №1 пройден успешно, однако осталась проблема автоматического выбора из 2-х решений верного. Напрашивается наиболее простой алгоритм. Проводятся расчеты для одних углов и других. Затем выбираются те углы, для которых координаты соответствующих точек обеих СК совпали.