3Д БУМ

3Д принтеры и всё что с ними связано

Алгоритм обработки данных в /-координатном стенде "Аттестат&quot

Первая решаемая задача — прямая задача о положении, т. е.-необходимо вычислить декартовы координаты центра системы координат O’X’Y’Z’ в системе 0XYZ и углы Эйлера. Заметим, что в отличие от структур ске­летов рук в виде разомкнутой кинематической цепи, для структур с параллельными кинематическими цепями (в том числе /-координатных) решение обратной задачи трудностей не вызывает, но решение прямой задачи, да к тому же в явном виде, проблематично. Но для определения положения контрольного тела (и захвата) требуется решить именно прямую задачу. Удобство конкретной /-координатной системы (см. рис. 7.13, б), реализованной на стенде, заключается в том, что для нее эта задача решается в явном виде, причем сводится к решению систем линейных уравнений.

Действительно, имея шесть измеренных /-координат, можно записать уравнения

h =л/(*4 ~ *,)2 + (у4 “ Уi)2 + (z4 ~ zif O’= 1,2,3), h = “xi)2 + (Уз ~Уі)2+ (z5 “ h)2,

h = 5 ~ *з)2 + (% “ Уз)2 + (zs — hf > (7>19)

h = ~ ху>2 + (>6 ~ Уз)2 + (z6 — 2з)2 •

Неизвестными здесь являются девять величин Х4, у4, Z4, Х5, у5, z5, х6, у6, z6. Однако известные расстояния /45, /56, /46 между точками 4 и 5, 5 и 6, 4 и 6 контрольного тела позволяют дополнить систему (7.19) уравне­ниями

hs = — х4)2 + (Уз — Уа)2 + (zs — Z4>2 ,

he = — xs)2 + (Уб — У5)2 + (z6 — zs)2. (7-20)

he = л/К — хл)2 + (Уб ~ Уд2 + (z6 — z4)2 •

Из трех первых уравнений системы (7.19) путем возведения в квадрат каждого и вычитания из первого полученного уравнения второго, из второго — третьего и из третьего — первого получим систему трех линейных уравнений относительно х4, у4, z4 и найдем эти величины.

Далее из четвертого и пятого уравнений системы (7.19) и пер­вого уравнения дополнительной системы (7.20) с известными теперь х4, у4, z4 в результате аналогичных процедур получим систему

трех линейных уравнений относительно х5, у5, z5 и найдем эти вели­чины.

Наконец, таким же образом определим последнюю тройку неизвестных дгб, у6, z6 из последнего уравнения системы (7.19) и оставшихся двух урав­нений дополнительной системы (7.20).

По найденным значениям координат точек контрольного тела в непод­вижной системе координат 0XYZ легко установить векторы х’, у’, Т, па­раллельные одноименным осям подвижной системы координат 0’X’YZ’ и координаты начала системы 0’X’YZ!

У5-У4 Z5“Z4

*5“ *4 Z5-Z4

£

1

£

н

1

н

Уб-^4 Z6"Z4

*6~*4 Z6 Z4

9

£

1

£*

X

1

X

х’ = (flj, Ьх, с,) = {[(*4 + *5)/2] — х6, [(у5 + у4)/2]- у6, [(z4 + z5)/2] — z6}, у’ = (л,, b2, c2) = (x5 — x4, y5 — y4, z5 — z4),

Z — (03,^3, c3) —

%=(*5+*4X Уо-=(Уб+У4)/2> z0′ =(z5+z4).

Далее, так как орты неподвижной системы координат 0XYZ суть ix = (1,0,0), iy = (0,1,0), iz = (0,0,1), то преобразование координат полностью определено

а

21

31

аи а

(7.21)

а 12 «22 а32 Уо’

а,3 а2з «зз

0 0

где х у’, z’ — координаты точек подвижной системы координат, а блок направляющих косинусов a,-,- (i, j = 1,3) определен следующим образом:

j і X

Y

Z

X’

a/4al+bi +ci bj V«i2 +ьї +cl cjjaf+bf+cf

Y

<hl№+bl +C2

b2/’Ja2 +b+c c2/M +bl+cl

Z!

aj — у]а% + bl + c

bj + bl +c

c3/-Ja3 +Ь3 +C3


Подставив в (7.21) значения координат центра схвата робота х’, у’, z’, получим массив значенийXj, yjr zj, подлежащий по достижении требуемого числа опытов и статистической обработке.

По схеме настройки стенда для некоторой конфигурации руки робота видно, что углы между осями систем координат ОXYX и 0’X’Y’Z’ будут находиться в некоторой окрестности их нулевых значений. Это обстоя­тельство следует учитывать при выборе системы углов Эйлера: границей области существования "второго" угла Эйлера не должно быть его нулевое значение. В противном случае ошибки вычисления двух других углов Эйлера в этой зоне будут весьма значительными.

Рис. 7.18. Углы ориентации у, V, <р подвижной системы координат. Три последовательных поворота неподвижной системы координат OXYZ, переводящих ее в систему О"X"Y’Z", оси которой параллельны одноименным осям подвижной системы координат 0XYZ’. Последовательность вращений: вокруг оси Y на угол у; вокруг оси ti на угол Ф; вокруг оси Z" на угол ф

Именно поэтому вычисляемыми углами ориентации на стенде "Аттестат" являются корабельные углы Крылова-Эйлера (рис. 7.18):

— первый угол у — дифферент (вращение вокруг оси У);

-второй угол крен (вращение вокруг оси П — смещение оси X;

-п/2 <■&< л/2);

— третий угол ф — "рысканье" (вращение вокруг оси Z"- дважды сме­щенной оси Z).

Элементы таблицы направляющих косинусов, выраженные через корабельные углы имеют вид [24]

(Хц = cosy • cos ф + siny • віпф • sin A a12 = shwp • cos Ф, a13 = — совф • siny + віпф • cosy • sin 0,

(X21 = — cosy • вІПф + sin у • СОБф • sin Д (X22 = совф • cosd,

CC23 = віпф • siny + СОвф • cosy • sinA,

(X31 = cos’d • siny, a32 = — sind,

(X33 = cosd • cosy.

Следовательно,

sin b = — b3/л/af + b3 +C3 ;

siny = +C3 ;

sin ф = bj+ bl + с]/+ b + cf) (a + c) .

Последние формулы определяют bj, у[р ф, — значения углов Эйлера, под­лежащие статистической обработке после проведения п выходов робота в заданную позицию.

Для любых предложений по сайту: [email protected]