Grad 3 Any-to-Rest-Solver

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

  1. 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 ⟫

  2. 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

t t0 ←Δt0→ t1 a a0 0 1 a'(t)=-jmax a(t) S0 S1

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)⟫

t t1 ←Δt1→ t2 ←Δt2→ t3 ←Δt3→ t4 a amax 0 1 a'(t)=+jmax 1 a'(t)=-jmax a1(t) a2(t) a3(t) S1 S2 S3 S4

Die Geschwindigkeit ⟪v_1⟫ bauen wir in drei Schritten auf ⟪0⟫ ab:

  1. 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)⟫.

  2. 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.

  3. 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)⟫

t t1 ←Δt1→ t2/t3 ←Δt3→ t4 a amax a2/3 0 1 a'(t)=+jmax 1 a'(t)=-jmax a1(t) a3(t) S1 S2/S3 S4

Bei kleinem ⟪abs(v_1)⟫ bauen wir die Geschwindigkeit ⟪v_1⟫ in zwei Schritten auf ⟪0⟫ ab:

  1. 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 ⟫

  2. 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 = 0 ), ( 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 + j_2 * Delta t_2 = 0 ), ( v_3 , = v_2 + a_2 * Delta t_2 + j_2/2 * Delta t_2^2 ), ( s_3 , = s_2 + v_2 * Delta t_2 + a_2/2 * Delta t_2^2 + j_2/6 * Delta t_2^3 ) :}⟫

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 ), ( 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 ) :}⟫