2 - Wydział Elektrotechniki i Automatyki

WYDZIAŁ ELEKTROTECHNIKI
I AUTOMATYKI
KATEDRA AUTOMATYKI
Dynamika mobilnych robotów kołowych
Dr inż. Mariusz Dąbkowski
DYNAMIKA ROBOTÓW
Zagadnienie rozpatrujące sposób, w jaki
ruch robota związany jest z siłami i
momentami napędowymi lub
przyłożonymi siłami zewnętrznymi
KONFIGURACJA PLATFORMY MOBILNEJ I ROZKŁAD
PRĘDKOŚCI PUNKTÓW CHARAKTERYSTYCZNYCH
f(xH,yH)=0
y4
ROBOT
PIONEER-2DX
x4
AB=AC=l1=0,35[m]
AH=l3=0,2 [m]
AD=l
r=0,15 [m]
Układ równań więzów nieholonomicznych dla robota
2-kołowego ma postać
x& A − α&rcos β = 0
y& A − α&r sin β = 0
a w zapisie wektorowym
J (q ) q& = 0
gdzie
⎡1 0 0 − rcosβ ⎤
J(q) = ⎢
⎥ – jakobian
−
0
1
0
sin
β
r
⎣
⎦
q = [ x A , y A , β ,α ] – wektor współrzędnych uogólnionych
q& = [ x& A , y& A , β& ,α& ] – wektor prędkości uogólnionych
Równania Lagrange’a II rodzaju z mnożnikami w postaci wektorowej
T
T
d ⎛ ∂E ⎞ ⎛ ∂E ⎞
⎜⎜
⎟⎟ − ⎜⎜
⎟⎟ = Q + J T (q)λ
dt ⎝ ∂q& ⎠ ⎝ ∂q ⎠
gdzie
E(q, q& ) – energia kinetyczna układu
Q – wektor sił uogólnionych
J(q) – jakobian
λ – wektor mnożników Lagrange’a przedstawiających siły
tarcia suchego leżące w płaszczyźnie styczności koła 1z
z jezdnią i zaczepione w punkcie zetknięcia tego koła z
jezdnią
Po podstawieniu odpowiednich składowych do równania Lagrange’a
otrzymuje się 4 równania
(m1 + m2 + m4 )&x&A + [(m1 − m2 )l1cosβ + m4 l 2 sin β ]β&& +
2
&
+ [(− m1 + m 2 )l1sinβ ](β ) = λ1
(m1 + m2 + m4 )&y&A + [(m1 − m2 )l1sinβ − m4 l2 cos β ]β&& +
2
&
[
]
(
)
(
)
+ m1 − m 2 l1cosβ β = λ2
[(m1 − m2 )l1cosβ + m4 l2 sin β ]&x&A + [(m1 − m2 )l1sinβ − m4 l 2 cos β ]&y&A +
+ [(m1 + m 2 )l12 + m4 l 22 + I z 4 + I x 1 + I x 2 + (I z 1 + I z 2 )h12 ]β&& +
+ (I z 1 + I z 2 )h1α&& = (M 1 − M 2 − N 1 f1 + N 2 f 2 )h1
(I z1 + I z 2 )α&& + (I z1 − I z 2 )h1 β&& = M 1 + M 2 − N 1 f1 − N 2 f 2 − λ1rcosβ − λ2 rsinβ
gdzie
m1=m2 – masy kół 1 i 2,
Ix1=Ix2 – zastępcze masowe momenty bezwładności kół 1 i 2 określone
względem osi x1i x2 związanymi z tymi kołami,
Iz1=Iz2 – zastępcze masowe momenty bezwładności kół 1 i 2 określone
względem osi obrotu własnego tych kół,
m4 – masa ramy,
Iz4 – zastępczy masowy moment bezwładności ramy określony
względem osi związanej z ramą,
N1, N2 – siły nacisku na koła odpowiednio 1 i 2,
f1, f2 – współczynniki tarcia toczenia kół,
M1, M2 – momenty napędzające koła,
r1=r2=r – promień kół,
l1, l2,h1 – odległości oraz współczynnik wynikające z geometrii układu.
W rozważaniach nie uwzględniono małej masy koła 3 oraz
oporu toczenia tego koła.
!!! Uwikłana postać prawych stron równań !!!
Transformacja odsprzęgająca
momentów napędzających koła
mnożniki
Lagrange’a
od
&& + C(q, q& )q& = B(q )τ + J T (q )λ
M (q )q
Wektor współrzędnych uogólnionych q = [ x A , y A , β ,α ] można
przedstawić w postaci
q = [q1 , q 2 ]T
gdzie
q1 = [ x A , y A ]T
q 2 = [ β , α ]T
Równania więzów nieholonomicznych w zapisie wektorowym
i przy uwzględnieniu nowych współrzędnych
gdzie
⎡ q& 1 ⎤
[J 1 (q) J 2 (q)]⎢ ⎥ = 0
⎣q& 2 ⎦
⎡1 0⎤
J 1 (q) = ⎢
⎥
⎣0 1 ⎦
⎡1 − rcosβ ⎤
J 2 (q) = ⎢
⎥
−
0
r
sin
β
⎣
⎦
Zależność pomiędzy prędkościami uogólnionymi q& 1 a q& 2
gdzie
J 12 (q) = − J 1−1 (q)J 2 (q)
q& 1 = J 12 (q)q& 2
Wektor współrzędnych
następująco
gdzie
uogólnionych
wraża
⎡ J 12 (q)⎤
q& = ⎢
q& 2 = T(q)q& 2
⎥
⎣ I 2x2 ⎦
I 2x2
– macierz jednostkowa
⎡I 2x2 J 12 (q)⎤
T(q) = ⎢
⎥
I
0
2x2
⎣
⎦
&i q
&& mają postać
Wektory q
q& = T(q)E2q& 2
gdzie
⎡ 0 ⎤
E2 = ⎢ ⎥
⎣I 2x2 ⎦
& (q, q& )E q&
&& = T(q)E 2q
&& 2 + T
q
2 2
się
więc
Dokonując transformacji równania stanu przestrzennego
otrzymuje się układ
&& 2 + C12 (q, q& 2 )q& 2 = B1 (q )τ + J 1T (q )λ
M 12 (q )q
&& 2 + C22 (q, q& 2 )q& 2 = B 2 (q )τ
M 22 (q )q
gdzie
M 12 = E1T T T MTE 2
(
,
M 22 = E T2 T T MTE 2
) ,C
& + T TCT E
C12 = E1T T T MT
2
B1 = E1T T T B
⎡I ⎤
E1 = ⎢ 2x2 ⎥
⎣ 0 ⎦
,
B 2 = E T2 T T B
22
(
)
& + T TCT E
= E T2 T T MT
2
Po podstawieniu odpowiednich danych układ równań można
zapisać
[
]
( )
[
]
m l [− β&& cos β + (β& ) sin β ]+ (2m + m )r [α&&sinβ − α&β&cosβ ] = λ
2
&
&
&
m4 l 2 β sin β + β cos β + (2m1 + m4 )r α&&cosβ − α&β&sinβ = λ1
2
4 2
1
(2m l
2
1 1
4
)
+ m 4 l 22 + I z 4 + 2 I x 1 + 2 I z 1 h12 β&& − m 4 l 2 rα&&β& =
= ( M 1 − M 2 − N 1 f 1 + N 2 f 2 )h1
[
2
]
( )
(2m1 + m4 )r + 2 I z1 α&& + m4 l 2 r β&
2
2
= M 1 + M 2 − N 1 f1 − N 2 f 2
Mnożniki Lagrange’a są odsprzężone od momentów
napędzających
Równania pozwalają rozwiązać zadanie proste dynamiki
(α , β ,α& , β& ,α&&, β&& → M , M , λ , λ )
1
2
1
2
oraz odwrotne
(M , M , λ , λ
1
2
1
2
→ α , β , α& , β& , α&&, β&&
)
Równania umożliwiają analizę sił tarcia
styczności kół napędzających z jezdnią.
Y
Tp
λ2
.
A
To
β
λ1
X
w
punktach
Tarcie obwodowe
TO = λ1cosβ + λ2sinβ
Tarcie poprzeczne
TP = −λ1sinβ + λ2cosβ
Całkowita siła tarcia
TA = λ + λ
2
1
2
2
lub
TA = To2 + T p2
Wyznaczone w toku analizy wartości sił tarcia w punkcie
styczności nie mogą przekraczać wartości dopuszczalnej
wynikającej z zależności
TA ≤ μ A N A