Решение прямой кинематической задачи для робота РМ-01

 

Решение прямой кинематической задачи позволяет определить положение и ориентацию характеристической точки схвата (инструмента) в мировых координатах в зависимости от значений внутренних координат суставов. Характеристическая точка описывается вектором положения р и тремя ортогональными векторами ориентации n, s, а, причем:

n – нормальный вектор, ортогональный губкам схвата; s - вектор, расположенный по направлению сжатия и разжатия схвата; а - вектор по нормальному направлению к плоскости фланца робота; р - позиционный вектор, определяющий положение схвата относительно базовой системы координат.

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

Эта матрица находится как произведение последовательности однородных матриц преобразования

,

а сами матрицы преобразования для РМ-01 имеют следующий вид

,

,

,

где

Поскольку инструмент в схвате манипулятора может меняться, а он крепится к шестому звену, то целесообразно вначале решать ПКЗ для неизменяемой части манипулятора, вычислив заранее произведение матриц описывающих относительное положение и ориентацию звеньев манипулятора, а затем, в зависимости от инструмента, система будетдомножать полученное значение на матрицу инструмента . Поэтому при решении ПКЗ для РМ-01 сначала определяется положение и ориентация единичного вектора схвата p, основание которого находится в точке пересечения осей трех последних степеней подвижности, а ориентация соответствует ориентации схвата. Компоненты матрицы ориентации и положения будут иметь в этом случае следующий вид

Вектор n находится как векторное произведение векторов s и а:

n = s x a.

Положение вектора p определяется по следующим формулам

В системе программирования робота РМ-01 имеется команда BASE позволяющая изменять положение мировой системы координат относительно положения заданного по умолчанию и поворачивать ее вокруг оси . Поэтому должна быть произведена коррекция положения вектора p в соответствии с положением мировой системы координат заданным в команде BASE ():

После домножения на матрицу инструмента определяется положение и ориентация конечной точки схвата

,

где Т - матрица характеристической точки схвата (инструмента), - матрица манипулятора, - матрица инструмента. Поскольку инструмент жестко скрепляется с шестым звеном, то удобнее длину шестого звена отнести к инструменту. Тогда, при нулевых параметрах инструмента, матрица инструмента будет иметь вид:

.

Представление положения и ориентации схвата в виде однородной матрицы размерностью 4х4 используется для внутренних преобразований. Для представления информации оператору

Обратное преобразование, т.е. соответствие между углами о, a, t и матрицей ориентации имеет следующий вид:

Таким образом, алгоритм решения ПКЗ для РМ-01 следующий:

1) по обобщенным координатам определяются координаты и направляющие косинусы вектора инструмента

--> n, s, a, p;

2) матрица направляющих косинусов преобразуется в углы о, а, t для представления информации оператору

n, s, а, р --> , о, а, t.