1. Aufgabe
Gegeben sind ein Startzustand ⟪S_0(t_0, s_0, v_0, a_0)⟫ und (symmetrische) Maximalwerte für Beschleunigung ⟪a_("max")⟫ und Ruck ⟪j_("max")⟫.
Gesucht ist eine Trajektorie zum Endzustand ⟪S_e(t_e, s_e, v_e, a_e)⟫ mit ⟪t_e⟫ minimal, ⟪s_e⟫ beliebig, ⟪v_e=0⟫ und ⟪a_e=0⟫ unter Einhaltung von ⟪a_("max")⟫ und ⟪j_("max")⟫.
2. Vorbereitungen
- Wir brauchen die Funktion ⟪"sign"(x)⟫:
⟪ "sign"(x) = { (+1 , if x gt 0 ), ( 0 , if x = 0 ), (-1 , if x lt 0 ) :} ⟫
Daraus folgt:
⟪(1)⟫⟪ "sign"(x) * abs(x) = x ⟫
- Beschleunigung ⟪a(t)⟫, Geschwindigkeit ⟪v(t)⟫ und Position ⟪s(t)⟫
einer Bewegung mit konstem Jerk ⟪j⟫ sind:
⟪{: ( a(t), = , a_0 + j * t ) :}⟫
⟪{: ( v(t), = , v_0 + int_(u=0)^t a(u) \ du ), ( , = , v_0 + int_(u=0)^t (a_0 + j u) \ du ), ( , = , v_0 + int_(u=0)^t a_0 \ du + int_(u=0)^t j u \ du ), ( , = , v_0 + a_0 t + j/2 t^2 ) :}⟫
⟪{: ( s(t), = , s_0 + int_(u=0)^t v(u) \ du ), ( , = , s_0 + int_(u=0)^t (v_0 + a_0 u + j/2 u^2 ) \ du ), ( , = , s_0 + int_(u=0)^t v_0 \ du + int_(u=0)^t a_0 u \ du + int_(u=0)^t j/2 u^2 \ du ), ( , = , s_0 + v_0 t + a_0/2 t^2 + j/6 t^3 ) :}⟫
3. Lösung
Die Lösung besteht aus bis zu 4 Phasen. In jeder Phase beginnen wir die Zeitmessung wieder bei 0, die Gesamtlaufzeit ergibt sich dann aus der Summe der ⟪ Delta t_i⟫.
3.1. Reduktion der Beschleunigung auf 0
Das Vorzeichen des Jerks ist umgekehrt zum Vorzeichen von ⟪a_0⟫, die Dauer der Phase ist proportional zum Betrag von ⟪a_0⟫:
⟪ j_a := -"sign"( a_0 ) * j_("max") ⟫
⟪ Delta t_0 := abs( a_0 ) // j_("max") ⟫
Wenn ⟪Delta t_0=0⟫, entfällt die Phase 0.
Während der Phase 1 bekommen wir folgende Werte:
Nach der Phase 1 erreichen wir den Zustand ⟪S_1(t_1, s_1, v_1, a_1)⟫:
⟪{: ( t_1, = , t_0 + Delta t_0 ), ( a_1, = , a_0 + j_0 * Delta t_0 ), ( , = , a_0 - "sign"(a_0) * cancel( j_max ) * abs( a_0 ) // cancel( j_max )), ( , = , a_0 - "sign"(a_0) * abs( a_0 ) ), ( , = , a_0 - a_0 = 0 "✓" ), ( v_1, = , v_0 + a_0 Delta t_0 + j_a/2 Delta t_0^2 ) :}⟫
3.2. Reduktion der Geschwindigkeit auf 0 bei ⟪abs(v_1) ge a_(max)^2//j_(max)⟫
Die Geschwindigkeit ⟪v_1⟫ bauen wir in drei Schritten auf ⟪0⟫ ab:
- Erhöhung der Verzögerung ⟪a⟫ von ⟪0⟫ auf ⟪a_(max)⟫ mit Jerk ⟪j_1 =
j_(max)⟫:
⟪ a_1(t) = -"sign"( v_1 ) * j_(max)*(t-t_1) ⟫
Dazu brauchen wir ⟪Delta t_1 = a_(max) // j_(max)⟫.
- Abbremsen mit konstanter (⟪j_2=0⟫) Verzögerung ⟪a = a_(max)⟫:
⟪ a(t) = -"sign"( v_1 ) * a_(max) ⟫
Die Zeit ⟪Delta t_2⟫ hängt von der abzubauenden Geschwindigkeit ⟪abs( v_1 )⟫ ab.
- Reduzierung der Verzögerung ⟪a⟫ von ⟪a_(max)⟫ auf ⟪0⟫ mit Jerk
⟪j_3 = -j_(max)⟫:
⟪ a(t) = -"sign"( v1 ) * (a_(max) - j_(max)*(t-t_3)) ⟫
Dazu brauchen wir ⟪Delta t_3 = Delta t_1⟫.
Nach den drei Phasen erreichen wir die Geschwindigkeit ⟪v_4⟫:
⟪ v_4 = v_1 + int_(t=0)^(Delta t_1) a_1(t)\ dt + int_(t=0)^(Delta t_2) a_2(t)\ dt + int_(t=0)^(Delta t_1) a_3(t)\ dt ⟫
Das Ziel ist ⟪v_4 = 0⟫:
⟪{: ( -v_1, = int_(t=0)^(Delta t_1) a_1(t)\ dt + int_(t=0)^(Delta t_2) a_2(t)\ dt + int_(t=0)^(Delta t_1) a_3(t)\ dt ), ( , = int_(t=0)^(Delta t_1) -"sign"(v_1)*j_(max)t \ dt + int_(t=0)^(Delta t_2) -"sign"(v_1)*a_(max)\ dt + int_(t=0)^(Delta t_1) -"sign"(v_1)(a_(max)-j_(max)t)\ dt ), ( , = -"sign"(v_1) (int_(t=0)^(Delta t_1) j_(max)t \ dt + int_(t=0)^(Delta t_2) a_(max)\ dt + int_(t=0)^(Delta t_1) (a_(max)-j_(max)t)\ dt )) :}⟫
⟪{: ( v_1/("sign"(v_1)), = int_(t=0)^(Delta t_1) j_(max)t \ dt + int_(t=0)^(Delta t_2) a_(max)\ dt + int_(t=0)^(Delta t_1) a_(max)\ dt -int_(t=0)^(Delta t_1) j_(max)t\ dt ), ( , = cancel( j_(max)/2 Delta t_1^2) + a_(max) Delta t_2 + a_(max) Delta t_1 - cancel( j_(max)/2 Delta t_1^2 )), ( abs( v_1 ) , = a_(max) ( Delta t_2 + Delta t_1 ) = a_(max) ( Delta t_2 + a_(max) / j_(max) ) ) :}⟫
Nach Division durch ⟪a_(max)⟫ und Subtraktion von ⟪a_(max) // j_(max)⟫ erhalten wir:
⟪ Delta t_2 = abs( v_1 ) / a_(max) - a_(max) / j_(max) ⟫
Aus ⟪abs(v_1) ge a_(max)^2//j_(max)⟫ folgt ⟪ Delta t_2 ge 0⟫.
3.3. Reduktion der Geschwindigkeit auf 0 bei ⟪abs(v_1) le a_(max)^2//j_(max)⟫
Bei kleinem ⟪abs(v_1)⟫ bauen wir die Geschwindigkeit ⟪v_1⟫ in zwei Schritten auf ⟪0⟫ ab:
- Erhöhung der Verzögerung ⟪a⟫ von ⟪0⟫ auf ⟪a_2⟫ mit Jerk ⟪j_1 =
-"sign"(v_1)*j_(max)⟫:
⟪ a_1(t) = -"sign"(v_1) * j_(max)*(t-t_1) ⟫
Nach der Zeit ⟪Delta t_1⟫ erreichen wir die Verzögerung ⟪a_2⟫/⟪a_3⟫:
⟪ a_2 = a_3 = -"sign"(v_1) * j_(max)*Delta t_1 ⟫
- Reduzierung der Verzögerung ⟪a⟫ von ⟪a_2⟫ auf ⟪0⟫ mit Jerk ⟪j_3 =
+"sign"(v_1)*j_(max)⟫:
⟪ a_3(t) = a_3 + "sign"( v1 ) * j_(max)*(t-t_3) ⟫
Nach der Zeit ⟪Delta t_3=Delta t_1⟫ erreichen wir die Verzögerung ⟪a_4⟫:
⟪ a_4 = a_3 + "sign"(v1) * j_(max) * Delta t1 ⟫
⟪ a_4 = -"sign"(v_1) * j_(max)*Delta t_1 + "sign"(v1) * j_(max) * Delta t1 = 0 \ ✓⟫
Nach den zwei Phasen erreichen wir die Geschwindigkeit ⟪v_4⟫:
⟪ v_4 = v_1 + int_(t=0)^(Delta t_1) a_1(t)\ dt + int_(t=0)^(Delta t_1) a_3(t)\ dt ⟫
Das Ziel ist ⟪v_4 = 0⟫:
⟪{: ( -v_1, = int_(t=0)^(Delta t_1) a_1(t)\ dt + int_(t=0)^(Delta t_1) a_3(t)\ dt ), ( , = int_(t=0)^(Delta t_1) -"sign"(v_1)*j_(max)t\ dt + int_(t=0)^(Delta t_1) (a_3 + "sign"(v_1)*j_(max)t)\ dt ), ( , = int_(t=0)^(Delta t_1) -"sign"(v_1)*j_(max)t\ dt + int_(t=0)^(Delta t_1) a_3\ dt + int_(t=0)^(Delta t_1) "sign"(v_1)*j_(max)t\ dt ), ( , = -cancel("sign"(v_1) * (j_(max))/2 Delta t_1^2) + a_3*Delta t + cancel("sign"(v_1) * (j_(max))/2 Delta t_1^2 )), ( , = -"sign"(v_1) * j_(max)*Delta t_1 * Delta t_1 ) :}⟫
⟪{: ( v_1/("sign"(v_1)), = j_(max)*Delta t_1^2 ), ( abs( v_1 ) , = j_(max)*Delta t_1^2 ) :}⟫
Nach Division durch ⟪j_(max)⟫ und Ziehen der Wurzel erhalten wir:
⟪ Delta t_1 = sqrt( abs( v_1 ) / j_(max)) ⟫
Aus ⟪abs(v_1) le a_(max)^2//j_(max)⟫ folgt ⟪ a_2 le a_(max)⟫.
4. Der Algorithmus:
Gegeben: Startzustand ⟪S_0(t_0, s_0, v_0, a_0)⟫ und (symmetrische) Maximalwerte für Beschleunigung ⟪a_("max")⟫ und Ruck ⟪j_("max")⟫.
Reduziere Beschleunigung auf 0:
⟪{: ( j_0 , = -"sign"( a_0 ) ), ( Delta t_0, = abs( a_0 ) // j_(max) ) :}⟫
State nach Phase 1:
⟪{: ( t_1 , = t_0 + Delta t_0 ), ( a_1 , = a_0 + j_0 * Delta t_0 = 0 ), ( v_1 , = v_0 + a_0 * Delta t_0 + j_0/2 * Delta t_0^2 ), ( s_1 , = s_0 + v_0 * Delta t_0 + a_0/2 * Delta t_0^2 + j_0/6 * Delta t_0^3 ) :}⟫
Reduziere Geschwindigkeit auf 0:
⟪{: ( j_1 = -"sign"( v_1 ) * j_(max) ), ( j_2 = 0 ), ( j_3 = +"sign"( v_1 ) * j_(max) ), ( Delta t_2 = max{ 0, abs( v_1 ) / a_(max) - a_(max)/j_(max) } ), ( Delta t_1 = { ( a_(max) / j_(max) , if Delta t_2 gt 0 ), ( sqrt( abs( v_1 ) / j_(max)) , if Delta t_2 = 0 ) :} ), ( Delta t_3 = Delta t_1 ) :}⟫
State nach Phase 2:
⟪{: ( t_2 , = t_1 + Delta t_1 ), ( a_2 , = a_1 + j_1 * Delta t_1 ), ( v_2 , = v_1 + a_1 * Delta t_1 + j_1/2 * Delta t_1^2 ), ( s_2 , = s_1 + v_1 * Delta t_1 + a_1/2 * Delta t_1^2 + j_1/6 * Delta t_1^3 ) :}⟫
State nach Phase 3:
⟪{: ( t_3 , = t_2 + Delta t_2 ), ( a_3 , = a_2 + ubrace(j_2)_0 * Delta t_2 = a_2 ), ( v_3 , = v_2 + a_2 * Delta t_2 + ubrace(j_2/2)_0 * Delta t_2^2 = v_2 + a_2 * Delta t_2 ), ( s_3 , = s_2 + v_2 * Delta t_2 + a_2/2 * Delta t_2^2 + ubrace(j_2/6)_0 * Delta t_2^3 = s_2 + v_2 * Delta t_2 + a_2/2 * Delta t_2^2 ) :}⟫
State nach Phase 4:
⟪{: ( t_4 , = t_3 + Delta t_3 ), ( a_4 , = a_3 + j_3 * Delta t_3 = 0 ), ( v_4 , = v_3 + a_3 * Delta t_3 + j_3/2 * Delta t_3^2 = 0 ), ( s_4 , = s_3 + v_3 * Delta t_3 + a_3/2 * Delta t_3^2 + j_3/6 * Delta t_3^3 ) :}⟫
Werte der Lösung abhängig von ⟪t⟫:
⟪ a(t) = { ( a_0 + j_0 * (t-t_0) , if t ge t_0 ^^ t lt t_1 ), ( a_1 + j_1 * (t-t_1) , if t ge t_1 ^^ t lt t_2 ), ( a_2 + j_2 * (t-t_2) , if t ge t_2 ^^ t lt t_3 ), ( a_3 + j_3 * (t-t_3) , if t ge t_3 ^^ t lt t_4 ) :}⟫
⟪ v(t) = { ( v_0 + a_0 * (t-t_0) + j_0/2 * (t-t_0)^2 , if t ge t_0 ^^ t lt t_1 ), ( v_1 + a_1 * (t-t_1) + j_1/2 * (t-t_1)^2 , if t ge t_1 ^^ t lt t_2 ), ( v_2 + a_2 * (t-t_2) + j_2/2 * (t-t_2)^2 , if t ge t_2 ^^ t lt t_3 ), ( v_3 + a_3 * (t-t_3) + j_3/2 * (t-t_3)^2 , if t ge t_3 ^^ t lt t_4 ) :}⟫
⟪ s(t) = { ( s_0 + v_0 * (t-t_0) + a_0/2 * (t-t_0)^2 + j_0/6 * (t-t_0)^3 , if t ge t_0 ^^ t lt t_1 ), ( s_1 + v_1 * (t-t_1) + a_1/2 * (t-t_1)^2 + j_1/6 * (t-t_1)^3 , if t ge t_1 ^^ t lt t_2 ), ( s_2 + v_2 * (t-t_2) + a_2/2 * (t-t_2)^2 + j_2/6 * (t-t_2)^3 , if t ge t_2 ^^ t lt t_3 ), ( s_3 + v_3 * (t-t_3) + a_3/2 * (t-t_3)^2 + j_3/6 * (t-t_3)^3 , if t ge t_3 ^^ t lt t_4 ) :}⟫