Дипломная (вкр): Формирование траектории и скорости для многозвенных манипуляторов

Дальнейшая задача формируется таким образом, для данного способа формирования гладких кривых, синтезировать систему формирования желаемой скорости движения схвата ММ на основе рассмотреного алгоритма в работе [1], для движения по непараметрическим кривым. Используя средства MATLAB/Simulink, рассмотреть работу синтезированной системы формирования скорости.

.2 Синтез системы формирования скорости при движении по непараметрическому сплайну

Для синтеза систем формирования желаемой скорости РО ММ, при движении по кривой заданной непараметрическим сплайном, будет использован алгоритм, ранее использованный для формирования скорости движения подводных необитаемых аппаратов и рассмотрен в работе [1].

Для упрощения описание работы синтезированной системы формирования скорости РО ММ будут рассмотрены плоские кривые, в случае движения объекта в плоскости, так как формирование скорости движения по координатным осям Oy и Oz аналогичны в данной синтезированной системе.

Алгоритм работы синтезированной системы формирования желаемой скорости движения схвата ММ, на основе метода, предложенного в работе [1], показан на рисунке 3. Данная блок схема показывает последовательность вычислении производимых в блоке формирования траектории модели ММ, использованной в системе MATLAB/Simulink, на рисунке 2 показана использованная модель объекта. Далее будет подробно описана работа синтезированной системы, основываясь на составленную блок схему.

Рисунок 2 -Модель ММ в системе MATLAB/Simulink.

Рисунок 3 - Алгоритм работы синтезированной системы на основе метода, предложенного в работе [1]

Работа синтезированной системы заключается в следующем:

§ На основе, заданных опорных точек, которые задает пользователь, формируются коэффициенты a, b, c, d для полинома 3-го порядка, описывающего заданный сплайн, коэффициенты формируются при помощи кубической интерполяции средств MATLAB.

§  Используя полученный полином, вычисляем производную y(x) по переменной x(t), далее эта переменная необходима для вычисления производной по времени координаты x(t).

 (2).

где .

 производнаяя координаты x по времени;

 желаемая скорость;

, производные функций желаемых траекторий от x.

Выражения 6 являются ключевыми в передоложенном методе, используя их вычисляют:

§ значение производной по времени координаты x(t), интегрируя которое получают значение самой координаты в зависимости от желаемой скорости в данный момент времени;

§  далее значение координаты y(x), используя уравнение траектории;

§  далее относительно сформированных координат x(t) и y(x) решается обратная задача кинематики для данного манипулятора и дальнейшие расчеты, связанные с работой ММ.

На основе данного метода очень просто реализовать систему формирования желаемой скорости движения РО ММ, но только если необходимо перемещаться по простой траектории, которую можно задать с помощью непараметрического сплайна, либо любой функцией, зависящей от координаты x. Если же необходимо двигаться по замкнутой траектории или по траектории где координата x может как возрастать, так и убывать, необходимо будет пользоваться системой которая будет синтезирована на основе второго предложенного метода.

То есть для использования синтезированной системы нам необходимо уравнение данной траектории в абсолютной (неподвижной) системе координат

= fy(x), z = fz(x),

где x, y, z - координаты центра масс динамического объекта, fy(x), fz(x) - заданные функции переменной x. Результатом использования метода является получение функций в виде

= x (t), y = y (t), z = z (t),

где t - время движения объекта по траектории. Синтезированную систему, с учетом кинематики ММ, можно использовать для формирования скорости движения РО в случае задания траектории в виде непараметрического сплайна. На практике такие траектории встречаются очень редко, но все равно есть элементарные операции, в которых нужно двигаться по таким траекториям с желаемой скоростью.

.3 Формирование желаемой траектории с помощью параметрического сплайна

Для формирования замкнутых траекторий, как правило, используют параметрические сплайны, например, кривую Безье, которая представляет собой полином третей степени от параметра τ, где τ ∈ [0,1] - нормированное время. В результате, координаты в абсолютной системе формируются в виде функций x(τ), y(τ).

, (3)

, - координаты опорных точек;

,

 параметрическое время.

Формула (3) показывает форму параметрического сплайна Безье.

Алгоритм формирования кривой использованный в данной работе заключается в следующем, на каждом шаге изменения параметра, рассчитывается пара координат x(τ), y(τ). Координаты дополнительных точек рассчитываются по формулам 4, 5,6.

 (4).

 (5).

 (6).

,  - координаты дополнительных точек;

,  - коэффициенты с помощью которых можно контролировать кривизну сплайна.

На основе приведенного алгоритма была сформирована программа в среде MATLAB для формирования параметрических траекторий. На рисунке 4 показан пример траектории сформированной при помощи данной программы.

В качестве опорных точек для задания данной кривой использовались точки с координатами: (0,01; 0,01), (0,1; 0,2), (0.2; 0.1), (0,3; 0.2), (0,4; 0,4), (0,3; 0.4), (0,2; 0,2), (0,1; 0,1).

Рисунок 4 -Траектория, заданная параметрическим сплайном Безье

Дальнейшая задача формируется таким образом, для данного способа формирования гладких кривых, синтезировать систему формирования желаемой скорости движения схвата ММ на основе предложенного алгоритмов формования скорости, для движения по параметрическим кривым. Используя средства MATLAB/Simulink, рассмотреть работу синтезированной системы формирования скорости.

.4 Синтез системы формирования скорости при движении по непараметрическому сплайну

робототехнический конвейер манипулятор сплайн

Для синтеза системы формирования желаемой скорости РО ММ, при движении по кривой заданной параметрическим сплайном на основе метода, рассмотренного в данной главе, который ранее использовался для формирования скорости подводных необитаемых аппаратов.

Для упрощения описание работы синтезированной системы формирования скорости РО ММ будут рассмотрены плоские кривые, в случае движения объекта в плоскости, так как формирование скорости движения по координатным осям Oy и Oz аналогичны в обоих синтезированных системах.

Так как с помощью предыдущей системы формирования скорости РО ММ можно работать только с траекториями у которых есть уравнение в абсолютной системе координат это является основным недостатком предыдущей системы.

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

На основе данного алгоритма была синтезирована система формирования желаемой скорости движения РО ММ по параметрической траектории, на рисунке 5 представлена использованная модель ММ в системе MATLAB/Simulink.

Желаемая скорость движения РО ММ по гладкой траектории определяется скоростью изменения нормированного времени τ. Поскольку расстояния между соседними опорными точками траектории в условиях промышленного производства можно определить заранее, то легко подобрать или получить соответствие между величиной τ нормированного и значением t реального времени в аналитическом виде. Такой способ значительно упрощает формирование управляющих сигналов при обработки деталей маленького габарита и перемещения РО ММ на небольшие расстояния или для выполнения технической операции траектория заранее точно известна.

Если же расстояния между опорными точками неизвестны, например, траектория движения РО формируется на основе данных сканирования крупногабаритного изделия, когда сканер выполняет несколько проходов, то можно использовать следующий алгоритм, на рисунке 6 представлена блок схема, описывающая работу, далее будет более подробно описана синтезированная система.

Рисунок 6 - Алгоритм работы синтезированной системы на основе рассмотренного метода

Рассмотренный алгоритм заключается в следующем:

·    по заданным опорным точкам формируется гладкая кривая с помощью параметрического сплайна, используя шаг параметрического времени по формуле 7, dt - шаг параметрического времени;

 (7).

* - расотяние которое должна пройти точка с заданной скоростью;

v - желаемая скорость;

 шаг дискритизации системы

t -пареметр необходимый для формирования парамерического сплайна;

Ti+1 - расстояние которое прошла точка с реальной скоростью;

·    вычисление расстояния, которое проидет управляющая точка за период дискретизации системы с текущей постоянной скоростью, формулы 7, 8;

 (8).

 расстояние между 2- мя предыдущими точками точками;

 расстояние между 2 -мя следующими точками;

 точка, текущее полжение управаляющей точки при движении по параметрической кривой;

 точка, следующее положение управляющей точки при движении по паремтрической кривой.

·    если расстояние до следующего положения управляющей точки, рассчитаное с начальным шагом τ меньше расстояния, проиденного с заданной скоростью, то шаг τ остается неизменным;

·        если это расстояние больше, то шаг τ необходимо пересчитать используя измененный шаг параметрического времени, как в формуле 9, таким образом, чтобы расстояние, проиденное управляющей точкой было близко к рассотянию, проийденному с заданной постоянной скоростью.

 (9).

 значение параметрического времени;

 значение шага параметрического времени;

 точка, возможное полжение управаляющей точки при движении по параметрической кривой;

 значение параметрического полинома.

 расстояние между 2- мя предыдущими точками точками;

 расстояние между 2 -мя следующими точками;

T* - расотяние которое должна пройти точка с заданной скоростью;

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

Если разница между расстояниями 2-х предыдущих опорных точек сильно отличается от растояния между 2-мя последующими точками, то скорость РО ММ может быть снижена либо завышена на определенном участке траектории. Это будет зависеть от величины разницы между расстояниями.