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

Автор: | 31.01.2021
Нет комментариев

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

Введение

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

  • 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-х решений верного. Напрашивается наиболее простой алгоритм. Проводятся расчеты для одних углов и других. Затем выбираются те углы, для которых координаты соответствующих точек обеих СК совпали.

Полезные ссылки: