ˆ
A
l
1
...l
k
kkk
=
τ
σ
11
"
ˆ
A
l
1
...l
k
−
1
k
−
1
,
0
,k
#
.
Таким образом
k
матриц
ˆ
U
l
1
...l
k
kjk
(
j
= 1
, . . . , k
)
вычисляются с помо-
щью
k
матриц
ˆ
A
l
1
...l
k
−
1
k
−
1
,jk
(
j
= 0
, . . . , k
−
1)
, полученных на предыдущем,
(
k
−
1)
-м шаге. С помощью соотношений, аналогичных (17), можно
показать, что преобразования (16) являются частным случаем пре-
образований (18) при
i > k
. Тогда
ˆ
U
l
1
...l
k
kji
=
τ
σ
00
"
ˆ
U
l
1
...l
k
−
1
k
−
1
,ji
#
(
i
= 1
, . . . , k
−
1
, j
= 1
, . . . , i
)
.
Далее приведен псевдокод функции, реализующей описанный алго-
ритм вычисления проекций кинетической энергии.
function
ProjKinEnergy (list, prev_u, prev_a, DH, coor_matr)
//list — список, состоящий из элементов индекса
l
1
. . . l
k
//prev_u — массив, содержащий матрицы
ˆ
U
l
1
...l
k
−
1
k
−
1
,ji
,
i
= 1
, . . . , k
−
1
, j
= 1
, . . . , i
//prev_a — массив, содержащий матрицы
ˆ
A
l
1
...l
k
−
1
k
−
1
,jk
, j
= 0
, . . . , k
−
1
n := число звеньев манипулятора;
k := номер текущего звена;
Создать массив cur_u матриц
4
×
4
из size(prev_u)+k элементов;
//cur_u содержит матрицы
ˆ
U
l
1
...l
k
kji
, i
= 1
, . . . , k, j
= 1
, . . . , i
Создать массив cur_a матриц
4
×
4
из k+1 элемента;
//cur_a содержит матрицы
ˆ
A
l
1
...l
k
kj,k
+1
, j
= 0
, . . . , k
sigma := TRUE, если
σ
k
= 1
, и FALSE, если
σ
k
= 0
;
m :=
|
Y
k
|
;
for
l := 1
to
m
do
Добавить l в конец list;
r := 0;
//Вычисление
ˆ
U
l
1
...l
k
kji
, i
= 1
..k
−
1
, j
= 1
..i
for
i := 1
to
k-1
do
for
j := 1
to
i
do
cur_u[r] := tau (l, prev_u[r], FALSE, FALSE, sigma);
r := r + 1;
end for
end for
//Вычисление
ˆ
U
l
1
...l
k
kjk
, j
= 1
. . . k
for
i := 1
to
k-1
do
cur_u[r] := tau (l, prev_a[i], FALSE, TRUE, sigma);
r := r + 1;
end for
cur_u[r] = tau (l, prev_a[0], TRUE, TRUE, sigma);
ISSN 0236-3933. Вестник МГТУ им. Н.Э. Баумана. Сер. “Приборостроение”. 2012. № 1 53
