Алгоритм формирования систем координат звеньев

Для каждого звена манипулятора с n степенями свободы этот алгоритм формирует ортонормированную систему координат. Системы координат нумеруются в порядке возрастания от основания к схвату манипулятора.

Параметры систем координат звеньев манипулятора Пума
Сочленение i					Пределы измерения
1	90	-90	0	0	-160-+160
2	0	0	431,8 мм	149,09 мм	-225-45
3	90	90	-20,32 мм	0	-45-225
4	0	-90	0	433,07 мм	-110-170
5	0	90	0	0	-100-100
6	0	0	0	56,25 мм	-266-266

Рисунок 5.4. Формирование систем координат звеньев для манипулятора Пума

Шаг 1. Формирование базовой системы координат.

Сформировать правую ортонормированную систему координат (х0, y0, z0), связанную с основанием, ось z0 вдоль оси 1-го сочленения к «плечу» манипулятора. Оси х0 и y0 выбираются произвольно при условии их перпендикулярности оси z0.

Шаг 2. Начало и цикл. Для всех i ( i=1,2 … n-1) выполнить шаги 3-6.

Шаг 3. Формирование осей сочленения.

Шаг 4. Формирование начала i-й системы координат. Расположить начало i-й системы координат на пересечении осей zi и zi-1 или на пересечении общей нормали к осям zi и zi-1 с осью zi.

Шаг 5. Формирование оси xi. Выбрать единичный вектор xi следующим образом: xi = ±( zi-1 ? zi)/ || zi-1 ? zi || или вдоль общего перпендикуляра к осям zi-1 и zi, если они параллельны.

Шаг 6. Формирование оси yi Положить yi = +( zi ? xi)/ ||zi ?xi|| , получив тем самым правостороннюю систему координат.

Шаг 7. Формирование системы координат схвата. Как правило, n-е сочленение является вращательным. Сформировать ось zn, направив ее вдоль оси zn-1 и от робота. Выбрать ось хn так, чтобы она была перпендикулярна осям zn-1 и zn.

Шаг 8. Определение параметров звеньев и сочленений. Для каждого i

(i =1…n) выполнить шаги 9-12.

Шаг 9. Определение di Расстояние di – от начала (i-1)-й системы координат до пересечения оси zi-1 с осью xi и началом i-й системы координат, отсчитываемой вдоль оси zi-1. Если i-е соединение – поступательное, то di – присоединенная переменная.

Шаг 10. Определение ai - расстояния между пересечением оси zi-1 с осью xi и началом i-й системы координат, отсчитываемой вдоль оси xi.

Шаг 11. Определение Qi – угла поворота оси xi -1 вокруг оси zi-1, чтобы она стала сонаправлена с осью xi. Если i -е сочленение – вращательное, то Qi-присоединенная переменная.

Шаг 12. Определение αi – угла поворота оси zi -1 вокруг оси xi, чтобы она стала сонаправлена с осью zi .

После построения ДХ-координат для всех звеньев можно построить однородные матрицы преобразования, связывающие i-ю и (i-1)-ю системы координат:

i-1Ai= Tz,dTz,QTx,aTx,α=?

?=.

Преобразуя (3-1), найдем, что матрица, обратная к i-1Аi, имеет вид:

, (5-2)

где - константы, а - присоединенная переменная, если рассматриваемое сочленение – вращательное.

Используя матрицу , можно связать однородные координаты рi точки р относительно i-й системы координат (точка р покоится в i–й системе координат)с односторонними координатами этой точки относительно (i-1)-й системы тсчета, связанной с (i-1)-м звеном. Эта связь устанавливается равенством:

, (5-3)

где и .

Для шестизвенного манипулятора Пума были определены шесть матриц , соответствующие показанным на рис. 5.4. системам координат. Эти матрицы представлены ниже:

,

, ,

,

, ,

,

,

где

; ; ; .

Лекция 6 Уравнения кинематики манипулятора

Рисунок 6.1.

Однородная матрица , определяющая положение i-й системы координат относительно базовой системы координат, представляет собой произведение последовательности однородных матриц преобразования i-1Ai и имеет вид:

0Ti= 0Ai 1Ai …i-1Ai===для i=1, 2, …, n,

где - матрица, определяющая ориентацию i-й системы координат, связанной с i-м звеном, по отношению к базовой системе координат. Это верхняя левая подматрица , имеющая размерность 3?3.

рi- вектор, соединяющий начало базовой системы координат с началом i-й системы координат. Это верхняя правая подматрица матрицы , имеющая размерность 3?1. В частности, при i=6 мы получаем матрицу , которая задает положение и ориентацию схвата манипулятора относительно базовой системы координат. Эта матрица часто используется при описании кинематики манипулятора. Ее называют «матрицей манипулятора».

Положим, что матрица Т имеет следующий вид:

T====,

где n – вектор нормали к схвату. В случае плоскопараллельного движения пальцев этот вектор перпендикулярен пальцам манипулятора;

s – касательный вектор схвата. Он лежит в плоскости движения пальцев и указывает направление движения пальцев во время открытия или закрытия схвата;

a - вектор подхода схвата.

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

Если положение манипулятора в абсолютном пространстве определяется матрицей B, а в схвате манипулятора зафиксирован инструмент, положение которого в системе координат схвата определяется матрицей H, то положение рабочего узла инструмента относительно абсолютной системы координат дается произведением матриц В, 0Т0 и Н, т.е.:

. (6-1)

При этом H ? , B ? .

Решение прямой задачи кинематики для шестизвенного манипулятора является вычислением T=0A6 с помощью последовательного перемножения шести матриц i-1Ai. Решение этой задачи приводит к единственной матрице Т при заданных и фиксированных системах координат, где для вращательного сочленения и для поступательного сочленения. Ограничения определяются только физическими пределами изменения для каждого сочленения манипулятора.

Матрица T манипулятора Пума имеет вид:

T = 0A11A22A33A44A55A6=, (6-2)

где ;

; ; (6-3) ;

;

; (6-4)

;

;

; (6-5)

;

;

.

Например, при имеем

T=,

что согласуется с выбором системы координат на рис. 5.4.

Из равенств (6-3) – (6-6) видно, что вычисление матрицы манипулятора Т требует обращения к программам вычисления 12 трансцендентных функций, выполнения 40 умножений и 20 сложений в том случае, если производится только вычисление правой подматрицы Т, имеющей размерность 3?3, а вектор n определяется как векторное произведение векторов s и a(n=s?a). Если объединить d6 с длиной рабочего инструмента, то d6=0, а длина инструмента увеличивается на d6 единиц. Это сокращает объем вычислений до 12 бращений и программ вычисления трансцендентных функций, 35 операций умножения и 16 операций сложения.