Zeitlicher Determinismus durch Timer, ISRs und Hardware im SoC – Teil 6

Teil 5 unserer Serie war gewissermaßen die mentale Brücke zwischen der physikalischen Betrachtung unseres Problems und der nun erfolgenden immer tieferen Betrachtung seiner numerischen und algorithmischen Umsetzung.

Ich empfehle daher dringend jedem Quereinsteiger, mindestens Teil 5 zu studieren, um hier folgen zu können.

Wir können unser in Teil 5 vorgestelltes Problem offensichtlich in drei Verarbeitungsebenen teilen:

Initialisierung
Wechsel der Zielposition
Laden / Berechnen der nächsten Zeit für den Timer (ISR)

Unser mathematisches Modell muss daher numerisch an die zugrundeliegende Hardware im Hinblick auf die Berechnungen (Rundungsfehler, Fehlerfortpflanzung, Fließkommazahlen oder Integers) wie auch algorithmisch (sehr kurz gehaltene ISR-Zeiten) angepasst werden.

Zunächst betrachten wir unsere Problemstellung „aus dem Stand“. Wir haben eine aktuelle Winkelposition $\varphi_0$ und eine neue Zielposition $\varphi_1$ . Später erweitern wir auf neue Zielpositionen mitten in einer Fahrt.

Um den Zugang zu erleichtern, projizieren wir unser mentales Modell auf eine Wegbewegung. Der Grund liegt in einem einfacheren Zugang für Leser, welche nicht dieser Fachrichtung entspringen und so gewohnte Notationen vorfinden. Statt eines Winkels $\varphi$ betrachten wir einen Weg $s$ , und damit statt einer Winkelgeschwindigkeit $\frac{\varphi}{t}$ die klassische Geschwindigkeit $v=\frac{s}{t}$ .

Wie in Teil 5 beschrieben, setzen wir ein $\Delta s_{\mathrm{settle}} = d_\mathrm{settle}$ , um vor der Zielposition wieder zu stoppen und von der Anfahrtsseite aus einzuregeln.

Damit folgt, wir müssen die Distanz $d_{\mathrm{move}} = \max(0,\; |s_1-s_0|-d_\mathrm{settle})$ fahren.

In unserem Fall fangen wir gleich zu Beginn $|s_1-s_0| \leq d_\mathrm{settle}$ ab, in diesem Fall wechseln wir sofort in den Einregel-Modus. Somit bleiben nur die Fälle $|s_1-s_0| > d_\mathrm{settle}$ bestehen.

Nun schauen wir uns noch einmal unsere Rampe an.

Dabei haben wir Abschnitt 4, dessen Wegstrecke zwischen $0$ und $\infty$ liegt, und die übrigen Abschnitte, welche jeweils zwischen $0$ und einem jeweiligen Maximalwert liegen. Dieser Unterschied ist wichtig. Wir bezeichnen diese folgend als $\max d_1$ , $\max d_2$ , $\max d_3$ , $\max d_5$ , $\max d_6$ und $\max d_7$ .

Durch Abschnitt 4 gibt es keine Begrenzung für $d_\mathrm{move}$ nach oben.

Anders sieht es aus, wenn eine Fahrt gerade kürzer ausfällt, als die Summe der anderen Abschnitte.

Dazu noch einmal die Reihenfolge der Kürzungen:

Abschnitt 4, bis dieser ganz verschwindet
Abschnitte 2 und 6 symmetrisch
Abschnitte 1, 3, 5 und 7 symmetrisch

Aus didaktischen Gründen starten wir mit den mathematisch komplexesten Abschnitten 1, 3, 5 und 7. Diese Phasen dienen der Begrenzung des Rucks.

In unserem Beispiel gehen wir davon aus, dass der Ruck $J$ beim Beschleunigen wie auch Abbremsen mit einem identischen Maximalwert festgelegt wird. Damit können die Funktionsverläufe der Abschnitte durch Transformationen auf einen gemeinsamen Funktionsverlauf als Basis gesetzt werden.

Wir betrachten zunächst Abschnitt 1. Hier werden Geschwindigkeit und Beschleunigung vom Stillstand $v=0, a=0$ bis zur maximalen Beschleunigung $a_\mathrm{end} = \max a$ angehoben. Dieser Abschnitt führt zu einem $v_\mathrm{end}$ und $t_\mathrm{end}$ .

Genauso wie wir Geschwindigkeit und Beschleunigung erhöht haben, müssen wir diese anschließend wieder erniedrigen. Im Abschnitt 3 wird die Beschleunigung bei weiter zunehmender Geschwindigkeit bis $a=0$ verringert, in Abschnitt 5 bei abnehmender Geschwindigkeit negative Beschleunigung aufgebaut und im Abschnitt 7 wieder abgebaut, wie auch die Restgeschwindigkeit. Am Ende herrschen wieder $v=0, a=0$ .

Nun schauen wir uns an, wie sich der Verlauf ändert, wenn die Abschnitte 1, 3, 5 und 7 nicht voll durchlaufen werden, da der Weg kürzer ausfällt. Zunächst gilt durch Integration von $v$ aus $a$ und $s$ aus $v$ :

\begin{aligned} a(t) &=\int_0^t j(\tau)\,\mathrm{d}\tau =\int_0^t J\,\mathrm{d}\tau =Jt,\\[4pt] v(t) &=\int_0^t a(\tau)\,\mathrm{d}\tau =\int_0^t J\tau\,\mathrm{d}\tau =\frac{J}{2}t^2,\\[4pt] s(t) &=\int_0^t v(\tau)\,\mathrm{d}\tau =\int_0^t \frac{J}{2}\tau^2\,\mathrm{d}\tau =\frac{J}{6}t^3. \end{aligned}

Nun legen wir die verkürzten Abschnitte alle auf einen gemeinsamen Startpunkt:

Der dunkelblaue Kurvenverlauf ist in allen Fällen gemeinsam. Unterschiedlich ist nur, wann dieser verlassen wird. Zu trennen sind dabei der Graph für Weg-Zeit $s(t)$ zu Geschwindigkeit-Zeit $v(t)$ und Beschleunigung-Zeit $a(t)$ . Die Graphen von $v(t)$ und $a(t)$ lassen sich in allen Fällen durch einfache Spiegelungen und Translationen von Abschnitt 1, im Bedarfsfall gekürzt, modellieren.

Tatsächlich gibt es Implementationen, welche auf dieser Zeitbasis aufsetzen. Wir gehen aber einen Schritt weiter. Bei der Fahrt wird eine Funktion fortwährend gerufen, die ISR des Timers. Diese generiert den nächsten Teilschritt des Steppers und muss dann den Timer mit der nächsten Weckzeit laden.

Für unsere Ansteuerung des Steppers ist daher nicht die Zeit die Basis, sondern seine Schritte bzw. Teilschritte. Nicht die Zeit, sondern der Schritt ist sozusagen unser Takt, und daher betrachten wir nun Zeit, Geschwindigkeit und Beschleunigung aus Perspektive des Weges:

In jedem Abschnitt geht es daher darum, so schnell wie möglich bei jedem Teilschritt das Neuladen des Timers $\Delta t(i)$ zu ermöglichen. Hier gilt es je nach Anforderung zwei grundsätzliche Ansätze zu unterscheiden:

$\Delta t(i)$ wird direkt vorberechnet und muss von der ISR nur noch in den Timer geladen werden.
Erfolgt die Timerprogrammierung relativ zum tatsächlichen Ausführungszeitpunkt der ISR, können sich ISR-Latenzen und Rundungsfehler über die Fahrt aufsummieren.
$\Delta t(i)$ wird aus vorberechneten absoluten Zeitpunkten $t(i)$ berechnet.
Dadurch bleiben die Sollzeitpunkte auf einen gemeinsamen zeitlichen Ursprung bezogen. Eine Akkumulation von Rundungsfehlern bei der Berechnung einzelner Zeitintervalle wird vermieden. Auch ISR-Latenzen führen bei geeigneter Timerprogrammierung nicht zu einer fortschreitenden zeitlichen Verschiebung.

Die Wahl hängt von den Rahmenbedingungen der Fahrt ab, etwa, ob ein Messpunkt auch während der Fahrt z.B. Sensordaten erfassen soll.

Unsere LUT muss als Schlüssel also nicht die Zeit, sondern den Weg, als Teilschritte, besitzen. Jedem Teilschritt wird die Zeit zum Nachladen des Timers zugeordnet, wie oben erläutert bereits als Differenzzeit zwischen zwei Steps oder als Differenz zum Beginn des Abschnitts.

Daher müssen wir unsere Funktionen $f$ von Zeit $t$ auf Weg $s$ umstellen.

\begin{aligned} s&=\frac{J}{6}t^3\\ 6s&=Jt^3\\ \frac{6s}{J}&=t^3\\ \sqrt[3]{\frac{6s}{J}}&=\sqrt[3]{t^3}=t\\ t_A(s)&=\sqrt[3]{\frac{6s}{J}} \end{aligned}

Wird $t(s)$ nun auch in $v(t)$ und $a(t)$ eingesetzt, so ergibt sich für $v(s)$ und $a(s)$ daraus:

\boxed{ v(s)=\frac{J}{2}\left(\frac{6s}{J}\right)^{2/3}, \qquad a(s)=J\left(\frac{6s}{J}\right)^{1/3} }

Darauf aufbauend betrachten wir nun unsere symmetrisch verkürzten Abschnitte 1, 3, 5 und 7 diesmal aus der Sicht des Weges:

Der Preis ist sofort sichtbar: nun können wir in den Abschnitten 3 und 5 nicht mehr nur mit Spiegelung und Verschiebung arbeiten. Der Grund liegt in $t=\frac{ds}{dv}$ . Durch die unterschiedlichen Start- und Zielgeschwindigkeiten $v_\mathrm{start}$ und $v_\mathrm{end}$ im Gegensatz zu den Abschnitten 1 und 7 ergeben sich die Skalierungseffekte für die Abschnitte 3 und 5.

Da die Abschnitte 1 und 7 dagegen immer $v=0$ als Start- oder Endgeschwindigkeit besitzen, wie auch $a=0$ als Start- oder Endbeschleunigung, ist auch die Skalierung hier immer gleich und muss nicht nachträglich einberechnet werden.

Die Grafiken zeigen ebenfalls, die Abschnitte 3 und 5 besitzen viel mehr Teilschritte als die Abschnitte 1 und 7, da hier die Geschwindigkeiten wesentlich höher liegen.

Im Abschnitt 1 gilt:

s(0)=0,\qquad v(0)=0,\qquad a(0)=0,\qquad j(t)=J

Und daraus folgend:

\begin{aligned} a(t) &=a(0)+\int_0^t j(\tau)\,\mathrm{d}\tau =\int_0^t J\,\mathrm{d}\tau =Jt,\\[4pt] v(t) &=v(0)+\int_0^t a(\tau)\,\mathrm{d}\tau =\int_0^t J\tau\,\mathrm{d}\tau =\frac{J}{2}t^2,\\[4pt] s(t) &=s(0)+\int_0^t v(\tau)\,\mathrm{d}\tau =\int_0^t \frac{J}{2}\tau^2\,\mathrm{d}\tau =\frac{J}{6}t^3. \end{aligned}

Abschnitt 5 startet dagegen mit einer Geschwindigkeit $V$ :

s(0)=0,\qquad v(0)=V,\qquad a(0)=0,\qquad j(t)=-J

Daraus folgt:

\begin{aligned} a(t) &=a(0)+\int_0^t j(\tau)\,\mathrm{d}\tau =\int_0^t (-J)\,\mathrm{d}\tau =-Jt,\\[4pt] v(t) &=v(0)+\int_0^t a(\tau)\,\mathrm{d}\tau =V+\int_0^t (-J\tau)\,\mathrm{d}\tau =V-\frac{J}{2}t^2,\\[4pt] s(t) &=s(0)+\int_0^t v(\tau)\,\mathrm{d}\tau =\int_0^t \left(V-\frac{J}{2}\tau^2\right)\,\mathrm{d}\tau =Vt-\frac{J}{6}t^3. \end{aligned}

Genau hier sehen wir den Skalierungseffekt von $a(s)$ , $v(s)$ und $t(s)$ durch $V$ hervorgerufen.

Leider haben wir noch einen weiteren Effekt. Für uns ist $t(s)$ durch die ISRs wesentlich interessanter als $s(t)$ . Für die Abschnitte 1 und 7 ist dies trivial, wie wir bereits gesehen haben: $t_A(s)=\sqrt[3]{\frac{6s}{J}}$ .

Bei den Abschnitten 3 und 5 mit der Umstellung von $s(t) =Vt-\frac{J}{6}t^3$ auf $t(s)$ wird das Problem erheblich größer, denn wir erhalten eine kubische Gleichung mit einem linearen Term:

t^3 – \frac{6v_{\max}}{J}t + \frac{6s}{J} = 0

Und die Werkzeuge zu deren Auflösung sind auf analytischem Weg Winkelfunktionen oder komplexe Zahlen (Cardano ohne komplexe Zahlen scheitert bei uns an der Wurzel negativer Zahlen):

t_5(s)= 2\sqrt{\frac{2v_{\mathrm{start}}}{J}}\, \sin\!\left[ \frac{1}{3}\arcsin\!\left( \frac{3s}{2v_{\mathrm{start}}} \sqrt{\frac{J}{2v_{\mathrm{start}}}} \right) \right]

t_5(s)= \operatorname{Re}\!\left[ (-1-\mathrm{i}\sqrt{3})\, \sqrt[3]{ -\frac{3s}{J} +\mathrm{i}\sqrt{ \frac{8v_{\mathrm{start}}^3}{J^3} -\frac{9s^2}{J^2} } } \right]

Und hier wird es interessant. Unsere Problemstellung lässt sich mit LUTs mathematisch so umstellen, dass es drei Berechnungsteile gibt. Möglich wird dies, da, wie im Vierabschnittsprofil über den Weg ersichtlich, die Verläufe immer wieder auf einen Verlauf mit Skalierung zurückführen lassen. Darauf aufbauend realisieren wir:

die Abbildung eines Grundverlaufes in der LUT, zur compile-Zeit oder beim Programmstart
die Berechnung der Skalierungswerte einmalig bei der Bahnberechnung
die Berechnung der nächsten Weckzeit auf Grundlage von LUT und Skalierungswerten in der ISR

Zur Verdeutlichung noch einmal die Verläufe:

Unser „Takt“ ist der Teilschritt. Berechnen wir dabei für den Fall der vollständigen Abschnitte 3 und 5, so erhalten wir die höchste Auflösung. Bei verkürzten Abschnitten skalieren wir immer nach unten. Heißt, es fehlen nie Stützstellen, wir müssen nur zwischen zwei Stützstellen interpolieren, wenn wir keine Stützstelle exakt treffen. Und gerade für die schnellste Fahrt mit vollständigen Abschnitten werden die Stützstellen direkt in der LUT gehalten, es ist dann keine Interpolation nötig.

Ausgangspunkt ist die physikalische Weggleichung

s=Vt-\frac J6t^3

Unsere LUT stellt die Beziehung zwischen einem Referenzweg $S$ und einer Referenzzeit $T$ her. Beide normieren wir hierzu:

T=\sqrt{\frac{2V}{J}}, \qquad S=VT

Den späteren Bezug zu unserer realen Bahnberechnung stellen wir her durch den normierten Weg $\hat{s}$ und die normierte Zeit $\hat{t}$ :

\hat{s}=\frac{s}{S}, \qquad \hat{t}=\frac{t}{T}

Nun stellen wir um

\begin{aligned} \hat{s}=\frac{s}{S} &=\frac{Vt}{S}-\frac{Jt^3}{6S}\\ &=\frac{VT}{S}\hat{t}-\frac{JT^3}{6S}\hat{t}^3. \end{aligned}

Die Wahl $S=VT$ setzt den Koeffizienten des linearen Terms auf eins. Damit folgt:

\begin{aligned} \hat{s} &=\hat{t}-\frac{JT^3}{6VT}\hat{t}^3\\ &=\hat{t}-\frac{JT^2}{6V}\hat{t}^3. \end{aligned}

Und hier ist die Stelle, warum wir $T=\sqrt{\frac{2V}{J}}$ gesetzt haben. Denn dadurch gilt $T^2=\frac{2V}{J}$ . Und damit reduziert sich unsere Gleichung zu:

\hat{s}=\hat{t}-\frac{\hat{t}^3}{3}

Diese Gleichung liefert den normierten Weg $\hat{s}$ aus der normierten Zeit $\hat{t}$ .

Dies ist die Basis für unsere Umkehrfunktion. Wir nennen sie $F_t$ . Sie liefert nun $\hat{t}$ aus $\hat{s}$ .

\hat{s}=\hat{t}-\frac{\hat{t}^3}{3} \quad\Longleftrightarrow\quad \hat{t}=F_t(\hat{s})

Im verwendeten Bereich $0\leq \hat{t} \leq1/\sqrt2$ ist die Wegfunktion streng monoton steigend und damit eindeutig umkehrbar.

Anmerkung: Die Funktion ist sogar auf $0\leq \hat{t} \leq1$ streng monoton steigend. Bei unserem symmetrischen Vierabschnittprofil gilt $V=J\tau^2$ . Mit $T=\sqrt{2V/J}$ folgt damit unmittelbar:

\frac{\tau}{T}=\frac1{\sqrt2}

Nun setzen wir $\hat{t}=t/T$ und $\hat{s}=s/S$ wieder ein:

\frac{t}{T}=F_t\!\left(\frac{s}{S}\right)

Durch Multiplikation mit $T$ ergibt die Form:

t(s)=T\,F_t\!\left(\frac{s}{S}\right)

Dabei rechnet $s/S$ den konkreten Bahnweg in einen normierten Weg $\hat{s}$ um, $F_t$ liefert die zugehörige normierte Zeit, und $T$ skaliert diese zurück in die reale Bahnzeit $t$ .

Durch diese Umformung haben wir unser Problem in drei Teile zerlegt:

Einmalig aufwendige Berechnung der LUT für $F_t$ zur Compile-Zeit oder bei der Initialisierung der Firmware
Berechnung von $T$ und $S$ bei der Bahnberechnung
Berechnung von $t(s)$ bei jeder ISR

Im Kern reduzieren wir damit die Auswertung der inversen Wegfunktion auf zwei Skalierungsmultiplikationen und einen LUT-Zugriff, gegebenenfalls mit Interpolation.

Im nächsten Teil wird die numerische Umsetzung dieses Ansatzes betrachtet.