DE4124654A1 - Verfahren zur kontinuierlichen und automatischen fahrzeugorientierung auf einer fahrbahn - Google Patents

Description

Die Erfindung bezieht sich auf ein Verfahren zur kontinuierlichen und automatischen Fahrzeugorientierung auf einer Fahrbahn mit einem bilderzeugenden System und einer computerisierten Bildauswertung zur Erzeugung von Fahrzeugsteuerdaten.

Aufgrund einer wachsenden Verkehrsleistung ist eine immer stärkere Transportkapazitätsauslastung von Verkehrswegen unvermeidlich.

Derzeit stößt diese Erweiterung jedoch auf eine Begrenzung, die durch den Menschen wegen seiner psychischen und physischen Möglichkeiten als Fahrzeugführer vorgegeben ist.

Als Ausweg bietet sich der teilweise oder vollständige Ersatz des Fahrzeugführers durch eine automatische Fahrzeugführung an, die auch schon unter den gegenwärtigen Bedingungen zur Erhöhung der Verkehrssicherheit und Komfortsteigerung wünschenswert wäre.

Zur Realisierung einer automatischen Fahrzeugführung ist es notwendig, wenn sich der Ersatz des Menschen nicht nur auf eine bloße Geschwindigkeitsregelung beschränken soll, über ein System zu verfügen, welches die selbstständige Orientierung des Automaten auf der Fahrbahn ermöglicht.

Bisher zu diesem Zweck geschaffene Einrichtungen sind hierzu nur sehr eingeschränkt fähig.

So ist z. B. ein Automat zur Fahrzeuglenkung aus dem EUREKA-Projekt "Prometheus" bekannt.

Diese Einrichtung weist zur Orientierung und Lenkung des Fahrzeuges auf der Fahrbahn eine Kamera auf, die so ausgerichtet ist, daß sie die rechte Fahrbahnbegrenzung anvisiert.

Aus dem erzeugten Bild wird mittels eines computerisierten Auswerteverfahrens versucht, nach Farbkontrasten eine Leitspur zu ermitteln.

Die Spur dient dann als Bezug für die Fahrzeugsteuerung.

Abgesehen von der Möglichkeit, ein Kraftfahrzeug schienenfahrzeuggleich auf einer Straße führen zu können, muß als hauptsächlicher Nachteil dieser Vorgehensweise festgestellt werden, daß es so nicht möglich ist, sich am tatsächlichen Fahrbahnverlauf zu orientieren.

Eine vorausschauende Bewertung des weiteren Straßenverlaufs ist nicht möglich.

Des weiteren führen Kontraste, die einen Eindruck einer Spur verursachen, aber nicht in Fahrbahnrichtung verlaufen, bzw. etwaige Auslöschungen des vom System z. B. als Spur aufgefaßten Randstreifens durch Bremsspuren, Schlagschatten etc., zu nicht akzeptablen Funktionsausfällen.

Eine andere diskutierte Einrichtung zur automatischen Fahrzeuglenkung trägt die Bezeichnung "Train".

Zur Sicherstellung einer fortwährend zuverlässigen Lenkung ist es bei ihr vorgesehen, die Kamera und die zugehörige Bildauswertung gegen einen Stromabnehmer, der zugleich ein seitliches Führungssignal erzeugen kann, auszutauschen.

Dem Vorteil der exakten zuverlässigen Fahrzeuglenkung steht hier allerdings der erhebliche Nachteil gegenüber, nur auf Straßen zu funktionieren, die über eine entsprechende Ausrüstung verfügen.

Wegen der erwähnten Unzulänglichkeiten der bisher in Realisierung bzw. Diskussion befindlichen Systeme zur automatischen Fahrzeugführung stellt sich die Erfindung die Aufgabe, ein Verfahren zu schaffen, welches implementiert in eine Einrichtung zur automatischen Fahrzeugführung, diese in die Lage versetzt, monokulare Bildfolgen einer am Fahrzeug montierten Kamera modellgestützt zur räumlichen Fahrbahnverlaufserfassung zu interpretieren.

Gemäß der Erfindung wird diese Aufgabe dadurch gelöst, daß von einer Fernsehkamera, die möglichst hoch über der Fahrbahn am Fahrzeug aufgehängt ist, ständig digitalisierte Bildfolgen im Videotakt an ein Rechnersystem mit einem Programm zur speziellen verfahrensgemäßen Ausgestaltung der Signalverarbeitung und Interpretation im Fahrzeug übertragen werden.

Im Verfahren werden generische Straßenmodelle und einfache generische dynamische Modelle für die Fahrzeugbewegung ausgenutzt, um bisher enthaltene Ergebnisse der Fahrbahn- und Fahrzeugrelativlage - Erkennung zur Auswertung des nächsten Bildes verwenden zu können.

Hierzu werden drei Teilmodelle kombiniert.

Monokulare Bilddaten nur des letzten Bildes werden mit rekursiven Schätzverfahren zur Bestimmung der Straßenparameter und des eigenen Relativzustands ausgewertet.

Dadurch entsteht im Rechner eine räumliche Vorstellung des aktuellen Straßenverlaufs im Vorausschaubereich.

In vorteilhafter Weise gelingt dabei eine Umsetzung von Veränderungen im Bild in regelungsorientierte Zustandsgrößen innerhalb nur eines Verfahrenszyklus.

Im folgenden wird das Verfahren zur kontinuierlichen und automatischen Orientierung auf einer Fahrbahn näher erläutert in der folgenden Anlage.

1. Modellgestützte Bildfolgen-Interpretation zur räumlichen Fahrbahnverlaufserfassung 1.1 Die modellierten Fahrzeug- und Fahrbahn-Freiheitsgrade

Die Modellierung stellt das zur "höheren" Interpretation nötige, meist anwendungsspezifische, Vorwissen über eine Szene dar. Erst hiermit wird eine Umsetzung vom "anonymen" Bildsignal in problemorientierte Bedeutung möglich.

Der 4D-Ansatz zur Bildfolgeninterpretation nutzt die Modellierung räumlich- geometrischer und zeitlich-dynamischer Aspekte einer Szene und ihrer Veränderungen. Diese raum/zeitliche Modellvorstellung leistet in Kombination mit rekursiven Schätzverfahren in der hier beschriebenen Anwendung zur Straßenfahrzeugführung sowohl die Erkennung des fahrzeugeigenen Lateralzustandes relativ zur Fahrbahn, als auch die Erfassung des räumlichen Fahrbahnverlaufes aus der 2D-Bildfolge während des Entlangfahrens. Hierfür existieren zwei schwach gekoppelte dynamische Teilsysteme: das der Fahrzeug-Querdynamik sowie der Fahrbahnkrümmungsdynamik.

Die Struktur des Differentialgleichungssystems, das die Fahrzeugseitenbewegung bzw. die Fahrbahnkrümmungsdynamik beschreibt, die geometrischen Modell- und Abbildungsparameter, das eingesetzte Zustandsschätzverfahren (Kalman Filter) sowie die zugehörigen Kovarianzen der Meßfehler und Prozeßstörungen stellen "analytisches Wissen" [Isermann u. Nold, 88] über den optisch zu erfassenden (und in der Folge zu steuernden) Prozeß "Entlangfahren an einer Straße" dar.

Die wesentlichen modellierten - und damit erfaßbaren - Freiheitsgrade und ihre Auswirkung auf das Bild der Fahrbahn sollen im folgenden qualitativ veranschaulicht werden (Bild 1a bis e):

Es zeigen im einzelnen Bild:

• a) Einfluß einer Seitenablage von der Fahrbahnmitte nach rechts,
• b) Einfluß eines Gierwinkels zwischen Fahrzeuglängs- bzw. Kameraachse und Fahrbahnrichtung nach links,
• c) Einfluß von horizontaler Fahrbahnkrümmung (Kurven)
• d) e) Einfluß von vertikaler Fahrbahnkrümmung (Steigungsänderungen)

Bild 1: Mögliche Erscheinungsbilder der Fahrbahn. Der Referenzverlauf der Fahrbahnbegrenzungen für eine zentrierte, ausgerichtete Fahrzeugposition auf ebener und gerader Straße ist jeweils mit dünner Strichstärke eingezeichnet.

1.2 Geometrisch-räumliche Modellierung - das Fahrbahnmodell 1.2.1 Allgemeines zum Fahrbahnmodell

Der räumliche Fahrbahnverlauf (bzw. der einer Skelettlinie) wird im wesentlichen durch die geometrischen Parameter der horizontalen bzw. vertikalen Fahrbahnkrümmung (c_0h, c_0v) über der Lauflänge beschrieben (vgl. Bild 2). Die Fahrbahnbegrenzungen werden als parallel verlaufend mit Abstand b angenommen. Die Fahrbahnwölbung und -verwindung, d. h. Änderungen des Hängewinkels, innerhalb des Vorausschaubereichs seien vernachlässigbar klein.

Bild 2 gibt einen räumlichen Fahrbahnverlauf mit horizontaler und vertikaler Krümmung wieder.

Die horizontale Krümmung c_0h (Krümmungsradius R_h=1/c_0h) kann sich linear mit der Lauflänge 1 ändern (dc_0h/d1=c_1h=const.). Dies entspricht den grundlegenden Bauvorschriften und Auslegungskriterien für Straßen für höhere Fahrgeschwindigkeiten [RAS-L-1, 84]. Der Fahrbahnverlauf im Lageplan (d. h. in der Draufsicht) besteht somit aus Segmenten konstanter Krümmung, also Geraden und stationären Kurven, sowie aus Kurvenübergängen mit sich linear ändernder Krümmung (Klothoiden). Kuppen und Wannen im Höhenverlauf haben näherungsweise Kreisform (in Realität durch quadratische Parabeln approximiert).

Abgesehen von der unmittelbaren Bedeutung der horizontalen Fahrbahnkrümmung für die Seiten- und Längsführung des Fahrzeugs [Dickmanns u. Zapp, 86] läßt sich über die lokalen Krümmungsparameter der räumliche Straßenverlauf im Vorausschaubereich sehr effizient geometrisch in einem beobachterbezogenen, mitbewegten Koordinatensystem beschreiben. Darüber hinaus lassen sich die zeitlichen, über den Vorausschaubereich gemittelten Beziehungen zwischen c_0h, c_1h und _0h aufgrund des Entlangfahrens mit bekannter Eigengeschwindigkeit v als kompaktes System linearer Differentialgleichungen formulieren, wie von [Dickmanns, 88] vorgeschlagen. Dies gilt weitgehend analog auch für die Vertikalkrümmungsparameter c_0v, c_1v und _0v. D. h. es existiert ein dynamisches Modell der Fahrbahn-Krümmungsparameter, das in Verbindung mit der geometrischen Modellierung und den bekannten Abbildungsgesetzmäßigkeiten die Bestimmung dieser Größen aus der Bildfolge mittels rekursiver Schätzverfahren erlaubt (vgl. Abschnitt 1.3).

1.2.2 Das lineare Krümmungsmodell zur Beschreibung des Fahrbahnverlaufs

In der Linienführung von Straßen für mittlere und höhere Fahrgeschwindigkeiten wird u. a. die Fahrdynamik dadurch berücksichtigt, daß Krümmungsänderungen, und damit auf das Fahrzeug wirkende Querkraftänderungen, nicht sprunghaft, sondern stetig verlaufen. Dies wird durch Kurvenübergänge (Klothoiden) mit sich linear über der Lauflänge 1 ändernder Krümmung erreicht, d. h. es gilt

c (1)=c₀+c₁ · 1. (1)

Hierbei ist c=1/R (Kurvenradius R) die lokale Krümmung an der Lauflängenkoordinate 1 und c₀ der Anfangswert der Krümmung am Ort 1=0.

c₁=dc/d1 (2)

stellt die abschnittsweise konstante Krümmungsänderung über der Lauflänge dar, die mit dem sog. Klothoidenparameter A über c₁=1/A² zusammenhängt (vgl. [RAS-1-L, 84).

Der Fahrbahnverlauf im Lageplan kann somit durch die Horizontal-Krümmungsparameter als Abfolge von Geraden, Kurvenübergangssegmenten (Klothoiden) und Kreisbögen dargestellt werden:

Die Fahrbahnrichtungsänderung Δχ_c (Bild 3) ergibt sich in Anwendung der Definition für die Krümmung c=dχ_c=dχ/d1 in der Differentialgeometrie als Integral der Krümmung über der Lauflänge mit Gl. (1) zu

Die durch die Richtungsänderung über der Lauflänge auftretende laterale Ablage Δy_c von der ursprünglichen Tangente (Bild 3) wird durch einen weiteren Integrationsschritt bestimmt.

Bild 3 stellt die lokale Geometrie eines Bogensegments dar.

Für kleine Winkeländerungen Δχ«1 (mit sinΔχ und cosΔχ≃1 und Gl. (3)) lassen sich in einem kartesischen Koordinatensystem folgende Näherungsbeziehungen für die Längs- und Querablage eines Punktes an der Lauflängenkoordinate Δ1 auf der Klothoide angeben:

Abschätzung des Linearisierungsfehlers

Bei einer maximalen Vorausschauentfernung von Δ1=20 [m] und einer Krümmung von c₀=0,01 [1/m] (Kurvenradius 100 m) ergibt sich folgender Näherungswert für die Querablage:

Gegenüber dem mathematisch exakten Wert

beträgt der Fehler durch die Linearisierung also ca. 0,5%.

Entsprechend [RAS-1-L, 84] werden Steigungsänderungen der Fahrbahn durch im Vertikalprofil parabolische, näherungsweise kreisförmige Übergangssegmente (Wannen u. Kuppen) realisiert. Als Parameter der Fahrbahn- Vertikalgeometrie werden hier die über die Vorausschauweite gemittelte vertikale Fahrbahnkrümmung c_0vm, sowie - als Hilfsgröße zur dynamischen Modellierung (speziell beim Übergang von ebenen zu vertikal gekrümmten Fahrbahnbereichen, vgl. Kap. 3.2) - die mittlere Änderung der Vertikalkrümmung über der Lauflänge c_1vm=dc_vm/d1 angesetzt.

Analog zu Gl. (1) bis (4) gilt dann für die Fahrbahn-Höhenänderung ΔH_cv aufgrund vertikaler Fahrbahnkrümmung über der Lauflänge Δ1 (ausgehend von horizontaler Fahrbahn):

1.2.3. Die horizontale Fahrbahn- und Abbildungsgeometrie

Im Fahrbahnerfassungs-Modul gehen die räumliche Fahrbahngeometrie und die perspektivischen Abbildungsgesetzmäßigkeiten explizit in den Erkennungsprozeß mit ein.

Es zeigt:
Bild 4: Horizontale Fahrbahn- und Abbildungsgeometrie. Der Fahrzeugschwerpunkt ist mit SP gekennzeichnet, das Projektionszentrum der Kamera durch PZ.

Für die Größen in Bild 4 gilt folgende Indexkonvention:
V=Fahrzeug; K=Kamera, R=Fahrbahn, B=in Bildkoordinaten,
g=rel. zu geodätischem Koordinatensystem.

Die horizontale Bildkoordinate y_B eines Fahrbahnrand-Kantenelementes in der Vorausschauentfernung L vom Projektionszentrum (PZ) wird durch die Abbildungsparameter sowie folgende Größen der Kamera- und Fahrzeug- Relativlage bzw. der ebenen Fahrbahngeometrie bestimmt:

f - Brennweite [mm],
k_y - Kameraskalierfaktor (horizontal) [px1/mm],
L - Vorausschauentfernung [m],
d - Abstand des Projektionszentrums vom Fahrzeugschwerpunkt [m],
ψ_KV - Kamera-Gierwinkel relativ zur Fahrzeuglängsachse [rad],
y_V - Ablage des Fahrzeugschwerpunkts von der Fahrbahnmitte [m],
ψ_V - Gierwinkel zwischen Fahrzeuglängsachse und Fahrbahntangente [rad],
b - Fahrbahnbreite [m],
c_0hm - mittlere horizontale Fahrbahnkrümmung [1/m],
c_1hm - mittlere horizontale Fahrbahnkrümmungsänderung [1/m²].

Für alle Winkelbeziehungen der horizontalen Abbildungsgeometrie werden vereinfachend die Linearisierungen der trigonometrischen Grundfunktionen sin(ψ)≃ψ bzw. cos(ψ)≃1 benutzt. Für kleine Winkel |ψ|<10° beträgt hierbei der maximale Fehler ca. 1,5%. Wegen des kleinen Verhältnisses von Brennweite zu Vorausschauentfernung (f/L«1) bzw. des kleinen Abstandes zwischen Projektionszentrum und Kameradrehpunkt wird näherungsweise für die weiteren Herleitungen die Lage des Projektionszentrums, der Bildebene und des Kameradrehpunktes auf der optischen Achse als identisch angenommen.

Es ergeben sich somit in einer Vorausschauentfernung L von der Kamera-Bildebene folgende (laterale) Ablageanteile der Fahrbahnbegrenzungen von der Fahrspurmitte (vgl. Bild 4):

wobei +b/2 für den rechten und -b/2 für den linken Fahrbahnrand gilt, und

gemäß Gl. (1.4) im vorigen Abschnitt.

Entsprechend gilt für die Ablage der Fahrbahnbegrenzung von der Kamera- (Blickrichtungs)-Achse (vgl. Bild 4):

y_RK=y_Rg-(y_V+ + (8)

mit

=(L+d) · ψ_V (9)

und

=L · ψ_KV. (10)

Die Perspektivabbildung

ergibt mit =f · k_y für die horizontale Bildkoordinate (skaliert in Pixel-Einheiten [pxl]) eines Kantenelementes der Fahrbahnbegrenzungen in der Vorausschauentfernung L

Die entsprechenden Elemente der Jacobischen Matrix C der Abbildungsgleichungen für das ebene und räumliche Fahrbahnmodell sind im Anhang A2 angegeben. Im ebenen Fall ist die Vorausschauentfernung L nur von der Kameraneigung gegenüber der Fahrbahn sowie von einer gewählten Bild- Vertikalkoordinate (Bildzeile) abhängig. Weist die Fahrbahn auch vertikale Krümmung auf, so geht diese in die Berechnung der Vorausschauentfernung mit ein (s. folgender Abschnitt). Da im räumlichen Fall die Vertikalkrümmungsparameter des Zustandsvektors des dynamischen Fahrbahnkrümmungsmodells sind, werden dann die Abbildungsgleichungen vom momentanen Vertikalkrümmungszustand abhängig, und damit nichtlinear bezüglich des Zustandsvektors.

1.2.4 Die vertikale Fahrbahn- und Abbildungsgeometrie

Die vertikale Abbildungsgeometrie wird im wesentlichen von der vertikalen Kameraposition und -Nicklage relativ zur Fahrbahn sowie von einer evtl. vorhandenen vertikalen Fahrbahnkrümmung bestimmt (vgl. Bild 5). Es wird angenommen, daß die Fahrzeug-Längsachse immer parallel zur Fahrbahn- Tangentialebene am Ort des Fahrzeug-Schwerpunkts liegt, d. h. hochfrequente Nicklagestörungen kleiner Amplitude werden vernachlässigt. Dies hat sich für stationäre Fahrzustände auf "normalen", d. h. glatt asphaltierten Straßen als realistisch erwiesen. Allerdings gilt dies nicht für unebene Straßen oder bei starken Beschleunigungs- und Abbremsmanövern, die deutliche Nickbewegungen verursachen können. Um diese Fälle bei gleichzeitiger korrekter Erkennung der Fahrbahnvertikalkrümmung bzw. Fahrbahnbreite behandeln zu können, wäre eine direkte Erfassung oder Schätzung der Fahrzeug-Nicklageänderung erforderlich.

Verdeutlicht wird in Bild 5 die vertikale Fahrbahn- und Abbildungsgeometrie.

Die benutzten Größen der vertikalen Fahrbahn- und Abbildungsgeometrie sind im folgenden zusammengefaßt:

f - Brennweite [mm],
k_z - Kameraskalierfaktor (vertikal) [pxl/mm],
d - Abstand des Projektionszentrums vom Fahrzeugschwerpunkt [m],
H_K - Höhe der Kamera über der Fahrbahntangentialebene [m],
R_K - Kamera-Nickwinkel relativ zur Fahrzeuglängsachse [rad],
z_B - Bild-Vertikalkoordinate in Pixel-Einheiten [pxl],
L₀ - Vorausschauentfernung bei ebener Fahrbahn [m],
L_cv - Vorausschauentfernung bei vertikal gekrümmter Fahrbahn [m],
H_cv - Höhenänderung aufgrund vertikal gekrümmter Fahrbahn [m],
c_0vm - mittlere Vertikalkrümmung der Fahrbahn [1/m],
c_1vm - mittlere Vertikalkrümmungsänderung der Fahrbahn [1/m²].

Die zur Berechnung der Jacobischen Matrix C der Meßgleichungen (Gl. (12)) erforderliche Vorausschauentfernung L_i=L(z_Bi, H_K, R_K, f, k_z, c_0vm, c_1vm) wird bzgl. einer ausgewählten Bild-Vertikalkoordinate (Bildzeile) z_Bi wie folgt ermittelt:

Die Neigung des Sehstrahls durch z_Bi gegenüber der Fahrbahntangentialebene ergibt sich zu

Im Falle einer ebenen Fahrbahn berechnet sich die der Bildvertikalkoordinate z_Bi entsprechende Vorausschauentfernung zu

Gemäß Gl. (6) gilt für die Fahrbahnhöhenänderung durch eine mittlere Vertikalkrümmung bzw. Krümmungsänderung über der Lauflänge L_cv+d (d. h. relativ zur lokalen Fahrbahntangentialebene am Ort des FZG-Schwerpunktes)

Mit Gl. (13) läßt sich die Fahrbahnhöhenänderung innerhalb der Vorausschau abhängig von z_Bi ausdrücken als

H_cv=H_K-L_cv · (16)

Aus (16) und (15) folgt für L_cv ein Polynom 3. Ordnung

mit den Koeffizienten

das über die Newton-Iteration [Heinhold u. Behringer, 76] numerisch gelöst wird. Wenn als Iterations-Startwert für L_cv die Lösung des jeweils vorhergehenden Abtastintervalls (oder auch der Wert für den ebenen Fall gemäß Gl. (14)) eingesetzt wird, konvergiert die Iteration typischerweise in 2-3 Schritten, d. h. der numerische Rechenaufwand ist relativ gering.

Durch Vernachlässigen des c_1vm-Einflusses auf die Vorausschauweite läßt sich das Polynom Gl. (17) auf 2. Ordnung vereinfachen und ist damit direkt analytisch lösbar (vgl. Gl. (19) unten). Der c_0vm-Schätzwert folgt einem Sprung im realen Vertikalkrümmungsverlauf wegen der Mittelung über die Vorausschauweite jedoch nur leicht verzögert ("Verschleifungseffekt"). Ein Vernachlässigen des c_1vm-Einflusses bewirkte beim Einfahren in vertikal gekrümmte Fahrbahnbereiche Fehler in der Vorausschauentfernung und entsprechende Differenzen in der erwarteten Breite des Fahrbahnbildes. Das seitliche Nachführverhalten der Auswertefenster auf die mit einer Vertikalkrümmungsänderung verbundene Breitenänderung des Fahrbahnbildes konnte durch explizite Berücksichtigung des c_1vm- Einflusses deutlich verbessert werden.

Ab einer gewissen negativen Vertikalkrümmung der Fahrbahn kann der Fall eintreten, daß das Fahrbahnbild unterhalb der gewählten Bildzeile z_Bi liegt, also auf der Höhe von z_Bi keine Kantenelemente als Merkmale extrahierbar sind. Der Krümmungswert für diesen Grenzfall (bei dem der Sehstrahl durch z_Bi die Tangente an die nach unten gekrümmte Fahrbahn in der Entfernung L_cv bildet) läßt sich näherungsweise, unter Vernachlässigung des c_1vm-Einflusses, aus einem Polynom 2. Ordnung für L_cv abschätzen. Aus

und Gl. (16) ergibt sich die vereinfachte, quadratische Gleichung

für L_cv mit den Lösungen

Der Grenzfall für die maximale negative Krümmung ("Tangentenfall") abhängig von z_Bi wird dadurch bestimmt, daß der Ausdruck unter der Wurzel 0 sein muß für reelle Lösung von L_cv, also

gilt. Dies ist erfüllt, solange die Vertikalkrümmung größer als der Grenzwert

ist, wobei wegen der Vernachlässigung von d und c_1vm noch ein "Sicherheits-Abstand" Δc_0res=0,0005 gegenüber dem "Tangentenfall"-Grenzwert aus dem quadratischen Ansatz eingeführt wurde.

Falls Gl. (22) nicht mehr erfüllt ist, entfällt die Berechnung der z_Bi entsprechenden Vorausschauentfernung, da die Nullstelleniteration für Gl. (17) keine (hier) sinnvolle Lösung ergeben kann. Da zudem in der Bildzeile z_Bi ein Fahrbahn-Kantenelement nicht mehr zuverlässig extrahierbar ist, wird bei der sequentiellen Zustands-Innovation des Kalman Filters der entsprechende Meßwert ausgelassen (vgl. Abschnitt 1.4.3.1).

Als zusätzliche Beschränkung sind die Vertikalkrümmungsparameter auf die bei einspurigen Straßen im freien Gelände als maximal angenommenen Wertebereiche von -0,003c_0vm0,005 bzw. -0,00015c_1vm0,00015 begrenzt (d. h. auf Krümmungsradien zwischen 330 m negativ und 200 m positiv, bei einer maximalen vertikalen Krümmungsänderung von ebener Straße auf 300 m Radius innerhalb 20 m Vorausschau).

1.3 Dynamische Modellierung 1.3.1 Die horizontale Fahrbahnkrümmungsdynamik

In Bild 6 sind für eine S-Kurve im Fahrbahnverlauf qualitativ die horizontale Krümmung bzw. Krümmungsänderung über der Lauflänge 1 (entsprechend dem linearen Krümmungsmodell, vgl. Abschnitt 1.2.2) sowie deren zeitliche Änderung aufgrund des Entlangfahrens mit einer konstanten Geschwindigkeit dargestellt. Für die Zeitableitung der Krümmung c_0h gilt:

Zur besseren Unterscheidung von der Ziffer 1 ("Eins") wird die Lauflängenkoordinate 1 immer mit Unterstrich geschrieben.

Es zeigt Bild 6 den Fahrbahnverlauf und horizontale Krümmungsgrößen.

Die Unstetigkeit des c_1h-Verlaufs über der Lauflänge bewirkt theoretisch Dirac-Impulse in der Zeitableitung _1h, die real bzw. als Differential- Beziehungen nicht modellierbar sind. Dies läßt sich jedoch umgehen, wenn hierfür näherungsweise über den lokalen Vorausschaubereich L gemittelte Größen c_0hm und c_1hm eingeführt werden, die in ihrem "Krümmungs-Effekt" über L dem der theoretischen Größen c_0h und c_1h über 1 _c äquivalent sind, d. h. die gleiche Krümmungsablage Δy_c erzeugen (vgl. Bild 7). Dieser Ansatz wurde von [Dickmanns, 88] vorgeschlagen. Es soll also gelten:

Δy_cm(c_0hm, c_1hm, L)=Δy_c(c_0h, c_1h, 1 _c). (24)

Zur Verdeutlichung
Bild 7: Die durch die gemittelten Krümmungsparameter (c_0hm, c_1hm) über die Vorausschauentfernung L bewirkte Ablage Δy_cm ist gleich der über die "Eindringtiefe" 1 _c in den c_1h- Stufenbereich auftretenden Querablage Δy_c.

Die Beiträge der gemittelten Größen zur Querablage

sind

und

Für Δy_c(1 _c) gilt im Bereich 01 _cL

Gleichsetzen von Gl. (25) mit Gl. (28) und Differenzieren nach 1 _c ergibt für c_1hm folgende Differentialgleichung:

Die Substitution =c′_1hm und eine Normierung auf L mit ξ= (d. h. für die normierte Eindringtiefe gilt 0ξ1) ergibt

c′_1hm(ξ)=3ξ²c_1h-3c_1hm(ξ). (30)

Z. B. via Laplace-Transformation ist folgende Lösung herleitbar:

Am Ort des c_1h-Sprungs (d. h. an ξ=1 bzw. 1 _c=L) mit dem Anfangswert c_1hm(0)=0 ergibt sich als Amplitude des gemittelten c_1h-Verlaufes

Ab ξ1 liegt der Vorausschaubereich L voll im c_1h-Stufenbereich. Es wird eine neue, um L gegenüber 1 _c (bzw. normiert um 1 gegenüber ξ) nach rechts verschobene Laufkoordinate ξ′=ξ-1 eingeführt.

Gl. (24) gilt nach wie vor, jedoch ist nun im Vorausschaubereich der Krümmungs-Anfangswert c_0h(ξ′)≠0. Damit ergibt sich für die Querablage des Straßenrandes von der Tangente infolge der Krümmung

bzw. nach Normieren auf L

Gl. (34) differenziert nach ξ′ ergibt, mit dc_0h/dξ′=c_1h

bzw. mit der Substitution c′_1hm=

c′_1hm(1)=3c_1h-3c_1hm(ξ′) (für ξ1, d. h. ξ′0). (36)

Obige Differentialgleichung hat die Lösung

c_1hm(ξ′)=c_1hm(ξ′=0) · e^{-3 ξ ′}+c_1h(1-e^{-3 ξ ′}). (37)

Wegen des Anfangswertes am Ort der Stufe c_1hm(ξ′=0)=c_1h · 0,5445 (vgl. Gl. (32)) kann die Lösung auch als

c_1hm(ξ′)=c_1h(1-0,4555 · e^{-3 ξ ′})

geschrieben werden.

Statt eines Dirac-Pulses im Fall der sprungförmigen Krümmungsänderung gilt nun für die Zeitableitung der gemittelten Krümmungsänderung

Mit Gl. (30) und (38) gilt für die Zeitableitung der gemittelten Krümmungsänderung c_1hm im Intervall 0ξ1, d. h. für 01 _cL:

Aus Gl. (36) bis (38) folgt für die Zeitableitung der gemittelten Krümmungsänderung c_1hm im Intervall ab ξ1 bzw. ξ′0, d. h. nach vollständigem Eindringen der Vorausschau in den c_1h-Stufenbereich:

Da der Ort einer Krümmungsänderung (d. h. c_1h-Stufe) in der Vorausschau, und damit die eventuelle "Eindringtiefe" 1 _c, jedoch nicht im voraus bekannt ist und nur sehr unzuverlässig aus dem geschätzten c_1h-Verlauf bei realen, verrauschten Meßwerten ermittelt werden kann, wird näherungsweise die c_1hm-Dynamik immer nach Gl. (40) modelliert. In der experimentellen Erprobung bzw. im Vergleich zu bekannten Referenzverläufen aus der Simulation lieferte die gemittelte Krümmungsdynamik sehr gute Schätzergebnisse (vgl. Schätzwertverläufe in Abschnitt 6).

Hiermit existieren kompakte, sehr einfache Differentialbeziehungen zwischen den für die optische Fahrbahnverfolgung und Fahrzeugquerführung gleichermaßen relevanten gemittelten Horizontalkrümmungsparametern sowie dem theoretischen (unstetigen) c_1h-Verlauf. Für die Zustandsschätzung wird die zeitliche Änderung von c_1h (theoretisch aus einer Folge von Dirac-Impulsen bestehend, deren Zeitpunkte unbekannt sind) als mittelwertfreier, weißer, gaußverteilter Rauschprozeß angesetzt. Der c_1h-Schätzwert wird somit im Kalman Filter ausschließlich über die Innovation durch die Meßwerte aus der Bildfolge "angetrieben".

Das Differentialgleichungssystem der horizontalen Fahrbahnkrümmungsdynamik lautet dann zusammengefaßt:

bzw. in Matrix-Vektor-Notation

1.3.2 Die vertikale Fahrbahnkrümmungsdynamik

Wie schon in Abschnitt 1.2.2 bzw. 1.2.4 erläutert, wird der lokale Vertikalverlauf der Fahrbahn über der Lauflänge 1 durch die beiden Parameter der mittleren Vertikalkrümmung c_0vm bzw. der mittleren Vertikalkrümmungsänderung c_1vm im Vorausschaubereich bis L dargestellt.

Analog zu Gl. (23) im vorigen Abschnitt gilt für die Zeitableitung der gemittelten Vertikalkrümmung

Die Zeitableitung der Krümmungsänderung _1v, die bei Einfahren in ein im Höhenprofil kreisförmiges Fahrbahnsegment (Kuppe, Wanne) wegen des Krümmungssprunges theoretisch einen Dirac-Impuls aufweist, wird ebenso wie _1h im vorigen Abschnitt als mittelwertfreier, weißer, gaußverteilter Rauschprozeß angesetzt.

Das kontinuierliche Teilsystem der gemittelten Vertikalkrümmungsdynamik lautet somit:

_0vm=c_1vm · v

_1vm= (t)

bzw. in Matrix-Vektor-Notation

1.3.3 Die Fahrzeug-Querdynamik

Im 4D-Ansatz wird u. a. die Kenntnis der aktuellen Steuergrößen und deren Auswirkung auf die Eigenbewegung des Fahrzeuges (bzw. der Kamera) relativ zur Umgebung dazu genutzt, Veränderungen des Bildes im nächsten Verarbeitungszyklus vorauszuberechnen, also zu "erwarten". Für die Fahrzeugspurführung durch Rechnersehen sind neben der Krümmung die laterale Fahrzeugposition und Winkellage relativ zu den Fahrbahnbegrenzungen wesentlich. Innerhalb des Fahrbahnerfassungsmoduls muß also die Seitenbewegung des Fahrzeugs modelliert sein. Eine gegenüber dem Modell von [Zapp, 88] vereinfachte Modellierung der Fahrzeugquerdynamik als System 3. Ordnung mit einem zusätzlichen Integrator als Lenkstellglied erwies sich für die optische Fahrbahn- und Relativlageerfassung als völlig ausreichend.

Bild 8 zeigt das Ersatzmodell der Fahrzeugseitenbewegung.
Modellparameter der Fahrzeugquerdynamik sind

a_Achs - Achsabstand (3,5 m),
k_r - Reifen-Seitenkraftbeiwert (150 kN/rad).
m - Fahrzeugmasse (4000 kg),
V - Fahrgeschwindigkeit.

In Klammern sind jeweils die Werte für das Versuchsfahrzeug "VaMoRs" L508D angegeben.

Die Zustandsgrößen der Seitenbewegung sind

λ - Lenkwinkel [rad],
β - Schwimmwinkel [rad],
y_V - Seitenablage von der Fahrbahnmitte [m],
ψ_V - Gierwinkel relativ zur Fahrbahntangente [rad].

Als abgeleitete Größe, die nicht explizit im Modell auftritt, sei noch der Bahnwinkel χ_V relativ zur Fahrbahntangente aufgeführt.

Das benutzte Fahrzeugquerdynamikmodell (vgl. Bild 8) entspricht in seiner Struktur weitgehend dem linearen ebenen Einspurmodell nach [Mitschke, 72; Donges, 78], das von [Zapp, 88] für den Fahrzeug-Querreglerentwurf benutzt wurde, jedoch mit folgenden zusätzlichen Vereinfachungen:

• 1. Der Fahrzeug-Schwerpunkt wird (symmetrisch) in der Mitte zwischen Vorder- und Hinterachse angenommen. Dadurch entfällt eine Unterscheidung zwischen der Hebelarmlänge vor und hinter dem Schwerpunkt. Es bleibt nur mehr ein Geometrieparameter a_Achs.
• 2. Die auf die Reifen wirkenden Umfangskräfte werden vernachlässigt. Die Seitenkraftbeiwerte für die Vorder- und Hinterreifen werden gleich groß angesetzt und für beide Reifen einer Achse im Parameter k_r zusammengefaßt.
• 3. Der Einfluß des Lenkwinkeleinschlags λ auf die Gierrate _V des Fahrzeugs ist gegenüber dem Modell nach Donges auf eine kinematische Beziehung vereinfacht. Unter der Annahme einer symmetrischen Schwerpunktlage entfällt zum einen der Einfluß des Schwimmwinkels β auf die Gierwinkelgeschwindigkeit. Zusätzlich wird eine kleine geschwindigkeitsabhängige Zeitkonstante von 0,1 bis 0,2 s (bei den in [Zapp, 88] benutzten Fahrzeugparameterwerten) zwischen λ und _V vernachlässigt, die in der ursprünglichen Differentialgleichung modelliert ist.

Bezüglich des Ansatzes der ursprünglichen Seitenbewegungsgleichungen aus dem Querkräftegleichgewicht bzw. Momentengleichgewicht um die Fahrzeughochachse wird auf [Donges, 78] und [Zapp, 88] verwiesen. Im folgenden werden die Bewegungsgleichungen für das vereinfachte, ebene Einspurmodell 3. Ordnung mit einem Integrator als Stellglied angegeben.

Die Lenkantriebsdynamik lautet also

(t)=k_λ · u-_λ(t). (44)

Mit den oben aufgeführten Vereinfachungen des Modells nach Donges lautet die Differentialgleichung für den Schwimmwinkel

Für die Gierwinkelrate abhängig von Lenkwinkel und Fahrgeschwindigkeit kann eine kinematische Näherung angewandt werden: Die Querablage der Vorderachse durch ein mit Lenkeinschlag λ gefahrenes Weginkrement d1 ergibt sich für kleine λ (vgl. Bild 9) zu

dy=d1 · sinλ≃d1 · λ. (46)

Näherungsweise kann diese Querablage auch über

dy≃a_Achs · dψ_Vg (47)

ausgedrückt werden.

Bild 9 veranschaulicht die Lenkkinematik.

Durch Gleichsetzen von Gl. (46) mit (47) und Differenzieren nach der Zeit folgt

bzw.

für die inertiale Gierrate des Fahrzeugs aufgrund eines Lenkeinschlags λ bei der Fahrgeschwindigkeit v. Die (zeitliche) Richtungsänderung der Fahrbahntangente aufgrund des Einfahrens in eine Kurve mit der Krümmung c_0h=dχ_R/d1 ergibt sich zu

bzw.

_R=c_0h · v. -(49)

Für die relative Gierbewegung zwischen Fahrzeug und Fahrbahn folgt dann aus der Differenz von Gl. (48) und Gl. (49)

Der Bahnwinkel χ_V des Fahrzeugs relativ zur lokalen Fahrbahntangente am Ort des Fahrzeug-Schwerpunktes (vgl. Bild 6) ergibt sich zu

χ_V=ψ_V+β, (51)

so daß für die Bewegungskomponente quer zur Fahrbahnrichtung

_V=v · sinχ_V-≃v · χ_V (52)

gilt.

Das kontinuierliche, lineare und geschwindigkeitsabhängige Zustands­ modell 4. Ordnung der Fahrzeugquerdynamik lautet somit zusammengefaßt

bzw. in Matrix-Vektor-Notation

Hierbei bedeuten

mit den Elementen

1.3.4 Das kontinuierliche Gesamtsystem

Die Systemmatrix des vollständigen, kontinuierlichen dynamischen Modells des Fahrbahnerfassungsmoduls setzt sich aus den Teilsystemen der Fahr­ zeugquerdynamik A_V, der Horizontalkrümmungsdynamik A_ch, sowie der Verti­ kalkrümmungsdynamik A_cv zusammen. Als Alternative zur dynamischen Model­ lierung der vertikalen Fahrbahnkrümmung wurde auch die Schätzung der Fahrbahnbreite als stationärer (aber verrauschter, 'unsicherer') Para­ meter realisiert. Hierfür müssen die letzten beiden Spalten bzw. die untersten Zeilen der Gesamtsystemmatrix (Gl. (54)) durch eine Null­ spalte bzw. Nullzeile ersetzt werden.

In Matrix-Vektor-Notation lautet das Gesamtsystem somit:

=Ax+bu_λ+n

bzw. ausgeschrieben

wobei a_F, b_F und c_F im vorigen Abschnitt angegeben sind. Die Elemente der A und b entsprechenden zeitdiskreten Transitionsmatrix Φ bzw. des diskreten Eingangsverstärkungsvektors g sind im Anhang A3 angegeben.

1.4 Rekursive Zustandsschätzung als Interpretationsmechanismus 1.4.1 Zentrale Funktionen der Zustandsschätzung

Die Zustandsschätzung erfolgt im 4D-Ansatz zur Bildfolgenauswertung über ein instationäres Kalman Filter. Es erfüllt zwei zentrale Funktionen im visuellen Interpretationsprozeß:

• a) Es stellt im Sinne einer abstrahierenden und datenreduzierenden Ver­ arbeitungskomponente eine direkte Transformation von Merkmalskoor­ dinaten in der Bildebene in problembezogene, d. h. hier regelungs­ orientierte, physikalische Größen (z. B. Fahrzeugrelativposition und Fahrbahnverlauf) in Raum und Zeit dar. Diese Transformation beinhal­ tet wiederum zwei wesentliche Komponenten:
  • i) die Inversion der perspektivischen Abbildung durch die Anpassung der Beweghungsmodelle im Sinne kleinster Fehlerquadrate der Vor­ hersagefehler.
  • ii) die Glättung verrauschter Prozeß- und Meßdaten.
    Beide Aspekte werden in einem einzigen Schritt und damit sehr effi­ zient abgearbeitet.
• b) Gleichzeitig bildet die Zustandsschätzung im Sinne einer in der Ver­ arbeitungshierachie nach unten gerichteten Verarbeitungskomponente die Kontroll- und Steuerungsinstanz für die lokale, verteilte Merk­ malsextraktion.

Das Grundprinzip der rekursiven Zustandsschätzung zur robusten Bildfol­ geninterpretation soll im folgenden kurz skizziert werden (eine ausführ­ liche Darstellung der angewandten Verfahren erfolgt in den anschließen­ den Abschnitten):

Mittels der bekannten, modellierten Fahrzeug- und Fahrbahnparameter- Dynamik (letztere infolge der Eigengeschwindigkeit v des Fahrzeugs auf der Fahrbahn, beide zusammengefaßt in Form der Transitionsmatrix Φ(v), Gln. (54) bzw. (55) unten) kann ausgehend von einem Schätzwert für den aktuellen Systemzustand () der zukünftige Systemzustand ( ) zum nächsten Verarbeitungszyklus vorausberechnet werden. Die auf das Fahr­ zeug wirkenden Stellgrößen (u) und die Fahrgeschwindigkeit (v) gehen in die zeitliche Zustands-Extrapolation explizit mit ein, zufällige auf das System wirkende Störungen jedoch nicht. Über die nichtlinearen Abbil­ dungsgesetzmäßigkeiten (Gln. (12) bzw. (56) unten) ist damit auch das 'erwartete' Bild der Fahrbahn berechenbar, in Form eines Vektors neuer Merkmalskoordinaten in der (2D)-Bildebene. Diese Information kann direkt zur Steuerung der Merkmalsextraktion verwendet werden, nämlich wo im Bild (=Fensterposition) und in welchem Sektor von Vorzugsorientie­ rungen (=Operator-Richtungsauswahl) Kantenkandidaten zu extrahieren sind.

Die Vorhersagefehler (y), d. h. die Abweichungen zwischen erwarteten und tatsächlichen Merkmalpositionen im Bild, führen über eine Gewich­ tungsmatrix K zu einer Korrektur des erwarteten Zustands (Gl. (58)). Dieser als Innovation bezeichnete Schritt bringt durch die Meßwerte neue Information über den beobachteten Prozeß in die Schätzung ein. Die räumlich-zeitliche Modellvorstellung in Form des Zustandsvektors wird somit den 2D-Szenenveränderungen im Bild nachgeführt.

Aus einem redundanten und mehrdeutigen Satz von Kantenelementen werden nur die selektiert und gehen in die Zustands-Innovation ein, die den erwarteten Koordinaten am nächsten liegen (Gl. (57)). Der Selektions­ schritt wird durch die in der Schätzfehler-Kovarianzmatrix P enthaltene Information unterstützt, die der 'Unsicherheit' der aktuellen Schätz­ werte entspricht: Die Abbildung der um einen Zeitschritt extrapolierten Schätzfehlerkovarianz in den Meßwertraum liefert die sogenannte 'innova­ tions variance' [Bierman, 77]. Diese stellt ein Maß für den zulässigen Toleranzbereich dar, in dem neu extrahierte Kantenelemente um die erwarteten Koordinaten streuen dürfen.

Bei sequentieller, skalarer Verarbeitung des Meßwertvektors können so 'Ausreißer' eliminiert werden. D. h. mit der Modellvorstellung inkon­ sistente Kantenkandidaten werden schon vor der Innovation aussortiert. Die Selektionsfähigkeit trägt dadurch wesentlich zur Schätzqualität, ganz besonders aber zur Robustheit des Ansatzes gegenüber Störungen bei. Auch stellt die Anzahl der verworfenen Merkmale unmittelbar ein Bewer­ tungskriterium für die aktuelle Übereinstimmung von Modell und Realität dar.

Die Abbildungsgleichungen (Gl. (56)) werden hierbei nur vorwärts aus­ gewertet, d. h. vom Zustandsraum (als 'Interpretations-Welt') in den Meßraum (Bildebene) abbildend und damit in der gleichen Richtung wie der parallel dazu stattfindende 'echte', physikalische Abbildungsvorgang über den Sensor. Die nicht-eindeutige inverse Perspektivtransformation von der Bildebene zurück in räumliche Koordinaten wird umgangen. Sie erfolgt implizit bei der Nachführung der Modellvorstellung auf die Sze­ nenveränderungen durch Minimierung der Summe der Quadrate der Vorher­ sagefehler.

Der Kalman Filter Algorithmus wurde in einer optimierten, numerisch effizienten und stabilen Form nach [Bierman, 77] implementiert. Hierbei ist die Schätzfehler-Kovarianzmatrix faktorisiert in P=U · D · U^T, wobei U eine obere Einheits-Dreiecksmatrix und D eine Diagonalmatrix darstellt. Die Anwendbarkeit und hohe Effizienz dieses Ansatzes für die visuelle Bewegungserfassung und -Steuerung in der Robotik wurde erstmals von [Wünsche, 86] demonstriert. Der vollständige Selektions-, Innovations- und Prädiktionszyklus für die Kovarianz und den Zustand des dynamischen Modells im Fahrbahnerfassungs-Modul, bei Verarbeitung von acht Kanten­ elementen der Fahrbahnbegrenzungen, wird z. B. in etwa 40 ms Rechenzeit auf einem Objektprozessor (20 MHz 80 386/87) des Multiprozessorsystems ausgeführt.

Die grundlegenden Schritte des Zustands-Schätzzyklus bei der Bildfolgen­ auswertung im '4D-Ansatz' sind:

_k=Φ_k-1(v) · _k-1-+b_k-1(v) · u_k-1 (Prädiktion) (55)

_k=g( _k, p) (nichtlineare Perspektivabbildung vorwärts, p enthält Abbildungsparameter) (56)

y_k={⟨y_ki| |y_ki- _ki|<ε(P)} (Selektion) (57)

_k= t- _k) (Innovation) (58)

wobei der Vektor der extrapolierten Zustände und die vom Filter geschätzten Zustände sind; y beinhaltet die Positionen der für die Inno­ vation selektierten Merkmale (Kantenelemente). Die Berechnung der Fil­ terverstärkungsmatrix K sowie der Schätzfehler-Kovarianzen P im UD-fak­ torisierten Verfahren nach Bierman wird in Abschnitt 1.4.3 eingehender erläutert.

Zusammengefaßt bietet der räumlich-zeitlich modellgestützte, rekursive Ansatz zur Bildfolgenauswertung folgende Vorteile:

• 1) Nur das jeweils letzte Bild der Bildfolge geht in die Verarbeitung ein. Ein Abspeichern von Bildsequenzen, und eine entsprechend zeit­ lich verzögerte Rückwärtsauswertung, entfällt.
• 2) Die Prädiktionsfähigkeit über das dynamische Modell erlaubt 'Erwar­ tungen' der Szenenveränderungen zur Kontrolle und Steuerung der Merk­ malsextraktion stüttzend miteinzubeziehen. Dies trägt besonders bei hohen Störanteilen oder lokalen Mehrdeutigkeiten im Bild wesentlich zur Robustheit des Ansatzes bei.
• 3) Die Anzahl der Meßwerte (verwertete Merkmale) pro Verarbeitungszyklus darf variieren, zeitweise kann sogar totales Ausbleiben von neuer Meßinformation über die dynamischen Modelle kompensiert werden. Dies ist in der Bildfolgenauswertung ebenfalls nutzbar bei schlechter Szenenqualität bzw. sich ändernden Sichtbarkeitsverhältnissen, z. B. bei Verdeckungen.
• 4) Meßunsicherheiten und Systemstörungen können im Kalman Filter expli­ zit berücksichtigt werden. Darüber hinaus lassen sich daraus auch Bewertungsmaße für die aktuelle Schätzqualität bestimmen.
• 5) Der Zustandsvektor stellt eine extrem kompakte Datenstruktur zur Beschreibung des momentanen Systemzustandes dar. Er eignet sich dadurch sehr gut zu effizientem Informationsaustausch zwischen Teilmodulen bzw. zu höheren Ebenen der Bildauswertung.
• 6) Das Verfahren ist in Kombination mit parallelen, lokalen Merkmalsex­ traktionsverfahren rechentechnisch sehr effizient. Verarbeitungsraten von 10 bis 25 Hz sind für Modelle achter bis zehnter Ordnung mit der derzeitigen Rechenleistung eines einzigen 32-bit Standard-Mikropro­ zessors ohne weiteres erreichbar.

1.4.2 Allgemeines zum Kalman Filter

Das Kalman Filter nach [Kalman, 60] ist ein zeitdiskreter, optimaler Schätzer für den Zustand eines dynamischen Systems, auf das Steuerungen und stochastische Störungen einwirken. Es stellt einen rekursiven, li­ nearen Schätzalgorithmus nach der Minimierung der Summe der gewichteten Fehlerquadrate dar, um den unbekannten Zustand eines dynamischen Systems aus verrauschten, zeitdiskreten Meßwerten bestmöglich zu bestimmen. Die statistischen Kenngrößen der Störgrößen und Meßfehler werden dafür als bekannt vorausgesetzt. Die Struktur des Kalman Filters findet sich auch im Luenberger-Beobachter wieder [Brammer u. Siffling, 75]. Der Unter­ schied besteht darin, daß die Rückführgewichtungen der Vorhersagefehler bei letzterem nicht nach stochastischen, sondern nach rein deterministi­ schen Kriterien (vorgegebene Beobachterdynamik) abgeleitet werden.

Gegeben sei ein zeitdiskretes dynamisches System

x_k+1=Φ_kx_k+b_ku_k+w_k (59)

auf das die Prozeßstörungen w_k einwirken und von dem die mit Meßfehlern v_k behafteten Messungen

y_k=C_kx_k+v_k (für lineare Meßgleichungen) (60)

bzw.

y_k=g(x_k, p)+v_k (für nichtlineare Meßgleichungen) (61)

zur Verfügung stehen. Die Störungen v_k und w_k werden als gaußverteilt, zeitlich unkorreliert (weiß) und mittelwertfrei angenommen, d. h. mit Erwartungswerten E{w}=0 und E{v}=0, und den Kovarianzmatrizen R=E{ww^T} und Q=E{vv^T}. Die Systemstörungen und Meßfehler sind zudem untereinander nicht korreliert.

Für den Schätzfehler =-x, wobei x den nicht zugänglichen 'wahren' Systemzustand und den Schätzwert darstellt, gelte E()=0 bzw. für dessen Kovarianzmatrix P=E{^T}.

Die konventionelle Kalman Filter Rekursion zur Berechnung der Schätz­ fehlerkovarianzen P bzw. der Filterverstärkungsmatrix K (für den k-ten Zeitschritt ausgehend von bekannten Anfangskovarianzen ₀) lautet:

Hierbei ist C_k im Fall nichtlinearer Meßgleichungen die Jacobische Ma­ trix der Meßgleichungen zum Zeitpunkt k, d. h. die Matrix der partiellen Ableitungen der m-vektorwertigen Funktion g(x_k, p) nach den n Elementen des Zustandsvektors x_k (vgl. auch Anhang A2),

Die wesentlichen Nachteile einer Implementierung des Algorithmus in obiger Form bezüglich Rechenaufwand und numerischer Eigenschaften sind:

• a) Gl. (63) erfordert die explizite Inversion einer n×n Matrix.
• b) In Gl. (64) sind potentielle Größenordnungsunterschiede der Operan­ den im Hinblick auf numerische Stabilität und Rechengenauigkeit kri­ tisch.

1.4.3 Varianten des Kalman Filter

Im Rahmen der Raumfahrtprojekte der NASA Mitte der 60er Jahre wurden intensive Weiterentwicklungen der rekursiven Filterverfahren durchge­ führt. Diese Arbeiten wurden zum einen wesentlich durch die Implementa­ tionsbeschränkungen der damaligen Bordcomputer bezüglich Wortlänge, Rechenleistung und Arbeitsspeichergröße motiviert, aber auch generell durch numerische Probleme, die in der konventionellen Formulierung des Kalman-Filter-Algorithmus auftreten können [Bierman, 77; Maybeck, 79].

1.4.3.1 Sequentielle Filter

Der zeitaufwendige Schritt einer expliziten Matrixinversion bei der Berechnung der Schätzfehlerkovarianzen läßt sich vermeiden, wenn R_k= Diag(r_i) eine Diagonalmatrix ist. Dies ist gegeben, wenn die Meßfehler untereinander nicht korreliert, also unabhängig sind (ein diagonales R ist auch für korrelierte Meßfehler durch eine entsprechende Transforma­ tion der Meßgleichungen zu erreichen, vgl. [Chui u. Chen, 87]). Hierbei können die Meßwerte y_i bzw. deren Fehlervarianzen r_i einzeln (in skala­ rer Form), nacheinander eingebracht werden. Dadurch ergibt sich ein sequentieller Algorithmus anstelle von Gl. (63) und (64), der pro Zeitschritt k für den Meßwertindex i=1, m_k durchlaufen wird (Gln. (56) bis (69)). Die Meßmatrix C wird entsprechend nur zeilenweise verarbeitet. Die konventionelle sequentielle Innovation für Schätzwert und Kovarianz für den i-ten Meßwert (mit x_i0= _k und P_i0= _k) lautet (zum k-ten Zeitschritt, wobei zur besseren Übersichtlichkeit der Zeit­ index k hier weggelassen wurde):

mit

α_i=c_iP_i-1c_i ^T+r_i. (67)

Die Jacobische Matrix C kann hierbei innerhalb eines Zeitschritts kon­ stant bleiben. Für den Vektor der Filterverstärkungen ergibt sich somit

Die Korrektur des aktuellen Zustandsschätzwerts durch Einbringen des i- ten Meßwerts lautet

x_i=x_i-1+k_i[y_i- _i]. (69)

wobei

_k, p). (70)

Die Matrixinversion aus Gl. (63) ist somit auf eine Rekursion mit ska­ larer Division zurückgeführt worden. Die sequentielle Verarbeitung ska­ larer Meßwerte anstatt blockweiser Verarbeitung ganzer Meßwertvektoren konstanter Länge ermöglicht es zudem, eine jeweils pro Zeitschritt vari­ ierende Anzahl m_k von Meßwerten zu berücksichtigen. Dies ist ein wich­ tiger Aspekt für die Anwendung dieser Schätzverfahren zur Bildfolgenaus­ wertung. Dabei kann sich die Anzahl der aktuell sichtbaren bzw. extra­ hierbaren Merkmale abhängig von den Aspektbedingungen (vgl. [Wünsche, 88]) oder bedingt durch Szeneninhalte und -Qualität (speziell im Fall natürlicher Szenen) signifikant ändern.

1.4.3.2 Square-Root Filter

Eine weitere rechentechnisch vorteilhafte Modifikation des konventionel­ len Kalman Filters bilden die sogenannten 'Square-Root Filter', wobei Dreieckszerlegungen der Kovarianzmatrix P in der Form

P=S · S^T

benutzt werden, S also einer 'Quadratwurzel' von P entspricht. Diese Varianten wurden ursprünglich angegeben von [Potter, 64; Andrews, 68]. Abgesehen davon, daß S nur mehr anstatt n² Elemente enthält und Dreiecksmatrizen einfacher zu invertieren sind (sofern überhaupt nötig), ist damit automatisch die definitionsgemäße, aber im konventionellen Fall durch numerische Effekte 'verlierbare' Symmetrie von P garantiert. Darüber hinaus weisen diese Filter wesentlich bessere numerische Eigen­ schaften auf, da nur mehr die Quadratwurzeln von potentiell sehr großen bzw. sehr kleinen Elementen vorkommen und damit die Größenordnungsunter­ schiede (Exponenten) von Operanden halbiert werden.

1.4.4 Das UD-faktorisierte Kalman Filter

Eine Sonderform der Square-Root Filter bildet das UD-faktorisierte Kalman Filter nach [Bierman, 75]. Hierbei ist die n×n Schätzfehler­ kovarianzmatrix P faktorisiert in

P=U · D · U^T (71)

wobei D eine Diagonalmatrix und U eine obere Einheits-Dreiecksmatrix (d. h. mit Einsen auf der Diagonale) darstellt. Die ursprüngliche, 'aus­ multiplizierte' Form der n×n Kovarianzmatrix P tritt im Algorithmus nicht mehr explizit auf.

Zu den numerisch günstigen Eigenschaften dieser Faktorisierung in Kombi­ nation mit der sequentiellen Formulierung des Kalman Filter kommen noch sehr effiziente Implementationsmöglichkeiten, die speziell für Echtzeit­ anwendungen optimierte Algorithmen ergeben. So werden z. B. Dreiecks­ matrizen spaltenweise dicht in eindimensionalen Feldern abgelegt. Durch Indextabellen läßt sich der Adreßberechnungsaufwand gegenüber 2-dimen­ sionalen Feldern wesentlich reduzieren. Auch kann die Besetzungsstruktur der Transitionsmatrizen ggf. vorteilhaft genutzt werden. Die Rechen­ genauigkeit und numerische Stabilität übertrifft selbst bei ausschließ­ licher Benutzung von Gleitkommaoperanden einfacher Genauigkeit (32-bit) die des konventionellen, nicht-faktorisierten Kalman Filters bei Verwen­ dung von Variablen doppelter Genauigkeit (64-bit).

1.4.4.1 Sequentielle Kovarianz-Innovation im UD-Algorithmus

Der Rechengang für die sequentielle, skalare Kovarianz-Innovation (engl. 'neasurement update') in der UD-faktorisierten Form entspricht grund­ sätzlich Gl. (66) bis (68) unter Anwendung der Substitution P=U · D · U^T. Er ist detailliert bei [Bierman, 77] oder [Maybeck, 79, pp. 392 ff.] angegeben. Ein zentraler Schritt ist dabei jeweils die Faktorisie­ rung einer symmetrischen, positiv semidefiniten Matrix über eine Varian­ te der sogenannten Cholesky-Zerlegung.

Der skalare Term α_i aus Gl. (67) wird von [Bierman, 77] als 'innova­ tions variance' bezeichnet. Er gibt die in den Meßwertraum (hier Merk­ malskoordinaten in der Bildebene) abgebildete Varianz des extrapolierten Schätzzustands (für den k-ten Zeitschritt vor Berücksichtigung des i-ten Meßwertes) an, addiert zur Varianz r_i des i-ten Meßwertes. Die 'innova­ tions variance' entspricht somit einer Varianz des Vorhersagefehlers zwischen dem neuem Meßwert y_i und dem erwarteten Wert _i=g_i( , p) aus der Prädiktion.

Unter der Annahme gaußverteilter Störungen mit Varianz σ²_yi=α_i liegen 99% aller Meßwerte in einem Band von ±3σ_yi um den erwarteten Wert _i. Die Innovations-Varianz α_i kann somit, wie von [Wünsche, 86; 88] reali­ siert, zur Meßwert-Konsistenzüberprüfung bzw. als Selektionskriterium zur Aussonderung von 'Meßwertausreißern' angewendet werden. D. h. Meß­ werte, die die Bedingung

|y_i- _i|<3σ_yi- bzw. (y_i- _i)²<9α_-i (72)

nicht erfüllen, gehen demnach nicht in die Zustands- und Kovarianzinno­ vation ein. Dies trägt wesentlich zur Robustheit gegenüber Störungen bzw. Mehrdeutigkeiten bei, die sich besonders in natürlichen Szenen durch die lokale Merkmalsextraktion ergeben können (vgl. auch Abschnitt 4 zur Merkmalsextraktion bzw. Abschnitt 6). Die im Fahrbahnerfassungs­ modul benutzten Werte der Meßwertvarianzen r_i, die u. a. in die 'Innova­ tions-Varianzen' α_i und damit in die Bestimmung der Selektionskriterien eingehen, sind im Anhang A4 angegeben.

1.4.4.2 Kovarianz-Prädiktion im UD-Algorithmus

Die zeitliche Extrapolation der Schätzfehlerkovarianzen nach Gl. (73) bewirkt immer eine Zunahme der Schätz-'Unsicherheit', da unter Störungs­ einfluß einen Zeitschritt vorausgerechnet wird (vgl. additiver Beitrag von Q), ohne daß (zunächst) neue Information über den Systemzustand mit eingeht. Der Kovarianz-Prädiktionsschritt nimmt bei Filtern mit rela­ tiv wenigen Meßwerten (d. h. deren Anzahl entspricht etwa der Systemord­ nung) einen signifikanten Anteil des Rechenaufwands pro Zeitschritt ein.

Deswegen sind speziell bei den Algorithmus-Varianten für Echtzeitanwen­ dungen laufzeitoptimierende Implementationsaspekte sehr stark berück­ sichtigt worden.

Die konventionelle Kovarianz-Prädiktion lautet

_k+1=Φ_{k k}Φ_-k ^T+Q_k. (73)

Mit der Substitution =ÛÛ^T bzw. = ^T ergibt Gl. (73)

bzw. mit =ΦÛ als Zwischenprodukt gilt

In der UD-faktorisierten Formulierung des Kalman-Filter Algorithmus nach Bierman und Thornton gliedert sich der Kovarianz-Prädiktionsschritt (engl. auch 'time update' oder 'time propagation') grundsätzlich in zwei aufeinanderfolgende Unterschritte:

• a) Die Matrixmultiplikation zur Berechnung des Zwischenproduktes =ΦÛ. Die Berechnung von erfolgt bei allgemeinen, voll besetzten Transi­ tionsmatrizen als konventionelle n×n Matrixmultiplikation.
  Im nicht seltenen Fall einer dünn besetzten Transitionsmatrix Φ kann die Matrixmultiplikation jedoch durch einen speziellen Algorithmus nach [Thornton u. Bierman, 80] ausgeführt werden, der nur die nichtverschwin­ denden Elemente von Φ benutzt und entsprechend schneller ist.
• b) Die neuerliche Orthogonalzerlegung von , und Q in und , die Faktoren der um einen Zeitschritt extrapolierten Kovarianzmatrix , erfolgt über den sogenannten 'modifizierten Gram-Schmidt' Algorithmus [Lawson u. Hanson, 74; Stewart, 79; Maybeck, 79].

Die 'modifizierte Gram-Schmidt-Orthogonalisierung' ist Kern der Zerle­ gung von und und soll hier kurz skizziert werden, da im folgenden Abschnitt eine laufzeitreduzierende Implementationsvariante diskutiert wird. Für die gewichteten Skalarprodukte zweier Vektoren wird die Nota­ tion

und

eingeführt.

Die neuen UD-Faktoren und von in Gl. (75) werden rekursiv über die folgenden Schritte für j=n, n-1 . . . , 1 berechnet (der Rekur­ sionsindex ist hochgestellt, gegenüber tiefgesetzten Zeilen- oder Spal­ tenindizes):

Eine sehr effiziente FORTRAN-Implementation dieses Algorithmus, unter Benutzung vektorgespeicherter Matrizen, wurde von Thornton und Bierman angegeben. Eine leicht modifizierte Variante dieses Algorithmus gab [Wünsche, 88] an, wobei die Kovarianzmatrix der Systemstörungen Q als reine Diagonalmatrix eingeht. Dadurch ergeben sich weitere laufzeitre­ duzierende Vereinfachungen. Die Kovarianzmatrix Q der diskreten System­ störungen wurde hier ebenfalls als konstante Diagonalmatrix angesetzt. Deren Elemente wurden über den Prozeß des sogenannten 'filter tuning' [Maybeck, 79] iterativ-experimentell bestimmt (die Werte der Q-Elemente sind zusammen mit den Anfangswerten der Schätzfehlerkovarianz ₀ im Anhang A4 angegeben).

1.4.4.3 Laufzeitoptimierung der UD-Kovarianz-Prädiktion unter Berücksichtigung der Φ-Besetzungsstruktur

Die Transitionsmatrix Φ der Bewegungsgleichungen technischer Systeme kann bei geringer Kopplung der Bewegungsfreiheitsgrade, wie auch im hier beschriebenen Anwendungsfall, blockweise Dreiecksstruktur oder dreiecks­ ähnliche Besetzungsstruktur aufweisen. Dies trifft auch auf zwei weitere Systeme zu, deren Bewegungserfassung mittels Rechnersehen und dem UD- faktorisierten Filteransatz am Institut für Systemdynamik und Flugme­ chanik untersucht wurden: a) das Satelliten-Andockmanöver [Wünsche, 86; 88], und b) die Relativlageschätzung zu Hindernissen von einem Straßen­ fahrzeug aus [Christians et al., 89].

Bei Verarbeitung von relativ wenigen Meßwerten durch das Filter, d. h. deren Anzahl entspricht der Systemordnung oder ist noch kleiner, bildet die Kovarianz-Extrapolation den größten geschlossenen (unteilbaren) Rechenzeit-Block innerhalb eines Zeitschritts. Im vorliegenden Fall wur­ den bei Systemen achter bzw. neunter Ordnung Rechenzeiten von ca. 12 bis 16 ms für die Kovarianz-Extrapolation benötigt (auf 80 386/387, 20 MHz, ohne Wartezyklen), wogegen für die sequentielle Innovation pro Meßwert nur 2,3 bis 2,7 ms erforderlich sind. Bei einer Gesamtzykluszeit von 40 ms (zwei Bildtakte bei 50 Hz Bildfrequenz) stellt dies einen signifikan­ ten Anteil der zur Verfügung stehenden Rechenzeit dar.

In weiterführenden Untersuchungen von Anwendungen des UD-faktorisierten Ansatzes wurde von [Bierman, 81] u. a. auf die Möglichkeit hingewiesen, Blockdreiecksstrukturen der Transitionsmatrix zur Laufzeitverbesserung bei der Kovarianz-Prädiktion zu berücksichtigen. Wegen des oben erwähn­ ten hohen Rechenzeitbedarfs relativ zur Gesamtzykluszeit, und da die Transitionsmatrix hier unterhalb der Diagonalen weitgehend unbesetzt ist, wurde diese Eigenschaft der Besetzungsstruktur von Φ zur weiteren Reduzierung des Rechenaufwandes genutzt. Hierbei können unbesetzte Be­ reiche unterhalb der Diagonalen zeilenweise berücksichtigt werden.

Die Transitionsmatrix Φ des Gesamtsystems der Fahrzeugquerdynamik und der Fahrbahnkrümmungsdynamik (vgl. Anhang A3) weist hier folgende Be­ setzungsstruktur auf:

Bild 10:

Die Besetzungsstruktur der Transitionsmatrix des Fahrbahnerfassungsmoduls

Hierbei sind von n²=81 Elementen k_nz=24 ungleich Null (Anteil λ_nz=k_nz/n²=30%) bzw. unterhalb der Diagonale sind 1_z=32 unbesetzt (Anteil λ_1z=1_z/n²=40%). Zum Vergleich seien auch die entsprechenden Besetzungsdichten der oben erwähnten Anwendungen angegeben:

Die dünne Besetzungsdichte λ_nz wird bereits im 'sparse Φ multiplication' Algorithmus von [Thornton u. Bierman, 80] bei der Berechnung von W=ΦU genutzt, um die Anzahl der Gleitkomma-Operationen des Typs A:=A+B · C proportional zur Besetzungsdichte λ_nz zu reduzieren.

Im ursprünglich angegebenen Algorithmus wird jedoch im darauffolgenden Schritt (Gl. (75)) der neuerlichen Faktorisierung über den 'modifizier­ ten Gram-Schmidt Algorithmus' die von Φ auf W übertragene Besetzungs­ struktur noch nicht berücksichtigt. Wegen der oberen Dreiecksform von U bleibt nämlich bei der Matrixmultiplikation W=ΦU die Besetzungsstruk­ tur von Φ erhalten.

Im Gram-Schmidt Algorithmus werden innerhalb einer zweifach geschachtel­ ten Schleife (vgl. Gl. (79)) aus Zeilenvektoren von W die gewichteten Skalarprodukte der Art [w_i, w_j] gebildet. Die zur Skalarproduktberech­ nung nötigen Operationen, in einer dreifach geschachtelten Schleife ausgeführt, werden also proportional n³ wiederholt. Hierbei werden die Zeilenvektoren w_i elementweise von links nach rechts verarbeitet. Da W die gleiche Besetzungsstruktur wie Φ besitzt (vgl. Bild 10), die ersten n_si Elemente jeder Zeile von W gleich Null sind und somit keinen Beitrag zum Skalarprodukt liefern, können pro i-ter Zeile von W jeweils n_si Schleifendurchläufe in Gl. (78) und (79) weggelassen werden.

Daraus resultiert eine signifikante Verringerung der Rechenzeit für die UD-Kovarianz-Prädiktion, die bei den gegebenen Systemordnungen etwa proportional zum Anteil der Nullelemente unterhalb der Diagonalen in Φ ist, hier also ca. 40% beträgt (bei höheren Systemordnungen aber noch größer sein kann). Die Laufzeiten der Kovarianz-Prädiktion für ein Sy­ stem achter Ordnung konnten damit von 11,8 auf 6,9 ms reduziert werden, für ein System neunter Ordnung von 15,8 auf 9,6 ms.

Gegenüber der Algorithmusversion nach Bierman, die in [Wünsche, 88] angegeben ist, mußten als Änderungen nur ein weiteres Indexarray der Dimension n als Übergabeparameter eingeführt werden, sowie die Start­ werte zweier Schleifenindexvariablen von 1 auf n_si gesetzt werden. Das Indexarray enthält die Spaltenindizes des jeweils ersten nichtver­ schwindenden Elementes einer Transitionsmatrixzeile. Insgesamt wurden damit nur drei Statements modifiziert um obige Laufzeitbesserung zu realisieren.

Anhang A1 zu Abschnitt 1.2.3 Die Elemente der Jacobischen Matrix der Meßgleichungen

Die Abbildungsgleichung Gl. (12) läßt sich verallgemeinert als Funktion g_i des Zustandsvektors x und der im Vektor p_i zusammengefaßten Abbil­ dungsparameter ( , L_i, d, ψ_K) darstellen als

y_Bi=g_i(x, p_i), (A 1.1)

bzw. vektoriell geschrieben

y_B=g(x, p). (A 1.2)

Hierbei ist y_B der Vektor der (bis zu 8 verwerteten) horizontalen Bild­ koordinaten der Fahrbahnbegrenzungen an verschiedenen Vorauschauentfer­ nungen L_i.

Die Meßmatrix C gibt im Fall nichtlinearer Abbildungsbeziehungen den linearisierten Zusammenhang zwischen einer (kleinen) Änderung des Zu­ standes Δx um den aktuellen Zustand x₀ und der entsprechenden Verschie­ bung der Merkmalskoordinaten Δy_B im Bild an. Sie ist die Jacobische Matrix der Abbildungsgleichungen, d. h. die Matrix der partiellen Ablei­ tungen der Elemente von g nach den Elementen des Zustandsvektors x:

Für das dynamische Modell mit Horizontalkrümmungserkennung und Fahrbahn­ breitenschätzung (d. h. für den Fall einer ebenen Fahrbahn) enthält der Zustandsvektor die Elemente

Die Zustandsgrößen λ, β und c_1h gehen nicht in die Abbildungsbeziehungen Gl. (12) ein. Damit ergibt sich für die i-te Zeile c_i (bzgl. des i-ten Bildkoordinaten-Meßwerts) der Meßmatrix C:

wobei +b/2 für Bildkoordinaten-Meßwerte der rechten und -b/2 für solche der linken Fahrbahnbegrenzung gilt.

Für das dynamische Modell zur Erfassung des räumlichen Fahrbahnverlau­ fes, d. h. mit Vertikalkrümmungserkennung, enthält der Zustandsvektor die Elemente

Hierbei ist die Vorausschauentfernung L abhängig von den Vertikalkrüm­ mungsgrößen c_0vm und c_1vm, d. h. abhängig vom aktuellen Zustand x₀ (vgl. vertikale Abbildungsgeometrie, Abschnitt 5.2.4). Die i-te Zeile c_i (bzgl. des i-ten Bildkoordinaten-Meßwerts y_Bi) der Jacobischen Matrix C der Meßgleichungen lautet hierfür:

Eine analytische Differentiation der Abbildungsgleichung Gl. (12) nach den Vertikalkrümmungszuständen c_0vm und c_1vm ist nicht mehr möglich, da für die Abhängigkeit der Vorausschauweite L von den Vertikalkrümmungszu­ ständen kein analytischer Ausdruck vorliegt. Die partiellen Ableitungen ∂y_Bi/∂c_0vm bzw. ∂y_Bi∂c_1vm zum aktuellen Zustand x₀ werden dann über numerische Differentiation bestimmt via

und

Anhang A2 zu Abschnitt 1.3 Die Elemente des diskreten Zustandsraummodells

Das kontinuierliche Modell der Fahrzeugseitenbewegung bzw. der Fahrbahn­ krümmungsdynamik (Gl. (54)) ist von der Fahrgeschwindigkeit v als auch evtl. von der Vorausschauweite L abhängig. Die Elemente der Transitions­ matrix Φ und des diskreten Stellverstärkungsvektors g müssen daher zu jedem Abtastzeitpunkt neu berechnet werden. D. h. für diese Größen müssen entsprechende analytische Ausdrücke vorliegen. Innerhalb eines Abtast­ schrittes werden L und v als konstant angenommen.

Über die Laplace-Transformation lassen sich die Elemente der Transi­ tionsmatrix in analytischer Form aus der kontinuierlichen Systemmatrix herleiten (vgl. [Ackermann, 83]):

Für den diskreten Eingangsverstärkungsvektor g abhängig von Φ und b gilt

Aus dem kontinuierlichen Zustandsraummodell A, b ergibt sich damit die entsprechende diskrete Darstellung zu:

Für die Modellvariante mit Fahrbahnbreitenschätzung anstelle der Verti­ kalkrümmungserkennung ist in der obigen Transitionsmatrix nur die letzte Spalte bzw. Zeile zu streichen; ansonsten bestehen keine Unterschiede in den diskreten Formen der dynamischen Modelle.

Mit den Faktoren

ergeben sich für die nichtverschwindenden bzw. nichttrivialen Elemente der Transitionsmatrix folgende Terme:

Die Ausdrücke für die nichtverschwindenden Elemente des diskreten Ein­ gangsverstärkungsvektors g lauten:

Anhang A3 zu Abschnitt 1.4.4.1 Die Elemente der Meßfehler-Kovarianzmatrix R

Für die Varianzen der Meßfehler, die als unkorreliert angenommen werden,

R=Diag (r_i)=Diag {σ_λ², , . . . , }

wurden im Fahrbahnerfassungsmodul folgende Werte angesetzt:

σ_λ²=1,0E-6 [rad²] bzw. =5,0 [px1²].

Die Streuung des Lenkwinkelmeßwerts σ_λ=1,0E-3 [rad]≃0,057° entspricht somit etwa einem viertelten Quantisierungsschritt des mit 0,24° Auf­ lösung digitalisierten Lenkwinkels. Für die Streuung der Kantenkoor­ dinatenmeßwerte in natürlichen Straßenszenen wurde ≃2,24 [px1] angesetzt, womit auch einer gewissen Kanten-'Unschärfe' von nicht markierten, unregelmäßigen Fahrbahnbegrenzungen, wie z. B. bei einem direkten Übergang von Asphalt zu Gras, Rechnung getragen wird.

Anhang A4 zu Abschnitt 1.4.4.2 Die Elemente der Systemstörungs-Kovarianzmatrix Q und die Anfangswerte der Schätzfehlerkovarianzmatrix P₀

Die Kovarianzmatrix Q der diskreten Systemstörungen wurde für das Fahr­ bahnerfassungsmodul als konstante Diagonalmatrix angesetzt. Deren Ele­ mente wurden über den Prozeß des sogenannten 'filter tuning' [Maybeck, 79] iterativ-experimentell bestimmt zu

Q=Diag (1.E-7, 1.E-5, 1.E-4, 1.E-7,
1.E-9, 1.E-11, 1.E-10, 3.E-9, 1.E-10}. (A 4.1)

Die dabei gemachten Vereinfachungen, d. h. die Vernachlässigung der Ne­ bendiagonalelemente sowie der Geschwindigkeitsabhängigkeit, erwiesen sich aus folgenden Gründen als sinnvoll und zulässig:

• - In der Praxis hat sich ein konstantes, diagonales Q bewährt.
• - Die numerische Berechnung von Q (diskret) aus einem kontinuierlichen, diagonalen Q über die Integration (vgl. [Brammer u. Siffling, 75, pp. 93 ff]) ergab schon eine deut­ liche Diagonaldominanz der Elemente von Q.

Darüber hinaus würde eine explizite on-line Berechnung von Q über obige Beziehung selbst bei Vernachlässigung aller Terme mit höheren Potenzen als T² einen nicht unerheblichen Rechenmehraufwand für die Kovarianz- Prädiktion bedeuten.

Im Gegensatz zu Q und R, welche das 'langfristige' Filterverhalten be­ einflussen, wird durch die Anfangsschätzfehlerkovarianz ₀ hauptsächlich das Einschwingverhalten bestimmt. Als günstige Wahl für schnelles Ein­ schwingen erwies sich die Besetzung

₀=Diag {0,1, 0, 0, 1, 0, 1, 0, 1, 0, 0, 0, 1, 0}, (A 4.3)

d. h. die Anfangswerte des Schwimmwinkels sowie der Krümmungsänderungen (im Stand) werden als exakt angesehen. Mit ₀ als Diagonalmatrix gilt zusätzlich

₀= ₀ sowie ₀=I, (A 4.4)

womit sich also eine anfängliche Zerlegung von ₀ erübrigt.

Literatur

[Andrews, 68] A. Andrews: A Square Root Formulation of the Kalman Covariance Equations, AIAA Journal Vol. 6, No. 6, 1968, pp. 1165-1166.
[Bierman, 77] G. J. Bierman: Factorization Methods for Discrete Sequential Estimation. Academic Press, New York, 1977.
[Bierman, 81] G. J. Biermann: Efficient Time Propagation of U-D Covariance Factors. IEEE Trans. Automatic Control, Vol. AC-4, Nr. 4, Aug. 1981, pp. 890-894.
[Christians et al., 89] T. Christians, H. J. Wünsche, E. D. Dickmanns: Räumliche Relativlageschätzung von Objekten (in einer Ebene) durch Rechnersehen. Interner Bericht am Institut für Systemdynamik und Flugmechanik der Universität der Bundeswehr München, LRT/WE 13/IB/89-6, April 1989.
[Chui u. Chen, 87] C. K. Chui, G. Chen: Kalman Filtering with Real-Time Applications. Springer-Verlag, Berlin, Heidelberg, New York, 1987.
[Dickmanns, 87] E. D. Dickmanns: 4D-Szenenanalyse mit integralen raum-/zeitlichen Modellen. In Paulus (Hrsg.): 9.DAGM-Symposium Mustererkennung, Braunschweig; Informatik Fachberichte 149, Springer-Verlag, Sept./Okt. 1987.
[Dickmanns, 88] E. D. Dickmanns: Dynamic Computer Vision for Mobile Robot Control. Proc. Int. Symposium on Robot Control, Sydney, Nov. 1988.
[Dickmanns u. Zapp, 86] E. D. Dickmanns, A. Zapp: A Curvature-based Scheme for Improving Road Vehicle Guidance by Computer Vision. Proc. SPIE Conf. on Mobile Robots, Vol. 727, Cambridge, MA, USA, 1986, pp. 161-168.
[Donges, 78] E. Donges: Der Fahrsimulator des Forschungsinstitutes für Anthropotechnik. Forschungsbericht Nr. 41, FGAN e. V., Forschungsinstitut für Anthropotechnik, Wachtberg-Werthhoven, Juli 1978.
[Heinhold u. Behringer, 76] J. Heinhold, F. Behringer: Einführung in die Höhere Mathematik, Teil 2: Infinitesimalrechnung. Carl Hanser Verlag, 1976.
[Isermann u. Nold, 88] R. Isermann, S. Nold: Wissensbasierte Fehlerdiagnose Technischer Prozesse. Proc. Prozeßrechensysteme 88, Informatik-Fachberichte 167, R. Lauber (Hrsg.), Stuttgart, 1988, pp. 738-748.
[Maybeck, 79] P. S. Maybeck: Stochastic Models, Estimation and Control. Vol.1. Academic Press, 1979.
[Mitschke, 72] M. Mitschke: Dynamik der Kraftfahrzeuge, Springer-Verlag, Berlin, Heidelberg, New York, 1972.
[Ozawa, Rosenfeld, 86] S. Ozawa, A. Rosenfeld: Synthesis of a road image as seen from a vehicle, Pattern Recognition, Vol. 19, No. 2, 1985, S. 123-145.
[RAS-L-1, 84] Richtlinien für die Anlage von Straßen (RAS), Teil: Linienführung, Abschnitt 1: Elemente der Linienführung, Forschungsgesellschaft für Straßen- und Verkehrswesen (Hrsg.) Köln, Ausgabe 1984.
[Sakurai et al., 87] K. Sakurai, H. Zen, H. Ohta, Y. Ushioda, S. Ozawa: Analysis of a road image as seen from a vehicle. Proc. 1st ICCV, London, 1987, pp. 651-656.
[Stewart, 73] G. W. Stewart: Introduction to Matrix Computations. Academic Press, 1973.
[Thornton u. Bierman, 80] C. L. Thornton, G. J. Bierman: UDU^T Covaricance Factorization for Kalman Filtering. In: Control and Dynamic Systems, Advances in Theory and Application, Vol. 16, Academic Press, New York, 1980, pp. 178-248.
[Wünsche, 86] H. J. Wünsche: Detection and Control of Mobile Robot Motion by Real-Time Computer Vision. Proc. SPIE Conf. on Mobile Robots Vol. 727, Cambridge, MA, USA, Oct. 1986, pp. 100-109.
[Wünsche, 88] H. J. Wünsche: Bewegungssteuerung durch Rechnersehen. Fachberichte Messen, Steuern, Regeln Bd. 20, Springer-Verlag, Berlin 1988.
[Zapp. 88] A. Zapp: Automatische Straßenfahrzeugführung durch Rechnersehen. Dissertation an der Fakultät für Luft- und Raumfahrttechnik der Universität der Bundeswehr München, 1988.

Claims (2)

1. Verfahren zur kontinuierlichen und automatischen Fahr­ zeugorientierung auf einer Fahrbahn mit einem bild­ erzeugenden System und einer computerisierten Bildaus­ wertung zur Erzeugung von Fahrzeugsteuerdaten, dadurch gekennzeichnet, daß die Bilddaten eines letzten monokular erzeugten Bil­ des einer fortlaufenden Bildfolge mit Hilfe eines einen elektronischen Rechner steuernden Programms, orientiert an räumlich-geometrischen und zeitlich-dynamischen As­ pekten einer Szene, modelliert werden, um sie dann mit­ tels rekursiver Schätzverfahren zur Bestimmung der Stra­ ßenparamter, speziell des horizontalen und des vertika­ len Krümmungsverlaufs im Vorausschaubereich sowie der Breite und der eigenen straßenbezogenen Situation heran­ zuziehen.

2. Verfahren nach Anspruch 1 dadurch gekennzeichnet, daß ein kompletter Verfahrenszyklus inklusive einer Ak­ tualisierung der Steuerdaten 1/10 bis 1/30 Sekunden dauert.