Метод отдельных тел

Метод отдельных тел
(метод А. Ф. Верещагина)
Юдинцев В. В.
Кафедра теоретической механики
Самарский государственный аэрокосмический университет
им. академика С. П. Королёва
(национальный исследовательский университет)
29 марта 2014 г.

Метод отдельных тел
Метод разработан Верещагиным А. Ф. в 1974 году:
Верещагин А. Ф. Компьютерное моделирование динамики
сложных механизмов роботов-манипуляторов Инженерная
кибернетика, вып. 6, 1974, с. 65-70.
Используются шарнирные координаты.
Не формируется матрица масс всей системы: все матричные
операции выполняются с матрицами и векторами размерности
≤ 6.
Вычислительная трудоёмкость пропорциональна количеству тел
системы: O(n).
Кафедра ТМ (СГАУ) Метод отдельных тел 29 марта 2014 г. 2 / 34

Уравнения движения

Уравнения движения тела n
Mnan = Qn + Rn (1)
где Mn – матрица инерции тела:
Mn =
mnE3×3 03×3
03×3 Jc
; (2)
an – столбец линейных и угловых ускорений тела n, Qn – столбец
активных сил и моментов, действующих на тело n, Rn – столбец сил и
моментов реакции, действующих на тело n:
an =
an
εn
=
¨rn
εn
, Qn =
Fn
Mn − ωn × Jn · ωn
, Rn =
Fc
n
Mc
n
.

Уравнения движения тела n − 1
Mn−1an−1 = Qn−1 + Rn−1 + R∗
n (3)
R∗
n – столбец сил и
моментов реакции,
приведённых к центру масс
тела n − 1:
R∗
n =
Fc
n
Mc
n + ρn × Fc
n
.

Кинематика относительного движения
Ускорение
an = Cnan−1 + Sn¨qn + an (4)

Матрица Cn
an = Cnan−1 + Sn¨qn + an
Cn =
E −ρn×
0 E
→
E −ρn×
0 E
¨rn−1
εn−1
=
¨rn−1 + εn−1 × ρn
εn−1
(5)

Шарнирное ускорение
Подставив в уравнение движения an и умножив результата на ST
n
ST
n · Mnan = Qn + Rn ← an = . . . qn, (6)
получим:
ST
n Mn(Cnan−1 + Sn¨qn + an) = ST
n Qn + ST
n Rn. (7)
или
¨qn = (ST
n MnSn
Un
)−1
ST
n (Qn + Rn − Mn(Cnan−1 + an)) (8)

Механическая система с идеальными связями
rk = rk(q1, q2, . . . , qn) =


xk(q1, q2, . . . , qn)
yk(q1, q2, . . . , qn)
zk(q1, q2, . . . , qn)


˙rk =


∂xk/∂q1 ∂xk/∂q2 . . . ∂xk/∂qn
∂yk/∂q1 ∂yk/∂q2 . . . ∂yk/∂qn
∂zk/∂q1 ∂zk/∂q2 . . . ∂zk/∂qn





˙q1
...
˙qn


 +
∂rk
∂t
= Sn ˙q +
∂rk
∂t
¨rk = Sn¨q + . . .

Матрица Sn и идеальные связи
Sk =


∂xk/∂q1 ∂xk/∂q2 . . . ∂xk/∂qn
∂yk/∂q1 ∂yk/∂q2 . . . ∂yk/∂qn
∂zk/∂q1 ∂zk/∂q2 . . . ∂zk/∂qn


Каждый столбец матрицы Sk – базисный
вектор криволинейного базиса,
касательного пространства:
Sk = eq1 eq2 . . . eqn
Для идеальных связей
Rk · eq1 = 0, Rk · eq2 = 0, . . . , Rk · eqn = 0
или
ST
k Rk = 0.

Обобщённые ускорения
С учётом
ST
n Rn = 0 (9)
Выражение для вторых производных обобщённых координат
¨qn = (ST
n MnSn
Un
)−1
ST
n (Qn + Rn − Mn(Cnan−1 + an)) (10)
принимает вид:
¨qn = U−1
n ST
n Qn − Mn(Cnan−1 + an) (11)
где
Un = ST
n MnSn. (12)

Обратный ход алгоритма метода отдельных тел
¨qn = U−1
n ST
n Qn − Mn(Cnan−1 + an) , (13)
an = Cnan−1 + Sn¨qn + an, (14)
Mnan = Qn + Rn, (15)
Mn−1an−1 = Qn−1 + Rn−1 + R∗
n. (16)
¨qn → an(¨qn) → mnan = . . . → Rn(an−1) → mn−1an−1 = . . .
M∗
n−1an−1 = Q∗
n−1 + Rn−1, n → k
где
M∗
n−1 = Mn−1 + CT
n MnCn − CT
n MnSnU−1
n ST
n MnCn,
Q∗
n−1 = Qn−1 + CT
n Mn SnU−1
n ST
n (Qn − Mnan) + an − Qn .

Алгоритм
Обратный ход алгоритма (k = n, n − 1, n − 2, . . . , 2)
M∗
k−1 = Mk−1 + CT
k M∗
kCk − CT
k M∗
kSkU−1∗
k ST
k M∗
kCk, (17)
Q∗
k−1 = Qk−1 + CT
k M∗
k SkU−1∗
k ST
k (Q∗
k − M∗
kak) + ak − Q∗
k , (18)
U∗
k = ST
k M∗
kSk. (19)
Mn, Qn → M∗
n−1, Q∗
n−1 → M∗
n−2, Q∗
n−2 → . . . → M∗
1, Q∗
1
Прямой ход алгоритма (k = 1, 2, 3, . . . , n)
¨qk = U∗−1
k ST
k Q∗
k − M∗
k(Ckak−1 + ak) (20)
ak = Ckak−1 + Sk ¨qk + ak. (21)
¨q1(w0), w1 → ¨q2(a1), a2, → . . . → ¨qn(an−1), an

Пример программы (MATLAB)
Плоская система стержней, последовательно
соединённых цилиндрическими шарнирами

Пример
Схема системы

Пример
Структура файлов
main.m dqdt.m
getSk.m Sk
getCk.m Ck
getWprim.m ak
getMkQkiUk.m M∗
k, Q∗
k, Uk
getA.m Ak

Пример
Главный файл-скрипт I
global Model;
Model=struct;
Количество тел
n=10; Model.n=n;
Шарнирные векторы: c
(k)
k,k+1 =
lk/2
0
=
l/2
0
, c
(k)
k,k = −
lk/2
0
= −
l/2
0
c=mat2cell(zeros(n*2,n),ones(1,n)*2,ones(1,n));
for i=1:n
c{i,i}=0.5*[-1;0]/n;
if i~=n
c{i,i+1}=0.5*[+1;0]/n;
end
end
Model.c=c;

Пример
Главный файл-скрипт II
Матрицы масс
mass=cell(n,1); m=10;
for i=1:n
mass{i}=[m/n 0 0; 0 m/n 0; 0 0 m/(12*n*n*n)];
end
Model.mass=mass;
Начальные условия
q0=zeros(n*2,1); q0(1)=-1.0;
Интегрирование
[t,q]=ode113(@dqdt,[0 10],q0);

Пример
Файл-функция правых частей ДУ I
[ ˙ϕ1, ˙ϕ2, . . . , ˙ϕn, ¨ϕ1, ¨ϕ2, . . . , ¨ϕn] = dqdt(t, [ϕ1, ϕ2, . . . , ϕn, ˙ϕ1, ˙ϕ2, . . . , ˙ϕn])
function dx = dqdt(t,x)
global Model;
n=size(x,1)/2;
q =x(1:n);
dq=x(n+1:2*n);
dx=[dq;dq];
Матрицы преобразования координат Ai из базиса тела i в базис 0:
A0=cell(n,1);
A0{1}=getA(q(1));
for i=2:n
A0{i}=A0{i-1}*getA(q(i));
end

Пример
Файл-функция правых частей ДУ II
Силы и моменты
Q=cell(n,1);
for i=1:n
Q{i}=[0;-Model.mass{i}(1,1)*9.81;0];
end
Обратный ход алгоритма.
Вычисление матриц M∗
k, Q∗
k, U−1
k
[Mk,Qk,iUk]=getMkQkiUk(q,dq,Model.mass,Model.c,Q,A0);

Пример
Файл-функция правых частей ДУ III
Прямой ход алгоритма. Вычисление ¨qk, ak.
w=[0;0;0];
for i=1:n
Ck=getCk(i, q, dq, Model.c, A0);
Sk=getSk(i, q, dq, Model.c, A0);
Wp=getWprim(i, q, dq, Model.c, A0);
dx(n+i)=iUk{i}*Sk’*(Qk{i}-Mk{i}*(Ck*w+Wp));
w=Ck*w+Sk*dx(n+i)+Wp;
end
end

Пример
Файл-функция getMkQkiUk (обратный ход алгоритма) I
function [Mk,Qk,iUk] = getMkQkiUk(q,dq,masses,c,Q,A0)
n=size(q,1);
Mk=cell(n,1);
Qk=cell(n,1);
iUk=cell(n,1);
Mk{n}=masses{n};
Qk{n}=Q{n};

Пример
Файл-функция getMkQkiUk (обратный ход алгоритма) II
for k=n:-1:1
Ck=getCk(k,q,dq,c,A0);
Sk=getSk(k,q,dq,c,A0);
Wprim=getWprim(k, q, dq, c, A0);
U=Sk’*Mk{k}*Sk;
iUk{k}=inv(U);
if k>1
Mk{k-1}=masses{k-1}+Ck’*Mk{k}*Ck-Ck’*Mk{k}*...
Sk*iUk{k}*Sk’*Mk{k}*Ck;
Qk{k-1}=Q{k-1}-Ck’*(Mk{k}*(Sk*iUk{k}*Sk’*...
(Qk{k}-Mk{k}*Wprim)+Wprim)-Qk{k});
end
end % for
end

Пример
Файл-функция getCk
Коэффициенты при ускорениях ak−1 в выражении
ak = Ckak−1 + Sk ¨qk + ak (22)
ak =


¨xk−1
¨yk−1
εk−1

 +
A−π/2(Akc
(k)
kk − Ak−1c
(k−1)
k,k−1)
0
εk−1 + . . .

Пример
ak =
E2×2 A−π/2(Akc
(k)
kk − Ak−1c
(k−1)
k,k−1)
0 1
Ck


¨xk−1
¨yk−1
εk−1

 + . . .

Пример
function ck = getCk(k, q, dq, c, A0)
ck=eye(3,3);
if k==1
ck(1:2,3)=-[0 1;-1 0]*(-A0{k}*c{k,k});
else
ck(1:2,3)=-[0 1;-1 0]*(A0{k-1}*c{k-1,k}-A0{k}*c{k,k});
end

Пример
Файл-функция getSk

Пример
Sk ¨qk = Sk ¨ϕk =


aτ
rx
aτ
ry
¨ϕk

 =
A−π/2Akckk
1
¨ϕk. (24)

Пример
Sk =
A−π/2Akckk
1
. (25)
function sk = getSk(k, q, dq, c, A0)
sk=[0;0;1];
sk(1:2)=[0 1;-1 0]*A0{k}*c{k,k};
end

Пример
Файл-функция getWprim
Составляющие ускорения, не зависящие от вторых производных
обобщённых координат ¨q.

Пример
ak = −ck−1,kω2
k−1 + ckkω2
k−1
an
e
+ 2ckk ˙ϕkωk−1
ak
+ ckk ˙ϕ2
k
an
r
(27)

Пример
ak = −ck−1,kω2
k−1 + ckkω2
k−1 + 2ckk ˙ϕkωk−1 + ckk ˙ϕ2
k. (28)
function wprim=getWprim(k, q, dq, c, A0)
if k==1
wprim=[(dq(k)+0)^2*A0{k}*c{k,k};0];
else
wkp=sum(dq(1:k-1));
wprim=[(dq(k)+wkp)^2*A0{k}*c{k,k}-wkp*wkp*A0{k-1}*c{k-1,k};0];
end
end

Пример
Файл-функция getA
Матрица плоского поворота, преобразующая координаты из базиса e2,
повёрнутого вокруг оси z относительно базиса e1, в базис e1.
function A = getA(angle)
A=[cos(angle) -sin(angle);sin(angle) cos(angle)];
end

Пример
Список использованных источников
1 Верещагин, А. Ф. Компьютерное моделирование динамики сложных
механизмов роботов-манипуляторов / А. Ф. Верещагин // Инженерная
кибернетика, вып. 6. — 1974. — С. 65–70.
2 Дмитроченко, О. Н. Эффективные методы численного моделирования
динамики нелинейных систем абсолютно твёрдых и деформируемых
тел: Дис... канд. физ. мат. наук: 01.02.01. – М., 2003. – 125 с.
3 Joel Storch and Stephen Gates, Motivating Kane’s Method for Obtaining
Equations of Motion for Dynamic System. Engineering Notes, Vol. 12, N. 4,
July-August 1989.

Метод отдельных тел

More Related Content

What's hot (20)

Viewers also liked (13)

Similar to Метод отдельных тел (16)

More from Theoretical mechanics department (20)

Метод отдельных тел