MOBILNE ROBOTY KOA
OWE
WYKA
AD 03  DYNAMIKA
Lagrange
dr inż. Tomasz Buratowski
Wydział Inżynierii Mechanicznej i Robotyki
Katedra Robotyki i Mechatroniki
Modelowanie dynamiki dwu-kołowego
robota mobilnego
W modelowaniu dynamiki robotów mobilnych często
używamy równań Lagrange a II typu z mnożnikami oraz
równań Maggie go który bazuje na równaniach Lagrange a
Równania dynamiczne ruchu mogą zostać użyte w celu
rozwiązania zarówno prostego jak i odwrotnego problemu
dynamiki.
Rozwiązując zadanie proste otrzymujemy parametry
ruchu.
Rozwiązując zadanie odwrotne otrzymujemy siły i
momenty działające na robota.
2
Dynamika  Lagrange II
Jednym z formalizmów matematycznych używanych w
modelowaniu dynamiki są równania Lagrange a II typu.
Ich ogólna postać:
T T
ć� �� ć� ��
d ś�E ś�E
T
(3.1)
�� �� �� ��
-� =� Q +� J (�q)�l�
�� �� �� ��
&�
dt ś�q ś�q
Ł� ł� Ł� ł�
gdzie:
ż� q  wektor współrzędnych uogólnionych
&�
E =� E(q, q)
ż� - energia systemu
ż� Q  wektor sił ugólnionych
ż� J(q)  jakobian wynikający z równań więzów
ż� l� - wektor mnożników Lagrange a
3
Dynamika  Lagrange II
Otrzymane równania dynamiki mogą okazać się trudne
do rozwiązania, dlatego też stosuje się transformacje
pozwalającą odsprzężyć mnożniki od momentów
napędzających. Jako wynik otrzymujemy równanie 3.1 w
postaci macierzy:
(3.2)
&�&� &�
M (q)q +� C(q,q)q =� B(q)t� +� JT (q)l�
where:
ż� M  macierz bezwładności
ż� C  macierz sił odśrodkowych i Coriolisa
ż� B  macierz współczynników sił i momentów
ż� t�  wektor sił i momentów
4
Dynamika  Lagrange II
Następnie rozkładamy wektor uogólnionych
współrzędnych Q otrzymując:
q �� Rn, q1 �� Rm, q2 �� Rn-�m
q =� [q1,q2 ]T (3.3)
Możemy teraz zapisać równanie więzów jako:
&�
q1
[�J1(q), J (q)]���q ł� =� 0 (3.4)
det J1(q) ą� 0
2
ę� ś�
&�2
�� ��
Wektor q2 musi być dobrany w taki sposób, aby był
zgodny z ilością stopni swobody systemu:
J12(q) gdzie:
�� ł�
&� &�2 &�
q =� =� T (q)q2
ę�
-�1
In-�m ś�q
(3.5)
J12 =� -�J1 (q)J2(q)
�� ��
In-m  macierz jednostkowa
&�
&�&� &� &�&�
q =� T(q)q2 +� T(q)q2
5
Dynamika  Lagrange II
Po użyciu procedury odsprzęgania opisanej poprzednio,
otrzymujemy dynamiczne równania ruchu:
T
&�&� &� &�
M12(q)q +� C12(q,q)q2 =� B (q)t� +� J1 (q)l�
(3.6)
&�&� &� &�
M22(q2)q +� C22(q2,q2)q2 =� B2(q2)t�
Równania te pozwalają na rozwiązanie zadania
prostego i odwrotnego dynamiki. Modelowanie
dynamiki jest bardzo istotnym elementem związanym
ze sterowaniem systemu omawianym podczas zajęć.
6
Lagrange II  modelowanie dynamiki
W celu uproszczenie obliczeń, koła 1 i 2 zostały
zastąpione jednym kołem wspólnym dla którego kąt obrotu
jest równy a�
7
Lagrange II  modelowanie dynamiki
Przyjęto wektor uogólnionych współrzędnych w postaci:
q =� [xA, yA, b�,a�]T
(3.7)
Energia kinetyczna układu (w funkcji uogólnionych
współrzędnych) pomijając koło podpierające ma postać:
1
&�
&�2 &�
Ek =� (m1 +� m2 +� m4)xA +� ((m1 -� m2)l1 cos(b� ) +� m4l2 sin(b� ))b�xA +�
2
1
2 (3.8)
&�
&� &�
+� (m1 +� m2 +� m4) yA +� ((m1 -� m2)l1 sin(b� ) -� m4l2 cos(b� ))b�xA +�
2
1 1
2 2 2
&� &�
&� &�
+� ((m1 +� m2)l12 +� Ix1 +� Iz1h2 +� Ix2 +� Iz2h2 +� Iz4 +� m4l2 )b� +� (Iz1h -� Iz2h)b�a� +� (Iz1 +� Iz2)a�
2 2
8
Lagrange II  modelowanie dynamiki
Uwzględniając samonastawne koło podpierające energia
kinetyczna przyjmuje postać:
1
&�
&�2 &�
Ek =� (m1 +� m2 +� m3 +� m4)xA +� ((m1 -� m2)l1 cos(b� ) +� (m3l5 +� m4l2)sin(b� ))b�xA +�
2
1
&�
&�2 &�
+� (m1 +� m2 +� m3 +� m4)yA +� ((m1 -� m2)l1 sin(b� ) -� (m3l5 +� m4l2)cos(b� ))b�yA +�
2
(3.9)
1
2 2
&�2
+� ((m1 +� m2)l12 +� Ix1 +� Iz1h2 +� Ix2 +� Iz2h2 +� Ix3 +� m3l5 +� Iz4 +� m4l2 )b� +�
2
2 2
&�2) Ix3b�l1b�&�vA 1 Ix3l12b�&�2vA
&� &� &�
1 1 Iz3(vA +� l12b�
2
&�
&� &�
+� (Iz1h -� Iz2h)b�a� +� (Iz1 +� Iz2)a� +� +�
2 2
&�2 +� &�2)2
2 2 r32 vA +� l12b� 2 (vA +� l12b�
gdzie: m1, m2, m3, m4  masy poszczególnych elementów systemu
" Ix1, Ix2, Ix3, Iz1, Iz2, Iz3, Iz4  momenty bezwładności
względem odpowiednich osi
" h  stosunek l1/r1
9
Lagrange II  modelowanie dynamiki
Przyjmując m1=m2, Ix1=Ix2, Iz1=Iz2, r=r1=r2 macierz
bezwładności dla systemu bez koła podpierającego ma
postać:
2m1 +� m4 0 m4l2 sin(b� ) 0
�� ł�
ę� ś�
0 2m1 +� m4 -� m4l2 cos(b� ) 0
ę� ś�
(3.10)
M =�
2 2
ę� ś�
m4l2 sin(b� ) -� m4l2 cos(b� ) 2m1l1 +� 2Ix1 +� 2Iz1h2 +� Iz4 +� m4l2 0
ę� ś�
0 0 0 2Iz1��
��
dla systemu z kołem podpierającym:
2m1 +� m3 +� m4 0 (m3l5 +� m4l2)sin(b� ) 0
�� ł�
ę� ś�
0 2m1 +� m3 +� m4 -� (m3l5 +� m4l2) cos(b� ) 0
ę� ś�
M =�
2 2 2
ę� ś�
(m3l5 +� m4l2)sin(b� ) -� (m3l5 +� m4l2) cos(b� ) 2m1l1 +� 2Ix1 +� 2Iz1h2 +� Iz4 +� m4l2 +� Ix3 +� m3l5 +� %1 0
ę� ś�
0 0 0 2Iz1��
��
3 5 4 2 6 2 2
&�2vA 8Ix3l1 b� 4vA -� 6Ix3l1 b� 2vA 12Ix3l1 b� 4vA Iz3l1
&� &� &�
Ix3l1vA 8Ix3l1 b�
%1 =� -� +� +� +�
2 2 2 2 2 2 2 2 2 2 2
&� &� &�2)3 &�
(3.11)
vA +� l1 b� (vA +� l1 b� )2 (vA +� l1 b� (vA +� l1 b� )4 r32
10
Lagrange II  modelowanie dynamiki
Macierz sił Coriolisa i siły odśrodkowej dla systemu bez
koła podpierającego:
&�
�� ł�
0 0 m4l2b� cos(b� ) 0
ę� ś�
&�
ę�0 0 m4l2b� sin(b� ) 0ś�
C =�
(3.12)
ę�0 0
0 0ś�
ę� ś�
0 0��
ę� ś�
��0 0
dla systemu z kołem podpierającym:
(3.13)
&�
�� ł�
0 0 (m3l5 +� m4l2)b� cos(b� ) 0
ę� ś�
&�
(m3l5 +� m4l2)b� sin(b� ) 0ś�
ę�0 0
6 5 2 4 4 3 2 4 2 6 3
&�6 &� &� &� &�
C =�
ę� -� (l1 b� +� (12l1 vA -�13vAl1 )b� -� (30l1 v3 +� 5l1 vA)b� +� 6l1v5 +� 9vA)b�&�b�Ix3l1 v3 ś�
A A A
0ś�
ę�0 0 2 2 2
&�
(vA +� l1 b� )5
ę� ś�
0 0��
��0 0
11
Lagrange II  modelowanie dynamiki
Wektor sił i momentów dla systemu bez koła
0
�� ł�
podpierającego:
ę� ś�
0
ę� ś�
t� =�
(3.14)
ę� &� &� ś�
(M1 -� N1 f1 sgn(a�))h -� (M2 -� N2 f2 sgn(a�))h
ę� ś�
&� &�
M1 +� M2 -� N1 f1 sgn(a�) -� N2 f2 sgn(a�)
�� ��
Dla systemu z kołem podpierającym:
&�
N3 f3 sgn(a�3)
�� ł�
(3.15)
-�
ę� ś�
r3
ę� ś�
&�
N3 f3 sgn(a�3)
ę� ś�
-�
t� =�
ę� ś�
r3
ę�(M1 -� N1 f1 sgn(a�))h -� (M2 -� N2 f2 sgn(a�))hś�
&� &�
ę� ś�
&� &�
M1 +� M2 -� N1 f1 sgn(a�) -� N2 f2 sgn(a�)
ę� ś�
�� ��
gdzie: M1, M2  momenty napędzające
" N1, N2, N3  Nacisk na odpowiednie koła
12
" f1, f2, f3  współczynnik tarcia dla poszczególnych kół
Lagrange II  modelowanie dynamiki
Przy użyciu transforamcji odsprzęgającej możemy
zapisać równania Lagrange a II typu (dla systemu bez koła
podpierającego):
&� &�2 &�
&�&� &�
m4l2 sin(b� )b�&� +� (m4 +� 2m1)r cos(b� )a� +� m4l2 cos(b� )b� -� (m4 +� 2m1)r sin(b� )b�a� =� l�1
(3.16)
2
&� &� &�
&�&� &�
-� m4l2 cos(b� )b�&� +� (m4 +� 2m1)r sin(b� )a� +� m4l2 sin(b� )b� +� (m4 +� 2m1)r cos(b� )b�a� =� l�2
2
&� &�
&� &� &�
(2Iz1h2 +� 2m1l12 +� 2Ix1 +� Iz4 +� m4l2 )b�&� -� m4l2rb�a� =� (M1 -� N1 f1 sgn(a�) -� M2 +� N2 f2 sgn(a�))h
2
&�
&�&� &� &�
(r2m4 +� 2r2m1 +� 2Iz1)a� +� m4l2rb� =� M1 +� M2 -� N1 f1 sgn(a�) -� N2 f2 sgn(a�)
13
Lagrange II  modelowanie dynamiki
Analogicznie dla systemu z kołem podpierającym:
&� &�2
&�&�
(m3l5 +� m4l2)b�&�sin(b� ) +� (2m1 +� m4 +� m3)ra� cos(b� ) +� (m4l2 +� m3l5)b� cos(b� ) -�
N3 f3 sgn(a�3)
&�
&�
+� (2m1 +� m4 +� m3)rb�a� sin(b� ) =� -� +� l�1
r3
&� &�2
&�&�
-� (m3l5 +� m4l2)b�&�cos(b� ) +� (2m1 +� m4 +� m3)ra� sin(b� ) +� (m4l2 +� m3l5)b� sin(b� ) -�
(3.17)
N3 f3 sgn(a�3)
&�
&�
+� (2m1 +� m4 +� m3)rb�a� cos(b� ) =� -� +� l�1
r3
2
&�2vA &�2vA
8Ix3l13b� 6Ix3l14b�
2 2
(2Iz1h2 +� Ix3 -�
&�2)2 +� 2m1l12 +� Iz4 +� 2Ix1 +� m3l5 +� m4l2 -� (vA2 +� l12b� +�
&�2)3
(vA2 +� l12b�
2 2 4
&�2vA(l16b� +� (-�13vAl14 +�12l15vA)b� +� (-�5vAl12 -� 30l13v3 )b� +� 9vA +� 6l1v5
&�6 &�4 &�2 6 A
Ix3l13b�
A
&�
+� )b�&�-�
&�2)5
(vA2 +� l12b�
&�
&� &� &�
+� (m4l2 +� m3l5)rb�a� =� (M1 -� N1 f1 sgn(a�) -� M2 +� N2 f2 sgn(a�))h
&�
&�2 r cos(b� )N3 f3 sgn(a�3) +�
&�&�
(2r2m1 +� r2m4 +� r2m3 +� 2Iz1)a� +� (rm4l2 +� rm3l5)b� =� -�
r3
&�
r sin(b� )N3 f3 sgn(a�3)
&� &�
-� +� M1 +� M2 -� N1 f1 sgn(a�) -� N2 f2 sgn(a�)
r3
14
Lagrange II  symulacja dynamiki
Dla ścieżki przedstawionej poniżej oraz tabeli z cechami
fizycznymi robota został przygotowany model symulacji
dynamiki:
Start-up
Straight line movement
Arc movement
Deceleration and stop
l2 [m] m1 [kg] m2 [kg] m3 [kg] m4 [kg]
Ix1[kgm2] Ix2[kgm2] Ix3[kgm2] Iz1[kgm2]
0.07 1.5 1.5 0.5 5.67 0.02 0.02 0.005 0.051
N1 [N] N2 [N] N3 [N] f1 f2 f3
Iz2[kgm2] Iz3[kgm2] Iz4[kgm2]
0.051 0.002 0.154 31.25 31.25 29.20 0.01 0.01 0.001
15
Lagrange II  symulacja dynamiki
Jako wejście zostanie użyty wynik z modelu
rozwiązującego zadanie odwrotne kinematyki (bez koła
podpierającego):
16
Lagrange II  symulacja dynamiki
(przypadek z kołem podpierającym):
17
Lagrange II  symulacja dynamiki
Momenty napędzające (bez koła podpierającego)
M1 [Nm]
M2 [Nm]
18
Lagrange II  symulacja dynamiki
Mnożniki Lagrange a (bez koła podpierającego)
19
Lagrange II  symulacja dynamiki
Momenty napędzające (z kołem podpierającym)
20
Lagrange II  symulacja dynamiki
Mnożniki Lagrange a (z kołem podpierającym)
21
DZI
KUJ
ZA UWAG