# IX Autonomous Lane Navigation Part 4/4 / Steering and Stabilization

This exercise deals with the steering and stabilization of a self-driving vehicle. There are several possible solutions to implement autonomous steering for a self-driving vehicle. These steering solutions are explained and can be applied in the second step of this exercise. At the end of this exercise, the course participants know the different implementation variants of the steering and stabilization of an autonomous vehicle and can apply them appropriately to the concrete course example of their own autonomous vehicle.

## Key Objectives

In this final exercise for lane navigation, we will focus on the steering and stabilization of our autonomous self-driving vehicle. The question will be discussed how it can be achieved that the car is located as far as possible between the two detected lane lines and also remains there. We will look at what happens in case of deviations or if, for example, only one lane line is detected. The goal of this exercise is an autonomous self-driving vehicle that is not only able to recognize lane lines but also to follow them. The vehicle can drive, steer and stabilize autonomously.

## Accompanying Theory

If you ask a human being how he would solve the problem of keeping a car in the middle of a lane, he would probably not have a difficult task. He would say something like: You would have to check how far you are from the left and right lanes. If you are too close to the left lane you would steer the car to the right and vice versa. He would use his eyes and spatial awareness to estimate how far away each lane is. The same is applicable to the speed. If the car is too slow he would press on the gas pedal if it is too fast he would step on the brake. How much force or torque he has to exert on the pedals and the steering wheel he has learned heuristically since he was - depending on the country - between 16 and 18 years old.

However, this seemingly simple consideration already involved a lot of components: Actuators (steering wheel and pedals), state measurement (how far the vehicle is from each lane and the current speed), and knowledge of a desired state (keeping the vehicle in the center of the lane as best as possible while maintaining speed limits). All of this is shown in the block diagram in Figure 1.

Fig. 1 - Block diagram of the closed loop controls for self driving cars.

Quelle: @Online {HowdoselfdrivingcarsdrivePart1LanekeepingassistbyEderSantanaMedium-2017-06-06, title = {How do self driving cars drive? Part 1: Lane keeping assist | by Eder Santana | Medium}, date = {2017-06-06}, year = {2017}, author = {Eder Santana}, file = {:./references/@edersantana-how-do-self-driving-cars-drive-part-1-lane-keeping-assist-581f6ff50349.html:html}, url = {https://medium.com/@edersantana/how-do-self-driving-cars-drive-part-1-lane-keeping-assist-581f6ff50349}, urldate = {2022-01-25}}

#### Bewegungsplanung: Sensoren

There are several possibilities to calculate how distant the car is from each lane. If computer vision is used, cameras are installed that point to the front or to the side to see the road. Classical computer vision uses edge detection capabilities to estimate the lanes as seen in the previous exercises. A second option would be to use deep learning to find the lanes - to train this, ground truths are needed that come either from hand labeling (not scalable) or from other sensors like LiDARs (better). LiDARs themselves are better for finding lane lines as they reflect light differently than asphalt.

After filtering out the lane lines (exercise 6-8), it is possible to calculate their distance to the car based on the geometry. Assuming that the cameras in the car are stable, the pixel inch transformation can be calculated.

The speed measurement is somewhat easier to estimate and is already provided by the vehicle's internal components.

### Bewegungsplanung: Lenken

Es wird angenommen dass die Steuerung möglich ist weil das Auto "gehackt" wurde um es programmatisch zu lenken. Das Problem reduziert sich darauf zu messen wo sich das Auto in der Spur befindet und wie es in den gewünschten Zustand gesteuert werden kann.

Sobald bekannt ist wo sich das Auto auf der Straße befindet und wie schnell es fährt muss es auf die gewünschte Position und Geschwindigkeit steuern. Eine weitere Vereinfachung ist die Annahme dass das Problem in zwei Teile unterteilt werden kann: Längskontrolle (wie viel Gas und Bremse) und Querkontrolle (wie viel Drehmoment auf die Lenkräder). Diese Aufteilung wird nicht immer funktionieren. Aber auf einer Autobahn oder Teststrecke auf der die Kurven sanft sind ist dies gut umsetzbar.

Die einfachste Steuerungsmethode ist der Proportional-Integral-Derivativ-Regler (PID). Es gibt noch mehrere andere Möglichkeiten der Steuerung aber der Einfachheit halber wird diese Methode verwendet.

Der PID-Regler ist eine Möglichkeit eine Anwendung so zu regeln dass sie reibungslos einem Wert oder einem Pfad folgt. In unserem Beispiel wollen wir dass unser autonomes Robot-Fahrzeug im gleichen Abstand einer Spur folgt.

class Robot(object):
  # some other functions ...
  def set(self, x, y, orientation):
     """
        Sets a robot coordinate.
     """
        self.x = x
        self.y = y
        self.orientation = orientation % (2.0 * np.pi)
  
  def move(self, steering, distance):
    """
        steering = front wheel steering angle, limited by max_steering_angle(max 90 degree)
        distance = total distance driven, most be non-negative
    """
    # some operation to calculate robot's next position and direction

Mit der Funktion "set" werden die Anfangskoordinaten und die Richtung des Fahrzeugs festgelegt. Dem Fahrzeug muss außerdem jedes Mal eine Anweisung zur Lenkung gegeben werden damit es sich dem Pfad nähern kann.

#### P
P steht für proportional. Unser Roboter-Auto befindet sich in einer 2-dimensionalen Welt mit den Koordinaten (x, y). Angenommen der Zielpfad ist y = 0, dann ist der aktuelle Fehler in Richtung Ziel y - 0 = y und basierend auf dem Fehler sollte die Lenkung des Autos entsprechend angepasst werden.

Wie kann die Lenkung so eingestellt werden dass der Fehler schnell verringert werden kann? Was würde ein Mensch tun wenn das gesteuerte Auto vom Zielkurs abweicht?

Eine einfache Möglichkeit besteht darin die Lenkung proportional zum Fehler anzupassen d. h. je weiter man vom Ziel entfernt ist desto größer ist der Winkel in dem man sein Auto lenkt. Mit diesem Gedanken im Hinterkopf ergibt sich folgende erste Lösung:

def run(robot, tau, n=100, speed=1.0):
    x_trajectory = []
    y_trajectory = []
    
    for _ in range(n):
        steering = -tau*robot.y
        robot.move(steering, speed)
        
        x_trajectory.append(robot.x)
        y_trajectory.append(robot.y)
    
    return x_trajectory, y_trajectory

In **n** Schritten wird das Fahrzeug eingestellt und in jedem Schritt lenkt das Auto proportional zum Fehler robot.y:

Fig. 2 - Lenkung proportional zum Fehler

wobei tau das Gewicht ist das dem Fehler zugewiesen wird. (Hinweis: Das Auto bewegt sich mit Geschwindigkeit = 1, und da der Zeitstempel 1 ist, ist die Entfernung = Geschwindigkeit * 1 = Geschwindigkeit)

Berechnung der Bewegungskurve des Roboter-Fahrzeugs:

# run - does a single control run
robot = Robot()
robot.set(0.0, 1.0, 0.0)

x_trajectory, y_trajectory = run(robot, 0.1)
n = len(x_trajectory)

fig, ax1 = plt.subplots(1, 1, figsize=(8, 4))
ax1.plot(x_trajectory, y_trajectory, 'g', label='P controller')
ax1.plot(x_trajectory, np.zeros(n), 'r', label='reference')
plt.legend()

Fig. 3 - P-Control

Das Roboter-Auto startet von (0, 1) aus in horizontaler Richtung. Die Bewegungskurve oszilliert auf dem Zielpfad. Es sieht so aus als ob unsere Lenkung zu drastisch ist so dass das Auto über das Ziel hinausschießt und wenn es versucht zurück zu lenken schießt es wieder über das Ziel hinaus. In einem solchen Auto zu sitzen würde sicherlich Kopfschmerzen bereiten. Kann der Lenkwinkel verringert werden sich das Fahrzeug dem Ziel näher?

#### PD

Die Frage führt uns zur PD-Steuerung wobei D für Derivat steht. Die Idee ist dass unser Auto bei abnehmendem Fehler gegenlenken sollte um ein Überschwingen zu vermeiden anstatt ständig nach unten zu lenken was zu einem Pendeln führt.

Der neue Term der in den Kontrollprozess aufgenommen werden soll ist δy/δt , wobei δt gleich 1 ist, also δy = current_y - last_y .

Die Umsetzung sieht wir folgt aus:

def run(robot, tau_p, tau_d, n=100, speed=1.0):
    x_trajectory = []
    y_trajectory = []
    
    prev_cte = robot.y
    for _ in range(n):
        dev = robot.y - prev_cte
        prev_cte = robot.y
        steer = -tau_p * robot.y - tau_d * dev
        robot.move(steer, speed)
        x_trajectory.append(robot.x)
        y_trajectory.append(robot.y)
        
    return x_trajectory, y_trajectory

Der abgeleitete Term ist hier dev = robot.y - prev_cte und tau_d ist das Gewicht davon.

Das Ergebnis der Bewegungskurve würde wie folgt aussehen:

Fig. 4 - Bewegungskurve nach PD Control

Jetzt sieht alles gut aus aber in Wirklichkeit ist dieses Modell noch nicht robust. Bedenken Sie zum Beispiel was wäre wenn es einen inhärenten Systemfehler gibt. Wenn das Rad zum Beispiel am Anfang eine kleine Panne hat und immer in eine andere Richtung fährt? Ist PD in der Lage dies zu beheben?

Die Antwort ist nein. Lassen Sie uns folgendes Beispiel betrachten:

robot = Robot()
robot.set(0, 1, 0)
robot.set_steering_drift(10/180.*np.pi)

def run(robot, tau_p, tau_d, n=100, speed=1.0):
    x_trajectory = []
    y_trajectory = []
    
    prev_cte = robot.y
    for _ in range(n):
        dev = robot.y - prev_cte
        prev_cte = robot.y
        steer = -tau_p * robot.y - tau_d * dev
        robot.move(steer, speed)
        x_trajectory.append(robot.x)
        y_trajectory.append(robot.y)
        
    return x_trajectory, y_trajectory

x_trajectory, y_trajectory = run(robot, 0.2, 1)
n = len(x_trajectory)

fig, ax1 = plt.subplots(1, 1, figsize=(8, 4))
ax1.plot(x_trajectory, y_trajectory, 'g', label='PD controller')
ax1.plot(x_trajectory, np.zeros(n), 'r', label='reference')
plt.legend()

Hier fügt set_steering_drift der Ausrichtung des Fahrzeugs ein gewisses Grund-Rauschen hinzu, das Ergebnis wird sein:

Fig. 5 - Bewegungskurve mit systemimmanenter Verzerrung

Es schwingt weit weg von unserem Zielpfad. Der Grund dafür ist dass unser Auto zwar auf das Ziel zusteuert aber die systemimmanente Verzerrung nicht ausgleichen kann.

#### PID

Um das Problem zu lösen braucht es den dritten Term I der für das Integral steht. Er hilft den Fehler zu überwachen und wenn der Fehler in einer bestimmten Zeit nicht reduziert wird sollte das Auto mehr lenken.

Das Integral in unserem Fall ist eigentlich die Summierung der Fehler über jeden Schritt und die Umsetzung könnte wie folgt aussehen:

def run(robot, tau_p, tau_d, tau_i, n=100, speed=1.0):
    x_trajectory = []
    y_trajectory = []
    
    prev_cte = robot.y
    sum_cte = 0
    for _ in range(n):
        sum_cte += robot.y
        dev = robot.y - prev_cte
        prev_cte = robot.y
        steer = -tau_p * robot.y - tau_d * dev - tau_i*sum_cte
        robot.move(steer, speed)
        x_trajectory.append(robot.x)
        y_trajectory.append(robot.y)
        
    return x_trajectory, y_trajectory

Bei jedem Schritt wird der aktuelle Fehler robot.y zu sum_cte addiert und tau_i ist das Gewicht dieses Terms. Durch die Hinzufügung dieses Terms wird vermieden dass das Fahrzeug irgendwo weit weg vom Ziel oszilliert da dieser Term immer größer wird und der Lenkwinkel entsprechend angepasst wird.

Dies ergibt das korrigierte Ergebnis:

Fig. 6 - Bewegungskurve mit PID

Mit einem gleichbleibenden Grundrauschen schafft es das Roboterauto auch diesmal das Ziel zu erreichen.

### Conclusion

Alles in allem ist der PID-Regler vor allem deshalb interessant weil er so intuitiv und einfach erklärt werden kann. Sie können sich die drei Komponenten P, I und D als drei verschiedene Personen vorstellen die für drei verschiedene Aspekte einer Aufgabe zuständig sind:

- P: die Hauptkraft die die Anwendung zum Zielzustand bringt
- I: überwacht den gesamten Weg und hilft aus dem Dilemma herauszukommen wenn es nicht weitergeht
- D: überwacht P und führt bei Bedarf Gegenmessungen durch

Lenkung und Fehlerbehebung bei diesem Vorgang wurden bearbeitet. Es folgen zwei weitere Szenarien und deren Lösung. Es ist bekannt dass eine wichtige Komponente beim Lenken die Berechnung des Lenkwinkels ist und das dieser Lenkwinkel des Fahrzeugs anhand der erkannten Fahrspurlinien berechnet wird. Dabei sollen 2 unterschiedliche Szenarien betrachtet werden:

1. Zwei Lane Lines wurden erkannt
2. Nur eine Lane Line wurde erkannt

#### 1. Zwei Lane Lines wurden erkannt

Angenommen unser Auto erkennt zwei Lane Lines, dann ist dies das einfachste Szenario da die Richtung des Kurses berechnet werden kann indem der Durchschnitt der Endpunkte der beiden Lane Lines gebildet wird. Die unten gezeigte rote Linie in Abbildung 1 ist der Steuerkurs. Es ist zu beachten dass das untere Ende der roten Kurslinie immer in der Mitte des unteren Teils des Bildschirms liegt da davon ausgegangen wird dass die Dashcam in der Mitte des Fahrzeugs installiert ist und geradeaus zeigt.

Fig. 7 Beide Lane Lines wurden erkannt

#### 2. Nur eine Lane Line wurde erkannt

Wenn nur eine Lane Line erkannt würde wäre das etwas schwierig da nicht mehr der Durchschnitt zweier Endpunkte gebildet werden kann. Wenn aber nur eine Spurlinie zu sehen ist z. B. nur die linke oder rechte Spur bedeutet dies dass stark nach rechts oder links gelenkt werden muss damit das Fahrzeug der Spur weiter folgen kann. Eine mögliche Lösung besteht darin die Kurslinie so einzustellen dass sie die gleiche Neigung wie die einzige erkannte Fahrspurlinie hat wie in Abbildung 2 gezeigt.

Fig. 8 - Nur eine Lane Line wurde erkannt

Wenn Sie bis jetzt gut aufgepasst haben haben Sie gesehen wie viele Vereinfachungen vorgenommen werden mussten um das Auto sicher zu steuern. Die Übungen haben gezeigt, dass das autonome Fahren ein komplexes Gebiet ist, das viele Komponenten einbeziehen und beachten muss. In weiteren Übungen könnte es weiterführend um die Erkennung von Passanten, das damit verbundene automatische Anhalten/Ausweichen gehen ebenso wie um die Erkennung von Verkehrsziechen. Sie haben gemerkt wie viele Anpassungen vorgenommen werden mussten um ein störungsfreies Ergebnis zu erzielen. Zum Beispiel keine steilen Kurven, getrennte Längs- und Quersteuerung, unvollkommenes Modell der Fahrzeugdynamik, einfache PID-Steuerung usw.. Es bedarf also viel Erfahrung und langer Forschung um auf unvorhergesehene und anspruchsvolle Streckenbedingungen sicher reagieren zu können und ohne menschliche Hilfe Entscheidungen fällen zu können. Die Automobilindustrie benötigt also dringend ausgebildetes Personal in der Automobilbranche um das autonome Fahren sicher zu gestalten und einen Mehrwert für Fahrer und Umwelt zu schaffen.

## Testen Sie sich selbst!

1. Wiederholung: Welche Möglichkeiten der Lane Detection können für das autonome Fahren angewendet werden?
*Computer Vision (Edge Detection) oder Deep Learning*
2. Welche Werte sind grundsätzlich für das Lenken des autonomen Autos zu beachten?
*Position des Autos, Winkel zur Fahrbahn, Geschwindigkeit*
3. Wie lässt sich die Problematik der Lenkung sinnvoll in 2 Teile gliedern?
*Längskontrolle (wie viel Gas und Bremse) und Querkontrolle (wie viel Drehmoment auf die Lenkräder)*
4. Wird diese Einteilung immer funktionieren? Wenn nein, welche Voraussetzungen müssen vorliegen?
*Diese Einteilung wird nicht immer funktionieren. Es muss eine möglichst gerade Strecke wie z.B. eine Autobahn oder Teststrecke auf der die Kurven sanft sind vorliegen.*
5. Nennen Sie eine einfache Steuerungsmethode, nach der sich dieser Kurs richtet.
*einfachste Steuerungsmethode ist der Proportional-Integral-Derivativ-Regler (PID)*
6. Beschreiben Sie in einem Satz, was der PID-Regler tut bzw. was der Vorteil der PID-Regelung ist.
*Der PID-Regler ist eine Möglichkeit eine Anwendung so zu regeln dass sie reibungslos einem Wert oder einem Pfad folgt.*
7. Sehen Sie sich die Abbildung 2 an. Sie zeigt die Implementierung eines PID-Reglers für ein selbstfahrendes Roboter-Auto. Was tut die Funktion "set"?
*Mit der Funktion “set” werden die Anfangskoordinaten und die Richtung des Fahrzeugs festgelegt.*
8. Sehen Sie sich die Bewegungskurve in Abb. 10 an. Was können Sie erkennen?
*Es schwingt weit weg von unserem Zielpfad. Der Grund dafür ist dass unser Auto zwar auf das Ziel zusteuert aber die systemimmanente Verzerrung nicht ausgleichen kann.*
9. Wie kann das Fahrverhalten aus Abb. 10 verbessert werden?
*Um das Problem zu lösen braucht es den dritten Term I der für das Integral steht. Er hilft den Fehler zu überwachen und wenn der Fehler in einer bestimmten Zeit nicht reduziert wird sollte das Auto mehr lenken.*
10. Implementieren Sie das Gelernte nun in Ihr autonomes selbstfahrendes Fahrzeug.
*Praktische Übung*
11. Practical Exercise : Implementieren Sie die erlernten Funktionen auf Ihr Fahrzeug.

## Further Read

Z. Chen and X. Huang, "End-to-end learning for lane keeping of self-driving cars," 2017 IEEE Intelligent Vehicles Symposium (IV), 2017, pp. 1856-1860, doi: 10.1109/IVS.2017.7995975.

G. Yin, Z. Wang and X. Jin, "Active steering of autonomous vehicle using model predictive control with Legendre function," 2016 Chinese Control and Decision Conference (CCDC), 2016, pp. 3277-3281, doi: 10.1109/CCDC.2016.7531548.

R. Guidolini, A. F. De Souza, F. Mutz and C. Badue, "Neural-based model predictive control for tackling steering delays of autonomous cars," 2017 International Joint Conference on Neural Networks (IJCNN), 2017, pp. 4324-4331, doi: 10.1109/IJCNN.2017.7966403.

D. Antonelli, L. Nesi, G. Pepe and A. Carcaterra, "A novel control strategy for autonomous cars," 2019 American Control Conference (ACC), 2019, pp. 711-716, doi: 10.23919/ACC.2019.8814944.

K. Jo, J. Kim, D. Kim, C. Jang and M. Sunwoo, "Development of Autonomous Car—Part II: A Case Study on the Implementation of an Autonomous Driving System Based on Distributed Architecture," in IEEE Transactions on Industrial Electronics, vol. 62, no. 8, pp. 5119-5132, Aug. 2015, doi: 10.1109/TIE.2015.2410258.