//Вычисление
ˆ
A
k,j,k
+1
, j
= 0
..k
for
i := 0
to
k-1
do
cur_a[i] := tau (l, prev_a[i], FALSE, FALSE, sigma);
end for
cur_a[k] := tau (l, prev_a[0], TRUE, FALSE, sigma);
//Вычисление проекций
r := 0;
for
i := 1
to
k
do
for
j := 1
to
i
do
for
p := 1
to
10
do
row := idx_to_row_number (list, i, j);
col := 10 * (k - 1) + p;
coor_matr[row, col] := trace (cur_u[r] * DH[p]);
end for
r := r + 1;
end for
end for
//Рекурсивный вызов функции, если текущее звено не
последнее
if
k != n
ProjKinEnergy (list, cur_u, cur_a, DH, coor_matr);
end if
Убрать из list последний элемент;
end for
end func
Функция idx_to_row_number() определяет строку матрицы координат
Z
, соответствующую текущему базисному вектору, задаваемому ин-
дексом
ijl
1
. . . l
k
1
. . .
1
. Мы не будем останавливаться на деталях реа-
лизации этой функции, поскольку это несущественно с точки зрения
принципа работы рассматриваемого алгоритма. Псевдокод функции
tau(), осуществляющей преобразование
τ
σ
fs
, не приводится ввиду его
очевидности.
Прямая рекурсивная схема вычисления проекций потенциальной
энергии не вызывает затруднений и может быть реализована следую-
щим образом:
function
ProjPotEnergy (list, prev_t, DH, coor_matr)
//list — то же, что и ранее, prev_t — матрица
˜
T
l
1
...l
k
−
1
k
−
1
n := число звеньев манипулятора;
k := номер текущего звена;
g :=
[
g
x
g
y
g
z
0]
;
u :=
[0 0 0 1]
T
;
m :=
|
X
k
|
;
54 ISSN 0236-3933. Вестник МГТУ им. Н.Э. Баумана. Сер. “Приборостроение”. 2012. № 1
