Планирование траектории движений робота рм-01
Цeль paбomы – Рассмотреть и проанализировать траектории движения схвата и суставов при выполнении команд движения робота с различными типами интерполяции.
1 Планирование траекторий для пр рм-01
В программном обеспечении ПР РМ-01 в основе всех программ интерполяции движения как во внутренних так и во внешних координатах лежат соотношения, связывающие перемещения и временные параметры при трапецеидальном профиле скорости (разгон, установившееся движение, торможение, рис.4.1).
Если обозначить перемещение через S,
скорость на среднем участке через
,
а время разгона, равномерного
движения и торможения, соответственно,
через
,
то, используя формулу площади трапеции,
получим:
или
Поскольку в расчетах по планированию
траектории времена разгона и торможения
полагают одинаковыми
,
приведенные выше соотношения упрощаются
где
При известных значениях S, T,
максимальные скорости и ускорения
определяют из следующих выражений :
При расчетах текущее значение перемещения определяется через приращение, которое с точностью до знака совпадает с перемещением которое осталось отработать:
S(t) =
+
dS(t),
где - значение S в конечной точке.
dS(t)
в этой формуле изменяется от (
)
в начальной точке до 0 в конечной точке.
Такой подход особенно удобен при
сопряжении траекторий, заданных
последовательностью операторов
движения. С учетом приведенных выше
соотношений закон изменения величины
S будет иметь вид
S(t)
=
+
(
)*(i
-
t
/(2*T*
))
для участка разгона
S(t) = + ( )*(T – t+ )/T
для участка равномерного движения
S(t) = + ( )*(t – T - ) /(2*T* )
для участка торможения.
При небольших перемещениях движение состоит из двух участков - разгона и торможения, профиль скорости в этом случае будет треугольным. Условием треугольного профиля скорости является выполнение неравенства:
,
где Sp - путь разгона при трапецеидальном профиле Sp = * .
Для треугольного профиля скорости временные параметры движения находят из соотношения:
,
.
Закон изменения S в этом случае тот же, что и для трапециидального профиля.
В системе программирования ПР РМ-01 существует возможность организации движения без останова в конечной точке установлением ключа DISABLE BREAK. (Рис.4.2) При этом производится совмещение участков торможения для текущего оператора движения и участка разгона для последующего. Для текущего оператора движения отрабатываются лишь два участка: разгон и равномерное движение, а на участке сопряжения текущее значение S вычисляется по формуле:
S(t)
=
+
dS(t) + dS'(t),
где dS и dS' приращения для текущего и предыдущего операторов движения соответственно.
Величины приращении определяют для данного участка по тем же формулам, что и для движения без сопряжения
,
,
При сопряжении участков траекторий, если они не лежат на одной прямой, происходит сглаживание траекторий. За время равное приблизительно 0.3 секунды до окончания текущего интервала движения робот планирует и начинает выполнять движение для следующего интервала, заканчивая одновременно движение текущего интервала. В результате сложения движений траектория рабочего органа не проходит точно через промежуточную точку.
Время движения между начальной и
конечной точками зависит от значений,
задаваемых командами системы
программирования SPEED,
SPEED%, SPEED
NEXT. Команда SPEED
задает значение скорости схвата при
движении по прямой линии в мм/с. Если
скорость не задавалась явно, то по
умолчанию она устанавливается равной
100 мм/с. Максимальное значение средней
скорости линейного движения для робота
РМ-01 составляет
=
500 мм/с. При задании движения с интерполяцией
во внутренних координатах численные
значения, задаваемые командой SPEED,
воспринимаются как масштабный коэффициент
по отношению к максимальной скорости
по суставам. Это выполняется таким
образом, что при движении по отдельным
степеням подвижности и установленной
скорости равной
угловая
скорость степени подвижности будет
равна ее максимальной скорости
.
Команда SPEED% задает коэффициент
масштабирования скорости. 100% задает
коэффициент масштабирования текущей
скорости 1. Полученная скорость
ограничивается до максимальной.
Алгоритм расчета разнится для операторов типа GO, задающих планирование во внутренних координатах, и для операторов типа GOS планирующих движение в пространстве мировых координат.
В случае планирования движения во внутренних координатах расчет Т и производится для каждого звена в отдельности, а затем выбираются наибольшие значения Т, . Для трапециидального профиля скорости величина Т вычисляется по формуле
,
где m – масштабный коэффициент скорости, определяемый произведением
m = SPEED*SPEED% / 100,
и
соответственно начальное и конечное
значение углов поворота внутренней
координаты робота,
- максимальное значение угловой
скорости по этой координате.
Величина для каждого звена является постоянной величиной. В дальнейшем при отработке движения каждый из суставов будет двигаться время, определяемое максимальными значениями Т и , a поскольку расстояния они проходят разные, то разной будет скорость поворота.
При планировании траектории в мировых (декартовых) координатах (операторы GOS, CIR) определяется модуль расстояния между начальной и конечной точками
и время
где Vmax - максимальная линейная скорость.
Обработка операторов типа GO
Если в программе имеются операторы типа GO, MOVE, MOVE JOINT и аналогичные им, то движение строится в пространстве угловых координат. Исходной информацией при этом являются координаты текущей точки, в которой находится в данный момент робот, координаты конечной точки, заданные оператором движения, а также данные из таблиц угловых скоростей и ускорений. Далее, в зависимости от того, в каких координатах задана конечная точка, решается либо прямая, либо обратная задача кинематики. По этой информации рассчитываются временные параметры движения и в каждый цикл расчетов на верхнем уровне управления (для ПР РМ-01 он составляет б4мс) находятся текущие значения внутренних координат по формулам, приведенным выше, для участков разгона, установившегося движения и торможения.
Обработка операторов типа GOS
При выполнении операторов типа GOS, MOVES и аналогичных им сначала производится сравнение конфигураций манипулятора в начальной и конечной точках и, в случае несовпадения их, выдастся сообщение об ошибке. При контурном движении робот не может поменять конфигурацию. Далее рассчитываются временные параметры движения, и в каждый цикл расчетов верхнего уровня управления определяется текущее значение внешних координат. Решается обратная кинематическая задача и находятся соответствующие внутренние координаты.