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

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

Пусть H(t)=H(t₁). Схват должен пройти последовательность узловых точек в декартовом пространстве: H(t₁), H(t₂)…H(t_n) . Для построения траектории узловым точкам ставятся в соответствие векторы присоединенных координат [q₁₁(t₁), q₁₂(t₂), …,q_1n(t_n)],[ q₂₁(t₁), q₂₂(t₂), …,q_2n(t_n)],…[ q_1n(t₁), q_2n(t₂), …,q_Nn(t_n)], где q_ji обозначает j-ю присоединенную переменную, соответствующую положению схвата в i-й узловой точке H(t). В данной процедуре построение траектории сочленения происходит для одного сочленения за один раз. Затем строится кубическая интерполяция траектории j-ой присоединенной переменной между точками q_j1(t₁), q_j2(t₂), …,q_jn(t_n). Индекс j в переменной q_ji не обязателен, поэтому q_ji ставим в соответствие q_i.

Главная задача – построить траекторию j-ой присоединенной переменной во времени с использованием кубического полинома. Пусть t₁<t₂<…<t_n-2<t_n-1<t_n– моменты прохождения узловых точек. В начальный момент t=t₁ и конечный момент t=t_n заданы соответственно начальные q_j1, v_j1, a_j1 и конечные q_jn, v_jn, a_jn(положение, скорость и ускорение). Кроме того, значения присоединенной переменной q_jk в моменты времени t_k заданы для k=3,4,…,n-2. Однако, значения q₂ и q_n-1 не фиксированы: как уже говорилось, это необходимо для обеспечения непрерывности кинематических характеристик вдоль всей траектории.

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

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

Q’’_ji(t)=[(t_i+1-t)/h_i]*Q’’_ji(t_i) + [(t-t_i)/h_i]*Q’’_ji(t_i+1),

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

j=1,…,N,

где h_i= t_i+1-t – время, затрачиваемое на прохождение i-го участка. Дважды интегрируя Q’’_ji(t) и учитывая граничные условия Q_ji(t_i)=q_ji и Q_ji(t_i+1)=q_i,i+1, получаем интерполирующую функцию следующего вида:

Q_ji(t)= [(Q’’_ji(t_i)/6h_i]*(t_i+1-t)³ + (Q’’_ji(t_i+1)/6h_i]*(t-t_i)³ +

+ [q_j,i+1/h_i – h_iQ’’_ji(t_i+1)/6](t-t_i) + [q_i,t/h_i – h_iQ’’_ji(t_i)/6](t_i+1-t) i=1,2,…,n-1,

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

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

A=

, (3)

A=

=

=

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

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

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

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

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

а) Если h₂

h₁ и h_n-2

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

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

Получаем:

и

.

Из h₁ >h₂ следует, что

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

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

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

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

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

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

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

w_ji – ускорение в H_i; оно соответствует Q_ji’’(t_i) если Q_ji(t) проходит через H_i в момент времени t_i.

X=(h₁, h₂,…, h_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.

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

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

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

Дифференцируя равенство (2) и заменяя Q_ji’’(t_i) и Q_ji’’(t_i+1) соответственно на w_ji и w_j,i+1, получаем:

Q_ji’(t)=w_ji/2h_i*(t_i+1-t)² + w_ji+1/2h_i*(t-t_i)² + [q_j,i+1/h_i – h_iw_j,i+1/6] – [q_ji/h_i – h_iw_ji/6],

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

Q_ji’’(t)= w_j,i+1/h_i*(t-t_i) + w_ji/h_i*(t-t_i+1),

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

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

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

где

И

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

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

Между двумя узловыми точками ускорение линейно зависит от времени. Поэтому максимальная абсолютная величина ускорения достигается в точке t_i или в точке t_i+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) всегда имеет решение.

Если временные интервалы h₁,…, h_n-1 ….………., тогда, в соответствии со свойством 1 I-го раздела w₂, w₃,…, w_n-1 определяются однозначно. Однако, ограничения по скорости, ускорению и скорости изменения ускорения могут не удовлетворять требованиям. В этом случае временные интервалы {h₁,…, h_n-1} могут быть увеличены для придания ограничениям значений, удовлетворяющих требованиям. Для этого представим Q_i(t) исходным полиномом присоединенной переменной, определённым на временном интервале [t_i, t_i+1]=[t_i, t_i+h_i]. Если все временные интервалы увеличить в соответствии с

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