Смекни!
smekni.com

Разработка и проектирование робота для разминирования (стр. 3 из 6)

где p – вектор, описывающий положение манипулятора, n, s и a – вектора нормали, перемещения и подхода соответственно. Оценка сочленения, соответствующая матрице H(t), зависит от структуры робота. Один из примеров решения для манипулятора PUMA представлен ниже.

Пусть H(t)=H(t1). Схват должен пройти последовательность узловых точек в декартовом пространстве: H(t1), H(t2)…H(tn) . Для построения траектории узловым точкам ставятся в соответствие векторы присоединенных координат [q11(t1), q12(t2), …,q1n(tn)],[ q21(t1), q22(t2), …,q2n(tn)],…[ q1n(t1), q2n(t2), …,qNn(tn)], где qji обозначает j-ю присоединенную переменную, соответствующую положению схвата в i-й узловой точке H(t). В данной процедуре построение траектории сочленения происходит для одного сочленения за один раз. Затем строится кубическая интерполяция траектории j-ой присоединенной переменной между точками qj1(t1), qj2(t2), …,qjn(tn). Индекс j в переменной qji не обязателен, поэтому qji ставим в соответствие qi.

Главная задача – построить траекторию j-ой присоединенной переменной во времени с использованием кубического полинома. Пусть t1<t2<…<tn-2<tn-1<tn– моменты прохождения узловых точек. В начальный момент t=t1 и конечный момент t=tn заданы соответственно начальные qj1, vj1, aj1 и конечные qjn, vjn, ajn(положение, скорость и ускорение). Кроме того, значения присоединенной переменной qjk в моменты времени tk заданы для k=3,4,…,n-2. Однако, значения q2 и qn-1 не фиксированы: как уже говорилось, это необходимо для обеспечения непрерывности кинематических характеристик вдоль всей траектории.

Пусть Qji(t) – кубический полином, описывающий поведение j-й присоединенной переменной между узловыми точками Hi и Hi+1 и определенный на интервале [t1, ti+1]. Задача состоит в “сшивке” между собой полиномов Qji(t) (i=1,2,…,n-1) так, чтобы они проходили через заданные узловые точки и обеспечивалась непрерывность положения, скорости и ускорения на всем интервале [t1, tn].

Поскольку Qji(t) – кубический полином, его вторая производная Q’’ji(t) должна быть линейной функцией времени t:

Q’’ji(t)=[(ti+1-t)/hi]*Q’’ji(ti) + [(t-ti)/hi]*Q’’ji(ti+1),

i=1,…,n-1, (1)

j=1,…,N,

где hi= ti+1-t – время, затрачиваемое на прохождение i-го участка. Дважды интегрируя Q’’ji(t) и учитывая граничные условия Qji(ti)=qji и Qji(ti+1)=qi,i+1, получаем интерполирующую функцию следующего вида:

Qji(t)= [(Q’’ji(ti)/6hi]*(ti+1-t)3 + (Q’’ji(ti+1)/6hi]*(t-ti)3 +

+ [qj,i+1/hi – hiQ’’ji(ti+1)/6](t-ti) + [qi,t/hi – hiQ’’ji(ti)/6](ti+1-t) i=1,2,…,n-1,

j=1,2,…,N. (2)


Таким образом, для i=1,2,…,n-1, Qji(t) определены, если известны Q’’ji(ti) и Q’’ji(ti+1). На основании этого можно записать систему n-2 линейных уравнений относительно неизвестных Q’’ji(ti), i=2,…, n-1,(описание системы в приложении А):

A=

, (3)

A=

=

=


Ленточная структура матрицы А позволяет легко определить неизвестную величину Q’’i(ti). В окончательном виде полиномы Qi’’(ti) выражаются временными интервалами hi и данными значениями присоединенных координат, скоростей и ускорений.

Доказательство единственного решения

Свойство 1: Задача интерполяции траектории имеет единственное решение, т.е. матрица А в уравнении (3) неособенная.

Доказательство: Известно, что hi – временные интервалы и должны быть положительны. Кроме того, в матрице А все строки, кроме 2 и n-3, удовлетворяют неравенству

, для строки i. (4)

а) Если h2

h1 и hn-2
hn-1, строки 2 и n-3 также будут удовлетворять неравенству (4). Поэтому, матрица А становится строго диагональной и неособенной.

б) Если h1 > h2, выполняем строковую операцию вычитания (строка 1)x(h2 – h21/h2)/(3h1+2h2+ h21/h2) из строки 2 для исключения а21.

Получаем:

и

.

Из h1 >h2 следует, что

. Поэтому матрица А эквивалентна строго диагональной матрице. Следовательно, уравнение (3) имеет единственное решение.

III.Описание траектории кубическими полиномами

В промышленности производительность зависит от скорости манипулирования робота. Для увеличения скорости работы манипулятора нужно минимизировать время движения вдоль заданной траектории. Задача оптимизации сводится к минимизации времени движения путем соответствующего выбора величин временных интервалов h1, h2,…, hn-1. с учётом ограничений присоединенных скоростей, ускорений, моментов и скоростей изменения ускорений. Для удобства примем:

VCj – ограничение по скорости для j-го сочленения,

wCj – ограничение по ускорению для j-го сочленения,

JCj – ограничение по скорости изменения ускорения для j-го сочленения.

Qji(t) – кубический полином, описывающий поведение j-й присоединенной переменной между узловыми точками i и i+1, т.е. между Hi и Hi+1 .

wji – ускорение в Hi; оно соответствует Qji’’(ti) если Qji(t) проходит через Hi в момент времени ti.

X=(h1, h2,…, hn-1),- вектор временных интервалов.

Задачу можно сформулировать следующим образом: минимизировать целевую функцию Т

при следующих ограничениях:


, j=1,2,…, N, i=1,2,…,n-1,

, j=1,2,…, N, i=1,2,…,n-1,

, j=1,2,…, N, i=1,2,…,n-1.

Строгое представление этих ограничений представлено ниже.

а) Ограничение по скорости.

, j=1,2,…, N, i=1,2,…,n-1,

Дифференцируя равенство (2) и заменяя Qji’’(ti) и Qji’’(ti+1) соответственно на wji и wj,i+1, получаем:

Qji’(t)=wji/2hi*(ti+1-t)2 + wji+1/2hi*(t-ti)2 + [qj,i+1/hi – hiwj,i+1/6] – [qji/hi – hiwji/6],

Также Qji’’(t) можно представить как

Qji’’(t)= wj,i+1/hi*(t-ti) + wji/hi*(t-ti+1),

Скорость достигает своего максимального по абсолютной величине значения в одной из точек ti , ti+1 или

, где
и Qji’’(
)=0. Ограничение по скорости тогда принимает вид :


для i=1,2,…,n-1j=1,2,…,N, (6)

где

И

б) Ограничения по ускорению:

, j=1,2,…, N, i=1,2,…,n-1,

Между двумя узловыми точками ускорение линейно зависит от времени. Поэтому максимальная абсолютная величина ускорения достигается в точке ti или в точке ti+1 и равна максимальной из величин

.С учетом этого ограничение по ускорению принимает следующий вид:

, j=1,2,…,N. (7)

в) Ограничение по скорости изменения ускорения:

, j=1,2,…, N, i=1,2,…,n-1.

Ограничение по скорости изменения ускорения можно представить в виде:

j=1,2,…, N, i=1,2,…,n-1. (8)

Пример возможного решения

Свойство 2: Задача оптимизации при наличии ограничений (6) - (8) всегда имеет решение.

Если временные интервалы h1,…, hn-1 ….………., тогда, в соответствии со свойством 1 I-го раздела w2, w3,…, wn-1 определяются однозначно. Однако, ограничения по скорости, ускорению и скорости изменения ускорения могут не удовлетворять требованиям. В этом случае временные интервалы {h1,…, hn-1} могут быть увеличены для придания ограничениям значений, удовлетворяющих требованиям. Для этого представим Qi(t) исходным полиномом присоединенной переменной, определённым на временном интервале [ti, ti+1]=[ti, ti+hi]. Если все временные интервалы увеличить в соответствии с

так, что новые временные интервалы станут равными
, тогда, в соответствии с (5) новое ускорение wi* будет определено как wi*=wi/
. Таким образом, полином Qi(
), определенный на интервале [
]=[
], будет представлен новым полиномом Q*i(
). Первая, вторая и третья производные от Q*i(
) будут иметь вид (1/
)Qi’(
), (1/
)Qi’’(
) и (1/
)Qi’’’(
) соответственно. Предположим