для манипулятора Пума

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

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

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

Шаг 3. Формирование осей сочленения. Направить ось z_i вдоль оси движения (вращательного или поступательного) i +1-го сочленения.

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

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

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

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

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

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

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

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

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

Шаг 12. Определение α_i – угла поворота оси z_{i -1} вокруг оси x_i, чтобы она стала сонаправлена с осью z_i.

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

^i-1 A _i = T _z,d T _z,Q T _x,a T _x,α = ×

× = . (5-1)

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

, (5-2)

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

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

, (5-3)

где и .

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

,

, ,

,

, ,

,

,

где

; ; ; .